CN116619365A - Optimal excitation and high-precision identification method for coordinated dynamics of composite robot arm - Google Patents

Abstract

The application belongs to the field of robot model identification, and discloses a method for optimal excitation and high-precision identification of coordination dynamics of a composite robot arm. The method can realize the vehicle-arm coordinated full-state dynamics linearization modeling and optimal synchronous excitation, and meanwhile, high-precision robust identification of dynamics model parameters is realized for measurement noise and abnormal data, and finally, the accuracy and physical consistency of composite robot model identification are effectively improved.

Description

Optimal excitation and high-precision identification method for coordinated dynamics of composite robot arm

technical field

The invention belongs to the field of robot model identification, and in particular relates to an optimal excitation and high-precision identification method for the coordinated dynamics of a composite robot arm.

Background technique

The dynamic model of the composite robot is an important basis for its motion control algorithm development and physical consistency simulation. It is also used to design disturbance observers and virtual force/torque sensors to improve the response performance of the control system. It can be seen that the dynamic model is accurate. The performance directly affects the working efficiency and precision of the composite robot. However, it is difficult to accurately obtain the dynamic model parameters of composite robots in actual engineering. For example, direct measurement methods cannot determine the inertia parameters of complex structural parts and ignore joint friction characteristics, and CAD software cannot effectively calculate dynamics due to simplified models such as motors/reducers/cables. learning parameters. Therefore, it is the only effective way to identify the dynamic parameters of the composite robot through experimental excitation, and it has important engineering significance and practical value.

The robot dynamics model identification technology is based on the experimental excitation data to minimize the deviation between the theoretical model output and the actual system output to estimate the model parameters. A lot of achievements have been made in parameter solving and other aspects, but less involved in the dynamic model identification of composite robots. It is difficult to effectively consider the influence of joint friction in the existing robot dynamic model construction methods, such as Coulomb friction, viscous friction, static friction, etc. will affect the accuracy of dynamic modeling; at the same time, the singularity of the dynamic linearization regression matrix will also affect the model For the accuracy of parameter identification, methods such as minimum inertia parameter set construction, ridge regression analysis, and orthogonal triangular decomposition have been proposed to solve the above problems. In addition, the effective design of the excitation trajectory to fully reflect the motion characteristics of the composite robot and suppress the external interference and measurement noise in the experiment is a prerequisite for accurately identifying the parameters of its dynamic model. Most existing methods use the overall condition number of the robot dynamic regression matrix As an optimization index to solve the optimal parameters of the excitation trajectory, similar condition numbers can also lead to different excitation effects or violate the physical characteristics of the system, and it is impossible to realize the synchronous excitation of the composite robot vehicle-arm coordinated motion. However, the existing related methods for solving the parameters of robot dynamic models, such as quasi-Newton method, Kalman filter, least squares regression, maximum likelihood estimation, particle swarm optimization, etc., rarely consider the abnormal data and noise in the experimental process. Adverse effects on identification accuracy. Therefore, there are still many challenges in the current dynamics identification technology of composite robots in terms of full-state linear modeling, vehicle-arm coordinated motion excitation trajectory design, and high-precision solution of dynamic parameters.

Contents of the invention

The purpose of the present invention is to provide an optimal excitation and high-precision identification method for the coordination dynamics of the composite robot arm, so as to solve the above-mentioned technical problems.

In order to solve the above-mentioned technical problems, the specific technical scheme of the optimal excitation and high-precision identification method of the coordinated dynamics of the composite robot arm of the present invention is as follows:

The optimal excitation and high-precision identification method for the coordinated dynamics of the composite robot arm includes the following steps:

S1: Based on the Newton-Euler recursive equation to design the linearized dynamic model of the coordinated motion of the composite robot arm, through

The non-singular linearized regression equation is obtained through QR decomposition, and the full-state dynamics modeling is realized;

S2: Construct an optimization functional based on the weighted condition number of the regression matrix of the linearized dynamic model of the compound robot and its dynamic sub-matrix, and use the finite term Fourier series to design the optimal synchronous excitation trajectory of the coordinated dynamics of the car arm, and the hoist

The excitation effect of the dynamic characteristics of the robot and the physical consistency of the model parameters;

S3: Based on the regularization of the measurement noise covariance matrix and the weighted suppression of abnormal data, an adaptive weighted iterative least squares solution algorithm for the parameters of the vehicle arm coordination dynamics model is designed, and a test trajectory is designed to verify the validity of the parameter identification results to realize the composite robot dynamics High-precision robust identification of learning models.

