S-shaped acceleration and deceleration planning method with initial acceleration not being 0

Abstract

The application relates to the technical field of industrial robot motion control, in particular to an S-shaped acceleration and deceleration planning method with initial acceleration not being 0. In the speed planning layer, the acceleration segmentation time and the acceleration and speed of each segment endpoint are calculated in the variable speed process by giving acceleration, speed, position and other parameters. In the step planning layer, the position, speed and acceleration at any time are calculated according to the calculation results of the speed planning layer. In the position planning layer, the position motion under the condition is obtained according to the calculation results of the speed planning layer, and whether the given position total length data needs to be re-planned is fed back by comparison. The method can overcome the problem that the prior art cannot handle the case that the initial speed is not 0, can select the shortest time solution according to the initial and final speed, and has wide applicability, simple calculation and strong compatibility compared with general S-shaped curve planning.

Description

Technical Field

[0001] This invention relates to the field of industrial robot motion control technology, and in particular to an S-shaped acceleration / deceleration planning method for initial acceleration that is not zero. Background Technology

[0002] For industrial robots, trajectory planning methods determine the path a mobile robot will follow, whether or not a map is available. This mainly refers to the curved trajectory of the robotic arm's end effector, or the curved profile of the displacement, velocity, and acceleration of the manipulator during its movement.

[0003] Currently, trajectory planning for S-shaped acceleration and deceleration mainly involves given initial and final velocities, maximum velocity, acceleration, and maximum acceleration / deceleration, with the initial and final acceleration being 0.

[0004] However, traditional trajectory planning methods usually cannot handle cases where the initial velocity is not zero, nor can they automatically select the shortest time solution based on the initial and final velocities, thus reducing the applicability of S-shaped acceleration and deceleration planning.

[0005] Therefore, a new technical solution is urgently needed to solve the above-mentioned technical problems. Summary of the Invention

[0006] The purpose of this invention is to overcome the problems of the prior art and provide an S-shaped acceleration / deceleration planning method with non-zero initial acceleration. This method can effectively solve the technical problems that traditional techniques cannot handle cases with non-zero initial velocity and cannot automatically select the shortest time solution based on the initial and final velocities.

[0007] The above objectives are achieved through the following technical solutions:

[0008] A method for S-shaped acceleration / deceleration planning with non-zero initial acceleration includes the following steps:

[0009] Step (1) Based on the initial acceleration A s Magnitude and direction, initial velocity V s Stable operating speed V f Determine whether the initial acceleration A needs to be adjusted. s Reduce to 0, and generate a new initial velocity V based on this. s 'and the new initial acceleration A s ';

[0010] Step (2) Based on the stable operating speed V f Initial velocity V s 'and final velocity V e The relationship is divided into four models and selected from them, including the acceleration-acceleration model, the deceleration-deceleration model, the acceleration-deceleration model, and the deceleration-acceleration model.

[0011] If step (3) is an acceleration / deceleration model or a deceleration / deceleration model, then the 7-segment acceleration / deceleration algorithm is not used to calculate the velocity from the initial velocity V. s 'Change to final velocity V e Total displacement L s If it is an acceleration / deceleration model or a deceleration / acceleration model, then a 7-segment acceleration / deceleration algorithm is used to calculate the velocity from the initial velocity V. s 'Change to final velocity V e Total displacement L s ;

[0012] Step (4) reaches a stable operating speed V f At that time, determine the total displacement L s Whether it is less than the given total distance L; including:

[0013] Step (4-1) Total displacement L s If the distance is less than the given total distance L, then calculate the time.

[0014] Step (4-2) Total displacement L s If the distance is not less than the given total distance L, then a new stable operating speed V is obtained through iterative contraction using the bisection method. f ';

[0015] Step (4-3) Determine the new stable operating speed V f 'Lower total displacement L s Is it equal to the given total distance L? If yes, calculate the time; if no, return to step (4-2).

[0016] Furthermore, the determination in step (1) of whether the initial acceleration A needs to be... s Reduced to 0, including:

[0017] If so, then the initial acceleration A s Reduce the initial velocity to 0, calculate the time at this point, and calculate the new initial velocity V at this point. s ';

