G3 continuous and stable-motion robot path fairing method

Abstract

The invention relates to a G3 continuous and stable-motion robot path fairing method. The method comprises the steps that firstly, robot tail end position path smoothing is conducted under a workpiece coordinate system, and continuous smooth switching of the position G3 at the path connection position is achieved by constructing a Bezier curve corner transition model for six times; secondly, robot tail end posture path fairing is carried out in an Euler angle space around a workpiece coordinate system, and posture G3 continuous smooth switching at the path connection position is achieved by constructing a Bezier curve corner transition model for six times; and finally, parameter synchronization processing is carried out on the tail end position and the posture path of the robot after transferring and fairing, position and posture transition parameters are adjusted in a self-adaptive mode by considering posture synchronization conditions and geometrical characteristics of the posture path, and it is ensured that the first-order derivative, the second-order derivative and the third-order derivative of the tail end posture relative to the arc length of the position path are continuous. According to the method, continuous smooth switching of the tail-section pose G3 of the robot can be achieved, no iterative computation is needed, the fairing error is controllable, and meanwhile angular motion of a tool cutter shaft at the tail end of the robot is more stable.

Description

Technical Field

[0001] The invention belongs to the technical field of robot arm path smoothing, and in particular is a robot path smoothing method with G3 continuity and stable motion. Background Art

[0002] In recent years, robots have gradually replaced multi-axis machine tools for processing curved and surface parts with their advantages of low cost, high flexibility, and large workspace. Unlike point-to-point motion in application scenarios such as handling and palletizing, this type of processing requires the robot's end effector to perform precise and smooth contour motion. Since the processing path is usually composed of multiple linear paths, the tangential and normal discontinuities at the corners will cause sudden changes in the robot's end tool motion, seriously affecting the processing efficiency and processing stability. Therefore, it is necessary to perform corner smoothing on the linear path of the six-axis robot.

[0003] The difficulty of robot linear path smoothing lies in how to achieve high-order smooth transition of end position and posture under the constraint of smoothing error, and how to synchronize the parameters of the end position and posture path. At present, most robot smoothing algorithms can achieve G2 continuity (curvature continuity) of the motion path, but due to the poor stiffness of the robot, this continuity is still difficult to ensure the smoothness of the end tool motion. In addition, in order to achieve parameter synchronization of the end position and posture path, the existing smoothing algorithm often re-parameterizes the remaining path, but because it ignores the geometric characteristics of the position and posture path, it will cause the tool axis angular velocity, angular acceleration and angular jump at the path transition point to fluctuate greatly. Therefore, there is an urgent need for a robot path smoothing method that can achieve high-order smooth transition of the motion path and ensure good angular motion performance of the end tool axis. Summary of the invention

[0004] To solve the above problems, the present invention provides a G3 continuous and smooth motion robot path smoothing method, which can not only analytically realize the G3 (curvature differential continuity) continuity of the motion path, but also ensure that the angular motion of the tool direction is smoother.

[0005] The technical solution of the present invention is described as follows in conjunction with the accompanying drawings:

[0006] A G3 continuous and smooth robot path smoothing method comprises the following steps:

[0007] Step 1: Perform path smoothing of the robot end position in the workpiece coordinate system, and achieve continuous and smooth transition of position G3 at the path connection by constructing a 6th-order Bezier curve corner transition model; the position transition parameters meet the position smoothing error tolerance and adjacent path length constraints;

[0008] Step 2: Smoothing the robot end posture path in the Euler angle space around the workpiece coordinate system, and realizing the continuous and smooth transition of posture G3 at the path connection by constructing a 6th-order Bezier curve corner transition model; the posture transition parameters meet the direction smoothing error tolerance and adjacent path length constraints;

[0009] Step 3: Perform parameter synchronization processing on the robot end position and posture path after the transfer and smoothing. Adaptively adjust the position and posture transition parameters by considering the posture synchronization conditions and the geometric characteristics of the posture path to ensure the continuity of the first-order, second-order, and third-order derivatives of the end posture relative to the arc length of the position path, thereby obtaining a G3 continuous and smooth robot path.

