S-shaped trajectory planning algorithm for multi-axis time synchronization

Abstract

The invention discloses an S-shaped trajectory planning algorithm for multi-axis time synchronization, which belongs to the field of robot moving trajectory algorithms and realizes synchronization of multi-axis movement time by properly modifying an original trajectory. And the problem that the speed, the acceleration or the motor torque of the robot exceeds the allowable range of the motor possibly caused by dynamic saturation limitation of a motion system is solved. The method mainly comprises the step of carrying out independent trajectory planning on each axis by adopting a sectional S-shaped trajectory planning method, and comprises the following steps of: judging whether the maximum acceleration can be reached or not; judging whether a constant speed section exists or not; whether the acceleration section and the deceleration section can reach the maximum acceleration and deceleration speed or not is judged; 6-axis movement time adjustment: setting the new movement time of the other 5-axis as the longest movement axis time; and S4, performing synchronous scaling on the six-axis time by utilizing the parameters of the axes redefined in the step S2, and re-planning a joint motion track. The S-shaped trajectory planning method is mainly used for S-shaped trajectory planning of multi-axis time synchronization of the robot.

Description

Technical Field

[0001] The present invention relates to the field of robot running trajectory algorithms, and in particular to a multi-axis time-synchronized S-type trajectory planning algorithm. Background Art

[0002] At present, industrial robots are widely used in the automated production industry and play an important role in it. Robots need to complete specific movements according to the instructions of the controller. Different control strategies will lead to different motion characteristics of robots, and the robot's trajectory planning algorithm is the core algorithm of the robot's motion control strategy, which aims to determine the change law of the robot's position, speed, and acceleration over time. An excellent trajectory planning algorithm can reduce the vibration of the robot body and achieve smooth movement of the joints. Different trajectory planning algorithms will affect the movement path, time, and trend of the joint robot, resulting in differences in the robot's operating performance and have different degrees of impact on its accuracy and life.

[0003] The motion control algorithms of robot joints mainly include trapezoidal curves, polynomial trajectory planning, and S-curve trajectory planning. The acceleration of trapezoidal curves and polynomial trajectory planning is usually discontinuous or not smooth, which will cause mechanical shock in practical applications, resulting in wear of the mechanical structure of the equipment. Therefore, in joint robots, S-type trajectory planning can achieve efficient trajectory planning without sacrificing system performance because it can smoothly control the changes in speed and acceleration. It is widely used in industry to reduce the impact on the mechanical structure, improve the accuracy of the robot and extend its service life.

[0004] However, most of the above trajectory planning methods are applicable to trajectory control in the case of a single motor. In the articulated robot system, multiple axes need to move in coordination. When the motion time of each axis is not synchronized, the motion trajectory of the end of the robot arm may deviate from the predetermined path, thereby reducing the motion accuracy. This error will significantly reduce the product quality in application scenarios such as precision manufacturing, assembly and processing, making the product fail to meet the expected design requirements. In addition, the asynchronous motion time may also cause the speed and acceleration between the axes to be uncoordinated, making the motion unsmooth, and subjecting to additional force and torque, causing the robot arm to vibrate and shake, affecting its working stability and work efficiency, and increasing mechanical wear. In applications that require force control, such as assembly and surface polishing, asynchronous axis motion will lead to uneven force distribution, which in turn affects the actual work effect. Therefore, in order to reduce the impact and vibration of the system and make full use of the servo performance of the motor, the articulated robot needs to synchronize the motion time of the six axes during motion control.

[0005] Existing robot trajectory time synchronization methods have multiple defects:

[0006] The direct planning method maintains the set acceleration under given constraints, runs at the highest speed and prolongs the uniform speed time as much as possible, so as to give full play to the motor performance. However, this method is difficult to be used universally among different trajectory planning algorithms. The trapezoidal synchronous planning is simple to derive and has high computational efficiency, while the direct planning method has low complexity. However, due to the high complexity of S-type planning, the direct planning method cannot be applied to S-type trajectory planning.

[0007] The generalized velocity method combines multiple displacements into a vector for calculation and then decomposes them, but this method can easily cause the axis speed and acceleration to exceed the limit;

[0008] Although the double spline method can achieve multi-axis synchronization, it mostly uses the same node vector for path fitting without considering kinematics-related constraints, and the implementation process is complicated. Summary of the invention

[0009] The purpose of the present invention is to provide an S-type trajectory planning algorithm for multi-axis time synchronization, which not only realizes the synchronization of multi-axis motion time by appropriately modifying the original trajectory, but also solves the problem that the robot speed, acceleration or motor torque may exceed the allowable range of the motor due to the dynamic saturation limit of the motion system.

[0010] The present invention is achieved through the following technical solutions:

[0011] A multi-axis time-synchronized S-shaped trajectory planning algorithm includes the following steps:

[0012] S1. Use the segmented S-type trajectory planning method to independently plan the trajectory of each axis, including the following steps:

[0013] 1) Determine whether the maximum acceleration can be reached;

[0014] 2) Determine whether there is a uniform speed segment;

[0015] 3) Determine whether the acceleration and deceleration sections can reach the maximum acceleration and deceleration;

[0016] S2, 6-axis motion time adjustment: set the new motion time of the remaining 5 axes to the longest motion axis time;

[0017] S3, using the parameters of each axis redefined in step S2, the 6-axis time is synchronously scaled and the joint motion trajectory is replanned;

[0018] S4. Calculate the position of any trajectory based on the obtained parameters.

[0019] Furthermore, step S1 also includes the following steps: the entire motion process is divided into an acceleration section, a uniform acceleration section, a deceleration section, a uniform speed section, an acceleration and deceleration section, a uniform deceleration section, and a deceleration and deceleration section; q0 is the initial position, q1 is the final position, V0 is the initial speed, V1 is the final speed, V max is the maximum speed, a max is the maximum acceleration, j max is the maximum jerk, T j1 is the acceleration time, T a is the acceleration time, T v is the uniform speed period, T j2 T is the deceleration time. d is the deceleration period;

[0020] 1) Determining whether the maximum acceleration can be reached includes the following steps:

[0021] Assume that the maximum speed V can be reached max , but there is no uniform acceleration segment, V0′ is the velocity at the end of the jerk segment, and the jerk segment can be obtained:

[0022]

[0023] a is the acceleration at any time in the deceleration and acceleration section, and the deceleration and acceleration section can be obtained as:

[0024]

[0025] According to formula 1.1 and formula 1.2, we can get if This means that the acceleration reaches its maximum value, which will cause the speed in the acceleration phase to exceed the maximum speed limit. In this case, a max To make adjustments, the process is as follows:

[0026]

[0027] if It means that the maximum acceleration can be reached, there is a uniform acceleration segment, and the time of the uniform acceleration segment is T j1匀 ,

[0028] The parameters obtained are as follows:

[0029]

[0030] S is the total path length of the motion, and the steps to calculate the uniform speed segment time are as follows:

[0031]

[0032] T d The solution process and T aSimilarly, the parameters of the deceleration phase of trajectory planning can be obtained by the same logic;

[0033] 2) Determining whether there is a uniform speed segment includes the following steps:

[0034] When T V >0, indicating a uniform speed segment, the total motion planning time T = T a +T V +T d , define the state at this time as state 1, State = 1, the trajectory is a 7-segment trajectory, and this state will be used in subsequent multi-axis time synchronization planning;

[0035] When T V When <0, it means there is no uniform speed segment and the maximum speed cannot be reached; assuming that both the maximum acceleration and the maximum deceleration can be reached, the trajectory at this time has an acceleration segment, a uniform acceleration segment, an acceleration / deceleration segment, a deceleration / acceleration segment, a uniform deceleration segment, and a deceleration / deceleration segment;

[0036]

[0037] According to formula 1.4 and formula 1.6, V max The values ​​are as follows:

[0038]

[0039] V max Bring it into T a , T d The values ​​of both can be obtained in the calculation formula;

[0040] 3) Determining whether the acceleration and deceleration stages can reach the maximum acceleration and deceleration includes the following steps:

[0041] T has been found in step 2). a , T d If the value of T a <0 means there is no acceleration segment, only deceleration segment, and the state at this time is defined as state 2, State = 2, and the trajectory is a 3-segment trajectory; if T d <0 means there is no deceleration segment, only acceleration segment, and the state at this time is defined as state 3, State=3, and the trajectory is a 3-segment trajectory; if T a >=2×T j1 &&T d >=2×T j2 , the state at this time is defined as state 4, State = 4, the trajectory has no uniform speed segment; when T a <2×T j1 ||T d <2×T j2When , it means that the acceleration section or deceleration section cannot reach the maximum acceleration. The maximum acceleration value needs to be adjusted to meet the conditions. Use Newton iteration method to reduce the maximum acceleration value and recalculate V max , T a , T d , until it satisfies T a >=2×T j1 &&T d >=2×T j2 .

[0042] Further, step S2 specifically includes: taking the longest motion axis time T max As a benchmark, the new motion time T of the remaining 5 axes is N Set this time to the T of each axis. a , T d , T V , T j1 , T j2 , V max , a max , j max , q1, q0 are passed to step S3.

[0043] Furthermore, step S3 also includes: calculating the movement time of each segment and the total time through step S1, taking the new movement time as the time for re-planning, and using the scaling factor to scale the acceleration segment, the uniform speed segment, and the deceleration segment respectively. In step S1, when performing single-axis planning, the trajectory has been divided into 4 states, which should be considered separately during scaling;

[0044] State 1 is a 7-segment S-shaped trajectory, set the scaling factor (T N >=T), multiply the time of each segment by the scaling factor λ to get the new acceleration segment time T aN , uniform speed period T VN , deceleration time T dN , at this time the maximum speed is set to V maxN , calculate the movement distance S before and after scaling respectively, as shown in formula 1.8:

[0045]

[0046] According to formula 1.8, we can get V maxN The value of is shown in formula 1.9:

[0047]

[0048] Find a new V maxN After that, we need to find the uniform acceleration time T j1N and the maximum acceleration a maxN, V′0 is the speed at the end of the acceleration segment, V″′0 is the speed at the end of the uniform acceleration segment, and V″′0 is the speed at the end of the deceleration segment;

[0049]

[0050] According to equations 1.9 and 1.10, we can solve for T j1N The values ​​are as follows:

[0051]

[0052] At this time, T j1N Take negative values;

[0053] To obtain the parameters of all trajectory replanning in state 1, as shown in formula 1.12:

[0054]

[0055] State 4 is a trajectory without a uniform velocity segment. The time scaling calculation method is similar to that of state 1. The formula T VN and V maxN The calculation formula is different. The uniform speed section is removed during the calculation, and the state 4 parameters are obtained as shown in formula 1.13:

[0056]

[0057] State 2 is the deceleration stage.

[0058]

[0059] Solve the current j by equation 1.14 max and the value of T2;

[0060] State 3 is the acceleration stage, T1 is the time of the uniform acceleration stage, and T2 is the time of the deceleration stage.

[0061]

[0062] Solve the current j by equation 1.15 max and the value of T2.

[0063] Furthermore, the curve graph of the trajectory and the parameters of each segment of the trajectory are substituted into the position equation to obtain the position at any time.

[0064] Compared with the prior art, the present invention has the following beneficial effects:

[0065] 1. First, perform S-shaped trajectory planning for the six axes respectively, and calculate the S-curve data of each independent joint; then, take the axis with the longest motion time as the benchmark, and use the time scaling method to adjust the motion curves of other axes; in this process, the maximum speed and maximum acceleration of each joint are adjusted based on the dynamic constraints to achieve multi-axis synchronous trajectory planning, so that its speed and acceleration will not exceed the limit of dynamic constraints, effectively improving the coordination of multi-axis motion and the overall operation accuracy of the robot.

[0066] 2. It not only realizes the synchronization of multi-axis motion time, but also solves the problem that the robot speed, acceleration or motor torque may exceed the allowable range of the motor due to the dynamic saturation limitation of the motion system. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 It is a schematic diagram of the 7-segment S-shaped trajectory planning of the present invention;

[0068] Figure 2 is a trajectory planning flow chart of the present invention;

[0069] Figure 3 It is a schematic diagram of the acceleration section speed curve of the present invention;

[0070] Figure 4 It is a schematic diagram of the speed curve of the deceleration section of the present invention;

[0071] Figure 5 is a flow chart of the multi-axis time synchronization trajectory planning algorithm of the present invention;

[0072] Figure 6 It is a single-axis time-scaled motion data graph of the present invention;

[0073] Figure 7 It is a 6-axis non-time-synchronous trajectory planning motion data curve diagram of the present invention;

[0074] Figure 8 It is a 6-axis time synchronization trajectory planning motion data curve diagram of the present invention. DETAILED DESCRIPTION

[0075] The present invention will be further described below in conjunction with the accompanying drawings.

[0076] Example 1

[0077] A multi-axis time-synchronized S-shaped trajectory planning algorithm includes the following steps:

[0078] S1. Use the segmented S-type trajectory planning method to independently plan the trajectory of each axis, including the following steps:

[0079] 1) Determine whether the maximum acceleration can be reached;

[0080] 2) Determine whether there is a uniform speed segment;

[0081] 3) Determine whether the acceleration and deceleration sections can reach the maximum acceleration and deceleration;

[0082] S2, 6-axis motion time adjustment: set the new motion time of the remaining 5 axes to the longest motion axis time;

[0083] S3, using the parameters of each axis redefined in step S2, the 6-axis time is synchronously scaled and the joint motion trajectory is replanned;

[0084] S4. Calculate the position of any trajectory based on the obtained parameters.

[0085] Example 2

[0086] A multi-axis time-synchronized S-shaped trajectory planning algorithm, step S1 also includes the following steps: the entire motion process is divided into an acceleration segment, a uniform acceleration segment, a deceleration segment, a uniform speed segment, an acceleration and deceleration segment, a uniform deceleration segment, and a deceleration and deceleration segment; ideally, it can be divided into 7 segments, such as Figure 1 As shown, let q0 be the initial position, q1 be the final position, V0 be the initial velocity, V1 be the final velocity, V max is the maximum speed, a max is the maximum acceleration, j max is the maximum jerk, T j1 is the acceleration time, T a is the acceleration time, T v is the uniform speed period, T j2 T is the deceleration time. d The deceleration time. In actual situations, the 7-stage mode may not be realized due to the maximum speed, acceleration, jerk and torque limits. In this case, the motion process needs to be adjusted, such as abandoning the uniform speed and acceleration segments, or reducing the maximum speed and acceleration of a single axis to meet the planning conditions.

[0087] 1) Determining whether the maximum acceleration can be reached includes the following steps:

[0088] Assume that the maximum speed V can be reached max , but there is no uniform acceleration segment, V′0 is the velocity at the end of the jerk segment, and the jerk segment can be obtained:

[0089]

[0090] a is the acceleration at any time in the deceleration and acceleration section, and the deceleration and acceleration section can be obtained as:

[0091]

[0092] According to formula 1.1 and formula 1.2, we can get if This means that the acceleration reaches its maximum value, which will cause the speed in the acceleration phase to exceed the maximum speed limit. In this case, a max To make adjustments, the process is as follows:

[0093]

[0094] if It means that the maximum acceleration can be reached, there is a uniform acceleration segment, and the time of the uniform acceleration segment is T j1匀 ,

[0095] The parameters obtained are as follows:

[0096]

[0097] S is the total path length of the motion, and the steps to calculate the uniform speed segment time are as follows:

[0098]

[0099] T d The solution process and T a Similarly, the parameters of the deceleration phase of trajectory planning can be obtained by the same logic;

[0100] 2) Determining whether there is a uniform speed segment includes the following steps:

[0101] Whether there is a uniform speed segment means whether the robot joint can reach the specified maximum speed; when T V >0, indicating a uniform speed segment, the total motion planning time T = T a +T V +T d , define the state at this time as state 1, State = 1, the trajectory is a standard 7-segment trajectory, and this state will be used in subsequent multi-axis time synchronization planning;

[0102] When T V When <0, it means there is no uniform speed segment and the maximum speed cannot be reached; assuming that both the maximum acceleration and the maximum deceleration can be reached, the trajectory at this time has an acceleration segment, a uniform acceleration segment, an acceleration / deceleration segment, a deceleration / acceleration segment, a uniform deceleration segment, and a deceleration / deceleration segment;

[0103]

[0104] According to formula 1.4 and formula 1.6, V max The values ​​are as follows:

[0105]

[0106] V max Bring it into T a 、Td The values ​​of both can be obtained in the calculation formula;

[0107] 3) Determining whether the acceleration and deceleration stages can reach the maximum acceleration and deceleration includes the following steps:

[0108] T has been found in step 2). a , T d If the value of T a <0 means there is no acceleration segment, only deceleration segment, and the state at this time is defined as state 2, State = 2, and the trajectory is a 3-segment trajectory; if T d <0 means there is no deceleration segment, only acceleration segment, and the state at this time is defined as state 3, State=3, and the trajectory is a 3-segment trajectory; if T a >=2×T j1 &&T d >=2×T j2 , the state at this time is defined as state 4, State = 4, the trajectory has no uniform speed segment; when T a <2×T j1 ||T d <2×T j2 When , it means that the acceleration section or deceleration section cannot reach the maximum acceleration. The maximum acceleration value needs to be adjusted to meet the conditions. Use Newton iteration method to reduce the maximum acceleration value and recalculate V max , T a , T d , until it satisfies T a >=2×T j1 &&T d >=2×T j2 ;

[0109] The above is the trajectory planning step of step S1, and the flow chart is as follows Figure 2 As shown;

[0110] Step S2 specifically includes: taking the longest motion axis time T max As a benchmark, the new motion time T of the remaining 5 axes is N Set this time to the T of each axis. a , T d , T V , T j1 , T j2 , V max , a man , j max , q1, q0 are passed to step S3;

[0111] Step S3 also includes: calculating the movement time and total time of each segment through step S1, taking the new movement time as the time for re-planning, and using the scaling factor to scale the acceleration segment, the uniform speed segment, and the deceleration segment respectively. In step S1, when performing single-axis planning, the trajectory has been divided into 4 states, which should be considered separately during scaling;

[0112] State 1 is a standard 7-segment S-shaped trajectory, with a scaling factor of (T N >=T), multiply the time of each segment by the scaling factor λ to get the new acceleration segment time T aN , uniform speed period T VN , deceleration time T dN , at this time the maximum speed is set to V maxN , calculate the movement distance S before and after scaling respectively, as shown in formula 1.8:

[0113]

[0114] According to formula 1.8, we can get V maxN The value of is shown in formula 1.9:

[0115]

[0116] Find a new V maxN After that, we need to find the uniform acceleration time T j1N and the maximum acceleration a maxN , Figure 3 is a schematic diagram of the acceleration section velocity curve, V′0 is the velocity at the end of the acceleration section, V″0 is the velocity at the end of the uniform acceleration section, and V″′0 is the velocity at the end of the deceleration section;

[0117]

[0118] According to equations 1.9 and 1.10, we can solve for T j1N The values ​​are as follows:

[0119]

[0120] At this time, T j1N Take a negative value. If you take a positive value, it will result in 2T j1N ≥T aN , this value does not conform to the trajectory planning, so the positive value is discarded; in order to obtain the parameters of all trajectory replanning in state 1, as shown in formula 1.12:

[0121]

[0122] State 4 is a trajectory without a uniform velocity segment. The time scaling calculation method is similar to that of state 1. The formula T VN and VmaxN The calculation formula is different. The uniform speed section is removed during the calculation, and the state 4 parameters are obtained as shown in formula 1.13:

[0123]

[0124] State 2 and state 3 are trajectories with only deceleration segments and only acceleration segments. This is a special case and cannot be directly time-scaled, which will break through the kinematic and dynamic limitations. These two situations will not appear in conventional multi-axis synchronous planning, but may appear in multi-segment trajectory fusion and stop motion, so they are handled separately. When planning in step S1, it is already the time-optimal motion state, which can maximize the performance of the motor. When there is only a deceleration segment and only an acceleration segment, if it is scaled according to the trajectory planning curve of step S1, it will cause kinematic saturation and dynamic saturation, which violates the saturation limit constraint of the drive system. Therefore, when these two states appear, the trend of the original planning curve cannot be maintained, and the acceleration segment or the deceleration segment needs to be removed so that it can first accelerate or decelerate at the maximum acceleration, and then decelerate and decelerate. When there is only a deceleration segment or an acceleration segment, there may also be an initial acceleration. For example, when there is only a deceleration segment, the joint has a positive acceleration. At this time, if the acceleration is reduced to 0 first and then gradually increased to the maximum acceleration in the reverse direction, the trajectory will produce an acceleration segment, which conflicts with the state of only a deceleration segment. Therefore, when performing time scaling, the acceleration at this time should be ignored, and a reverse maximum acceleration should be reassigned to it, and then gradually reduced to 0, so that the entire plan presents a state of first dense and then sparse, such as Figure 4 As shown, state 2 is the deceleration stage.

[0125]

[0126] Solve the current j by equation 1.14 max and the value of T2;

[0127] State 3 is the acceleration stage, T1 is the time of the uniform acceleration stage, and T2 is the time of the deceleration stage.

[0128]

[0129] Solve the current j by equation 1.15 max and the value of T2;

[0130] Through the above three steps, the curve graph of the trajectory and the parameters of each segment of the trajectory are substituted into the position equation to obtain the position at any time. When executing a certain segment of the trajectory, multi-axis time synchronization trajectory planning is first performed to obtain key parameters. When calculating the trajectory position, the amount of calculation can be reduced and the system operation efficiency can be improved. The multi-axis time synchronization trajectory planning algorithm flow is as follows: Figure 5 As shown, the rest is the same as Example 1.

[0131] In summary, this scheme plans the trajectories of the six axes independently and divides the robot motion into four states to obtain the maximum motion time. Based on this time, the trajectory is replanned by processing the four motion states of the robot and applying the time scaling method, so that it can move synchronously on multiple axes and meet the kinematic and dynamic requirements of the robot.

[0132] This patent proposes a multi-axis time-synchronous trajectory planning algorithm, which can realize multi-axis linkage and give full play to the motor servo performance while reducing the impact and vibration on the system.

[0133] The algorithm comprehensively analyzes the trajectory planning calculation strategies under four different states of the robot, realizes time synchronization and modifies the trajectory while reducing the amount of calculation, avoids the trajectory from violating the system saturation constraint restrictions, and meets the robot's kinematic and dynamic requirements.

[0134] Figure 6 The figure shows the effect comparison of an axis before and after using the time-synchronized trajectory planning algorithm. From the figure, we can observe that the starting and ending positions of the two planned movements are the same, the movement time is synchronized from 1.28s to 2s, the movement time is lengthened, and the speed acceleration does not exceed the limit, which is in line with the expected effect.

[0135] Figure 7 and Figure 8 The effect diagram of the 6-axis multi-axis time synchronization trajectory planning algorithm before and after is shown. Figure 7 The motion data curve of non-time-synchronous trajectory planning is shown, and it can be seen that the motion time of each axis is different. Figure 8 The time-synchronous trajectory planning motion data curve is displayed. The motion time of the six axes is unified into one time, and the starting and ending points of each axis remain unchanged. The acceleration and deceleration sections of each axis are basically concentrated in the front and back parts of the whole period. Each axis moves and stops at the same time, and has similar motion trends, which is in line with the expected effect.

Claims

1. A multi-axis time-synchronized S-shaped trajectory planning algorithm, characterized by: The following steps are included: S1. Use the segmented S-type trajectory planning method to independently plan the trajectory of each axis, including the following steps: 1) Determine whether the maximum acceleration can be reached; 2) Determine whether there is a uniform speed segment; 3) Determine whether the acceleration and deceleration sections can reach the maximum acceleration and deceleration; S2, 6-axis motion time adjustment: set the new motion time of the remaining 5 axes to the longest motion axis time; S3, using the parameters of each axis redefined in step S2, the 6-axis time is synchronously scaled and the joint motion trajectory is replanned; S4. Calculate the position of any trajectory based on the obtained parameters.

