Mechanical arm trajectory planning method based on hybrid polynomial and acceleration smoothing

Abstract

The invention provides a mechanical arm trajectory planning method based on hybrid polynomial and acceleration smoothing, relates to the technical field of mechanical arm control, and solves the limitation problems of unsmooth response, impact and the like of a mechanical arm caused by an acceleration sudden change phenomenon in an application process. The method comprises the following steps: for two target track points, describing a motion track of a mechanical arm by adopting a quintic polynomial; for the three target trajectory points including the middle point, the speed and the acceleration of the middle point are determined firstly, and then trajectory planning between the two points is used; for four or more target trajectory points, a cubic polynomial interpolation method is adopted, so that a formed trajectory curve meets the continuity of the position, the speed and the acceleration; the acceleration smoothness is realized by detecting and setting the time of two adjacent sections of curves on the trajectory curve. According to the method, for track point scenes with different points, the most suitable polynomial is adopted for track planning, and sudden change of the acceleration is effectively avoided through smooth processing of the acceleration.

Description

Technical Field

[0001] The present invention relates to the technical field of robotic arm control, and particularly to a robotic arm trajectory planning method based on hybrid polynomials and acceleration smoothing. Background Art

[0002] With the rapid development of intelligent manufacturing technology, industrial robotic arms have been widely used in advanced manufacturing fields such as precision assembly, material handling, and welding processing. In complex working conditions, the end effector of the robotic arm needs to complete high-speed and high-precision spatial motion along a predetermined trajectory, which poses strict requirements on the trajectory planning ability of the motion control system. As an intermediate link connecting path planning and motion control, the core task of trajectory planning is to generate a continuous and smooth trajectory that satisfies kinematic constraints, which directly affects the motion performance and operation quality of the robotic arm.

[0003] Existing trajectory planning methods generally use polynomial interpolation algorithms to construct motion trajectories in joint space or Cartesian space. Among them, although the linear interpolation algorithm is computationally simple, the generated trajectory has a step-like jump in terms of speed, resulting in the actuator being subjected to instantaneous impact loads. Cubic spline interpolation effectively improves the smoothness of the velocity curve by introducing acceleration continuity constraints and has become the most widely used trajectory generation method in industrial controllers. However, the acceleration derivative of this method is still discontinuous at path nodes. When the robotic arm is in high-speed and heavy-load working conditions, such mutations in high-order motion parameters will cause mechanical vibrations, resulting in end-point positioning errors, seriously affecting operation tasks that require strict motion smoothness, such as precision assembly.

[0004] Theoretical research shows that the root cause of acceleration mutations is that traditional cubic spline curves can only ensure acceleration continuity and cannot achieve a smooth transition of acceleration derivatives. When the robotic arm performs high-speed start-stop or path-switching actions, this high-order discontinuity will accumulate elastic potential energy in the transmission system, not only accelerating the mechanical fatigue of key components such as harmonic reducers but also causing torque oscillations in servo motors. Existing improvement schemes mostly use fifth-order polynomial interpolation to improve the trajectory smoothness, but the resulting complexity of solving non-linear equations increases exponentially, facing double challenges of computational resources and response speed in real-time control scenarios.

[0005] In recent years, some studies have attempted to introduce S-shaped acceleration and deceleration curves to optimize the acceleration profile and achieve smooth transitions by presetting speed inflection points. Although such methods improve the dynamic characteristics, the fixed-mode acceleration curves are difficult to adapt to the geometric features of complex paths, and residual vibrations will still occur when connecting asymmetric trajectories or multi-segment continuous trajectories. In addition, although the trajectory optimization scheme based on heuristic algorithms can obtain the theoretically optimal solution, its random search mechanism leads to excessive computational time and is difficult to meet the real-time control requirements of high-dynamic scenarios.

[0006] In current industrial applications, trajectory planning algorithms need to make a trade-off between computational efficiency and motion quality, and this contradiction is particularly prominent in high-speed and precision operation scenarios. How to construct a trajectory planning method with both high smoothness and low computational complexity to achieve continuous transition of various motion parameters during the movement of the robotic arm has become a key technical bottleneck for improving the motion control performance of high-end equipment. Summary of the Invention

