General Arc Instruction Position and Attitude Transition Synchronization Planning Method and System for Robot Arms

Abstract

A method and system for synchronously planning the position and attitude transition of a general circular arc instruction of a robotic arm disclosed by the present invention. The method includes: determining the center of a circle and the radius based on user data, where the user data includes a starting point, an ending point, and a spatial reference point; obtaining a target circular arc by combining the spatial reference point with the center of the circle, and proportionally dividing the target circular arc to obtain different arc lengths; determining the synchronization conditions, specifically including obtaining the set transition segment parameters and multiplying them by different arc lengths to obtain the transition starting point and the transition ending point; performing curve speed planning according to the total arc length of the circular arc, and determining the position and attitude values according to the interpolation points, and generating joint control commands for the robotic arm based on the position and attitude values. Based on geometric characteristic interpolation, the present invention can be dynamically adjusted without additional design of attitude interpolation, and can also achieve high-order smoothing of attitude transition. Through synchronous mapping, the change of attitude is associated with the arc length parameter of the circular arc path, making the attitude transition more natural and avoiding abrupt rotation.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and more specifically, to a method and system for synchronously planning the position and attitude transition of a general arc instruction for a robotic arm. Background Art

[0002] In the field of motion control of robots and robotic arms, arc instruction planning is an important part of realizing complex trajectory and attitude synchronous control. Circular arc motion is usually used in applications such as welding, gluing, and cutting by industrial robots, requiring the end of the robotic arm to move along a circular arc trajectory, and the attitude change to be synchronized with the position.

[0003] Currently, the existing technical solutions are as follows:

[0004] 1. Directly calculate the circular arc path using geometric relationships. Usually, the circular arc trajectory is determined by known start point, end point, and reference point, and then the attitude is described using a rotation matrix or quaternion. Advantages: Simple calculation, can directly utilize the mathematical properties of the circular arc, high calculation efficiency, and easy to implement, suitable for regular circular arc paths with fixed center and radius; at the same time, there are obvious disadvantages, that is, the attitude planning is not flexible, additional design is often required for attitude interpolation, which may lead to uneven attitude transition, and the lack of dynamic optimization, unable to handle speed and acceleration constraints well

[0005] 2. Generate continuous and smooth paths and attitudes through spline interpolation or parameterization methods. Among them, attitude interpolation commonly uses Euler angle interpolation or quaternion interpolation. Advantages: Good smoothness, the position and attitude have good continuity and smoothness on the trajectory, and high flexibility, supporting the planning of complex trajectories and diverse attitudes; there are also obvious disadvantages, that is, the calculation complexity is high, interpolation calculation requires more resources, especially in scenarios with high real-time requirements, and may be distorted. When the number of key points is insufficient or the distribution is unreasonable, it may lead to path distortion or attitude error. Summary of the Invention

[0006] The purpose of the present invention is to provide a method and system for synchronously planning the position and attitude transition of a general arc instruction for a robotic arm. Based on geometric characteristic interpolation, it can be dynamically adjusted, without additional design for attitude interpolation, and can also achieve high-order smoothness of attitude transition. Through synchronous mapping, the change of attitude is associated with the arc length parameter of the circular arc path, making the attitude transition more natural and avoiding abrupt rotation.

[0007] The first aspect of the present invention provides a method for synchronously planning the position and attitude transition of a general arc instruction for a robotic arm, including the following steps:

[0008] Determine the center and radius based on user data, where the user data includes a start point, an end point, and a spatial reference point;

[0009] A target arc is obtained based on a spatial reference point combined with the center of the circle, and the target arc is proportionally divided to obtain different arc lengths;

[0010] Determine the synchronization conditions, specifically including obtaining the set transition segment parameters and multiplying them by different arc lengths to obtain the transition start point and transition end point;

[0011] Curve velocity planning is performed according to the total arc length, and position and attitude values are determined according to the interpolation points, and joint control commands of the robot arm are generated based on the position and attitude values.

[0012] In this solution, the center and radius of the circle are determined based on user data, wherein the user data includes a starting point, an end point, and a spatial reference point, specifically including:

[0013] Get spatial reference points P1, P2, and P3, where P1: (x1, y1, z1, a1, b1, c1), P2: (x2, y2, z2, a2, b2, c2), and P3: (x3, y3, z3, a3, b3, c3).

[0014] Determine a plane and a plane normal vector based on spatial reference points P1, P2, and P3;

[0015] After the plane is determined, the center of the circle is obtained by jointly determining the equidistance constraint and the plane constraint, and the radius is calculated simultaneously.

[0016] In this solution, the target arc is obtained based on the spatial reference point combined with the center of the circle, and the target arc is proportionally divided to obtain different arc lengths, specifically including:

[0017] Calculate the attitude difference of spatial reference points P1, P2 and P3 to obtain the synchronous planning value;

[0018] The target arc obtained by the spatial reference points P1, P2 and P3 and the center of the circle is divided into different arc lengths based on different positions, wherein the segment between the spatial reference points P1 and P2 is arcL1, and the segment between the spatial reference points P2 and P3 is arcL2.

[0019] In this solution, the synchronization condition is determined by obtaining the set transition segment parameters and multiplying them by different arc lengths to obtain the transition start point and transition end point, which specifically includes:

[0020] Get the blend-ratio parameter of the transition segment, which is a value in the interval [0,1] and the maximum length of which does not exceed half of the shortest of the two arc lengths.

[0021] The different arc lengths are multiplied by the transition segment parameter blend-ratio to obtain arc length positions corresponding to the transition start point and the transition end point.

[0022] In this solution, curve velocity planning is performed according to the total arc length of the circular arc, and the position and attitude values are determined based on the interpolation points. Based on the position and attitude values, joint control commands for the robotic arm are generated, specifically including:

[0023] Perform S-curve velocity planning according to the total arc length of the circular arc to determine the displacement function and velocity function;

[0024] Based on the displacement function and the velocity function, determine the interpolation points corresponding to the total arc length of the circular arc;

[0025] Based on the interpolation points, perform position and attitude synchronous proportional mapping motion to obtain the position and attitude values for interpolation motion. Specifically, based on the position and attitude values, it is decomposed into different joint control commands corresponding to the current robotic arm to control the motion of the robotic arm.

[0026] In this solution, the method further includes, during the motion based on the interpolation points, connecting the transition sections of two arc lengths through a fifth-degree polynomial. The fifth-degree polynomial is as follows:

[0027] x(t)= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5;

[0028] where x(t) is the displacement at a certain moment t; a_0, a_1, a_2, a_3, a_4, a_5 are the coefficients of the polynomial, and t is the time.

[0029] In the second aspect of the present invention, a robotic arm general circular arc instruction position and attitude transition synchronization planning system is further provided, including a memory and a processor. The memory includes a robotic arm general circular arc instruction position and attitude transition synchronization planning method program. When the robotic arm general circular arc instruction position and attitude transition synchronization planning method program is executed by the processor, the following steps are implemented:

[0030] Determine the center of the circle and the radius based on user data, where the user data includes the starting point, the ending point, and the spatial reference point;

[0031] Based on the spatial reference point and the center of the circle, obtain the target circular arc, and perform proportional division on the target circular arc to obtain different arc lengths;

[0032] Determine the synchronization conditions, specifically including obtaining the set transition section parameters and multiplying them by different arc lengths to obtain the transition starting point and the transition ending point;

[0033] Perform curve velocity planning according to the total arc length of the circular arc, and determine the position and attitude values based on the interpolation points. Based on the position and attitude values, generate joint control commands for the robotic arm.

[0034] In this solution, the center and radius of the circle are determined based on user data, wherein the user data includes a starting point, an end point, and a spatial reference point, specifically including:

[0035] Get spatial reference points P1, P2, and P3, where P1: (x1, y1, z1, a1, b1, c1), P2: (x2, y2, z2, a2, b2, c2), and P3: (x3, y3, z3, a3, b3, c3).

[0036] Determine a plane and a plane normal vector based on spatial reference points P1, P2, and P3;

[0037] After the plane is determined, the center of the circle is obtained by jointly determining the equidistance constraint and the plane constraint, and the radius is calculated simultaneously.

[0038] In this solution, the target arc is obtained based on the spatial reference point combined with the center of the circle, and the target arc is proportionally divided to obtain different arc lengths, specifically including:

[0039] Calculate the attitude difference of spatial reference points P1, P2 and P3 to obtain the synchronous planning value;

[0040] The target arc obtained by the spatial reference points P1, P2 and P3 and the center of the circle is divided into different arc lengths based on different positions, wherein the segment between the spatial reference points P1 and P2 is arcL1, and the segment between the spatial reference points P2 and P3 is arcL2.

[0041] In this solution, the synchronization condition is determined by obtaining the set transition segment parameters and multiplying them by different arc lengths to obtain the transition start point and transition end point, which specifically includes:

[0042] Get the blend-ratio parameter of the transition segment, which is a value in the interval [0,1] and the maximum length of which does not exceed half of the shortest of the two arc lengths.

[0043] The different arc lengths are multiplied by the transition segment parameter blend-ratio to obtain arc length positions corresponding to the transition start point and the transition end point.

[0044] In this solution, curve velocity planning is performed based on the total arc length, and the position and attitude values are determined based on the interpolation points. Based on the position and attitude values, joint control commands for the robot arm are generated, specifically including:

[0045] Performing S-curve velocity planning according to the total arc length to determine a displacement function and a velocity function;

[0046] Determining interpolation points corresponding to the total arc length of the arc based on the displacement function and the velocity function;

[0047] Performing position and attitude synchronous proportional mapping motion based on the interpolation points to obtain the interpolation motion of the position and attitude values, where specifically, the position and attitude values are decomposed into different joint control commands corresponding to the current robotic arm to control the motion of the robotic arm.

[0048] In this solution, the method further includes, during the motion based on the interpolation points, connecting the transition sections of two arc lengths through a quintic polynomial, where the quintic polynomial is as follows:

[0049] x(t)= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5;

[0050] where x(t) is the displacement at a certain moment t; a_0, a_1, a_2, a_3, a_4, a_5 are the coefficients of the polynomial, and t is the time.