[0018] If not, then accelerate from an initial acceleration of 0 to an acceleration of A. s At that time, and according to the initial velocity V s Calculate the new initial velocity V based on displacement S0. s '.

[0019] Furthermore, the acceleration from an initial acceleration of 0 to an acceleration of A... s At that time, and according to the initial velocity V s Calculate the new initial velocity V based on displacement S0. s Specifically, from the current initial acceleration A s and initial velocity V s Initially, accelerate directly to the final velocity V. e Calculate the acceleration from 0 to As The time t0 used is used to calculate the new initial velocity V. s 'and from the initial velocity V s 'Accelerate to initial velocity V s The distance S0 used;

[0020] Where t0 = A s / J max J max S0 represents the maximum jerk; S0 is the hypothetical case, where the acceleration is calculated from 0 to A. s The total distance is negative because it did not actually occur and is only used for convenience in subsequent calculations.

[0021] Furthermore, the initial acceleration A s Reduce the initial velocity to 0, calculate the time at this point, and calculate the new initial velocity V at this point. s Specifically, the acceleration is first reduced to 0 and the velocity at this point is recorded as the new initial velocity V of the new 7-segment acceleration / deceleration program. s ', initial acceleration A s =0, then accelerate to the final velocity V e This yields the first time interval t0 of the acceleration / deceleration planning, the direction of acceleration, and the distance S0 traveled.

[0022] Where t0 = A s / J max J max S0 represents the maximum jerk; S0 represents the actual situation, and the total distance is positive.

[0023] Furthermore, if step (3) involves an acceleration / deceleration model or a deceleration / deceleration model, then the 7-segment acceleration / deceleration algorithm is not used to calculate the velocity from the initial velocity V. s 'Change to final velocity V e Total displacement L s Specifically, this involves calculating the initial velocity V. s 'Terminal velocity V e If the displacement in the shortest time is less than the given total distance L, then: if the condition is met, calculate according to the monotonic case and supplement the uniform speed segment; if the condition is not met, then report an error.

[0024] Beneficial effects

[0025] This invention provides an S-shaped acceleration / deceleration planning method for situations with non-zero initial acceleration. It primarily addresses the scenario where a non-zero initial acceleration exists, performing S-shaped acceleration / deceleration planning while maintaining a final acceleration of zero. This method overcomes the limitations of existing technologies in handling non-zero initial velocities, automatically selecting the shortest time solution based on the initial and final velocities. Furthermore, compared to general S-curve planning, it has a wider applicability, is less computationally complex, and possesses strong compatibility. Attached Figure Description

[0026] Figure 1 This is a flowchart of an S-shaped acceleration / deceleration planning method with an initial acceleration that is not zero, as described in this invention. Detailed Implementation

[0027] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. The described embodiments are merely some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0028] like Figure 1 As shown, an S-shaped acceleration / deceleration planning method with non-zero initial acceleration is presented, which mainly addresses the situation in actual production where the initial acceleration is non-zero and S-shaped acceleration / deceleration planning is performed directly.

[0029] This method is broken down into three levels:

[0030] In the velocity planning layer, given parameters such as acceleration, velocity, and position, the time interval of acceleration segments and the acceleration and velocity at the endpoints of each segment are calculated during the speed change process.

[0031] Based on the calculation results of the velocity planning layer, the position, velocity, and acceleration at any given time are calculated in the step planning layer.

[0032] Based on the calculation results of the velocity planning layer, the positional motion situation under this condition is obtained in the position planning layer. The total length data of the given position is compared to provide feedback on whether velocity planning needs to be re-performed.

[0033] This method includes the following steps:

[0034] Step (1) Based on the initial acceleration A s Magnitude and direction, initial velocity V s Stable operating speed V f Determine whether the initial acceleration A needs to be adjusted. s Reduce to 0, and generate a new initial velocity V based on this. s 'and the new initial acceleration A s ';

[0035] Step (2) Based on the stable operating speed V f Initial velocity V s 'and final velocity V e The relationship is divided into four models and selected from them, including the acceleration-acceleration model, the deceleration-deceleration model, the acceleration-deceleration model, and the deceleration-acceleration model.

