A robot motion trajectory smoothing method, system and storage medium

Abstract

The application relates to a robot motion trajectory smoothing method and system. The specific steps comprise the following steps: step S1: acquiring discrete path points of robot motion, and constructing a B-spline position path curve satisfying third-order derivative continuity; step S2: representing a robot end effector pose in the form of Euler angles and performing smoothing interpolation, and utilizing a Jacobian matrix to constrain pose error; step S3: iteratively optimizing position path curve curvature under the requirements of tolerance and continuity; step S4: determining a position and pose synchronization condition, and then calculating a pose path curve transition length and a control point parameter; step S5: optimizing a velocity curve and realizing non-abrupt transition; and step S6: converting discrete commands into joint angle instructions through inverse kinematics, and controlling the robot to move along the designed trajectory. The application is suitable for robot motion trajectory smoothing of complex manufacturing tasks, can solve the problems that a generated path curve does not pass through initial path points and exists velocity abruptness, and significantly improves robot operation precision and efficiency.

Description

Technical Field

[0001] This invention belongs to the field of robot control, specifically relating to a method, system, and storage medium for smoothing robot motion trajectories. Background Technology

[0002] Traditionally, robots have been primarily used for point-to-point object transport tasks. These tasks do not have specific requirements for the robot's motion trajectory; typically, the robot's joint angles are directly controlled in joint space. The planned objects are motion quantities such as joint variables, joint angular velocities, and joint angular accelerations. However, the resulting robot trajectories are not intuitive and contain errors. With the improvement of robot stiffness and precision, robots have begun to be widely used in more complex manufacturing tasks, such as milling, welding, and deburring. These tasks require the robot to move continuously and accurately along a curved profile defined in Cartesian space, while ensuring the continuity of velocity and acceleration in the robot's motion trajectory.

[0003] Trajectory planning in Cartesian space is more complex than in joint space, typically using a continuous linear path generated by discrete reference commands to approximate the original curve profile. However, due to tangential discontinuities at corners of the linear path, the robot actually traverses corners at speeds far lower than the commanded feed rate to ensure the joints do not exceed their kinematic limits, thus avoiding shocks and vibrations. To address this issue, the original linear path must be smoothed.

[0004] Based on application type, linear path smoothing methods can be divided into two categories: global smoothing methods and local smoothing methods. Global smoothing methods achieve overall smoothing of the linear path by fitting all discrete path points. This method provides high global continuity but struggles to accurately assess and constrain end effector position and attitude path errors. In contrast, local smoothing methods connect adjacent linear path segments by replacing local angles while preserving part of the initial linear path. This method not only achieves high-order continuity between the inserted transition segment and the original linear segment but also allows for precise control of path errors introduced by the smoothing process. However, the path curves generated by local smoothing methods typically do not pass through discrete path points, which may lead to information loss, and the robot's operating speed at transition curves is lower and exhibits abrupt changes. Summary of the Invention

[0005] The main objective of this invention is to overcome the shortcomings of the prior art and provide a method, system, and storage medium for smoothing robot motion trajectories. This invention constructs position and orientation path curves at local corners that can accurately pass through discrete path points and achieves synchronous control. This solves the problems of feature information loss due to path curves not passing through discrete path points and sudden speed changes at transition curves caused by existing technologies. It ensures that the robot runs accurately along the designed trajectory to meet the demands of high-precision and high-efficiency industrial applications, providing an effective solution for efficient robot applications in complex manufacturing tasks.

[0006] To achieve the above objectives, the technical solution provided by this invention is as follows:

[0007] A method for smoothing robot motion trajectories, characterized by comprising:

[0008] Step S1: Obtain the discrete path points of the robot's motion, determine the transition length of the position path curve, and generate a path that satisfies C by setting specific node vectors and control point construction rules. 3 A continuous and accurate B-spline position path curve passing through discrete path points; adjusting control points to constrain position path errors;

[0009] Step S2. Represent the robot end effector attitude in Euler angles and perform smooth interpolation within the rotation space formed by Euler angles; determine the transition length of the attitude path curve, and generate a B-spline attitude path curve using the same structure as the position path curve; estimate the attitude path error using the Jacobian matrix and rotation angle, and constrain it by adjusting the control points.