[0051] The third aspect of the present invention provides a computer-readable storage medium, which includes a program for a general circular arc instruction position and attitude transition synchronization planning method of a robotic arm of a machine. When the program for the general circular arc instruction position and attitude transition synchronization planning method of the robotic arm is executed by a processor, the steps of a general circular arc instruction position and attitude transition synchronization planning method as described in any one of the above are implemented.

[0052] A general circular arc instruction position and attitude transition synchronization planning method and system for a robotic arm disclosed by the present invention, based on geometric characteristic interpolation, can be dynamically adjusted, without the need for additional design of attitude interpolation, and can also achieve high-order smoothing of attitude transition, with simple calculation and not requiring too much additional computing power.

[0053] Among them, through synchronous mapping, the change of attitude is associated with the arc length parameter of the circular arc path, making the attitude transition more natural and not having abrupt rotations. The synchronous mapping can ensure the continuity of the first-order, second-order, and even higher-order derivatives of the position and attitude on the path, improving the overall smoothness of the trajectory.

[0054] In addition, the motion coordination is high. Through mapping, the changes of attitude and position are directly related, avoiding the problem that the attitude does not follow after the path is completed, and allowing the speed of attitude change along the path to be adjusted proportionally to adapt to different task requirements. For example, increasing the speed of attitude change at key points can ensure that the relationship between attitude and position remains consistent throughout the circular arc trajectory and is not prone to local incoordination phenomena. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 Shows a flowchart of a general circular arc instruction position and attitude transition synchronization planning method for a robotic arm of the present invention;

[0056] Figure 2 The block diagram of a mechanical arm general circular arc instruction position and attitude transition synchronization planning system according to the present invention is shown. Specific embodiments

[0057] In order to more clearly understand the above objects, features and advantages of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, without conflict, the embodiments of the present application and the features in the embodiments can be combined with each other.

[0058] In the following description, many specific details are set forth in order to fully understand the present invention. However, the present invention can also be implemented in other ways different from those described herein. Therefore, the protection scope of the present invention is not limited by the specific embodiments disclosed below.

[0059] Figure 1 The flowchart of a mechanical arm general circular arc instruction position and attitude transition synchronization planning method according to the present application is shown.

[0060] As Figure 1 shown, the present application discloses a mechanical arm general circular arc instruction position and attitude transition synchronization planning method, including the following steps:

[0061] S102, determining the center of the circle and the radius based on user data, where the user data includes a starting point, an ending point, and a spatial reference point;

[0062] S104, obtaining a target circular arc based on the spatial reference point and the center of the circle, and performing proportional division on the target circular arc to obtain different arc lengths;

[0063] S106, determining synchronization conditions, specifically including obtaining the set transition section parameters multiplied by different arc lengths to obtain a transition starting point and a transition ending point;

[0064] S108, performing curve speed planning according to the total arc length of the circular arc, determining the position and attitude values according to the interpolation points, and generating joint control commands for the robotic arm based on the position and attitude values.

[0065] It should be noted that in this embodiment, specifically, the center of the circle and the radius are determined according to the starting point, the ending point and the reference spatial point, so as to calculate the arc and directly generate the circular arc trajectory. And through proportional distribution of the circular arc length and the attitude difference, and then synchronized to the attitude planning according to the generated position interpolation ratio, and presented in the form of Euler angles, it can achieve good position and attitude synchronization under limited example conditions, and achieve a non-zero speed transition of the attitude.

[0066] Specifically, the center and radius of the circle are first determined based on the user data, where the user data includes the starting point, end point and spatial reference point provided by the user, where the number of spatial reference points is at least three. Accordingly, in this embodiment, there are three spatial reference points P1, P2 and P3, and a plane is determined by these three points, and then the center and radius of the circle are calculated from the determined plane.

[0067] Furthermore, a target arc is obtained based on the spatial reference point combined with the center of the circle, the target arc is proportionally divided to obtain different arc lengths, and synchronization conditions are determined, specifically including obtaining the set transition segment parameters and multiplying them with different arc lengths to obtain the transition start point and transition end point. Finally, curve speed planning is performed according to the total arc length, and the position and posture values are determined according to the interpolation points, and the joint control commands of the robot arm are generated based on the position and posture values.

[0068] According to an embodiment of the present invention, the determination of the center and radius of the circle based on user data, wherein the user data includes a starting point, an end point, and a spatial reference point, specifically includes:

[0069] Get spatial reference points P1, P2, and P3, where P1: (x1, y1, z1, a1, b1, c1), P2: (x2, y2, z2, a2, b2, c2), and P3: (x3, y3, z3, a3, b3, c3).

[0070] Determine a plane and a plane normal vector based on spatial reference points P1, P2, and P3;

[0071] After the plane is determined, the center of the circle is obtained by jointly determining the equidistance constraint and the plane constraint, and the radius is calculated simultaneously.

[0072] It should be noted that, in this embodiment, when a plane is determined by three spatial reference points P1, P2, and P3, a plane normal vector is first determined. The plane normal vector is obtained by the difference product of two vectors, as follows:

[0073] ;

[0074] Among them, n=((y2-y1)×(z3-z1)-(z2-z1)×(y3-y1), (z2-z1)×(x3-x1)-(x2-x1)×(z3-z1), (x2-x1)×(y3-y1)-(y2-y1)×(x3-x1));