[0036] If step (3) is an acceleration / deceleration model or a deceleration / deceleration model, then the 7-segment acceleration / deceleration algorithm is not used to calculate the velocity from the initial velocity V. s 'Change to final velocity V e Total displacement L s If it is an acceleration / deceleration model or a deceleration / acceleration model, then a 7-segment acceleration / deceleration algorithm is used to calculate the velocity from the initial velocity V. s 'Change to final velocity V e Total displacement L s ;

[0037] Step (4) reaches a stable operating speed V f At that time, determine the total displacement L s Whether it is less than the given total distance L; including:

[0038] Step (4-1) Total displacement L s If the distance is less than the given total distance L, then calculate the time.

[0039] Step (4-2) Total displacement L s If the distance is not less than the given total distance L, then a new stable operating speed V is obtained through iterative contraction using the bisection method. f ';

[0040] Step (4-3) Determine the new stable operating speed V f 'Lower total displacement L s Is it equal to the given total distance L? If yes, calculate the time; if no, return to step (4-2).

[0041] In step (1), it is determined whether the initial acceleration A needs to be adjusted. s Reduced to 0, including:

[0042] If so, then the initial acceleration A s Reduce the initial velocity to 0, calculate the time at this point, and calculate the new initial velocity V at this point. s ';

[0043] If not, then accelerate from an initial acceleration of 0 to an acceleration of A. s At that time, and according to the initial velocity V s Calculate the new initial velocity V based on displacement S0. s '.

[0044] Specifically, based on the initial acceleration A s Initial velocity V s Final velocity V e Given a total distance L, there are two cases when calculating:

[0045] Scenario 1

[0046] From the current initial acceleration A s and initial velocity Vs Initially, accelerate directly to the final velocity V. e Calculate the acceleration from 0 to A s Time taken t0 = A s / J max (J max (For maximum jerk), and used to calculate the new initial velocity V. s (Calculation conditions: initial velocity is V) s The initial acceleration increases from 0 to A. s At that time, the speed is V s ), and from V s 'Accelerate to V s The distance S0 used (this distance is a hypothetical case, i.e., the acceleration is calculated from 0 to A) s (This did not actually happen; it is only used for convenience in subsequent calculations, so the total distance is negative).

[0047] Scenario 2

[0048] First, reduce the acceleration to 0 and record the velocity at this point as the new initial velocity V of the new 7-segment acceleration / deceleration plan. s ', initial acceleration A s =0, then accelerate to the final velocity V e .

[0049] The first time interval t0 = A is obtained from the acceleration / deceleration program. s / J max And the direction of acceleration, and calculate the distance S0 used (this distance is the actual distance, and the sign is positive).

[0050] The new initial acceleration A calculated based on step (2) s '、 Initial velocity V s Let's perform a 7-segment S-shaped acceleration / deceleration curve planning; denoted as t1 to t7 for each segment, then we have:

[0051] Step (3-1) According to the given uniform motion velocity condition V f (i.e., stable operating speed), based on the calculated new initial velocity V s Final velocity V e Maximum acceleration A max Maximum jerk J max Given the same constraints, calculate the time intervals.

[0052] When V f -V s '≥A max 2 / J max , then t1=t3=A max / J max , t2=(V f-V s '-A max 2 / J max ) / A max

[0053] otherwise t2 = 0;

[0054] The calculation methods for t5, t6, and t7 are the same;

[0055] Calculate the total distance traveled when reaching a given speed:

[0056]

[0057] If the distance is less than the total given distance, then calculate according to V. f If the motion is at a constant speed for t4, otherwise proceed to step (3-2);

[0058] At this point, t4 = (LS - S0) / V f .

[0059] Step (3-2) does not reach V f In the case where there is no uniform velocity segment t4 = 0, according to V s '、V e V f The magnitude relationship is divided into four models: acceleration-acceleration model, deceleration-deceleration model, acceleration-deceleration model, and deceleration-acceleration model;

[0060] Among them, acceleration (V) s '≤V f ≤V e ) and deceleration (V) s '≥V f ≥V e The model does not use the 7-stage acceleration / deceleration algorithm; the calculation starts from V. s 'To V e Can the displacement be less than the given total distance L in the shortest time satisfy the following condition:

[0061] If the conditions are met, calculate and supplement the uniform speed segment according to the monotonic case;

[0062] If the conditions are not met, an error will be reported;

[0063] When |V e -V s '|≥A max 2 / J max When, then t1=t3=A max / J max , t2=(|V e -V s '|-Amax 2 / J max ) / A max

[0064] otherwise t2 = 0;

[0065] The time interval of uniform motion is: t=(LS-S0) / max{V e V s '}

[0066] For acceleration followed by deceleration (V) s '<V f >V e ) and deceleration followed by acceleration (V) s >V f <V e The model, V, is obtained through iterative contraction using the bisection method. f '(V f Given the critical velocities before and after the actual displacement, calculate the time intervals for each segment under the current displacement condition.

[0067] In the bisection iteration, V f The initial range of the iteration is (max(V)). e V s ) < V f '<V f );

[0068] Calculate the critical velocity V = (max(V)) e V s )+V f The total displacement L under condition ) / 2 s ;

[0069] This embodiment uses an acceleration / deceleration model as an example:

[0070] If L s <L, then the next iteration range is ((max(V)) e V s )+V f ) / 2<V f '<V f );

[0071] If L s =L, then V f '=(max(V e V s )+V f ) / 2;

[0072] If L s If the value is greater than L, then the next iteration range is (max(V)). e V s) < V f '<(max(V e V s )+V f ) / 2);

[0073] Iteration yields V f Stop at 'time' and according to V f 'Calculate the acceleration and deceleration times for the 7 segments (t4 = 0 because there is no uniform speed segment).

[0074] The method for determining the time intervals under the current displacement condition is the same as in step (3-1), still using the acceleration / deceleration model as an example:

[0075] When V f '-V s '≥A max 2 / J max , then t1=t3=A max / J max , t2=(V f '-V s '-A max 2 / J max ) / A max

[0076] otherwise t2 = 0;

[0077] The calculation methods for t5, t6, and t7 are the same.

[0078] Then, using the above formulas for position, velocity, and acceleration with respect to time, we can calculate the position, velocity, and acceleration at any given time; by integrating the acceleration, we can obtain the acceleration formula; and by integrating the velocity, we can obtain the position at any given time.

[0079] As a specific embodiment of this solution, the following is an example:

[0080] Given an initial acceleration A s Initial velocity V s Final velocity V e Maximum acceleration A max Maximum deceleration D max Acceleration J max Stable operating speed V f .

[0081] If the initial acceleration A s When the limit is exceeded, the following two situations may occur:

[0082] 1. If the acceleration exceeds the maximum acceleration, reduce the acceleration to the maximum acceleration and calculate the displacement, velocity, and time at this point. This is the first segment of the final planning scenario.

[0083] 2. If the acceleration is less than the maximum deceleration, increase the acceleration to the maximum deceleration and calculate the displacement, velocity, and time at this point. This is the first segment of the final planning scenario.

[0084] If the initial acceleration does not exceed the limit, then the time and displacement are recorded as 0, and the velocity and acceleration remain unchanged;

[0085] At this point, based on the conditions, it is determined whether the acceleration needs to be changed to 0 first:

[0086] By reducing the acceleration to 0, the velocity at this point is calculated and denoted as V. s1 ;

[0087] By reversing the acceleration to zero, the velocity at this point is calculated and denoted as V. s2 ;

[0088] When the initial acceleration is greater than or equal to 0

[0089] V s1 ≥V f The acceleration decreases to zero and then decelerates again; when calculating the time v... s =V s1 ;

[0090] Otherwise, accelerate directly. When calculating acceleration and deceleration time, the intermediate variable is the initial velocity v. s =V s2 ;

[0091] When the initial acceleration is less than 0

[0092] V s1 ≤V f When the acceleration increases to zero and then accelerates again, the time v is calculated. s =V s1 ;

[0093] Otherwise, decelerate directly. When calculating acceleration and deceleration time, the intermediate variable is the initial velocity v. s =V s2 ;

[0094] Then according to V f and V e The relationship determines the direction of velocity change in the second stage of S-shaped acceleration and deceleration, thus determining which of the four types of S-shaped acceleration and deceleration is: first increase then decrease, monotonically increase, first decrease then increase, or monotonically decrease.