[0010] Step S3: Calculate the curvature of the position path curve, iteratively optimize the control point position and transition length to reduce the curvature, while meeting the tolerance and continuity requirements, and use least squares regression to fit the relationship between the position transition length and the control point at different angles;

[0011] Step S4: By deriving the relevant differential equations, determine the necessary and sufficient conditions for position and attitude synchronization, and then calculate the transition length and control points of the attitude path curve;

[0012] Step S5: Calculate the allowable speed for each transition curve segment based on the maximum speed, tangential acceleration, tangential jerk, and chord tolerance constraints; define the region where the allowable speed is less than the maximum speed as the speed-sensitive region, and set the speed in the speed-sensitive region to the minimum value of the region; achieve a seamless transition of speeds in different regions through an S-shaped acceleration / deceleration algorithm;

[0013] Step S6: Use parameter interpolation based on Taylor expansion and arc length compensation to obtain the smooth motion trajectory of the robot, and obtain the joint angle drive command through robot inverse kinematics calculation to finally control the robot to run along the designed trajectory.

[0014] Preferably, step S1 involves: obtaining discrete path points, Q, of the robot's motion in the workpiece coordinate system. i (i = 0, 1, ..., n) represent the positions of each discrete path point. Taking the discrete path point positions Q1, Q2, and Q3 of two adjacent positional paths as an example, the positional transition length l is determined. p satisfy

[0015]

[0016] A portion of the linear path is retained for synchronous adjustment of position and attitude; a B-spline position path curve C is constructed by setting specific node vectors and control points. p (u), such that it passes through every discrete path point and satisfies C. 3 Continuity requirement, control point P i (i = 0, 1, ..., 10) satisfies

[0017] 2||P0P1||=2||P9P 10 ||=||P1P2||=||P2P3||=||P7P8||=||P8P9||.

[0018] Analytical calculation of the maximum error ε of the location path p By adjusting the control points to ensure they are within the user-defined tolerance range, the formula for calculating the maximum error of the position path is:

[0019]

[0020] In the formula, d p Let P5P4 and P5P6 represent the lengths of vectors P5P4 and P5P6, and α be the angle between the initial position path segments.

[0021] Preferably, step S2 involves: representing the rotation matrix R corresponding to each path point as Euler angles B = [α, β, γ] around the three axes of the fixed workpiece coordinate system. T Form, each Euler angle coordinate B i The initial attitude path segment (i = 0, 1, ..., n) is formed. Smooth interpolation is performed within the rotation space defined by Euler angles, and the attitude path curve C is generated using the same structure as the position path curve. 。 (u); The attitude error of the end effector is estimated using the Jacobian matrix J and the rotational spatial coordinates, and the range of the attitude error of the end effector is constrained by adjusting the control points. The formula for calculating the Jacobian matrix J is as follows:

[0022]

[0023] In the formula, S α S β S γC ɑ C β and C γ They represent sinα, sinβ, sinγ, cosα, cosβ, and cosγ, respectively.

[0024] Preferably, step S3 involves calculating the first and second derivatives of the position path curve C(u) to obtain the curvature.

[0025]

[0026] Iterative optimization of control points P on the location path curve i With transition length l p To reduce curvature while meeting tolerances and C 3 Continuity requirement: Control points P of the curve at different angles are fitted using least squares regression. i With transition length l p The relationship between them is

[0027]

[0028] Preferably, step S4 involves: deriving the first, second, and third order differential equations of the end effector's attitude relative to displacement to determine the necessary and sufficient conditions for synchronization between position and attitude paths; and, in conjunction with position path-related parameters, ensuring that the transition length of the attitude path curve satisfies...

[0029]

[0030] In addition, to comply with attitude tolerance constraints, the attitude path curve should also satisfy...

[0031]

[0032] In the formula, d o β represents the lengths of vectors O5O4 and O5O6, and β is the angle between the initial attitude path segments.