Further, the step 1 includes constructing a linearized dynamic model of each link of the manipulator of the composite robot based on the Newton-Euler recurrence equation, the formula is as follows

In formula (1) Represents the driving force required by the joint i when the link i moves alone /> and torque

and/> represent the linear acceleration and gravitational acceleration of joint i respectively, /> Represents the angular velocity of joint i, m _i represents the mass of link i, /> represents the center of mass of connecting rod i, /> Represents the moment of inertia vector of link i relative to its link coordinate system, β _i = [I _ai , F _ci , F _si , F _vi ] ^T and I _ai , F _ci , F _si , F _vi represent the rotation of joint i respectively Inertia, Coulomb, static and viscous friction coefficients,

Represents the friction torque observation matrix of joint i, where ω _s >0 represents the angular velocity threshold of static friction; operator /> Represent the anti-symmetric matrix of the operation vector and the left multiplication matrix of the moment of inertia; /> Represents the linearized dynamic regression matrix of connecting rod i, Φ _i =[m _i ,mic _{i ,} I _i ,β _i ] ^T represents the unknown inertia parameter of connecting rod i.

Further, the step 1 includes constructing a linearized dynamic model of the omnidirectional mobile platform of the composite robot, the formula is as follows

In formula (2) Indicates the Cartesian space coordinates of the mobile platform, /> Indicates the driving torque of the mobile platform mecanum wheel set, /> represents the transformation matrix of driving torque, m _O and I _O represent the mass and moment of inertia of the mobile platform respectively, β _O = [F _Ovx , F _Ovy , F _Ovz , F _Ocx , F _Ocy , F _Ocz ] ^T represents the movement of the mobile platform along the translation axis The viscous friction coefficient and the Coulomb friction coefficient in the direction of x _O , y _O and the rotation axis z _O , where the operator /> and /> Represents the friction moment observation matrix of the mobile platform; /> Φ _O ＝[m _O , I _O , β _O ] ^T represents the linearized dynamic regression matrix and unknown inertia parameters of the mobile platform, respectively.

Further, the step 1 includes designing a full-state linearized dynamic model of the compound robot car-arm coordinated motion based on formula (1) and formula (2), the formula is as follows

In formula (3) represents the driving torque and τ ₁ ,…,τ _n represent the driving torque of each joint of the manipulator arm;/> represent the regression matrix of the linearized dynamic model and the unknown inertia parameters, respectively, /> Represents the generalized joint space coordinates of the compound robot and θ ₁ ,…,θ _n represent the joint rotation angles of the manipulator arm, S _O ＝E ^-1 (q _O )A _O ,/> and/> where z _i =[0,0,1] ^T , and/> Represent the attitude rotation matrix and the origin translation vector of the link i+1 coordinate system relative to the link i coordinate system;

Because the unknown inertia parameter Φ of the composite robot includes three parts: unidentifiable parameters, independently identifiable parameters and only combinable identifiable parameters, its linear dynamic regression matrix There is a problem of dissatisfaction with the rank phenomenon, which affects the identification accuracy of the inertia parameters. Firstly, the value is selected by random sampling /> Compute the kinetic regression matrix and delete its zero-element columns to get /> Where r is the number of random sampling points and satisfies (n+4)r≥c, c≤(14n+8), thus excluding unidentifiable inertia parameters;

matrix The rank b of represents the number of elements of the minimum inertia parameter set identifiable by the composite robot, which is obtained by QR decomposition /> where /> is an orthogonal matrix, /> is an upper triangular matrix, set the smaller constant ε>0, then the diagonal element |R _ii |≤ε corresponds to the inertia parameter that can only be identified in combination /> and its regression matrix /> Delete the corresponding column of S _c2 from S _c to obtain the regression matrix of the minimum inertia parameter set and its parameters /> Further QR decomposition /> get

In formula (4) is the minimum inertia parameter set for compound robot dynamics, Represents the corresponding linear kinetic regression matrix; /> They are sub-regression matrices composed of the columns corresponding to S _c1 and S _c2 in S respectively; thus, a linearized full-state linearized dynamics model of the compound robot arm coordination suitable for parameter identification is obtained.

Further, the step 2 includes using the finite term Fourier series to design the periodic limited bandwidth excitation trajectory of the vehicle-arm coordinated motion, the formula is as follows