[0007] Based on the current situation in the background technology, the purpose of the present invention is to solve the limitation problems in high-speed and precision operation scenarios, such as the uneven response of the robotic arm, impact, and serious influence on motion accuracy and system stability caused by the sudden change of acceleration. Therefore, a robotic arm trajectory planning method based on hybrid polynomials and acceleration smoothing is proposed. In the process of robotic arm trajectory planning, the present invention integrates quintic polynomials, cubic polynomials, and acceleration smoothing technology. For trajectory point scenarios with different numbers of points, the most suitable polynomial is used for trajectory planning, and through the smoothing process of acceleration, the sudden change of acceleration is effectively avoided.

[0008] The present invention adopts the following technical solutions to achieve the purpose:

[0009] A robotic arm trajectory planning method based on hybrid polynomials and acceleration smoothing, the method includes the following steps:

[0010] S1. Obtain the number of target trajectory points and the corresponding motion parameter requirements in the trajectory planning task, and adopt different trajectory planning schemes according to different numbers of target trajectory points; the target trajectory points are three-dimensional space points corresponding to the starting point, intermediate point, or end point of the preset end point of the robotic arm on the planned trajectory.

[0011] S2. For the trajectory planning between two target trajectory points, use a quintic polynomial to describe the motion trajectory of the robotic arm.

[0012] S3. For the trajectory planning between three target trajectory points including an intermediate point, first determine the speed and acceleration corresponding to the intermediate point, and then use the trajectory planning scheme between two target trajectory points in step S2 for the trajectory planning between the intermediate point and any other target trajectory point.

[0013] S4. For the trajectory planning between four or more target trajectory points, use the cubic polynomial interpolation method to make the trajectory curve formed by multiple target trajectory points satisfy the continuity of position, speed, and acceleration; by detecting and setting the time of adjacent two segments of the curve on the trajectory curve, the smoothing of acceleration at multiple target trajectory points is realized.

[0014] Specifically, in step S2, the form of the adopted quintic polynomial is as follows:

[0015] θ(t) = a + bt + ct2 +dt 3 +et 4 +ft 5

[0016] Wherein, θ(t) represents the change of the position θ of the preset end point of the robotic arm with time t; a, b, c, d, e, and f are the coefficients of the fifth-degree polynomial to be solved.

[0017] Specifically, taking two target trajectory points as the starting point and the ending point respectively, after obtaining the positions, velocities, and accelerations corresponding to the starting point and the ending point respectively, six constraint conditions are formed as follows:

[0018] θ 0 = θ 0 (0) = a

[0019]

[0020] θ T = θ 0 (T) = a + bT + cT 2 + dT 3 + eT 4 + fT 5

[0021]

[0022] Wherein, θ 0 , v 0 and a cc0 represent the position, velocity, and acceleration of the starting point respectively, θ T , v T and a ccT represent the position, velocity, and acceleration of the ending point respectively, and T represents the time required for the trajectory curve from the starting point to the ending point; by simultaneously solving the corresponding equations of the above six constraint conditions, the coefficients of the fifth-degree polynomial are obtained, and the trajectory planning between two target trajectory points is realized.

[0023] Specifically, in step S3, for the trajectory curve starting from the starting point and reaching the ending point after passing through the intermediate point, the intermediate point divides the trajectory curve into two front and back curve segments; the solution of the velocity v 1 corresponding to the intermediate point is as follows:

[0024] v ave0 = (S 1 - S 0 ) / T 0

[0025] v ave1 = (S 2 - S 1 ) / T 1

[0026] v 1 = (v ave0 + v ave1 ) / 2

[0027] In the formula, v ave0 and v ave1 respectively represent the average velocities corresponding to the two sections of the curve; S 0 , S 1 and S 2 respectively represent the corresponding positions of the starting point, the middle point and the end point; T 0 and T 1 respectively represent the times corresponding to the two sections of the curve.

[0028] Furthermore, in step S3, for the trajectory curve starting from the starting point and reaching the end point after passing through the middle point, the middle point divides the trajectory curve into two sections of curves; if the position change mode from the starting point to the middle point is the same as the position change mode from the middle point to the end point, that is, it represents that the change of the robotic arm rotation angle at the middle point position is a uniform increase or decrease, the speed at the middle point can be determined to be the same as the speed when the robotic arm is in uniform motion. At this time, the acceleration a cc1 of the middle point is directly set to 0;

[0029] If the position change mode from the starting point to the middle point is opposite to the position change mode from the middle point to the end point, it represents that the speeds on both the first section of the curve and the second section of the curve have processes of increase and decrease; first, calculate the average velocities of the two sections of the curve, as shown in the following formula:

[0030] v ave0 = (S 1 - S 0 ) / T 0

