Robot turning track smooth interpolation method and system based on vector synthesis and storage medium

Abstract

The invention discloses a robot turning trajectory smooth interpolation method and system based on vector synthesis, and a storage medium. The method comprises the following steps: S1, calculating the number of interpolation points before and after a trajectory inflection point B; s2, selecting a smaller number of interpolation points as a reference, and determining the number of interpolation points from a trajectory inflection point B to a separation point D and a regression point E in combination with a preset overlapping degree parameter alpha so as to determine the positions of the points D and E; s3, fine interpolation is carried out in the joint space through a joint angle vector or in the Cartesian space through a position and posture vector, and the interpolation process is carried out in a segmented mode; s4, vector synthesis is carried out on the DB section track and the BE section track to generate a smooth transition DE section track, and synthesis is carried out in a segmented mode; s5, replacing the DB and BE segments of the original trajectory with a DE segment as a whole, and completing trajectory smoothing; the method can smooth the track of the robot at the turning position in real time.

Description

TECHNICAL FIELD

[0001] The present application relates to robot motion planning, in particular to a robot turning trajectory smoothing interpolation method, system and storage medium based on vector synthesis. BACKGROUND

[0002] In robot motion control, trajectory planning is a key technology to ensure smoothness and accuracy, especially at turning points, where trajectory discontinuity can cause velocity or acceleration to change abruptly, leading to mechanical stress or motion instability. Traditional interpolation methods such as linear interpolation or spline interpolation are prone to sharp changes at turning points, affecting performance. In the prior art, smoothing methods based on polynomial curves or Bezier curves can improve continuity, but have high computational complexity, especially when dealing with a large number of interpolation points in long trajectories or low-speed operation, making it difficult to meet real-time control requirements. Interpolation methods based on S-shaped velocity curves improve smoothness, but still struggle to balance computational efficiency and accuracy in complex turning scenarios. Therefore, there is an urgent need for an interpolation method that balances smoothness and real-time performance. SUMMARY

[0003] The purpose of the present application is to provide a robot turning trajectory smoothing interpolation method, system and storage medium based on vector synthesis that can smoothly interpolate robot trajectories at turning points in real time.

[0004] Technical solution: The robot turning trajectory smoothing interpolation method based on vector synthesis according to the present application comprises the following steps:

[0005] S1. Calculate the number of interpolation points N from the start point A of the trajectory to the inflection point B of the trajectory and from the inflection point B of the trajectory to the end point C of the trajectory according to the S-shaped velocity curve respectively AB and N BC ;

[0006] S2. Take N = min(N AB ,N BC ) as the reference, determine the number of interpolation points from the inflection point B to the separation point D and the regression point E according to N and a preset overlap parameter α, and determine the positions of D and E points accordingly;

[0007] S3. Perform fine interpolation on the AB segment and the BC segment in joint space with joint angle vectors or in Cartesian space with position and attitude vectors, and the interpolation process is performed in segments;

[0008] S4. Perform vector synthesis on the DB segment and the BE segment trajectory to generate a smooth transition DE segment trajectory, and the synthesis is performed in segments;

[0009] S5. Replace the DB and BE segments of the original trajectory with the DE segment as a whole to complete the trajectory smoothing.

[0010] The number of interpolation points before and after the inflection point of the trajectory is determined through S-shaped curve pre-interpolation, a smaller number of interpolation points is selected as a base number, and the actual number of points to be interpolated before and after the inflection point and the starting point position of interpolation are determined in combination with a preset overlap parameter, so that the calculation amount of subsequent smoothing processing is reduced, and the smoothing effect is considered; then fine interpolation and vector synthesis are performed in the joint or Cartesian space to generate a smooth DE segment, the two processes are performed in sections, the delay caused by a long trajectory is avoided, the low-speed high-precision scene is adapted, and it is ensured that the trajectory smoothing processing can be performed in real time, and the trajectory at the turning point of the robot can be smoothed in real time through the method.

[0011] The robot turning trajectory smoothing interpolation system based on vector synthesis comprises:

[0012] The pre-interpolation module is used for calculating the number of interpolation points N of the trajectory starting point A to the trajectory inflection point B and the trajectory inflection point B to the trajectory ending point C respectively according to an S-shaped speed curve AB and N BC ;

[0013] The interpolation starting point determination module is used for taking N=min(N AB ,N BC ) as a base, determining the number of interpolation points of the trajectory inflection point B to the separation point D and the regression point E according to N and a preset overlap parameter alpha, and determining the positions of the D and E points;

