TIME OPTIMAL SPEED PLANNING METHOD AND SYSTEM BASED ON CONSTRAINT CLASSIFICATION

Information

  • Patent Application
  • 20230185262
  • Publication Number
    20230185262
  • Date Filed
    November 22, 2022
    a year ago
  • Date Published
    June 15, 2023
    11 months ago
Abstract
A time optimal speed planning method and system based on constraint classification. The method comprises: reading path information and carrying out curve fitting to obtain a path curve; sampling the path curve, and considering static constraint to obtain a static upper bound value of a speed curve; considering dynamic constraint, and combining the static upper bound value of the speed curve to construct a time optimal speed model; carrying out convex transformation on the time optimal speed model to obtain a convex model; and solving the convex model based on a quadratic sequence planning method to obtain a final speed curve. The system comprises: a path curve module, a static constraint module, a dynamic constraint module, a model transformation module and a solving module.
Description
CROSS REFERENCE TO RELATED APPLICATIONS

This application claims foreign priority of Chinese Patent Application No. 202111505094.4, filed on Dec. 10, 2021 in the China National Intellectual Property Administration, the disclosures of all of which are hereby incorporated by reference.


TECHNICAL FIELD

The present invention relates to the field of precision motion control, and particularly to a time optimal speed planning method and system based on constraint classification.


BACKGROUND

With the development of power source, aerospace and other technologies, the requirements for machining quality and machining efficiency of parts with a complex curved surface are getting higher and higher. In order to ensure the machining efficiency, it is necessary to plan a machining time optimal speed curve according to a path to be taken, and there are two speed planning methods, comprising an S-shaped planning method and an optimal planning method, wherein a speed curve planned by the S-shaped planning method in the case of complex path cannot achieve time optimum, and over scaling often occurs in places where a path curvature is changeable and changes greatly or other constraints change greatly in the optimal planning method, so that the stability of constraint solution is poor.


SUMMARY

In order to solve the technical problems above, the present invention aims to provide a time optimal speed planning method based on constraint classification, in which a time optimal speed curve can still be planned under the condition of complex path, more types of constraints can be considered, and the condition of excessive scaling cannot occur.


A first technical solution used in the present invention is: a time optimal speed planning method based on constraint classification, which comprises the following steps of:


reading path information and carrying out curve fitting to obtain a path curve;


sampling the path curve, and considering static constraint to obtain a static upper bound value of a speed curve;


considering dynamic constraint, and combining the static upper bound value of the speed curve to construct a time optimal speed model;


carrying out convex transformation on the time optimal speed model to obtain a convex model; and


solving the convex model based on a quadratic sequence planning method to obtain a final speed curve.


Further, a formula of the curve fitting is expressed as follows:








C

(
u
)

=





i
=
0

n




N

i
,
p


(
u
)



w
i



P
i







i
=
0

n




N

i
,
p


(
u
)



w
i





,


u
1


u


u

n
+
p
+
1



,




wherein C(u) represents the path curve, Pi represents a control point, wi represents a weight coefficient corresponding to the control point, Ni,p represents an ith p-order function, and u represents a spline parameter variable.


Further, the step of considering the dynamic constraint, and combining the static upper bound value of the speed curve to construct the time optimal speed model, specifically comprises:


substituting the static upper bound value of the speed curve into a dynamic constraint equation set point by point, and constructing an initial model with time optimum as the objective function; and


transforming the initial model according to a feed speed and the static upper bound value to obtain the time optimal speed model.


Further, a formula of the time optimal speed model is expressed as follows:







min

O

=




i
=
1

n


(


f

up
,
i


-

F
i


)








s
.
t
.