[0010] Furthermore, the specific method of step one is as follows:

[0011] 11) Describe the robot tool end position P = [x, y, z] in the workpiece coordinate system. Let the three consecutive position points in the original linear position path be P i-1 , P i , P i+1 At the corner formed by these three points, a 6th-order Bezier curve is used to construct a G3 continuous corner transition model. The curve equation is as follows:

[0012]

[0013] Where P(u) is the angle at the corner point P i The end position transfer curve inserted at B i are the control points of the inserted sixth-order Bezier curve P(u), the number of control points is 7; i is the control point number; is the Bezier curve basis function; u is the curve parameter;

[0014] 12) Set conditions:

[0015] a. The first three control points B0, B1, and B2 of the inserted curve are located at the end position path P i-1 P i On the segment, the last three control points B4, B5, and B6 are located at the end position path P i-1 P i section;

[0016] b. Right now

[0017]

[0018] In the formula, B i is the control point of the constructed position Bezier curve P(u); where i is the control point number, and the length relationship between the control points can be obtained according to the above formula as follows:

[0019]

[0020] In the formula, l pa , l pb is the position curve transition parameter; to ensure symmetry, l pa = l pb = l p ;

[0021] 13) Set position transition parameters l p Requirements:

[0022]

[0023] Where P i-1 , P i , P i+1 are three adjacent position points in the position path; θ p is the angle formed, ε pos is the position smoothing error tolerance.

[0024] Furthermore, the specific method of step 2 is as follows:

[0025] 21) Describe the robot end posture φ=[α,β,γ] in the Euler angle space around the workpiece coordinate system. Let the three consecutive posture points in the original posture path be φ i-1 ,φ i ,φ i+1 , as with position smoothing, a 6th-order Bezier curve corner transition model is constructed at the corner, and the curve equation is as follows:

[0026]

[0027] Where φ(u) is the angle at the corner point φ i The terminal attitude transition curve inserted at j is the control point of the inserted sixth-order Bezier curve φ(u), j is the control point number, and the number of control points is 7; is the Bezier curve basis function; u is the curve parameter;

[0028] 22) Set conditions:

[0029] a. The first three control points Q0, Q1, Q2 of the inserted sixth degree Bezier curve should be located on the attitude linear segment φ i-1 φ i The last three control points Q4, Q5, and Q6 should be located on the attitude linear segment φ i φ i+1 superior;

[0030] b. Right now

[0031]

[0032] In the formula, Q j is the control point of the constructed posture Bezier curve φ(u), where j is the control point number. According to the above formula, the length relationship between the control points is as follows:

[0033]

[0034] In the formula, l oa , l ob is the attitude spline curve transition parameter, the value is not necessarily the same, in order to obtain better smoothness, remember l ob and l oa The ratio is k;

[0035] 23) Set the attitude spline transition parameters l oa , l ob Requirements:

[0036]

[0037] In the formula, φ i-1 ,φ i ,φ i+1 are three adjacent posture points in the posture path; θ o is the angle formed; ε ori is the end tool direction smoothing error tolerance; is the attitude transition parameter l ob and l oa The ratio of ||J o || is the Jacobian matrix between the end tool orientation and the Euler angle, that is:

[0038]

[0039] In the formula, s α 、s β 、s γ They are the abbreviations of sin(α), sin(β), and sin(γ); c α 、c β 、c γ They are the abbreviations of cos(α), cos(β), and cos(γ), respectively. α, β, and γ are the ZYX Euler angles of the end tool relative to the workpiece coordinate system (WCS).

[0040] Furthermore, the specific method of step three is as follows:

[0041] 31) Set conditions:

[0042] a.

[0043] In the formula, s p is the arc length of the end position path, simplifying the above formula, the posture synchronization condition is:

[0044]

[0045] So the attitude spline transition parameter l ob and l oa Proportion

[0046] b.

