CN109991934A - A kind of time optimal online S type deceleration planning method - Google Patents
A kind of time optimal online S type deceleration planning method Download PDFInfo
- Publication number
- CN109991934A CN109991934A CN201910178004.1A CN201910178004A CN109991934A CN 109991934 A CN109991934 A CN 109991934A CN 201910178004 A CN201910178004 A CN 201910178004A CN 109991934 A CN109991934 A CN 109991934A
- Authority
- CN
- China
- Prior art keywords
- acceleration
- type
- time
- curve
- displacement
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/416—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control of velocity, acceleration or deceleration
Abstract
The invention discloses a kind of time optimal online S type deceleration planning methods, this method has the characteristics that can be realized to be calculated in real time online, and the interpolation track for meeting kinematic constraint condition can be cooked up according to arbitrary initial motion state and the null end state of acceleration.The process for simplifying off-line calculation can be realized the signal automatic programming movement track in real time returned under circumstances not known according to sensor.
Description
Technical field
The present invention relates to industrial robot motion control field, servo motor speed control field, mobile robot exists
Line traffic control is more particularly to the online S type deceleration planning method in terms of speed planning method and robot motion planning.
Background technique
Traditional method for planning track does not consider the initial acceleration of initial motion state usually, and initial velocity is not equal to zero
Situation, because the case where zero is not equal to for initial motion state trajectory planning and its difficulty, and complexity is calculated, it is not able to satisfy
The requirement of real-time.For high-performance robot control system, no matter which kind of motion state is current robot be in, and passes through
It is inputted external sensor signal as robot controller, robot all should make a response immediately to input signal.
Summary of the invention
In order to overcome traditional method for planning track that cannot carry out track rule according to arbitrary initial motion state and end state
It draws.The present invention provides a kind of time optimal online S type deceleration planning method, and this method can be used in oneself of robot trajectory
Dynamic online generation.Under conditions of considering speed, acceleration and acceleration constraint condition and robot motion's whole story state, with
Time is most short for optimization aim progress direct solution.
The purpose of the present invention is what is be achieved through the following technical solutions:
A kind of time optimal online S type deceleration planning method, comprising the following steps:
(1) parameter required for Input Online motion planning: input first and last motion state and kinematic constraint condition, first and last fortune
Dynamic state includes initial position Ps, initial velocity Vs, initial acceleration As, end position Ptrgt, terminate speed Vtrgt, kinematic constraint
Condition includes maximum acceleration Jmax, peak acceleration Amax, maximum speed Vmax；
(2) do not consider that displacement calculates initial acceleration curve type Typea *: the parameter inputted according to step (1) is not being examined
In the case where considering displacement constraint, meets other constraint conditions and calculate initial acceleration curve type Typea *；
First determining whether to be changed to by initial velocity in the shortest time terminates in rate process, and whether acceleration A, which needs, reaches
To the limit；If not needing to reach the limit acceleration, Type is calculated according to first and last motion statea *, by initial acceleration curve
Type Typea *It is divided into four kinds of situations；If acceleration needs reach the limit values, Type is calculated also according to first and last motion statea *?
Corresponding four kinds of situations；It is selected among above-mentioned eight kinds of total situations a kind of as Typea *；
(3) Type returned by step (2)a *Calculate initial jerk Js；The Type returned first against step (2)a *
Calculate Typea *The displacement P of corresponding accelerating curve*, convenience of calculation is considered, by above-mentioned eight kinds of Typea *It is divided into four classes；
(4) S type accelerating curve type Type is determineda；If the Δ P that step (3) calculates*Equal to displacement of targets Δ P, then
TypeaEqual to Typea *；Otherwise the J calculated by step (3) is neededsCalculate Typea；To plan total time most short principle, consideration is
It is no that S type accelerating curve type is divided into eight there are even acceleration region, with the presence or absence of at the uniform velocity region and acceleration and deceleration transition number
Kind, it is selected among eight kinds of S type accelerating curve types by following principle a kind of as Typea；Judgment principle of classifying is as follows:
Principle A: it is assumed that during motion planning, speed reaches speed limit just and finally reaches target velocity, than
Displacement and realistic objective displacement more at this time；
Principle B: it is assumed that during motion planning, acceleration reaches capacity just and finally reaches target velocity, compares
Displacement and realistic objective displacement at this time；
Principle C: it is assumed that during motion planning, acceleration has reached the limit, and speed reaches capacity just and finally reaches
To target velocity, compare displacement and realistic objective displacement at this time；
Principle D: it is assumed that during motion planning, acceleration and speed all reach capacity just, compare speed at this time
With the size of realistic objective speed；
Principle E: it is assumed that during motion planning, acceleration reaches capacity just, and finally reaches target velocity, compares
There are also displacements and realistic objective displacement at this time for the size of speed and realistic objective speed at this time；
By the combined use of above several principles, one kind finally is selected from above eight kinds of S type accelerating curve type
It is most short to meet the overall planning time, and kinematic constraint condition can be met simultaneously；
(5) it calculates accelerating curve segmentation parameter: Type is calculated by step (4)a, arranged for different accelerating curves
Different equation groups are write to be solved to obtain each corresponding time of accelerating curve；
Accelerating part time and deceleration part time are passed through non-linear side respectively by the case where for uniform motion is not present
Journey group solves；For the case where there are even acceleration or even braking sections, centainly reach velocity restraint condition in motion process, i.e., it is intermediate
Velocity limits VpIt is known that calculation method is identical, is at this time system of linear equations then and there is no even acceleration or the case where even deceleration；
To solve acceleration time, deceleration time and at the uniform velocity time；Accelerating curve segmentation parameter [t is obtained eventually1a, t1b, t1c, t2a,
t2b, t2c, totg]；
(6) motion profile is generated according to accelerating curve segmentation parameter: by accelerating curve type TypeaWith step (5)
The accelerating curve segmentation parameter of calculating substitutes into S type acceleration and deceleration curves formula and produces motion profile.
Further, in step (2), if not needing to reach the limit acceleration, speed is judged according to first and last motion state
Variation tendency is spent by initial acceleration curve type Typea *It is divided into four kinds of situations, corresponding four kinds of situations are respectively as follows: a, initially add
Rate curve first accelerates acceleration and deceleration again, is denoted as+PosTri；B, initial acceleration curve elder generation acceleration and deceleration are slowed down again, be denoted as-
NegTri；C, initial acceleration curve first slows down, then plus accelerate then subtract acceleration, be denoted as-PosTri；D, initial acceleration is write music
Line first subtracts acceleration, then then acceleration and deceleration are slowed down ,+NegTri is denoted as；
The case where reaching the limit values, is needed for acceleration, velocity variations trend is judged according to first and last motion state, it is corresponding
Four kinds of situations are as follows: e, initial acceleration curve first accelerate, then even acceleration finally subtracts acceleration, is denoted as+PosTrap；F, initial to add
The acceleration and deceleration of rate curve elder generation, then the last deceleration of even deceleration, are denoted as-NegTrap；G, initial acceleration curve first slows down, then
Accelerate then even acceleration, finally subtracts acceleration, be denoted as-PosTrap；H, initial acceleration curve first subtracts acceleration, then acceleration and deceleration are right
Even deceleration afterwards finally slows down, is denoted as+NegTrap.
Further, in step (3), in the displacement P for calculating corresponding accelerating curve*Aspect, by eight kinds of Typea *Point
For four classes, wherein+PosTri ,-NegTri are a kind of ,-PosTri ,+NegTri are a kind of ,+PosTrap ,-NegTrap it is a kind of and-
PosTrap ,+NegTrap are a kind of；
If displacement of targets Δ P is equal to Δ P*, then TypeaEqual to Typea *；If displacement of targets Δ P is not equal to Δ P*, then
Typea *In vain；If displacement of targets Δ P is greater than Δ P*, then JsEqual to Jmax；If displacement of targets Δ P is less than Δ P*, then JsEqual to-
Jmax；It is more than speed Maximum Constraint that second class and the 4th class, which are subtracted acceleration, subtract decelerating area there may be speed, is advised at this time
Determine Js=0, i.e. input parameter error, can not plan under kinematic constraint condition.
Further, planning total time most short principle is considered in step (4), by accelerating curve type TypeaIt is divided into eight
Kind include: TriTri, TrapTri, TriTrap, TrapTrap, TriZeroTri, TrapZeroTri, TriZeroTrap,
TrapZeroTrap；Wherein Tri is indicated first plus acceleration subtracts acceleration again or first acceleration and deceleration are slowed down again；Trap indicates that there are even acceleration
Or the case where even deceleration, i.e., first plus accelerate, then even acceleration subtracts accelerator；Zero indicates that acceleration is equal to zero, i.e. at the uniform velocity section mistake
Journey.
Compared with prior art, the beneficial effects brought by the technical solution of the present invention are as follows:
1. the method for the present invention can be according to meeting kinematic constraint condition (Jmax, Amax, Vmax) arbitrary initial motion state (As,
Vs, Ps) and consider to terminate the motion state (A that acceleration is zerotrgt, Vtrgt, Ptrgt) S type deceleration planning is carried out, it generates and meets
Kinematic constraint condition and time optimal unique S type acceleration and deceleration speed trajectory.
2. the various situations of the invention for considering S type acceleration and deceleration curves in detail, greatly simplify the S under any original state
The derivation algorithm of type acceleration and deceleration curves can calculate S type acceleration and deceleration trajectory parameters within the Microsecond grade time and generate motion profile,
Real-time is higher, is suitable for online real-time planned trajectory.Since the track of planning is optimal in time, therefore work effect can be improved
Rate.
3. the present invention can be according to the real-time programming movement track of sensor return signal.For under circumstances not known robot from
Dynamic programming movement path provides a method.Help to realize the intelligence and flexibility of robot.
Detailed description of the invention
Fig. 1 shows time optimal online S type deceleration planning Method And Principle schematic diagrames.
Fig. 2 indicates not considering eight kinds of initial acceleration curve type Type in the case of displacement of targetsa *Schematic diagram.It is wherein horizontal
Axis is time t, and the longitudinal axis is acceleration A；1 ,+PosTri in figure, 2 ,-NegTri, 3 ,-PosTri, 4 ,+NegTri, 5 ,+
PosTrap, 6 ,-NegTrap, 7 ,-PosTrap, 8 ,+PosTrap.
Fig. 3 indicates eight kinds of S type accelerating curve type TypeaSchematic diagram.(As∈[-Amax, Amax], Js=± Jmax)；Its
Middle horizontal axis is time t, and the longitudinal axis is acceleration A, 1, TriTri in figure, 2, TrapTri, 3, TriTrap, 4, TrapTrap, 5,
TriZeroTri, 6, TrapZeroTri, 7, TriZeroTrap, 8, TrapZeroTrap.
Fig. 4 (a) to Fig. 4 (f) indicates the result figure under some experiment parameters.Wherein abscissa is time t, Fig. 4 (a) to figure
The ordinate of 4 (f) upper, middle and lower subgraph is respectively acceleration, speed and displacement.
Specific embodiment
Below in conjunction with the drawings and specific embodiments, invention is further described in detail.It should be appreciated that this place is retouched
The specific embodiment stated is only used to explain the present invention, is not intended to limit the present invention.
Linear velocity planing method provided by the invention.It includes kinematic constraint condition that it, which inputs parameter, first and last motion state, and
Interpolation time dt.Kinematic constraint condition includes maximum acceleration, peak acceleration and maximum speed, is denoted as (Jmax, Amax,
Vmax).Initial motion state includes initial acceleration, initial velocity, and initial displacement is denoted as (As, Vs, Ps).Initial motion state
Physical condition should be met, otherwise motion planning may exceed kinematic constraint condition.
It is zero that end motion state, which includes aimed acceleration, and target velocity, displacement of targets is denoted as (Atrgt, Vtrgt, Ptrgt)。
Its output parameter includes planning optimal time totg, according to the interpolation time generate real-time acceleration J (t), acceleration A (t),
Speed V (t), and displacement P (t).
Online S type deceleration planning method and technology scheme is specific as shown in Figure 1, being the detailed description to Fig. 1 below.:
Step 1, parameter required for Input Online motion planning:
First and last motion state is inputted first in the form of array and kinematic constraint condition, first and last motion state include initial bit
Set Ps, initial velocity Vs, initial acceleration As, end position Ptrgt, terminate speed Vtrgt, constraint condition includes maximum acceleration
Jmax, peak acceleration Amax, maximum speed value Vmax.It is denoted as INPUT=[Ps, Vs, As, Ptrgt, Vtrgt, Vmax, Amax, Jmax]。
Step 2, do not consider that displacement calculates initial acceleration curve type Typea *:
According to input parameter, in the case where not considering displacement, the initial acceleration that calculating meets other constraint conditions is write music
Line type Typea *。
Speed is changed in end rate process by initial velocity in the shortest time for judgement, and whether acceleration A, which needs, reaches
To acceleration limiting.If not needing to reach the limit acceleration, Type is calculated according to movement first and last statea *。
Do not consider the initial acceleration curve type Type of displacementa *It is divided into four kinds of situations, as shown in Figure 2.Wherein 1, speed
Curve first accelerates acceleration and deceleration again, is denoted as+PosTri；2, rate curve elder generation acceleration and deceleration are slowed down again, are denoted as-NegTri；3, fast
Line of writing music first slows down, then plus accelerate then subtract acceleration, be denoted as-PosTri；4, rate curve first subtracts acceleration, then acceleration and deceleration are then
Slow down, is denoted as+NegTri.
As the difference Δ V and initial acceleration A of first and last speedsSymbol is different, and | Δ V | when dropping to zero greater than initial acceleration
Otherwise corresponding velocity variable Shi Zewei+PosTri or-NegTri is-PosTri or+NegTri.
The case where reaching the limit values, is needed for acceleration, calculates Type also according to movement first and last statea *, equally
Corresponding four kinds of situations are shown in Fig. 2.5, rate curve first accelerates, then even acceleration finally subtracts acceleration, is denoted as+PosTrap；6, speed is bent
The acceleration and deceleration of line elder generation, then the last deceleration of even deceleration, are denoted as-NegTrap；7, rate curve first slows down, then plus accelerates then even
Accelerate, finally subtracts acceleration, be denoted as-PosTrap；8, rate curve first subtracts acceleration, then acceleration and deceleration then even deceleration, finally subtracts
Speed is denoted as+NegTrap.
Ibid, as the difference Δ V of first and last speed and initial acceleration AsSymbol is different, and | Δ V | it is greater than initial acceleration and drops
Corresponding velocity variable Shi Zewei+PosTrap or-NegTrap, is otherwise-PosTrap or+NegTrap when to zero.In this way
1 kind of initial acceleration curve type Type can be selected from eight kinds of situations of Fig. 2a *.It is specifically shown in step 3 explanation.
Step 3, the Type returned by step 2a *Calculate initial jerk Js:
According to Typea *Calculate Typea *The displacement P of corresponding accelerating curve*.Eight kinds of initial acceleration class of a curve above
Type can be divided into four classes in terms of calculating displacement, i.e.+PosTri ,-NegTri is a kind of, and-PosTri ,+NegTri are a kind of ,+
PosTrap ,-NegTrap are a kind of, and-PosTrap ,+NegTrap are a kind of.
The Type calculated according to step 2a *, calculate Typea *The displacement P of corresponding accelerating curve*.If displacement of targets
Δ P is equal to Δ P*, then TypeaEqual to Typea *；If displacement of targets Δ P is not equal to Δ Pmin, then Typea *In vain；If target position
It moves Δ P and is greater than Δ P*, then JsEqual to Jmax；If displacement of targets Δ P is less than Δ P*, then JsEqual to-Jmax.For the 2nd class and the 4th class
Midrange speed may be more than speed Maximum Constraint, provide J at this times=0, i.e. input parameter error, can not be in kinematic constraint
Under the conditions of plan.
Specifically, the number of the intersection point of accelerating curve described in the present embodiment and time shaft is not consider initial acceleration
Null special circumstances.As shown in Fig. 2, wherein the first width figure can be expressed as+PosTri or-NegTri, including initially add
Speed is greater than zero, and initial jerk is greater than zero, initial acceleration less than zero, and the minus situation of initial jerk is special
Point is that acceleration maximum value does not reach acceleration upper limit value, and accelerating curve and time shaft intersection point only one.Second width figure can
To be expressed as-PosTri or+NegTri, including initial acceleration less than zero, it is big that initial jerk is greater than zero, initial acceleration
In zero, the minus situation of initial jerk, its main feature is that the intersection point of accelerating curve and time shaft there are two and most greatly
Speed is less than acceleration upper limit value.Third width figure can be expressed as+PosTrap or-NegTrap, including initial velocity and initial
Acceleration symbol is identical and intermediate certain moment acceleration has reached acceleration upper limit value, and accelerating curve and time shaft only have one
A intersection point.4th width figure can be expressed as-PosTrap or+PosTrap, including initial acceleration and initial jerk symbol
Difference, certain intermediate moment acceleration reached the acceleration upper limit and accelerating curve and time shaft intersection point there are two.
Step 4, S type accelerating curve type Type is determineda:
If the Δ P that step (3) calculates*Equal to displacement of targets Δ P, then TypeaEqual to Typea *；Otherwise it needs to be counted by step 3
The J of calculationsCalculate Typea.Planning total time most short principle is considered, by TypeaBe divided into eight kinds of TriTri, TrapTri, TriTrap,
TrapTrap, TriZeroTri, TrapZeroTri, TriZeroTrap, TrapZeroTrap, it is as shown in Figure 3 respectively.
Eight kinds of S type accelerating curve type shown in Fig. 3 contains several situations shown in Fig. 2 simultaneously.The wherein first kind
Curve can be indicated with TriTri, its main feature is that initial acceleration is identical as initial jerk symbol, peak acceleration is less than
There are two acceleration upper limit value, accelerating curve and time shaft intersection points and the time interval of acceleration identically vanishing is not present.The
Two class curves can be indicated with TrapTri, its main feature is that initial acceleration is identical as initial jerk symbol, acceleration region
Moment peak acceleration has reached acceleration upper limit value, accelerating curve and time shaft intersection point there are two and there is no acceleration it is identical
In zero time interval.Third class curve can be indicated with TriTrap, its main feature is that initial acceleration and initial jerk accord with
Number identical, decelerating area moment peak acceleration has reached acceleration upper limit value, and there are two accelerating curves and time shaft intersection point
And the time interval of acceleration identically vanishing is not present.4th class curve can be indicated with TrapTrap, its main feature is that initial add
Speed is identical as initial jerk symbol, and peak acceleration has reached the acceleration upper limit at a certain moment respectively with decelerating area for acceleration
There are two value, accelerating curve and time shaft intersection points and the time interval of acceleration identically vanishing is not present.5th class acceleration
Curve can be expressed as TriZeroTri, relative to first kind accelerating curve it is characterized in that, its there are acceleration be equal to zero
Time interval, that is, there is uniform motion section.6th class accelerating curve is expressed as TrapZeroTri, relative to second
The obvious characteristic of kind accelerating curve is that it includes uniform motion section.7th kind of accelerating curve can be expressed as
TriZeroTrap, the obvious characteristic for being different from the third accelerating curve are interlude there are uniform motion sections.8th
Kind accelerating curve can be expressed as TrapZeroTrap, and the obvious characteristic for being different from the 4th kind of accelerating curve is centre
There is at the uniform velocity section in section.
Choose the classification judgment principle of accelerating curve type:
Principle A: it is assumed that during motion planning, speed reaches speed limit just and finally reaches target velocity, than
Displacement and realistic objective displacement more at this time.
Principle B: it is assumed that during motion planning, acceleration reaches capacity just and finally reaches target velocity, compares
Displacement and realistic objective displacement at this time.
Principle C: it is assumed that during motion planning, acceleration has reached the limit, and speed reaches capacity just and finally reaches
To target velocity, compare displacement and realistic objective size at this time.
Principle D: it is assumed that during motion planning, acceleration and speed all reach capacity just, compare speed at this time
With the size of realistic objective speed.
Principle E: it is assumed that during motion planning, acceleration reaches capacity just, and finally reaches target velocity, compares
There are also displacements and realistic objective displacement at this time for the size of speed and realistic objective speed at this time.
By the combined use of above a variety of principles, a kind of curve may finally be selected from above 8 kinds of accelerating curves
It is most short to meet the overall planning time, and kinematic constraint condition can be met simultaneously.
With initial jerk Js> 0 and acceleration reach just peak acceleration then acceleration be reduced to zero process
Speed is greater than for speed maximum value.
Judge 1: if initial velocity is less than final goal speed, entering and judge 2.
Judge 2: being judged by principle A, if accelerating curve is TriTri, compares displacement and realistic objective displacement at this time
Size.The Type if displacement at this time is less than realistic objective displacementaEqual to TriZeroTri.It otherwise is TriTri.If be not inconsistent
Conjunction judges that 1 condition then enters and judges 3.
Judge 3: if deceleration part minimum acceleration reaches acceleration lower limit value and final speed reaches target velocity,
Whether judgement at the uniform velocity section speed is less than limitation.If judging, 3 are less than limitation, enter and judge 4.
Judge 4: being judged by principle B, it is assumed that accelerating curve type is TriTri, compares final mean annual increment movement and realistic objective position
Move size.The Type if displacement at this time is greater than realistic objective displacementaEqual to TriTri, otherwise enters and judge 5.
Judge 5: judging whether final speed is greater than target velocity by principle D.If being less than target velocity, TypeaIt is equal to
Otherwise TriZeroTri enters and judges 6.
Judge 6: equally by the displacement of principle A alternative plans and displacement of targets size, if planning displacement is greater than displacement of targets,
TypeaIt is otherwise TriZeroTrap equal to TriTrap.
If judging, 3 are more than or equal to limitation, enter and judge 7.
Judge 7: by principle A, deceleration part is Tri type, compares displacement and realistic objective displacement at this time, if more than
Displacement of targets, then TypeaIt is otherwise TriZeroTri equal to TriTri.
Step 5, accelerating curve segmentation parameter is calculated:
Type is calculated by step 4a, write different equation groups for different accelerating curves column and solved and accelerated
The each corresponding time of line of writing music.
For the case where there is no even acceleration or even decelerations, accelerating part time and deceleration part time are used into t respectively10,
And t21It indicates, above eight kinds of accelerating curves can be reduced to 5 universal equations and 2 difference equations in this way, wherein there is 7
Unknown quantity.According to equation solution t10And t21。
With TypeaFor TriTri, intermediate variable S is introduced1、S2、ΔP1a、ΔP1b、Vp、tb.Wherein VpFor intermediate speed
Spend extreme value.
tb=-| As/J|
Universal equation
S1=Vp-Vs
S2=Vtrgt-Vp
ΔP1a=(Jstb 3/6+tbVb)+(t10-tb)(Vb+Vp)/2
ΔP1b=Δ P-t21(Vp+Vtrgt)/2
ΔP1a-ΔP1b=0
Distinguish equation
For the case where there are even acceleration or even braking sections, centainly reach velocity restraint condition in motion process, i.e., it is intermediate
Velocity limits VpIt is known that it is then identical as calculation method the case where not depositing then even acceleration or even deceleration, it can be added at this time with direct solution
Fast part-time t10With deceleration time t21With even plus (subtracting) fast time tm.According to t10, t21, tmIt can be in the hope of accelerating curve point
Section parameter
[t1a, t1b, t1c, t2a, t2b, t2c, totg]
Wherein t1aIt indicates plus accelerating sections ends up the moment, t1bIndicate even accelerating sections ending moment, t1cWhen to subtract accelerating sections ending
It carves, t2aIt ends up the moment at the uniform velocity section, t2bIt ends up the moment for accelerating and decelerating part, t2cIt ends up the moment for even braking section, totgTo slow down
The section ending moment.
Step 6, motion profile is generated according to accelerating curve segmentation parameter:
Then by accelerating curve type TypeaThe acceleration and deceleration of S type are substituted into the accelerating curve segmentation parameter that step 5 calculates
Locus formula generates track J (t), A (t), V (t), P (t).
S type acceleration and deceleration curves formula is as follows:
Specific track is as follows:
0≤t of first segment≤t1a,
Second segment t1a< t≤t1b, wherein P1a、V1a、A1aThe respectively displacement of joint first segment end, speed, acceleration,
Third section t1b< t≤t1c, wherein P1b、V1b、A1bThe respectively displacement of joint second segment end, speed, acceleration
4th section of t1c< t≤t2a, wherein P1c、V1c、A1cThe respectively displacement of joint third section end, speed, acceleration
P (t)=P1c+V1c(t-t1c)
5th section of t2a< t≤t2b, wherein P2a、V2a、A2aThe respectively displacement of the 4th section of end in joint, speed, acceleration
6th section of t2b< t≤t2c, wherein P2b、V2b、A2bThe respectively displacement of the 5th section of end in joint, speed, acceleration
7th section of t2c< t≤totg, wherein P2c、V2c、A2cThe respectively displacement of the 6th section of end in joint, speed, acceleration
J (t), A (t), V (t) are respectively equal to P (t) and ask three rank second orders and single order derived function to t.
It is test parameter below:
INPUT=[Ps, Vs, As, Ptrgt, vtrgt, Vmax, Amax, Jmax]
INPUT1=[0,100,50,500, -100,250,100,100]
INPUT2=[0,100,50,500, -100,200,100,100]
INPUT3=[0,100,50,500, -100,250,150,100]
INPUT4=[0,100,50,500, -100,250,200,100]
INPUT5=[0,100,50,500, -100,200,150,100]
INPUT6=[0,100,50,500, -100,200,200,100]
Fig. 4 (a) to Fig. 4 (f) show this method planning time optimal S type acceleration and deceleration trajectory planning as a result, more than 6 groups
The corresponding geometric locus of parameter is inputted respectively as Fig. 4 (a), Fig. 4 (b), Fig. 4 (c), Fig. 4 (d), Fig. 4 (e) and Fig. 4 (f) are shown.
The present invention is not limited to embodiments described above.Above the description of specific embodiment is intended to describe and say
Bright technical solution of the present invention, the above mentioned embodiment is only schematical, is not restrictive.This is not being departed from
In the case of invention objective and scope of the claimed protection, those skilled in the art may be used also under the inspiration of the present invention
The specific transformation of many forms is made, within these are all belonged to the scope of protection of the present invention.
Claims (4)
1. a kind of time optimal online S type deceleration planning method, which comprises the following steps:
(1) parameter required for Input Online motion planning: input first and last motion state and kinematic constraint condition, first and last move shape
State includes initial position Ps, initial velocity Vs, initial acceleration As, end position Ptrgt, terminate speed Vtrgt, kinematic constraint condition
Including maximum acceleration Jmax, peak acceleration Amax, maximum speed Vmax；
(2) do not consider that displacement calculates initial acceleration curve type Typea *: the parameter inputted according to step (1) is not considering position
In the case where moving constraint, meets other constraint conditions and calculate initial acceleration curve type Typea *；
First determining whether to be changed to by initial velocity in the shortest time terminates in rate process, and whether acceleration A needs to reach pole
Limit；If not needing to reach the limit acceleration, Type is calculated according to first and last motion statea *, by initial acceleration curve type
Typea *It is divided into four kinds of situations；If acceleration needs reach the limit values, Type is calculated also according to first and last motion statea *Also it corresponds to
Four kinds of situations；It is selected among above-mentioned eight kinds of total situations a kind of as Typea *；
(3) Type returned by step (2)a *Calculate initial jerk Js；The Type returned first against step (2)a *It calculates
Typea *The displacement P of corresponding accelerating curve*, convenience of calculation is considered, by above-mentioned eight kinds of Typea *It is divided into four classes；
(4) S type accelerating curve type Type is determineda；If the Δ P that step (3) calculates*Equal to displacement of targets Δ P, then TypeaDeng
In Typea *；Otherwise the J calculated by step (3) is neededsCalculate Typea；To plan total time most short principle, consider whether exist
S type accelerating curve type is divided into eight kinds with the presence or absence of at the uniform velocity region and acceleration and deceleration transition number by even acceleration region, is led to
Cross following principle selected among eight kinds of S type accelerating curve types it is a kind of as Typea；Judgment principle of classifying is as follows:
Principle A: it is assumed that during motion planning, speed reaches speed limit just and finally reaches target velocity, compares this
When displacement and realistic objective displacement；
Principle B: it is assumed that during motion planning, acceleration reaches capacity just and finally reaches target velocity, compares at this time
Displacement and realistic objective displacement；
Principle C: it is assumed that during motion planning, acceleration has reached the limit, and speed reaches capacity just and is finally reached mesh
Speed is marked, displacement and realistic objective displacement at this time is compared；
Principle D: it is assumed that during motion planning, acceleration and speed all reach capacity just, compare speed and reality at this time
The size of border target velocity；
Principle E: it is assumed that during motion planning, acceleration reaches capacity just, and finally reaches target velocity, compares at this time
Speed and the size of realistic objective speed there are also displacements and realistic objective displacement at this time；
By the combined use of above several principles, a kind of satisfaction finally is selected from above eight kinds of S type accelerating curve type
The overall planning time is most short, and can meet kinematic constraint condition simultaneously；
(5) it calculates accelerating curve segmentation parameter: Type is calculated by step (4)a, write not for different accelerating curves column
It is solved to obtain each corresponding time of accelerating curve with equation group；
The case where for uniform motion is not present, accelerating part time and deceleration part time are passed through into Nonlinear System of Equations respectively
It solves；For the case where there are even acceleration or even braking sections, centainly reach velocity restraint condition, i.e. midrange speed in motion process
Extreme value VpIt is known that calculation method is identical, is at this time system of linear equations then and there is no even acceleration or the case where even deceleration；To
Solve acceleration time, deceleration time and at the uniform velocity time；Accelerating curve segmentation parameter [t is obtained eventually1a,t1b,t1c,t2a,t2b,t2c,
totg]；
(6) motion profile is generated according to accelerating curve segmentation parameter: by accelerating curve type TypeaIt is calculated with step (5)
Accelerating curve segmentation parameter substitutes into S type acceleration and deceleration curves formula and produces motion profile.
2. a kind of time optimal online S type deceleration planning method according to claim 1, which is characterized in that step (2)
In, if not needing to reach the limit acceleration, judge that velocity variations trend writes music initial acceleration according to first and last motion state
Line type Typea *It is divided into four kinds of situations, corresponding four kinds of situations are respectively as follows: a, initial acceleration curve first accelerates to add and subtract again
Speed is denoted as+PosTri；B, initial acceleration curve elder generation acceleration and deceleration are slowed down again, are denoted as-NegTri；C, initial acceleration curve
First slow down, then plus accelerate then subtract acceleration, be denoted as-PosTri；D, initial acceleration curve first subtracts acceleration, then acceleration and deceleration are then
Slow down, is denoted as+NegTri；
The case where reaching the limit values, is needed for acceleration, velocity variations trend is judged according to first and last motion state, it is four kinds corresponding
Situation are as follows: e, initial acceleration curve first accelerate, then even acceleration finally subtracts acceleration, is denoted as+PosTrap；F, initial acceleration
The acceleration and deceleration of curve elder generation, then the last deceleration of even deceleration, are denoted as-NegTrap；G, initial acceleration curve first slows down, then plus adds
Fast and then even acceleration, finally subtracts acceleration, is denoted as-PosTrap；H, initial acceleration curve first subtracts acceleration, then acceleration and deceleration are then even
Slow down, finally slows down, be denoted as+NegTrap.
3. a kind of time optimal online S type deceleration planning method according to claim 1, which is characterized in that step (3)
In, in the displacement P for calculating corresponding accelerating curve*Aspect, by eight kinds of Typea *It is divided into four classes, wherein+PosTri ,-
NegTri is a kind of, and-PosTri ,+NegTri are a kind of, and+PosTrap ,-NegTrap are a kind of and-PosTrap ,+NegTrap are a kind of；
If displacement of targets Δ P is equal to Δ P*, then TypeaEqual to Typea *；If displacement of targets Δ P is not equal to Δ Pmin, then
Typea *In vain；If displacement of targets Δ P is greater than Δ P*, then JsEqual to Jmax；If displacement of targets Δ P is less than Δ P*, then JsEqual to-
Jmax；It is more than speed Maximum Constraint that second class and the 4th class, which are subtracted acceleration, subtract decelerating area there may be speed, is advised at this time
Determine Js=0, i.e. input parameter error, can not plan under kinematic constraint condition.
4. a kind of time optimal online S type deceleration planning method according to claim 1, which is characterized in that step (4)
Total time most short principle is planned in middle consideration, by accelerating curve type TypeaBe divided into eight kinds include: TriTri, TrapTri,
TriTrap,TrapTrap,TriZeroTri,TrapZeroTri,TriZeroTrap,TrapZeroTrap；Wherein Tri is indicated
First plus acceleration subtracts acceleration again or first acceleration and deceleration are slowed down again；Trap indicates the case where there are even acceleration or even decelerations, i.e., first adds
Speed, then even acceleration subtract accelerator；Zero indicates that acceleration is equal to zero, i.e. at the uniform velocity section process.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910178004.1A CN109991934B (en) | 2019-03-10 | 2019-03-10 | Time-optimal online S-type acceleration and deceleration planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910178004.1A CN109991934B (en) | 2019-03-10 | 2019-03-10 | Time-optimal online S-type acceleration and deceleration planning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109991934A true CN109991934A (en) | 2019-07-09 |
| CN109991934B CN109991934B (en) | 2021-02-09 |
Family
ID=67129604
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910178004.1A Active CN109991934B (en) | 2019-03-10 | 2019-03-10 | Time-optimal online S-type acceleration and deceleration planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109991934B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112498355A (en) * | 2020-11-02 | 2021-03-16 | 浙江吉利控股集团有限公司 | Speed planning method and device |
| WO2021175487A1 (en) * | 2020-03-03 | 2021-09-10 | Sew-Eurodrive Gmbh & Co. Kg | Method for decelerating an object and machine |
| CN113703399A (en) * | 2021-08-26 | 2021-11-26 | 广东拓斯达科技股份有限公司 | Motion trajectory planning method, device, equipment and storage medium |
| CN115963782A (en) * | 2023-01-31 | 2023-04-14 | 北京立迈胜控制技术有限责任公司 | S-shaped velocity planning method based on non-zero initial acceleration |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2004258700A (en) * | 2003-02-24 | 2004-09-16 | Keyence Corp | Method of setting synchronous curve for positioning system and computer program therefor |
| CN101477354A (en) * | 2009-01-19 | 2009-07-08 | 东元总合科技（杭州）有限公司 | Position S type instruction generation method |
| CN103713581A (en) * | 2013-12-12 | 2014-04-09 | 南京工程学院 | Method for realizing S curve acceleration and deceleration of motion control system |
| JP5941998B2 (en) * | 2013-01-21 | 2016-06-29 | 国立大学法人横浜国立大学 | Method for correcting representative displacement waveform and method for evaluating residual seismic performance of buildings |
| CN106168790A (en) * | 2016-02-29 | 2016-11-30 | 华南理工大学 | A kind of online change target velocity and the S-shaped Acceleration-deceleration Control Method of position |
| CN106444635A (en) * | 2015-08-04 | 2017-02-22 | 深圳市雷赛智能控制股份有限公司 | Asymmetric S-curve acceleration and deceleration control method and device for motion control system |
| CN108663994A (en) * | 2018-06-04 | 2018-10-16 | 绵阳逢研科技有限公司 | A kind of servo motor S curve Acceleration-deceleration Control Method |
| CN108829044A (en) * | 2018-07-27 | 2018-11-16 | 山东大学 | There is the S curve deceleration planning method of negative value in a kind of reply whole story speed |
| CN108958173A (en) * | 2018-07-27 | 2018-12-07 | 山东大学 | S curve deceleration planning method under any velocity of displacement based on trapezoidal solution |
| CN108983595A (en) * | 2018-07-18 | 2018-12-11 | 天津大学 | A kind of automatic setting method of feedforward controller parameter |
-
2019
- 2019-03-10 CN CN201910178004.1A patent/CN109991934B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2004258700A (en) * | 2003-02-24 | 2004-09-16 | Keyence Corp | Method of setting synchronous curve for positioning system and computer program therefor |
| CN101477354A (en) * | 2009-01-19 | 2009-07-08 | 东元总合科技（杭州）有限公司 | Position S type instruction generation method |
| JP5941998B2 (en) * | 2013-01-21 | 2016-06-29 | 国立大学法人横浜国立大学 | Method for correcting representative displacement waveform and method for evaluating residual seismic performance of buildings |
| CN103713581A (en) * | 2013-12-12 | 2014-04-09 | 南京工程学院 | Method for realizing S curve acceleration and deceleration of motion control system |
| CN106444635A (en) * | 2015-08-04 | 2017-02-22 | 深圳市雷赛智能控制股份有限公司 | Asymmetric S-curve acceleration and deceleration control method and device for motion control system |
| CN106168790A (en) * | 2016-02-29 | 2016-11-30 | 华南理工大学 | A kind of online change target velocity and the S-shaped Acceleration-deceleration Control Method of position |
| CN108663994A (en) * | 2018-06-04 | 2018-10-16 | 绵阳逢研科技有限公司 | A kind of servo motor S curve Acceleration-deceleration Control Method |
| CN108983595A (en) * | 2018-07-18 | 2018-12-11 | 天津大学 | A kind of automatic setting method of feedforward controller parameter |
| CN108829044A (en) * | 2018-07-27 | 2018-11-16 | 山东大学 | There is the S curve deceleration planning method of negative value in a kind of reply whole story speed |
| CN108958173A (en) * | 2018-07-27 | 2018-12-07 | 山东大学 | S curve deceleration planning method under any velocity of displacement based on trapezoidal solution |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2021175487A1 (en) * | 2020-03-03 | 2021-09-10 | Sew-Eurodrive Gmbh & Co. Kg | Method for decelerating an object and machine |
| CN112498355A (en) * | 2020-11-02 | 2021-03-16 | 浙江吉利控股集团有限公司 | Speed planning method and device |
| CN113703399A (en) * | 2021-08-26 | 2021-11-26 | 广东拓斯达科技股份有限公司 | Motion trajectory planning method, device, equipment and storage medium |
| CN113703399B (en) * | 2021-08-26 | 2022-09-16 | 广东拓斯达科技股份有限公司 | Motion trajectory planning method, device, equipment and storage medium |
| CN115963782A (en) * | 2023-01-31 | 2023-04-14 | 北京立迈胜控制技术有限责任公司 | S-shaped velocity planning method based on non-zero initial acceleration |
| CN115963782B (en) * | 2023-01-31 | 2023-08-18 | 北京立迈胜控制技术有限责任公司 | S-shaped speed planning method based on initial acceleration being non-zero |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109991934B (en) | 2021-02-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109991934A (en) | A kind of time optimal online S type deceleration planning method | |
| CN110362910B (en) | Game theory-based automatic driving vehicle lane change conflict coordination model establishment method | |
| CN109345020B (en) | Non-signalized intersection vehicle driving behavior prediction method under complete information | |
| CN110018689B (en) | Dynamic window-based multi-virtual target point global dynamic path planning algorithm | |
| CN108919795B (en) | Automatic driving automobile lane change decision method and device | |
| Zhang et al. | A game theoretic model predictive controller with aggressiveness estimation for mandatory lane change | |
| CN105983968B (en) | The track generating means of the robot of track of the generation with curve part | |
| CN109814496A (en) | Multiaxis method for synchronizing time in a kind of S type acceleration and deceleration trajectory planning | |
| CN112550314B (en) | Embedded optimization type control method suitable for unmanned driving, driving control module and automatic driving control system thereof | |
| CN109991974A (en) | Automatic Pilot path follower method, device and control equipment | |
| Yang et al. | Longitudinal tracking control of vehicle platooning using DDPG-based PID | |
| Ali et al. | Combined ANFIS method with FA, PSO, and ICA as Steering control optimization on electric car | |
| CN114852105A (en) | Method and system for planning track change of automatic driving vehicle | |
| CN114253274B (en) | Data-driven-based networked hybrid vehicle formation rolling optimization control method | |
| CN115392111A (en) | Train tracking control method, equipment and storage medium | |
| CN110119528A (en) | A kind of random traffic flow simulation system of bridge based on intelligent body cellular automata | |
| CN113641099B (en) | Impedance control imitation learning training method for surpassing expert demonstration | |
| CN116551703B (en) | Motion planning method based on machine learning in complex environment | |
| CN114987511A (en) | Method for simulating human driving behavior to train neural network-based motion controller | |
| CN108776432A (en) | Network-based airfield runway detection robot forecast Control Algorithm | |
| CN110398993A (en) | Method for control speed, device and computer readable storage medium | |
| CN116127853A (en) | Unmanned driving overtaking decision method based on DDPG (distributed data base) with time sequence information fused | |
| CN113353102B (en) | Unprotected left-turn driving control method based on deep reinforcement learning | |
| Zhao et al. | Integrated longitudinal and lateral control system design and case study on an electric vehicle | |
| CN114047759A (en) | Local path planning method based on DWA and artificial potential field fusion |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |