CN109991934B - Time-optimal online S-type acceleration and deceleration planning method - Google Patents

Abstract

The invention discloses an online S-shaped acceleration and deceleration planning method with optimal time, which has the characteristic of realizing online real-time calculation and can plan an interpolation track meeting motion constraint conditions according to any initial motion state and an end state with zero acceleration. The off-line calculation process is simplified, and the real-time automatic planning of the motion trail according to the signals returned by the sensors in an unknown environment can be realized.

Description

Time-optimal online S-type acceleration and deceleration planning method

Technical Field

The invention relates to the field of motion control of industrial robots and the field of speed control of servo motors, in particular to an online S-shaped acceleration and deceleration planning method in the aspects of a speed planning method and robot motion planning for a mobile robot.

Background

The traditional trajectory planning method usually does not consider the initial acceleration of the initial motion state, and the initial speed is not equal to zero, because the trajectory planning is difficult and complicated in calculation for the situation that the initial motion state is not equal to zero, and the requirement of real-time property cannot be met. For a high performance robot control system, regardless of the current motion state of the robot, the robot should immediately react to the input signal by inputting an external sensor signal as a robot controller.

Disclosure of Invention

The method aims to overcome the defect that the traditional trajectory planning method cannot plan the trajectory according to any initial motion state and any end state. The invention provides an online S-shaped acceleration and deceleration planning method with optimal time, which can be used for automatic online generation of a robot track. And under the condition of considering speed, acceleration and jerk constraint conditions and the starting and ending states of the robot motion, directly solving by taking the shortest time as an optimization target.

The purpose of the invention is realized by the following technical scheme:

an online S-shaped acceleration and deceleration planning method with optimal time comprises the following steps:

(1) inputting parameters required by online motion planning: inputting a first and last motion state and motion constraint conditions, wherein the first and last motion state comprises an initial position P_sInitial velocity V_sInitial acceleration A_sAnd an end position P_trgtEnd velocity V_trgtThe motion constraint condition includes maximum jerk J_maxMaximum acceleration A_maxMaximum velocity V_max；

(2) Calculating an initial acceleration Curve Type regardless of Displacement_a ^*: according to the parameters input in the step (1), under the condition of not considering displacement constraint, meeting other constraint conditions to calculate the Type of the initial acceleration curve_a ^*；

Firstly, judging whether the acceleration A needs to reach the limit in the process of changing from the initial speed to the final speed in the shortest time; if the acceleration limit value does not need to be reached, calculating Type according to the head and tail motion states_a ^*The Type of initial acceleration curve_a ^*Four cases are divided; if the acceleration needs to reach the limit value, the Type is calculated according to the head and tail motion state_a ^*Also corresponding to four cases; selecting one of the eight cases as Type_a ^*；

(3) Type returned by step (2)_a ^*Calculating an initial jerk J_s(ii) a Firstly aiming at the returned Type in the step (2)_a ^*Calculate Type_a ^*Corresponding displacement Δ P of the acceleration curve^*Considering the convenience of calculation, the eight types of types are used_a ^*The method is divided into four categories;

(4) determining an S-Type acceleration curve Type_a(ii) a If the calculated delta P in the step (3)^*Equal to the target displacement Δ P, then Type_aIs equal to Type_a ^*(ii) a Otherwise J calculated by step (3) is required_sCalculate Type_a(ii) a Considering whether a uniform acceleration region exists, whether a uniform velocity region exists and acceleration and deceleration are carried out according to the principle that the total planning time is shortestThe S-shaped acceleration curve types are divided into eight types by the transition times, and one Type is selected from the eight types of S-shaped acceleration curves as the Type according to the following principle_a(ii) a The classification judgment principle is as follows:

principle A: supposing that the speed just reaches the speed limit and finally reaches the target speed in the motion planning process, and comparing the displacement at the moment with the actual target displacement;

principle B: supposing that in the motion planning process, the acceleration just reaches the limit and finally reaches the target speed, and comparing the displacement at the moment with the actual target displacement;

principle C: supposing that in the motion planning process, the acceleration reaches the limit, the speed just reaches the limit and finally reaches the target speed, and comparing the displacement at the moment with the actual target displacement;

principle D: supposing that in the motion planning process, the acceleration and the speed just reach the limit, and comparing the current speed with the actual target speed;

principle E: supposing that in the motion planning process, the acceleration just reaches the limit and finally reaches the target speed, and comparing the current speed with the actual target speed and the current displacement with the actual target displacement;

through the combined use of the principles, one type of the eight S-shaped acceleration curve types is selected to meet the requirement of shortest total planning time and meet the motion constraint condition at the same time;

(5) calculating the segmentation parameters of the acceleration curve: calculating to obtain Type in step (4)_aWriting different equation sets aiming at different acceleration curve columns and solving to obtain the time corresponding to each section of the acceleration curve;

for the condition that the uniform motion does not exist, solving the acceleration part time and the deceleration part time through a nonlinear equation system respectively; for the condition of existence of uniform acceleration or uniform deceleration section, the speed constraint condition, namely the middle speed extreme value V, is reached in the motion process_pIf the calculation method is known, the calculation method is the same as that under the condition that uniform acceleration or uniform deceleration does not exist, and the calculation method is a linear equation system at the moment; thereby solving the acceleration time and the deceleration timeAnd uniform speed time; finally obtaining the subsection parameter t of the acceleration curve_1a，t_1b，t_1c，t_2a，t_2b，t_2c，t_otg]；

(6) Generating a motion track according to the acceleration curve segmentation parameters: from acceleration curve Type_aAnd (5) substituting the acceleration curve segmentation parameters calculated in the step (5) into an S-type acceleration and deceleration curve formula to generate the motion track.

Further, in the step (2), if the acceleration limit value does not need to be reached, judging the speed change trend according to the head and tail motion states to determine the Type of the initial acceleration curve_a ^*The method is divided into four cases, and the corresponding four cases are respectively: a. the initial acceleration curve is firstly accelerated and then decelerated, and is marked as + PosTri; b. the initial acceleration curve is accelerated and decelerated firstly and then decelerated, and is marked as-NegTri; c. the initial acceleration curve is firstly reduced and decelerated, then accelerated and then reduced and accelerated, and is marked as-PosTri; d. the initial acceleration curve is firstly reduced and accelerated, then is reduced and decelerated, and is marked as + NegTri;

for the condition that the acceleration needs to reach the limit value, judging the speed change trend according to the first and last motion states, wherein the four conditions are as follows: e. the initial acceleration curve is accelerated in an adding mode, then accelerated in a uniform mode and finally accelerated in a reducing mode, and the acceleration curve is marked as + PosTrap; f. the initial acceleration curve is accelerated and decelerated firstly, then decelerated evenly and finally decelerated, and is marked as-NegTrap; g. the initial acceleration curve firstly reduces and decelerates, then accelerates, then uniformly accelerates, finally reduces and accelerates, and is marked as-PosTrap; h. the initial acceleration curve is firstly reduced and accelerated, then decelerated, uniformly decelerated and finally reduced and decelerated, and is marked as + NegTrap.

Further, in step (3), the displacement Δ P of the corresponding acceleration curve is calculated^*Aspect, will eight types of Type_a ^*The kit is divided into four types, wherein the types of the + PosTri and the-NegTri, the types of the-PosTri and the + NegTri, the types of the + PosTrap and the-NegTrap and the types of the-PosTrap and the + NegTrap are included;

if the target displacement Δ P is equal to Δ P^*Then Type_aIs equal to Type_a ^*(ii) a If the target displacement Δ P is not equal to Δ P^*Then Type_a ^*Invalid; if the target displacement Δ P is greater than Δ P^*Then J is_sIs equal to J_max(ii) a If the target displacement Δ P is smaller than Δ P^*Then J is_sIs equal to-J_max(ii) a There may be speed over speed maximum constraints for the second and fourth types of deceleration, and deceleration regions, where J is specified_sAnd (5) 0, namely the input parameters are wrong, and the planning cannot be carried out under the motion constraint condition.

Further, considering the principle of shortest planning total time in the step (4), the Type of the acceleration curve is determined_aThe method comprises the following eight steps: TriTriTri, Traptri, TriTrap, Traptrap, TriZeroTri, TrapZeroTri, TriZeroTrap, TrapZeroTrap; wherein Tri represents acceleration first and then deceleration or acceleration first and then deceleration second; trap represents the condition of uniform acceleration or uniform deceleration, namely the acceleration process is firstly added and then uniformly accelerated and then subtracted; zero denotes that the acceleration is equal to Zero, i.e. a constant velocity segment process.

Compared with the prior art, the technical scheme of the invention has the following beneficial effects:

1. the method can satisfy the motion constraint condition (J)_max，A_max，V_max) Of any initial state of motion (A)_s，V_s，P_s) And a motion state (A) in which the end acceleration is zero is taken into account_trgt，V_trgt，P_trgt) And performing S-type acceleration and deceleration planning to generate a unique S-type acceleration and deceleration trajectory which meets the motion constraint condition and has optimal time.

2. The method considers various conditions of the S-shaped acceleration and deceleration curve in detail, greatly simplifies the solving algorithm of the S-shaped acceleration and deceleration curve in any initial state, can calculate S-shaped acceleration and deceleration track parameters in microsecond time and generate the motion track, has high real-time performance, and is suitable for planning the track on line in real time. Since the planned trajectory is optimal in time, the work efficiency can be improved.

3. The invention can plan the motion track in real time according to the return signal of the sensor. A method is provided for automatically planning a motion path of a robot in an unknown environment. The intelligent and flexible robot is facilitated to be realized.

Drawings

Fig. 1 shows a schematic diagram of a time-optimal online S-type acceleration and deceleration planning method.

FIG. 2 shows eight initial acceleration curve types without considering target displacement_a ^*Schematic representation. Wherein the horizontal axis is time t, and the vertical axis is acceleration A; in the figure 1, + PosTri, 2, -NegTri, 3, -PosTri, 4, + NegTri, 5, + PosTrap, 6, -NegTrap, 7, -PosTrap, 8, + PosTrap.

FIG. 3 shows eight S-shaped acceleration curve types_aSchematic representation. (A)_s∈[-A_max，A_max]，J_s＝±J_max) (ii) a Wherein the horizontal axis is time t, the vertical axis is acceleration A, and the diagram shows 1, TriTriTri, 2, Traptri, 3, TriTrap, 4, Traptrap, 5, TrizeroTri, 6, TraptroTri, 7, TrizeroTrap, 8 and TraptroTrap.

Fig. 4(a) to 4(f) show graphs of results for some experimental parameters. The abscissa is time t, and the ordinates of the upper, middle and lower plots of fig. 4(a) to 4(f) are acceleration, velocity and displacement, respectively.

Detailed Description

The invention is described in further detail below with reference to the figures and the specific examples. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

The invention provides a linear velocity planning method. The input parameters comprise motion constraint conditions, initial and final motion states and interpolation time dt. The motion constraints include maximum jerk, maximum acceleration, and maximum velocity, denoted as (J)_max，A_max，V_max). The initial motion state includes an initial acceleration, an initial velocity, an initial displacement, denoted as (A)_s，V_s，P_s). The initial motion state should satisfy the actual conditions, otherwise the motion plan may exceed the motion constraint conditions.

The ending motion state comprises zero target acceleration, zero target velocity, and target displacement, and is denoted as (A)_trgt，V_trgt，P_trgt). The output parameters include the optimal time of planningt_otgReal-time jerk J (t), acceleration A (t), velocity V (t), and displacement P (t) generated according to the interpolation time.

The technical scheme of the online S-type acceleration and deceleration planning method is specifically shown in fig. 1, and the following is a detailed description of fig. 1. :

step 1, inputting parameters required by online motion planning:

firstly, inputting a head and tail motion state and a motion constraint condition in an array form, wherein the head and tail motion state comprises an initial position P_sInitial velocity V_sInitial acceleration A_sAnd an end position P_trgtEnd velocity V_trgtThe constraint condition includes maximum jerk J_maxMaximum acceleration A_maxMaximum velocity value V_max. Is marked as INPUT ═ P_s，V_s，A_s，P_trgt，V_trgt，V_max，A_max，J_max]。

Step 2, calculating the Type of the initial acceleration curve without considering the displacement_a ^*：

Calculating the Type of the initial acceleration curve meeting other constraint conditions without considering the displacement according to the input parameters_a ^*。

And judging whether the acceleration A needs to reach the acceleration limit in the process of changing the speed from the initial speed to the final speed in the shortest time. If the acceleration limit value does not need to be reached, calculating Type according to the motion head and tail states_a ^*。

Initial acceleration curve Type without considering displacement_a ^*There are four cases, as shown in fig. 2. Wherein 1, the speed curve is firstly accelerated and then decelerated and marked as + PosTri; 2. the speed curve is firstly accelerated and decelerated and then decelerated, and is marked as-NegTri; 3. the speed curve is firstly reduced and decelerated, then accelerated and then reduced and accelerated, and is marked as-PosTri; 4. the speed curve is firstly reduced and accelerated, then reduced and decelerated, and is marked as + NegTri.

When the difference between the head and the tail speeds is delta V and the initial acceleration A_sDifferent signs, and | Δ V | is greater than the initial acceleration and is reduced to zeroThe speed variation is + PosTri or-NegTri, or-PosTri or + NegTri.

For the case that the acceleration needs to reach the limit value, the Type is also calculated according to the motion head and tail states_a ^*The same applies to the four cases, see fig. 2. 5. The speed curve is firstly accelerated, then accelerated uniformly and finally accelerated in a speed reducing way, and is marked as + PosTrap; 6. the speed curve is accelerated and decelerated firstly, then decelerated evenly and finally decelerated, and is marked as-NegTrap; 7. the speed curve firstly reduces and decelerates, then accelerates, then uniformly accelerates, finally reduces and accelerates, and is marked as-PosTrap; 8. the speed curve is firstly reduced and accelerated, then is reduced and then is uniformly reduced, and finally is reduced and decelerated, and is marked as + NegTrap.

When the difference between the head and tail speeds is Δ V and the initial acceleration A_sThe sign is different, and if | Δ V | is larger than the corresponding speed variation when the initial acceleration drops to zero, it is + PosTrap or-NegTrap, otherwise it is-PosTrap or + NegTrap. This makes it possible to select 1 Type of initial acceleration curve from the eight cases of FIG. 2_a ^*. See step 3 for details.

Step 3, Type returned from step 2_a ^*Calculating an initial jerk J_s：

According to Type_a ^*Calculate Type_a ^*Corresponding displacement Δ P of the acceleration curve^*. The above eight types of initial acceleration curves can be classified into four categories in terms of calculating displacement, i.e., + PosTri and-NegTri, PosTri and + NegTri, and + PosTrap and-NegTrap, PosTrap and + NegTrap.

Type calculated according to step 2_a ^*Calculate Type_a ^*Corresponding displacement Δ P of the acceleration curve^*. If the target displacement Δ P is equal to Δ P^*Then Type_aIs equal to Type_a ^*(ii) a If the target displacement Δ P is not equal to Δ P_minThen Type_a ^*Invalid; if the target displacement Δ P is greater than Δ P^*Then J is_sIs equal to J_max(ii) a If the target displacement Δ P is smaller than Δ P^*Then J is_sIs equal to-J_max. Intermediate speeds may exceed for class 2 and class 4Speed maximum constraint, in which case J is specified_sAnd (5) 0, namely the input parameters are wrong, and the planning cannot be carried out under the motion constraint condition.

Specifically, the number of intersections of the acceleration curve and the time axis described in this embodiment is a special case in which the initial acceleration is not considered to be equal to zero. As shown in fig. 2, the first graph may be represented as + PosTri or-NegTri, and includes the case where the initial acceleration is greater than zero, the initial jerk is greater than zero, the initial acceleration is less than zero, and the initial jerk is less than zero, and is characterized in that the maximum acceleration value does not reach the acceleration upper limit value, and there is only one intersection point of the acceleration curve and the time axis. The second graph can be represented as-PosTri or + NegTri, and comprises the condition that the initial acceleration is less than zero, the initial jerk is greater than zero, the initial acceleration is greater than zero, and the initial jerk is less than zero. The third graph can be expressed as a + PosTrap or a-NegTrap, and includes that the initial speed and the initial acceleration have the same sign, and the acceleration reaches the acceleration upper limit value at the middle time, and the acceleration curve has only one intersection point with the time axis. The fourth plot, which may be referred to as a PosTrap or a + PosTrap, includes an initial acceleration that is different in sign from the initial jerk, an intermediate time when the acceleration has reached an upper acceleration limit, and two acceleration curves that intersect the time axis.

