A linear trajectory planning method for a robotic arm based on a five-segment S-shaped velocity curve

Abstract

The invention discloses a method for planning a straight-line trajectory of a robot arm based on a five-segment S-shaped speed curve, and belongs to the field of robot arm control. The invention interpolates intermediate points of a straight-line trajectory whose speed presents a five-segment S-shaped speed curve changing over time under high rigidity, so that the robot arm can run at a safe speed and accurately reach a specified position while using high rigidity.

Description

Technical Field

[0001] The present invention relates to the field of robot arm control, and in particular to a robot arm linear trajectory planning method based on a five-segment S-shaped speed curve. Background Art

[0002] The position control of the robot arm is mainly divided into joint space control and Cartesian space control. The control of the robot arm joint space requires the inverse kinematics module to map the Cartesian task trajectory to the joint space, and then track the trajectory through the joint layer controller after obtaining the joint space trajectory. For the joint layer controller, the simplest is when the robot arm moves slowly and the transmission part uses a large reduction ratio. At this time, the robot arm system can be regarded as a linear system, and its nonlinear coupling part can be ignored. At this time, PD control is used for each joint. Although the robot arm joint space controller based on PD control of each independent joint has asymptotic stability, it cannot guarantee the stability of all task points.

[0003] The above control does not take into account the dynamic model of the robot arm, so it is called "non-model-based control". When the robot arm moves at a fast speed, the nonlinear coupling between the various parts of the robot arm is serious. If precise position control is to be performed, the dynamic model of the robot arm needs to be considered. That is, "model-based control", also known as "dynamic control". This model-based robot arm control scheme can make the robot arm have better dynamic performance. The control of the model-based robot arm usually includes an inner control loop and an outer control loop, in which the inner control loop mainly feeds back the dynamic compensation, which decouples the nonlinear control system into a linear system. The controller in the outer control loop can adopt a PD controller, an adaptive controller, etc.

[0004] Both model-based PD control and non-model-based PD control have the same problem. When the stiffness matrix used as the gain is large, although it can ensure that the robot arm can reach the specified point more accurately, the movement speed of the robot arm will increase with the increase of the gain, and safety problems are very likely to occur. Summary of the invention

[0005] In response to the problems existing in the above-mentioned technology, the purpose of the present invention is to provide a method for planning a straight-line trajectory of a robotic arm based on a five-segment S-shaped speed curve. This method interpolates the intermediate points of a straight-line trajectory whose speed changes over time in a five-segment S-shaped curve under high stiffness, so that the robotic arm can run at a safe speed and accurately reach a specified position while using high stiffness.

[0006] In order to achieve the above object, the technical solution adopted by the present invention is as follows:

[0007] A method for planning a straight-line trajectory of a robotic arm based on a five-segment S-shaped velocity curve comprises the following steps:

[0008] 1) Divide the velocity into five processes, namely: accelerated motion with increasing acceleration, accelerated motion with decreasing acceleration, uniform motion, decelerated motion with increasing acceleration and decelerated motion with decreasing acceleration, and then decompose the velocity into the x, y and z directions of the robot base coordinate system. All x, y and z directions mentioned in the present invention refer to the x, y and z directions of the robot base coordinate system. The time of the five motions in the present invention is the same. First, establish a model of acceleration with respect to time in the x, y and z directions. Secondly, integrate the jerk model functions established in the x, y and z directions respectively, so as to obtain the models of acceleration with respect to time in the x, y and z directions. Then integrate the acceleration model functions in the x, y and z directions respectively, and finally obtain the functions of velocity with respect to time in the x, y and z directions.

[0009] 2) Solving the position function: Integrate the velocity model established in step 1) to obtain the position function of five segments in the x, y, and z directions, which are: the position function of the acceleration phase with increasing acceleration, the position function of the acceleration phase with decreasing acceleration, the position function of the uniform speed phase, the position function of the deceleration phase with increasing acceleration, and the position function of the deceleration phase with decreasing acceleration;

[0010] 3) Add constraints: Add constraints to the function of position in the x, y, and z directions with respect to time obtained in step 2). The constraint condition is that the target position must be reached when the velocity decays to 0;

[0011] 4) Design controller: Design a robot controller based on PD control, calculate the torque and send it to the robot control cabinet, and control each joint of the robot to reach the specified position.

