A method and apparatus for transition of a robot cartesian trajectory to a joint trajectory

Abstract

A robot Cartesian trajectory and joint trajectory transition method and device, the method comprising: obtaining the starting point Q s and the ending point Q m of an initial Cartesian trajectory respectively corresponding to joint angle vectors θ1 and θ2, and the ending point Q e of an initial joint trajectory corresponding to a joint angle vector θ3, determining the starting point Q1 and the ending point Q2 of a transition trajectory based on θ1, θ2 and θ3; determining a plurality of control vertices between Q1 and Q2, and performing cubic non-uniform B-spline interpolation based on Q1 and Q2 and the plurality of control vertices to obtain the transition trajectory Q1Q2; planning an updated Cartesian trajectory between Q s and Q1; planning an updated joint trajectory between Q2 and Q e ; and concatenating the updated Cartesian trajectory, the transition trajectory Q1Q2 and the updated joint trajectory in sequence to obtain an optimized trajectory Q s Q e , which can achieve smooth transition of robot Cartesian trajectory and joint trajectory, ensure continuous velocity and acceleration of the trajectory while avoiding the phenomenon of whiplash and overshoot.

Description

Technical Field

[0001] This article relates to the field of robot trajectory planning, and in particular to a method and device for transitioning between Cartesian trajectories and joint trajectories in robots. Background Technology

[0002] Robot motion trajectories can be divided into Cartesian trajectories and joint trajectories. When performing certain tasks, robots need to execute Cartesian space motion commands and joint space motion commands multiple times. This causes the robot to start and stop repeatedly to complete the corresponding motion commands, which greatly reduces the robot's motion efficiency. Therefore, designing a transition algorithm between Cartesian trajectories and joint trajectories can not only significantly improve the robot's work efficiency, but also enable the robot to execute different spatial motion commands more flexibly. Currently, there are research on transition methods between Cartesian trajectories and joint trajectories. One transition method aims to ensure the continuity of velocity and acceleration between the Cartesian space trajectory and the joint space trajectory after the transition. However, the transition trajectory of this method is completed in joint space, which cannot guarantee the shape of the robot's Cartesian space trajectory. When the transition trajectory interval is too large, the robot's Cartesian trajectory in the transition interval is prone to problems such as twisting and overshooting. Another transition method performs inverse kinematics on the planned Cartesian trajectory to obtain the corresponding joint information, and uses the end point of the Cartesian trajectory as the starting point of the joint trajectory to complete the corresponding trajectory planning in joint space. However, when the velocity at the end of the Cartesian trajectory is 0, the robot will still experience start and stop phenomena, resulting in low motion efficiency. Summary of the Invention

[0003] This application provides a method and apparatus for transitioning between a robot's Cartesian trajectory and a joint trajectory, which can achieve a smooth transition between the two trajectories, ensuring the continuity of the trajectory's speed and acceleration while avoiding yaw and overshoot phenomena.

[0004] On one hand, embodiments of this application provide a method for transitioning between a robot's Cartesian trajectory and a joint trajectory. The Cartesian trajectory is the motion trajectory of the robot's end effector in Cartesian space, and the joint trajectory is the motion trajectory of the robot's joints in joint space. The method includes the following steps:

[0005] Obtain the starting point Q of the initial Cartesian trajectory. s and the endpoint Q m The corresponding joint angle vectors θ1 and θ2, and the endpoint Q of the initial joint trajectory, respectively. e The corresponding joint angle vector θ3 is used to determine the start point Q1 and end point Q2 of the transition trajectory based on θ1, θ2 and θ3;

[0006] Multiple control vertices are determined between Q1 and Q2, and cubic non-uniform B-spline curve interpolation is performed based on Q1 and Q2 and the multiple control vertices to obtain the transition trajectory Q1Q2;

[0007] In Q s Plan the updated Cartesian trajectory between Q1 and Q2; e The joint trajectory is updated after the planning is completed;

[0008] The updated Cartesian trajectory, the transition trajectory Q1Q2, and the updated joint trajectory are sequentially concatenated to obtain the optimized trajectory Q. s Q e .

[0009] On the other hand, embodiments of this application also provide a transition device for robot Cartesian trajectory and joint trajectory, including a processor and a memory;

[0010] The memory is used to store the transition program between the robot's Cartesian trajectory and joint trajectory;

[0011] The processor is used to read the transition program between the robot's Cartesian trajectory and joint trajectory, and to perform the transition method between the robot's Cartesian trajectory and joint trajectory as described in the above embodiments.

[0012] Compared with related technologies, the method and apparatus for transitioning between Cartesian trajectory and joint trajectory of a robot according to the embodiments of this application obtains the transition trajectory between Cartesian space and joint space, which can realize the smooth transition between the robot's Cartesian trajectory and joint trajectory, ensure the continuity of trajectory speed and acceleration, avoid the phenomenon of swaying and overshoot at the robot end, avoid the robot starting and stopping between the two trajectories, and improve the robot's working efficiency.

[0013] Other features and advantages of this application will be set forth in the following description, and will be apparent in part from the description, or may be learned by practicing the application. Other advantages of this application can be realized and obtained by means of the solutions described in the description and the accompanying drawings. Attached Figure Description

[0014] The accompanying drawings are used to provide an understanding of the technical solutions of this application and constitute a part of the specification. They are used together with the embodiments of this application to explain the technical solutions of this application and do not constitute a limitation on the technical solutions of this application.

[0015] Figure 1 This is a flowchart of the transition method between the robot's Cartesian trajectory and joint trajectory according to an embodiment of this application;

[0016] Figure 2 The optimized trajectory Q is an embodiment of this application. s Q e Distribution map of various points in the middle;

[0017] Figure 3This is a rendering of the transition trajectory Q1Q2 as a specific example of this application;

[0018] Figure 4 This is an enlarged view of the effect diagram of the transition trajectory Q1Q2 in a specific example of this application;

[0019] Figure 5 This is a graph showing the trend of angular velocity change of joint 3 in the transition trajectory Q1Q2, which is a specific example of this application.

[0020] Figure 6 This is a graph showing the trend of angular acceleration of joint 3 in the transition trajectory Q1Q2, as specifically exemplified in this application.