Step 4, determining the Type of the S-shaped acceleration curve_a：

If the calculated delta P in the step (3)^*Equal to the target displacement Δ P, then Type_aIs equal to Type_a ^*(ii) a Otherwise J calculated by step 3 is required_sCalculate Type_a. Considering the principle that the total planning time is shortest, the Type is divided into_aThe three-dimensional model is divided into eight Tri, Traptri, TriTrap, Traptrap, TrizeroTri, TrapZeroTri, TriZeroTrap and TrapZeroTrap, which are respectively shown in FIG. 3.

The eight S-shaped acceleration curve types shown in fig. 3 encompass several cases shown in fig. 2. The first type of curve can be represented by TriTriTriTri, and is characterized in that the initial acceleration and the initial jerk have the same sign, the maximum acceleration is smaller than the acceleration upper limit value, two time intervals with the acceleration constant equal to zero exist at the intersection of the acceleration curve and the time axis. The second kind of curve can be represented by TrpTri, and is characterized in that the initial acceleration and the initial jerk have the same sign, the maximum acceleration of the acceleration area at a certain time reaches the acceleration upper limit value, two intersection points of the acceleration curve and the time axis exist, and no time interval with the acceleration constantly equal to zero exists. The third kind of curve can be expressed by TriTrap, and is characterized in that the initial acceleration and the initial acceleration have the same sign, the maximum acceleration at a certain time in the deceleration area reaches the acceleration upper limit value, two intersection points of the acceleration curve and the time axis exist, and no time interval with the acceleration constantly equal to zero exists. The fourth type of curve can be represented by Traptrap, and is characterized in that the initial acceleration and the initial jerk have the same sign, the maximum acceleration of the acceleration area and the deceleration area reaches the acceleration upper limit value at a certain moment, two intersection points of the acceleration curve and the time axis exist, and no time interval with the acceleration constantly equal to zero exists. The acceleration curve of the fifth type, which may be denoted as TriZeroTri, is characterized with respect to the acceleration curve of the first type by the presence of time intervals in which the acceleration is equal to zero, i.e. by the presence of uniform motion intervals. The acceleration curve of the sixth type is denoted as trap zerotri, which is distinguished in relation to the acceleration curve of the second type in that it comprises a uniform motion interval. The seventh acceleration curve may be denoted as a TriZeroTrap, which is distinguished from the third acceleration curve by the presence of a constant motion interval in the middle segment. The eighth acceleration curve may be denoted as trap zerotrap, which is distinguished from the fourth acceleration curve by the presence of a constant speed interval in the middle segment.

Selecting a classification judgment principle of the acceleration curve types:

principle A: assuming that the speed just reaches the speed limit and finally reaches the target speed in the motion planning process, the displacement at the moment is compared with the actual target displacement.

Principle B: assuming that the acceleration just reaches the limit and finally reaches the target speed in the motion planning process, the displacement at the moment is compared with the actual target displacement.

Principle C: assuming that during the motion planning process, the acceleration has reached a limit, the velocity just reached a limit and finally the target velocity, the displacement at that time is compared to the actual target magnitude.

Principle D: assuming that both the acceleration and the velocity just reach the limit during the motion planning process, the magnitude of the velocity at that time is compared with the actual target velocity.

Principle E: assuming that the acceleration just reaches the limit and finally reaches the target speed in the motion planning process, the magnitude of the speed at the moment and the actual target speed and the magnitude of the displacement at the moment and the actual target displacement are compared.

Through the combined use of the various principles, one curve can be selected from the 8 acceleration curves to meet the condition that the total planning time is shortest and the motion constraint condition can be met at the same time.

At an initial jerk J_sThe speed of the process with acceleration just reaching the maximum acceleration and then the acceleration dropping to zero is greater than the maximum speed for example.

Judgment 1: if the initial speed is less than the final target speed, decision 2 is entered.

And (3) judging: and judging according to the principle A, if the acceleration curve is TriTriTriTri, comparing the displacement at the moment with the actual target displacement. Type if the displacement at this time is smaller than the actual target displacement_aEqual to TriZeroTri. Otherwise, the result is TriTriTriTriTri. If the decision 1 condition is not met, then decision 3 is entered.