[0075] Correspondingly, the plane equation obtained is: nx(x-x1)+ny(y-y1)+nz(z-z1)=0. After the plane is determined, the center of the circle is obtained by jointly constraining the equidistance constraint and the plane constraint, and the radius is calculated simultaneously.

[0076] According to an embodiment of the present invention, the target arc is obtained by combining the spatial reference points with the center of the circle, and different arc lengths are obtained by proportionally dividing the target arc, specifically including:

[0077] Calculate the attitude differences of the spatial reference points P1, P2, and P3 to obtain the synchronous planning value;

[0078] And divide the target arc obtained by the spatial reference points P1, P2, and P3 and the center of the circle into different arc lengths based on different positions. Among them, the section between the spatial reference points P1 and P2 is arcL1, and the section between the spatial reference points P2 and P3 is arcL2.

[0079] It should be noted that in this embodiment, the attitude differences of the spatial reference points P1, P2, and P3 are calculated , , select the maximum value among the three attitude differences as the synchronous planning value, and divide the arc generated by the position into proportions. For example, the section from p1 to p2 is arcL1, and the section from p2 to p3 is arcL2. The total arc length is arcL = arcL1 + arcL2.

[0080] According to an embodiment of the present invention, the determination of the synchronous condition specifically includes obtaining the transition start point and the transition end point by multiplying the set transition section parameter by different arc lengths, specifically including:

[0081] Obtain the set transition section parameter blend-ratio, which is a value in the interval [0, 1], and the maximum length does not exceed half of the shortest arc length among the two arc lengths;

[0082] Multiply different arc lengths by the transition section parameter blend-ratio to obtain the arc length positions corresponding to the transition start point and the transition end point.

[0083] It should be noted that in this embodiment, to meet the synchronous conditions of position and attitude, certain conditions need to be met during planning. By obtaining the set transition section parameter blend-ratio, which is a value in the interval [0, 1] and setting the maximum length not to exceed half of the shortest arc length among the two arc lengths, and then multiplying different arc lengths by blend-ratio, the arc length positions of the transition start point and the transition end point can be obtained to provide data support for subsequent planning.

[0084] According to an embodiment of the present invention, curve speed planning is performed according to the total arc length of the arc, and the position and attitude values are determined according to the interpolation points. Based on the position and attitude values, joint control commands for the robotic arm are generated, specifically including:

[0085] Perform S-curve speed planning according to the total arc length of the arc to determine the displacement function and the speed function;

[0086] Determine the interpolation points corresponding to the total arc length of the circular arc based on the displacement function and the velocity function;

[0087] Perform position and attitude synchronous proportional mapping motion based on the interpolation points to obtain the interpolation motion of the position and attitude values. Specifically, decompose the position and attitude values into different joint control commands corresponding to the current robotic arm to control the motion of the robotic arm.

[0088] It should be noted that in this embodiment, this solution focuses on position planning and supplements attitude synchronous planning. Perform S-curve velocity planning according to the total arc length of the circular arc. Among them, S-curve velocity planning is a velocity control method commonly used in robotic arms, robots, automation systems, and vehicle path planning. Its main advantage is that it can provide a smooth acceleration and deceleration process, reduce the vibration and stress of the mechanical system, and optimize the smoothness of the motion. Specifically, S-curve velocity planning adopts smooth acceleration and deceleration curves (the acceleration and deceleration change in an S shape on the time axis), avoiding the instantaneous changes that may occur in traditional linear acceleration and deceleration planning. Through this smooth transition method, it can significantly reduce the vibration of the mechanical system and irregular inertial forces, making the motion more stable. In practical applications, precise grasping, welding, spraying, cutting, and other tasks can be performed.

[0089] Specifically, the displacement function and the velocity function are prior arts and are only used as applications in this embodiment without further elaboration. It should be noted that the displacement function is as follows:

[0090] ;

[0091] ;

[0092] The velocity function is as follows:

[0093] ;

[0094] Among them, represents the initial velocity, J represents the jerk, a represents the acceleration, v represents the maximum velocity, T represents the motion time of each segment, represents the motion time of the first segment, represents the motion time of the second segment, represents the motion time of the third segment, represents the motion time of the fourth segment, represents the motion time of the fifth segment, represents the motion time of the sixth segment, represents the motion time of the seventh segment.

[0095] Further, interpolation points corresponding to the total arc length of the circular arc are determined based on the displacement function and the velocity function, and position and attitude synchronous proportional mapping motion is performed according to the interpolation points. For example, if the arc length interpolation value at a certain moment is Δs, this interpolation value is compared with arcL1. If it is greater, it means that the motion has reached the circular arc segment p2 - p3 at this time; otherwise, it has reached the circular arc segment p1 - p2. If it is defined as the first circular arc segment at this time, the ratio Δs / (arcL1) is calculated to obtain the occupation ratio of this interpolation value to the arcL1 of the p1 - p2 circular arc segment, and it is synchronously mapped to Δθ / , where Δθ is the attitude interpolation value corresponding to the circular arc interpolation value, which can ensure the synchronous motion of position and attitude. At this time, the interpolation motion continues. When reaching the transition segment, the interpolation speed, acceleration, and interpolation attitude value of the starting point position of the transition segment are obtained, and the position interpolation speed, acceleration, and interpolation attitude value at the end of the transition are calculated.