[0047] 32) Taking into account the pose synchronization conditions, the geometric characteristics of the adjacent pose paths and the pose smoothing error constraints, the pose transition parameters are adaptively determined, and the result is:

[0048]

[0049] 33) According to the position transition parameter l p and attitude transition parameter l oa , l ob , respectively determine the control points of the constructed position curve P(u) and attitude curve φ(u);

[0050]

[0051] In the formula, P i Point to P i-1 The unit vector of P i Point to P i+1 The unit vector of is φ i Pointing to φ i-1 The unit vector of is φ i Pointing to φ i+1 The unit vector of .

[0052] 34) After obtaining the control points, a G3 continuous, error-controllable and parameter-synchronized position and posture transition curve is generated according to the control points, thereby obtaining a smooth and continuous motion path that is conducive to the smooth movement of the industrial robot.

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

[0054] 1) The present invention adopts a local smoothing technical solution. By constructing a local smoothing model of the sixth-order Bezier curve at the connection of the line segments, the pose path G3 can be continuously and smoothly transferred. The smoothing error is controllable and no iterative calculation is required, which further improves the continuity of the robot's motion path and is conducive to fast and stable processing of the robot.

[0055] 2) The present invention can analytically realize the synchronization of posture path parameters by adaptively adjusting the posture transition parameters by considering parameter synchronization conditions and adjacent path geometric constraints, and can overcome the problem of drastic fluctuations in the angular motion of the end tool tool axis, which is further beneficial to the stability of robot processing. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0057] Figure 1 It is a flowchart of the present invention;

[0058] Figure 2 It is the smoothing diagram of the robot end position path;

[0059] Figure 3 It is the smoothing diagram of the robot's terminal posture path;

[0060] Figure 4 It is the synchronization diagram of the terminal posture;

[0061] Figure 5 This is a comparison diagram of the terminal trajectories of the present invention and the existing method;

[0062] Figure 6 A comparison diagram of joint acceleration between the present invention and the prior art method;

[0063] Figure 7 This is a comparison diagram of the joint jumps of the present invention and the existing method. DETAILED DESCRIPTION

[0064] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the present invention, rather than to limit the present invention. It should also be noted that, for ease of description, only parts related to the present invention, rather than all structures, are shown in the accompanying drawings.

[0065] Embodiment 1

[0066] like Figure 1As shown, this embodiment provides a G3 continuous and smooth robot path smoothing method, comprising the following steps:

[0067] Step 1: Perform path smoothing of the robot end position in the workpiece coordinate system. By constructing a 6th-order Bezier curve corner transition model, the position G3 continuous and smooth transition at the path connection is achieved; the position transition parameters meet the position smoothing error tolerance and adjacent path length constraints. The specific method is as follows:

[0068] 11) Describe the robot tool end position P = [x, y, z] in the workpiece coordinate system. Let the three consecutive position points in the original linear position path be P i-1 , P i , P i+1 At the corner formed by these three points, a 6th-order Bezier curve is used to construct a G3 continuous corner transition model. The curve equation is as follows:

[0069]

[0070] Where P(u) is the angle at the corner point P i The end position transfer curve inserted at B i are the control points of the inserted sixth-order Bezier curve P(u), the number of control points is 7; i is the control point number; is the Bezier curve basis function; u is the curve parameter;

[0071] 12) Set conditions:

[0072] a. To ensure the tangential continuity of P(u) and the remaining straight line segments, the first three control points B0, B1, and B2 of the inserted curve are located at the end position path P i-1 P i On the segment, the last three control points B4, B5, and B6 are located at the end position path P i-1 P i section;

[0073] b. To achieve G3 continuity (curvature differential continuity) at the path connection, the following conditions must also be met:

[0074] Right now

[0075]

[0076] In the formula, B i is the control point of the constructed position Bezier curve P(u); where i is the control point number, and the length relationship between the control points can be obtained according to the above formula as follows:

[0077]

[0078] In the formula, l pa , l pb is the position curve transition parameter; to ensure symmetry, l pa = l pb = l p ;