[0014] The fine interpolation module is used for performing fine interpolation on the AB segment and the BC segment in the joint space in the joint angle vector or in the Cartesian space in the position and attitude vector;

[0015] The vector synthesis module is used for performing vector synthesis on the DB segment and the BE segment trajectory to generate a smooth transition DE segment trajectory;

[0016] The trajectory reconstruction module is used for replacing the DB and BE segments of the original trajectory as a whole with the DE segment to complete trajectory smoothing.

[0017] The computer readable storage medium storing one or more programs comprises one or more programs including instructions, and the instructions, when executed by a computing device, cause the computing device to perform any of the above methods.

[0018] Beneficial effects: By pre-interpolating to determine a smaller number of interpolation points, the actual number of interpolation points can be determined based on this, significantly improving the system's response speed and control accuracy. Simultaneously, the vector synthesis interpolation method achieves smooth connections at trajectory turning points, solving the velocity abrupt change problem at corners encountered by traditional methods. Furthermore, the segmented processing during interpolation and synthesis improves real-time performance. This method is compatible with both joint space and Cartesian space representations, exhibiting strong adaptability and high computational efficiency, making it suitable for high-precision, high-real-time robot trajectory planning tasks. Attached Figure Description

[0019] Figure 1 The flowchart shows the interpolation method based on vector synthesis.

[0020] Figure 2 This is a schematic diagram of the robot's turning trajectory;

[0021] Figure 3 A schematic diagram of the robot's S-shaped velocity, acceleration, and jerk curves;

[0022] Figure 4 The original trajectory and the synthesized trajectory of the robot are interpolated from actual machine images.

[0023] Figure 5 for Figure 4 A magnified view of a section at point B. Detailed Implementation

[0024] As shown in the figure, the robot turning trajectory smoothing interpolation method based on vector synthesis according to the present invention includes the following steps:

[0025] S1. Based on the S-shaped velocity curve, calculate the number N of interpolation points from the trajectory start point A to the trajectory inflection point B, and from the trajectory inflection point B to the trajectory end point C. AB and N BC ;

[0026] The S-shaped velocity curve is defined as:

[0027]

[0028] Where v(t) is the robot's velocity at time t, v max T represents the robot's maximum speed and T represents the duration of the trajectory segment.

[0029] The S-shaped velocity curve reduces motion shock by smoothing acceleration and deceleration. The number of interpolation points is calculated based on the trajectory length, robot speed, and control frequency.

[0030] The specific calculation formula is as follows:

[0031]

[0032] Among them, S(T) AB ) and S(T BC ) are the trajectory lengths of segments AB and BC calculated according to the S-shaped velocity curve, respectively; f is the robot control frequency, v max This is the robot's maximum speed.

[0033] S2, Choose N = min(N) AB N BC Using N as a benchmark, the number of interpolation points from trajectory inflection point B to separation point D and regression point E is determined based on N and the preset overlap parameter α. The number of interpolation points from trajectory inflection point B to separation point D and regression point E is N*α. This determines the positions of points D and E. For example, point D is the N*αth point counting backward from point B. Where 0≤α≤0.5.

[0034] S3. Perform fine interpolation on segments AB and BC in joint space using joint angle vectors or in Cartesian space using position and orientation vectors. The interpolation process is performed in segments.

[0035] Joint angle vectors or position and attitude vectors are generated at fixed time intervals Δt. Δt is generally determined based on the machine's response capability, and commonly used values ​​are 1ms, 2ms, 4ms, 8ms, etc. It is also called the interpolation period.

[0036] In joint space, each interpolation point is a joint angle vector for each axis, represented as θ = [θ1, θ2, ..., θ]. n ], where n is the number of robot axes.

[0037] In Cartesian space, each interpolation point is represented by a position vector and an attitude vector. The position vector is represented as p = [x, y, z], and the attitude vector is represented as a quaternion q = [q w ,q x ,q y ,q z ].

[0038] Interpolation points are generated based on Δt to ensure a smooth transition in position and attitude. Fine interpolation is based on the pre-interpolated S-shaped velocity curve results, and dense interpolation points can be generated using linear interpolation or spline interpolation.

[0039] S4. Perform vector synthesis on the trajectories of DB segment and BE segment to generate the smoothly transitioning trajectory of DE segment, and perform the synthesis in segments;

[0040] The vector composition formula in joint space is:

[0041] Joint space:

[0042] θ DE (γ)=θ DB (γ)+θ BE (γ)-θ B