[0096] According to an embodiment of the present invention, the method further includes, during the motion based on the interpolation points, connecting the transition segments of two arc lengths through a fifth - degree polynomial. Among them, the fifth - degree polynomial is as follows:

[0097] x(t)= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5;

[0098] Among them, x(t) is the displacement at a certain moment t; a_0, a_1, a_2, a_3, a_4, a_5 are the coefficients of the polynomial, and t is the time.

[0099] It should be noted that in this embodiment, the transition segment is connected through fifth - degree polynomial planning. The purpose is to solve the problem of attitude speed jump at the connection points of the attitude of two arc lengths and two postures in the proportional mapping. Through polynomial connection, no jump in speed and acceleration can be achieved. Among them, the fifth - degree polynomial is widely used in motion planning and can simultaneously control the starting and ending constraints of displacement, speed, and acceleration, and is suitable for trajectory planning that requires smooth transition.

[0100] Specifically, the general form of the fifth - degree polynomial is as follows:

[0101] x(t)= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5;

[0102] Where: x(t) is the displacement at a certain moment t; a_0, a_1, a_2, a_3, a_4, a_5 are the coefficients of the polynomial, and t is the time. Further, the transition section is connected by a fifth-degree polynomial and the interpolation position and interpolation attitude are output, so as to smoothly transition to the second arc length and the second attitude. Then, according to (Δs - arcL1) / (arcL2), the ratio of this interpolation value to the arc L2 of the p2 - p3 section circular arc is obtained, and it is synchronously mapped to Δθ / , where Δθ is the attitude interpolation value corresponding to the circular arc interpolation value. Until the entire circular arc movement is completed, at this time, the attitude movement also stops correspondingly.

[0103] Figure 2 The block diagram of a general circular arc instruction position and attitude transition synchronization planning system for a robotic arm according to the present invention is shown.

[0104] As Figure 2 shown, the present invention discloses a general circular arc instruction position and attitude transition synchronization planning system for a robotic arm, including a memory and a processor. The memory includes a program for the general circular arc instruction position and attitude transition synchronization planning method for the robotic arm. When the program for the general circular arc instruction position and attitude transition synchronization planning method for the robotic arm is executed by the processor, the following steps are implemented:

[0105] Determine the center of the circle and the radius based on user data, where the user data includes a starting point, an ending point, and a spatial reference point;

[0106] Based on the spatial reference point and in combination with the center of the circle, obtain a target circular arc, and perform proportional division on the target circular arc to obtain different arc lengths;

[0107] Determine the synchronization conditions, specifically including obtaining the set transition section parameters multiplied by different arc lengths to obtain the transition starting point and the transition ending point;

[0108] Perform curve speed planning according to the total arc length of the circular arc, determine the position and attitude values according to the interpolation points, and generate joint control commands for the robotic arm based on the position and attitude values.

[0109] It should be noted that in this embodiment, specifically, the center of the circle and the radius are determined according to the starting point, the ending point, and the reference spatial point, so as to calculate the arc and directly generate the circular arc trajectory. And through proportional distribution of the circular arc length and the attitude difference, and then synchronize to the attitude planning according to the generated position interpolation ratio, and present it in the form of Euler angles, it can achieve good position and attitude synchronization under limited example conditions, and achieve a non-zero speed transition of the attitude.

[0110] Specifically, the center and radius of the circle are first determined based on the user data, where the user data includes the starting point, end point and spatial reference point provided by the user, where the number of spatial reference points is at least three. Accordingly, in this embodiment, there are three spatial reference points P1, P2 and P3, and a plane is determined by these three points, and then the center and radius of the circle are calculated from the determined plane.

[0111] Furthermore, a target arc is obtained based on the spatial reference point combined with the center of the circle, the target arc is proportionally divided to obtain different arc lengths, and synchronization conditions are determined, specifically including obtaining the set transition segment parameters and multiplying them with different arc lengths to obtain the transition start point and transition end point. Finally, curve speed planning is performed according to the total arc length, and the position and posture values are determined according to the interpolation points, and the joint control commands of the robot arm are generated based on the position and posture values.

[0112] According to an embodiment of the present invention, the determination of the center and radius of the circle based on user data, wherein the user data includes a starting point, an end point, and a spatial reference point, specifically includes:

[0113] Get spatial reference points P1, P2, and P3, where P1: (x1, y1, z1, a1, b1, c1), P2: (x2, y2, z2, a2, b2, c2), and P3: (x3, y3, z3, a3, b3, c3).

[0114] Determine a plane and a plane normal vector based on spatial reference points P1, P2, and P3;

[0115] After the plane is determined, the center of the circle is obtained by jointly determining the equidistance constraint and the plane constraint, and the radius is calculated simultaneously.

[0116] It should be noted that, in this embodiment, when a plane is determined by three spatial reference points P1, P2, and P3, a plane normal vector is first determined. The plane normal vector is obtained by the difference product of two vectors, as follows:

[0117] ;

[0118] Among them, n=((y2-y1)×(z3-z1)-(z2-z1)×(y3-y1), (z2-z1)×(x3-x1)-(x2-x1)×(z3-z1), (x2-x1)×(y3-y1)-(y2-y1)×(x3-x1));