[0031] v ave1 = (S 2 - S 1 ) / T 1

[0032] In the formula, v ave0 and v ave1 respectively represent the average velocities corresponding to the two sections of the curve; S 0 , S 1 and S 2 respectively represent the corresponding positions of the starting point, the middle point and the end point; T 0 and T 1 respectively represent the times corresponding to the two sections of the curve; at this time, let the maximum velocity of the first section of the curve be 2v ave0 , let the maximum velocity of the second section of the curve be 2v ave1 , and take the time from the maximum velocity of the first section of the curve to the maximum velocity of the second section of the curve as (T 0 + T1 ) / 2, the acceleration a of the midpoint cc1 That is, set it as:

[0033] a cc1 = 4(|v ave0 - v ave1 |) / (T 0 + T 1 )

[0034] If the speed on the first curve remains unchanged and only the speed on the second curve increases or decreases, when calculating the average speed v of the second curve ave1 , let the maximum speed of the second curve be 2v ave1 , determine the acceleration a corresponding to the second curve cc = 4v ave1 / T 1 ; Subsequently, set the acceleration a of the midpoint cc1 as a cc1 = a cc / 2 = 2v ave1 / T 1 or a cc1 = a cc / 4 = v ave1 / T 1 .

[0035] Specifically, in step S4, for the trajectory curve formed by four or more target trajectory points in sequence, this trajectory curve is divided into multiple curves by the target trajectory points; the mathematical representation of the cubic polynomial corresponding to the i-th curve is as follows:

[0036] θ i (t) = a i + b i t + c i t 2 + d i t 3

[0037] In the formula, θ i (t) represents the position of the i-th curve at time t, and a i , b i , c i , d i are the undetermined coefficients of the i-th curve.

[0038] Furthermore, the adjacent curve segments satisfy the continuity constraint conditions of position, speed, and acceleration; after recording the time required for the i-th curve as T i , using the cubic polynomial interpolation method, the respective constraint conditions to be satisfied are as follows:

[0039] Position continuity, that is, the positions of adjacent curve segments are equal at the connection points:

[0040] θ i (T i ) = θ i+1 (0) = a i +b i T i +c i T i 2 +d i T i 3 = a i+1

[0041] Velocity continuity, that is, the velocities of adjacent curve segments are equal at the connection points:

[0042]

[0043] Acceleration continuity: that is, the accelerations of adjacent curve segments are equal at the connection points:

[0044]

[0045] Denote the acceleration of each target trajectory point as m i , and the position as S i After that, let m i = 2c i , and then according to the continuity constraint conditions of position, velocity and acceleration, the following equations are obtained:

[0046]

[0047] After constructing the corresponding matrix relationship through the above equations, solve the acceleration vector m, and then obtain the parameters corresponding to each curve segment.

[0048] Specifically, denote the total number of curve segments in the trajectory curve as n, then the total number of target trajectory points is n + 1. According to the above equations, the matrix relationship is obtained: Am = 6k; where A is the matrix formed by the left side of the equation, k is the matrix formed by the right side of the equation, and m represents the acceleration vector; after n curve segments correspond to n equations, the left side of each equation can be combined into matrix A, and the right side can be combined into matrix k, as follows:

[0049]

[0050] The acceleration vector m is calculated through LU decomposition of the equation corresponding to the above matrix relationship Am = 6k, and then the corresponding acceleration value is determined; when the acceleration value is determined, according to the continuity constraint conditions of position, velocity and acceleration satisfied between adjacent curve segments, the parameters corresponding to each curve segment can be obtained.

[0051] Preferably, in step S4, for a trajectory curve formed by four or more target trajectory points in sequence, the trajectory curve is divided into multiple segments of curves by the target trajectory points, and the time required for the i-th segment of curve is denoted as T i ; after the planning of the trajectory curve is completed, the times T i and T i±1 corresponding to two adjacent segments of curves are detected, and it is determined whether to perform acceleration smoothing operation only when both the times T i and T i±1 are less than a preset judgment threshold value.

[0052] Specifically, when judging, first detect whether the position change mode corresponding to the first segment of curve and the position change mode corresponding to the second segment of curve in two adjacent segments of curves are the same. The position change mode includes both the rotation direction and the linear movement direction of the robotic arm; when the position change modes are the same, no acceleration smoothing operation is performed; when the position change modes are different, change the time corresponding to the second segment of curve and set it to a preset smoothing time value.

[0053] In summary, due to the adoption of this technical solution, the beneficial effects of the present invention are as follows:

[0054] The present invention combines different-order polynomials with acceleration smoothing technology to significantly improve the motion quality of the robotic arm while ensuring trajectory continuity. For the multi-trajectory point planning scenario, a quintic polynomial is used to construct the trajectory of the key section, realizing the high-order smoothing characteristic of continuous acceleration derivative, and fundamentally eliminating the acceleration mutation problem of the traditional cubic spline interpolation at the path nodes; at the same time, by switching the cubic polynomial, the computational complexity is greatly reduced on the premise of ensuring the basic smoothness. By dynamically adjusting the polynomial order and time interval parameters, the acceleration curve shows a progressive transition at the trajectory connection, effectively suppressing the elastic vibration of the transmission system and the servo torque fluctuation.

[0055] The acceleration smoothing technology introduced in the present invention realizes the global optimization of motion parameters while maintaining the trajectory generation efficiency by restricting the acceleration change rate of adjacent sections. Compared with the single polynomial interpolation method, this hybrid strategy combines high-order trajectory smoothness and real-time calculation feasibility. Especially when dealing with multi-segment continuous trajectories or asymmetric paths, it can avoid the residual oscillation generated by the fixed-mode acceleration and deceleration curve. In practical applications, this method can significantly reduce the positioning deviation of the end effector of the robotic arm, improve the dynamic response stability in the high-speed precision operation scenario, and at the same time extend the service life of the key transmission components. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 is a schematic diagram briefly showing the overall process of the method of the present invention;

[0057] Figure 2 Schematic diagram of trajectory planning between two target trajectory points in the present invention;

[0058] Figure 3 Schematic diagram of trajectory planning for acceleration solution case 1 among three points in the present invention;

[0059] Figure 4 Schematic diagram of trajectory planning for acceleration solution case 2 among three points in the present invention;

[0060] Figure 5 Schematic diagram of trajectory planning for acceleration solution case 3 among three points in the present invention;

[0061] Figure 6 Schematic diagram of test effect before acceleration smoothing in the present invention;

[0062] Figure 7 Schematic diagram of test effect after acceleration smoothing in the present invention. Detailed implementation manners

[0063] To make the objectives, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. Usually, the components of the embodiments of the present invention described and illustrated herein can be arranged and designed in various different configurations.

[0064] Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed present invention, but merely represents selected embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the protection scope of the present invention.

[0065] Embodiment

[0066] A robotic arm trajectory planning method based on hybrid polynomials and acceleration smoothing. The overall process of this method can be seen in the Figure 1 brief schematic illustration. It integrates quintic polynomials, cubic polynomials and acceleration smoothing technology. Under different numbers of target trajectory points, appropriate polynomials are used for trajectory planning, and acceleration smoothing processing is also adopted to avoid sudden changes in acceleration. The steps of the method are summarized as follows:

[0067] S1. Obtain the number of target trajectory points and the corresponding motion parameter requirements in the trajectory planning task, and adopt different trajectory planning schemes according to different numbers of target trajectory points; the target trajectory points are three-dimensional space points corresponding to the starting point, intermediate point or end point of the preset end point of the robotic arm on the planned trajectory;

[0068] S2. For the trajectory planning between two target trajectory points, a quintic polynomial is used to describe the motion trajectory of the robotic arm;

[0069] S3. For the trajectory planning between three target trajectory points including an intermediate point, first determine the velocity and acceleration corresponding to the intermediate point, and then for the trajectory planning between this intermediate point and any other target trajectory point, adopt the trajectory planning scheme between two target trajectory points in step S2;

[0070] S4. For the trajectory planning between four or more target trajectory points, adopt the cubic polynomial interpolation method to make the trajectory curve formed by multiple target trajectory points satisfy the continuity of position, velocity and acceleration; by detecting and setting the time of adjacent two segments of the trajectory curve, the smoothness of the acceleration at multiple target trajectory points is realized.

[0071] In step S2, a quintic polynomial is used to describe the motion trajectory between two target trajectory points. Its main goal is to optimize the motion between the control target trajectory points to make the changes of acceleration and velocity smoother. The form of the quintic polynomial adopted in this embodiment is as follows:

[0072] θ(t) = a + bt + ct 2 + dt 3 + et 4 + ft 5

[0073] In the formula, θ(t) represents the change of the position θ of the preset end point of the robotic arm with time t; a, b, c, d, e, f are the coefficients of the quintic polynomial to be solved.

