A time-optimal trajectory planning method based on flexible joint model

Abstract

The present invention discloses a time-optimal trajectory planning method based on a flexible joint model; the method is as follows: 1: According to the target path of the robot end effector, obtain the path endpoint sequence A; 2: According to the path endpoint sequence A, obtain the joint path sequence B; 3: Construct the objective function. 4: Obtain the time-optimal trajectory sequence C; 5: Interpolate the time-optimal trajectory sequence C to obtain the robot control sequence D; 6: Each joint of the robot is controlled according to the robot control sequence D, so that the end effector of the robot moves along the target trajectory. In view of the fact that the joints of the light robot have control lag due to the synchronous belt transmission mechanism and the harmonic reducer, the present invention constructs a dynamic model of the robot's flexible joints, and solves the optimal trajectory of each joint based on the dynamic model, thereby obtaining the time-optimal trajectory of the light robot.

Description

Technical Field

[0001] The present invention belongs to the field of robot trajectory planning, and specifically relates to a time optimal trajectory planning method based on a flexible joint model, which establishes a new flexible joint dynamics model, which considers the influence of the flexible factors in the robot joints on the motion, and is more in line with the actual motion law of the light robot. At the same time, the Sequential Quadratic Programming (SQP) method is used to find the time optimal trajectory of the robot. Background Art

[0002] With the development of science and technology and the progress of manufacturing industry, there are more and more occasions that require collaboration between robots and humans, and direct contact between humans and robots is becoming more and more frequent. However, the original industrial robots have simple control, single functions, and are bulky and dangerous. They are obviously not competent for this job, so light robots came into being.

[0003] Compared with industrial robots, light robots have many improvements, but this amplifies the impact of flexibility factors in robot joints on movement. First of all, light robots need to work flexibly to perform the functions of human hands, which requires the support of a variety of sensors, including torque sensors. The principle of torque sensors is to detect torque through the deformation of strain beams; at the same time, most light robots use harmonic reducers with flexible wheels. These all increase the flexibility of joints. In addition, flexibility also increases the buffering of robots when they collide, playing a role in protecting people and robots. For the above reasons, the angular displacement of the motor and the angular displacement of the connecting rod driven by the light robot are not the same when it moves. Therefore, when establishing the robot dynamics model, the elastic deformation caused by the torque sensor and the harmonic reducer is abstracted as a spring, with the robot connecting rod and the motor connected on both sides.

[0004] However, when designing the optimal trajectory planning of robots, the dynamic model that regards the robot as a pure rigid body structure is still used. The trajectory obtained in this way is not only not the optimal trajectory of the robot, but the robot's end effector will vibrate due to the influence of the flexible joints during movement, making the trajectory uneven and possibly causing the trajectory to deform. Summary of the invention

[0005] The purpose of this invention is to propose a new flexible joint dynamics model; and with time optimization as the indicator requirement, a joint space trajectory planning method based on the SQP algorithm is proposed to calculate the time optimal position point, and then obtain the robot time-position control sequence through the quintic B-spline interpolation method. This sequence allows the robot to run in a shorter time, with a smoother speed curve and less vibration.

[0006] A time-optimal trajectory planning method based on a flexible joint model comprises the following steps:

[0007] Step 1: Divide the target path of the robot end effector into multiple parts, and finally obtain a path endpoint sequence A = [a1, a2, ..., a N ] T ; a i is the coordinate of the i-th path endpoint; i = 1, 2, ..., N. N is the number of path endpoints.

[0008] Step 2: Obtain the joint path sequence B = [b1, b2, ..., b N ] T , where b i =[q i，1 ,q i，2 ..., q i，n ] T ,q i，j is the angular displacement of the jth joint corresponding to the i-th path endpoint. j = 1, 2, ..., n. n is the number of joints of the robot.

[0009] Step 3: Construct constraint relationships.

[0010] 3-1. A flexible joint dynamics model is used to describe each joint of the robot; in the flexible joint dynamics model, the output shaft of the servo motor (1) and the connecting rod (3) are connected via a flexible joint (2); the flexible joint (2) enables elastic rotation between the output shaft of the servo motor (1) and the connecting rod (3).

