Robot kinetic parameter servo current filtering method and device

Abstract

The invention discloses a robot kinetic parameter servo current filtering method. The method comprises the following steps: establishing a joint robot kinetic equation; running the robot by adopting the optimized excitation track, and collecting first parameters of all joints of the robot; obtaining kinetic parameters and establishing a state equation and an observation equation; acquiring a second parameter of the robot; taking the acceleration in the second parameter as a state vector state equation, and taking the calculation moment as an observation equation; establishing an observation matrix according to the observation equation; generating an estimation covariance matrix and a Kalman gain matrix; calculating a K moment filtering current and updating the estimation covariance matrix; the robot is controlled to operate stably. According to the robot kinetic parameter servo current filtering method and device, the servo current is filtered by means of nonlinear Kalman filtering, the current change caused by coupling motion between joint shafts is solved, the collected servo current of each joint shaft is more real, the robot runs more stably, vibration is suppressed, and the running precision of the robot is improved.

Description

Technical Field

[0001] The invention relates to the field of robot control, and in particular to a robot dynamics parameter servo current filtering method and device. Background Art

[0002] In robot-assisted surgery, precise control of the robot and its joints has been a problem that has plagued the industry for many years. Existing control technologies usually use general digital filtering methods, such as first-order filtering or low-pass filtering, to achieve control of 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, which is not conducive to robot motion control and is prone to robot vibration and noise. The control accuracy also needs to be improved.

[0003] Based on the shortcomings of the existing technology, it is necessary to develop a robot dynamics parameter servo current filtering method and device with higher control accuracy. Summary of the invention

[0004] The present invention provides a robot dynamics parameter servo current filtering method and device:

[0005] In a first aspect, the present invention provides a robot dynamics parameter servo current filtering method, comprising the following steps:

[0006] Step 101: Establish the dynamic equation of the joint robot;

[0007] Step 102: operating the robot using the optimized excitation trajectory, and collecting first parameters of each joint of the robot, wherein the first parameters include a specified position, speed, acceleration, and torque;

[0008] Step 103: using the least square method to identify kinetic parameters;

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

[0010] Step 105: collecting second parameters of the robot, wherein the second parameters include joint position, velocity, acceleration and joint current;

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

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

[0013] Step 108: Establish an observation matrix according to the observation equation;

[0014] Step 109: Generate an estimated covariance matrix according to the state equation and the observation equation;

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

[0016] Step 111: Calculate the filter current at time k according to 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 filter current to further control the robot to run smoothly.

[0019] Furthermore, the kinetic equation is expressed by the following formula 1:

[0020]

[0021] Where: τ∈R n is the control torque input vector; τ fi ∈R n is the joint friction torque vector; are the centrifugal force and Coriolis force vectors; G(q)∈R n is the gravity vector; M(q)∈R n is the mass matrix; q∈R n is the joint position vector;

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

[0023]

[0024] Where τ∈R n Control torque input vector; is the identification matrix or observation matrix;

[0025] θ s ∈R 12n is the inertia parameter;

[0026] The inertia parameters and joint friction coefficient of each connecting rod are shown in Formula 3:

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

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

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

[0030]

[0031] Among them is the angular velocity of the joint, is the sign control function;

[0032]

[0033] The inertia parameter of formula 2 is simplified to the minimum concentrated inertia parameter, and formula 6 is obtained:

[0034]

[0035] Furthermore, the least square method for kinetic parameter identification adopts formula 7;

[0036]

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

[0038] Furthermore, the Kalman filter processing is performed on the first parameter to obtain the dynamic parameter: conduct Jacobian matrix processing:

[0039]

[0040] Where F1 is the calculation torque equation of joint 1, F n is the calculation torque equation of joint n, where n is a positive integer; further, 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 to 10:

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

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

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

[0044] Furthermore, the estimated covariance matrix adopts formula 11:

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

[0046] where Q is the process covariance matrix.

[0047] Furthermore, the Kalman gain matrix K k Using formula 12;

[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}Use formula 13 to collect:

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

[0051] Among them I k ={I1, ...I n} is the set of servo currents of n joints at time k, N t = {N t1 , ...N tn} is the torque coefficient of each joint; I′ k-1 ={I′1,...I′ n} is the filtered current set at the previous moment.

[0052] Furthermore, the updated estimated covariance matrix P k Using formula 14,