[0119] Correspondingly, the plane equation obtained is: nx(x-x1)+ny(y-y1)+nz(z-z1)=0. After the plane is determined, the center of the circle is obtained by jointly constraining the equidistance constraint and the plane constraint, and the radius is calculated simultaneously.

[0120] According to an embodiment of the present invention, obtaining a target arc by combining the spatial reference points with the center of the circle, and proportionally dividing the target arc to obtain different arc lengths specifically includes:

[0121] Calculating the attitude differences of the spatial reference points P1, P2, and P3 to obtain a synchronous planning value;

[0122] And dividing the target arc obtained by the spatial reference points P1, P2, and P3 and the center of the circle into different arc lengths based on different positions. Among them, the section between the spatial reference points P1 and P2 is arcL1, and the section between the spatial reference points P2 and P3 is arcL2.

[0123] It should be noted that in this embodiment, calculating the attitude differences of the spatial reference points P1, P2, and P3, , , selecting the maximum value among the three attitude differences as the synchronous planning value, and dividing the arc generated by the position into proportions. For example, the section from p1 to p2 is arcL1, and the section from p2 to p3 is arcL2. The total arc length is arcL = arcL1 + arcL2.

[0124] According to an embodiment of the present invention, determining the synchronous condition specifically includes obtaining the set transition section parameters multiplied by different arc lengths to obtain the transition start point and the transition end point, specifically including:

[0125] Obtaining the set transition section parameter blend-ratio, which is specifically a value in the interval [0, 1], and the maximum length does not exceed half of the shortest arc length among the two arcs;

[0126] Multiplying the different arc lengths by the transition section parameter blend-ratio to obtain the arc length positions corresponding to the transition start point and the transition end point.

[0127] It should be noted that in this embodiment, to meet the synchronous conditions of position and attitude, certain conditions need to be met during planning. By obtaining the set transition section parameter blend-ratio, which is specifically a value in the interval [0, 1] and setting the maximum length not to exceed half of the shortest arc length among the two arcs, and then multiplying the different arc lengths by blend-ratio, the arc length positions of the transition start point and the transition end point can be obtained to provide data support for subsequent planning.

[0128] According to an embodiment of the present invention, performing curve speed planning according to the total arc length of the arc, and determining the position and attitude values according to the interpolation points, and generating joint control commands for the robotic arm based on the position and attitude values specifically includes:

[0129] Performing S-curve speed planning according to the total arc length of the arc to determine the displacement function and the speed function;

[0130] Determine interpolation points corresponding to the total arc length of the circular arc based on the displacement function and the velocity function;

[0131] Perform position and attitude synchronous proportional mapping motion based on the interpolation points to obtain interpolation motion of the position and attitude values. Specifically, decompose the position and attitude values into different joint control commands corresponding to the current robotic arm to control the motion of the robotic arm.

[0132] It should be noted that in this embodiment, this solution focuses on position planning and supplements attitude synchronous planning. Perform S-curve velocity planning according to the total arc length of the circular arc. Among them, S-curve velocity planning is a velocity control method commonly used in robotic arm, robot, automation system and vehicle path planning. Its main advantage lies in being able to provide a smooth acceleration and deceleration process, reduce the vibration and stress of the mechanical system, and optimize the smoothness of the motion. Specifically, S-curve velocity planning adopts smooth acceleration and deceleration curves (the acceleration and deceleration change in an S shape on the time axis), avoiding the instantaneous changes that may occur in traditional linear acceleration and deceleration planning. Through this smooth transition method, it can significantly reduce the vibration of the mechanical system and irregular inertial forces, making the motion more stable. In practical applications, precise grasping, welding, spraying, cutting and other tasks can be carried out.

[0133] Specifically, the displacement function and the velocity function are prior arts. In this embodiment, they are only used as applications and will not be elaborated. It should be noted that the displacement function is as follows:

[0134] ;

[0135] ;

[0136] The velocity function is as follows:

[0137] ;

[0138] Among them, represents the initial velocity, J represents the jerk, a represents the acceleration, v represents the maximum velocity, T represents the motion time of each segment, represents the motion time of the first segment, represents the motion time of the second segment, represents the motion time of the third segment, represents the motion time of the fourth segment, represents the motion time of the fifth segment, represents the motion time of the sixth segment, represents the motion time of the seventh segment.

[0139] Further, interpolation points corresponding to the total arc length of the circular arc are determined based on the displacement function and the velocity function, and position and attitude synchronous proportional mapping motion is performed according to the interpolation points. For example, if the arc length interpolation value at a certain moment is Δs, this interpolation value is compared with arcL1. If it is greater, it means that the motion has reached the circular arc section p2 - p3 at this time; otherwise, it has reached the circular arc section p1 - p2. If it is defined as the first circular arc section at this time, the ratio Δs / (arcL1) is calculated to obtain the ratio of this interpolation value to arcL1 of the circular arc section p1 - p2, and it is synchronously mapped to Δθ / , where Δθ is the attitude interpolation value corresponding to the circular arc interpolation value, which can ensure the synchronous motion of position and attitude. At this time, the interpolation motion continues. When reaching the transition section, the interpolation speed, acceleration, and interpolation attitude value at the starting point position of the transition section are obtained, and the position interpolation speed, acceleration, and interpolation attitude value at the end of the transition are calculated.