In formula (5), q _j0 represents the initial offset of the excitation trajectory of each joint of the composite robot, ω _f and N represent the fundamental frequency and order of the Fourier series respectively, let η=[η ₁ ,…,η _n+3 ] ^T and η _j =[a _j1 ,...,a _jN ,b _j1 ,...,b _jN ,q _j0 ] ^T represents the coefficient to be optimized for each joint excitation trajectory. In order to make the dynamic regression matrix S _b have a good condition number and suppress the adverse effect of measurement noise on the parameter identification accuracy during the experiment, the following optimization model is designed to solve the excitation trajectory coefficient

s.t.

Cond _F ( ) in formula (6) represents the condition number of the Frobenius norm of the matrix, S _bg , S _bi , S _bf represent the gravity moment parameters, inertia moment parameters, and friction moment parameters in the dynamic regression matrix S _b of the composite robot, respectively For the corresponding dynamic sub-matrix, optimizing the condition number of the sub-matrix can ensure the continuous excitation and parameter consistency of the corresponding physical characteristics; |S _bij | _max and |S _bij | _min represent the maximum and minimum absolute values of the elements of matrix S _b respectively , this optimization item can ensure that each element of S _b is in the same order of magnitude and reduce abnormal data interference; λ ₁ , λ ₂ , λ ₃ , λ ₄ , λ ₅ represent weight coefficients; the optimization constraint item is used to limit the initial value of the excitation trajectory and the The position, velocity, and acceleration of the robot satisfy the physical constraints, and the above optimization problem is solved to obtain the optimal synchronous excitation trajectory of the coordinated dynamics of the composite robot arm, which improves the dynamic characteristic excitation effect and the physical consistency of the model parameters.

Further, said step 3 includes the following specific steps:

Assuming that the number of sampling points during the experiment is m, the corresponding dynamic observation matrix and response moment vector are respectively Let the moment residuals for the dynamic model identification /> and the initial value of the measurement noise covariance matrix where /> represents the identity matrix, /> represent The deformation matrix of ; let the dynamic observation matrix and response moment vector of weighted regularization be respectively where ω represents the dot product of matrix elements, /> represents the weight vector of the measured data, while the weight matrix /> Each column of is consistent with the vector W. Weighted Least Squares Algorithm for Designing Dynamic Model Parameters of Composite Robot

And design the adaptive law of measurement noise covariance matrix and measurement data weighting vector as follows

In formulas (7) and (8) Represents a block diagonal matrix and the diagonal block elements are covariance matrices

∑, k represents the number of iterations of the algorithm, Represents the unit column vector, α>0 represents the weighted threshold of abnormal data, the operators abs( ) and sgn( ) respectively calculate the absolute value and sign function value of each element of the matrix, and obtain the adaptive weighting of the parameters of the vehicle arm coordination dynamics model Iterative solution algorithm.

Further, the adaptive weighted iterative solution algorithm for the parameters of the vehicle arm coordination dynamics model is an adaptive weighted iterative least squares algorithm, and the steps are as follows:

Input: sampled data Initial weight W, /> weighted threshold α;

Output: Kinetic model parameter Φ _b ;

Initialize the covariance matrix

When condition one: the weight vector W does not converge or is less than the maximum number of iterations; the following steps are performed in a loop:

When the second condition: the covariance matrix Σ does not converge or is less than the maximum number of iterations; execute the following steps in a loop:

Adopt formula (7) to calculate kinetic model parameter Φ _b ;

Use formula (8) to update the covariance matrix Σ;

Update weighted regularized

End condition two loop;

Use formula (8) to update the weight vector W;

End condition one loop;

Verify the kinetic model parameters Φ _b ;

Based on the above algorithm, the identification results of the dynamic model of the composite robot are obtained, and the test trajectory is designed by formula (6) to verify the validity and robustness of the model parameters.

The optimal excitation and high-precision identification method for the coordination dynamics of the composite robot arm of the present invention has the following advantages: the optimal excitation and high-precision identification method for the coordination dynamics of the composite robot arm of the present invention can realize the full-state dynamic linearity of the coordination of the vehicle arm Simultaneous modeling and optimal synchronous excitation, and high-precision robust identification of dynamic model parameters for measurement noise and abnormal data, ultimately effectively improving the accuracy and physical consistency of composite robot model identification.

Description of drawings

Fig. 1 is the flow chart of the optimal excitation and high-precision identification method of the coordinated dynamics of the composite robot arm of the present invention;

Fig. 2 is a schematic diagram of the optimal excitation trajectory of the coordinated dynamics of the composite robot arm of the present invention;

Fig. 3 is a schematic diagram of the parameter identification test effect of the coordination dynamics model of the composite robot arm of the present invention.

Detailed ways