[0021] Figure 7 This is a schematic diagram of the transition device between the robot's Cartesian trajectory and joint trajectory according to an embodiment of this application. Detailed Implementation

[0022] This application describes several embodiments, but these descriptions are exemplary and not restrictive, and it will be apparent to those skilled in the art that many more embodiments and implementations are possible within the scope of the embodiments described herein. Although many possible combinations of features are shown in the drawings and discussed in the detailed description, many other combinations of the disclosed features are also possible. Unless specifically limited, any feature or element of any embodiment may be used in combination with, or may replace, any feature or element of any other embodiment.

[0023] This application includes and contemplates combinations of features and elements known to those skilled in the art. The embodiments, features, and elements disclosed in this application may also be combined with any conventional features or elements to form a unique inventive scheme as defined by the claims. Any feature or element of any embodiment may also be combined with features or elements from other inventive schemes to form another unique inventive scheme as defined by the claims. Therefore, it should be understood that any feature shown and / or discussed in this application may be implemented individually or in any suitable combination. Therefore, the embodiments are not limited except by the limitations imposed by the appended claims and their equivalents. Furthermore, various modifications and changes may be made within the scope of the appended claims.

[0024] Furthermore, in describing representative embodiments, the specification may have presented methods and / or processes as a specific sequence of steps. However, the method or process should not be limited to the specific order of steps described herein, to the extent that it does not depend on such a specific order. As will be understood by those skilled in the art, other sequences of steps are also possible. Therefore, the specific order of steps set forth in the specification should not be construed as a limitation of the claims. Moreover, the claims concerning the method and / or process should not be limited to the steps performed in the written order, and those skilled in the art will readily understand that these orders can be varied and still remain within the spirit and scope of the embodiments of this application.

[0025] This application provides a method for transitioning between a robot's Cartesian trajectory and a joint trajectory. The Cartesian trajectory is the motion trajectory of the robot's end effector in Cartesian space, and the joint trajectory is the motion trajectory of the robot's joints in joint space. The method includes steps S100-S400, as follows: Figure 1 As shown,

[0026] S100: Obtain the starting point Q of the initial Cartesian trajectory. s and the endpoint Q m The corresponding joint angle vectors θ1 and θ2, and the endpoint Q of the initial joint trajectory, respectively. e The corresponding joint angle vector θ3 is used to determine the start point Q1 and end point Q2 of the transition trajectory based on θ1, θ2 and θ3;

[0027] S200: Determine multiple control vertices between Q1 and Q2, and perform cubic non-uniform B-spline curve interpolation based on Q1, Q2 and the multiple control vertices to obtain the transition trajectory Q1Q2;

[0028] S300: In Q s Plan the updated Cartesian trajectory between Q1 and Q2; e The joint trajectory is updated after the planning is completed;

[0029] S400: Sequentially concatenate the updated Cartesian trajectory, the transition trajectory Q1Q2, and the updated joint trajectory to obtain the optimized trajectory Q. s Q e .

[0030] In this embodiment, the robot end effector can be any one of a clamp, a suction device, a welding torch, a bionic hand, or a gripper. The above examples of robot end effectors are all exemplary descriptions and are not intended to limit this application. They will not be described in detail here.

[0031] In this embodiment, the number of robot joints can be arbitrary, depending on the type of robot and the application scenario. The robot can be a serial robot with six or fewer rotary joints, a SCARA robot with three to four joints, a Delta robot with three parallel joint arms, a spherical robot with at least two rotary joints, a humanoid robot with dozens of joints, etc. The above robot examples are all exemplary descriptions and are not intended to limit this application. They will not be described in detail here.

[0032] In this embodiment, the transition trajectory Q1Q2 is obtained by cubic non-uniform B-spline curve interpolation, which can ensure the continuity of robot velocity and acceleration in the transition trajectory and ensure a smooth transition between Cartesian trajectory and joint trajectory.

[0033] The transition method between the robot's Cartesian trajectory and joint trajectory in this embodiment uses an optimized trajectory to replace the original Cartesian trajectory and joint trajectory, obtaining a transition trajectory between Cartesian space and joint space. This enables a smooth transition between the robot's Cartesian trajectory and joint trajectory, ensuring continuous velocity and acceleration while avoiding swaying and overshoot at the robot's end effector. It also avoids the robot starting and stopping between the two trajectory segments, thus improving the robot's operational efficiency.

[0034] In one exemplary embodiment, step S100 may include steps S110-S140:

[0035] S110: Obtain Q s The corresponding joint angle vector θ1 is obtained, and the end pose vector P1 corresponding to θ1 is obtained based on the forward position model P=Η(θ);

[0036] S120: Obtain Q m The corresponding joint angle vector θ2 is obtained, and the end pose vector P2 corresponding to θ2 is obtained based on the position forward model P=Η(θ);

[0037] S130: In the Cartesian space, based on the percentage of time σ1 of the Cartesian trajectory motion, and P1 and P2 in Q... s and Q m Determine the starting point Q1 of the transition trajectory between them;

[0038] S140: Obtain Q e The corresponding joint angle vector θ3, in the joint space, is based on θ2 and θ3 in Q, according to the percentage of joint trajectory motion time σ2. m and Q e The endpoint Q2 of the transition trajectory is determined between these points.

[0039] In this embodiment, H(θ) is the transformation function from the robot joint angle vector θ to the robot end-effector pose vector P. P includes the end-effector position coordinates and end-effector pose coordinates, 0<σ1<1, 0<σ2<1, and σ1 and σ2 are preset values ​​determined according to the robot's transition accuracy requirements.

[0040] In other embodiments, Q can be used in other ways. s and Q m Determine the starting point Q1 of the transition trajectory between them, for example, based on Q. s and Q m The length of the initial Cartesian trajectory is given by the ratio of the initial length to the preset first length, and the position point on the initial Cartesian trajectory corresponding to the first length ratio is selected as Q1.

[0041] In other embodiments, Q can be used in other ways. m and Q e Determine the endpoint Q2 of the transition trajectory between them, for example, based on Q. m and Q e The length of the initial joint trajectory and the preset second length ratio are used to select the position point on the initial joint trajectory that corresponds to the second length ratio as Q2.

[0042] In one exemplary embodiment, step S130 may include steps S131-S133:

[0043] S131: In the Cartesian space, convert the end attitude coordinates in P1 and P2 into attitude quaternions respectively. R Φ1 and R Φ2;

[0044] S132: Using the end position coordinates described in P1 and R Φ1 as Q s The endpoint data, in the end position coordinates described in P2 and R Φ2 as Q m The endpoint data is used to perform seven-segment S-velocity programming and Slerp interpolation on Q. s and Q m Position and attitude planning is performed to obtain the first initial trajectory Q. s Q m And obtain the first initial trajectory Q s Q m The motion time t;

[0045] S133: Calculate time t′ = σ1t based on the percentage of time σ1 of the Cartesian trajectory motion, and place time t′ on the first initial trajectory Q. s Q m The corresponding trajectory point is taken as the starting point Q1 of the transition trajectory.

[0046] In one exemplary embodiment, step S140 may include steps S141-S145:

[0047] S141: Perform the following operation on each joint in the joint space: with θ2 as Q m The endpoint data, with θ3 as Q e Endpoint data in Q m and Q e Perform five-order polynomial programming to obtain the initial trajectory and running time t of a single joint trajectory. i ′, where i = 1, 2, ..., N, and N is the total number of joints;

[0048] S142: In all t i Take the maximum value t in ' j =max{t i ′}, where j represents the j-th joint among N joints;

[0049] S143: with t j Let ' be the time scale, and perform the following operations on each of the N-1 joints other than the j-th joint: perform time synchronization processing on the initial trajectory of the single joint, so that the running time of the single joint trajectory is t. i ' equals t j ′, to obtain the trajectory of a single joint after time synchronization;

[0050] S144: Combine the N-1 time-synchronized single joint trajectories and the initial single joint trajectory of the j-th joint as the second initial trajectory Q. m Q e ;

[0051] S145: Calculate time t1′ = (1-σ2)t based on the percentage of joint trajectory movement time σ2. j ′, at time t1′ on the second initial trajectory Q m Q e The corresponding trajectory point is taken as the endpoint Q2 of the transition trajectory.

[0052] In one exemplary embodiment, steps S150-S180 may be included after step S100 and before step S200:

[0053] S150: Based on the first initial trajectory Q s Q m Obtain the end-effector pose coordinates P of Q1 in the Cartesian space. Q1 and the terminal velocity vector V Q1 c , where P Q1 Including the end position coordinates Q1c and end attitude coordinates P′ Q1 ; Set the end attitude coordinates P′ Q1 Convert to attitude quaternion R Φ Q1 ;

[0054] S160: According to the second initial trajectory Q m Q e Obtain the joint angle vector θ corresponding to Q2 Q2 and joint angular velocity vector

[0055] S170: θ Q2 Inputting the forward model P = H(θ) yields the end-effector pose coordinates P of Q2 in the Cartesian space. Q2 , where P Q2 Including the end position coordinates Q2 c and end attitude coordinates P′ Q2 ; Set the end attitude coordinates P′ Q2 Convert to attitude quaternion R Φ Q2 ;

[0056] S180: Will Input velocity forward kinematics model Obtain the terminal velocity vector V Q2 c , where J is the robot Jacobian matrix.

[0057] In this embodiment, the parameters of the robot end effector corresponding to the starting point Q1 and ending point Q2 of the transition trajectory can be obtained through steps S150-S180: end effector velocity vector, end effector position coordinates and attitude quaternion. Multiple control vertices between Q1 and Q2 can be determined based on these parameters of the robot end effector. This embodiment is a preferred implementation of this application and is not intended to limit this application.

[0058] In one exemplary embodiment, determining multiple control vertices between Q1 and Q2 in step S200 may include steps S211-S216:

[0059] S211: Based on Q1 c The first straight-line distance l is determined by the coordinates of the end position in P2. p1 Based on Q2 c The second straight-line distance l is determined by the coordinates of the end position in P2. p2 ;

[0060] S212: Q1 c V Q1 c and lp1 Substitute into the formula Obtain the position coordinates of the second control vertex d2 in the Cartesian space;

[0061] S213: Q1 c V Q1 c and l p1 Substitute into the formula Obtain the position coordinates of the third control vertex d3 in the Cartesian space;

[0062] S214: Q2 c V Q2 c and l p2 Substitute into the formula Obtain the position coordinates of the fifth control vertex d5 in the Cartesian space;

[0063] S215: Q2 c V Q2 c and l p2 Substitute into the formula Obtain the position coordinates of the sixth control vertex d6 in the Cartesian space;

[0064] S216: Through formula Obtain the position coordinates of the fourth control vertex d4 in the Cartesian space;

[0065] In this embodiment, E = [I 3×3 O 3×3 ], I 3×3 It is a 3x3 identity matrix, O 3×3 It is a 3x3 matrix of zeros, ||EV Q1 c ||2 represents matrix EV Q1 c The 2-norm, ||EV Q2 c ||2 represents matrix EV Q2 c The 2-norm.

[0066] In this embodiment, the method for determining control vertices d2, d3, d4, d5, and d6 between Q1 and Q2 is to determine the position coordinates of these control points in Cartesian space. This method is applicable to determining control vertices of cubic non-uniform B-spline curves. It not only solves the problem of difficulty in selecting control points in joint space for cubic non-uniform B-spline curves, but also effectively achieves velocity continuity between the transition trajectory and the joint trajectory, improving the smoothness of the transition trajectory.

[0067] In this embodiment, the method of determining control vertices d2, d3, d4, d5, and d6 between Q1 and Q2 is not only applicable to straight Cartesian trajectories, but also to achieving a good transition between Cartesian trajectories and transition trajectories when the initial Cartesian trajectory is a circular arc trajectory. This ensures the continuity of velocity changes, reduces the degree of acceleration changes, and makes the transition trajectory smoother. Since the initial joint trajectory is an irregular curve in Cartesian space, this embodiment proposes a method for determining control vertices.

[0068] In an exemplary embodiment, step S200, which involves performing cubic non-uniform B-spline curve interpolation based on Q1 and Q2 and the plurality of control vertices to obtain the transition trajectory Q1Q2, may include steps S221-S226:

[0069] S221: According to V Q1 c Calculate the resultant velocity ||v| of the starting point Q1 of the transition trajectory. Q1 c ||2, according to V Q2 c Calculate the resultant velocity ||v| at the endpoint Q2 of the transition trajectory. Q2 c ||2;

[0070] S222: ||v Q1 c ||2 and||v Q2 c ||2 are respectively used as the initial velocity and final velocity of the transition trajectory, and S-velocity planning is performed to obtain the feed velocity V(u) of the transition trajectory;

[0071] S223: Using the second-order Taylor expansion method, based on V(u) and the interpolation period T... s Calculate the nodal increment u of a cubic non-uniform B-spline curve. λ+1 ;

[0072] S224: According to Q1 c Q2 c u λ+1 And the multiple control vertices, using the De Boor recursive formula to calculate the position coordinates of all trajectory points of the transition trajectory;

[0073] S225: Using the Slerp interpolation method in attitude quaternions R Φ Q1 and R Φ Q2 Interpolation is performed between them to obtain the attitude quaternions of all trajectory points of the transition trajectory;

[0074] S226: Synchronize the position coordinates of all trajectory points and the pose quaternions of all trajectory points to obtain the transition trajectory Q1Q2.

[0075] In this embodiment, multiple control vertices can be d2, d3, d4, d5, and d6. These five control vertices, together with Q1 and Q2, form the seven control vertices of the cubic non-uniform B-spline curve. The order of these seven control vertices is Q1, d2, d3, d4, d5, d6, and Q2, which are used to determine the shape of the cubic non-uniform B-spline curve.

[0076] In this embodiment, in step S223, the node increment u of the cubic non-uniform B-spline curve is calculated. λ+1 The formula is formula (1):

[0077]

[0078] In formula (1), C(u) λ ) represents the De Boor recursive formula for a cubic non-uniform B-spline curve, C′(u λ ) and C″(u λ ) represent cubic non-uniform B-spline curves with parameter u. λ The first and second derivatives at point C(u) are given by C(u). λ The calculation formula for ) is formula (2):

[0079]

[0080] In formula (2), k is the degree of the cubic non-uniform B-spline curve, k = 3; n is the number of control vertices, n = 7;

[0081]

[0082] Regulation

[0083] In one exemplary embodiment, step S300 in Q s The updated Cartesian trajectory between Q1 and Q1 may include step S310:

[0084] S310: In the Cartesian space, Q s The attitude velocities of Q1 and Q1 are both set to 0, combined with the end-position coordinates of Q1. c The terminal velocity vector V Q1 c and the attitude quaternion R Φ Q1 For the first initial trajectory Q s Q m Qs Perform S-velocity replanning between Q1 and Q1 to obtain the updated Cartesian trajectory Q. s Q1.

[0085] In one exemplary embodiment, step S300 is performed at Q2 and Q e The planned updated joint trajectory may include step S320:

[0086] S320: In the joint space, Q2 and Q e The accelerations are set to 0, combined with the joint angle vector θ of Q2. Q2 The joint angular velocity vector For the second initial trajectory Q m Q e Q2 and Q e Five polynomial reprogramming operations are performed to obtain the updated joint trajectory Q2Q. e .

[0087] In one exemplary embodiment, steps S330-S340 may be included after step S320:

[0088] S330: Set the time scaling factor σ3 and calculate time t2′=(1-σ3)t j ', t2' will be the updated joint trajectory Q2Q e The corresponding trajectory point is taken as Q3, where 0 < σ3 < σ2 < 1;

[0089] S340: In the updated joint trajectory Q2Q e A fifth-order polynomial programming process is performed between Q2 and Q3 to obtain a smooth trajectory Q2Q3.

[0090] In this embodiment, the smooth trajectory Q2Q3 can be obtained through steps S330-S340 to achieve the purpose of continuous global trajectory acceleration. Obtaining the smooth trajectory Q2Q3 is a preferred implementation of this application and is not intended to limit this application. Even without obtaining the smooth trajectory Q2Q3, it is still possible to obtain the updated Cartesian trajectory Q. s Q1, transition trajectory Q1Q2, and updated joint trajectory Q2Q e The optimized trajectory Q is obtained s Q e .

[0091] In one exemplary embodiment, step S340 may include steps S341-S342:

[0092] S341: Based on the updated joint trajectory Q2Q e Determine the joint angle vector θ corresponding to Q3 Q3Joint angular velocity vector and joint acceleration vector

[0093] S342: In the joint space, t2′-t1′ is used as the smoothing time for velocity and acceleration, combined with θ Q3 , and Perform a fifth-order polynomial programming operation between Q2 and Q3 to obtain the smooth trajectory Q2Q3.

[0094] In one exemplary embodiment, step S400 may include step S410:

[0095] S410: Update the Cartesian trajectory Q s Q1, the transition trajectory Q1Q2, the smooth trajectory Q2Q3, and the updated joint trajectory Q2Q e Q3 to Q e The segments are sequentially spliced ​​together to obtain the optimized trajectory Q. s Q e .

[0096] In this embodiment, the optimized trajectory Q s Q e The distribution of each point in the middle is as follows Figure 2 As shown, Q s Q is the starting point of the optimized trajectory, corresponding to the end-effector pose vector P1 in Cartesian space and the joint angle vector θ1 in joint space; e Q1 is the endpoint of the optimized trajectory, corresponding to the joint angle vector θ3 in joint space; Q2 is the starting point of the transition trajectory; Q3 is the endpoint of the smooth trajectory; Q4 is the starting point of the smooth trajectory. m This corresponds to the end pose vector P2 in Cartesian space and the joint angle vector θ2 in joint space.

[0097] In one exemplary embodiment, steps S010-S020 may be included before step S100:

[0098] S010: Construct the robot's forward position model P = H(θ) and inverse position model θ = G(P);

[0099] S020: Construct the robot's forward velocity kinematics model based on the aforementioned inverse position model θ=G(P). and

[0100] Inverse kinematics model of velocity

[0101] In this embodiment, H(θ) is the transformation function from the robot joint angle vector θ to the robot end effector pose vector P, and G(P) is the transformation function from the robot end effector pose vector P to the robot joint angle vector θ; θ = [θ1, θ2, ..., θ] N ] T N is the total number of joints, θ i Let P be the joint angle vector of the i-th joint in the joint space, i∈[1,N]; let P be the end-effector pose vector of the robot end in the Cartesian space, P=[x p y p z p ψ px ψ py ψ pz ] T x p y p , z p Let ψ be the coordinates of the end position. px , ψ py , ψ pz These are the end-effector attitude coordinates.