[0033] Preferably, step S5 involves: calculating the allowable velocity v based on kinematic and geometric constraints, comprehensively considering the maximum velocity, tangential acceleration, tangential jerk, and chord tolerance constraints of each spline curve segment. lim (u), and defines the region where the speed is allowed to be lower than a set threshold as the speed-sensitive region, and sets the speed in this region as the minimum value of the region. The speed transition between different regions is achieved without abrupt changes through an S-shaped acceleration and deceleration algorithm.

[0034] Preferably, step S6 employs: obtaining a smooth motion trajectory for the robot based on parameter interpolation using Taylor expansion and arc length compensation to reduce speed errors caused by trajectory errors; and combining inverse kinematics to calculate discrete joint angle drive commands to ultimately control the robot to run along the planned trajectory.

[0035] A robot motion trajectory smoothing system, characterized in that it comprises:

[0036] The path smoothing module is configured to fit discrete path points with B-spline curves, adjust the position error of control points, and perform smooth interpolation in the rotation space to achieve synchronization of position and attitude path.

[0037] The speed planning module is configured to calculate the allowable speed and smoothly transition it using an S-shaped acceleration / deceleration algorithm.

[0038] The parameter interpolation module is configured to obtain the robot's smooth motion trajectory by using parameter interpolation with Taylor expansion and arc length compensation, and to calculate joint angle drive commands by combining inverse kinematics.

[0039] A computer-readable storage medium storing a computer program thereon, characterized in that, when the computer program is executed by a processor, it is capable of implementing any of the above-described method steps and calling the functional modules of any of the above-described systems.

[0040] Compared with the prior art, the present invention has the following beneficial effects:

[0041] 1. The robot motion trajectory smoothing method designed in this invention can achieve global C-axis smoothing of the position path. 3 Synchronous control of position and attitude of continuous and end effector can be achieved by analytically calculating the inserted local B-spline curves without any iterative calculations.

[0042] 2. This invention enables smooth curves to pass through initial discrete path points based on tolerance constraints and curvature optimization;

[0043] 3. This invention can effectively reduce the curvature of the designed path curve, increase the robot's running speed, and reduce speed fluctuations. Attached Figure Description

[0044] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings of the embodiments will be briefly described below.

[0045] Figure 1 This is a schematic diagram of a robot motion trajectory smoothing method provided in an embodiment of the present invention;

[0046] Figure 2 A location path curve structure diagram provided for embodiments of the present invention;

[0047] Figure 3 This is a diagram showing the attitude path curve structure provided in an embodiment of the present invention.

[0048] Figure 4 The robot experimental platform shown in the embodiment of the present invention;

[0049] Figure 5 This is an example of the robot's initial path diagram shown in an embodiment of the present invention;

[0050] Figure 6 This is a schematic diagram of the position path curve shown in an embodiment of the present invention;

[0051] Figure 7 This is a schematic diagram of the attitude path curve shown in an embodiment of the present invention;

[0052] Figure 8 This is a schematic diagram showing the comparison of tracking errors of various joints of a robot according to an embodiment of the present invention. Specific implementation methods

[0053] The following describes the implementation of the present invention through specific embodiments and in conjunction with the accompanying drawings.

[0054] To address the problem of feature information loss caused by path curves not passing through discrete path points in existing technologies, this invention provides a method for smoothing robot motion trajectories. This method constructs position and attitude path curves that can accurately pass through discrete path points and achieve synchronization, enabling the robot to run precisely along the designed trajectory.

[0055] Figure 1 This is a schematic diagram of the robot motion trajectory smoothing method provided in an embodiment of the present invention, which will be described in detail below.

[0056] The robot motion trajectory smoothing method provided by the present invention includes:

[0057] In the workpiece coordinate system, obtain the discrete path points of the robot's motion, Q. i (i = 0, 1, ..., n) represent the positions of each discrete path point. Taking the discrete path point positions Q1, Q2, and Q3 of two adjacent positional paths as an example, the positional transition length l is determined. p satisfy

[0058]

[0059] A quintic B-spline curve C used for smoothing position paths p (u) can be expressed by the following formula.

[0060]