[0079] 13) To avoid overlapping and intersecting of the constructed transition curves and to ensure that the smoothing error is within the position smoothing error tolerance, set the transition parameter l p Requirements:

[0080]

[0081] In the formula, Figure 2 As shown, P i-1 , P i , P i+1 are three adjacent position points in the position path; θ p is the angle formed, ε pos is the position smoothing error tolerance.

[0082] Step 2: Perform path smoothing of the robot end posture in the Euler angle space around the workpiece coordinate system (WCS). By constructing a 6th-order Bezier curve corner transition model, the posture G3 continuous and smooth transition at the path connection is achieved; the posture transition parameters meet the direction smoothing error tolerance and adjacent path length constraints. The specific method is as follows:

[0083] 21) Describe the robot end posture φ = [α, β, γ] in the Euler angle space around the workpiece coordinate system (WCS), such as Figure 3 As shown, let the three consecutive posture points in the original posture path be φ i-1 ,φ i ,φ i+1 , as with position smoothing, a 6th-order Bezier curve corner transition model is constructed at the corner, and the curve equation is as follows:

[0084]

[0085] Where φ(u) is the angle at the corner point φ i The terminal attitude transition curve inserted at j is the control point of the inserted sixth-order Bezier curve φ(u), j is the control point number, and the number of control points is 7; is the Bezier curve basis function; u is the curve parameter;

[0086] 22) Set conditions:

[0087] a. To ensure the tangential continuity of the attitude transition curve φ(u) and the original straight line segment, as Figure 3As shown, the first three control points Q0, Q1, and Q2 of the inserted 6-degree Bezier curve should be located on the attitude linear segment φ i-1 φ i The last three control points Q4, Q5, and Q6 should be located on the attitude linear segment φ i φ i+1 superior;

[0088] b. To achieve G3 continuity (curvature differential continuity) at the path connection, the following conditions must also be met:

[0089] Right now

[0090]

[0091] In the formula, Q j is the control point of the constructed posture Bezier curve φ(u), where j is the control point number. According to the above formula, the length relationship between the control points is as follows:

[0092]

[0093] In the formula, l oa , l ob is the attitude spline curve transition parameter, the value is not necessarily the same, in order to obtain better smoothness, remember l ob and l oa The ratio is k;

[0094] 23) In order to avoid overlapping and intersecting of the constructed attitude transfer curves and ensure that the smoothing error is within the smoothing error tolerance of the end tool direction, the attitude spline curve transition parameter l oa , l ob Requirements:

[0095]

[0096] In the formula, Figure 3 As shown, φ i-1 ,φ i ,φ i+1 are three adjacent posture points in the posture path; θ o is the angle formed; ε ori is the end tool direction smoothing error tolerance; is the attitude transition parameter l ob and l oa The ratio of ||J o || is the Jacobian matrix between the end tool orientation and the Euler angle, that is:

[0097]

[0098] In the formula, s α 、sβ 、s γ They are the abbreviations of sin(α), sin(β), and sin(γ); c α 、c β 、c γ They are the abbreviations of cos(α), cos(β), and cos(γ), respectively. α, β, and γ are the ZYX Euler angles of the end tool relative to the workpiece coordinate system (WCS).

[0099] Step 3: Perform parameter synchronization processing on the robot end position and posture path after the transfer and smoothing. By considering the posture synchronization conditions and the geometric characteristics of the posture path, the position and posture transition parameters are adaptively adjusted to ensure that the first-order, second-order, and third-order derivatives of the end posture relative to the arc length of the position path are continuous, thereby obtaining a G3 continuous and smooth robot path. The final path is high-order continuous and parameter synchronized, and due to the adaptive adjustment of the posture transition parameters, the robot can be guaranteed to move more smoothly, as follows:

[0100] 31) Set conditions:

[0101] a. Figure 4 As shown, to achieve parameter synchronization, the position and attitude paths need to share the same curve parameters and the first, second, and third order differentials of the end attitude relative to the arc length of the position path must be continuous at the span, that is, they must satisfy:

[0102]

[0103] In the formula, s p is the arc length of the end position path, simplifying the above formula, the posture synchronization condition is:

[0104]

[0105] So the attitude spline transition parameter l ob and l oa Proportion

[0106] b. Considering the above-mentioned posture synchronization conditions, in order to ensure that the end tool direction error is within the constraint range, its position path transition parameter l p Also need to meet:

[0107]

[0108] 32) Taking into account the pose synchronization conditions, the geometric characteristics of the adjacent pose paths and the pose smoothing error constraints, the pose transition parameters are adaptively determined, and the result is:

[0109]

[0110] 33) According to the position transition parameter l p and attitude transition parameter l oa , l ob , respectively determine the control points of the constructed position curve P(u) and attitude curve φ(u);

[0111] like Figure 2 As shown, P i Point to P i-1 The unit vector of P i Point to P i+1 The unit vector of is φ i Pointing to φ i-1 The unit vector of is φ i Pointing to φ i+1 The unit vector of .

[0112]

[0113] 34) After obtaining the control points, a G3 continuous, error-controllable and parameter-synchronized position and posture transition curve is generated according to the control points, thereby obtaining a smooth and continuous motion path that is conducive to the smooth movement of the industrial robot.

[0114] In summary, the present invention proposes a G3 continuous and smooth motion robot path smoothing method, which can analytically realize the G3 continuous (curvature differential continuity) smooth transition of the robot's terminal posture without any iterative calculation. The smoothing error is controllable and the angular motion of the tool axis of the robot's terminal tool is smoother.

[0115] Embodiment 2

[0116] In order to verify the effectiveness and superiority of the method proposed in the invention, under the same speed planning, the existing robot path smoothing method is selected to compare and verify with the present invention;

[0117] Depend on Figure 5 It can be seen that in terms of the terminal trajectory, the G3 continuous and smooth motion robot path smoothing method proposed in the present invention has a smaller fluctuation in the angular velocity of the terminal tool axis and continuous motion, which further shows that the method proposed in this article can make the rotation of the robot's terminal tool axis smoother.

[0118] Depend on Figure 6 It can be seen that in terms of joint acceleration, the G3 continuous and smooth motion robot path smoothing method proposed in the present invention has continuous joint acceleration without mutation and a smaller fluctuation amplitude in comparison, which indirectly illustrates that the method in this paper can improve the motion smoothness of the robot.

[0119] Depend on Figure 7It can be seen that in terms of joint jump, the G3 continuous and smooth motion robot path smoothing method proposed in the present invention has smaller fluctuations in joint jump, which can also further verify the effectiveness of the method proposed in this article.

[0120] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A G3 continuous and smooth robot path smoothing method, characterized in that: The following steps are involved: Step 1: Perform path smoothing of the robot end position in the workpiece coordinate system, and achieve continuous and smooth transition of position G3 at the path connection by constructing a 6th-order Bezier curve corner transition model; the position transition parameters meet the position smoothing error tolerance and adjacent path length constraints; Step 2: Smoothing the robot end posture path in the Euler angle space around the workpiece coordinate system, and realizing the continuous and smooth transition of posture G3 at the path connection by constructing a 6th-order Bezier curve corner transition model; the posture transition parameters meet the direction smoothing error tolerance and adjacent path length constraints; Step 3: Perform parameter synchronization processing on the robot end position and posture path after the transfer and smoothing. Adaptively adjust the position and posture transition parameters by considering the posture synchronization conditions and the geometric characteristics of the posture path to ensure the continuity of the first-order, second-order, and third-order derivatives of the end posture relative to the arc length of the position path, thereby obtaining a G3 continuous and smooth robot path.