In order to better understand the purpose, structure and function of the present invention, the optimal excitation and high-precision identification method for the coordination dynamics of the composite robot arm of the present invention will be further described in detail in conjunction with the accompanying drawings.

The present invention designs a non-singular linearized regression equation through the dynamic recursive model of the composite robot car-arm coordinated motion, and then designs the optimal excitation trajectory of the car-arm coordinated motion based on the finite item Fourier series and the weighted condition number of the dynamic regression matrix , and based on abnormal data suppression and prior knowledge of measurement noise, a high-precision adaptive weighted iterative solution algorithm for dynamic parameters is designed, and finally a high-precision robust identification of the vehicle-arm coordination dynamic model of the composite robot is realized.

As shown in Figure 1, the specific implementation scheme of the present invention comprises the following steps:

S1: Based on the Newton-Euler recursive equation, the linearized dynamic model of the composite robot car-arm coordinated motion is designed, and the non-singular linearized regression equation is obtained through QR decomposition to realize the full-state dynamics modeling;

Considering the inertia and friction of the joint drive system, the linearized dynamic model of each link of the manipulator arm of the composite robot is constructed based on the Newton-Euler recursive equation as follows

In formula (1) Represents the driving force required by the joint i when the link i moves alone /> and torque and/> represent the linear acceleration and gravitational acceleration of joint i respectively, Represents the angular velocity of joint i, m _i represents the mass of link i, /> represents the center of mass of connecting rod i, /> Represents the moment of inertia vector of link i relative to its link coordinate system, β _i = [I _ai , F _ci , F _si , F _vi ] ^T and I _ai , F _ci , F _si , F _vi represent the rotation of joint i respectively Inertia, Coulomb, static and viscous friction coefficients, /> Represents the friction torque observation matrix of joint i, where ω _s >0 represents the angular velocity threshold of static friction; the operator Represent the anti-symmetric matrix of the operation vector and the left multiplication matrix of the moment of inertia; Represents the linearized dynamic regression matrix of connecting rod i, Φ _i =[m _i ,mic _{i ,} I _i ,β _i ] ^T represents the unknown inertia parameter of connecting rod i.

Construct the linearized dynamic model of the omnidirectional mobile platform of the composite robot as follows

In formula (2) Indicates the Cartesian space coordinates of the mobile platform, /> Indicates the driving torque of the mobile platform mecanum wheel set, /> represents the transformation matrix of driving torque, m _O and I _O represent the mass and moment of inertia of the mobile platform respectively, β _O = [F _Ovx , F _Ovy , F _Ovz , F _Ocx , F _Ocy , F _Ocz ] ^T represents the movement of the mobile platform along the translation axis The viscous friction coefficient and the Coulomb friction coefficient in the direction of x _O , y _O and the rotation axis z _O , where the operator /> and /> Represents the friction moment observation matrix of the mobile platform; Φ _O ＝[m _O , I _O , β _O ] ^T represents the linearized dynamic regression matrix and unknown inertia parameters of the mobile platform, respectively.

Based on formula (1) and formula (2), the full-state linearized dynamic model of the composite robot car-arm coordinated motion is designed as follows

In formula (3) represents the driving torque and τ ₁ ,…,τ _n represent the driving torque of each joint of the manipulator /> represent the regression matrix of the linearized dynamic model and the unknown inertia parameters, respectively, /> Represents the generalized joint space coordinates of the compound robot and θ ₁ ,…,θ _n represent the joint rotation angles of the manipulator arm, S _O ＝E ^-1 (q _O )A _O , and/> where z _i =[0,0,1] ^T , /> and/> Represent the attitude rotation matrix and the origin translation vector of the link i+1 coordinate system relative to the link i coordinate system, respectively.

Since the unknown inertia parameter Φ of the composite robot includes three parts: unidentifiable parameters, independently identifiable parameters and only combinable identifiable parameters, its linear dynamic regression matrix There is a dissatisfaction rank phenomenon, which affects the identification accuracy of inertia parameters. For this, first take the value by random sampling /> Compute the kinetic regression matrix and delete its zero-element columns to get /> Where r is the number of random sampling points and satisfies (n+4)r≥c, c≤(14n+8), so as to exclude unidentifiable inertia parameters.

easy to know matrix The rank b of represents the number of elements of the minimum inertia parameter set identifiable by the composite robot, which is obtained by QR decomposition /> where /> is an orthogonal matrix, is an upper triangular matrix, set the smaller constant ε>0, then the diagonal element |R _ii |≤ε corresponds to the inertia parameter that can only be identified in combination /> and its regression matrix /> Delete the corresponding column of S _c2 from S _c to obtain the regression matrix of the minimum inertia parameter set /> and its parameters /> Further QR decomposition Available

