A G3 continuous and smooth robot path smoothing method

Abstract

This invention provides a method for smoothing robot paths with continuous G3 motion. First, the robot end-effector position path is smoothed in the workpiece coordinate system by constructing a sixth-order Bezier curve corner transition model to achieve a smooth G3 transition at the path junction. Second, the robot end-effector posture path is smoothed in the Euler angle space around the workpiece coordinate system by constructing a sixth-order Bezier curve corner transition model to achieve a smooth G3 transition at the path junction. Finally, the robot end-effector position and posture paths after smoothing are processed for parameter synchronization. By considering the pose synchronization conditions and the geometric characteristics of the pose path, the position and posture transition parameters are adaptively adjusted to ensure the continuity of the first, second, and third derivatives of the end-effector posture relative to the arc length of the position path. This invention achieves a smooth G3 transition of the robot end-effector posture without any iterative calculations, and while the smoothing error is controllable, the motion of the robot end-effector tool axis is more stable.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm path smoothing technology, specifically a G3 continuous and smooth robot path smoothing method. Background Technology

[0002] In recent years, robots have gradually replaced multi-axis machine tools for machining curved and curved surface parts due to their advantages such as low cost, high flexibility, and large workspace. Unlike point-to-point motion in applications such as handling and palletizing, this type of machining requires precise and smooth contour movements from the robot's end effector. Since the machining path is usually composed of multiple linear paths, discontinuities in the tangential and normal directions at corners can cause abrupt changes in the robot's end effector motion, severely affecting machining efficiency and smoothness. Therefore, corner smoothing is required for the linear paths of six-axis robots.

[0003] The challenge of smoothing linear paths in robots lies in achieving high-order smooth transitions between end-effector position and orientation under smoothing error constraints, and in synchronizing the parameters of the end-effector position and orientation paths. Currently, most robot smoothing algorithms can achieve G2 continuity (curvature continuity) in the motion path, but due to the robot's poor stiffness, this continuity still struggles to ensure the smoothness of the end-effector's motion. Furthermore, to achieve parameter synchronization between the end-effector position and orientation paths, existing smoothing algorithms often re-parameterize the remaining path; however, this ignores the geometric characteristics of the position and orientation paths, leading to significant fluctuations in the tool axis angular velocity, angular acceleration, and angular jump at the path transition points. Therefore, a robot path smoothing method is urgently needed that can achieve high-order smooth transitions in the motion path while ensuring good angular motion performance of the end-effector. Summary of the Invention

[0004] To address the aforementioned issues, this invention provides a method for smoothing robot paths with continuous G3 curvature and stable motion. This method not only analytically achieves continuous G3 (curvature differential continuity) motion paths but also ensures smoother angular motion in the tool direction.

[0005] The technical solution of this invention is described below in conjunction with the accompanying drawings:

[0006] A method for smoothing a robot path with continuous and stable motion using G3, comprising the following steps:

[0007] Step 1: Smooth the robot end-effector position path in the workpiece coordinate system. By constructing a 6th Bezier curve corner transition model, the position G3 at the path connection point is continuously and smoothly transitioned. The position transition parameters meet the position smoothing error tolerance and adjacent path length constraints.

[0008] Step 2: Smooth the robot end effector's attitude path in the Euler angle space around the workpiece coordinate system. By constructing a 6th Bezier curve corner transition model, the attitude G3 at the path connection point is continuously and smoothly transitioned. The attitude 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 attitude path after smoothing. By considering the pose synchronization conditions and the geometric characteristics of the pose path, adaptively adjust the position and attitude transition parameters to ensure that the first, second, and third derivatives of the end pose relative to the arc length of the position path are continuous, thereby obtaining a robot path with continuous G3 and smooth motion.

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

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

[0012]

[0013] In the formula, P(u) represents the value at the corner point P. i The transition curve at the end of the insertion point; B i The control points for the inserted 6th-order Bezier curve P(u) are 7 in number; i is the control point number. Here are the basis functions for the Bezier curve; u is the curve parameter.

[0014] 12) Setting 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 of path P. i-1 P i section;

[0016] b、 Right now

[0017]

[0018] In the formula, B i Let i be the control points for constructing the position Bezier curve P(u); where i is the control point number. Based on the above formula, the length relationship between the control points can be obtained as follows:

[0019]

[0020] In the formula, l pa l pb For the position curve transition parameters; to ensure symmetry, make l pa =l pb =l p ;

[0021] 13) Set the position transition parameter l p Conditions met:

[0022]

[0023] In the formula, P i-1 P i P i+1 θ represents three consecutively adjacent points along the path. p For the included angle, ε pos This is the tolerance for positional smoothness error.

[0024] Furthermore, the specific method for step two is as follows:

[0025] 21) Describe the robot's end effector attitude φ = [α, β, γ] in the Euler angle space around the workpiece coordinate system. Let φ be the three sequentially connected attitude points in the original attitude path. i-1 φ i φ i+1 Similar to positional smoothing, a 6th-order Bezier curve corner transition model is constructed at the corner, and its curve equation is as follows:

[0026]

[0027] In the formula, φ(u) represents the value at the corner point φ i The end attitude transition curve inserted at the location; Q j Here are the control points for the inserted 6th-order Bezier curve φ(u), where j is the control point number, and the number of control points is 7. Here are the basis functions for the Bezier curve; u is the curve parameter.

[0028] 22) Setting conditions:

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

[0030] b、 Right now

[0031]

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

[0033]

[0034] In the formula, l oa l ob The values ​​of the transition parameters for the attitude spline curves are not necessarily the same. To obtain better smoothness, let l be a parameter. ob and l oa The ratio is k;

[0035] 23) Set the attitude spline curve transition parameters l oa l ob Conditions met:

[0036]

[0037] In the formula, φ i-1 φ i φ i+1 θ represents three consecutively adjacent attitude points in the attitude path. o The included angle formed by ε; ori For end-tool direction smoothing error tolerance; For attitude transition parameters l ob and l oa The ratio; ||J o || is the Jacobian matrix between the end-tool direction and the Euler angles, i.e.:

[0038]

[0039] Where s α s β s γ These are abbreviations for sin(α), sin(β), and sin(γ), respectively; c α c β c γ These are abbreviations for cos(α), cos(β), and cos(γ), respectively, where α, β, and γ are ZYX type Euler angles of the end tool relative to the workpiece coordinate system (WCS).

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

[0041] 31) Setting conditions:

[0042] a,

[0043] Where s p Let the path arc length at the end position be denoted as . Simplifying the above equation, we obtain the pose synchronization condition as follows:

[0044]

[0045] Therefore, the attitude spline curve transition parameter l ob and l oa Proportion

[0046] b、

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

[0048]

[0049] 33) Based on the position transition parameter l p and attitude transition parameters l oa l ob The control points for the constructed position curve P(u) and attitude curve φ(u) are determined respectively.

[0050]

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

[0052] 34) After obtaining the control points, a position and attitude transition curve that is continuous, error controllable and parameter synchronized is generated based on 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 this invention are as follows:

[0054] 1) This invention adopts a local smoothing technology solution. By constructing a local smoothing model of a 6th order Bezier curve at the line segment connection, the pose path G3 can be continuously and smoothly transitioned. Moreover, 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 the robot's fast and stable processing.

[0055] 2) By considering parameter synchronization conditions and adjacent path geometric constraints, the present invention can adaptively adjust the pose transition parameters, thereby analytically achieving pose path parameter synchronization and overcoming the problem of severe fluctuations in the end-effector axis angular motion, which further improves the stability of robot machining. Attached Figure Description

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

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

[0058] Figure 2 This is a smooth path diagram for the robot's end effector position.

[0059] Figure 3 This is a smooth path diagram of the robot's end effector.

[0060] Figure 4 This is a diagram showing the end-effector pose synchronization.

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

[0062] Figure 6 This is a comparison diagram of joint acceleration between the present invention and existing methods;

[0063] Figure 7 This is a comparison diagram of the joint jump between the present invention and existing methods. Detailed Implementation

[0064] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and not intended to limit it. Furthermore, it should be noted that, for ease of description, the accompanying drawings show only the parts relevant to the present invention, and not all of the structures.

[0065] Example 1

[0066] like Figure 1As shown, this embodiment provides a method for smoothing a robot path with continuous and stable motion using G3, including the following steps:

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

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

[0069]

[0070] In the formula, P(u) represents the value at the corner point P. i The transition curve at the end of the insertion point; B i The control points for the inserted 6th-order Bezier curve P(u) are 7 in number; i is the control point number. Here are the basis functions for the Bezier curve; u is the curve parameter.

[0071] 12) Setting conditions:

[0072] a. To ensure the tangential continuity of P(u) and the remaining straight line segment, 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 of path P. i-1 P i section;

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

[0074] Right now

[0075]

[0076] In the formula, B i Let i be the control points for constructing the position Bezier curve P(u); where i is the control point number. Based on the above formula, the length relationship between the control points can be obtained as follows:

[0077]

[0078] In the formula, l pa l pb For the position curve transition parameters; to ensure symmetry, make l pa =l pb =l p ;

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

[0080]

[0081] In the formula, such as Figure 2 As shown, P i-1 P i P i+1 θ represents three consecutively adjacent points along the path. p For the included angle, ε pos This is the tolerance for positional smoothness error.

[0082] Step 2: Smooth the robot end effector's attitude path in the Euler angle space around the workpiece coordinate system (WCS). By constructing a 6th-order Bezier curve corner transition model, a smooth G3 attitude transition at the path junction is achieved. The attitude transition parameters satisfy the direction smoothing error tolerance and adjacent path length constraints. The specific method is as follows:

[0083] 21) Describe the robot end effector pose φ = [α, β, γ] in the Euler angle space about the workpiece coordinate system (WCS), such as Figure 3 As shown, let φ be the three consecutively connected attitude points in the original attitude path. i-1 φ i φ i+1 Similar to positional smoothing, a 6th-order Bezier curve corner transition model is constructed at the corner, and its curve equation is as follows:

[0084]

[0085] In the formula, φ(u) represents the value at the corner point φ i The end attitude transition curve inserted at the location; Q j Here are the control points for the inserted 6th-order Bezier curve φ(u), where j is the control point number, and the number of control points is 7. Here are the basis functions for the Bezier curve; u is the curve parameter.

[0086] 22) Setting conditions:

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

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

[0089] Right now

[0090]

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

[0092]

[0093] In the formula, l oa l ob The values ​​of the transition parameters for the attitude spline curves are not necessarily the same. To obtain better smoothness, let l be a parameter. ob and l oa The ratio is k;

[0094] 23) To avoid overlapping and intersecting of the constructed attitude transition curves and to ensure that the smoothing error is within the smoothing error tolerance in the end-tool direction, the attitude spline curve transition parameter l oa l ob Conditions met:

[0095]

[0096] In the formula, such as Figure 3 As shown, φ i-1 φ i φ i+1 θ represents three consecutively adjacent attitude points in the attitude path. o The included angle formed by ε; ori For end-tool direction smoothing error tolerance; For attitude transition parameters l ob and l oa The ratio; ||J o || is the Jacobian matrix between the end-tool direction and the Euler angles, i.e.:

[0097]

[0098] Where s α sβ s γ These are abbreviations for sin(α), sin(β), and sin(γ), respectively; c α c β c γ These are abbreviations for cos(α), cos(β), and cos(γ), respectively, where α, β, and γ are ZYX type Euler angles of the end tool relative to the workpiece coordinate system (WCS).

[0099] Step 3: Perform parameter synchronization processing on the robot's end effector position and attitude path after smoothing. By considering the pose synchronization conditions and the geometric characteristics of the pose path, adaptively adjust the position and attitude transition parameters to ensure the continuity of the first, second, and third derivatives of the end effector attitude relative to the arc length of the position path. This results in a G3 continuous and smooth robot path. The final path is high-order continuous and parameter synchronized. Furthermore, due to the adaptive adjustment of the pose transition parameters, it ensures more stable robot motion, as detailed below:

[0100] 31) Setting conditions:

[0101] a. such as Figure 4 As shown, to achieve parameter synchronization, the position and attitude paths must share the same curve parameters, and the first, second, and third derivatives of the end-effector attitude relative to the arc length of the position path must be continuous at the cross-segment points, i.e., the following must be satisfied:

[0102]

[0103] Where s p Let the path arc length at the end position be denoted as . Simplifying the above equation, we obtain the pose synchronization condition as follows:

[0104]

[0105] Therefore, the attitude spline curve transition parameter l ob and l oa Proportion

[0106] b. Considering the above pose synchronization conditions, in order to ensure that the end-effector orientation error is within the constraint range, its position path transition parameter l p Also required:

[0107]

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

[0109]

[0110] 33) Based on the position transition parameter l p and attitude transition parameters l oa l ob The control points for the constructed position curve P(u) and attitude curve φ(u) are determined respectively.

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

[0112]

[0113] 34) After obtaining the control points, a position and attitude transition curve that is continuous, error controllable and parameter synchronized is generated based on 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 robot path smoothing method proposed in this invention, which achieves smooth transition of robot end-stage pose with G3 continuity (curvature differential continuity) analytically without any iterative calculations, and with controllable smoothing error, makes the robot end-stage tool axis angular motion more stable.

[0115] Example 2

[0116] To verify the effectiveness and superiority of the method proposed in this invention, an existing robot path smoothing method was selected and compared with this invention under the same speed planning.

[0117] Depend on Figure 5 As can be seen, in terms of end-effector trajectory, the robot path smoothing method with continuous and stable motion proposed in this invention has smaller fluctuations in the angular velocity of the end-effector and continuous motion, which shows that the method proposed in this paper can make the rotation of the robot end-effector smoother.

[0118] Depend on Figure 6 As can be seen, in terms of joint acceleration, the G3 continuous and smooth robot path smoothing method proposed in this invention has continuous joint acceleration without abrupt changes and with smaller fluctuation amplitude, which indirectly shows that the method in this paper can improve the motion stability of the robot.

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

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

Claims

