A robot dynamics parameter servo current filtering method and device

Abstract

This invention discloses a servo current filtering method for robot dynamic parameters, comprising the following steps: establishing the dynamic equations of an articulated robot; running the robot using an optimized excitation trajectory and collecting the first parameters of each joint; acquiring the dynamic parameters and establishing state equations and observation equations; collecting the second parameters of the robot; using the acceleration in the second parameters as the state vector in the state equation, and calculating the torque as the observation equation; establishing the observation matrix based on the observation equation; generating the estimated covariance matrix and Kalman gain matrix; calculating the filtered current at time K and updating the estimated covariance matrix; and controlling the robot to run smoothly. The servo current filtering method and device of this invention, using nonlinear Kalman filtering to filter the servo current, solves the current changes caused by the coupled motion between joint axes, resulting in more realistic servo currents for each joint axis, smoother robot operation, reduced vibration, and improved robot running accuracy.

Description

Technical Field

[0001] This invention relates to the field of robot control, specifically to a method and apparatus for filtering servo current for robot dynamic parameters. Background Technology

[0002] Precise control of the robot and its joints in robot-assisted surgery has been a problem that has plagued the industry for many years. Existing control technologies usually use common digital filtering methods, such as first-order filtering or low-pass filtering, to control the robot and its joints. This control method is essentially a robot servo current filtering method. During the control process, the current changes caused by the coupling between the robot's motion axes are not considered, resulting in large current distortion after filtering. This is not conducive to robot motion control, and it is easy to generate robot vibration and noise. The control accuracy also needs to be improved.

[0003] Given the shortcomings of existing technologies, it is necessary to develop a servo current filtering method and device for robot dynamic parameters with higher control precision. Summary of the Invention

[0004] This invention provides a method and apparatus for filtering servo current of robot dynamic parameters:

[0005] In a first aspect, the present invention provides a method for filtering servo current of robot dynamic parameters, comprising the following steps:

[0006] Step 101: Establish the dynamic equations of the articulated robot;

[0007] Step 102: Run the robot using an optimized excitation trajectory and collect the first parameters of each joint of the robot. The first parameters include the specified position, velocity, acceleration and torque.

[0008] Step 103: Identify dynamic parameters using the least squares method;

[0009] Step 104: Obtain dynamic parameters and establish state equations and observation equations;

[0010] Step 105: Collect the robot's second parameters, which include joint position, velocity, acceleration, and joint current;

[0011] Step 106: Obtain the joint current from the second parameter;

[0012] Step 107: Use the acceleration in the second parameter as the state vector and the state equation, and calculate the torque as the observation equation;

[0013] Step 108: Establish the observation matrix based on the observation equation;

[0014] Step 109: Generate the estimated covariance matrix based on the state equation and the observation equation;

[0015] Step 110: Generate the Kalman gain matrix;

[0016] Step 111: Calculate the filter current at time k based on the joint current and the Kalman gain matrix;

[0017] Step 112: Update the estimated covariance matrix;

[0018] Step 113: Adjust the robot control and servo system parameters according to the filtered current to further control the robot to run smoothly.

[0019] Furthermore, the dynamic equation is expressed by the following Equation 1:

[0020]

[0021] In the formula: τ∈R n It is the control torque input vector; τ fi ∈R n It is the torque vector of joint friction force; It is the vector of centrifugal force and Coriolis force; G(q)∈R n It is the gravity vector; M(q)∈R n It is the mass matrix; q∈R n It is a joint position vector;

[0022] Further linear simplification of the dynamic equation yields Formula 2:

[0023]

[0024] In the formula, τ∈R n Control torque input vector; It is either an identification matrix or an observation matrix;

[0025] θ s ∈R 12n It is an inertial parameter;

[0026] The inertial parameters and joint friction coefficients of each link are composed as shown in Formula 3:

[0027] θ s =[I xx ,I xy ,I xz ,I yy ,I yz ,I zz Formula 3;

[0028] Where fv and fc are terms of joint friction, {I xx ,Iyy ,I zz} is the moment of inertia, {I xy ,I xz ,I yz {Mx, My, Mz} is the inertial tensor, and {Mx, My, Mz} is the moment of mass.

[0029] Formula 4 is the modeling equation for friction:

[0030]

[0031] Among them Let be the angular velocity of the joint. It is a symbolic control function;

[0032]

[0033] By simplifying the inertial parameters in Equation 2 into the minimum lumped inertial parameters, we obtain Equation 6:

[0034]

[0035] Furthermore, the least squares method for identifying dynamic parameters uses Formula 7;

[0036]

[0037] In the formula It is a matrix composed of joint position, velocity, and acceleration, where τ is the collected joint torque.

[0038] Furthermore, the step of performing Kalman filtering on the first parameter to obtain the dynamic parameters specifically involves: applying formula 6... conduct Jacobian matrix processing:

[0039]

[0040] Where F1 is the calculated torque equation for joint 1, F n The calculation torque equation for joint n is given, where n is a positive integer; furthermore, the acceleration in the second parameter is used as the state vector state equation, and the calculated torque is used as the observation equation, specifically using formulas 9-10:

[0041] X k =A*X k-1 +W, formula 9;

[0042] τ=H*X k +R, Formula 10;

[0043] in Let A be the position, velocity, and acceleration data of each joint at time k, W be the state matrix, H be the standard deviation of the process, and R be the measurement matrix.

[0044] Furthermore, the estimated covariance matrix is ​​obtained using Formula 11:

[0045] P k-1 =A*P k-1 *A T +Q, formula 11;

[0046] Wherein is the covariance matrix of the Q process.

[0047] Furthermore, the Kalman gain matrix K k Formula 12 is used;

[0048] K k =P k-1 *H T *(H*P k-1 *H T +R) -1 , Formula 12;

[0049] Furthermore, the filter current I′ k ={I′1,…I′ n Data was collected using formula 13:

[0050] I' k =I′ k-1 +K k (I k -H*X k *N t ), formula 13;

[0051] Where I k ={I1, ... I n Let} be the set of n joint servo currents at time k, and N t ={N t1 , ...N tn} represents the torque coefficient of each joint; I′ k-1 ={I′1,...I′ n} represents the filtered current set from the previous moment.

[0052] Furthermore, the updated estimated covariance matrix P k Formula 14 is used.

[0053] P k =(EK) k *H)*P k-1 , Formula 14;

[0054] Where E is the identity matrix.

[0055] In a second aspect, the present invention provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the robot dynamics parameter servo current filtering method described in any one of the first aspects.

[0056] The robot dynamics parameter servo current filtering method and device of the present invention utilizes the fusion of robot dynamics parameters and kinematic parameters, and uses nonlinear Kalman filtering to filter the servo current of each axis. This solves the problem of current changes caused by coupled motion between joint axes, and the collected servo current of each joint axis is more realistic, effectively controlling the robot to run smoothly, improving the robot's running accuracy, and suppressing robot vibration. Attached Figure Description

[0057] Figure 1 Flowchart of servo current filtering method for robot dynamic parameters.

[0058] Figure 2 Schematic diagram of servo current filtering device for robot dynamic parameters. Detailed Implementation

[0059] To make the technical problems to be solved, the technical solutions, and the beneficial effects of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0060] refer to Figure 1 Flowchart of servo current filtering method for robot dynamic parameters.

[0061] Step 101: Establish and linearize the dynamic equations of the articulated robot. For a serial robot with N degrees of freedom, where N is a positive integer, its rigid body dynamic equations, considering friction, are expressed by the following Equation 1:

[0062]

[0063] In the formula: τ∈R n It collects joint torque; τ fi ∈R n It is the torque vector of joint friction force;

[0064] It is the vector of centrifugal force and Coriolis force; G(q)∈R n It is the gravity vector; M(q)∈R n It is quality

[0065] Quantity matrix; q∈R n It is a joint position vector;

[0066] Further linear simplification of the dynamic equations yields the following formula 2:

[0067]

[0068] In the formula, τ∈R n It is the control torque input vector; It is the identification matrix or observation matrix; θ s ∈R 12n It is an inertial parameter;

[0069] The inertial parameters and joint friction coefficients of each link are composed as shown in Formula 3:

[0070] θ s =[I xx ,I xy ,I xz ,I yy ,I yz ,I zz [Mx,My,Mz,M,fv,fc] (Formula 3)

[0071] Where fv and fc are terms of joint friction, {I xx ,I yy ,I zz} is the moment of inertia, {I xy ,I xz i yz {Mx, My, Mz} is the inertia tensor, {Mx, My, Mz} is the moment of mass, and Equation 4 is the modeling equation for friction:

[0072]

[0073] Among them Let be the angular velocity of the joint. It is a symbolic control function;

[0074]

[0075] By simplifying the inertial parameters in Equation 2 into the minimum lumped inertial parameters, we obtain Equation 6:

[0076]

[0077] in It is a coefficient matrix composed of joint position, velocity, and acceleration. It is the minimum set of parameters; It is the equation for calculating torque;

[0078] Step 102: Run the robot using an optimized excitation trajectory and collect the first parameters of each joint of the robot. The first parameters include the specified position, velocity, acceleration and torque.

[0079] Step 103: Identify dynamic parameters using the least squares method;

[0080]

[0081] In the formula It is a matrix composed of joint position, velocity, and acceleration, where τ is the collected joint torque;

[0082] Step 104: Obtain dynamic parameters and establish state and observation equations. Specifically, perform Kalman filtering on the first parameter to obtain the dynamic parameters, and then apply equation 6... conduct

[0083] Jacobian matrix processing:

[0084]

[0085] Where F1 is the calculated torque equation for joint 1, F n Here is the equation for the calculated torque of joint n, where n is a positive integer;

[0086] Step 105: Collect the robot's second parameters, which include joint position, velocity, acceleration, and joint current;

[0087] Step 106: Obtain the joint current from the second parameter;

[0088] Step 107: Obtain the real-time joint position, velocity, and acceleration from the second parameter. Use the acceleration from the second parameter as the state vector state equation, and the calculated torque as the observation equation, as shown in Equations 9-10:

[0089] X k =A*X k-1 +W (Formula 9)

[0090] τ=H*X k +R (Formula 10)

[0091] in Let A be the position, velocity, and acceleration data of each joint at time k, W be the state matrix, H be the standard deviation of the process, R be the measurement matrix, and R be the measurement variance.

[0092] Step 108: Establish the observation matrix based on the observation equation;

[0093] Step 109: Generate the estimated covariance matrix based on the state equation and observation equation from Step 104:

[0094] P k-1 =A*P k-1 *AT +Q (Formula 11)

[0095] Where Q is the process covariance matrix;

[0096] Step 110: Generate the Kalman gain matrix K k ;

[0097] K k =P k-1 *H T *(H*P k-1 *H T +R) -1 (Formula 12)

[0098] Step 111: Calculate the filter current I′ at time k based on the Kalman gain matrix from step 110 and the joint current from step 106. k ={I′1,...I′ n}:

[0099] I' k =I′ k-1 +K k (I k -H*X k *N t ) (Formula 13)

[0100] Where I k ={I1, ... I n Let} be the set of n joint servo currents at time k, and N t ={N t1 , ...N tn} represents the torque coefficient of each joint; I′ k-1 ={I′1,...I′ n} represents the filtered current set from the previous moment;

[0101] Step 112: Update the estimated covariance matrix P k ,

[0102] P k =(EK) k *H)*P k-1 (Formula 14)

[0103] Where E is the identity matrix.

[0104] Step 113: Adjust the robot control and servo system parameters according to the filtered current to further control the robot to run smoothly.

[0105] refer to Figure 2A schematic diagram of a robot dynamics parameter servo current filtering device. The robot dynamics parameter servo current filtering device 10 of the present invention further includes one or more memories 20 and one or more processors 30, wherein the one or more computer programs are stored in the memories 20 and configured to be executed by the one or more processors 30, and the processors 30 implement the steps of the robot dynamics parameter servo current filtering method when executing the computer programs.

[0106] Those skilled in the art will understand that all or part of the steps in the various methods of the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, which may include: read-only memory (ROM), random access memory (RAM), disk or optical disk, etc.

[0107] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for filtering servo current for robot dynamic parameters, characterized in that, Includes the following steps: Step 101: Establish the dynamic equations of the articulated robot; Step 102: Run the robot using an optimized excitation trajectory and collect the first parameters of each joint of the robot. The first parameters include the specified position, velocity, acceleration and torque. Step 103: Identify dynamic parameters using the least squares method; Step 104: Obtain dynamic parameters and establish state equations and observation equations; Step 105: Collect the robot's second parameters, which include joint position, velocity, acceleration, and joint current; Step 106: Obtain the joint current from the second parameter; Step 107: Use the acceleration in the second parameter as the state vector and the state equation, and calculate the torque as the observation equation; Step 108: Establish the observation matrix based on the observation equation; Step 109: Generate the estimated covariance matrix based on the state equation and the observation equation; Step 110: Generate the Kalman gain matrix; Step 111: Calculate based on joint current and Kalman gain matrix The filter current at any time, the filter current Data was collected using formula 13: , Official 13; in For time k Each joint servo current set, This represents the torque coefficient for each joint; This is the filtered current set from the previous moment; Step 112: Update the estimated covariance matrix; Step 113: Adjust the robot control and servo system parameters according to the filtered current to further control the robot to run smoothly.

2. The robot dynamics parameter servo current filtering method as described in claim 1, characterized in that, The dynamic equation is expressed by the following formula 1: Official 1; In the formula: It is the control torque input vector; It is the torque vector of joint friction force; It is the vector matrix of centrifugal force and Coriolis force; It is the gravity vector; It is a quality matrix. It is the angular acceleration of the joint; It is a joint position vector; Further linear simplification of the dynamic equation yields Formula 2: , formula 2; In the formula; It is an identification matrix or an observation matrix; It is an inertial parameter; The inertial parameters and joint friction coefficients of each link are composed as shown in Formula 3: Formula 3; in It is a term related to joint friction. It is the moment of inertia, { It is the inertial tensor, { } is the centroid moment. It is mass; Equation 4 is the modeling equation for friction: , Official 4; Among them Let be the angular velocity of the joint. It is a symbolic control function; , Official 5; By simplifying the inertial parameters in Equation 2 into the minimum lumped inertial parameters, we obtain Equation 6: Formula 6; It is a matrix composed of joint positions, velocities, and accelerations. It is the minimum lumped inertial parameter. It is the functional equation for the control torque input vector.

3. The robot dynamics parameter servo current filtering method as described in claim 2, characterized in that, The least squares method is used to identify dynamic parameters using formula 7; Official 7.

4. The robot dynamics parameter servo current filtering method as described in claim 2, characterized in that, The first parameter is processed by Kalman filtering to obtain the dynamic parameters, specifically: Equation 6... conduct Jacobian matrix processing: Official 8; in The equation for calculating the torque of joint 1 is as follows: Let n be the equation for calculating the torque of joint n, where n is a positive integer.

5. The robot dynamics parameter servo current filtering method as described in claim 1, characterized in that, The acceleration in the second parameter is used as the state vector in the state equation, and the calculated torque is used as the observation equation, specifically using formulas 9-10: Official 9; Formula 10; in for The position, velocity, and acceleration data of each joint at any given time. The state matrix, The standard deviation of the process. For the measurement matrix, To measure variance.

6. The robot dynamics parameter servo current filtering method as described in claim 5, characterized in that, The estimated covariance matrix is ​​obtained using Formula 11: Official 11; in The process covariance matrix, the yes The covariance matrix is ​​updated at each time step.

7. The robot dynamics parameter servo current filtering method as described in claim 6, characterized in that, The Kalman gain matrix Formula 12 is used: , Formula 12.

8. The robot dynamics parameter servo current filtering method as described in claim 7, characterized in that, The updated estimated covariance matrix Formula 14 is used. Formula 14; where E is the identity matrix.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the robot dynamics parameter servo current filtering method as described in any one of claims 1 to 8.

10. A servo current filtering device for robot dynamic parameters, comprising: One or more processors; Memory; as well as One or more computer programs, wherein the one or more computer programs are stored in the memory and configured to be executed by the one or more processors, characterized in that, when the processor executes the computer program, it implements the steps of the robot dynamics parameter servo current filtering method as described in any one of claims 1 to 8.

Images