A trajectory planning method for industrial robots passing through auxiliary points in circular arcs

Abstract

The present invention discloses a method for planning an industrial robot's arc trajectory passing through auxiliary points, and relates to the field of industrial robot trajectory planning. The method solves the problem that the robot's arc motion trajectory is not smooth enough when passing through auxiliary points. The present invention uses a quaternion description method to plan the posture of the arc motion trajectory, and designs a quaternion polynomial to perform trajectory planning on the posture, which ultimately effectively solves the problem that the robot's arc motion trajectory cannot smoothly pass through the auxiliary point posture. The following steps are performed: obtaining motion parameters related to arc motion trajectory planning; position geometry path planning; posture geometry path planning; arc velocity interpolation. Designing a quaternion polynomial to perform trajectory planning on the posture avoids the large number of trigonometric function operations and universal joint deadlock problems of conventional Euler angle planning. The smooth operation of the robot's terminal trajectory is guaranteed. The synchronization of the arc motion trajectory posture is guaranteed.

Description

Technical Field

[0001] The present invention relates to the field of industrial robot trajectory planning, and in particular to a method for planning an industrial robot arc trajectory passing through an auxiliary point. Background Art

[0002] In the practical application of industrial robots, the robot's end-point needs to move along a circular trajectory to complete processes such as welding, cutting, and palletizing. During this motion, it must sequentially pass through the arc's starting point, auxiliary points, and end point. Currently, in most circular trajectory motions, the robot only passes through the arc's auxiliary points, not the posture. This can cause process problems in certain field applications. For example, in welding applications, the weld seam is an arc. If the welding gun posture does not pass through the auxiliary points during the welding process, it may interfere with the workpiece being welded, causing collisions and damaging both the workpiece and the welding gun. Therefore, to expand the application scope of industrial robots, a trajectory planning method for circular arc postures passing through auxiliary points is needed to ensure the continuity of the trajectory's position, posture velocity, and acceleration, as well as the synchronization of position and posture.

[0003] A Chinese invention patent application, Publication No. CN 105353725 A, entitled "A Method for Interpolating Circular Arcs in Space Through Auxiliary Points for Industrial Robots," proposes a method for interpolating circular arcs in space through auxiliary points. However, the patent describes the posture using Euler angles, which can lead to gimbal lock during rotation. Furthermore, the patent only guarantees the continuity of the rate of change of the posture, without mentioning the second-order continuity of the posture. A paper entitled "A Class of C2-Continuous Unit Quaternion Interpolation Spline Curves" proposes a method for rapidly generating unit quaternion interpolation spline curves by selecting an appropriate quartic polynomial blending function for interpolation, but the smoothness of the interpolated curve needs to be improved. A paper entitled "Implementation of a Spatial Circular Arc Pose Trajectory Planning Algorithm for Manipulators" proposes a circular arc pose trajectory planning method, but the computation is complex and cannot adjust the smoothness of the circular pose geometric path. Therefore, improving industrial robot trajectory planning methods and solving the problem of smooth planning of the robot's circular motion through auxiliary points have become urgent technical challenges for those skilled in the art. Summary of the Invention

[0004] In response to the above problems, the present invention proposes a method for planning the circular arc trajectory of an industrial robot passing through auxiliary points, which solves the problem that the robot's circular arc motion trajectory is not smooth enough when passing through the auxiliary points, and the universal joint deadlock problem caused by using Euler angle interpolation. The present invention uses a quaternion description method to plan the posture of the circular arc motion trajectory, and designs a quaternion polynomial to plan the trajectory of the posture, which ultimately effectively solves the problem that the robot's circular arc motion trajectory cannot smoothly pass through the auxiliary points.

[0005] The technical solution of the present invention is to carry out the following steps:

[0006] Step 1: Obtain motion parameters related to arc motion trajectory planning;

[0007] Teach to obtain the spatial position and posture of the starting point, the spatial position and posture of the auxiliary point, and the spatial position and posture of the end point of the arc motion trajectory;