In formula (4) is the minimum inertia parameter set for compound robot dynamics, /> Represents the corresponding linear kinetic regression matrix; /> They are sub-regression matrices composed of columns corresponding to S _c1 and S _c2 in S, respectively; thus, a full-state linearized dynamic model of the car-arm coordination of the compound robot suitable for parameter identification is obtained.

S2: Construct an optimization functional based on the weighted condition number of the regression matrix of the linearized dynamic model of the compound robot and its dynamic sub-matrix, and use the finite term Fourier series to design the optimal synchronous excitation trajectory of the vehicle-arm coordinated dynamics to improve the dynamics of the robot Characteristic incentive effects and physical consistency of model parameters;

In order to realize the continuous excitation of the dynamic characteristics of the composite robot, the periodic limited-bandwidth excitation trajectory of the vehicle-arm coordinated motion is designed using the finite term Fourier series, as follows

In formula (5), q _j0 represents the initial offset of the excitation trajectory of each joint of the composite robot, ω _f and N represent the fundamental frequency and order of the Fourier series respectively, let η=[η ₁ ,…,η _n+3 ] ^T and η _j =[a _j1 ,...,a _jN ,b _j1 ,...,b _jN ,q _j0 ] ^T represents the coefficient to be optimized for each joint excitation trajectory. In order to make the dynamic regression matrix S _b have a good condition number and suppress the adverse effect of measurement noise on the parameter identification accuracy during the experiment, the following optimization model is designed to solve the excitation trajectory coefficient

s.t.

Cond _F ( ) in formula (6) represents the condition number of the Frobenius norm of the matrix, S _bg , S _bi , S _bf represent the gravity moment parameters, inertia moment parameters, and friction moment parameters in the dynamic regression matrix S _b of the composite robot, respectively For the corresponding dynamic sub-matrix, optimizing the condition number of the sub-matrix can ensure the continuous excitation and parameter consistency of the corresponding physical characteristics; |S _bij | _max and |S _bij | _min represent the maximum and minimum absolute values of the elements of matrix S _b respectively , this optimization item can ensure that each element of S _b is in the same order of magnitude and reduce abnormal data interference; λ ₁ , λ ₂ , λ ₃ , λ ₄ , λ ₅ represent weight coefficients; the optimization constraint item is used to limit the initial value of the excitation trajectory and the The position, velocity, and acceleration of satisfy the physical constraints. By solving the above optimization problem, the optimal synchronous excitation trajectory of the vehicle-arm coordination dynamics of the composite robot is obtained, which improves the dynamic characteristic excitation effect and the physical consistency of the model parameters.

S3: Based on the regularization of the measurement noise covariance matrix and the weighted suppression of abnormal data, an adaptive weighted iterative least squares solution algorithm for the vehicle-arm coordination dynamics model parameters is designed, and a test trajectory is designed to verify the validity of the parameter identification results to realize the composite robot High-precision robust identification of dynamical models.

Assuming that the number of sampling points during the experiment is m, the corresponding dynamic observation matrix and response moment vector are respectively Let the moment residuals for the dynamic model identification /> and the initial value of the measurement noise covariance matrix where /> represents the identity matrix, /> Representative /> The deformation matrix of the weighted regularization dynamics observation matrix and the response moment vector are respectively /> where ⊙ represents the dot product of matrix elements, /> represents the weight vector of the measured data, while the weight matrix /> Each column of is consistent with the vector W. Weighted Least Squares Algorithm for Designing Dynamic Model Parameters of Composite Robot

And design the adaptive law of measurement noise covariance matrix and measurement data weighting vector as follows

In formulas (7) and (8) Represents the block diagonal matrix and the diagonal block elements are the covariance matrix Σ, k represents the number of algorithm iterations, /> Represents the unit column vector, α>0 represents the weighted threshold of abnormal data, and the operators abs( ) and sgn( ) respectively calculate the absolute value and sign function value of each element of the matrix. The adaptive weighted iterative solution algorithm to obtain the vehicle-arm coordination dynamics model parameters is as follows

Based on the above algorithm, the identification results of the dynamic model of the composite robot are obtained, and the test trajectory is designed by formula (6) to verify the validity and robustness of the model parameters.

Example