[0102] In this embodiment, J is the robot Jacobian matrix, J -1 Let J be the inverse matrix; V is the joint angular velocity vector of the i-th joint in the joint space; V is the end effector velocity vector of the robot's end effector in the Cartesian space, V = [v x v y v z ω x ω y ω z ] T ,v x v y and v z These are the velocity components of the robot's end effector in the x, y, and z directions in Cartesian space, respectively, ω. x ω y and ω z These are the rotational angular velocity components of the robot end effector around the x, y, and z axes of the Cartesian coordinate system in the Cartesian space.

[0103] In this embodiment, four robot models can be constructed through steps S010-S020: forward kinematics model P=H(θ), inverse kinematics model θ=G(P), and velocity forward kinematics model. and velocity inverse kinematics model These four pre-built models can be used whenever a transition between Cartesian trajectories and joint trajectories is required.

[0104] To illustrate the transition method between the robot's Cartesian trajectory and joint trajectory in this application embodiment, a specific example is described in detail below, including steps S1-S26. In this specific example, the number of joints N = 6.

[0105] S1: Construct the robot's forward position model P = H(θ) and inverse position model θ = G(P);

[0106] Specifically, the coordinate transformation matrices of joint i and joint i-1 are calculated based on the robot's DH parameters. i- 1 T i (i = 1, 2, 3, ..., 6) i-1 T i The expression is:

[0107]

[0108] in accordance with i-1 T i The expression calculates the transformation matrix from the i-th joint to the joint coordinate system. 0 T i :

[0109]

[0110] in accordance with 0 T i The forward position model P = H(θ) is obtained, and the inverse position model θ = G(P) is obtained by using the inversion method.

[0111] S2: Construct the robot's forward velocity kinematics model based on the inverse position model θ=G(P). and

[0112] Inverse kinematics model of velocity

[0113] Specifically, according to S1 0 T i Define z i =[ i a x i a y i a z ] T , 0 q i =[ i q x i q y i q z ] T , The Jacobian matrix J is calculated using the following formula:

[0114]

[0115] Based on the Jacobian matrix J, the forward kinematics model of velocity can be obtained. and

[0116] Inverse kinematics model of velocity

[0117] S3: Obtain the starting point Q of the initial Cartesian trajectory. s and the endpoint Q m The corresponding joint angle vectors θ1 and θ2 are obtained respectively, and the end pose vectors P1 and P2 corresponding to θ1 and θ2 are obtained based on the forward position solution model P=Η(θ);

[0118] Specifically, θ1 = [-10 -100 36 -23 -20], θ2 = [30 20 -15 -60 -45 20], the six numbers in θ1 and θ2 represent the joint angles of the six joints respectively, and the units of θ1 and θ2 are Deg. Inputting θ1 and θ2 into the position forward model P = H(θ) respectively yields P1 and P2.

[0119] S4: In the Cartesian space, convert the end attitude coordinates in P1 and P2 into attitude quaternions respectively. R Φ1 and R Φ2, using seven-segment S-velocity programming and Slerp interpolation methods respectively in Q s and Q m Position and attitude planning is performed, and the planned position and attitude are synchronized to obtain the first initial trajectory Q. s Q m And obtain the first initial trajectory Q s Q m The motion time t;

[0120] S5: Calculate time t′ = σ1t based on the percentage of time σ1 of the Cartesian trajectory, and place time t′ on the first initial trajectory Q. s Q m The corresponding trajectory point is taken as the starting point Q1 of the transition trajectory;

[0121] S6: Based on the first initial trajectory Q s Q m Obtain the end-effector pose coordinates P of Q1 in the Cartesian space. Q1 and the terminal velocity vector V Q1 c , where P Q1 Including the end position coordinates Q1 cand end attitude coordinates P′ Q1 ; Set the end attitude coordinates P′ Q1 Convert to attitude quaternion R Φ Q1 ;

[0122] Specifically, the first initial trajectory Q obtained in this specific example s Q m The motion time t = 1.644s, the percentage of motion time for the Cartesian trajectory σ1 = 0.8, and the final position coordinates Q1 c = [0.9899 0.6062 0.5866], terminal velocity vector V Q1 c = [-0.0562 0.36-10], attitude quaternion R Φ Q1 = [0.5101-0.83950.1375-0.1271].

[0123] S7: Obtain the endpoint Q of the initial joint trajectory. e The corresponding joint angle vector θ3, in the joint space, performs the following operation for each joint: with θ2 as Q m The endpoint data, with θ3 as Q e Endpoint data in Q m and Q e Perform five-order polynomial programming to obtain the initial trajectory and running time t of a single joint trajectory. i ′, where i = 1, 2, ..., 6;

[0124] S8: In all t i Take the maximum value t in ' j =max{t i ′}, where j represents the j-th joint out of 6 joints;

[0125] Specifically, θ3 = [71.453.3 - 21.143.2 - 37.864.8], where the six numbers in θ3 represent the joint angles of the six joints, in deg; t j = 2.0231s.

[0126] S9: with t j Using ' as the time scale, perform the following operations on the other 5 joints (excluding the j-th joint): perform time synchronization processing on the initial trajectory of the single joint, so that the running time of the single joint trajectory is t. i ' equals t j ′, to obtain the trajectory of a single joint after time synchronization;

[0127] S10: Combine the five time-synchronized single joint trajectories and the initial single joint trajectory of the j-th joint as the second initial trajectory Q. m Q e ;

[0128] S11: Calculate time t1′ = (1-σ2)t based on the percentage of joint trajectory movement time σ2. j ′, at time t1′ on the second initial trajectory Q m Q e The corresponding trajectory point is taken as the endpoint Q2 of the transition trajectory;

[0129] S12: According to the second initial trajectory Q m Q e Obtain the joint angle vector θ corresponding to Q2 Q2 and joint angular velocity vector

[0130] Specifically, θ Q2 = [0.5653 0.3826-0.26791.03030.70200.3942], where the six numbers represent the joint angles of the six joints, in rad; The six numbers represent the joint angular velocities of the six joints, in rad / s.

