Robot time optimal speed planning method

Abstract

The invention relates to a robot time optimal speed planning method comprising the following steps: giving a determined Cartesian space robot end motion path, and realizing path parameterization and discretization through a path parameter s; a kinematics model and a dynamics model of the robot are established, a more accurate dynamics model is obtained through dynamics parameter identification, and therefore joint speed, moment and jerk constraints are introduced to guarantee the feasibility of motion; relocating the constraints in the joint space to Cartesian space using a Jacobian matrix and its derivative; the reduction of the motion time is taken as an optimization target, a phase plane graph of path parameters is solved by adopting nonlinear optimization iteration, and speed planning with optimal time is carried out according to correlation calculation of a Jacobian matrix. The method is high in universality, the working time of the robot can be effectively shortened, the smooth and stable joint value, speed and acceleration curve of the robot are obtained, the working efficiency is improved, meanwhile, the service life of the robot is prolonged, and the working precision of the robot is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot trajectory planning, and specifically to a method for time-optimal speed planning of a robot. Background Art

[0002] Robots are widely used in industrial assembly lines and manufacturing systems. Due to the increasing workload of operation tasks and continuous improvement of production requirements, the motion performance of robot systems is expected to reach higher requirements. Time-optimal speed planning is a common way to improve the productivity of robot systems. It enables the robot to achieve smooth and time-optimal motion along a specified path while respecting physical constraints. Currently, the more commonly used robot speed planning algorithms include dynamic programming (DP), numerical integration (NI), and convex optimization (CO). The DP-based method is easy to implement, does not encounter local minimum problems, and it traverses all states of each path point. However, the state space to be searched is very large, resulting in slow implementation. In addition, due to the problem of grid accuracy, the DP method cannot truly achieve the global optimal point. The NI-based method is very fast, but it is difficult to accurately find the switching point between the acceleration and deceleration phases, so it usually cannot achieve the time-optimal effect. The CO-based method is easy to implement and very robust, and it can consider multiple optimization objectives other than time. However, the optimization problem solved by CO is very large, and the number of variables and constraint inequalities increases with the increase of the discretization grid size, resulting in an implementation speed one order of magnitude slower than that of the NI-based method. The time-optimal speed planning of a robot is a highly coupled non-linear problem. In addition to the above methods, there are also many intelligent optimization algorithms, such as genetic algorithms, particle swarm algorithms, and simulated annealing algorithms, etc. However, the current research on the time-optimal speed planning problem of robots is generally carried out in the joint space, which cannot visualize the end position of the robot, and the solution efficiency is low, making it difficult to complete work tasks with clear requirements for the end path of the robot, such as arc welding, grinding, etc.; and most of the research also does not consider the third-order constraint, and the acceleration is prone to sudden changes, which to a certain extent affects the working life and working accuracy of the robot. Summary of the Invention

[0003] In view of the above deficiencies in the prior art, the present invention designs a method for time-optimal speed planning of a robot. Given a determined end-effector motion path of the robot in Cartesian space, path parameterization and discretization are achieved through the path parameter s. The kinematic and dynamic models of the robot are established, and a more accurate dynamic model is obtained through dynamic parameter identification, thereby introducing joint velocity, torque, and jerk constraints to ensure the feasibility of motion. The Jacobian matrix and its derivative are used to relocate the constraints in the joint space to the Cartesian space. With the goal of minimizing the motion time, a non-linear optimization is used to iteratively solve the phase plane diagram of the path parameters, and time-optimal speed planning is carried out based on the relevant calculations of the Jacobian matrix. This method has strong versatility, can effectively reduce the working time of the robot, and obtain smooth and stable joint value, velocity, and acceleration curves of the robot, improving the service life and working accuracy of the robot while enhancing the working efficiency.

[0004] The technical solution adopted by the present invention to achieve the above object is: a method for time-optimal speed planning of a robot, comprising the following steps:

[0005] S1. Given a known piecewise C 2 continuous geometric path P, perform parameterization and discretization processing on the geometric path;

[0006] S2. Input the initial motion state and target motion state of the robot, as well as the motion constraint conditions;

[0007] S3. Establish a dynamic model of the robot including friction and motor rotor rotation;

[0008] S4. Estimate the dynamic parameters based on the dynamic parameter identification method to obtain the minimum parameter set, calculate the final theoretical torque value, and obtain an accurate dynamic model;

[0009] S5. Combine the accurate dynamic model of the robot and the trajectory curve of the robot end-effector to establish torque constraints;

[0010] S6. Obtain kinematic constraints, including the maximum velocity constraint and maximum jerk constraint of each joint of the robot;

