CN105759726A - Adaptive curve interpolation method based on contour error constraint - Google Patents

Abstract

The invention discloses an adaptive curve interpolation method based on contour error constraint, belongs to the technical field of precision high-efficiency numerical control machining, and especially relates to a parameter spline curve interpolation feeding speed adaptive planning method based on contour error constraint. The method comprises: establishing an incidence relation model among contour errors, feeding speed, servo system parameters, and curve curvature for a typical second-order feeding servo system, and computing, by using current interpolation point curvature information and a contour error constraint value, a current interpolation point processing feeding speed permissible value under the contour error constraint; determining a magnitude relation between the permissible feeding speed of the contour error constraint and a program-preset speed, and using a minor value as the current interpolation point adaptive feeding speed value; and computing the curve parameters of a next interpolation point by using the adaptive feeding speed and achieving parameter curve interpolation considering the contour error constraint. The method is low in calculated amount, good in practicality, and capable of effectively increasing curve interpolation processing locus contour precision.

Description

Self adaptation curve interpolating method based on profile errors constraint

Technical field

The invention belongs to precise high-efficiency Computerized Numerical Control processing technology field, particularly to a kind of parametric spline curve interpolation feed speed Adaptive Planning method based on profile errors constraint.

Background technology

Along with the fast development in the Important Project such as Aero-Space, energy source and power field, the machining accuracy of the complex-curved important Parts of high-end equipment and the requirement of working (machining) efficiency are promoted by manufacturing industry day by day.In view of parametric spline curve can Precise Representation any free shape curve cutter rail, overcoming small straight line, arc section to replace the approximate error produced in curve cutter rail process thus improving surface machining accuracy, therefore parametric curve interpolator technology obtains the extensive concern of high-end numerical control field.In parametric curve interpolator process, the planning of feed speed is most important.Existing mainly it is constrained to master with geometry (namely bending high level error) constraint and kinesiology (i.e. acceleration, acceleration) about curve interpolating speed planning method, considers that the profile errors that feed system servo lagging characteristics brings out retrains.Therefore, when processing feed speed is very fast and cutter rail curvature of curve is bigger, very easily cause bigger processing profile errors, reduce the machining accuracy of part.To sum up, the parameter curve adaptive compensation technology taking into account profile errors constraint is studied significant to realizing the processing of complex curved surface parts precise high-efficiency.

Prior art literature is summed up and finds, document " CubicSplineTrajectoryGenerationwithAxisJerkandTrackingEr rorConstraints ", KeZhang etc., InternationalJournalofPrecisionEngineeringandManufacturi ng, 2013,14 (7): 1141-1146, the document is with feed shaft acceleration and following error for constraints, generating C batten cutter rail, in reality being processed, single shaft following error is limited in the limit of error set in advance.Although the method can effectively reduce following error, but following error and machining profile error there is no direct correlation relation, and the reduction of following error does not represent the reduction of profile errors.Document " Smoothfeedrateplanningforcontinuousshortlinetoolpathwith contourerrorconstraint ", JingchuanDong etc., InternationalJournalofMachineToolsandManufacture, 2014,76:1-12, the document proposes the feed speed planing method of a kind of profile errors constraint, but feed shaft servosystem is reduced to a first-order system and discusses by the document, simplification degree is too high, it is possible to cause simplified system and the original system non_uniform response when frequency is bigger.

Summary of the invention

It is contemplated that overcome prior art defect, invent a kind of self adaptation curve interpolating method based on profile errors constraint, with profile errors for constraints calculating processing feed speed adaptation value, calculated curve interpolation parameters accordingly, Interpolation Process directly considers profile errors constraint, effectively suppresses numerical control processing track profile errors.

The technical scheme is that a kind of self adaptation curve interpolating method based on profile errors constraint, its characteristic is in that, the method is first against typical case's second order feed servo-system, set up association relation model between profile errors and feed speed, servo parameter, curvature of curve, utilize current interpolated point curvature information and profile errors binding occurrence to calculate processing feed speed allowable value in current interpolated point place under profile errors constraint；Secondly, it is judged that feed speed allowable and the program of profile errors constraint preset feed speed magnitude relationship, using smaller value as current interpolated point self adaptation feed speed value；Finally, this self adaptation feed speed is utilized to calculate the parameter of curve of next interpolated point, it is achieved to take into account the parametric curve interpolator of profile errors constraint；Specifically comprise the following steps that