2. A G3 continuous and smooth robot path smoothing method according to claim 1, characterized in that: The specific method of step one is as follows: 11) Describe the robot tool end position P = [x, y, z] in the workpiece coordinate system, and let the three consecutive position points in the original linear position path be P i-1 , P i , P i+1 At the corner formed by these three points, a 6th-order Bezier curve is used to construct a G3 continuous corner transition model. The curve equation is as follows: Where P(u) is the angle at the corner point P i The end position transfer curve inserted at B i are the control points of the inserted sixth-order Bezier curve P(u), the number of control points is 7; i is the control point number; is the Bezier curve basis function; u is the curve parameter; 12) Set conditions: a. The first three control points B0, B1, and B2 of the inserted curve are located at the end position path P i-1 P i On the segment, the last three control points B4, B5, and B6 are located at the end position path P i-1 P i section; Right now In the formula, B i is the control point of the constructed position Bezier curve P(u); where i is the control point number, and the length relationship between the control points can be obtained according to the above formula as follows: In the formula, l pa , l pb is the position curve transition parameter; to ensure symmetry, l pa = l pb = l p ; 13) Set position transition parameters l p Requirements: Where P i-1 , P i , P i+1 are three adjacent position points in the position path; θ p is the angle formed, ε pos is the position smoothing error tolerance.

3. A G3 continuous and smooth robot path smoothing method according to claim 1, characterized in that: The specific method of step 2 is as follows: 21) Describe the robot end posture φ=[α,β,γ] in the Euler angle space around the workpiece coordinate system. Let the three consecutive posture points in the original posture path be φ i-1 ,φ i ,φ i+1 , as with position smoothing, a 6th-order Bezier curve corner transition model is constructed at the corner, and the curve equation is as follows: Where φ(u) is the angle at the corner point φ i The terminal attitude transition curve inserted at j is the control point of the inserted sixth-order Bezier curve φ(u), j is the control point number, and the number of control points is 7; is the Bezier curve basis function; u is the curve parameter; 22) Set conditions: a. The first three control points Q0, Q1, Q2 of the inserted sixth degree Bezier curve should be located on the attitude linear segment φ i-1 φ i The last three control points Q4, Q5, and Q6 should be located on the attitude linear segment φ i φ i+1 superior; Right now In the formula, Q j is the control point of the constructed posture Bezier curve φ(u), where j is the control point number. According to the above formula, the length relationship between the control points is as follows: In the formula, l oa , l ob is the attitude spline curve transition parameter, the value is not necessarily the same, in order to obtain better smoothness, remember l ob and l oa The ratio is k; 23) Set the attitude spline transition parameters l oa , l ob Requirements: In the formula, φ i-1 ,φ i ,φ i+1 are three adjacent posture points in the posture path; θ o is the angle formed; ε ori is the end tool direction smoothing error tolerance; is the attitude transition parameter l ob and l oa The ratio of ||J o || is the Jacobian matrix between the end tool orientation and the Euler angle, that is: In the formula, s α 、s β 、s γ They are the abbreviations of sin(α), sin(β), and sin(γ); c α 、c β 、c γ They are the abbreviations of cos(α), cos(β), and cos(γ), respectively. α, β, and γ are the ZYX Euler angles of the end tool relative to the workpiece coordinate system (WCS).

4. A G3 continuous and smooth robot path smoothing method according to claim 1, characterized in that: The specific method of step three is as follows: 31) Set conditions: In the formula, s p is the arc length of the end position path, simplifying the above formula, the posture synchronization condition is: So the attitude spline transition parameter l ob and l oa Proportion 32) Taking into account the pose synchronization conditions, the geometric characteristics of adjacent pose paths, and the pose smoothing error constraints, the pose transition parameters are adaptively determined, and the following is obtained: 33) According to the position transition parameter l p and attitude transition parameter l oa , l ob , respectively determine the control points of the constructed position curve P(u) and attitude curve φ(u); In the formula, P i Point to P i-1 The unit vector of P i Point to P i+1 The unit vector of is φ i Pointing to φ i-1 The unit vector of is φ i Pointing to φ i+1 The unit vector of 34) After obtaining the control points, a G3 continuous, error-controllable and parameter-synchronized position and posture transition curve is generated according to the control points, thereby obtaining a smooth and continuous motion path that is conducive to the smooth movement of the industrial robot.

Images