[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, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the robot dynamic parameter servo current filtering method described in any one of the first aspects are implemented.

[0056] The robot dynamic parameter servo current filtering method and device of the present invention utilize the fusion of robot dynamic parameters and kinematic parameters, and use the nonlinear Kalman filtering method to filter the servo current of each axis, thereby solving the current change caused by the coupled motion between the joint axes. The collected servo current of each joint axis is more realistic, effectively controlling the smooth operation of the robot, improving the robot's operation accuracy, and suppressing the robot's vibration. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 : Flowchart of robot dynamic parameter servo current filtering method.

[0058] Figure 2 : Schematic diagram of the robot dynamic parameter servo current filter device. DETAILED DESCRIPTION

[0059] In order to make the technical problems, technical solutions and beneficial effects to be solved by the present invention more clearly understood, the present invention is further described in detail below in conjunction with 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 used to limit the present invention.

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

[0061] Step 101: Establish and linearize the dynamic equation of the joint robot. For a serial robot with N degrees of freedom, where N is a positive integer, its rigid body dynamic equation is expressed by the following formula 1 when friction is considered:

[0062]

[0063] Where: τ∈R n is the collected joint torque; τ fi ∈R n is the joint friction torque vector;

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

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

[0066] Further linear simplification of the kinetic equation can obtain the following formula 2:

[0067]

[0068] Where τ∈R n is the control torque input vector; is the identification matrix or observation matrix; θ s ∈R 12n is the inertia parameter;

[0069] The inertia parameters and joint friction coefficient of each connecting rod are 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, fc are the terms of joint friction, {I xx ,I yy ,I zz} is the moment of inertia, {I xy ,I xz ,i yz} is the inertia tensor, {Mx,My,Mz} is the center of mass moment, and Formula 4 is the modeling equation for friction:

[0072]

[0073] Among them is the angular velocity of the joint, is the sign control function;

[0074]

[0075] The inertia parameter of formula 2 is simplified to the minimum concentrated inertia parameter, and formula 6 is obtained:

[0076]

[0077] in is a coefficient matrix consisting of joint position, velocity, and acceleration. is the minimum parameter set; is the moment equation;

[0078] Step 102: operating the robot using the optimized excitation trajectory, and collecting first parameters of each joint of the robot, wherein the first parameters include a specified position, speed, acceleration, and torque;

[0079] Step 103: using the least square method to identify kinetic parameters;

[0080]

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

[0082] Step 104: Obtain dynamic parameters and establish state equations and observation equations, specifically: perform Kalman filtering on the first parameter to obtain dynamic parameters, and conduct

[0083] Jacobian matrix processing:

[0084]

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

[0086] Step 105: collecting second parameters of the robot, wherein the second parameters include joint position, velocity, acceleration and joint current;

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

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

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

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

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

[0092] Step 108: Establish an observation matrix according to the observation equation;

[0093] Step 109: Generate an estimated covariance matrix based on the state equation and observation equation of 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 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 filtered current I′ at time k based on the Kalman gain matrix of step 110 and the joint current of 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] Among them I k ={I1, ...I n} is the set of servo currents of n joints at time k, N t = {N t1 , ...N tn} is the torque coefficient of each joint; I′ k-1 ={I′1,...I′ n} is the current set after filtering at 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 filter current to further control the robot to run smoothly.

[0105] refer to Figure 2Schematic diagram of a robot dynamic parameter servo current filter device. The robot dynamic parameter servo current filter 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 are configured to be executed by the one or more processors 30, and the processor 30 implements the steps of the robot dynamic parameter servo current filtering method when executing the computer program.

[0106] A person skilled in the art may understand that all or part of the steps in the various methods of the above embodiments may be completed by instructing related hardware through a program, and the program may be stored in a computer-readable storage medium, and the storage medium may include: a read-only memory (ROM), a random access memory (RAM), a disk or an 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 in the protection scope of the present invention.

Claims