1. A method for smoothing a robot path with continuous and stable motion using G3, characterized in that, Includes the following steps: Step 1: Smooth the robot end-effector position path in the workpiece coordinate system. By constructing a 6th Bezier curve corner transition model, the position G3 at the path connection point is continuously and smoothly transitioned. The position transition parameters meet the position smoothing error tolerance and adjacent path length constraints. Step 2: Smooth the robot end effector's attitude path in the Euler angle space around the workpiece coordinate system. By constructing a 6th Bezier curve corner transition model, the attitude G3 at the path connection point is continuously and smoothly transitioned. The attitude 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 attitude path after smoothing. By considering the pose synchronization conditions and the geometric characteristics of the pose path, adaptively adjust the position and attitude transition parameters to ensure that the first, second, and third derivatives of the end pose relative to the arc length of the position path are continuous, thereby obtaining a G3 continuous and smooth robot path. The specific method for step one is as follows: 11) Describe the position of the robot tool end effector P = [x, y, z] in the workpiece coordinate system. Let P be the three consecutively connected points in the original linear position path. i-1 P i P i+1 At the corner formed by these three points, a G3 continuous corner transition model is constructed using a 6th-order Bezier curve, and the curve equation is as follows: In the formula, P(u) represents the value at the corner point P. i The transition curve at the end of the insertion point; B i The control points for the inserted 6th-order Bezier curve P(u) are 7 in number; i is the control point number. Here are the basis functions for the Bezier curve; u is the curve parameter. 12) Setting 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 of path P. i P i+1 section; b、 Right now In the formula, B i Let i be the control points for constructing the position Bezier curve P(u); where i is the control point number. Based on the above formula, the length relationship between the control points can be obtained as follows: In the formula, l pa l pb For the position curve transition parameters; to ensure symmetry, make l pa =l pb =l p ; 13) Set the position transition parameter l p Conditions met: In the formula, P i-1 P i P i+1 θ represents three consecutively adjacent points along the path. p ε is the included angle formed by the angle. pos This is the tolerance for positional smoothness error.

2. The method for smoothing a robot path with continuous and stable motion according to claim 1, characterized in that, The specific method for step two is as follows: 21) Describe the robot's end effector attitude φ = [α, β, γ] in the Euler angle space around the workpiece coordinate system. Let φ be the three sequentially connected attitude points in the original attitude path. i-1 φ i φ i+1 Similar to positional smoothing, a 6th-order Bezier curve corner transition model is constructed at the corner, and its curve equation is as follows: In the formula, φ(u) represents the value at the corner point φ i The end attitude transition curve inserted at the location; Q j Here are the control points for the inserted 6th-order Bezier curve φ(u), where j is the control point number and there are 7 control points. Here are the basis functions for the Bezier curve; u is the curve parameter. 22) Setting conditions: a. The first three control points Q0, Q1, and Q2 of the Bezier curve inserted six times should be located on the attitude linear segment φ. i-1 φ i Above, the last three control points Q4, Q5, and Q6 should be located within the attitude linear segment φ. i φ i+1 superior; b、 Right now In the formula, Q j Let j be the control points of the constructed attitude Bezier curve φ(u), where j is the control point number. According to the above formula, the length relationship between the control points can be obtained as follows: In the formula, l oa l ob For the attitude spline curve transition parameters, l ob and l oa The ratio is k; 23) Set the attitude spline curve transition parameters l oa l ob Conditions met: In the formula, φ i-1 φ i φ i+1 θ represents three consecutively adjacent attitude points in the attitude path. o The included angle formed by ε; ori For end-tool direction smoothing error tolerance; For attitude transition parameters l ob and l oa The ratio; ||J o || is the Jacobian matrix between the end-tool direction and the Euler angles, i.e.: In the formula, s α s β s γ These are abbreviations for sin(α), sin(β), and sin(γ), respectively; c α c β c γ These are abbreviations for cos(α), cos(β), and cos(γ), respectively, where α, β, and γ are ZYX type Euler angles of the end tool relative to the workpiece coordinate system (WCS).

3. The method for smoothing a robot path with continuous and stable motion according to claim 1, characterized in that, The specific method for step three is as follows: 31) Setting conditions: a、 In the formula, s p Let the path arc length at the end position be denoted as . Simplifying the above equation, we obtain the pose synchronization condition as follows: Therefore, the attitude spline curve transition parameter l ob and l oa Proportion b、 32) Taking into account the pose synchronization conditions, the geometric characteristics of adjacent pose paths, and the pose smoothness error constraints, the pose transition parameters are adaptively determined, resulting in: 33) Based on the position transition parameter l p and attitude transition parameters l oa l ob The control points for the constructed position curve P(u) and attitude curve φ(u) are determined respectively. 34) After obtaining the control points, a position and attitude transition curve that is continuous, error controllable and parameter synchronized is generated based on the control points, thereby obtaining a smooth and continuous motion path that is conducive to the smooth movement of the industrial robot.

Images