[0140] According to an embodiment of the present invention, the method further includes, during the motion based on the interpolation points, connecting the transition sections of two arc lengths through a quintic polynomial. The quintic polynomial is as follows:

[0141] x(t)= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5;

[0142] where x(t) is the displacement at a certain moment t; a_0, a_1, a_2, a_3, a_4, a_5 are the coefficients of the polynomial, and t is the time.

[0143] It should be noted that in this embodiment, the transition section is connected through quintic polynomial planning. The purpose is to solve the problem of attitude speed jump at the connection points of two arc lengths and two attitudes in the proportional mapping. Through polynomial connection, no jump in speed and acceleration can be achieved. Among them, the quintic polynomial is widely used in motion planning and can simultaneously control the starting point and ending point constraints of displacement, speed, and acceleration, which is suitable for trajectory planning that requires smooth transition.

[0144] Specifically, the general form of the quintic polynomial is as follows:

[0145] x(t)= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5;

[0146] Where: x(t) is the displacement at a certain moment t; a_0, a_1, a_2, a_3, a_4, a_5 are the coefficients of the polynomial, and t is the time. Further, the transition section is connected by a fifth-degree polynomial and the interpolation position and interpolation attitude are output, so that it can smoothly transition to the second arc length and the second attitude. Then, according to (Δs - arcL1) / (arcL2), the ratio of this interpolation value to the arc L2 of the p2 - p3 section circular arc is obtained, and it is synchronously mapped to Δθ / , where Δθ is the attitude interpolation value corresponding to the circular arc interpolation value. Until the entire circular arc movement is completed, at this time the attitude movement also stops correspondingly.

[0147] The third aspect of the present invention provides a computer-readable storage medium, which includes a program for a general circular arc instruction position and attitude transition synchronization planning method of a robotic arm. When the program for the general circular arc instruction position and attitude transition synchronization planning method of the robotic arm is executed by a processor, the steps of a general circular arc instruction position and attitude transition synchronization planning method as described in any one of the above are implemented.

[0148] A general circular arc instruction position and attitude transition synchronization planning method and system disclosed by the present invention, based on geometric characteristic interpolation, can be dynamically adjusted, without the need for additional design of attitude interpolation, and can also achieve high-order smoothing of attitude transition, with simple calculation and not requiring too much additional computing power.

[0149] Among them, through synchronous mapping, the change of the attitude is associated with the arc length parameter of the circular arc path, making the attitude transition more natural and not having sudden rotations. The synchronous mapping can ensure the continuity of the first-order, second-order, and even higher-order derivatives of the position and attitude on the path, improving the overall smoothness of the trajectory.

[0150] In addition, the motion coordination is high. Through mapping, the changes of the attitude and the position are directly related, avoiding the problem that the attitude does not follow after the path is completed, and allowing the speed of the attitude change with the path to be adjusted proportionally to adapt to different task requirements. For example, increasing the speed of the attitude change at key points can ensure that the relationship between the attitude and the position remains consistent throughout the circular arc trajectory and is not prone to local incoordination phenomena.

[0151] In several embodiments provided by the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. The device embodiments described above are merely illustrative. For example, the division of the units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components can be combined, or can be integrated into another system, or some features can be ignored, or not executed. In addition, the coupling, direct coupling, or communication connection between the displayed or discussed components can be through some interfaces. The indirect coupling or communication connection of devices or units can be electrical, mechanical, or other forms.

[0152] The units described above as separate components may or may not be physically separated. The components displayed as units may or may not be physical units. They can be located in one place or distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0153] In addition, each functional unit in the embodiments of the present invention can be fully integrated into a processing unit, or each unit can be separately used as a unit, or two or more units can be integrated into one unit. The above-mentioned integrated units can be implemented in the form of hardware, or in the form of a combination of hardware and software functional units.

[0154] Those of ordinary skill in the art can understand that all or part of the steps to implement the above method embodiments can be completed by hardware related to program instructions. The foregoing program can be stored in a computer-readable storage medium. When the program is executed, it executes the steps including the above method embodiments. The foregoing storage medium includes: removable storage devices, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks, etc., which can store program codes.

[0155] Alternatively, if the above-mentioned integrated units of the present invention are implemented in the form of software function modules and sold or used as independent products, they can also be stored in a computer-readable storage medium. Based on such an understanding, the technical solution of the embodiments of the present invention, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the methods described in the embodiments of the present invention. The foregoing storage medium includes: removable storage devices, ROM, RAM, magnetic disks, or optical disks, etc., which can store program codes.

Claims