[0074] See simultaneously Figure 2 For the schematic diagram of, take the two target trajectory points as the starting point and the ending point respectively. After obtaining the positions, velocities and accelerations corresponding to the starting point and the ending point respectively, six constraint conditions are formed as follows:

[0075] θ 0 = θ 0 (0) = a

[0076]

[0077] θ T = θ 0 (T) = a + bT + cT 2 + dT 3 + eT 4 + fT 5

[0078]

[0079] In the formula, θ 0 、v0 and a cc0 respectively represent the position, velocity, and acceleration of the starting point (labeled as a in Figure 2 ), θ 0 ), θ T , v T , and a ccT respectively represent the position, velocity, and acceleration of the ending point (correspondingly labeled as θ Figure 2 , v 1 , and a 1 in 1 ), and T represents the time required for the trajectory curve from the starting point to the ending point; after simultaneously solving the corresponding equations of the above six constraint conditions, the coefficients of the fifth-degree polynomial are obtained, realizing the trajectory planning between two target trajectory points.

[0080] In step S3, the motion trajectory between three target trajectory points is also described by a fifth-degree polynomial. Only by first obtaining the velocity v 1 and acceleration a cc1 of the intermediate point can the two-point method be further used for solution. For the trajectory curve starting from the starting point, passing through the intermediate point, and then reaching the ending point, the intermediate point divides the trajectory curve into two front and back sections of curves; the solution of the velocity v 1 corresponding to the intermediate point is as follows:

[0081] v ave0 = (S 1 - S 0 ) / T 0

[0082] v ave1 = (S 2 - S 1 ) / T 1

[0083] v 1 = (v ave0 + v ave1 ) / 2

[0084] In the formula, v ave0 and v ave1 respectively represent the average velocities corresponding to the two sections of curves; S 0 , S 1 , and S 2 respectively represent the corresponding positions of the starting point, the intermediate point, and the ending point; T 0 and T 1 respectively represent the times corresponding to the two sections of curves.

[0085] In this embodiment, the acceleration a cc1 of the intermediate point will be divided into multiple cases; first, please refer to Figure 3As shown in the figure, if the position change mode from the starting point to the midpoint is the same as that from the midpoint to the end point, it means that when the change in the rotation angle of the robotic arm at the midpoint position is a uniform increase or decrease (the uniform increase or decrease of the rotation angle of the robotic arm represents a constant moving speed of its endpoint trajectory), it can be determined that the speed at the midpoint is the same as the speed when the robotic arm is in uniform motion. At this time, the acceleration a of the midpoint cc1 ( Figure 3 The a in 1 ) is directly set to 0. Figure 3 In, the physical quantity with subscript 0 corresponds to the starting point, the physical quantity with subscript 1 corresponds to the midpoint, and the physical quantity with subscript 2 corresponds to the end point.

[0086] For example Figure 4 As shown, if the position change mode from the starting point to the midpoint is opposite to that from the midpoint to the end point, it means that the speeds on both the first curve and the second curve have processes of increase and decrease. In this case, first calculate the average speeds of the two curves as follows:

[0087] v ave0 =(S 1 -S 0 ) / T 0

[0088] v ave1 =(S 2 -S 1 ) / T 1

[0089] In the formula, v ave0 and v ave1 respectively represent the average speeds corresponding to the two curves; S 0 , S 1 and S 2 respectively represent the corresponding positions of the starting point, the midpoint, and the end point; T 0 and T 1 respectively represent the times corresponding to the two curves; at this time, let the maximum speed of the first curve be 2v ave0 , let the maximum speed of the second curve be 2v ave1 , and take the time from the maximum speed of the first curve to the maximum speed of the second curve as (T 0 +T 1 ) / 2, then the acceleration a cc1 of the midpoint is set as:

[0090] a cc1 =4(|v ave0 -v ave1 |) / (T 0 +T 1 )

[0091] Figure 4 Among them, the physical quantity with subscript 0 corresponds to the starting point, the physical quantity with subscript 1 corresponds to the middle point, and the physical quantity with subscript 2 corresponds to the ending point.