[0012] Furthermore, in step 1), the acceleration models established in the x, y, and z directions with respect to time are as shown in the following equations (1-1), (1-2), and (1-3):

[0013]

[0014]

[0015]

[0016] Among them, j x (t), j y (t), j z (t) represents the function of acceleration in the x direction, the function of acceleration in the y direction, and the function of acceleration in the z direction respectively; t and t1 represent the time variable and the time taken for the accelerated motion with increasing acceleration respectively; Jx , J y , J z They represent the jerk corresponding to the x-direction at time t, the jerk corresponding to the y-direction at time t, and the jerk corresponding to the z-direction at time t respectively; the model functions of the accelerations in the x-, y-, and z-directions with respect to time are as shown in equations (1-4), (1-5), and (1-6):

[0017]

[0018]

[0019]

[0020] Among them, a x (t), a y (t), a z (t) represents the function of acceleration in the x direction with respect to time, the function of acceleration in the y direction with respect to time, and the function of acceleration in the z direction with respect to time; the function of velocity in the x, y, and z directions with respect to time is shown in equations (1-7), (1-8), and (1-9):

[0021]

[0022]

[0023]

[0024] Among them, v x (t), v y (t), v z (t) represents the function of velocity in the x direction, the function of velocity in the y direction, and the function of velocity in the z direction with respect to time, respectively;

[0025] Furthermore, in step 2), the velocity model is integrated to obtain the function of the position in the x, y, and z directions with respect to time, as shown in equations (1-10), (1-11), and (1-12):

[0026]

[0027]

[0028]

[0029] Among them, x(t), y(t), and z(t) represent the position in the x direction as a function of time, the position in the y direction, and the position in the z direction as a function of time, respectively; x0, y0, and z0 represent the initial position in the x direction, the initial position in the y direction, and the initial position in the z direction, respectively.

[0030] Furthermore, in step 3), position constraints in the x, y, and z directions are added, as shown in formula (1-13):

[0031]

[0032] Substitute equation (1-13) into (1-10), (1-11), and (1-12) to obtain the acceleration J in the x, y, and z directions: x , J y , J z , as shown in formula (1-14):

[0033]

[0034] where x tar ,y tar 、z tar They represent the x-coordinate, y-coordinate, and z-coordinate of the target point respectively.

[0035] Furthermore, in step 4), the robot arm controller designed based on PD control is shown in formula (1-15):

[0036]

[0037] Where τ represents the torque of the robot arm; J represents the Jacobian matrix; K p Represents the proportional gain of PD control and represents the stiffness matrix of the robot in robot control; K d It represents the differential control of PD control and the damping matrix of the robot in robot control; q d represents the expected position; q represents the actual position; Indicates the expected speed; Indicates actual speed.

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

[0039] 1) A robot linear trajectory planning method based on a five-segment S-shaped velocity curve is proposed to reduce the position error q in PD control. d -q and speed error This allows the robot arm to maintain a very small τ under large rigidity, allowing the robot arm to approach the target point at a safer speed while maintaining high precision.

[0040] 2) The time of the five-segment motion can be changed by modifying t1, and the stiffness of the specified segment of the running trajectory can be changed at any time to complete specific tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 is a flow chart of a method implementation of an embodiment of the present invention;

[0042] Figure 2 is a graph of the acceleration in the x direction as a function of time according to an embodiment of the present invention;

[0043] Figure 3 is a graph of the acceleration in the y direction as a function of time according to an embodiment of the present invention;

[0044] Figure 4 is a graph of the acceleration in the z direction as a function of time according to an embodiment of the present invention;

[0045] Figure 5 is a graph of acceleration in the x-direction as a function of time proposed by an embodiment of the present invention;

[0046] Figure 6 is a graph of acceleration in the y direction as a function of time proposed by an embodiment of the present invention;

[0047] Figure 7 is a graph of acceleration in the z direction as a function of time proposed by an embodiment of the present invention;

[0048] Figure 8 is a graph of the velocity in the x direction as a function of time proposed by an embodiment of the present invention;

[0049] Fig. 9 is a graph of the velocity in the y direction as a function of time proposed by an embodiment of the present invention;

[0050] Fig.10 is a graph of the speed in the z direction as a function of time proposed by an embodiment of the present invention;

[0051] Fig.11 is a graph of the position in the x direction as a function of time proposed by an embodiment of the present invention;