[0043] Where, θ DE (γ) is the joint angle vector of the γth interpolation point in segment DE, θ DB (γ) and θ BE (γ) represents the joint angle vectors of the γth interpolation point in segments DB and BE, respectively. Since segments AB and BC are interpolated piecewise, corresponding segments DB and BE are also interpolated piecewise. The interpolation starting points for DB and BE are points D and B, respectively. The γth interpolation point of each segment is the γth interpolation point at a distance from points D and B, respectively. By synthesizing the joint angle vectors of the corresponding points according to the above formula, the joint vector of the γth interpolation point of the final synthesized trajectory segment DE can be obtained. The meaning of γ is the same in position synthesis and attitude synthesis below, and will not be repeated here. θ B Let B be the joint angle vector.

[0044] The formula for vector composition in Cartesian space is:

[0045] Cartesian space:

[0046] Positional composition:

[0047] P DE (γ)=P DB (γ)+P BE (γ)-P B

[0048] Attitude synthesis:

[0049] O DE (γ)=O DB (γ)+O BE (γ)-O B

[0050] Among them, P DE (γ) is the position vector of the γth interpolation point in segment DE, P DB (γ) and P BE (γ) are the position vectors of the γth interpolation point in segments DB and BE, respectively. B Let O be the position vector of point B; DE (γ) is the attitude vector of the γth interpolation point in segment DE, O DB (γ) and O BE (γ) represents the attitude vector of the γth interpolation point in segments DB and BE, respectively. B Let B be the attitude vector of point B.

[0051] S5. Replace the DB and BE segments of the original trajectory with the DE segment to complete the trajectory smoothing.

[0052] Segments A to D follow the original AB trajectory. Segments D to E are the synthesized DE trajectory. Segments E to C follow the original BC trajectory. The original DB segment is replaced by the DE segment, the BE segment is deleted, and only the EC segment is retained.

[0053] To address the high computational cost of fine interpolation and vector synthesis (steps S3 and S4), a segmented computation approach is adopted. Each computation calculates 50 to 500 interpolation points, with the specific number dynamically adjusted based on system real-time requirements. When the buffer is nearly exhausted (e.g., 10% of the points remain), the next batch of points is calculated using parallel threads. The algorithm is as follows:

[0054] Initialize the buffer to store 50 to 500 interpolation points.

[0055] Motion is performed using buffer points.

[0056] If the number of buffer points falls below a certain threshold, a buffer point calculation thread is started to generate 50 to 500 interpolation points and store them in the buffer.

[0057] The robot turning trajectory smoothing interpolation system based on vector synthesis described in this invention includes:

[0058] Pre-interpolation module: Used to calculate the pre-interpolation from the starting point A to the inflection point B and from the inflection point B to the ending point C using the S-shaped velocity curve, and to calculate the number of interpolation points N for segment AB. AB The number of interpolation points N in segment BC BC ;

[0059] Interpolation start point determination module: used to select N = min(N AB N BC Using N as a benchmark, the number of interpolation points from trajectory inflection point B to separation point D and regression point E is determined based on N and the preset overlap parameter α, thereby determining the positions of points D and E;

[0060] Fine interpolation module: used to perform fine interpolation on segments AB and BC in joint space using joint angle vectors or in Cartesian space using position and orientation vectors;

[0061] Vector synthesis module: used to perform vector synthesis on the DB and BE segments to generate a smoothly transitioning DE segment trajectory;

[0062] Trajectory Reconstruction Module: Used to replace the DB and BE segments of the original trajectory with the DE segment to achieve trajectory smoothing.

[0063] The computer-readable storage medium for storing one or more programs according to the present invention includes one or more programs comprising instructions that, when executed by a computing device, cause the computing device to perform any of the methods described above.

[0064] To better verify the effectiveness of the method described in this invention, a specific example is provided below:

[0065] The robot's velocity curve, acceleration curve, and acceleration-deceleration curve are as follows: Figure 3 As shown, its trajectory is as follows Figure 4 As shown, in the verification, the trajectory movement direction is from point A to point B, from point B to point C, and finally back to point A. The test was run twice, generating the original trajectory and the synthesized trajectory respectively. These were placed in the same image for easy comparison, as the two trajectories have a high degree of overlap. Figure 4 There are no two trajectories visible in the image. Figure 5 The magnified view shows two distinct trajectories at the inflection point B: the smoothly curved one is the composite trajectory, and the other is the original trajectory. The X and Y coordinates of each point are A(-200mm, 1000mm), B(200mm, 1000mm), and C(500mm, 1200mm). The velocity in segment AB is 2 m / min, and the velocity in segment BC is 5 m / min. Figure 5 This is a magnified view of the turning area. When selecting points D and E according to this algorithm, the original number of points DB and BE must be the same. As can be seen from the figure, although the original number of points DB and BE is the same, the different speeds result in a stretching effect, and the synthesized trajectory is asymmetrical.

Claims

1. A method for smooth interpolation of robot turning trajectories based on vector synthesis, characterized in that, Includes the following steps: S1. Based on the S-shaped velocity curve, calculate the number N of interpolation points from the trajectory start point A to the trajectory inflection point B, and from the trajectory inflection point B to the trajectory end point C. AB and N BC ; S2, with N = min(N) AB N BC Using N as a benchmark, the number of interpolation points from trajectory inflection point B to separation point D and regression point E is determined based on N and the preset overlap parameter α, thereby determining the positions of points D and E; S3. Perform fine interpolation on segments AB and BC in joint space using joint angle vectors or in Cartesian space using position and orientation vectors. The interpolation process is performed in segments. S4. Perform vector synthesis on the trajectories of DB segment and BE segment to generate the smoothly transitioning trajectory of DE segment, and perform the synthesis in segments; S5. Replace the DB and BE segments of the original trajectory with the DE segment to complete the trajectory smoothing.

2. The method according to claim 1, characterized in that, The S-shaped velocity curve is defined as follows: Where v(t) is the robot's velocity at time t, v max T represents the robot's maximum speed and T represents the duration of the trajectory segment.

3. The method according to claim 2, characterized in that, In step S1, N AB and N BC The calculation formula is: Among them, S(T) AB ) and S(T BC ) are the trajectory lengths of segments AB and BC calculated according to the S-shaped velocity curve, respectively; f is the robot control frequency, v max This is the robot's maximum speed.