[0131] S13: θ Q2 Inputting the forward model P = H(θ) yields the end-effector pose coordinates P of Q2 in the Cartesian space. Q2 , where P Q2 Including the end position coordinates Q2 c and end attitude coordinates P′ Q2 ; Set the end attitude coordinates P′ Q2 Convert to attitude quaternion R Φ Q2 ;

[0132] S14: Will Input velocity forward kinematics model Obtain the terminal velocity vector V Q2 c ;

[0133] S15: Determine five control vertices d2, d3, d4, d5, and d6 between Q1 and Q2, and obtain the position coordinates of these five control vertices in the Cartesian space;

[0134] Specifically, d2 = [0.9823 0.6221 0.5817]; d3 = [0.9948 0.6378 0.5769]; d4 = [0.9957 0.6587 0.5688]; d5 = [0.9967 0.6796 0.5607]; d6 = [0.9952 0.6901 0.5527].

[0135] S16: According to V Q1 c Calculate the resultant velocity ||v| of the starting point Q1 of the transition trajectory. Q1 c ||2, according to V Q2 c Calculate the resultant velocity ||v| at the endpoint Q2 of the transition trajectory. Q2 c ||2;

[0136] Specifically, ||v Q1 c ||2=0.3819m / s;||v Q2 c ||2=0.3987m / s.

[0137] S17: ||v Q1 c ||2 and||v Q2 c ||2 are respectively used as the initial velocity and final velocity of the transition trajectory, and S-velocity planning is performed to obtain the feed velocity V(u) of the transition trajectory; using the second-order Taylor expansion method, based on V(u) and the interpolation period T s Calculate the nodal increment u of a cubic non-uniform B-spline curve. λ+1 ;

[0138] S18: According to Q1 c Q2 c u λ+1 And control vertices d2, d3, d4, d5, and d6, and calculate the position coordinates of all trajectory points of the transition trajectory using the De Boor recursive formula;

[0139] S19: Using the Slerp interpolation method in attitude quaternions R Φ Q1 and R Φ Q2 Interpolation is performed between them to obtain the attitude quaternions of all trajectory points of the transition trajectory;

[0140] S20: Synchronize the position coordinates and pose quaternions of all trajectory points to obtain the transition trajectory Q1Q2.

[0141] S21: In the Cartesian space, Q s The attitude velocities of Q1 and Q1 are both set to 0, combined with the end-position coordinates of Q1. c The terminal velocity vector V Q1 c and the attitude quaternion R Φ Q1 For the first initial trajectory Q s Q m Q s Perform S-velocity replanning between Q1 and Q1 to obtain the updated Cartesian trajectory Q. s Q1;

[0142] S22: In the joint space, Q2 and Q e The accelerations are set to 0, combined with the joint angle vector θ of Q2. Q2 The joint angular velocity vector For the second initial trajectory Q m Q e Q2 and Q e Five polynomial reprogramming operations are performed to obtain the updated joint trajectory Q2Q. e ;

[0143] S23: Set the time scaling factor σ3 and calculate time t2′=(1-σ3)t j ', t2' will be the updated joint trajectory Q2Q e The corresponding trajectory point is taken as Q3, where 0 < σ3 < σ2 < 1;

[0144] S24: Based on the updated joint trajectory Q2Q e Determine the joint angle vector θ corresponding to Q3 Q3 Joint angular velocity vector and joint acceleration vector

[0145] S25: In the joint space, t2′-t1′ is used as the smoothing time for velocity and acceleration, combined with θ Q3 , and Perform a fifth-order polynomial programming operation between Q2 and Q3 to obtain the smooth trajectory Q2Q3.

[0146] S26: Update the Cartesian trajectory Q s Q1, the transition trajectory Q1Q2, the smooth trajectory Q2Q3, and the updated joint trajectory Q2Q e Q3 to Q e The segments are sequentially spliced ​​together to obtain the optimized trajectory Q.s Q e .

[0147] The effect diagram of the transition trajectory Q1Q2 obtained in this specific example is as follows: Figure 3 and Figure 4 As shown, Figure 3 and Figure 4 The coordinate system in the diagram is a Cartesian coordinate system in Cartesian space. The three axes of the Cartesian coordinate system are X, Y, and Z, with units of meters (m). The solid line represents the initial Cartesian trajectory, the dashed line represents the initial joint trajectory, and the trajectory segment with circles on the solid line represents the transition trajectory. Figure 4 for Figure 3 In the magnified view of the transition trajectory Q1Q2, the initial trajectory includes the initial joint trajectory and the initial Cartesian trajectory, which are: Figure 3 and Figure 4 As can be seen, the transition trajectory Q1Q2 obtained in this specific example achieves a smooth transition between the Cartesian trajectory and the joint trajectory.

[0148] The trend diagram of the angular velocity and angular acceleration of joint 3 in the transition trajectory Q1Q2 obtained in this specific example is shown below. Figure 5 and 6 As shown, the horizontal axis represents time (s), and the vertical axes represent angular velocity (rad / s) and angular acceleration (rad / s²), respectively. 2 ),Depend on Figure 5 and Figure 6 It can be seen that the angular velocity and angular acceleration of joint 3 change continuously in the transition trajectory Q1Q2, ensuring the global continuity of velocity and acceleration.

[0149] This application also provides a transition device for robot Cartesian trajectory and joint trajectory, such as... Figure 7 As shown, it includes a processor and memory;

[0150] The memory is used to store the transition program between the robot's Cartesian trajectory and joint trajectory;

[0151] The processor is used to read the transition program between the robot's Cartesian trajectory and joint trajectory, and to perform the transition method between the robot's Cartesian trajectory and joint trajectory as described in the above embodiments.