[0092] As Figure 5 shown, if the speed on the first curve remains unchanged and only the speed on the second curve has an increasing or decreasing process, when calculating the average speed v of the second curve ave1 After that, let the maximum speed of the second curve be 2v ave1 , and determine the acceleration a corresponding to the second curve cc = 4v ave1 / T 1 ; Subsequently, in order to make the acceleration smoothly transition from the first curve to the second curve, 1 / 2 or 1 / 4 of the acceleration a cc can be taken, that is, the acceleration a at the middle point cc1 is set to a cc1 = a cc / 2 = 2v ave1 / T 1 or a cc1 = a cc / 4 = v ave1 / T 1 . Figure 5 Among them, the physical quantity with subscript 0 corresponds to the starting point, the physical quantity with subscript 1 corresponds to the middle point, and the physical quantity with subscript 2 corresponds to the ending point.

[0093] In step S4, for the trajectory curve successively formed by four or more target trajectory points, the trajectory curve is divided into multiple curves by the target trajectory points; the mathematical representation of the cubic polynomial corresponding to the i-th curve is as follows:

[0094] θ i (t) = a i + b i t + c i t 2 + d i t 3

[0095] In the formula, θ i (t) represents the position of the i-th curve at time t, and a i , b i , c i , d i are the undetermined coefficients of the i-th curve.

[0096] To ensure the smoothness of the curve, the continuity constraint conditions of position, speed, and acceleration need to be satisfied between adjacent curve segments; after recording the time required for the i-th curve as T i , using the cubic polynomial interpolation method, the respective constraint conditions to be satisfied are as follows:

[0097] Position continuity, that is, the positions of adjacent curve segments are equal at the connection points:

[0098] θ i (T i ) = θ i+1 (0) = a i +b i T i +c i T i 2 +d i T i 3 = a i+1

[0099] Velocity continuity, that is, the velocities (first derivatives) of adjacent curve segments are equal at the connection points:

[0100]

[0101] Acceleration continuity: that is, the accelerations (second derivatives) of adjacent curve segments are equal at the connection points:

[0102]

[0103] Denote the acceleration of each target trajectory point as m i , and the position as S i After that, let m i = 2c i , and then according to the continuity constraint conditions of position, velocity and acceleration, the following equations are obtained:

[0104]

[0105] By the above equations, after constructing the corresponding matrix relationship, solve the acceleration vector m, and then obtain the parameters corresponding to each curve segment.

[0106] In this embodiment, denote the total number of curve segments in the trajectory curve as n, then the total number of target trajectory points is n + 1. According to the above equations, the matrix relationship is obtained: Am = 6k; where A is the matrix formed by the left side of the equation, k is the matrix formed by the right side of the equation, and m represents the acceleration vector; after n curve segments correspond to n equations, the left parts of each equation can be combined into matrix A, and the right parts can be combined into matrix k, as follows:

[0107]

[0108]

[0109] The equation corresponding to the matrix relationship Am = 6k calculates the acceleration vector m through LU decomposition, and then determines the corresponding acceleration value; after the acceleration value is determined, the parameters corresponding to each curve segment can be obtained according to the continuity constraint conditions of position, velocity, and acceleration satisfied between adjacent curve segments.

[0110] In the above cubic polynomial interpolation method for four or more target trajectory points in this embodiment, the time of each curve segment is obtained by dividing the position difference by an average velocity. When the position difference of a certain middle curve segment is very small, its corresponding time will also be very small. If the times of two adjacent curve segments are both very small, and the position of the previous curve segment increases (the rotation angle of the robotic arm becomes larger) while the position of the subsequent curve segment decreases (the rotation direction of the robotic arm is opposite and the rotation angle becomes smaller), it is necessary to complete the speed transformation in a very short time, which will generate a large acceleration and cause the robotic arm to vibrate violently.

[0111] To overcome the above situation, this embodiment preferably adopts the following method for processing:

[0112] Denote the time required for the i-th curve segment as T i , after the trajectory curve is planned, detect the times T i and T i±1 corresponding to two adjacent curve segments, and only when both T i and T i±1 are less than a preset judgment threshold, determine whether to perform the acceleration smoothing operation. In this embodiment, taking the case where the standard rotation angular velocity of the robotic arm is 30 degrees per second as an example, based on the empirical value of the average velocity in practice, the judgment threshold can be set to 0.3 seconds; only after the times of the previous curve segment and the adjacent subsequent curve segment are both less than 0.3 seconds, determine whether to perform the acceleration smoothing operation.

[0113] If the time condition is satisfied, when judging, first detect whether the position change mode corresponding to the first curve segment and the position change mode corresponding to the second curve segment in two adjacent curve segments are the same. The position change mode includes the rotation direction of the robotic arm and the linear movement direction, etc.; when the position change modes are the same, there will be no large-scale speed transformation, so the acceleration smoothing operation is not performed; when the position change modes are different, that is, change the time corresponding to the second curve segment and set it to a preset smoothing time value; in this embodiment, the smoothing time value can be the same as the value of the judgment threshold, that is, also 0.3 seconds.