[0008] Step 2: Position geometry path planning: Based on the obtained spatial positions of the starting point, auxiliary point, and end point of the arc motion, solve the simultaneous equations to obtain the coordinates of the center of the circle with the constraints that the three points are coplanar and the distances from the three points to the spatial center coordinates are equal, and calculate the radius, the center angle from the auxiliary point to the end point, the center angle from the starting point to the end point, and the total arc length;

[0009] Step 3: Posture geometry path planning;

[0010] The three-point posture of the teaching is converted from Euler angle to quaternion description; according to the posture of the starting point, auxiliary point, and end point, a posture auxiliary point is designed between the posture of the arc starting point and the auxiliary point, and between the posture of the auxiliary point and the posture of the arc end point. With the help of the three-dimensional space spline curve interpolation idea, a quaternion posture trajectory curve that meets C2 continuity is constructed;

[0011] Step 4: Arc speed interpolation;

[0012] Step 4.1, arc position and speed planning;

[0013] After the arc motion path is generated in step 2, the speed planning can be performed on the position space of the path. The speed planning method and the boundary conditions of maximum speed and maximum acceleration are used to plan the speed, acceleration, motion time from the arc position starting point to the auxiliary point, and the total motion time of the arc position in the arc position space;

[0014] Step 4.2: Arc attitude speed planning;

[0015] Based on the parameterized posture geometric path obtained in step 3 and the motion time from the arc position starting point to the auxiliary point and the total motion time of the arc position obtained in step 4.1, the posture speed planning module plans the posture speed;

[0016] Finally, the robot end position and posture vector at each moment of the circular motion trajectory are obtained through real-time interpolation in steps 4.1 and 4.2 above.

[0017] Furthermore, step 3 is specifically as follows:

[0018] The three-point posture of the teaching is converted from Euler angles (a, b, c) to quaternion q. The formula for converting the posture of each axis into quaternion description is:

[0019]

[0020] The starting point of the arc is q s (w s , x s ,y s , z s ), the arc auxiliary point posture is q m (w m , x m ,y m , z m ), the arc end point posture is q e (w e , x e ,y e , z e ).

[0021] According to the posture of the starting point, auxiliary point and end point, a posture auxiliary point is designed between the posture of the arc starting point and the posture of the auxiliary point, and between the posture of the auxiliary point and the posture of the arc end point. aux ,q aux2 ; With the help of the three-dimensional spline curve interpolation idea, a quaternion attitude trajectory curve that satisfies C2 continuity is constructed, where q s -1 ,q m -1 ,q e -1 q s ,q m ,q e The inverse of q aux1 -1 ,q aux2 -1 q aux1 ,q aux2 The inverse of , θ represents the quaternion angle, Indicates q s -1 q m and q m -1 q e Minimum value of angle;

[0022] The quaternion attitude interpolation formula is:

[0023]

[0024]

[0025]

[0026]

[0027]

[0028] γ(u i )=-u i 6 +3u i 5 -3u i 4 +u i 3 i=1,2

[0029] α(u i )=u i 4 -2u i 3 +2u i i=1,2

[0030] β(u i )=-u i 4 +2u i 3 i=1,2

[0031] In the above formula, α(u i ),β(u i ),γ(u i ) is a polynomial that satisfies the conditions, k is a compliance parameter, and its value range is (0, 1). The larger the value, the smaller the curvature change of the posture curve and the better the smoothness. i is 1 between the starting point of the arc and the auxiliary point, and i is 2 between the auxiliary point of the arc and the end point. Q i (u i ) is the expected posture path function through a series of interpolations during the two periods, u i The rate of motion describes how the path is moved, as a function of time.

[0032] Furthermore, the working method of the attitude and speed planning module in step 4.2 is:

[0033] The design motion rate is u i (t), i is 1 between the arc starting point and the auxiliary point, and i is 2 between the arc auxiliary point and the end point. The corresponding coefficient c is obtained by fitting a fifth-order polynomial curve.

[0034] u i (t) = c0 + c1t + c2t 2 +c3t 3 +c4t 4 +c5t 5 i=1,2

[0035]