[0052] Fig.12 is a graph of the position in the y direction as a function of time proposed by an embodiment of the present invention;

[0053] Fig.13 It is a curve diagram of the position in the z direction as a function of time proposed by an embodiment of the present invention. DETAILED DESCRIPTION

[0054] The present invention will be further described in detail below in conjunction with specific embodiments.

[0055] The purpose of the present invention is to propose a method for planning a straight-line trajectory of a robotic arm based on a five-segment S-shaped speed curve. This method interpolates intermediate points of a straight-line trajectory whose speed changes over time in a five-segment S-shaped curve under high stiffness, so that the robotic arm can run at a safe speed and accurately reach a specified position while using high stiffness.

[0056] In this embodiment, a Frank Emika Panda seven-degree-of-freedom robot arm produced in Germany is used.

[0057] A robot arm linear trajectory planning method based on a five-segment S-shaped velocity curve, the specific steps are as follows:

[0058] Step 1: Modeling the velocity: We divide the velocity into five processes, namely: accelerated motion with increasing acceleration, accelerated motion with decreasing acceleration, uniform motion, decelerated motion with increasing acceleration and decelerated motion with decreasing acceleration, and then decompose the velocity into the x, y, and z directions of the robot base coordinate system. All x, y, and z directions mentioned later in the present invention refer to the x, y, and z directions of the robot base coordinate system. The time of the five motions in the present invention is the same. First, a model of acceleration with respect to time is established in the x, y, and z directions, as shown in equations (1-16), (1-17), and (1-18):

[0059]

[0060]

[0061]

[0062] Among them, j x (t), j y (t), j z (t) represents the function of acceleration in the x direction, the function of acceleration in the y direction, and the function of acceleration in the z direction respectively; t and t1 represent the time variable and the time taken for the accelerated motion with increasing acceleration respectively; J x , J y , J z They represent the jerk corresponding to the time t in the x direction, the jerk corresponding to the time t in the y direction, and the jerk corresponding to the time t in the z direction, respectively. Then, the jerk model functions established in the x, y, and z directions are integrated to obtain the model functions of the acceleration in the x, y, and z directions with respect to time, as shown in equations (1-19), (1-20), and (1-21):

[0063]

[0064]

[0065]

[0066] Among them, a x (t), a y (t), a z (t) represents the function of acceleration in the x direction with respect to time, the function of acceleration in the y direction with respect to time, and the function of acceleration in the z direction with respect to time, respectively. Then, the acceleration model functions in the x, y, and z directions are integrated respectively, and finally the functions of velocity in the x, y, and z directions with respect to time are obtained, as shown in equations (1-22), (1-23), and (1-24):

[0067]

[0068]

[0069]

[0070] Among them, v x (t), v y (t), v z (t) represents the function of velocity in the x direction, the function of velocity in the y direction, and the function of velocity in the z direction with respect to time, respectively;

[0071] Step 2: Solve the position function: Integrate the velocity model established in step 1 to obtain the position function of five segments in the x, y, and z directions, which are: the position function of the acceleration stage with increasing acceleration, the position function of the acceleration stage with decreasing acceleration, the position function of the uniform speed stage, the position function of the deceleration stage with increasing acceleration, and the position function of the deceleration stage with decreasing acceleration, as shown in equations (1-25), (1-26), and (1-27):

[0072]

[0073]

[0074]

[0075] Wherein, x(t), y(t), and z(t) represent the function of position in the x direction with respect to time, the function of position in the y direction with respect to time, and the function of position in the z direction with respect to time, respectively; x0, y0, and z0 represent the initial position in the x direction, the initial position in the y direction, and the initial position in the z direction, respectively;

[0076] Step 3: Add constraints: Add constraints to the position function of time in the x, y, and z directions obtained in step 2. The constraint condition is that the target position must be reached when the velocity decays to 0. The position constraints in the x, y, and z directions are as follows:

[0077]

[0078] Substitute equation (1-28) into (1-25), (1-26), and (1-27) to obtain the acceleration J in the x, y, and z directions: x , J y , J z , as shown in formula (1-29):

[0079]

[0080] where x tar ,y tar 、z tar They represent the x-coordinate, y-coordinate, and z-coordinate of the target point respectively.