{







"\[LeftBracketingBar]"



A
i

(
s
)



"\[RightBracketingBar]"




a

t
,
max











"\[LeftBracketingBar]"



J
i

(
s
)



"\[RightBracketingBar]"




j

t
,
max











"\[LeftBracketingBar]"



A

^

,
i





(
s
)




"\[RightBracketingBar]"




a

^

,
max












"\[LeftBracketingBar]"



J

^

,
i



(
s
)



"\[RightBracketingBar]"




j

^

,
max











f
1

=
0

,


f
2

=
10

,


f

n
-
1


=
10

,


f
n

=
0













i
=
3

,
4
,


,

n
-
2

,




wherein fup,i represents a maximum speed value at an ith sampling point, Fi represents an optimal speed value at the ith sampling point, Ai(s) and Ji(s) represent an acceleration and a jerk at the ith sampling point in a feed direction, A∧,i(s) and J∧,i(s) represent an acceleration and a jerk at the ith sampling point in a ∧ axis direction, at,max and jt,max respectively represent a maximum acceleration and a maximum jerk in a tangent direction, and a∧,max and j∧,max represent a maximum acceleration and a maximum jerk in the ∧ axis direction.


Further, the step of carrying out the convex transformation on the time optimal speed model to obtain the convex model, specifically comprises:


transforming an objective function of the time optimal speed model into a matrix form to obtain an objective function of the matrix form;


transforming a constraint condition into a matrix form through matrix decomposition and a preset scaling function to obtain a constraint condition of the matrix form; and


obtaining the convex model according to the objective function of the matrix form and the constraint condition of the matrix form.


Further, a formula of the convex model is expressed as follows:







min

O

=



F
T


EF

-


F
up
T



F
up









s
.
t
.

{







1
2



F
T



P
i
+


F

+


1
2



F
up
T



ϒ

(

P
i
-

)


F

+


q
i
T


F

+
c


0








(


E
1

+

E
2

+

E

n
-
1


+

E
n


)


F

=
b












i
=
3

,
4
,


,

n
-
2.





wherein F represents an optimal speed to be solved, E represents a unit matrix, Pi+ represents a matrix with a positive eigenvalue after decomposition of an ith quadratic matrix Pi, Pi represents a matrix with a negative eigenvalue after decomposition of the ith quadratic matrix Pi, the matrix Pi refers to specific embodiments, and qi, b and c represent constant vectors.


A second technical solution used in the present invention is: a time optimal speed planning system based on constraint classification, which comprises:


a path curve module configured for reading path information and carrying out curve fitting to obtain a path curve;


a static constraint module configured for sampling the path curve, and considering static constraint to obtain a static upper bound value of a speed curve;


a dynamic constraint module configured for considering dynamic constraint, and combining the static upper bound value of the speed curve to construct a time optimal speed model;


a model transformation module configured for carrying out convex transformation on the time optimal speed model to obtain a convex model; and


a solving module configured for solving the convex model based on a quadratic sequence planning method to obtain a final speed curve.


The method and the system of the present invention have the beneficial effects that: according to the present invention, the constraint types are classified to adapt to subsequent model transformation, and the original model is transformed into the convex model in combination with the model transformation, so that the problem that a solution of a non-convex model falls into local optimum is prevented, and the problem of excessive scaling is solved.





BRIEF DESCRIPTION OF THE DRAWINGS


FIG. 1 is a flow chart of steps of a time optimal speed planning method based on constraint classification according to the present invention;



FIG. 2 is a structure block diagram of a time optimal speed planning system based on constraint classification according to the present invention; and



FIG. 3 shows feed speed curves planned by the method of the present invention and a traditional method.





DETAILED DESCRIPTION

The present invention is further described hereinafter with reference to the drawings and the specific embodiments. The numbers of the steps in the following embodiments are only set for convenience of explanation, and the sequence of the steps is not restricted. The execution sequence of the steps in the embodiments may be adjusted adaptively according to the understanding of those skilled in the art.


With reference to FIG. 1, the present invention provides a time optimal speed planning method based on constraint classification, and the method comprises the following steps.


In S1, path information is read and curve fitting is carried out to obtain a path curve.


In S1.1, path information (x1, y1), (x2, y2), (x3, y3), . . . , (xn, yn) is read.


In S1.2, NURBS curve fitting is employed to obtain the path curve as follows:








C

(
u
)

=





i
=
0

n




N

i
,
p


(
u
)



w
i



P
i







i
=
0

n




N

i
,
p


(
u
)



w
i





,


u
1


u


u

n
+
p
+
1



,




wherein C(u) represents the path curve, Pi (i=1, 2, . . . , n) represents a control point, wi represents a weight coefficient corresponding to the control point, Ni,p represents an ith p-order function, u represents a spline parameter variable and 0≤u≤1, and Ni,p is a base function and calculated as follows:








N

i
,
p


(
u
)

=

{




0
,





u
i


u


u

i
+
1








1
,



otherwise












N

i
,
p


(
u
)

=




u
-

u
i




u

i
+
p


-

u
i






N

i
,

p
-
1



(
u
)


+




u

i
+
p
-
1


-
u



u

i
+
p
+
1


-

u

i
+
1








N


i
+
1

,

p
-
1



(
u
)

.







Constraints are divided into static constraint and dynamic constraint in the present invention.


The static constraint is that:


a maximum feed speed ft,max, a maximum axis speed v[vx,max, vy,max, vz,max, . . . ]T and a maximum motor speed v=[vm1,max, vm2,max, vm3,max, . . . ]T determined by performances of a machine tool are a set of static constraints, which may be expressed as:






f
p,max=min{ft,max,K1(v∧,max),K1(K2(v∨,max))},


wherein Kn is inverse kinematics transformation.


A centripetal acceleration fn,a and a centripetal jerk constraint fn,j, are as follows:






f
n,a=√{square root over (ρan,max)}






f
n,j=√{square root over (ρ2jn,max)},


wherein an,max and jn,max represent a maximum centripetal acceleration and a maximum centripetal jerk, and ρ represents a curvature of the path curve as follows:






ρ
=







C


(
u
)



3






C


(
u
)

×


C


(
u
)





.





A chord error constraint and a contour error constraint are as follows:







f
chrd

=


2
T





ρ
2

-


(

ρ
-

δ
max


)

2












f
cntr

=


ρω
n




1
-

ξ
2

+




(


2


ξ
2


-
1

)

2

-



ε
max
2

-

2


ε
max


ρ




(

ρ
-

ε
max


)

2








,




wherein δmax represents a maximum chord error, εmax represents a maximum contour error, and ωn and ξ respectively represent inherent frequency and damping of a numerical control system.


Therefore, a maximum feed speed satisfying the static constraint is as follows:






f
sta,max=min{fp,max,fn,a,fn,j,fchrd,fcntr}.


The dynamic constraint is that:


a form of feed speed constraint satisfying tangential kinematics constraint is as follows:






{








"\[LeftBracketingBar]"


F

(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"



L
.

(
s
)



"\[RightBracketingBar]"




f

t
,
max












"\[LeftBracketingBar]"


A

(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"



L
¨

(
s
)



"\[RightBracketingBar]"




a

t
,
max












"\[LeftBracketingBar]"


J

(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"



L


(
s
)



"\[RightBracketingBar]"




j

t
,
max







,





wherein s represents an arc length of the path C(u), which may be calculated by a Simpson's integral method, L(s) is a length function of a feed path along the path C(u), {dot over (L)}(s), {umlaut over (L)}(s), custom-character(s) and are respectively first-order, second-order and third-order derivatives of the L(s) in a time domain t, and ft,max, at,max and jt,max respectively represent a maximum feed speed, a maximum tangential acceleration and a maximum tangential jerk allowed by the machine tool.


There is an inverse kinematics relationship between a tangential motion of a terminal of the numerical control system and a motion of each axis, and similarly, a kinematics constraint of a single axis is as follows:






{








"\[LeftBracketingBar]"



V
^

(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"



M
.

(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"



M



F



"\[RightBracketingBar]"




v

^

,
max














"\[LeftBracketingBar]"



A
^

(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"



M
¨

(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"




M




F
2


+


M




A
t





"\[RightBracketingBar]"




a

^

,
max














"\[LeftBracketingBar]"



J
^

(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"



M


(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"




M
′′′



F
3


+

3


M




A
t


F

+


M




J
t





"\[RightBracketingBar]"




j

^

,
max









,





wherein ∧=x, y, z, θx, θy, θz, M(s) represents a displacement function of each axis, “⋅” represents derivation of time t, “′” represents derivation of parameter s, and v∧,max, a∧,max and j∧,max represent maximum motion parameters allowed by each axis.


It can be known from the constraint analysis above that a maximum feed speed satisfying the dynamic constraint is as follows:






f
dyn,max=min{Tt,at,max),Tt,j(jt,max),T∧,a(a∧,max),T∧,j(j∧,max)},


then under all constraints, a maximum feed speed value is as follows:






F=min{fsta,max,fdyn,max}.


In S2, the path curve is sampled, and the static constraint is considered to obtain a static upper bound value of a speed curve.


Specifically, in order to realize rapid solution, we first consider the static constraint to obtain the static upper bound of the speed curve, and then consider the dynamic constraint. When the path curve is sampled, taking 1000 sampling points as an example, the static upper bound value of the feed speed curve is calculated by scanning a whole path at isoparametric intervals [0, Δu, 2Δu, . . . , (n−2)Δu, 1] as follows:






F
up=[fsta,max,1,fsta,max,2, . . . , fsta,max,n]T.


In S3, the dynamic constraint is considered, and the static upper bound value of the speed curve is combined to construct a time optimal speed model.


In S3.1, the static upper bound value of the speed curve is substituted into a dynamic constraint equation set point by point, and an initial model is constructed with time optimum as the objective function.


Specifically, 1,000 static upper bound values of speed are substituted into two dynamic constraint equation sets point by point, with a total of 8,000 constraint conditions, and the following model is constructed with time optimum as the objective function as follows:










min

v




T

=




t
0


t
f



1

dt











F
=


[


f
1

,

f
2

,


,

f
n


]

T








s
.
t
.

{







"\[LeftBracketingBar]"



A
i

(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"




k

1
,
i




f
i
2


-


k

1
,
i




f
i



f

i
-
1






"\[RightBracketingBar]"




a

t
,
max












"\[LeftBracketingBar]"



J
i

(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"



2


k

1
,
i

2



f
i
3


+


k

2
,
i




f
i
2



f

i
-
2



+


k

1
,
i

2



f
i



f

i
-
1

2


-


k

3
,
i




f
i
2



f

i
-
1






"\[RightBracketingBar]"




j

t
,
max












"\[LeftBracketingBar]"



A



,
i



(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"




(



k

1
,
i




M
i



+

M
i



)



f
i
2


-


k

1
,
i




M
i




f
i



f

i
-
1






"\[RightBracketingBar]"




a



,
max













"\[LeftBracketingBar]"



J



,
i



(
s
)



"\[RightBracketingBar]"


=




"\[LeftBracketingBar]"







k

1
,
i

2



M
i




f
i



f

i
-
1

2


-


(


3


k

1
,
i




M
i



+


k

3
,
i




M
i




)



f
i
2



f

i
-
1



+








(


M
i


+

3


k

1
,
i




M



+

2


k

1
,
i

2



M
i




)



f
i
3


+


k

2
,
i




M
i




f
i
2



f

i
-
2









"\[RightBracketingBar]"




j



,
max












f
1

=
0

,


f
2

=
10

,


f

n
-
1


=
10

,


f
n

=
0















i
=
3

,
4
,


,

n
-
2


,

wherein











k

1
,
i


=

1


s
i

-

s

i
-
1





,


k

2
,
i


=


K

1
,
i




s

i
-
1


-

s

i
-
2





,


k

3
,
i


=


3


K

1
,
i

2


+


K

2
,
i


.








In S3.2, the initial model is transformed according to the feed speed and the static upper bound value to obtain the time optimal speed model.


Specifically, Fup represents an upper bound of a speed at each point on the path, and although some points at the speed fail to satisfy single axis kinematics constraint, since tmin=s/Fup, a machining time along the path is shorter when an actual feed speed value is closer to Fup. Therefore, the time optimal objective function may be expressed by a minimum difference sum of the feed speed F and the Fup. Since the feed speed is F∈[0, Fup], the time optimal speed model may be expressed as follows:







min


O

=




i
=
1

n


(


f

up
,
i


-

F
i


)








s
.
t
.

{







"\[LeftBracketingBar]"



A
i

(
s
)



"\[RightBracketingBar]"




a

t
,
max











"\[LeftBracketingBar]"



J
i

(
s
)



"\[RightBracketingBar]"




j

t
,
max











"\[LeftBracketingBar]"



A



,
i





(
s
)




"\[RightBracketingBar]"




a



,
max












"\[LeftBracketingBar]"



J



,
i



(
s
)



"\[RightBracketingBar]"




j



,
max











f
1

=
0

,


f
2

=


10.

f

n
-
1



=
10


,


f
n

=
0













i
=
3

,
4
,


,

n
-
2


,




wherein fup,i represents a maximum speed value at an ith sampling point, Fi represents an optimal speed value at the ith sampling point, Ai(s) and Ji(s) represent an acceleration and a jerk at the ith sampling point in a feed direction, A□,i(s) and J□,i(s) represent an acceleration and a jerk at the ith sampling point in a ∧ axis direction, at,max and jt,max respectively represent a maximum acceleration and a maximum jerk in a tangent direction, and a∧,max and j∧,max represent a maximum acceleration and a maximum jerk in the ∧ axis direction.


In S4, convex transformation is carried out on the time optimal speed model to obtain a convex model.


Specifically, the time optimal speed model above is a nonlinear nonconvex model. During solution, solution of a nonconvex problem often falls into a dilemma of local optimum on the one hand, and when there are too many sampling points, a solution space dimension is too large and a solving efficiency is slow on the other hand. Aiming at the two problems, the nonconvex problem is simplified into a convex problem.


In S4.1, an objective function of the time optimal speed model is transformed into a matrix form to obtain an objective function of the matrix form.


Specifically, the objective function is transformed as follows:








min


O

=




i
=
1

n



(


f

up
,
i


-

f
i


)

2



,

i
=
3

,
4
,


,

n
.





The transformed matrix form is as follows:





minO=FTEF−FupTFup.


In S4.2, a constraint condition is transformed into a matrix form through matrix decomposition and a preset scaling function to obtain a constraint condition of the matrix form.


The transformation of the constraint condition is that:

    • the constraint condition is observed, and a general form of the constraint condition is found as follows:





cons=a11fi-22+a22fi-12+a33fi2+a12fi-2fi-1+a13fi-2fi-1+a23fi-2fi-1+b1fi-2+b2fi-1+b3fi+c.


The transformed matrix form is as follows:







cons
=



1
2



F
T



P
i


F

+


q
i
T


F

+
c


,

i
=
3

,
4
,


,
n
,
wherein








P
i

=

(




O


(

i
-
2

)

×

(

i
-
2

)


















P
^

i















O


(

n
-
i

)

×

(

n
-
i

)






)


,









P
^

i

=

(




2


a
11





a
12




a
13






a
12




2


a
22





a
23






a
13




a
23




2


a
33





)


,




and c is a constant.


Specifically, the matrix decomposition is that: an eigenvalue of a matrix is calculated, λmin<0 is set to be a minimum eigenvalue of the matrix {circumflex over (P)}i, and then for any σ>|λmin|, it has the following relationship:






{circumflex over (P)}
i
+
={circumflex over (P)}
i
+σE
custom-character
=0,{circumflex over (P)}i=σEcustom-character=0.





Therefore,






{circumflex over (P)}
i
={circumflex over (P)}
i
+
−{circumflex over (P)}
i
.


A scaling function is set to be








ϒ

(
x
)

=


γ


1
2



(

1
-

sgn
(
x
)


)



·
x


,




wherein γ∈[0,1] is a scaling coefficient, then there is a form as follows:







cons


=




1
2



F
T



P
i
+


F

-


1
2



F
up
T



ϒ

(

P
i
-

)


F

+


q
i
T


F

+
c



cons
.






In S4.3, the convex model is obtained according to the objective function of the matrix form and the constraint condition of the matrix form as follows:







min


O

=



F
T


EF

-


F
up
T



F
up









s
.
t
.

{







1
2



F
T



P
i
+


F

+


1
2



F
up
T



ϒ

(

P
i
-

)


F

+


q
i
T


F

+
c


0








(


E
1

+

E
2

+

E

n
-
1


+

E
n


)


F

=
b












i
=
3

,
4
,


,

n
-
2

,




wherein F represents an optimal speed to be solved, E represents a unit matrix, Pi+ represents a matrix with a positive eigenvalue after decomposition of an ith quadratic matrix Pi, Pi represents a matrix with a negative eigenvalue after decomposition of the ith quadratic matrix Pi, the matrix Pi refers to specific embodiments, and qi, b and c represent constant vectors.


In S5, the convex model is solved based on a quadratic sequence planning method to obtain a final speed curve.


Specifically, the quadratic sequence planning method is used for solution, inequality constraint is forcibly changed into equality constraint first, then the objective function and all equality constraints are constructed into an unconstrained Lagrange's equation, the Lagrange's equation is solved by using an extreme value condition of a multivariate function, and a result is solved by spline curve fitting to obtain the final speed curve.


As shown in FIG. 2, a time optimal speed planning system based on constraint classification comprises:


a path curve module configured for reading path information and carrying out curve fitting to obtain a path curve;


a static constraint module configured for sampling the path curve, and considering static constraint to obtain a static upper bound value of a speed curve;


a dynamic constraint module configured for considering dynamic constraint, and combining the static upper bound value of the speed curve to construct a time optimal speed model;


a model transformation module configured for carrying out convex transformation on the time optimal speed model to obtain a convex model; and


a solving module configured for solving the convex model based on a quadratic sequence planning method to obtain a final speed curve.


All the contents in the above method embodiments are applicable to the system embodiments, the specific functions realized by the system embodiments are the same as those realized by the method embodiments above, and the beneficial effects achieved by the system embodiments are the same as those achieved by the method embodiments above.


The above describes the preferred embodiments of the present invention in detail, but the present invention is not limited to the embodiments. Those skilled in the art may further make various equivalent modifications or substitutions without violating the spirit of the present invention, and these equivalent modifications or substitutions are all included in the scope defined by the claims of the present application.

Claims
  • 1. A time optimal speed planning method based on constraint classification, comprising the following steps of: reading path information and carrying out curve fitting to obtain a path curve;sampling the path curve, and considering static constraint to obtain a static upper bound value of a speed curve;considering dynamic constraint, and combining the static upper bound value of the speed curve to construct a time optimal speed model;transforming an objective function of the time optimal speed model into a matrix form to obtain an objective function of the matrix form;transforming a constraint condition into a matrix form through matrix decomposition and a preset scaling function to obtain a constraint condition of the matrix form,wherein the constraint condition of the matrix form is expressed as follows:
  • 2. The time optimal speed planning method based on constraint classification according to claim 1, wherein a formula of the curve fitting is expressed as follows:
  • 3. The time optimal speed planning method based on constraint classification according to claim 2, wherein the step of considering the dynamic constraint, and combining the static upper bound value of the speed curve to construct the time optimal speed model, specifically comprises: substituting the static upper bound value of the speed curve into a dynamic constraint equation set point by point, and constructing an initial model with time optimum as the objective function; andtransforming the initial model according to a feed speed and the static upper bound value to obtain the time optimal speed model.
  • 4. The time optimal speed planning method based on constraint classification according to claim 3, wherein a formula of the time optimal speed model is expressed as follows:
  • 5. A time optimal speed planning system based on constraint classification, comprising: a path curve module configured for reading path information and carrying out curve fitting to obtain a path curve;a static constraint module configured for sampling the path curve, and considering static constraint to obtain a static upper bound value of a speed curve;a dynamic constraint module configured for considering dynamic constraint, and combining the static upper bound value of the speed curve to construct a time optimal speed model;a model transformation module configured for transforming an objective function of the time optimal speed model into a matrix form to obtain an objective function of the matrix form; transforming a constraint condition into a matrix form through matrix decomposition and a preset scaling function to obtain a constraint condition of the matrix form; and obtaining a convex model according to the objective function of the matrix form and the constraint condition of the matrix form,wherein the constraint condition of the matrix form is expressed as follows:
Priority Claims (1)
Number Date Country Kind
202111505094.4 Dec 2021 CN national