[0036] Through u i(t) Real-time interpolation outputs the robot end posture vector at each moment of the arc motion trajectory, converts the interpolated quaternion into Euler angles, and completes the trajectory planning of the posture on the arc segment. The formula for converting quaternion to Euler angles is:

[0037]

[0038] Finally, the robot end position and posture vector at each moment of the circular motion trajectory are obtained through real-time interpolation in steps 4.1 and 4.2 above.

[0039] The beneficial effects of the present invention are:

[0040] 1. The arc passing through auxiliary point attitude trajectory planning method proposed in the present invention uses quaternions to describe the attitude for planning, avoiding the large number of trigonometric function calculations and universal joint deadlock problems of conventional Euler angle planning.

[0041] 2. The present invention plans the arc starting point, auxiliary point, and end point posture path through quaternion polynomials. The robot posture speed and acceleration will not change suddenly in the entire arc trajectory, ensuring the smooth operation of the robot terminal trajectory.

[0042] 3. The present invention can change the smoothness of the posture interpolation curve by adjusting the compliance parameters.

[0043] 4. The present invention synchronizes the planning of posture speed based on the speed planning time of the arc position path, thereby ensuring the synchronization of the posture of the arc motion trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a workflow diagram of the present invention;

[0045] Figure 2 It is a schematic diagram of the posture interpolation curve in the present invention. DETAILED DESCRIPTION

[0046] In order to clearly illustrate the technical features of this patent, this patent is described in detail below through specific implementation methods and in combination with its accompanying drawings.

[0047] The quaternion polynomial attitude curve designed by the present invention can smoothly pass through the attitude of the arc auxiliary point, and by changing the compliance coefficient, the smoothness of the attitude interpolation curve is adjusted, thereby solving the problem that the arc motion trajectory cannot smoothly pass through the auxiliary point attitude.

[0048] On the one hand, the arc-through-auxiliary-point posture method provided by the present invention plans the robot's arc segment trajectory, ensuring that the robot's posture velocity and posture acceleration do not undergo sudden changes during the entire arc motion trajectory planning, thereby ensuring smooth operation of the robot's terminal trajectory. The smoothness of the posture can be adjusted by changing the compliance parameter. On the other hand, the present invention synchronously plans the robot's arc posture velocity based on the arc position and velocity planning method, ensuring the synchronization of the robot's arc position and posture.

[0049] The main methods and steps are as follows:

[0050] Step 1: Obtain motion parameters related to arc motion trajectory planning;

[0051] Teach to obtain the starting point spatial position P of the arc motion trajectory s (x s ,y s , z s ) and posture R s (a s , b s , c s ), auxiliary point spatial position P m (x m ,y m , z m ) and posture R m (a m , b m , c m ), the end point spatial position P e (x e ,y e , z e ) and posture R e (a e , b e , c e ).

[0052] Step 2: Position geometry path planning;

[0053] According to the obtained arc motion starting point spatial position P s (x s ,y s , z s ), auxiliary point spatial position P m (x m ,y m , z m ), the end point spatial position P e (x e ,y e , z e), with the constraints that the three points are coplanar and the distances from the three points to the center coordinates of the space circle are equal, solve the simultaneous equations to obtain the coordinates of the center of the circle, and further calculate the radius, the center angle from the auxiliary point to the end point, the center angle from the start point to the end point, and the total arc length.

[0054] Step 3: Posture geometry path planning;

[0055] The three-point posture of the teaching is converted from Euler angles (a, b, c) to quaternion q. The formula for converting the posture of each axis into quaternion description is:

[0056]

[0057] The starting point of the arc is q s (w s , x s ,y s , z s ), the arc auxiliary point posture is q m (w m , x m ,y m , z m ), the arc end point posture is q e (w e , x e ,y e , z e ).