[0081] Step 4: Design controller: Design a robot controller based on PD control, calculate the torque and send it to the robot control cabinet, control each joint of the robot to reach the specified position, and the robot controller designed based on PD control is shown in formula (1-30):

[0082]

[0083] Where τ represents the torque of the robot arm; J represents the Jacobian matrix; K p Represents the proportional gain of PD control and represents the stiffness matrix of the robot in robot control; K d It represents the differential control of PD control and the damping matrix of the robot in robot control; q d represents the expected position; q represents the actual position; Indicates the expected speed; Indicates actual speed.

[0084] The above are preferred embodiments of the present invention. Any changes made according to the technical solution of the present invention, as long as the resulting functions do not exceed the scope of the technical solution of the present invention, belong to the protection scope of the present invention.

Claims

1. A method for planning a straight-line trajectory of a robotic arm based on a five-segment S-shaped velocity curve, characterized in that: The following steps are involved: 1) Modeling the velocity: The velocity is divided into five processes, namely: acceleration with increasing acceleration, acceleration with decreasing acceleration, uniform motion, deceleration with increasing acceleration, and deceleration with decreasing acceleration. The velocity is then decomposed into the x, y, and z directions of the robot base coordinate system. The time of the above five motions is the same. First, a model of acceleration with respect to time is established in the x, y, and z directions, as shown in equations (1-1), (1-2), and (1-3): Among them, j x (t), j y (t), j z (t) represents the function of acceleration in the x direction, the function of acceleration in the y direction, and the function of acceleration in the z direction respectively; t and t1 represent the time variable and the time taken for the accelerated motion with increasing acceleration respectively; J x , J y , J z They represent the jerk corresponding to the time t in the x direction, the jerk corresponding to the time t in the y direction, and the jerk corresponding to the time t in the z direction, respectively. Then, the jerk model functions established in the x, y, and z directions are integrated to obtain the model functions of the acceleration in the x, y, and z directions with respect to time, as shown in equations (1-4), (1-5), and (1-6): Among them, a x (t), a y (t), a z (t) represents the function of acceleration in the x direction with respect to time, the function of acceleration in the y direction with respect to time, and the function of acceleration in the z direction with respect to time, respectively. Then, the acceleration model functions in the x, y, and z directions are integrated, and finally the functions of velocity in the x, y, and z directions with respect to time are obtained, as shown in equations (1-7), (1-8), and (1-9): Among them, v x (t), v y (t), v z (t) represents the function of velocity in the x direction, the function of velocity in the y direction, and the function of velocity in the z direction with respect to time, respectively; 2) Solve the position function: Integrate the velocity model established in step 1) to obtain the position function of five segments in the x, y, and z directions, which are: the position function of the acceleration phase with increasing acceleration, the position function of the acceleration phase with decreasing acceleration, the position function of the uniform speed phase, the position function of the deceleration phase with increasing acceleration, and the position function of the deceleration phase with decreasing acceleration, as shown in equations (1-10), (1-11), and (1-12): Wherein, x(t), y(t), and z(t) represent the function of position in the x direction with respect to time, the function of position in the y direction with respect to time, and the function of position in the z direction with respect to time, respectively; x0, y0, and z0 represent the initial position in the x direction, the initial position in the y direction, and the initial position in the z direction, respectively; 3) Add constraints: Add constraints to the function of position in the x, y, and z directions with respect to time obtained in step 2). The constraint condition is that the target position must be reached when the velocity decays to 0. The position constraints in the x, y, and z directions are as follows: Substitute equation (1-13) into (1-10), (1-11), and (1-12) to obtain the acceleration J in the x, y, and z directions: x , J y , J z , as shown in formula (1-14): where x tar ,y tar 、z tar Respectively represent the x-coordinate of the target point, the y-coordinate of the target point, and the z-coordinate of the target point; 4) Design controller: Design a robot controller based on PD control, calculate the torque and send it to the robot control cabinet, control each joint of the robot to reach the specified position, and the robot controller designed based on PD control is shown in formula (1-15): Where τ represents the torque of the robot arm; J represents the Jacobian matrix; K p Represents the proportional gain of PD control and represents the stiffness matrix of the robot in robot control; K d It represents the differential control of PD control and the damping matrix of the robot in robot control; q d represents the expected position; q represents the actual position; Indicates the expected speed; Indicates actual speed.

Images