And 3, judgment: and if the minimum acceleration of the deceleration part reaches the acceleration lower limit value and the final speed reaches the target speed, judging whether the speed of the constant speed section is less than the speed limit value. If decision 3 is less than the speed limit, then decision 4 is entered.

And 4, judgment: and judging according to the principle B, and comparing the final displacement with the actual target displacement by assuming that the type of the acceleration curve is TriTriTriTri. Type if the displacement at this time is larger than the actual target displacement_aAnd (5) if the value is equal to the TriTriTri, otherwise, entering judgment 5.

And 5, judgment: whether the final speed is greater than the target speed is judged by the principle D. If less than the target speed, Type_aEqual to TriZeroTri, otherwise go to decision 6.

Judgment of6: similarly, the planning displacement and the target displacement are compared according to the principle A, and if the planning displacement is larger than the target displacement, the Type_aEqual to TriTrap, otherwise TriZeroTrap.

If the determination 3 is greater than or equal to the speed limit value, the routine proceeds to a determination 7.

And 7, judgment: according to principle A, the deceleration part is of a Tri Type, the displacement at the moment is compared with the actual target displacement, and if the displacement is larger than the target displacement, the Type is_aEqual to TriTriTri, otherwise TrizeroTri.

Step 5, calculating the segmentation parameters of the acceleration curve:

calculating to obtain Type from step 4_aAnd writing different equation sets aiming at different acceleration curve columns, and solving to obtain the time corresponding to each section of the acceleration curve.

For the case of no uniform acceleration or uniform deceleration, the acceleration part time and the deceleration part time are respectively used as t₁₀And t₂₁This shows that the above eight acceleration curves can be reduced to 5 general equations and 2 distinct equations, with 7 unknowns. Solving t from the equation₁₀And t₂₁。

With Type_aTaking TriTri as an example, an intermediate variable S is introduced₁、S₂、ΔP_1a、ΔP_1b、V_p、t_b. Wherein V_pThe intermediate speed limit.

t_b＝-|A_s/J|

General equation

S₁＝V_p-V_s

S₂＝V_trgt-V_p

ΔP_1a＝(J_st_b ³/6+t_bV_b)+(t₁₀-t_b)(V_b+V_p)/2

ΔP_1b＝ΔP-t₂₁(V_p+V_trgt)/2

ΔP_1a-ΔP_1b＝0

Equation of difference

For the condition of existence of uniform acceleration or uniform deceleration section, the speed constraint condition, namely the middle speed extreme value V, is reached in the motion process_pIf known, the calculation method is the same as the calculation method of the case of uniform acceleration or uniform deceleration without existence, and the acceleration part time t can be directly solved at the moment₁₀And a deceleration time t₂₁Sum and uniform acceleration (deceleration) time t_m. According to t₁₀，t₂₁，t_mCan obtain the sectional parameters of the acceleration curve

[t_1a，t_1b，t_1c，t_2a，t_2b，t_2c，t_otg]

Wherein t is_1aIndicating the end of the acceleration section, t_1bIndicating the end of the uniform acceleration period, t_1cTo reduce the end of the acceleration section, t_2aAt the end of the uniform velocity segment, t_2bFor the end of the acceleration or deceleration section, t_2cAt the end of the uniform deceleration section, t_otgTo decrease the end time of the deceleration section.

Step 6, generating a motion track according to the acceleration curve segmentation parameters:

then the acceleration curve Type_aAnd substituting the segmented parameters of the acceleration curve calculated in the step 5 into an S-type acceleration and deceleration track formula to generate tracks J (t), A (t), V (t) and P (t).

The formula of the S-shaped acceleration and deceleration curve is as follows:

the specific trajectories are as follows:

t is more than or equal to 0 and less than or equal to t in the first section_1a，

Second section t_1a＜t≤t_1bIn which P is_1a、V_1a、A_1aRespectively, the displacement, velocity and acceleration of the end of the first section of the joint,

third segment t_1b＜t≤t_1cIn which P is_1b、V_1b、A_1bRespectively displacement, velocity, acceleration of the end of the second segment of the joint

Fourth segment t_1c＜t≤t_2aIn which P is_1c、V_1c、A_1cDisplacement, velocity and acceleration of the end of the third segment of the joint

P(t)＝P_1c+V_1c(t-t_1c)