The process of optimal excitation and high-precision identification of the composite robot car-arm coordinated dynamics of the present invention is shown in Figure 1, and the specific implementation object is a composite robot composed of a Mecanum wheeled mobile platform and a six-degree-of-freedom mechanical arm. Each joint is equipped with a brushless DC servo motor from Maxsine Company, and the joint angle is measured by a 17-bit encoder; a self-developed real-time controller based on the Ubuntu14.0RT kernel is adopted, and the EtherCAT communication protocol and the Acontis master station of Beckhoff Company are realized at a sampling frequency of 1kHz Data processing and algorithm execution. The position constraints of each joint of the composite robot are q _1min ＝q _2min ＝-2m, q _3min ＝-6.28rad, q _4min ＝q _7min ＝q _smin ＝q _9min ＝-3.14rad, q _5min ＝q _6min ＝-1.57rad, q _1max =q _2max =2m, q _3max =6.28rad, q _4max =q _7max =q _8max =q _9max =3.14rad, q _5max =1.57rad, q _6max =3.84rad, the speed constraint is The acceleration is constrained to />

The optimization model of the optimal excitation design of the composite robot car-arm coordinated dynamics of the present invention is shown in formula (5) and formula (6), wherein the fundamental frequency ω _f =0.1π and order N=5 of the Fourier series, each The weight coefficients of the optimization items are λ ₁ =1, λ ₂ =λ ₃ =λ ₄ =0.5, λ ₅ =0.2. Solve the above optimization problem to obtain the optimal synchronous excitation trajectory of the vehicle-arm coordinated motion, as shown in Figure 2, then run the trajectory on the composite robot and collect the angle and torque information of each joint in real time, taking into account the joint motor current and differential data The processing will inevitably introduce the influence of high-frequency noise, and the Butterworth filter algorithm is used to preprocess the excitation data, regardless of the cut-off frequency slope, so that the amplitude-frequency response curve in the passband can be smoothed to the maximum extent, and the filtering requirements for smooth amplitude can be achieved , to obtain more accurate position, velocity, acceleration and torque data of each joint.

The solution algorithm of the high-precision identification of the composite robot car-arm coordination dynamics model of the present invention is shown in formula (7) and formula (8), wherein the number of sampling points is m=200 in the experimental process, and the initial value of the weighted vector of the measurement data is W= 1 _2000×1 , the weighted threshold of abnormal data is α=2.5, and the maximum number of iterations is 200. Based on this, the dynamic model parameters of the composite robot are obtained, and the weight coefficients in formula (6) are further reset to λ ₁ =1, λ ₂ =λ ₃ =λ ₄ =0.8, λ ₅ =0.4, and the optimized model is obtained for The test trajectory verified by the model parameter identification results is run on the composite robot to collect the motion and torque data of each joint, and then the tracking accuracy of the calculated torque and the actual driving torque of each joint dynamic model is analyzed, and finally the test result is obtained as shown in Figure 3 , and calculate the torque residual values of each joint as Δτ _O1 = 0.69Nm, Δτ _O2 = 0.75Nm, Δτ _O3 = 0.70Nm, Δτ _O4 = 0.62Nm, Δτ ₁ = 0.93Nm, Δτ ₂ = 0.60Nm, Δτ ₃ = 0.24Nm, Δτ ₄ = 0.19Nm, Δτ ₅ = 0.21Nm, Δτ ₆ = 0.15Nm, it can be seen that the calculated torque of the dynamic model of the composite robot can effectively track the actual driving torque of each joint, and finally realized the vehicle-arm coordination dynamics model High-precision robust identification.

It can be understood that the present invention is described through some embodiments, and those skilled in the art know that various changes or equivalent substitutions can be made to these features and embodiments without departing from the spirit and scope of the present invention. In addition, the features and embodiments may be modified to adapt a particular situation and material to the teachings of the invention without departing from the spirit and scope of the invention. Therefore, the present invention is not limited by the specific embodiments disclosed here, and all embodiments falling within the scope of the claims of the present application belong to the protection scope of the present invention.

Claims (7)