[0061] In the formula, k = 5 is the order of the B-spline; P i (i = 0, 1, ..., n) represents the control points; the node vector of the B-spline is set to U = [0, 0, 0, 0, 0, 0, 0.25, 0.5, 0.5, 0.5, 0.75, 1, 1, 1, 1, 1, 1], and the repetition at u = 0 and u = 1 is set to 6 to ensure that the curve passes through P0 and P1. 10N i，k For B-spline basis functions, the following recursive equation can be used to evaluate them.

[0062]

[0063] To ensure that C at the intersection of the linear path and the spline curve 3 Continuity, meaning that the following equations should hold at u = 0 and u = 1.

[0064]

[0065] In the formula, ω = 2, 3 represent the second and third derivatives, respectively, and C... 1 Continuity has been achieved by the location of the control points, and the equations are further calculated as follows:

[0066]

[0067] Set vectors P0P1 and P9P1. 10 The lengths are 0.5l pa 0.5l pb Due to the symmetry of the initially generated transition curve, to ensure the C at the intersection of the linear path and the B-spline curve... 3 The continuity and the relationship between control points can be represented as follows:

[0068] 2||P0P1||=2||P9P 10 ||=||P1P2||=||P2P3||=||P7P8||=||P8P9||=l p

[0069] Figure 2 This is a location path curve structure diagram provided in an embodiment of the present invention, P0~P 10 These are the control points of the B-spline curve in the figure. The formula for calculating the control points is:

[0070]

[0071] In the formula, l pa =l pb =l p ;c is the pre-set smoothing length ratio, set to c=5;e1~e4 are the unit vectors of vectors P5P4, P5P6, P5P3 and P5P7, respectively;d p This represents the lengths of vectors P5P4 and P5P6; vectors P5P4 and P5P6 have opposite directions and equal lengths to ensure that the curve passes through the discrete path point Q2.

[0072] Due to the symmetry of the position path curve, only the maximum position path error ε in the interval (0.5, 1] ​​is calculated. p Let B-spline function Cp (u) Establish the equation with the derivative component perpendicular to Q2Q3 being zero, i.e., C p,y We obtain u'(u) = 0 and solve for u = 0.6327. We then further calculate the maximum position path error ε. p for

[0073]

[0074] The rotation matrix R corresponding to each path point is represented by Euler angles B = [α, β, γ], which represent the rotations around the XYZ axes of the fixed workpiece coordinate system. T Form, each Euler angle coordinate B i The initial attitude path segment (i = 0, 1, ..., n) is formed and smooth interpolation is performed within the rotation space defined by Euler angles. The transition length of the attitude path curve is determined, and it is constructed using the same structure as the position path. Spline curves are inserted at the angle between every two initial path points to achieve the C-axis of the end effector attitude. 3 Continuous motion. Figure 3 The attitude path curve structure diagram provided in the embodiment of the present invention uses a B-spline curve C. o The expression (u) is

[0075]

[0076] In the formula, O i (i = 0, 1…n) represents the control points; similarly, the degree of the attitude path curve is set to k = 5.

[0077] When the attitude path error is small, it can be determined by the Jacobian matrix of the end effector attitude and the rotation space coordinates O = [α, β, γ]. T To approximate

[0078] ΔPO≈JΔO

[0079] In the formula, J is the Jacobian matrix of the end effector attitude, and its calculation formula is:

[0080]

[0081] The attitude path error ΔPO is defined as the shortest deviation between the initial attitude path and the constructed smooth spline curve, and therefore the following relationship exists.

[0082] ΔPO≤min{||JΔO||| u＝0.3673 ,||JΔO||| u＝0.6327}

[0083] Since the position path curve and attitude path curve have similar structures, the formula for the maximum error of the position path can also be used for the attitude path curve. The above formula can be further simplified to...

[0084] ΔPO≤min{0.518cos(β / 2)d o J| u＝0.3673 e9, 0.518cos(β / 2)d o J| u＝0.6327 e 10}

[0085] In the formula, e9, e 10 It is a unit vector perpendicular to B1B2 and B2B3.

[0086] Furthermore, because the attitude path error should satisfy ΔPO≤2sin(ε) ow / 2), therefore, the sufficient condition for satisfying the attitude tolerance constraint is

[0087]