[0152] It will be understood by those skilled in the art that all or some of the steps, systems, or apparatuses disclosed above, and their functional modules / units, can be implemented as software, firmware, hardware, or suitable combinations thereof. In hardware implementations, the division between functional modules / units mentioned above does not necessarily correspond to the division of physical components; for example, a physical component may have multiple functions, or a function or step may be performed collaboratively by several physical components. Some or all components may be implemented as software executed by a processor, such as a digital signal processor or microprocessor, or as hardware, or as an integrated circuit, such as an application-specific integrated circuit (ASIC). Such software may be distributed on a computer-readable medium, which may include computer storage media (or non-transitory media) and communication media (or transient media). As is known to those skilled in the art, the term computer storage media includes volatile and non-volatile, removable and non-removable media implemented in any method or technology for storing information (such as computer-readable instructions, data structures, program modules, or other data). Computer storage media include, but are not limited to, RAM, ROM, EEPROM, flash memory or other memory technologies, CD-ROM, digital versatile disc (DVD) or other optical disc storage, magnetic cartridges, magnetic tape, disk storage or other magnetic storage devices, or any other medium that can be used to store desired information and can be accessed by a computer. Furthermore, it is well known to those skilled in the art that communication media typically contain computer-readable instructions, data structures, program modules, or other data in modulated data signals such as carrier waves or other transmission mechanisms, and may include any information delivery medium.

Claims

1. A method for transitioning between a robot's Cartesian trajectory and a joint trajectory, wherein the Cartesian trajectory is the motion trajectory of the robot's end effector in Cartesian space, and the joint trajectory is the motion trajectory of the robot's joints in joint space, characterized in that... Includes the following steps: Obtain the starting point of the initial Cartesian trajectory Q s and the end point Q m The corresponding joint angle vectors and and the endpoint of the initial joint trajectory Q e Corresponding joint angle vector ,based on , and Determine the starting point of the transition trajectory Q 1 and the finish line Q 2; exist Q 1 and Q Multiple control vertices are determined between 2, and based on Q 1 and Q 2. The transition trajectory is obtained by performing cubic non-uniform B-spline curve interpolation on the multiple control vertices. Q 1 Q 2; exist Q s and Q The Cartesian trajectory is updated between 1 and 2; Q 2 and Q e The joint trajectory is updated after the planning is completed; The updated Cartesian trajectory and the transition trajectory Q 1 Q 2. The updated joint trajectory is sequentially spliced ​​together to obtain the optimized trajectory. Q s Q e ; Wherein, at the starting point of determining the transition trajectory Q 1 and the finish line Q 2 after that, in the context of Q 1 and Q Before determining multiple control vertices between 2, including: based on the first initial trajectory Q s Q m Get Q 1. End-effector pose coordinates in the Cartesian space and terminal velocity vector ,in, Including end position coordinates and end attitude coordinates ; the end attitude coordinates Convert to attitude quaternion According to the second initial trajectory Q m Q e Get Q 2 corresponds to the joint angle vector and joint angular velocity vector ;Will Input position forward solution model get Q 2. Endpoint pose coordinates corresponding to the Cartesian space ,in, Including end position coordinates and end attitude coordinates ; the end attitude coordinates Convert to attitude quaternion ;Will Input velocity forward kinematics model Obtain the terminal velocity vector ,in, For the robot's Jacobian matrix; The above Q 1 and Q Multiple control vertices are defined between 2, including: based on and P The coordinates of the end position in 2 determine the first straight-line distance. ;based on and P The coordinates of the end position in 2 determine the second straight-line distance. ;Will , and Substitute into the formula Obtain the second control vertex Position coordinates in the Cartesian space; , and Substitute into the formula Obtain the third control vertex Position coordinates in the Cartesian space; , and Substitute into the formula Obtain the fifth control vertex Position coordinates in the Cartesian space; , and Substitute into the formula Obtain the sixth control vertex Position coordinates in the Cartesian space; via the formula Obtain the fourth control vertex The position coordinates in the Cartesian space; where, , It is a 3x3 identity matrix. It is a 3x3 matrix of zeros. For matrix The 2-norm, For matrix The 2-norm; The basis Q 1 and Q 2. The transition trajectory is obtained by performing cubic non-uniform B-spline curve interpolation on the multiple control vertices. Q 1 Q 2, including: according to Calculate the starting point of the transition trajectory Q 1 resultant velocity ,according to Calculate the endpoint of the transition trajectory Q 2 resultant velocity ;Will and The initial and final velocities of the transition trajectory are respectively used as the initial and final velocities, and S-velocity planning is performed to obtain the feed rate of the transition trajectory. The second-order Taylor expansion method is adopted, based on... and interpolation period Calculate the nodal increment of a cubic non-uniform B-spline curve u λ+1 ;according to , , u λ+1 The position coordinates of all trajectory points of the transition trajectory are calculated using the De Boor recursive formula, along with the multiple control vertices; the Slerp interpolation method is used in the attitude quaternion. and Interpolation is performed between the points to obtain the pose quaternions of all trajectory points in the transition trajectory; the position coordinates and pose quaternions of all trajectory points are synchronized to obtain the transition trajectory. Q 1 Q 2.

2. The transition method between Cartesian trajectory and joint trajectory of a robot as described in claim 1, characterized in that, The starting point for obtaining the initial Cartesian trajectory Q s and the end point Q m The corresponding joint angle vectors and and the endpoint of the initial joint trajectory Q e Corresponding joint angle vector ,based on , and Determine the starting point of the transition trajectory Q 1 and the finish line Q 2, including: Get Q s Corresponding joint angle vector And based on the position forward solution model get The corresponding end pose vector P 1; Get Q m Corresponding joint angle vector And based on the aforementioned position forward solution model get The corresponding end pose vector P 2; In the Cartesian space, according to the percentage of time of motion of the Cartesian trajectory ,based on P 1 and P 2 in Q s and Q m Determine the starting point of the transition trajectory between them. Q 1; Get Q e Corresponding joint angle vector In the joint space, based on the percentage of joint trajectory movement time ,based on and exist Q m and Q e The endpoint of the transition trajectory is determined between these points. Q 2; in, Robot joint angle vector To the robot end-effector pose vector Transformation function, Includes end-effector position coordinates and end-effector attitude coordinates, 0 < <1, 0< <1, and These are preset values ​​determined based on the robot's transition accuracy requirements.