1. A compound robot car arm coordination dynamics optimal excitation and high-precision identification method, is characterized in that, comprises the steps: S1: Based on the Newton-Euler recurrence equation, the linearized dynamic model of the coordinated motion of the composite robot arm is designed, and the non-singular linearized regression equation is obtained through QR decomposition to realize the full-state dynamics modeling; S2: Construct an optimization functional based on the regression matrix of the linearized dynamic model of the compound robot and the weighted condition number of its dynamic sub-matrix, and use the finite term Fourier series to design the optimal synchronous excitation trajectory of the coordinated dynamics of the vehicle arm to improve the dynamic characteristics of the robot Incentive effects and physical consistency of model parameters; S3: Based on the regularization of the measurement noise covariance matrix and the weighted suppression of abnormal data, an adaptive weighted iterative least squares solution algorithm for the parameters of the vehicle arm coordination dynamics model is designed, and a test trajectory is designed to verify the validity of the parameter identification results to realize the composite robot dynamics High-precision robust identification of learning models. 2. The optimal excitation and high-precision identification method of the coordinated dynamics of the composite robot arm according to claim 1, wherein said step 1 includes constructing each connecting rod of the composite robot manipulator based on the Newton-Euler recurrence equation The linearized kinetic model of , the formula is as follows In formula (1) Represents the driving force required by the joint i when the link i moves alone /> and torque and/> represent the linear acceleration and gravitational acceleration of joint i respectively, Represents the angular velocity of joint i, m _i represents the mass of link i, /> represents the center of mass of connecting rod i, /> Represents the moment of inertia vector of link i relative to its link coordinate system, β _i =[I _ai , F _ci , F _si , F _vi ] ^T and I _ai , F _ci , F _si , F _vi represent the rotation of joint i respectively Inertia, Coulomb, static and viscous friction coefficients, /> Represents the friction torque observation matrix of joint i, where ω _s >0 represents the angular velocity threshold of static friction; operator /> Represent the anti-symmetric matrix of the operation vector and the left multiplication matrix of the moment of inertia; /> Represents the linearized dynamic regression matrix of connecting rod i, Φ _i =[m _i , mi c _{i ,} I _i , β _i ] ^T represents the unknown inertia parameter of connecting rod i. 3. The optimal excitation and high-precision identification method for the coordinated dynamics of the composite robot arm according to claim 2, wherein the step 1 includes constructing a linearized dynamics model of the omnidirectional mobile platform of the composite robot, the formula is as follows In formula (2) Indicates the Cartesian space coordinates of the mobile platform, /> Indicates the driving torque of the mobile platform mecanum wheel set, /> represents the transformation matrix of driving torque, m _O and I _O represent the mass and moment of inertia of the mobile platform respectively, β _O = [F _Ovx , F _Ovy , F _Ovz , F _Ocx , F _Ocy , F _Ocz ] ^T represents the movement of the mobile platform along the translation axis The viscous friction coefficient and the Coulomb friction coefficient in the direction of x _O , y _O and the rotation axis z _O , where the operator /> and Represents the friction moment observation matrix of the mobile platform; /> Φ _O = [m _O , I _O , β _O ] ^T represents the linearized dynamic regression matrix and unknown inertia parameters of the mobile platform, respectively. 4. the optimal excitation and high-precision identification method of the coordinated dynamics of the composite robot car arm according to claim 3, is characterized in that, described step 1 comprises based on formula (1) and formula (2), design composite robot car- The full-state linearized dynamic model of arm coordinated motion, the formula is as follows In formula (3) represents the driving torque and τ ₁ ,...,τ _n represent the driving torque of each joint of the manipulator arm;/> represent the regression matrix of the linearized dynamic model and the unknown inertia parameters, respectively, /> Represents the generalized joint space coordinates of the compound robot and θ ₁ ,..., θ _n represent the rotation angles of each joint of the manipulator arm, S _O ＝E ^-1 (q _O )A _O ,/> and/> where z _i =[0,0,1] ^T , and./> Represent the attitude rotation matrix and the origin translation vector of the link i+1 coordinate system relative to the link i coordinate system; Because the unknown inertia parameter Φ of the composite robot includes three parts: unidentifiable parameters, independently identifiable parameters and only combinable identifiable parameters, its linear dynamic regression matrix There is a problem of dissatisfaction with the rank phenomenon, which affects the identification accuracy of the inertia parameters. Firstly, the value is selected by random sampling /> Compute the kinetic regression matrix and delete its zero-element columns to get /> Where r is the number of random sampling points and satisfies (n+4)r≥c, c≤(14n+8), thus excluding unidentifiable inertia parameters; matrix The rank b of represents the number of elements of the minimum inertia parameter set identifiable by the composite robot, which is obtained by QR decomposition /> where /> is an orthogonal matrix, /> is an upper triangular matrix, set the smaller constant ε>0, then the diagonal elements |R _ii |≤ε correspond to the inertia parameters that can only be identified in combination/> and its regression matrix /> from /> Delete the corresponding column of S _c2 to get the regression matrix of the minimum inertia parameter set /> and its parameters /> Further QR decomposition /> get In formula (4) is the minimum inertia parameter set for compound robot dynamics, Represents the corresponding linear kinetic regression matrix; /> They are sub-regression matrices composed of the columns corresponding to S _c1 and S _c2 in S respectively; thus, a linearized full-state linearized dynamics model of the compound robot arm coordination suitable for parameter identification is obtained. 5. The optimal excitation and high-precision identification method of the coordinated dynamics of the composite robot arm according to claim 3, wherein said step 2 includes using finite term Fourier series to design the periodic finite period of the coordinated motion of the vehicle-arm. The bandwidth excitation trajectory, the formula is as follows In formula (5), q _j0 represents the initial offset of the excitation trajectory of each joint of the compound robot, ω _f and N represent the fundamental frequency and order of the Fourier series respectively, let η=[η ₁ ,...,η _n+3 ] ^T and η _j =[a _j1 ,..., a _jN , b _j1 ,..., b _jN , q _j0 ] ^T represents the coefficients to be optimized for each joint excitation trajectory, in order to make the dynamic regression matrix S _b have a good condition number to suppress the experimental process In view of the adverse effect of measurement noise on the parameter identification accuracy, the following optimization model is designed to solve the excitation trajectory coefficient s.t. Cond _F ( ) in formula (6) represents the condition number of the Frobenius norm of the matrix, S _bg , S _bi , S _bf represent the gravity moment parameters, inertia moment parameters, and friction moment parameters in the dynamic regression matrix S _b of the composite robot, respectively For the corresponding dynamic sub-matrix, optimizing the condition number of the sub-matrix can ensure the continuous excitation and parameter consistency of the corresponding physical characteristics; |S _bij | _max and |S _bij | _min represent the maximum and minimum absolute values of the elements of matrix S _b respectively , this optimization item can ensure that each element of S _b is in the same order of magnitude and reduce abnormal data interference; λ ₁ , λ ₂ , λ ₃ , λ ₄ , λ ₅ represent weight coefficients; the optimization constraint item is used to limit the initial value of the excitation trajectory and the The position, velocity, and acceleration of the robot satisfy the physical constraints, and the above optimization problem is solved to obtain the optimal synchronous excitation trajectory of the coordinated dynamics of the composite robot arm, which improves the dynamic characteristic excitation effect and the physical consistency of the model parameters. 6. The optimal excitation and high-precision identification method for the coordinated dynamics of the composite robot arm according to claim 3, wherein the step 3 includes the following specific steps: Assuming that the number of sampling points during the experiment is m, the corresponding dynamic observation matrix and response moment vector are respectively Let the moment residuals for the dynamic model identification /> and the initial value of the measurement noise covariance matrix where /> represents the identity matrix, /> Representative /> The deformation matrix of ; let the dynamic observation matrix and response moment vector of weighted regularization be respectively where ⊙ represents the dot product of matrix elements, /> represents the weight vector of the measured data, while the weight matrix /> Each column of is consistent with the vector W. Weighted Least Squares Algorithm for Designing Dynamic Model Parameters of Composite Robot And design the adaptive law of measurement noise covariance matrix and measurement data weighting vector as follows In formulas (7) and (8) Represents the block diagonal matrix and the diagonal block elements are the covariance matrix Σ, k represents the number of algorithm iterations, /> Represents the unit column vector, α>0 represents the weighted threshold of abnormal data, the operators abs( ) and sgn( ) respectively calculate the absolute value and sign function value of each element of the matrix, and obtain the adaptive weighting of the parameters of the vehicle arm coordination dynamics model Iterative solution algorithm. 7. The optimal excitation and high-precision identification method for the coordination dynamics of the composite robot arm according to claim 6, wherein the adaptive weighted iterative solution algorithm for the coordination dynamics model parameters of the vehicle arm is an adaptive weighted iteration The least squares algorithm, the steps are as follows: Input: sampled data Initial weight W, /> weighted threshold α; Output: Kinetic model parameter Φ _b ; Initialize the covariance matrix When condition one: the weight vector W does not converge or is less than the maximum number of iterations; the following steps are performed in a loop: When the second condition: the covariance matrix Σ does not converge or is less than the maximum number of iterations; execute the following steps in a loop: Adopt formula (7) to calculate kinetic model parameter Φ _b ; Use formula (8) to update the covariance matrix Σ; Update weighted regularized End condition two loop; Use formula (8) to update the weight vector W; End condition one loop; Verify the kinetic model parameters Φ _b ; Based on the above algorithm, the identification results of the dynamic model of the composite robot are obtained, and the test trajectory is designed by formula (6) to verify the validity and robustness of the model parameters.