[0011] S7. Use the Jacobian matrix to establish a transformation matrix between the Cartesian space and the joint space, and transform the robot torque constraints and kinematic constraints into path parameter constraints in the Cartesian space to obtain a model between the path parameters and joint information;

[0012] S8. Based on the robot torque and kinematic constraints, establish a time-optimal speed planning model of the robot;

[0013] S9. Based on the optimal velocity planning model and the constraint conditions, use a non - linear optimization algorithm to iteratively solve the phase plane of the path parameters, path parameter velocity, and path parameter acceleration in the Cartesian space to obtain the optimal path parameter velocity that satisfies the constraints

[0014] S10. Reverse - transform the obtained Cartesian - space path parameter information to the joint space through the torque and kinematic path parameter constraint equations to obtain the time - optimal joint velocity when the robot end - effector moves along the given geometric path P.

[0015] The parameterization and discretization of the geometric path include: introducing the path parameter s as a monotonically increasing scalar function of time t, s:[0,T]→[0,s end , and the geometric path can be expressed as P(s(t)), t∈[0,1]. Divide the interval [0,s end into N equal - length segments at equal intervals, with N + 1 grid points s0, s1, …, s N .

[0016] The motion state and the target motion state include the initial velocity, initial acceleration, target velocity, and target acceleration.

[0017] The motion constraint conditions include the maximum velocity, maximum torque, and maximum jerk.

[0018] The dynamic model is:

[0019] τ represents the joint torque; θ, and represent the joint value, joint velocity, and joint acceleration respectively; M(θ) represents the joint - space inertia matrix; C(θ) represents the Coriolis and centripetal coupling matrix; G(θ) represents the gravity load; f v , f c represent the viscous friction and Coulomb friction coefficients respectively; I a represents the motor - rotor moment of inertia.

[0020] The dynamic parameter identification includes:

[0021] 1) Linearize the above - mentioned dynamic model;

[0022] 2) Use the QR decomposition method to reorganize the dynamic parameters to obtain the minimum regression matrix and the generalized minimum parameter set;

[0023] 3) Design the excitation trajectory;

[0024] 4) Collect the joint information during the robot's motion;

[0025] 5) Estimate the dynamic parameters of the robot based on the least squares method to obtain the minimum parameter set, calculate the final theoretical torque value, and obtain an accurate dynamic model.

[0026] The maximum speed constraint is obtained based on the speed limit of the robot's drive motor; the maximum acceleration constraint is obtained based on the torque of the robot's joint motor and the change rate of the robot's acceleration.

[0027] The model between the path parameters and the joint information is:

[0028]

[0029]

[0030]

[0031] Among them, θ, and respectively represent the joint value, joint speed, joint acceleration, and joint jerk; J represents the Jacobian matrix; P′, P″, and P″′ respectively represent the first-order derivative, second-order derivative, and third-order derivative of the geometric path P with respect to the path parameter s; and respectively represent the path parameter speed, path parameter acceleration, and path parameter jerk.

[0032] The objective function is:

[0033] Among them, s is the path parameter, which is a monotonically increasing scalar function of time t, s: [0, T] → [0, s end ; is the path parameter speed.

[0034] The optimal joint speed is used to control the robot's movement to achieve the shortest movement time of the robot when moving along a given path.

[0035] The present invention has the following beneficial effects and advantages:

[0036] Aiming at the problems that time-optimal planning in the field of robot trajectory planning is generally carried out in the joint space and does not consider the third-order constraints, resulting in uncertain end trajectories of the robot and easy mutations in acceleration, the present invention designs a robot time-optimal speed planning method. This method has strong versatility, has wide application value, can effectively reduce the working time of the robot, and improve the service life and working accuracy of the robot. Brief Description of the Drawings

[0037] Figure 1 is the flowchart of the time-optimal speed planning method of the present invention;

[0038] Figure 2 This is the flow chart for identifying the kinetic parameters of the present invention. Specific embodiments

[0039] To make the above objects, features and advantages of the present invention more obvious and understandable, the following further details the specific implementation methods of the present invention with reference to the accompanying drawings. Many specific details are set forth in the following description in order to fully understand the present invention, but the present invention can be implemented in many other ways different from those described herein. Those skilled in the art can make similar improvements without departing from the connotation of the invention. Therefore, the present invention is not limited by the specific implementations disclosed below.

[0040] In view of the problems in the field of robot trajectory planning that the time-optimal planning is generally carried out in the joint space and does not consider the third-order constraints, resulting in uncertain end-effector trajectories of the robot and easy acceleration mutations, etc., the present invention designs a robot time-optimal velocity planning method. Given a path in the Cartesian space, with the goal of minimizing the motion time, the robot torque and kinematic constraints are established. By introducing path parameters and the Jacobian matrix, the constraints in the joint space are transformed into the Cartesian space. The phase plane diagram of the path parameters is solved by non-linear optimization iteration, and the time-optimal velocity planning is carried out according to the relevant calculations of the Jacobian matrix. This method has strong versatility, can effectively reduce the working time of the robot, and improve the service life and working accuracy of the robot.

[0041] Reference Figure 1 As shown, the present invention provides a robot time-optimal velocity planning method:

[0042] S1. Input a known piecewise continuous geometric path P in the Cartesian space C, and perform parameterization and discretization processing on the geometric path to obtain a concise curve equation and simplify the optimization problem. It includes: introducing a path parameter s which is a monotonically increasing scalar function of time t, s: [0, T] → [0, s 2 , and the geometric path can be expressed as P(s(t)), t ∈ [0, 1]. The interval [0, s end is equally divided into N segments, with N + 1 grid points s0, s1, …, s end ; N ;

[0043] S2. Input the initial motion state and target motion state of the robot, including the initial velocity, initial acceleration, target velocity, target acceleration, and motion constraint conditions, including the maximum velocity, maximum torque, and maximum jerk;

[0044] S3. Establish a robot dynamic model including friction and motor rotor rotation as:

[0045]

[0046] τ represents the joint torque; θ, and respectively represent the joint value, joint velocity, and joint acceleration; M(θ) represents the joint space inertia matrix; C(θ) represents the Coriolis and centripetal coupling matrix; G(θ) represents the gravity load; f v 、f c respectively represent the viscous friction and Coulomb friction coefficients; I a represents the moment of inertia of the motor rotor.

[0047] S4. Dynamic parameter identification, including:

[0048] 1) Linearize the above dynamic model, and represent the joint torque τ in the form of the product of the regression matrix and the basic dynamic parameter set P;

[0049] 2) Recombine the dynamic parameters using the QR decomposition method to obtain the minimum regression matrix and the generalized minimum parameter set P min ;

[0050] 3) Design the excitation trajectory using the finite-term Fourier series equation, where N represents the number of harmonics; a il and b il represent the amplitudes; the fundamental frequency ω f = 2πf f , f f is the trajectory running frequency, and the fundamental frequencies of all joints are the same; q i0 is the constant term;

[0051] 4) Collect the joint information during the robot's movement, including the joint value, joint velocity, joint acceleration, and joint torque. The actual regression matrix H′ W can be determined through the collected joint value, joint velocity, and joint acceleration;

[0052] 5) Given the actual regression matrix H′ W and the actual joint torque τ, estimate the dynamic parameters of the robot based on the least squares method to obtain the minimum parameter set, and then calculate the final theoretical torque value through the equation in step 2 above to obtain an accurate dynamic model. The specific process is as Figure 2 shown;

[0053] S5. Combine the accurate dynamic model of the robot and the trajectory curve at the end of the robot to establish the joint torque constraint of the robot, that is, the torque limit value that can be achieved during the joint movement;

[0054] S6. Obtain the maximum speed constraints of each joint of the robot according to the speed limit of the robot drive motor. Based on the torque of the robot joint motor and considering the change rate of the robot acceleration, obtain the maximum jerk constraints of each joint of the robot, which are the kinematic constraints and are described using a threshold range.

[0055] S7. Use the Jacobian matrix to establish a transformation matrix between the Cartesian space and the joint space, and convert the robot torque constraints and kinematic constraints into path parameter constraints in the Cartesian space. The relationship between the path parameters and the joint information is as follows:

[0056]

[0057]

[0058]

[0059] where, θ, and represent the joint value, joint velocity, joint acceleration, and joint jerk respectively; J represents the Jacobian matrix; P′, P″, and P″′ represent the first, second, and third differentials of the geometric path P with respect to the path parameter s respectively; and represent the path parameter velocity, path parameter acceleration, and path parameter jerk respectively.

[0060] S8. Based on the robot torque and kinematic constraints, establish a time-optimal velocity planning model for the robot. The objective function is:

[0061]