[0011] The equality constraints based on the flexible joint dynamics model are as follows:

[0012]

[0013] Among them, M l is the connecting rod mass matrix; M m is the motor rotor mass matrix; θ is the motor rotation angular displacement; q is the connecting rod angular displacement; K(θ-q) is the torque of the flexible joint; is the Coriolis force; g(q) is the gravity term; τ ext is the torque of the external force on each joint; τ m is the motor output torque; is the angular displacement of the link at the beginning of the robot motion, is the angular displacement of the connecting rod after the robot moves. T is the total time of the path operation.

[0014] 3-2. Construct the inequality constraints as follows:

[0015]

[0016] Among them, τ m_min , τm_max are the lower and upper limits of the motor output torque respectively; q min q max are the lower and upper limits of the connecting rod angular displacement respectively; are the lower and upper limits of the motor angular velocity respectively; τ e_min , τ e_max They are the lower limit and upper limit of the flexible torque respectively.

[0017] Step 4: Construct an optimization problem based on the equality constraints and inequality constraints obtained in step 3, and find the optimal solution. The time-optimal trajectory sequence C = [c1, c2, ..., c N ] T .

[0018] Step 5: Interpolate the time optimal trajectory sequence C obtained in step 4 to obtain the robot control sequence D.

[0019] Step 6: The joints of the robot are controlled according to the robot control sequence D so that the end effector of the robot moves along the target trajectory.

[0020] Preferably, in step 2, the process of obtaining the joint path sequence B is: establishing the DH parameter table of the robot, and then converting the Cartesian space coordinates of the robot end effector into joint space coordinates through robot inverse kinematics, thereby converting the path endpoint sequence A into the joint path sequence B.

[0021] Preferably, after step 2 is executed, necessary data is sent to the host computer, including joint path sequence B, upper and lower limits of motor torque of each joint, upper and lower limits of connecting rod rotation angle, upper and lower limits of motor speed and upper and lower limits of flexible torque.

[0022] As a preferred embodiment, the specific process of step 4 is as follows: constructing the optimization problem is as follows:

[0023]

[0024]

[0025] Where l is the number of equality constraints; m is the number of inequality constraints; is the gradient operator; h i (x k ) is the i-th equality constraint of the optimization problem; g i (x k ) is the i-th inequality constraint of the optimization problem. k is the search direction of the kth iteration. k is the Lagrangian equation of the kth iteration. For a given Lagrangian operator μ, λ, we have:

[0026] The iterative steps to solve this optimization problem are: 1) Given an initial point x 0 , set the convergence accuracy ε, let L 0 =I; I is the unit matrix; 2) The original problem is solved at the iteration point x k It is simplified into a quadratic programming problem; 3) Solve the above quadratic programming problem and obtain the search direction d k , get the next iteration point x k+1 =x k +d k ; 4) Determine whether the iteration termination condition is met. If the condition is met, x k+1 As the optimal solution, f(x k+1 ) as the optimal value of the objective function, terminate the calculation; otherwise, go to step 5); 5) calculate the new Lagrangian function L k+1 , proceed to step 2).

[0027] Preferably, in step 5, the time optimal trajectory sequence C is interpolated using a k-order B-spline curve; the value of k is 3 or 5. The specific process of interpolation is as follows:

[0028] The expression of k-order B-spline curve is:

[0029] Where, d i is the control vertex of the curve; N i，k (u) represents the basis function of k-order B-spline:

[0030]

[0031] In the formula, k represents the order of the B-spline, i represents the number of the control vertices of the B-spline curve, and N i，k The interval of (u) is u∈[u i ,u i+k+1 ], whose node vector U∈[u0,u1,...,u n+2k ].

[0032] Update the expression of B-spline curve to:

[0033]

[0034] The cumulative chord length parameterization method is used to calculate u k+1 ,u k+2 , ..., u n+k-1 Normalization:

[0035]

[0036] After k times of B-spline interpolation, the robot control sequence D = [d1, d2, ..., dN ] T .

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

[0038] The present invention combines the characteristics of control lag at the joints of the lightweight robot due to the synchronous belt transmission mechanism and the harmonic reducer, constructs a dynamic model of the robot's flexible joints, and solves the optimal trajectory of each joint based on the dynamic model, thereby obtaining the time optimal trajectory of the lightweight robot; in addition, the sequential quadratic programming method adopted in the present invention performs well in solving nonlinear optimization problems, thereby improving the accuracy of the time optimal trajectory obtained in this application. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 It is a flow chart of trajectory planning of the present invention;

[0040] Figure 2 A transmission system diagram of a dynamic model of a single joint of the robot in the present invention;

[0041] Figure 3 This is a flowchart of the iterative solution in step 4 of the present invention.

[0042] Reference numerals: Figure 2 In the figure, 1 is the servo motor, 2 is the flexible joint, and 3 is the connecting rod. DETAILED DESCRIPTION

[0043] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings.

[0044] In this embodiment, a small load joint robot SR1-540 developed by Guangzhou Qifan Industrial Robot Company is used as the controlled robot. The components of the SR1-540 robot mainly include a robot body, a control system and a drive system. In one execution command, the robot needs to complete a straight line trajectory.

[0045] A time-optimal trajectory planning method based on a flexible joint model comprises the following steps:

[0046] Step 1: Get a target path for the robot end effector. The SQP optimization algorithm is suitable for discrete algorithms, so the path needs to be discretized. The discretization method is to divide the path into multiple parts, and finally obtain a path endpoint sequence A = [a1, a2, ..., a N ] T , where a i =[x i ,y i , z i ] T , xi ,y i , z i are the x-axis, y-axis, and z-axis coordinate values ​​in the robot base coordinate system. N is the number of discrete path endpoints. The path can be obtained by manually moving the robot in the teaching mode or can be given directly. In this embodiment, the value of N is 100; a1, a2, ..., a N It is obtained by averaging the target path.

[0047] Step 2: Create the robot's DH parameter table, and then use the robot inverse kinematics to convert the robot's end effector Cartesian space coordinates into joint space coordinates, thereby converting the Cartesian path sequence into a joint space sequence. For a robot with n degrees of freedom, the joint path sequence B = [b1, b2, ..., b N ] T , where b i =[q i，1 ,q i，2 ..., q i，n ] T ,q i，j is the angular displacement of the jth joint corresponding to the i-th path endpoint. j=1, 2, ..., n; n is the number of joints of the robot; the SR1-540 robot is a 6-DOF robot, so n=6 in this embodiment.

[0048] The DH parameter table of the SR1-540 robot is shown in Table 1.

[0049] Table 1

[0050]

[0051] Step 3: Send the necessary data to the host computer. The necessary data includes the joint path sequence B, the upper and lower limits of the motor torque of each joint, the upper and lower limits of the connecting rod rotation angle, the upper and lower limits of the motor speed, and the upper and lower limits of the flexible torque. In this embodiment, the algorithm solution is completed by MATLAB 2021b on the computer. In the user manual of the SR1-540 robot, various robot parameters are provided, including the connecting rod mass M = [3.36, 6.80, 5.22, 3.12, 2.30, 0.66] T , unit kg; the inertia matrix of the connecting rod I = [I1, I2, ..., I6] T ; Connecting rod angle lower limit q min =[-145, -10, -34, -220, -120, -360] T , upper limit of corner position q max =[120, 175, 218, 220, 200, 360] T, in degrees; the maximum output torque τ of the motor m_max =[85,195,80,50,56,17] T , in Newton meters; the maximum speed of the motor V max =[125, 160, 384, 125, 251, 480] T , in degrees per second.

[0052] A time optimization problem is established based on the above parameters.

[0053] Establishing an optimal problem includes three aspects: objective function, equality constraint and inequality constraint. The present invention solves the time optimal problem, so the objective function is Where T is the total path running time.