[0114] The effect of the acceleration smoothing operation can be compared with Figure 6 and Figure 7 , where Figure 6 is the effect without smoothing, Figure 7In the case of two adjacent curves with times both less than 0.3 seconds, the time of the latter curve is set to 0.3 seconds to achieve smooth acceleration.

Claims

1. A robot arm trajectory planning method based on mixed polynomial and acceleration smoothing, characterized in that: The steps include: S1. Obtain the number of target trajectory points and the corresponding motion parameter requirements in the trajectory planning task, and adopt different trajectory planning schemes according to the different numbers of target trajectory points; the target trajectory point is the three-dimensional space point corresponding to the starting point, middle point or end point of the preset endpoint of the robot arm on the planned trajectory; S2. For trajectory planning between two target trajectory points, a fifth-order polynomial is used to describe the motion trajectory of the robot arm; S3, for the trajectory planning between the three target trajectory points including the middle point, first determine the speed and acceleration corresponding to the middle point, then the trajectory planning between the middle point and any other target trajectory point adopts the trajectory planning scheme between the two target trajectory points in step S2; S4. For trajectory planning between four or more target trajectory points, a cubic polynomial interpolation method is used to ensure that the trajectory curve formed by multiple target trajectory points meets the continuity of position, velocity and acceleration; by detecting and setting the time of two adjacent curves on the trajectory curve, the acceleration smoothing at multiple target trajectory points is achieved.

2. The robot arm trajectory planning method according to claim 1, characterized in that: In step S2, the fifth-order polynomial used is in the following form: θ(t)=a+bt+ct 2 +dt 3 +et 4 +ft 5 Where θ(t) represents the change of the position θ of the preset endpoint of the robot arm with time t; a, b, c, d, e, and f are the coefficients of the quintic polynomial to be solved.

3. The robot arm trajectory planning method according to claim 2, characterized in that: The two target trajectory points are taken as the starting point and the end point respectively. After obtaining the position, velocity and acceleration corresponding to the starting point and the end point respectively, six constraints are formed as follows: θ0=θ0(0)=a θ T =θ0(T)=a+bT+cT 2 +dT 3 +eT 4 +fT 5 In the formula, θ0, v0 and a cc0 Represent the position, velocity and acceleration of the starting point, θ T 、v T and a ccT They represent the position, velocity and acceleration of the end point respectively, and T represents the time required for the trajectory curve from the starting point to the end point. By jointly solving the corresponding formulas of the above 6 constraints, the coefficients of the quintic polynomial are obtained to realize the trajectory planning between the two target trajectory points.

4. The robot arm trajectory planning method according to claim 1, characterized in that: In step S3, for a trajectory curve starting from the starting point and reaching the end point after passing through the middle point, the middle point divides the trajectory curve into two segments, the front and back segments; the speed v1 corresponding to the middle point is solved as follows: v ave0 =(S1-S0) / T0 <h2 style=";text-align:left;direction:ltr">v<h2 style=";text-align:left;direction:ltr"> ave1 <h2 style=";text-align:left;direction:ltr"> (S2-S1) / T1 v1=(v ave0 +v ave1 ) / 2 In the formula, v ave0 and v ave1 That is, they represent the average speeds corresponding to the two curves respectively; S0, S1 and S2 represent the corresponding positions of the starting point, the middle point and the end point respectively; T0 and T1 represent the time corresponding to the two curves respectively.