Fifth segment t_2a＜t≤t_2bIn which P is_2a、V_2a、A_2aRespectively displacement, velocity and acceleration of the end of the fourth segment of the joint

The sixth section t_2b＜t≤t_2cIn which P is_2b、V_2b、A_2bRespectively displacement, velocity and acceleration of the end of the fifth segment of the joint

Seventh segment t_2c＜t≤t_otgIn which P is_2c、V_2c、A_2cRespectively displacement, velocity and acceleration of the end of the sixth section of the joint

J (t), A (t), V (t) are respectively equal to P (t) to evaluate t for third order second order and first derivative functions.

The following are the test parameters:

INPUT＝[P_s，V_s，A_s，P_trgt，v_trgt，V_max，A_max，J_max]

INPUT₁＝[0，100，50，500，-100，250，100，100]

INPUT₂＝[0，100，50，500，-100，200，100，100]

INPUT₃＝[0，100，50，500，-100，250，150，100]

INPUT₄＝[0，100，50，500，-100，250，200，100]

INPUT₅＝[0，100，50，500，-100，200，150，100]

INPUT₆＝[0，100，50，500，-100，200，200，100]

fig. 4(a) to 4(f) show the time-optimal S-type acceleration and deceleration trajectory planning result planned by the method, and the trajectory curves corresponding to the above 6 sets of input parameters are respectively shown in fig. 4(a), 4(b), 4(c), 4(d), 4(e) and 4 (f).

The present invention is not limited to the above-described embodiments. The foregoing description of the specific embodiments is intended to describe and illustrate the technical solutions of the present invention, and the above specific embodiments are merely illustrative and not restrictive. Those skilled in the art can make many changes and modifications to the invention without departing from the spirit and scope of the invention as defined in the appended claims.

Claims (1)

1. An online S-shaped acceleration and deceleration planning method with optimal time is characterized by comprising the following steps:

(1) inputting parameters required for online motion planning: inputting a first and last motion state and motion constraint conditions, wherein the first and last motion state comprises an initial position P_sInitial velocity V_sInitial acceleration A_sAnd an end position P_trgtEnd velocity V_trgtThe motion constraint condition includes maximum jerk J_maxMaximum acceleration A_maxMaximum velocity V_max；

(2) Calculating an initial acceleration Curve Type regardless of Displacement_a ^*: according to the parameters input in the step (1), under the condition of not considering displacement constraint, meeting other constraint conditions to calculate the Type of the initial acceleration curve_a ^*；

Firstly, judging whether the acceleration A needs to reach the limit in the process of changing from the initial speed to the final speed in the shortest time; if the acceleration limit value does not need to be reached, calculating Type according to the head and tail motion states_a ^*The Type of initial acceleration curve_a ^*Four cases are divided; the corresponding four cases are respectively: a. the initial acceleration curve is firstly accelerated and then decelerated, and is marked as + PosTri; b. the initial acceleration curve is accelerated and decelerated firstly and then decelerated, and is marked as-NegTri; c. the initial acceleration curve is firstly reduced and decelerated, then accelerated and then reduced and accelerated, and is marked as-PosTri; d. the initial acceleration curve is firstly reduced and accelerated, then is reduced and decelerated, and is marked as + NegTri; if the acceleration needs to reach the limit value, the Type is calculated according to the head and tail motion state_a ^*The other four cases are also corresponded: e. the initial acceleration curve is accelerated in an adding mode, then accelerated in a uniform mode and finally accelerated in a reducing mode, and the acceleration curve is marked as + PosTrap; f. the initial acceleration curve is accelerated and decelerated firstly, then decelerated evenly and finally decelerated, and is marked as-NegTrap; g. the initial acceleration curve firstly reduces and decelerates, then accelerates, then uniformly accelerates, finally reduces and accelerates, and is marked as-PosTrap; h. the initial acceleration curve firstly reduces and accelerates, then decelerates, then uniformly decelerates, finally reduces and decelerates, and is marked as + NegTrap; selecting one of the eight cases as Type_a ^*；

(3) Type returned by step (2)_a ^*Calculating an initial jerk J_s(ii) a First returns to step (2)Type (2)_a ^*Calculate Type_a ^*Corresponding displacement Δ P of the acceleration curve^*Considering the convenience of calculation, the eight types of types in the step (2) are used_a ^*The method is divided into four categories; wherein, the types of + PosTri and-NegTri, -PosTri and + NegTri, the types of + PosTrap and-NegTrap and the types of-PosTrap and + NegTrap;