4. The method according to claim 1, characterized in that: In step S2, the number of interpolation points from trajectory inflection point B to separation point D and regression point E is N*α.

5. The method according to claim 1, characterized in that: In step S3, joint angle vectors or position and attitude vectors are generated according to a fixed time interval Δt.

6. The method according to claim 1, characterized in that: The vector composition formula in the joint space in step S4 is as follows: Joint space: i DE (c)=θ DB (c)+θ BE (c)-i B Where, θ DE (γ) is the joint angle vector of the γth interpolation point in segment DE, θ DB (γ) and θ BE (γ) are the joint angle vectors of the γth interpolation point in segments DB and BE, respectively, θ B Let B be the joint angle vector.

7. The method according to claim 1, characterized in that: The vector composition formula in Cartesian space in step S4 is as follows: Cartesian space: Positional composition: P DE (c)=P DB (c)+P BE (c)-P B Attitude synthesis: P DE (c)=O DB (c)+O BE (c)-O B Among them, P DE (γ) is the position vector of the γth interpolation point in segment DE, P DB (γ) and P BE (γ) are the position vectors of the γth interpolation point in segments DB and BE, respectively. B Let O be the position vector of point B; DE (γ) is the attitude vector of the γth interpolation point in segment DE, O DB (γ) and O DB (γ) represents the attitude vector of the γth interpolation point in segments DB and BE, respectively. B Let B be the attitude vector of point B.

8. The method according to claim 1, characterized in that, In steps S3 and S4, the process is divided into segments: a set number of data to be calculated is pre-cached, a set number of points are taken from the pre-cached data for calculation each time, and when the amount of pre-cached data is lower than a set threshold, the data to be calculated is replenished to the set value.

9. A smooth interpolation system for robot turning trajectories based on vector synthesis, characterized in that, include: Pre-interpolation module: Used to calculate the number N of interpolation points from the starting point A to the inflection point B, and from the inflection point B to the ending point C, based on the S-shaped velocity curve. AB and N BC ; Interpolation start point determination module: used to determine the starting point of interpolation with N = min(N AB N BC Using N as a benchmark, the number of interpolation points from trajectory inflection point B to separation point D and regression point E is determined based on N and the preset overlap parameter α, thereby determining the positions of points D and E; Fine interpolation module: used to perform fine interpolation on segments AB and BC in joint space using joint angle vectors or in Cartesian space using position and orientation vectors; Vector synthesis module: used to perform vector synthesis on the DB and BE segments to generate a smoothly transitioning DE segment trajectory; Trajectory Reconstruction Module: Used to replace the DB and BE segments of the original trajectory with the DE segment to achieve trajectory smoothing.

10. A computer-readable storage medium for storing one or more programs, characterized in that: The program includes one or more instructions that, when executed by a computing device, cause the computing device to perform any of the methods according to claims 1 to 8.

Images