1. A general arc instruction position and attitude transition synchronization planning method for a robotic arm, characterized in that, The following steps are involved: Determining a center point and a radius of a circle based on user data, wherein the user data includes a start point, an end point, and a spatial reference point; A target arc is obtained based on the spatial reference point in combination with the center of the circle, and the target arc is proportionally divided to obtain different arc lengths, specifically including: calculating the posture difference of the spatial reference points P1, P2 and P3 to obtain a synchronous planning value, wherein the posture difference of the spatial reference points P1, P2 and P3 is calculated, and the largest value of the posture difference is selected as the synchronous planning value; and the target arc obtained by the spatial reference points P1, P2 and P3 and the center of the circle is divided into different arc lengths based on different positions, wherein the segment between the spatial reference points P1 and P2 is arcL1, and the segment between the spatial reference points P2 and P3 is arcL2; Determining synchronization conditions, specifically including obtaining a set transition segment parameter and multiplying different arc lengths to obtain a transition start point and a transition end point, specifically including: obtaining a set transition segment parameter blend-ratio, specifically a value in the interval [0,1], and a maximum length not exceeding half of the shortest arc length of the two arc lengths; multiplying the different arc lengths by the transition segment parameter blend-ratio to obtain arc length positions corresponding to the transition start point and the transition end point; Curve speed planning is performed according to the total arc length, and position and posture values are determined according to the interpolation points. Joint control commands of the robot arm are generated based on the position and posture values, specifically including: performing S-curve speed planning according to the total arc length to determine the displacement function and the speed function; determining the interpolation points corresponding to the total arc length based on the displacement function and the speed function; performing position and posture synchronous proportional mapping movement based on the interpolation points to obtain the position and posture values for interpolation movement, wherein the position and posture values are specifically decomposed into different joint control commands corresponding to the current robot arm to control the movement of the robot arm.

2. The method for synchronously planning the position and attitude transition of a general circular arc instruction of a robotic arm according to claim 1, wherein The determining of the center and radius of the circle based on user data, wherein the user data includes a starting point, an end point, and a spatial reference point, specifically includes: Get spatial reference points P1, P2 and P3; Determine a plane and a plane normal vector based on spatial reference points P1, P2, and P3; After the plane is determined, the center of the circle is obtained by jointly determining the equidistance constraint and the plane constraint, and the radius is calculated simultaneously.

3. A method for synchronously planning the position and attitude transition of a general arc instruction of a robotic arm according to claim 2, characterized in that, The method further includes: in the interpolation point motion process, connecting two arc length transition segments by a fifth-order polynomial, wherein the fifth-order polynomial is as follows: x(t)= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5; Where x(t) is the displacement at time t; a_0, a_1, a_2, a_3, a_4, a_5 are the coefficients of the polynomial, and t is time.

4. A general circular arc instruction position and attitude transition synchronization planning system for a robotic arm, characterized in that, The invention comprises a memory and a processor, wherein the memory comprises a method program for synchronously planning a general circular arc instruction position and a posture transition of a robotic arm, and when the method program for synchronously planning a general circular arc instruction position and a posture transition of a robotic arm is executed by the processor, the following steps are implemented: Determining a center point and a radius of a circle based on user data, wherein the user data includes a start point, an end point, and a spatial reference point; A target arc is obtained based on the spatial reference point in combination with the center of the circle, and the target arc is proportionally divided to obtain different arc lengths, specifically including: calculating the posture difference of the spatial reference points P1, P2 and P3 to obtain a synchronous planning value, wherein the posture difference of the spatial reference points P1, P2 and P3 is calculated, and the largest value of the posture difference is selected as the synchronous planning value; and the target arc obtained by the spatial reference points P1, P2 and P3 and the center of the circle is divided into different arc lengths based on different positions, wherein the segment between the spatial reference points P1 and P2 is arcL1, and the segment between the spatial reference points P2 and P3 is arcL2; Determining synchronization conditions, specifically including obtaining a set transition segment parameter and multiplying different arc lengths to obtain a transition start point and a transition end point, specifically including: obtaining a set transition segment parameter blend-ratio, specifically a value in the interval [0,1], and a maximum length not exceeding half of the shortest arc length of the two arc lengths; multiplying the different arc lengths by the transition segment parameter blend-ratio to obtain arc length positions corresponding to the transition start point and the transition end point; Curve speed planning is performed according to the total arc length, and position and posture values are determined according to the interpolation points. Joint control commands of the robot arm are generated based on the position and posture values, specifically including: performing S-curve speed planning according to the total arc length to determine the displacement function and the speed function; determining the interpolation points corresponding to the total arc length based on the displacement function and the speed function; performing position and posture synchronous proportional mapping movement based on the interpolation points to obtain the position and posture values for interpolation movement, wherein the position and posture values are specifically decomposed into different joint control commands corresponding to the current robot arm to control the movement of the robot arm.

5. A general arc instruction position and attitude transition synchronization planning system for a robotic arm according to claim 4, characterized in that, The determining of the center and radius of the circle based on user data, wherein the user data includes a starting point, an end point, and a spatial reference point, specifically includes: Get spatial reference points P1, P2 and P3; Determine a plane and a plane normal vector based on spatial reference points P1, P2, and P3; After the plane is determined, the center of the circle is obtained by jointly determining the equidistance constraint and the plane constraint, and the radius is calculated simultaneously.

6. A computer-readable storage medium, characterized in that, The computer-readable storage medium includes a method program for synchronously planning the position and posture transition of a general circular arc instruction of a robot arm. When the method program for synchronously planning the position and posture transition of a general circular arc instruction of a robot arm is executed by a processor, the steps of the method for synchronously planning the position and posture transition of a general circular arc instruction of a robot arm as described in any one of claims 1 to 3 are implemented.

Images