if the target displacement Δ P is equal to Δ P^*Then Type_aIs equal to Type_a ^*(ii) a If the target displacement Δ P is not equal to Δ P_minThen Type_a ^*Invalid; if the target displacement Δ P is greater than Δ P^*Then J is_sIs equal to J_max(ii) a If the target displacement Δ P is smaller than Δ P^*Then J is_sIs equal to-J_max(ii) a There may be speed over speed maximum constraints for the second and fourth types of deceleration, and deceleration regions, where J is specified_sWhen the input parameter is wrong, the programming cannot be performed under the motion constraint condition;

(4) determining an S-Type acceleration curve Type_a(ii) a If the calculated delta P in the step (3)^*Equal to the target displacement Δ P, then Type_aIs equal to Type_a ^*(ii) a Otherwise J calculated by step (3) is required_sCalculate Type_a(ii) a The method divides the types of the S-shaped acceleration curve into eight types according to the principle that the total planning time is shortest and whether a uniform acceleration area exists, whether a uniform speed area exists and the number of acceleration and deceleration transition times are considered, and specifically comprises the following steps: TriTriTri, Traptri, TriTrap, Traptrap, TriZeroTri, TrapZeroTri, TriZeroTrap, TrapZeroTrap; wherein Tri represents acceleration first and then deceleration or acceleration first and then deceleration second; trap represents the condition of uniform acceleration or uniform deceleration, namely the acceleration process is firstly added and then uniformly accelerated and then subtracted; zero represents that the acceleration is equal to Zero, namely a uniform speed segment process; one of the eight S-shaped acceleration curve types is selected as Type according to the following principle_a(ii) a The classification judgment principle is as follows:

principle A: supposing that the speed just reaches the speed limit and finally reaches the target speed in the motion planning process, and comparing the displacement at the moment with the actual target displacement;

principle B: supposing that in the motion planning process, the acceleration just reaches the limit and finally reaches the target speed, and comparing the displacement at the moment with the actual target displacement;

principle C: supposing that in the motion planning process, the acceleration reaches the limit, the speed just reaches the limit and finally reaches the target speed, and comparing the displacement at the moment with the actual target displacement;

principle D: supposing that in the motion planning process, the acceleration and the speed just reach the limit, and comparing the current speed with the actual target speed;

principle E: supposing that in the motion planning process, the acceleration just reaches the limit and finally reaches the target speed, and comparing the current speed with the actual target speed and the current displacement with the actual target displacement;

through the combined use of the principles, one type of the eight S-shaped acceleration curve types is selected to meet the requirement of shortest total planning time and meet the motion constraint condition at the same time;

(5) calculating the segmentation parameters of the acceleration curve: calculating to obtain Type in step (4)_aWriting different equation sets aiming at different acceleration curve columns and solving to obtain the time corresponding to each section of the acceleration curve;

for the condition that the uniform motion does not exist, solving the acceleration part time and the deceleration part time through a nonlinear equation system respectively; for the condition of existence of uniform acceleration or uniform deceleration section, the speed constraint condition, namely the middle speed extreme value V, is reached in the motion process_pIf the calculation method is known, the calculation method is the same as that under the condition that uniform acceleration or uniform deceleration does not exist, and the calculation method is a linear equation system at the moment; thereby solving the acceleration time, the deceleration time and the uniform speed time; finally obtaining the subsection parameter t of the acceleration curve_1a，t_1b，t_1c，t_2a，t_2b，t_2c，t_otg](ii) a Wherein t is_1aIndicating the end of the acceleration section, t_1bIndicating the end of the uniform acceleration period, t_1cTo reduce the end of the acceleration section, t_2aAt the end of the uniform velocity segment, t_2bFor the end of the acceleration or deceleration section, t_2cAt the end of the uniform deceleration sectionMoment t_otgThe ending moment of the deceleration section is reduced;

(6) generating a motion track according to the acceleration curve segmentation parameters: from acceleration curve Type_aAnd (5) substituting the acceleration curve segmentation parameters calculated in the step (5) into an S-type acceleration and deceleration curve formula to generate the motion track.

Images