[0088] In d p ≤d pw Under the premise that the transition length l is known, p In this case, an iterative optimization approach is used to reduce the curvature of the position path curve. Where d p The initial value is set to 0, and then iteratively increased as the program loops until the curvature no longer decreases. The formula for calculating the curvature is as follows:

[0089]

[0090] The final calculated d p With l p The correspondence can be viewed as a function of angle α as the independent variable, and the least squares regression method can be used for fitting.

[0091]

[0092] To achieve synchronization between the position and attitude of the end effector, the first, second, and third derivatives of the end effector's attitude with respect to the displacement s should satisfy the following at the start and end points of the transition curve:

[0093]

[0094] Combining the control polygon structure of the position path and attitude path described above, the necessary and sufficient condition for the third-order continuity of the end effector's attitude with respect to the displacement s is:

[0095]

[0096] Furthermore, the attitude path curve under attitude tolerance constraints satisfies

[0097]

[0098] To ensure the robot's velocity remains continuous without abrupt changes after interpolation planning, the allowable velocity is calculated based on kinematic and geometric constraints, taking into account the maximum velocity, tangential acceleration, tangential jerk, and chord tolerance limits for each spline curve segment.

[0099]

[0100] In the formula, F, ε, a max and j max These are defined as maximum velocity, position path tolerance, maximum tangential acceleration, and maximum tangential jerk, respectively, with T being the interpolation period.

[0101] The region where the speed is allowed to be lower than the set maximum speed is defined as the sensitive region, and the speed in the region is set to the minimum value within the region. By employing an S-shaped acceleration / deceleration algorithm, a smooth transition in speed between different regions is achieved.

[0102] The robot's smooth motion trajectory is obtained using parameter interpolation based on Taylor expansion and arc length compensation. Joint angle drive commands are then derived through inverse kinematics calculations, ultimately controlling the robot to move along the designed trajectory. The interpolation parameter calculation formula is as follows:

[0103]

[0104] In the formula, s k Indicates at t k The total arc length at time u k Indicates t k Interpolation parameters at time points.

[0105] To verify the effectiveness of the robot motion trajectory smoothing method provided by this invention, a UR10 robot with 6 rotary joints (6R) is used as an example. Figure 4 The diagram shows a robot experimental platform according to an embodiment of the present invention. The host computer receives motion data in real time at a maximum frequency of 500Hz, collecting the robot's joint positions and velocities. The sampling period and interpolation period are set to 0.002s, and the position and orientation path tolerances are set to 0.3mm and 0.005rad, respectively. During velocity scheduling, the maximum velocity, tangential acceleration, and tangential jerk are set to 25mm / s and 2000mm / s, respectively. 2 and 20000mm / s 3 .

[0106] Figure 5 The robot initial path diagram shown in this embodiment of the invention is a typical original path that includes multiple angles and end effector poses, which has been smoothed.

[0107] Figure 6 This is a schematic diagram of the position path curve shown in an embodiment of the present invention. Figure 7The diagram shows the attitude path curve in an embodiment of the present invention. The method proposed in this invention and the incisional smoothing method are used to smooth the initial path, respectively. By comparison, it can be verified that the method of the present invention can achieve the advantage of using the initial discrete path points on the basis of tolerance constraints and curvature optimization.

[0108] Figure 8 The maximum tracking error (difference between target and actual values) of each joint was compared between the method of this invention and existing incisional smoothing methods. The method of this invention outperforms existing incisional smoothing methods in terms of robot running accuracy and can significantly reduce the maximum tracking error of robot operation.

[0109] The present invention has been described above with reference to preferred embodiments; however, these embodiments are merely exemplary and illustrative. Various substitutions and modifications can be made to the present invention based on these embodiments, all of which fall within the scope of protection of the present invention.

Claims