[0062] where, s is the path parameter, which is a monotonically increasing scalar function of time t, s: [0, T] → [0, s end ; is the path parameter velocity;

[0063] S9. Based on the optimal velocity planning model, through the input constraints, use a non-linear optimization algorithm to iteratively solve the phase plane curve of the path parameter, path parameter velocity, and path parameter acceleration in the Cartesian space, and obtain the optimal path parameter velocity s that satisfies the constraints.

[0064] S10. Convert the obtained Cartesian space path parameter information to the joint space through the torque and kinematic path parameter constraint equations, and obtain the time-optimal joint velocity when the robot end moves along the given geometric path P.

[0065] Finally, it should be noted that the above is the preferred embodiment of the present invention. It should be pointed out that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should be regarded as the protection scope of the present invention.

Claims

1. A method for optimal time speed planning of a robot, characterized in that, It includes the following steps: S1. Given a known piecewise C continuous geometric path P in Cartesian space, perform parameterization and discretization on the geometric path; 2 ​ S2. Input the initial motion state and target motion state of the robot, as well as the motion constraint conditions; S3. Establish a robot dynamics model that includes friction and the rotation of the motor rotor; S4. Estimate the dynamic parameters based on the dynamic parameter identification method to obtain the minimum parameter set, calculate the final theoretical torque value, and obtain an accurate dynamics model; S5. Combine the accurate dynamics model of the robot and the trajectory curve at the end of the robot to establish torque constraints; S6. Obtain kinematic constraints, including the maximum speed constraint and maximum jerk constraint of each joint of the robot; S7. Use the Jacobian matrix to establish a transformation matrix between the Cartesian space and the joint space, and convert the robot torque constraints and kinematic constraints into path parameter constraints in the Cartesian space to obtain a model between the path parameters and joint information; S8. Establish a time-optimal velocity planning model for the robot based on the robot torque and kinematic constraints; S9. Based on the optimal velocity planning model and the constraint conditions, use a non-linear optimization algorithm to iteratively solve the phase plane of the path parameters, the path parameter velocity, and the path parameter acceleration in the Cartesian space curve to obtain the optimal path parameter velocity that satisfies the constraints S10. Inversely transform the obtained Cartesian space path parameter information through the torque and kinematic path parameter constraint equations to the joint space to obtain the time-optimal joint velocity when the end of the robot moves along the given geometric path P.

2. The method for optimal time velocity planning of a robot according to claim 1, wherein The parameterization and discretization processing of the geometric path includes: introducing a path parameter s, which is a monotonically increasing scalar function of time t, s: [0, T] → [0, s end , and the geometric path can be expressed as P(s(t)), t ∈ [0, 1]. The interval [0, s end is equally divided into N segments, with N + 1 grid points s0, s1, …, s N .

3. A method for optimal time speed planning of a robot according to claim 1, characterized in that, The motion state and target motion state include the initial velocity, initial acceleration, target velocity, and target acceleration.

4. A method for optimal time speed planning of a robot according to claim 1, characterized in that, The motion constraint conditions include the maximum speed, maximum torque, and maximum jerk.

5. A method for optimal time velocity planning of a robot according to claim 1, characterized in that, The kinetic model is as follows: τ represents the joint torque; θ, and represent the joint value, joint velocity, and joint acceleration, respectively; M(θ) represents the joint space inertia matrix; C(θ) represents the Coriolis and centripetal coupling matrix; G(θ) represents the gravity load; f v and f c represent the viscous friction coefficient and the Coulomb friction coefficient respectively; I a represents the moment of inertia of the motor rotor.

6. A method for optimal time velocity planning of a robot according to claim 1, characterized in that The dynamic parameter identification includes: 1) Linearize the above dynamics model; 2) Recombine the dynamic parameters using the QR decomposition method to obtain the minimum regression matrix and the generalized minimum parameter set; 3) Design an excitation trajectory; 4) Collect the joint information during the robot's motion; 5) Estimate the dynamic parameters of the robot based on the least squares method to obtain the minimum parameter set, calculate the final theoretical torque value, and obtain an accurate dynamics model.

7. A method for optimal time velocity planning of a robot according to claim 1, characterized in that, The maximum speed constraint is obtained based on the speed limit of the robot drive motor; the maximum acceleration constraint is obtained based on the torque of the robot joint motor and the change rate of the robot acceleration.

8. A method for optimal time speed planning of a robot according to claim 1, characterized in that, The model between the path parameters and joint information is: Among them, θ, and respectively represent joint value, joint velocity, joint acceleration, and joint jerk; J represents the Jacobian matrix; P′, P″, and P″′ respectively represent the first, second, and third differentials of the geometric path P with respect to the path parameter s; and respectively represent path parameter velocity, path parameter acceleration, and path parameter jerk.

9. A method for optimal time speed planning of a robot according to claim 1, characterized in that The objective function is as follows: where s is a path parameter, which is a monotonically increasing scalar function of time t, s: [0, T] → [0, s end ; is the path parameter speed.

10. The method for optimal time velocity planning of a robot according to claim 1, characterized in that, The optimal joint velocity is used to control the robot's motion to achieve the shortest robot motion time when moving along the given path.

Images