Under the constraint of first step profile errors, processing feed speed allowable calculates

Closed loop transfer function G (s) of the exemplary position closed loop feed servo-system controlled by proportional controller is:

$G\left(s\right)=\frac{K}{{\mathrm{Js}}^2+Bs+K}---\left(1\right)$

Wherein, K is position ring proportional controller gain, and J is servomotor and the equivalent moment of inertia of leading screw even load, and B is the equivalent viscous damping of load, and s is Laplace operator；

OrderThen formula (1) can be expressed as typical case's second-order system:

$G\left(s\right)=\frac{{\mathrm{\ω}}_n^2}{s^2+2{\mathrm{\ζ\ω}}_{n}s+{\mathrm{\ω}}_n^2}---\left(2\right)$

Wherein, ω_nFor undamped natural frequency of a mechanical system, ζ is damping ratio；Making s=j ω, the frequency response function G (j ω) obtaining system is:

$G\left(j\mathrm{\ω}\right)=\frac{{\mathrm{\ω}}_n^2({\mathrm{\ω}}_n^2-{\mathrm{\ω}}^2-2{\mathrm{\ζ\ω}}_n\mathrm{\ω}j)}{{({\mathrm{\ω}}_n^2-{\mathrm{\ω}}^2)}^2+{\left(2{\mathrm{\ζ\ω}}_n\mathrm{\ω}\right)}^2}---\left(3\right)$

Wherein, ω is system input angle frequency；

Therefore, amplitude-frequency characteristic function M (ω) of system and phase-frequency characteristic function phi (ω) are respectively as follows:

$M\left(\mathrm{\ω}\right)=\frac{{\mathrm{\ω}}_n^2}{\sqrt{{({\mathrm{\ω}}_n^2-{\mathrm{\ω}}^2)}^2+{\left(2{\mathrm{\ζ\ω}}_n\mathrm{\ω}\right)}^2}}---\left(4\right)$

$\mathrm{\φ}\left(\mathrm{\ω}\right)=-arctan\left(\frac{2{\mathrm{\ζ\ω}}_n\mathrm{\ω}}{{\mathrm{\ω}}_n^2-{\mathrm{\ω}}^2}\right)---\left(5\right)$

During the profile that radius of curvature is ρ when adopting feed speed v to process, each axle input instruction r of digital control system_x、r_yFor:

$\left\{\begin{array}{c}{r}_x=\mathrm{\ρ}\cos \left(\mathrm{\ω}t\right)-r_{x,0}\\ r_y=\mathrm{\ρ}\sin \left(\mathrm{\ω}t\right)-r_{y,0}\end{array}\right.---\left(6\right)$

Wherein, angular frequency=v/ ρ, (r_x,0,r_y,0) for the center of circle of the circle of curvature；When reaching stable state, each axle output p of system_x、p_yFor:

$\left\{\begin{array}{c}{p}_x={\mathrm{\ρM}}_x\left(\mathrm{\ω}\right)\cos (\mathrm{\ω}t+\mathrm{\φ}(\mathrm{\ω}))-r_{x,0}\\ p_y={\mathrm{\ρM}}_y\left(\mathrm{\ω}\right)\sin (\mathrm{\ω}t+{\mathrm{\φ}}_y(\mathrm{\ω}\left)\right)-r_{y,0}\end{array}\right.---\left(7\right)$

When each axle dynamic characteristics of servo system coupling is good, M_x(ω)=M_y(ω)=M (ω), actual machining profile radius of curvature is M (ω) times of desired radius of curvature, and now profile errors ε is represented by:

ε=ρ | 1-M (ω) | (8)

For digital control system, to when controlling parameter testing, often dampingratioζ is adjusted between 0.707 to 1.0, the damping characteristic good to ensure servosystem；Through type (4) is it can be seen that as ζ>0.707 time, for any ω>0, all have M (ω)<1, therefore formula (8) is rewritable is:

ε=ρ (1-M (ω)) (9)

Again because M (ω) is permanent in 0, therefore by formula (9) it can be seen that when processing the profile that radius of curvature is ρ, profile errors maximum ε_maxLess than ρ；Therefore, for given profile errors limit restraint value ε_lim, work as ε_limDuring >=ρ, no matter much processing feed speed is, can meet requirement, it is not necessary to speed is planned；Work as ε_lim< during ρ, pass through solving equation

$M\left(\mathrm{\&omega;}\right)=1-\frac{{\mathrm{\&epsiv;}}_{\lim }}{\mathrm{\&rho;}}---\left(10\right)$

It is met the angular frequency of profile errors constraints_cValue, and then it is met the processing feed speed maximum allowable value v of profile errors constraint_c=ρ ω_c；

Unique analytic solutions of solving equation (10) below；Make Q=1-ε_lim/ ρ, W=ω², equation (10) becomes:

$W^2+2(2{\mathrm{\&zeta;}}^2-1){\mathrm{\&omega;}}_n^{2}W+{\mathrm{\&omega;}}_n^4-Q=0---\left(11\right)$

Solve:

$W_{1,2}=(1-2{\mathrm{\&zeta;}}^2){\mathrm{\&omega;}}_n^2\&PlusMinus;\sqrt{{(2{\mathrm{\&zeta;}}^2-1)}^2{\mathrm{\&omega;}}_n^4-{\mathrm{\&omega;}}_n^4+Q}---\left(12\right)$

Due to ζ > 0.707 time, (1-2 ζ²) < 0, and W=ω²> 0, therefore obtain:

${\mathrm{\&omega;}}^2=(1-2{\mathrm{\&zeta;}}^2){\mathrm{\&omega;}}_n^2+\sqrt{{(2{\mathrm{\&zeta;}}^2-1)}^2{\mathrm{\&omega;}}_n^4-{\mathrm{\&omega;}}_n^4+Q}---\left(13\right)$

I.e. non trivial solution ω_cFor:

${\mathrm{\&omega;}}_c=\sqrt{(1-2{\mathrm{\&zeta;}}^2){\mathrm{\&omega;}}_n^2+\sqrt{{(2{\mathrm{\&zeta;}}^2-1)}^2{\mathrm{\&omega;}}_n^4-{\mathrm{\&omega;}}_n^4+Q}}---\left(14\right)$

By ω_c=v_c/ ρ, Q=1-ε_lim/ ρ substitutes into above formula, obtains profile errors ε_limThe lower maximum allowable processing feed speed value v of constraint_cFor:

$v_c={\mathrm{\&rho;\&omega;}}_n\sqrt{1-2{\mathrm{\&zeta;}}^2+\sqrt{{(2{\mathrm{\&zeta;}}^2-1)}^2-\frac{{\mathrm{\&epsiv;}}_{\lim }^2-2{\mathrm{\&epsiv;}}_{\lim }\mathrm{\&rho;}}{{(\mathrm{\&rho;}-{\mathrm{\&epsiv;}}_{\lim })}^2}}}---\left(15\right)$

Second step processing feed speed adaptation value calculates

Obtaining on the basis of current interpolated point place curvature of curve, first step method is adopted to calculate the maximum allowable processing feed speed under profile errors constraint, and compare with program default processing feed speed value, select the processing feed speed adaptation value v that smaller retrains as current interpolated point place profile errors_i:

v_i=min{v_c,v_p}(16)

Wherein, v_pProcessing feed speed value is preset for program；

3rd step interpolation parameters calculates

According to the profile errors constraint lower feeding speed adaptive value v that second step obtains_i, adopt second order Taylor series expansion method to calculate the parameter of curve value u at parametric spline curve next one interpolated point place_i+1:

$u_{i+1}=u_i+\frac{v_i}{\left|\right|C^{\&prime;}\left(u_i\right)\left|\right|}Ts-\frac{v_i^2\left(C^{\&prime;}\right(u_i),C^{\&prime;\&prime;}\left(u_i\right))}{2\left|\right|C^{\&prime;}\left(u_i\right)|{|}^2}{\mathrm{Ts}}^2---\left(17\right)$

Wherein, u_iFor current interpolated point parameter of curve value, C ' (u_i) lead mistake, C " (u for the single order of current interpolated point place SPL_i) leading mistake for the second order of current interpolated point place SPL, Ts is interpolation cycle；

Judge whether to arrive final on trajectory, if not reaching home, making i=i+1, repeating said process；If reaching terminal, then terminate interpolation, thus realizing taking into account the self adaptation curve interpolating of profile errors constraint.

The invention has the beneficial effects as follows: the maximum allowable processing feed speed model establishing under profile errors constraint, provide important reference for the planning of NC Interpolation process feed speed；Take into full account the dynamic characteristic of feed servo-system when parametric curve interpolator, the precise high-efficiency of complex curved surface parts has been processed significant.

Accompanying drawing explanation

The self adaptation curve interpolating method overall flow figure that Fig. 1 retrains based on profile errors；

Fig. 2 " bone " shape non-homogeneous B spline curve geometric model figure；

Fig. 3 adopts the profile errors comparison diagram of this method and constant speed profile interpolating method machining locus；Wherein, A axle represents SPL parameter, and B axle represents profile errors absolute value, and unit is mm, and curve 1 is constant speed profile interpolation machining locus profile errors value, and curve 2 is this method machining locus profile errors value.

Detailed description of the invention

Combination technology scheme and accompanying drawing describe the specific embodiment of the present invention in detail.

In parametric spline curve Interpolation Process, if being left out machine dynamic characteristics, very easily causing that feed speed is relatively big, machining locus curvature produces profile errors time bigger, affecting curved surface part machining accuracy.Accordingly, a kind of self adaptation curve interpolating method based on profile errors constraint is invented.

For non-homogeneous B spline curve interpolation, by MATLAB computed in software and emulate, describing the invention process process in detail, overall flow is referring to accompanying drawing 1.

The first step, as shown in Figure 1, first, making i=1, i is current interpolated point sequence number, calculates the radius of curvature ρ of current interpolated point place curve:

$\mathrm{\&rho;}=\frac{\left|\right|C^{\&prime;}\left(u_i\right)|{|}^3}{\left|\right|C^{\&prime;}\left(u_i\right)\&times;C^{\&prime;\&prime;}\left(u_i\right)\left|\right|}$

In this example, interpolation track is one " bone " sigmoid curves, and as shown in Figure 2, parameter of curve is geometric model: exponent number: 2；Control point: { (0,0)；(-8,-20)；(30,-5)；(60,-20)；(47,0)；(60,20)；(30,5)；(-8,20)；(0,0)}；Weight factor: { 1,0.9,0.75,1.5,6,3.5,1.8,1.5,1}；Knot vector: { 0,0,0,0.15,0.3,0.45,0.6,0.75,0.85,1,1,1}；

Second step, by current interpolated point curve curvature radius ρ, feed shaft servo parameter ω_n, ζ, profile errors limit restraint value ε_limSubstitute into formula (15), obtain the maximum allowable processing feed speed value v under the profile errors constraint of current interpolated point place_c；And then, according to formula (16), calculate current interpolated point place self adaptation feed speed value v_i；

In this example, take undamped natural frequency of a mechanical system ω_n=67.08 (1/s), dampingratioζ=0.745, profile errors limit restraint value ε_lim=0.01mm, presets feed speed v_p=80 (mm/s)；

3rd step, according to current interpolated point self adaptation feed speed value v_i, adopt formula (17) to calculate next interpolated point place parameter of curve u_i+1；In this example, take interpolation cycle Ts=0.002s；Judge whether to arrive final on trajectory, if not reaching home, making i=i+1, repeating said process；If reaching terminal, then terminate interpolation；

For " bone " sigmoid curves in this example, adopting above-mentioned steps to carry out parametric spline curve adaptive interpolation, the wide error of gained machining locus wheel contrasts as shown in Figure 3 with the profile errors of constant speed profile interpolation gained machining locus；It can be seen that adopt this method can effectively suppress machining profile error from accompanying drawing 3, improve parametric curve interpolator machining profile precision.

When the present invention is directed to parametric curve interpolator under high feed speed, it is limited to the problem that deep camber machining locus profile errors that machine dynamic characteristics causes is bigger, invent the self adaptation curve interpolating method based on profile errors constraint, it is used for suppressing curve interpolating machining locus profile errors, the development of Numeric Control Technology and the precise high-efficiency processing of high-performance complex curved surface parts are significant.

Claims (1)

1. the self adaptation curve interpolating method based on profile errors constraint, its characteristic is in that, the method is first against typical case's second order feed servo-system, set up association relation model between profile errors and feed speed, servo parameter, curvature of curve, utilize current interpolated point curvature information and profile errors binding occurrence to calculate processing feed speed allowable value in current interpolated point place under profile errors constraint；Secondly, it is judged that feed speed allowable and the program of profile errors constraint preset feed speed magnitude relationship, using smaller value as current interpolated point self adaptation feed speed value；Finally, this self adaptation feed speed is utilized to calculate the parameter of curve of next interpolated point, it is achieved to take into account the parametric curve interpolator of profile errors constraint；Specifically comprise the following steps that

Under the constraint of first step profile errors, processing feed speed allowable calculates

Closed loop transfer function G (s) of the exemplary position closed loop feed servo-system controlled by proportional controller is:

$G\left(s\right)=\frac{K}{{\mathrm{Js}}^2+Bs+K}---\left(1\right)$

Wherein, K is position ring proportional controller gain, and J is servomotor and the equivalent moment of inertia of leading screw even load, and B is the equivalent viscous damping of load, and s is Laplace operator；

OrderThen formula (1) is expressed as typical case's second-order system:

$G\left(s\right)=\frac{{\mathrm{\&omega;}}_n^2}{s^2+2{\mathrm{\&zeta;\&omega;}}_{n}s+{\mathrm{\&omega;}}_n^2}---\left(2\right)$

Wherein, ω_nFor undamped natural frequency of a mechanical system, ζ is damping ratio；Making s=j ω, the frequency response function G (j ω) obtaining system is:

$G\left(j\mathrm{\&omega;}\right)=\frac{{\mathrm{\&omega;}}_n^2({\mathrm{\&omega;}}_n^2-{\mathrm{\&omega;}}^2-2{\mathrm{\&zeta;\&omega;}}_n\mathrm{\&omega;}j)}{{({\mathrm{\&omega;}}_n^2-{\mathrm{\&omega;}}^2)}^2+{\left(2{\mathrm{\&zeta;\&omega;}}_n\mathrm{\&omega;}\right)}^2}---\left(3\right)$

Wherein, ω is system input angle frequency；

Therefore, amplitude-frequency characteristic function M (ω) of system and phase-frequency characteristic function phi (ω) are respectively as follows:

$M\left(\mathrm{\&omega;}\right)=\frac{{\mathrm{\&omega;}}_n^2}{\sqrt{{({\mathrm{\&omega;}}_n^2-{\mathrm{\&omega;}}^2)}^2+{\left(2{\mathrm{\&zeta;\&omega;}}_n\mathrm{\&omega;}\right)}^2}}---\left(4\right)$

$\mathrm{\&phi;}\left(\mathrm{\&omega;}\right)=-\arctan \left(\frac{2{\mathrm{\&zeta;\&omega;}}_n\mathrm{\&omega;}}{{\mathrm{\&omega;}}_n^2-{\mathrm{\&omega;}}^2}\right)---\left(5\right)$

During the profile that radius of curvature is ρ when adopting feed speed v to process, each axle input instruction r of digital control system_x、r_yFor:

$\left\{\begin{array}{c}{r}_x=\mathrm{\&rho;}\cos \left(\mathrm{\&omega;}t\right)-r_{x,0}\\ r_y=\mathrm{\&rho;}\sin \left(\mathrm{\&omega;}t\right)-r_{y,0}\end{array}\right.---\left(6\right)$

Wherein, angular frequency=v/ ρ, (r_x,0,r_y,0) for the center of circle of the circle of curvature；When reaching stable state, each axle output p of system_x、p_yFor:

$\left\{\begin{array}{c}{p}_x={\mathrm{\&rho;M}}_x\left(\mathrm{\&omega;}\right)\cos (\mathrm{\&omega;}t+{\mathrm{\&phi;}}_x(\mathrm{\&omega;}\left)\right)-r_{x,0}\\ p_y={\mathrm{\&rho;M}}_y\left(\mathrm{\&omega;}\right)\sin (\mathrm{\&omega;}t+{\mathrm{\&phi;}}_y(\mathrm{\&omega;}\left)\right)-r_{y,0}\end{array}\right.---\left(7\right)$

When each axle dynamic characteristics of servo system coupling is good, M_x(ω)=M_y(ω)=M (ω), actual machining profile radius of curvature is M (ω) times of desired radius of curvature, and now profile errors ε is expressed as:

ε=ρ | 1-M (ω) | (8)

For digital control system, to when controlling parameter testing, often dampingratioζ is adjusted between 0.707 to 1.0, the damping characteristic good to ensure servosystem；Through type (4) is as ζ>0.707 time, for any ω>0, all have M (ω)<1, therefore formula (8) is rewritten as:

ε=ρ (1-M (ω)) (9)

Again because M (ω) is permanent in 0, therefore by formula (9), when processing the profile that radius of curvature is ρ, profile errors maximum ε_maxLess than ρ；Therefore, for given profile errors limit restraint value ε_lim, work as ε_limDuring >=ρ, no matter much processing feed speed is, all meets requirement, it is not necessary to speed is planned；Work as ε_lim< during ρ, pass through solving equation

$M\left(\mathrm{\&omega;}\right)=1-\frac{{\mathrm{\&epsiv;}}_{\lim }}{\mathrm{\&rho;}}---\left(10\right)$

It is met the angular frequency of profile errors constraints_cValue, and then it is met the processing feed speed maximum allowable value v of profile errors constraint_c=ρ ω_c；

Unique analytic solutions of solving equation (10) below；Make Q=1-ε_lim/ ρ, W=ω², equation (10) becomes:

$W^2+2(2{\mathrm{\&zeta;}}^2-1){\mathrm{\&omega;}}_n^{2}W+{\mathrm{\&omega;}}_n^4-Q=0---\left(11\right)$

Solve:

$W_{1,2}=(1-2{\mathrm{\&zeta;}}^2){\mathrm{\&omega;}}_n^2\&PlusMinus;\sqrt{{(2{\mathrm{\&zeta;}}^2-1)}^2{\mathrm{\&omega;}}_n^4-{\mathrm{\&omega;}}_n^4+Q}---\left(12\right)$

Due to ζ > 0.707 time, (1-2 ζ²) < 0, and W=ω²> 0, therefore obtain:

${\mathrm{\&omega;}}^2=(1-2{\mathrm{\&zeta;}}^2){\mathrm{\&omega;}}_n^2+\sqrt{{(2{\mathrm{\&zeta;}}^2-1)}^2{\mathrm{\&omega;}}_n^4-{\mathrm{\&omega;}}_n^4+Q}---\left(13\right)$

I.e. non trivial solution ω_cFor:

${\mathrm{\&omega;}}_c=\sqrt{(1-2{\mathrm{\&zeta;}}^2){\mathrm{\&omega;}}_n^2+\sqrt{{(2{\mathrm{\&zeta;}}^2-1)}^2{\mathrm{\&omega;}}_n^4-{\mathrm{\&omega;}}_n^4+Q}}---\left(14\right)$

By ω_c=v_c/ ρ, Q=1-ε_lim/ ρ substitutes into above formula, obtains profile errors ε_limThe lower maximum allowable processing feed speed value v of constraint_cFor:

$v_c={\mathrm{\&rho;\&omega;}}_n\sqrt{1-2{\mathrm{\&zeta;}}^2+\sqrt{{(2{\mathrm{\&zeta;}}^2-1)}^2-\frac{{\mathrm{\&epsiv;}}_{\lim }^2-2{\mathrm{\&epsiv;}}_{\lim }\mathrm{\&rho;}}{{(\mathrm{\&rho;}-{\mathrm{\&epsiv;}}_{\lim })}^2}}}---\left(15\right)$

Second step processing feed speed adaptation value calculates

Obtaining on the basis of current interpolated point place curvature of curve, first step method is adopted to calculate the maximum allowable processing feed speed under profile errors constraint, and compare with program default processing feed speed value, select the processing feed speed adaptation value v that smaller retrains as current interpolated point place profile errors_i:

v_i=min{v_c,v_p}(16)

Wherein, v_pProcessing feed speed value is preset for program；

3rd step interpolation parameters calculates

According to the profile errors constraint lower feeding speed adaptive value v that second step obtains_i, adopt second order Taylor series expansion method to calculate the parameter of curve value u at parametric spline curve next one interpolated point place_i+1:

$u_{i+1}=u_i+\frac{v_i}{\left|\right|C^{\&prime;}\left(u_i\right)\left|\right|}Ts-\frac{v_i^2\left(C^{\&prime;}\right(u_i),C^{\&prime;\&prime;}(u_i\left)\right)}{2\left|\right|C^{\&prime;}\left(u_i\right)|{|}^4}{\mathrm{Ts}}^2---\left(17\right)$

Wherein, u_iFor current interpolated point parameter of curve value, C ' (u_i) lead mistake, C " (u for the single order of current interpolated point place SPL_i) leading mistake for the second order of current interpolated point place SPL, Ts is interpolation cycle；

Judge whether to arrive final on trajectory, if not reaching home, making i=i+1, repeating said process；If reaching terminal, then terminate interpolation, thus realizing taking into account the self adaptation curve interpolating of profile errors constraint.