1. A robot motion trajectory smoothing method, characterized by, Comprising: Step S1: In the workpiece coordinate system, obtain discrete path points of robot motion, Q i The position transition length l is determined by taking the discrete path points Q1, Q2 and Q3 of the adjacent two position path segments as examples p satisfies Reserve part of the linear path for the simultaneous adjustment of the end effector position and orientation; construct the B-spline position path curve C by setting specific node vectors and control points p (u) that passes through each discrete path point and satisfies C 3 continuity requirements, control points P i (i = 0, 1,..., 10) satisfy 2 || P0 P1 || = 2 || P9 P 10 || = || P1 P2 || = || P2 P3 || = || P7 P8 || = || P8 P9 || Analytical calculation of position path maximum error ε p By adjusting the control points to ensure that they are within the user-defined tolerance range, the formula for calculating the position path maximum error is where d p denotes the length of the vectors P5P4 and P5P6, and a is the initial position path segment angle; Step S2: the position path point corresponding rotation matrix R is expressed as Euler angle B = [α, β, γ] rotating around the fixed workpiece coordinate system three-axis T Form, each Euler angle coordinate B i (i = 0, 1,..., n) constitutes an initial pose path segment, and the pose path curve C is generated by smooth interpolation in the rotation space composed of Euler angles and the same structure as the position path curve o (u); the pose path error is estimated by using the Jacobian matrix J and the rotation space coordinates, and the constraint is performed by adjusting the control points, and the calculation formula of the Jacobian matrix J is where S α , S β , S γ , C α , C β and C γ represent sin α, sin β, sin γ, cos α, cos β and cos γ, respectively. Step S3: Calculate the first and second derivatives of the position path curve C(u) to obtain the curvature Iterative optimization of position path curve control points P i With transition length l p To reduce curvature and meet tolerance and C 3 Continuity requirements, by least squares regression fitting of curve control points P i With transition length l p The relationship between Step S4: Derive the first, second and third order differential equations of the end effector pose with respect to displacement, determine the necessary and sufficient conditions for position and pose path synchronization; combined with the relevant parameters of the position path, the transition length of the pose path curve satisfies In addition, in order to follow the pose tolerance constraint, the pose path curve should also satisfy where d o denotes the length of the vectors O5O4and O5O6, and β is the initial pose path segment angle; Step S5: Calculate the allowable velocity of each transition curve according to the maximum velocity, tangential acceleration, tangential jerk and chord tolerance constraints; define the region where the allowable velocity is less than the set maximum velocity as the velocity sensitive region, and set the velocity sensitive region velocity as the minimum value of the region; the velocities of different regions are realized without abrupt transition through S-shaped acceleration and deceleration algorithm; Step S6: Use parameter interpolation based on Taylor expansion and arc length compensation to obtain the smooth motion trajectory of the robot, and calculate the joint angle driving command through the inverse kinematics of the robot to finally control the robot to run along the designed trajectory.

2. The robot motion trajectory smoothing method of claim 1, wherein, The step S5 adopts: based on kinematics and geometric constraints, comprehensively considering the maximum speed, tangential acceleration, tangential jerk and chord tolerance constraints of each spline curve, calculating the allowable speed v lim (u), and defining the region with allowable speed lower than the set threshold as a speed sensitive region, setting the speed of the region as the region minimum value; and realizing the speed non-sudden transition between different regions through S-shaped acceleration and deceleration algorithm.

3. The robot motion trajectory smoothing method of claim 1, wherein, The step S6 adopts: parameter interpolation based on Taylor expansion and arc length compensation to obtain the smooth motion trajectory of the robot, so as to reduce the velocity error caused by trajectory error; combined with inverse kinematics, the discrete joint angle driving command is calculated, and finally the robot is controlled to run along the planned trajectory.

4. A robot motion trajectory smoothing system for implementing the robot motion trajectory smoothing method as claimed in claims 1 to 3, characterized by, Comprising: A path smoothing module configured to fit the discrete path points with B-spline curves, represent the pose of the robot end effector in the form of Euler angles, and perform smooth interpolation in the rotation space to realize the synchronization of position and pose paths; A velocity planning module configured to calculate the allowable velocity and smoothly transition through S-shaped acceleration and deceleration algorithm; A parameter interpolation module configured to use parameter interpolation based on Taylor expansion and arc length compensation to obtain the smooth motion trajectory of the robot, and calculate the joint angle driving command combined with inverse kinematics.

5. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program, when executed by the processor, can implement any method steps in claims 1-3, and call any functional modules of the system in claim 4.

Images