[0054] 1) Equality constraint: The equality constraint is a dynamic model of the robot. The present invention provides a flexible joint dynamic model, which takes into account the influence of the flexible factor in the robot joint. Figure 2 The figure is a model of a flexible joint transmission system. The output shaft of the servo motor 1 and the connecting rod 3 of the flexible joint transmission system are connected through a flexible joint 2; the flexible joint 2 allows the output shaft of the servo motor 1 and the connecting rod 3 to rotate elastically, so as to better fit the actual working state of the robot joint.

[0055] Set the relevant parameters as follows: M m is the motor rotor mass matrix, θ is the angular displacement of the motor output to the flexible joint (i.e., the output angular displacement of the motor after deceleration by the reducer), K is the joint stiffness, M l is the mass matrix of the connecting rod, and q is the angular displacement of the connecting rod. The difference between the angular displacement q of the connecting rod and the angular displacement θ output by the motor to the flexible joint is the relative angular displacement caused by the elastic deformation of the transmission structure between the motor and the connecting rod;

[0056] The specific derivation formula is as follows:

[0057] To derive the dynamic equation using the Euler-Lagrange equation, it is necessary to find the expression of the kinetic potential energy of the robot system.

[0058] The kinetic energy expression of the connecting rod is: In the formula, is the first-order derivative of the connecting rod angular displacement q (i.e. angular velocity).

[0059] The absolute speed of the motor rotor i is: Where V mi is the absolute speed of the motor rotor of the i-th joint relative to the base coordinate system, J mi (q) is the velocity Jacobian matrix from the i-th joint to the robot base coordinate system, T iis the homogeneous transformation matrix from the i-th joint to the robot base coordinate system, z i is the homogeneous coordinate of the axis of the motor rotor of the i-th joint in the i-th joint coordinate system.

[0060] Therefore, the kinetic energy expression of the motor rotor is: Where M mi is the mass matrix of the motor rotor at the i-th joint.

[0061] The above formula can be expressed in matrix form as follows: Where J is the rotational mass matrix of the motor rotor after deceleration; S(q) is the mass matrix of the coupling part, which is a strictly upper triangular matrix and is only related to the reduction ratio and the rotational inertia of the motor rotor.

[0062] In summary, the complete kinetic energy equation of the flexible joint robot is:

[0063] The potential energy equation is expressed as: Where K(θ-q) is the torque of the flexible joint.

[0064] Substituting the above kinetic potential energy expression into the Euler-Lagrange equation, we can get:

[0065] where g(q) is the gravity term, is the Coriolis force, τ ext is the torque of the external force on each joint, τ m The output torque of the motor.

[0066] The existence of the coupling term S(q) in the complete model increases the complexity of the dynamic model. However, in most cases, the moment of inertia of the motor rotor is much smaller than that of the connecting rod, so the part of the rotor translational kinetic energy affected by the connecting rod movement can be completely ignored. At this time, let S(q) = 0. The simplified robot dynamic equation is as follows:

[0067]

[0068] The above equation is the equality constraint of the optimization problem. In addition, the robot's speed is 0 when starting and stopping, and it should reach the specified position when stopping, so there is also an equality constraint: Where T is the total time of trajectory operation, is the angular displacement of the link at the beginning of the robot motion, is the angular displacement of the connecting rod after the robot motion is completed.

[0069] 2) Inequality constraints. In order to enable the robot to move quickly and smoothly, and also protect the motor from overload, the present invention designs motor torque constraints, connecting rod rotation angle constraints, motor speed constraints and flexible torque constraints: Among them, τ m_min , τ m_max are the lower and upper limits of the motor output torque respectively; q min ,q max are the lower and upper limits of the connecting rod angular displacement respectively; are the lower and upper limits of the motor angular velocity respectively; τ e_min , τ e_max They are the lower limit and upper limit of the flexible torque respectively.

[0070] Step 4: Find the optimal solution to the problem.

[0071] The connecting rod angular displacement q is taken as the object to be optimized and as the iteration point; for an optimization problem:

[0072]

[0073] Use Taylor expansion to convert the objective function of the above problem at the iteration point x k After simplifying the constraints into linear functions, we get the following quadratic programming problem:

[0074]

[0075] Wherein, l is the number of equality constraints; in this embodiment, l=5; m is the number of inequality constraints; in this embodiment, m=8. i (x k ) represents the i-th equality constraint; g i (x k ) represents the i-th inequality constraint; d k is the search direction of the kth iteration. k is the Lagrangian equation of the kth iteration. For a given Lagrangian operator μ, λ, we have:

[0076] Solving this quadratic programming problem, we can get the next search direction d of the original problem k , then x k+1 =x k +d k .

[0077] In summary, the iterative steps of the sequential quadratic programming algorithm are: 1) Given an initial point x 0 , set the convergence accuracy ε, let L 0 =I; I is the unit matrix; 2) The original problem is solved at the iteration point x kIt is simplified into a quadratic programming problem; 3) Solve the above quadratic programming problem and obtain the search direction d k , get the next iteration point x k+1 ; 4) Determine whether the iteration termination condition is met. If the condition is met, x k+1 As the optimal solution, f(x k+1 ) as the optimal value of the objective function, terminate the calculation; otherwise, go to step 5); 5) calculate the new Lagrangian function L k+1 , proceed to step 2).

[0078] For the specific flow chart, see Figure 3 Finally, we get the time-optimal trajectory sequence C = [c1, c2, ..., c N ] T .

[0079] Step 5: Use the quintic B-spline curve to interpolate the time optimal trajectory sequence C. B-spline has high computational efficiency and good enough smoothness. Due to the local controllability and convex hull properties of B-spline curve, trajectory optimization can be performed.

[0080] The expression of k-order B-spline curve is:

[0081] Where, d i (i=0, 1, ..., n) is the control vertex of the curve, N i，k (u), (i=0, 1, ..., n) represents the basis function of k-order B-spline:

[0082]

[0083] In the formula, k represents the order of the B-spline, i represents the number of the control vertices of the B-spline curve, and N i，k The interval of (u) is u∈[u i ,u i+k+1 ], whose node vector U∈[u0,u1,...,u n+2k ].

[0084] B-spline curves have local support. i ,u i+k+1 ], there are at most k+1 non-zero basis functions N r，k (u)(r=ik,i-k+1,...,i). Therefore, the B-spline curve can also be expressed as:

[0085]

[0086] In order for the k-order B-spline curve to pass through the starting point and the end point, the condition u0=u1=...=u k =0,u n+k=u n+k+1 =...=u n+2k = 1. The cumulative chord length parameterization method is used to calculate u k+1 ,u k+2 , ..., u n+k-1 Normalization:

[0087]

[0088] After five B-spline interpolations, the robot control sequence D = [d1, d2, ...] T The number of control nodes in the robot control sequence D is the total number of nodes after interpolation.

[0089] Step 6: Send the control sequence D to the robot controller to control the robot to complete the specified trajectory.

Claims