[0058] According to the posture of the starting point, auxiliary point and end point, a posture auxiliary point is designed between the posture of the arc starting point and the posture of the auxiliary point, and between the posture of the auxiliary point and the posture of the arc end point. aux1 ,q aux2 ; With the help of the three-dimensional spline curve interpolation idea, a quaternion attitude trajectory curve that satisfies C2 continuity is constructed. C2 represents the differentiability of the function curve. The combined parameter curve has a second-order continuous derivative at the connection point. This type of smoothness is called C2 continuity; where q s -1 ,q m -1 ,q e -1 q s ,q m ,q e The inverse of q aux1 -1 ,q aux2 -1 q aux1 ,q aux2 The inverse of , θ represents the quaternion angle, Indicates q s -1 q m and q m-1 q e The minimum value of the angle.

[0059] The quaternion attitude interpolation formula is:

[0060]

[0061]

[0062]

[0063]

[0064]

[0065] γ(u i )=-u i 6 +3u i 5 -3u i 4 +u i 3 i=1,2

[0066] α(u i )=u i 4 -2u i 3 +2u i i=1,2

[0067] β(u i )=-u i 4 +2u i 3 i=1,2

[0068] In the above formula, α(u i ),β(u i ),γ(u i ) is a polynomial that satisfies the conditions, k is a compliance parameter, and its value range is (0, 1). The larger the value, the smaller the curvature change of the posture curve and the better the smoothness. i is 1 between the starting point of the arc and the auxiliary point, and i is 2 between the auxiliary point of the arc and the end point. Q i (u i ) is the expected posture path function through a series of interpolations during the two periods, u i The rate of motion describes how the path is moved, as a function of time.

[0069] Step 4: Arc speed interpolation;

[0070] Step 4.1, arc position and speed planning;

[0071] After the arc motion path is generated by step 2, the speed planning of the position space of the path can be done. You can choose the trapezoidal acceleration and deceleration control planning method or the S-curve acceleration and deceleration control planning method or other planning methods. The corresponding speed planning method and the maximum speed v max , maximum acceleration a max The boundary conditions of the arc position space are used to plan the speed, acceleration, and movement time t from the starting point of the arc position to the auxiliary point. m and the total motion time t of the arc position e , where the maximum speed is given by the command, and the maximum acceleration is a robot system parameter or given by the command. In this step, the arc position velocity module F(t) interpolates and outputs the robot end position vector at each moment of the arc motion trajectory in real time based on time t. The boundary conditions of the arc position trajectory are as follows:

[0072]

[0073] Step 4.2: Arc attitude speed planning;

[0074] Based on the parameterized posture geometric path obtained in step 3 and the motion time t from the arc position starting point to the auxiliary point obtained in step 4.1 m and the total motion time t of the arc position e The attitude speed planning module plans the attitude speed, and the design motion rate is u i (t), i is 1 between the arc starting point and the auxiliary point, and i is 2 between the arc auxiliary point and the end point. The corresponding coefficient c is obtained by fitting a fifth-order polynomial curve.

[0075] u i (t) = c0 + c1t + c2t 2 +c3t 3 +c4t 4 +c5t 5 i=1,2

[0076]

[0077] Through u i (t) Real-time interpolation outputs the robot end posture vector at each moment of the arc motion trajectory, converts the interpolated quaternion into Euler angles, and completes the trajectory planning of the posture on the arc segment. The formula for converting quaternion to Euler angles is:

[0078]

[0079] Finally, the robot end position and posture vector at each moment of the circular motion trajectory are obtained through real-time interpolation in steps 4.1 and 4.2 above.

[0080] There are many specific implementation ways of the present invention. The above is only the preferred implementation method of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements can be made without departing from the principles of the present invention. These improvements should also be considered as the scope of protection of the present invention.

Claims