3. The transition method between Cartesian trajectory and joint trajectory of a robot as described in claim 2, characterized in that, In the Cartesian space, based on the percentage of motion time of the Cartesian trajectory. ,based on P 1 and P 2 in Q s and Q m Determine the starting point of the transition trajectory between them. Q 1. Includes: In the Cartesian space, P 1 and P The end attitude coordinates mentioned in section 2 are converted into attitude quaternions respectively. and ; by P The end position coordinates mentioned in 1 and As Q s endpoint data, in P The end position coordinates mentioned in 2 and As Q m The endpoint data is used to perform seven-segment S-velocity programming and Slerp interpolation methods respectively. Q s and Q m Position and attitude planning is performed between them to obtain the first initial trajectory. Q s Q m and obtain the first initial trajectory. Q s Q m exercise time t ; Based on the percentage of motion time in the Descartes trajectory Calculation time , will be the moment In the first initial trajectory Q s Q m The corresponding trajectory point is used as the starting point of the transition trajectory. Q 1.

4. The transition method between Cartesian trajectory and joint trajectory of a robot as described in claim 2, characterized in that, In the joint space, based on the percentage of joint trajectory movement time... ,based on and exist Q m and Q e The endpoint of the transition trajectory is determined between these points. Q 2, including: Perform the following operations on each joint in the joint space: for Q m endpoint data, in for Q e endpoint data, in Q m and Q e Perform five polynomial programming operations to obtain the initial trajectory and running time of a single joint trajectory. ,in i =1,2,…… N , N This represents the total number of joints. In all Take the maximum value from the middle ,in, j represent N The first joint j One joint; by Using time scale, for the excluding the first j Other than the joint N - Each joint performs the following operations: performs time synchronization processing on the initial trajectory of the single joint, so that the running time of the single joint trajectory is... equal To obtain the trajectory of a single joint after time synchronization; Will N -1 The single joint trajectory after time synchronization and the first j The initial trajectories of each joint are collectively used as the second initial trajectory. Q m Q e ; Based on the percentage of joint trajectory movement time Calculation time = , will be the moment In the second initial trajectory Q m Q e The corresponding trajectory point is taken as the endpoint of the transition trajectory. Q 2.

5. The transition method between a robot's Cartesian trajectory and joint trajectory as described in claim 3 or 4, characterized in that, The above Q s and Q The updated Cartesian trajectory between points 1 and 2 includes: In the Cartesian space, Q s and Q The attitude and velocity of 1 are set to 0 respectively, combined with Q The end position coordinates of 1 The terminal velocity vector and the attitude quaternion For the first initial trajectory Q s Q m of Q s and Q Replanning the S-velocity between 1 and 2 to obtain the updated Cartesian trajectory. Q s Q 1.

6. The transition method between a robot's Cartesian trajectory and joint trajectory as described in claim 3 or 4, characterized in that, The above Q 2 and Q e The planned updated joint trajectories include: In the joint space, Q 2 and Q e The accelerations were set to 0 respectively, combined with Q The joint angle vector of 2 The joint angular velocity vector For the second initial trajectory Q m Q e of Q 2 and Q e Five polynomial reprogramming operations are performed to obtain the updated joint trajectory. Q 2 Q e .

7. The transition method between Cartesian trajectory and joint trajectory of a robot as described in claim 6, characterized in that, After obtaining the updated joint trajectory Q 2 Q e Following that, it also includes: Set time ratio coefficient And calculate the time. = ,Will The updated joint trajectory Q 2 Q e The corresponding trajectory points are used as Q 3, where 0 < < <1; The updated joint trajectory Q 2 Q e of Q 2 and Q Perform five more polynomial programming operations between the three points to obtain a smooth trajectory. Q 2 Q 3.

8. The transition method between Cartesian trajectory and joint trajectory of a robot as described in claim 7, characterized in that, The updated joint trajectory Q 2 and Q Perform five more polynomial programming operations between the three points to obtain a smooth trajectory. Q 2 Q 3, including: According to the updated joint trajectory Q 2 Q e Sure Q 3 corresponding joint angle vector Joint angular velocity vector and joint acceleration vector ; In the joint space, - As the smoothing time of velocity and acceleration, combined , and exist Q 2 and Q Perform a fifth-order polynomial programming operation between 3 to obtain the smooth trajectory. Q 2 Q 3.

9. The transition method between Cartesian trajectory and joint trajectory of a robot as described in claim 8, characterized in that, The updated Cartesian trajectory and the transition trajectory are then... Q 1 Q 2. The updated joint trajectory is sequentially spliced ​​together to obtain the optimized trajectory. Q s Q e ,include: The updated Cartesian trajectory Q s Q 1. The transition trajectory Q 1 Q 2. The smooth trajectory Q 2 Q 3 and the updated joint trajectory Q 2 Q e of Q 3 to Q e The segments are sequentially spliced ​​together to obtain the optimized trajectory. Q s Q e .

10. The transition method between Cartesian trajectory and joint trajectory of a robot as described in claim 1, characterized in that, At the starting point of obtaining the initial Cartesian trajectory Q s and the end point Q m The corresponding joint angle vectors and and the endpoint of the initial joint trajectory Q e Corresponding joint angle vector Previously, including: Constructing the robot's forward positioning model and position inverse solution model ;in, Robot joint angle vector To the robot end-effector pose vector Transformation function, For the robot's end-effector pose vector To robot joint angle vector Transformation function; , N This represents the total number of joints. For the first i The joint angle vector of each joint in the joint space. i ∈[1, N ]; Let be the end-effector pose vector of the robot in the Cartesian space. , , , The coordinates of the end position, , , These are the end-effector attitude coordinates; Based on the inverse position solution model Constructing the forward kinematics model of the robot's velocity and velocity inverse kinematics model ,in, For the robot's Jacobian matrix, for The inverse matrix; , For the first i The joint angular velocity vector of each joint in the joint space; Let be the end-effector velocity vector of the robot's end effector in the Cartesian space. , , and The robot end effector in the Cartesian space x, y and z The velocity component in the direction, , and These are the robot end effectors orbiting the Cartesian coordinate system in Cartesian space. x, y and z The rotational angular velocity component of the shaft.

11. A transition device for a robot's Cartesian trajectory and joint trajectory, comprising a processor and a memory, characterized in that: The memory is used to store the transition program between the robot's Cartesian trajectory and joint trajectory; The processor is configured to read the transition program between the robot's Cartesian trajectory and joint trajectory, and perform the transition method between the robot's Cartesian trajectory and joint trajectory as described in any one of claims 1-10.

Images