1. A time-optimal trajectory planning method based on a flexible joint model, characterized by: Step 1: Divide the target path of the robot end effector into multiple parts, and finally obtain a path endpoint sequence A = [a1, a2, ..., a N ] T ; a i is the coordinate of the i-th path endpoint; i=1,2,...,N; N is the number of path endpoints; Step 2: Obtain the joint path sequence B = [b1, b2, ..., b N ] T , where b i =[q i,1 ,q i,2 …,q i,n ] T ,q i,j is the angular displacement of the jth joint corresponding to the i-th path endpoint; j = 1, 2, ..., n; n is the number of joints of the robot; Step 3: Construct constraint relationships; 3-1. A flexible joint dynamics model is used to describe each joint of the robot; in the flexible joint dynamics model, the output shaft of the servo motor (1) and the connecting rod (3) are connected via a flexible joint (2); the flexible joint (2) enables elastic rotation between the output shaft of the servo motor (1) and the connecting rod (3); The equality constraints based on the flexible joint dynamics model are as follows: Among them, M l is the connecting rod mass matrix; M m is the motor rotor mass matrix; θ is the motor rotation angular displacement; q is the connecting rod angular displacement; K(θ-q) is the torque of the flexible joint; is the Coriolis force; g(q) is the gravity term; τ ext is the torque of the external force on each joint; τ m is the motor output torque; is the angular displacement of the link at the beginning of the robot motion, is the angular displacement of the connecting rod after the robot moves; T is the total time of the path operation; 3-2. Construct the inequality constraints as follows: Among them, τ m_min , τ m_max are the lower and upper limits of the motor output torque respectively; q min q max are the lower and upper limits of the connecting rod angular displacement respectively; are the lower and upper limits of the motor angular velocity respectively; τ e_min , τ e_max They are the lower and upper limits of the flexible torque respectively; Step 4: Construct an optimization problem based on the equality constraints and inequality constraints obtained in step 3, and find the optimal solution. The time optimal trajectory sequence C = [c1, c2, ..., c N ] T ; Step 5: Interpolate the time optimal trajectory sequence C obtained in step 4 to obtain the robot control sequence D; Step 6: The joints of the robot are controlled according to the robot control sequence D so that the end effector of the robot moves along the target trajectory.

2. The time-optimal trajectory planning method based on a flexible joint model according to claim 1, characterized in that: In step 2, the process of obtaining the joint path sequence B is: establishing the DH parameter table of the robot, and then converting the Cartesian space coordinates of the robot end effector into joint space coordinates through the robot inverse kinematics, thereby converting the path endpoint sequence A into the joint path sequence B.

3. The time-optimal trajectory planning method based on a flexible joint model according to claim 1, characterized in that: After step 2 is executed, the necessary data is sent to the host computer; the necessary data includes the joint path sequence B, the upper and lower limits of the motor torque of each joint, the upper and lower limits of the connecting rod rotation angle, the upper and lower limits of the motor speed, and the upper and lower limits of the flexible torque.

4. The time-optimal trajectory planning method based on a flexible joint model according to claim 1, characterized in that: The specific process of step 4 is: construct the optimization problem as follows: Where l is the number of equality constraints; m is the number of inequality constraints; is the gradient operator; h i (x k ) is the i-th equality constraint of the optimization problem; g i (x k ) is the i-th inequality constraint of the optimization problem; d k is the search direction of the kth iteration; L k is the Lagrangian equation of the kth iteration; for a given Lagrangian operator μ, λ, we have: The iterative steps to solve this optimization problem are: 1) Given an initial point x 0 , set the convergence accuracy ε, let L 0 =I; I is the unit matrix; 2) Solve the original problem at iteration point x k It is simplified into a quadratic programming problem; 3) Solve the above quadratic programming problem and obtain the search direction d k , get the next iteration point x k+1 =x k +d k ; 4) Determine whether the iteration termination condition is met. If the condition is met, x k+1 As the optimal solution, f(x k+1 ) as the optimal value of the objective function, terminate the calculation; otherwise, go to step 5); 5) calculate the new Lagrangian function L k+1 , proceed to step 2).

5. The time-optimal trajectory planning method based on a flexible joint model according to claim 1, characterized in that: In step 5, the k-order B-spline curve is used to interpolate the time optimal trajectory sequence C; the value of k is 3 or 5; the specific process of interpolation is as follows: The expression of k-order B-spline curve is: Where, d i is the control vertex of the curve; N i,k (u) represents the basis function of k-order B-spline: In the formula, k represents the order of the B-spline, i represents the number of the control vertices of the B-spline curve, and N i,k The interval of (u) is u∈[u i ,u i+k+1 ], whose node vector U∈[u0,u1,…,u n+2k ]; Update the expression of B-spline curve to: The cumulative chord length parameterization method is used to calculate u k+1 ,u k+2 ,…,u n+k-1 Normalization: After k times of B-spline interpolation, the robot control sequence D = [d1, d2, ..., d N ] T .

Images