2. The multi-axis time-synchronized S-shaped trajectory planning algorithm according to claim 1, characterized in that: Step S1 also includes the following steps: the entire motion process is divided into an acceleration section, a uniform acceleration section, a deceleration section, a uniform speed section, an acceleration and deceleration section, a uniform deceleration section, and a deceleration and deceleration section; q0 is the initial position, q1 is the final position, V0 is the initial speed, V1 is the final speed, V max is the maximum speed, a max is the maximum acceleration, j max is the maximum jerk, T j1 is the acceleration time, T a is the acceleration time, T v is the uniform speed period, T j2 T is the deceleration time. d is the deceleration period; 1) Determining whether the maximum acceleration can be reached includes the following steps: Assume that the maximum speed V can be reached max , but there is no uniform acceleration segment, V′0 is the velocity at the end of the jerk segment, and the jerk segment can be obtained: a is the acceleration at any time in the deceleration and acceleration section, and the deceleration and acceleration section can be obtained as: According to formula 1.1 and formula 1.2, we can get if This means that the acceleration reaches its maximum value, which will cause the speed in the acceleration phase to exceed the maximum speed limit. In this case, a max To make adjustments, the process is as follows: if It means that the maximum acceleration can be reached, there is a uniform acceleration segment, and the time of the uniform acceleration segment is T j1匀 , The parameters obtained are as follows: S is the total path length of the motion, and the steps to calculate the uniform speed segment time are as follows: T d The solution process and T a Similarly, the parameters of the deceleration phase of trajectory planning can be obtained by the same logic; 2) Determining whether there is a uniform speed segment includes the following steps: When T V >0, indicating a uniform speed segment, the total motion planning time T = T a +T V +T d , define the state at this time as state 1, State = 1, the trajectory is a 7-segment trajectory, and this state will be used in subsequent multi-axis time synchronization planning; When T V When <0, it means there is no uniform speed segment and the maximum speed cannot be reached; assuming that both the maximum acceleration and the maximum deceleration can be reached, the trajectory at this time has an acceleration segment, a uniform acceleration segment, an acceleration / deceleration segment, a deceleration / acceleration segment, a uniform deceleration segment, and a deceleration / deceleration segment; According to formula 1.4 and formula 1.6, V max The values ​​are as follows: V max Bring it into T a , T d The values ​​of both can be obtained in the calculation formula; 3) Determining whether the acceleration and deceleration stages can reach the maximum acceleration and deceleration includes the following steps: T has been found in step 2). a , T d If the value of T a <0 means there is no acceleration segment, only deceleration segment, and the state at this time is defined as state 2, State = 2, and the trajectory is a 3-segment trajectory; if T d <0 means there is no deceleration segment, only acceleration segment, and the state at this time is defined as state 3, State=3, and the trajectory is a 3-segment trajectory; if T a >=2×T j1 &&T d >=2×T j2 , the state at this time is defined as state 4, State = 4, the trajectory has no uniform speed segment; when T a <2×T j1 ||T d <2×T j2 When , it means that the acceleration section or deceleration section cannot reach the maximum acceleration. The maximum acceleration value needs to be adjusted to meet the conditions. Use Newton iteration method to reduce the maximum acceleration value and recalculate V max , T a , T d , until it satisfies T a >=2×T j1 &&T d >=2×T j2 .

3. The multi-axis time-synchronized S-shaped trajectory planning algorithm according to claim 2, characterized in that: Step S2 specifically includes: taking the longest motion axis time T max As a benchmark, the new motion time T of the remaining 5 axes is N Set this time to the T of each axis. a , T d , T V , T j1 , T j2 , V max , a max , j max , q1, q0 are passed to step S3.

4. The multi-axis time-synchronized S-shaped trajectory planning algorithm according to claim 3, characterized in that: Step S3 also includes: calculating the movement time and total time of each segment through step S1, taking the new movement time as the time for re-planning, and using the scaling factor to scale the acceleration segment, the uniform speed segment, and the deceleration segment respectively. In step S1, when performing single-axis planning, the trajectory has been divided into 4 states, which should be considered separately during scaling; State 1 is a 7-segment S-shaped trajectory, set the scaling factor Multiply the time of each segment by the scaling factor λ to get the new acceleration segment time T aN , uniform speed period T VN , deceleration time T dN , at this time the maximum speed is set to V maxN , calculate the movement distance S before and after scaling respectively, as shown in formula 1.8: According to formula 1.8, we can get V maxN The value of is shown in formula 1.9: Find a new V maxN After that, we need to find the uniform acceleration time T j1N and the maximum acceleration a maxN , V′0 is the speed at the end of the acceleration segment, V″0 is the speed at the end of the uniform acceleration segment, and V″′0 is the speed at the end of the deceleration segment; According to equations 1.9 and 1.10, we can solve for T j1N The values ​​are as follows: At this time, T j1N Take negative values; To obtain the parameters of all trajectory replanning in state 1, as shown in formula 1.12: State 4 is a trajectory without a uniform velocity segment. The time scaling calculation method is similar to that of state 1. The formula T VN and V maxN The calculation formula is different. The uniform speed section is removed during the calculation, and the state 4 parameters are obtained as shown in formula 1.13: State 2 is the deceleration stage. Solve the current j by equation 1.14 max and the value of T2; State 3 is the acceleration stage, T1 is the time of the uniform acceleration stage, and T2 is the time of the deceleration stage. Solve the current j by equation 1.15 max and the value of T2.

5. The multi-axis time-synchronized S-shaped trajectory planning algorithm according to claim 4, characterized in that: Substitute the curve graph of the trajectory and the parameters of each segment of the trajectory into the position equation to obtain the position at any time.

Images