1. A robot dynamic parameter servo current filtering method, characterized in that: The following steps are involved: Step 101: Establish the dynamic equation of the joint robot; Step 102: operating the robot using the optimized excitation trajectory, and collecting first parameters of each joint of the robot, wherein the first parameters include a specified position, speed, acceleration, and torque; Step 103: using the least square method to identify kinetic parameters; Step 104: Obtain kinetic parameters and establish state equations and observation equations; Step 105: collecting second parameters of the robot, wherein the second parameters include joint position, velocity, acceleration and joint current; Step 106: Obtain the joint current in the second parameter; Step 107: Using the acceleration in the second parameter as the state vector state equation, and calculating the torque as the observation equation; Step 108: Establish an observation matrix according to the observation equation; Step 109: Generate an estimated covariance matrix according to the state equation and the observation equation; Step 110: Generate a Kalman gain matrix; Step 111: Calculate the momentary filter current according to the joint current and the Kalman gain matrix; Step 112: Update the estimated covariance matrix; Step 113: Adjust the robot control and servo system parameters according to the filter current to further control the robot to run smoothly.

2. The robot dynamics parameter servo current filtering method according to claim 1, characterized in that: The kinetic equation is expressed by the following formula 1: Where: τ∈R n is the control torque input vector; τ fi ∈R n is the joint friction torque vector; are the centrifugal force and Coriolis force vectors; G(q)∈R n is the gravity vector; M(q)∈R n is the mass matrix; q∈R n is the joint position vector; Further linear simplification of the kinetic equation yields Formula 2: Where τ∈R n Control torque input vector; is the identification matrix or observation matrix; θ s ∈R 12n is the inertia parameter; The inertia parameters and joint friction coefficient of each connecting rod are shown in Formula 3: θ s = [I xx , I xy , I xz , I yy , I yz , I zz , Mx, My, Mz, M, fv, fc[, Formula 3; Where fv, fc are the terms of joint friction, {I xx ,I yy ,I zz } is the moment of inertia, {I xy ,I xz ,I yz } is the inertia tensor, {Mx,My,Mz} is the center-of-mass moment; Formula 4 is the modeling equation for friction: Among them is the angular velocity of the joint, is the sign control function; The inertia parameter of formula 2 is simplified to the minimum concentrated inertia parameter, and formula 6 is obtained:

3. The robot dynamic parameter servo current filtering method according to claim 1, characterized in that: The least square method for kinetic parameter identification adopts formula 7; in It is a matrix composed of joint position, velocity, and acceleration, and τ is the collected joint torque.

4. The robot dynamics parameter servo current filtering method according to claim 1, characterized in that: The Kalman filter processing is performed on the first parameter to obtain the dynamic parameter: conduct Jacobian matrix processing: Where F1 is the calculation torque equation of joint 1, F n is the torque equation for joint n, where n is a positive integer.

5. The robot dynamics parameter servo current filtering method according to claim 1, characterized in that: The acceleration in the second parameter is used as the state vector state equation, and the calculated torque is used as the observation equation and is specifically performed using formulas 9 to 10: X k =A*X k-1 +W, formula 9; τ = H * X k + R, Equation 10; in is the position, velocity, and acceleration data of each joint at time k, A is the state matrix, W is the standard deviation of the process, H is the measurement matrix, and R is the measurement variance.

6. The robot dynamics parameter servo current filtering method according to claim 1, characterized in that: The estimated covariance matrix uses formula 11: P k-1 = A * P k-1 * A T + Q, Formula 11; Where Q is the process covariance matrix.

7. The robot dynamics parameter servo current filtering method according to claim 1, characterized in that: The Kalman gain matrix K k Use formula 12; K k = P k-1 * H T *(H * P k-1 * H T + R) -1 , Formula 12 8. The robot dynamics parameter servo current filtering method according to claim 1, characterized in that: The filter current I' k ={I′1,...I′ n }Use formula 13 to collect: I′ k = I′ k-1 + K k (I k - H * X k * N t ),Formula 13; Among them I k ={I1, ...I n } is the set of servo currents of n joints at time k, N t = {N t1 , ...N tn } is the torque coefficient of each joint; I′ k-1 ={I′1,...I′ n } is the filtered current set at the previous moment.

9. The robot dynamics parameter servo current filtering method according to claim 1, characterized in that: The updated estimated covariance matrix P k Using formula 14, P k = (E - K k * H) * P k-1 , Formula 14; Where E is the identity matrix.

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

11. A robot dynamic parameter servo current filter device, 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 are configured to be executed by the one or more processors, characterized in that the processor implements the steps of the robot dynamic parameter servo current filtering method as described in any one of claims 1 to 9 when executing the computer program.

Images