1. A method for planning an arc trajectory of an industrial robot passing through an auxiliary point, characterized in that: Follow these steps: Step 1: Obtain motion parameters related to arc motion trajectory planning; Teach to obtain the spatial position and posture of the starting point, the spatial position and posture of the auxiliary point, and the spatial position and posture of the end point of the arc motion trajectory; Step 2: Position geometry path planning: Based on the obtained spatial positions of the starting point, auxiliary point, and end point of the arc motion, solve the simultaneous equations to obtain the coordinates of the center of the circle with the constraints that the three points are coplanar and the distances from the three points to the spatial center coordinates are equal, and calculate the radius, the center angle from the auxiliary point to the end point, the center angle from the starting point to the end point, and the total arc length; Step 3: Posture geometry path planning; The three-point posture taught is converted from Euler angles to quaternion descriptions. Based on the postures of the starting point, auxiliary point, and end point, an auxiliary point is designed between the posture of the arc starting point and the auxiliary point, and between the posture of the auxiliary point and the posture of the arc end point. With the help of the three-dimensional spline curve interpolation idea, a quaternion posture trajectory curve that satisfies C2 continuity is constructed. Step 3 is as follows: The three-point posture of the teaching is converted from Euler angles (a, b, c) to quaternion q. The formula for converting the posture of each axis to quaternion description is: The starting point of the arc is q s (w s ,x s ,y s ,z s ), the arc auxiliary point posture is q m (w m ,x m ,y m ,z m ), the arc end point posture is q e (w e ,x e ,y e ,z e ); According to the posture of the starting point, auxiliary point and end point, a posture auxiliary point is designed between the posture of the arc starting point and the posture of the auxiliary point, and between the posture of the auxiliary point and the posture of the arc end point. aux1 ,q aux2 ; With the help of the three-dimensional spline curve interpolation idea, a quaternion attitude trajectory curve that satisfies C2 continuity is constructed, where q s -1 ,q m -1 ,q e -1 q s ,q m ,q e The inverse of q aux1 -1 ,q aux2 -1 q aux1 ,q aux2 The inverse of , θ represents the quaternion angle, Indicates q s -1 q m and q m -1 q e Minimum value of angle; The quaternion attitude interpolation formula is: γ(u i )=-u i 6 +3u i 5 -3u i 4 +and i 3 i=1,2 α(in i )=in i 4 -2u i 3 +2in i and=1.2 β(in i )=-in i 4 +2in i 3 and=1.2 In the above formula, α(u i ),β(u i ),γ(u i ) is a polynomial that satisfies the conditions, k is a compliance parameter, and its value range is (0,1). The larger the value, the smaller the curvature change of the posture curve and the better the smoothness. i is 1 between the starting point of the arc and the auxiliary point, and i is 2 between the auxiliary point of the arc and the end point. Q i (u i ) is the expected posture path function through a series of interpolations during the two periods, u i The rate of motion that describes how to move along this path is a function of time; Step 4: Arc speed interpolation; Step 4.1, arc position and speed planning; After the arc motion path is generated in step 2, the speed planning can be performed on the position space of the path. The speed planning method and the boundary conditions of maximum speed and maximum acceleration are used to plan the speed, acceleration, motion time from the arc position starting point to the auxiliary point, and the total motion time of the arc position in the arc position space; Step 4.2: Arc attitude speed planning; Based on the parameterized posture geometric path obtained in step 3 and the motion time from the arc position starting point to the auxiliary point and the total motion time of the arc position obtained in step 4.1, the posture speed planning module plans the posture speed; Finally, the robot end position and posture vector at each moment of the circular motion trajectory are obtained through real-time interpolation in steps 4.1 and 4.2 above.

2. The method for planning an arc trajectory of an industrial robot passing through an auxiliary point according to claim 1, characterized in that: The working method of the attitude and speed planning module in step 4.2 is: The design motion rate is u i (t), i is 1 between the arc starting point and the auxiliary point, and i is 2 between the arc auxiliary point and the end point. The corresponding coefficients c0, c1, c2, c3, c4, and c5 are obtained by fitting a fifth-order polynomial curve; u i (t)=c0+c1t+c2t 2 +c3t 3 +c4t 4 +c5t 5 i=1,2 Through u i (t) Interpolate and output the robot end posture vector at each moment of the arc motion trajectory in real time, convert the interpolated quaternion into Euler angles, and complete the trajectory planning of the posture on the arc segment; the formula for converting quaternion to Euler angles is: Finally, the robot end position and posture vector at each moment of the circular motion trajectory are obtained through real-time interpolation in steps 4.1 and 4.2 above.

Images