5. The robot arm trajectory planning method according to claim 1, characterized in that: In step S3, for the trajectory curve starting from the starting point and reaching the end point after passing the middle point, the middle point divides the trajectory curve into two segments, front and back. If the position change mode from the starting point to the middle point is the same as the position change mode from the middle point to the end point, that is, when the change of the rotation angle of the robot arm at the middle point position is a uniform increase or decrease, it can be determined that the speed of the middle point is the same as the speed of the robot arm when it is in uniform motion. At this time, the acceleration a of the middle point is cc1 Set it to 0 directly; If the position change from the starting point to the middle point is opposite to the position change from the middle point to the end point, it means that the speed on the first curve and the second curve both increase and decrease. First, calculate the average speed of the two curves, as follows: v ave0 =(S1-S0) / T0 <h2 style=";text-align:left;direction:ltr">v<h2 style=";text-align:left;direction:ltr"> ave1 <h2 style=";text-align:left;direction:ltr"> (S2-S1) / T1 In the formula, v ave0 and v ave1 That is, they represent the average speeds of the two curves; S0, S1 and S2 represent the corresponding positions of the starting point, the middle point and the end point respectively; T0 and T1 represent the time corresponding to the two curves respectively; at this time, let the maximum speed of the first curve be 2v ave0 , let the maximum speed of the second curve be 2v ave1 , and the time from the maximum speed of the first curve to the maximum speed of the second curve is taken as (T0+T1) / 2, the acceleration a of the middle point cc1 That is, set it to: a cc1 =4(|v ave0 -v ave1 |) / (T0+T1) If the speed on the first curve segment remains unchanged and only the speed on the second curve segment increases or decreases, when calculating the average speed v of the second curve segment, ave1 Then, let the maximum speed of the second curve be 2v ave1 , determine the acceleration a corresponding to the second curve cc =4v ave1 / T1; then, the acceleration a of the midpoint cc1 Set to a cc1 =a cc / 2=2v ave1 / T1 or a cc1 =a cc / 4=v ave1 / T1.

6. The robot arm trajectory planning method according to claim 1, characterized in that: In step S4, for a trajectory curve formed by four or more target trajectory points in sequence, the trajectory curve is divided into multiple curve segments by the target trajectory points; the mathematical expression of the cubic polynomial corresponding to the i-th curve segment is as follows: θ i (t)=a i +b i t+c i t 2 +d i t 3 In the formula, θ i (t) represents the position of the i-th curve at time t, a i , b i 、c i ,d i is the unknown coefficient of the i-th curve.

7. The robot arm trajectory planning method according to claim 6, characterized in that: The adjacent curve segments satisfy the continuity constraints of position, velocity and acceleration; the time required for the i-th curve segment is recorded as T i Finally, the cubic polynomial interpolation method is used, and the constraints that need to be met are as follows: Positional continuity, that is, adjacent curve segments have equal positions at the connection points: θ i (T i )=θ i+1 (0)=a i +b i T i +c i T i 2 +d i T i 3 =a i+1 Velocity continuity, that is, the velocities of adjacent curve segments are equal at the connection points: θ i (T i )=θ i+1 (0)→b i +2c i T i +3d i T i 2 =b i+1 Acceleration continuity: that is, the accelerations of adjacent curve segments are equal at the connection points: The acceleration of each target trajectory point is recorded as m i , the position is denoted as S i Then, let m i =2c i , and then according to the continuity constraints of position, velocity and acceleration, the following equation is obtained: Through the above equations, after constructing the corresponding matrix relationship, the acceleration vector m is solved, and then the parameters corresponding to each curve are obtained.

8. The robot arm trajectory planning method according to claim 7, characterized in that: The total number of curve segments in the trajectory curve is recorded as n, and the total number of target trajectory points is n+1. According to the above equation, the matrix relationship is: Am=6k; where A is the matrix on the left side of the equation, k is the matrix on the right side of the equation, and m represents the acceleration vector; after n segments of the curve correspond to n equations, the left side of each equation can be merged into matrix A, and the right side can be merged into matrix k, as follows: The equation corresponding to the above matrix relationship Am=6k is decomposed by LU to calculate the acceleration vector m, and then the corresponding acceleration value is determined; when the acceleration value is determined, the parameters corresponding to each curve segment can be obtained based on the continuity constraints of position, velocity and acceleration satisfied between adjacent curve segments.

9. The robot arm trajectory planning method according to claim 1, characterized in that: In step S4, for a trajectory curve formed by four or more target trajectory points in sequence, the trajectory curve is divided into multiple segments by the target trajectory points, wherein the time required for the i-th segment is recorded as T i ; After the trajectory curve is planned, the time T corresponding to the two adjacent curves is detected i and T i±1 , and at time T i and T i±1 When both are smaller than the preset judgment threshold, it is determined whether to perform the acceleration smoothing operation.

10. The robot arm trajectory planning method according to claim 9, characterized in that: When judging, first detect whether the position change mode corresponding to the first curve and the position change mode corresponding to the second curve in the two adjacent curves are the same, and the position change mode includes the rotation direction and linear movement direction of the robot arm; when the position change mode is the same, the acceleration smoothing operation is not performed; when the position change mode is different, change the time corresponding to the second curve and set it to the preset smoothing time value.

Images