[0095] Based on the S-shaped acceleration / deceleration type and various parameters, the initial velocity v is used for calculation. s Final velocity V e Maximum acceleration / deceleration A max D max acceleration J max Given a uniform velocity V fThe acceleration and deceleration times of the seven segments were calculated, and the total displacement was finally obtained.

[0096] Calculate whether uniform motion t4 exists based on the total displacement and given distance. If it does, calculate the fourth time segment. Otherwise, return to replan the 7 acceleration and deceleration time segments. Finally, obtain a conclusion that meets the conditions, and the planning is complete.

[0097] The above description is merely illustrative of the embodiments of the present invention and is not intended to limit the present invention. For those skilled in the art, any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for planning S-shaped acceleration and deceleration with a non-zero initial acceleration, characterized in that, include: Step (1) Based on the initial acceleration Magnitude, direction, and initial velocity Stable operating speed Determine whether the initial acceleration needs to be adjusted. Reduce to 0, and generate a new initial velocity based on this. and initial acceleration ; Step (2) Based on stable operating speed Initial velocity and final velocity The relationship is divided into four models and selected from them, including the acceleration-acceleration model, the deceleration-deceleration model, the acceleration-deceleration model, and the deceleration-acceleration model. If step (3) is an acceleration-acceleration model or a deceleration-deceleration model, then the 7-segment acceleration / deceleration algorithm is not used to calculate the velocity from the initial velocity. Change to final velocity Total displacement ; If it is an acceleration / deceleration model or a deceleration / acceleration model, then a 7-segment acceleration / deceleration algorithm is used to calculate the velocity from the initial velocity. Change to final velocity Total displacement ; Step (4) reaches a stable operating speed At that time, determine the total displacement Is it less than the given total distance? ;include: Step (4-1) Total Displacement Less than the given total distance Then calculate the time; Step (4-2) Total Displacement Not less than the given total distance Then, a new stable operating speed is obtained through iterative contraction using the bisection method. ; Step (4-3) Determine the new stable operating speed Lower total displacement Is it equal to the given total distance? If yes, calculate the time; otherwise, return to step (4-2). The step (1) involves determining whether the initial acceleration needs to be adjusted. Reduced to 0, including: If so, then the initial acceleration Reduce the initial velocity to 0, calculate the time at this point, and calculate the new initial velocity at this point. ; If not, then accelerate from an initial acceleration of 0 to an acceleration of At that time, and according to the initial velocity and displacement Calculate the initial velocity ; The initial acceleration of 0 is accelerated to an acceleration of At that time, and according to the initial velocity and displacement Calculate the initial velocity Specifically, from the current initial acceleration and initial velocity Start by accelerating directly to final speed. Calculate acceleration from 0 to Time used And used to calculate the new initial velocity. And from the initial velocity Accelerate to initial velocity Distance used ;in, , This is the maximum jerk. This is a hypothetical scenario, where the acceleration is calculated from 0 to... The total distance is negative because it did not actually occur and is only used for convenience in subsequent calculations. The initial acceleration Reduce the initial velocity to 0, calculate the time at this point, and calculate the new initial velocity at this point. Specifically, the acceleration is first reduced to 0 and the velocity at this point is recorded as the new initial velocity for the new 7-segment acceleration / deceleration program. New initial acceleration Then accelerate to final speed The first time segment of the acceleration / deceleration plan is obtained. And the direction of acceleration, and calculate the distance traveled. ;in, , This is the maximum jerk. As is the actual situation, the total distance is positive.

2. The S-shaped acceleration / deceleration planning method with non-zero initial acceleration according to claim 1, characterized in that, If the acceleration / deceleration model or the deceleration / deceleration model is used in step (3), then the 7-segment acceleration / deceleration algorithm is not used to calculate the velocity from the initial velocity. Change to final velocity Total displacement Specifically, this involves calculating the initial velocity. Final velocity The displacement is less than the given total distance in the shortest time. Can the conditions be met? If the conditions are met, calculate and supplement the uniform speed segment according to the monotonic case; if the conditions are not met, report an error.

Images