Coordinated dynamic optimal excitation and high-precision identification method for compound robot arm

Abstract

The application belongs to the field of robot model identification, and discloses a compound robot vehicle-arm coordinated dynamics optimal excitation and high-precision identification method. The application designs a non-singular linear regression equation through a dynamics recursive model of compound robot vehicle-arm coordinated movement, then designs an optimal excitation trajectory of vehicle-arm coordinated movement based on a limited term Fourier series and a dynamics regression matrix weighted condition number, and designs a high-precision adaptive weighted iterative solution algorithm of dynamics parameters based on abnormal data suppression and measurement noise prior knowledge, and finally realizes high-precision robust identification of the compound robot vehicle-arm coordinated dynamics model. The method can realize vehicle-arm coordinated full-state dynamics linearization modeling and optimal synchronous excitation, and realize high-precision robust identification of the dynamics model parameters in the face of measurement noise and abnormal data, and finally effectively improves the accuracy and physical consistency of the compound robot model identification.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of robot model identification, and particularly relates to a compound robot vehicle-arm coordinated dynamics optimal excitation and high-precision identification method. BACKGROUND

[0002] The dynamics model of a compound robot is an important basis for the development of motion control algorithms and physically consistent simulation, and 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 accuracy of the dynamics model directly affects the work efficiency and precision of the compound robot. However, it is difficult to accurately obtain the dynamics model parameters of the compound robot in actual engineering, such as the direct measurement method which cannot determine the inertia parameters of complex structural components and ignores the joint friction characteristics, and the CAD software which cannot effectively calculate the dynamics parameters due to the model simplification of motors / reducers / cables, etc. Therefore, it is the only effective way to identify the dynamics parameters of the compound robot through experimental excitation, and it has important engineering significance and practical value.

[0003] 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. Existing researches mainly focus on the derivation of linearized dynamics model of serial robot arms, excitation trajectory design, and dynamics model parameter solving, and have formed rich results, but few involve the dynamics model identification of compound robots. The existing robot dynamics model construction method cannot effectively consider the influence of joint friction, such as Coulomb friction, viscous friction, and static friction, which will affect the accuracy of dynamics modeling; at the same time, the singularity of the dynamics linearization regression matrix will also affect the accuracy of the model parameter identification, for which the minimum inertia parameter set construction, ridge regression analysis, and orthogonal triangular decomposition methods have been proposed to solve the above problems. In addition, effectively designing the excitation trajectory to fully reflect the motion characteristics of the compound robot and to suppress external disturbances and measurement noise in the experiment is a prerequisite for accurately identifying the dynamics model parameters of the compound robot. The existing methods mostly use the overall condition number of the robot dynamics regression matrix as the optimization index to solve the optimal parameters of the excitation trajectory, but similar condition numbers can also lead to different excitation effects or violate the physical characteristics of the system, and cannot realize the synchronous excitation of the compound robot vehicle-arm coordinated motion. The existing related methods for solving the parameters of the robot dynamics model, such as the quasi-Newton method, Kalman filtering, least squares regression, maximum likelihood estimation, and particle swarm optimization, all rarely consider the adverse effects of abnormal data and noise in the experimental process on the identification accuracy. Therefore, the current dynamics identification technology of the compound robot still has many challenges in the aspects of full-state linearization modeling, vehicle-arm coordinated motion excitation trajectory design, and high-precision dynamics parameter solving. SUMMARY

[0004] The application aims to provide a composite robot arm coordination dynamics optimal excitation and high-precision identification method to solve the above technical problems.

[0005] To solve the above technical problems, the specific technical scheme of the composite robot arm coordination dynamics optimal excitation and high-precision identification method is as follows:

[0006] The composite robot arm coordination dynamics optimal excitation and high-precision identification method comprises the following steps:

[0007] S1: Design a linearized dynamics model of the composite robot arm coordination motion based on the Newton-Euler recursion equation, and pass

[0008] Through QR decomposition, a nonsingular linear regression equation is obtained to realize full-state dynamics modeling;

[0009] S2: Construct an optimization functional based on the weighted condition number of the regression matrix and its dynamic submatrix of the composite robot linearized dynamics model, and design an optimal synchronous excitation trajectory of the arm coordination dynamics using a finite term Fourier series to improve

[0010] the robot dynamic characteristic excitation effect and the physical consistency of the model parameters;

[0011] S3: Design an adaptive weighted iterative least squares solution algorithm for the arm coordination dynamics model parameters based on the regularization of the measurement noise covariance matrix and the weighted suppression of abnormal data, and design a test trajectory to verify the effectiveness of the parameter identification results to realize high-precision robust identification of the composite robot dynamics model.

[0012] Further, step 1 comprises constructing a linearized dynamics model of each link of the composite robot operating arm based on the Newton-Euler recursion equation, and the formula is as follows

[0013]

[0014] In formula (1) represents the driving force required for joint i when the link i moves alone and the torque

[0015] 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 link i, represents the inertia moment vector of link i relative to its link coordinate system, β i = [I ai , F ci , F si , Fvi ] T and I ai , F ci , F si , F vi represent the moment of inertia, Coulomb friction, static friction and viscous friction coefficient of joint i, respectively,

[0016] represents the friction torque observation matrix of joint i, where ω s > 0 represents the angular velocity threshold of static friction; operator represent the skew-symmetric matrix of the operation vector and the inertia matrix left multiplication matrix, respectively; represents the linearized dynamics regression matrix of link i, Φ i = [m i , m i c i , I i , β i ] T represents the unknown inertia parameters of link i.

[0017] Further, the step 1 comprises constructing a linearized dynamics model of the compound robot omnidirectional mobile platform, which is formulated as

[0018]

[0019] In formula (2), x represents the Cartesian space coordinates of the mobile platform, represents the driving torque of the Mecanum wheel group of the mobile platform, represents the driving torque conversion matrix, m O , 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 viscous friction coefficient and Coulomb friction coefficient of the mobile platform along the translation axes x O , y O and the rotation axis z O , where the operators and represent the friction torque observation matrix of the mobile platform; Φ O = [m O , I O , β O ] Trespectively represent the linearized dynamics regression matrix and unknown inertia parameters of the mobile platform.

[0020] Further, the step 1 comprises designing a full-state linearized dynamics model of the compound robot vehicle-arm coordinated motion based on formula (1) and formula (2) as follows

[0021]

[0022] in formula (3) denote the driving torques and τ1,…,τ n denote the driving torques of each joint of the operating arm; respectively represent the linearized dynamics regression matrix and unknown inertia parameters of the mobile platform, denote the generalized joint space coordinates of the compound robot and θ1,…,θ n denote the rotation angles of each joint of the operating arm, S O = E -1 (q O )A O , and wherein z i = [0, 0, 1] T , and denote the attitude rotation matrix and origin translation vector of the coordinate system of the link i+1 relative to the coordinate system of the link i respectively;

[0023] For the compound robot, the unknown inertia parameters Φ include three parts of unidentifiable parameters, independently identifiable parameters and only combinable identifiable parameters, which leads to the linear dynamics regression matrix not being full rank, thereby affecting the identification accuracy of the inertia parameters. First, the values of the random sampling are calculated, and the zero element columns of the dynamics regression matrix are deleted to obtain wherein r is the number of random sampling points and satisfies (n+4)r≥c, c≤(14n+8), so as to exclude the unidentifiable inertia parameters;

[0024] The rank b of the matrix represents the number of elements of the smallest set of identifiable inertia parameters of the compound robot, and is obtained by QR decomposition wherein Q is an orthogonal matrix, is an upper triangular matrix, and a smaller constant ε>0 is set. Then, the diagonal elements |R ii |≤ε correspond to the only combinable identifiable inertia parameters and the regression matrix thereof from S c , S c2corresponding column and its parameters further QR decomposition obtaining

[0025]

[0026] in formula (4) is the minimum inertia parameter set of the compound robot dynamics, denotes the corresponding linear dynamics regression matrix; are sub-regression matrices composed of the columns of S c1 and S c2 corresponding to S , respectively; thus obtaining the compound robot arm-coordinated full-state linearized dynamics model suitable for parameter identification.

[0027] further, the step 2 comprises designing a periodic finite-bandwidth excitation trajectory of the arm-coordinated motion of the robot using a finite-term Fourier series, as follows

[0028]

[0029] in formula (5) j0 denotes the initial bias 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, and let η = [η1, …, η n+3 ] T and η j = [a j1 , …, a jN , b j1 , …, b jN , q j0 ] T denote the optimization coefficients of the excitation trajectory of each joint. In order to make the dynamics regression matrix S b have a good condition number to suppress the adverse effects of measurement noise on the accuracy of parameter identification in the experiment, the following optimization model is designed to solve the excitation trajectory coefficients

[0030]

[0031] s.t.

[0032]

[0033] in formula (6) F Cond(·) denotes the condition number of the matrix Frobenius norm, S bg , S bi , S bf denote the compound robot dynamics regression matrix Sb The dynamic sub-matrix corresponding to the gravity moment parameter, the inertia moment parameter and the friction moment parameter, and the optimization sub-matrix condition number can ensure the sustained excitation of the corresponding physical characteristics and the parameter consistency. bij max bij min respectively represent the maximum value and the minimum value of the absolute value of each element of the matrix S b The optimization term can ensure that each element of S b is in the same order of magnitude and reduces the interference of abnormal data; λ1, λ2, λ3, λ4, and λ5 represent weight coefficients; the optimization constraint term is used to limit the initial value of the excitation trajectory and the position, velocity, and acceleration of each joint to satisfy the physical constraints.

[0034] Further, the step 3 includes the following specific steps:

[0035] Let the number of sampling points in the experimental process be m, and the corresponding dynamic observation matrix and the response moment vector are respectively Let the moment residual of the dynamic model identification be and the initial value of the measurement noise covariance matrix be where represents the unit matrix, represents the deformation matrix of ; let the weighted regularized dynamic observation matrix and the response moment vector be respectively where ω represents the dot product of the matrix elements, represents the weight vector of the measurement data, and the weight matrix is consistent with the vector W in each column. A weighted least squares solution algorithm for the dynamic model parameters of the compound robot is designed

[0036]

[0037] and the adaptive law of the measurement noise covariance matrix and the measurement data weight vector is designed as follows

[0038]

[0039] In formulas (7) and (8), represents a block diagonal matrix, and the diagonal block elements are the covariance matrix

[0040] ∑, k represents the number of iterations of the algorithm, ​​​a represents a unit column vector, a>0 represents an abnormal data weighting threshold, the operators abs(·) and sgn(·) respectively represent absolute value and sign function value of each element of a matrix, and an adaptive weighted iterative solving algorithm of a coordinated dynamics model parameter of a vehicle arm is obtained.

[0041] Further, the adaptive weighted iterative solving algorithm of the coordinated dynamics model parameter of the vehicle arm is an adaptive weighted iterative least square algorithm, and the steps are as follows:

[0042] Input: sampled data Initial weight W, Weighting threshold a;

[0043] Output: dynamics model parameter Φ b ;

[0044] Initialize the covariance matrix

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

[0046] When condition two: the covariance matrix ∑ does not converge or is less than the maximum number of iterations; the following steps are executed in a loop:

[0047] The dynamics model parameter Φ is calculated using formula (7) b ;

[0048] The covariance matrix ∑ is updated using formula (8);

[0049] The weighted regularization is updated

[0050] End condition two loop;

[0051] The weight vector W is updated using formula (8);

[0052] End condition one loop;

[0053] Verify the dynamics model parameter Φ b ;

[0054] Based on the identification result of the dynamics model of the composite robot obtained by the above algorithm, formula (6) is used to design a test trajectory to verify the effectiveness and robustness of the model parameter.

[0055] The optimal excitation and high-precision identification method of the coordinated dynamics of the composite robot vehicle arm has the following advantages: the optimal excitation and high-precision identification method of the coordinated dynamics of the composite robot vehicle arm can realize linear modeling and optimal synchronous excitation of the coordinated dynamics of the vehicle arm in all states, and can realize high-precision robust identification of the dynamics model parameter in the face of measurement noise and abnormal data, thereby effectively improving the accuracy and physical consistency of the model identification of the composite robot. Attached Figure Description

[0056] Figure 1 This is a flowchart of the optimal excitation and high-precision identification method for the coordinated dynamics of the composite robot arm of the present invention;

[0057] Figure 2 This is a schematic diagram of the optimal excitation trajectory for the coordinated dynamics of the composite robot arm of the present invention;

[0058] Figure 3 This is a schematic diagram illustrating the parameter identification and testing results of the composite robot arm coordination dynamics model of the present invention. Detailed Implementation

[0059] To better understand the purpose, structure, and function of this invention, the following detailed description of the optimal excitation and high-precision identification method for the coordinated dynamics of the composite robot arm is provided in conjunction with the accompanying drawings.

[0060] This invention designs a non-singular linearized regression equation based on the dynamic recursive model of the coordinated motion of the composite robot vehicle-arm. Then, based on the finite-term Fourier series and the weighted condition number of the dynamic regression matrix, it designs the optimal excitation trajectory for the coordinated motion of the vehicle-arm. Finally, based on the suppression of abnormal data and prior knowledge of measurement noise, it designs a high-precision adaptive weighted iterative solution algorithm for dynamic parameters, ultimately achieving high-precision and robust identification of the coordinated dynamic model of the composite robot vehicle-arm.

[0061] like Figure 1 As shown, the specific implementation of the present invention includes the following steps:

[0062] S1: Based on the Newton-Euler recursive equation, a linearized dynamic model of the coordinated motion of the composite robot vehicle and arm is designed. The non-singular linear regression equation is obtained through QR decomposition to achieve full-state dynamic modeling.

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

[0064]

[0065] In formula (1) This represents the driving force required by joint i when link i moves alone. and torque and Let represent the linear acceleration and gravitational acceleration of joint i, respectively. m represents the angular velocity of joint i. i This represents the mass of link i. The center of mass of link i is represented. represents the inertia moment vector of link i relative to its link coordinate frame, β i = [I ai , F ci , F si , F vi ] T and I ai , F ci , F si , F vi represent the rotational inertia, Coulomb friction, static friction and viscous friction coefficient of joint i, respectively, represents the friction torque observation matrix of joint i, where ω s > 0 represents the angular velocity threshold of static friction; operators represent the skew-symmetric matrix of the operation vector and the inertia moment left multiplication matrix, respectively; represents the linearized dynamics regression matrix of link i, Φ i = [m i , m i c i , I i , β i ] T represents the unknown inertia parameters of link i.

[0066] The linearized dynamics model of the omnidirectional mobile platform of the compound robot is constructed as follows

[0067]

[0068] In formula (2), x represents the Cartesian space coordinates of the mobile platform, represents the driving torque of the Mecanum wheel group of the mobile platform, represents the driving torque conversion matrix, m O , I O represent the mass and rotational inertia of the mobile platform, respectively, β O = [F Ovx , F Ovy , F Ovz , F Ocx , F Ocy , F Ocz ] T represents the viscous friction coefficient and Coulomb friction coefficient of the mobile platform along the translation axes x O , y O and the rotation axis z O , where operators and represent the friction torque observation matrix of the mobile platform; Φ O = [m O , I O,β O ] T These represent the linearized dynamic regression matrix and the unknown inertia parameter of the mobile platform, respectively.

[0069] Based on formulas (1) and (2), the full-state linearized dynamic model of the coordinated motion of the composite robot vehicle and arm is designed as follows:

[0070]

[0071] In formula (3) Represents the driving torque and τ1,…,τ n Indicates the driving torque of each joint of the manipulator. These represent the regression matrix of the linearized dynamics model and the unknown inertia parameter, respectively. The generalized joint space coordinates of the composite robot and θ1,…,θ n S represents the rotation angle of each joint of the manipulator. O =E -1 (q O A O , and Where z i =[0,0,1] T , and These represent the attitude rotation matrix of the link i+1 coordinate system relative to the link i coordinate system and the origin translation vector, respectively.

[0072] Because the unknown inertia parameter Φ of the composite robot includes three parts: unidentifiable parameters, independently identifiable parameters, and parameters that can only be combined for identification, its linear dynamic regression matrix is ​​affected. The presence of incomplete rank inertia affects the accuracy of inertia parameter identification. To address this, random sampling is first employed. Calculate the dynamic regression matrix and remove its zero-element columns to obtain Where r is the number of random sampling points and satisfies (n+4)r≥c and c≤(14n+8), thus excluding unidentifiable inertia parameters.

[0073] Easy-to-understand matrix The rank *b* represents the number of elements in the smallest identifiable set of inertia parameters of the composite robot, obtained through QR decomposition. in For orthogonal matrices For an upper triangular matrix, if a small constant ε > 0 is set, then the diagonal elements |R ii |≤ε corresponds to inertia parameters that can only be identified by combination. and its regression matrix From S c Delete S c2corresponding column and its parameters further QR decomposition can be obtained

[0074]

[0075] in equation (4) is the minimum inertia parameter set of the compound robot dynamics, denotes the corresponding linear dynamics regression matrix; are the sub-regression matrices composed of the columns in S c1 and S c2 corresponding to S , respectively; thus the compound robot car-arm coordination full-state linearized dynamics model suitable for parameter identification is obtained.

[0076] S2: based on the weighted condition number of the compound robot linearized dynamics model regression matrix and its dynamic sub-matrix, an optimization functional is constructed, and the finite Fourier series is used to design the optimal synchronous excitation trajectory of the car-arm coordination dynamics, so as to improve the excitation effect of the robot dynamic characteristics and the physical consistency of the model parameters;

[0077] In order to realize the continuous excitation of the dynamics characteristics of the compound robot, the finite Fourier series is used to design the periodic finite bandwidth excitation trajectory of the car-arm coordination motion, as follows

[0078]

[0079] in equation (5) j0 denotes the initial bias 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, and let η = [η1, …, η n+3 ] T and η j = [a j1 , …, a jN , b j1 , …, b jN , q j0 ] T denote the to-be-optimized coefficients of the excitation trajectory of each joint. In order to make the dynamics regression matrix S b have a good condition number, so as to suppress the adverse effects of measurement noise on the parameter identification accuracy in the experiment, the following optimization model is designed to solve the excitation trajectory coefficients

[0080]

[0081] s.t.

[0082]

[0083] Cond F (·) represents the condition number of the matrix Frobenius norm, S bg , S bi , S bf respectively represent the composite robot dynamics regression matrix S b The dynamic sub-matrix corresponding to the gravity moment parameter, the inertia moment parameter and the friction moment parameter, the optimization sub-matrix condition number can ensure the continuous excitation and parameter consistency of the corresponding physical characteristics; |S bij | max , |S bij | min respectively represent the matrix S b The maximum and minimum of the absolute value of each element, and the optimization term can ensure that each element of S b is in the same order of magnitude and reduces the interference of abnormal data; λ1, λ2, λ3, λ4, λ5 represent weight coefficients; the optimization constraint term is used to limit the initial value of the excitation trajectory and the position, velocity and acceleration of each joint to meet the physical constraints. Solve the above optimization problem to obtain the optimal synchronous excitation trajectory of the car-arm coordinated dynamics of the composite robot, and improve the excitation effect of dynamic characteristics and the physical consistency of model parameters.

[0084] S3: Design an adaptive weighted iterative least squares solution algorithm for the car-arm coordinated dynamics model parameters based on measurement noise covariance matrix regularization and abnormal data weighting suppression, and design a test trajectory to verify the effectiveness of the parameter identification results, and realize high-precision robust identification of the dynamics model of the composite robot.

[0085] Let the number of sampling points in the experiment process be m, then the corresponding dynamics observation matrix and response moment vector are Let the moment residual of the dynamics model identification and the initial value of the measurement noise covariance matrix Where represents the unit matrix, represents the deformation matrix of Let the weighted regularized dynamics observation matrix and response moment vector be Where ⊙ represents the point multiplication of matrix elements, represents the weight vector of the measurement data, and the weight matrix Each column of the weight matrix is consistent with the vector W. Design a weighted least squares solution algorithm for the dynamics model parameters of the composite robot

[0086]

[0087] And the adaptive law of the measurement noise covariance matrix and the measurement data weighting vector is as follows

[0088]

[0089] In formula (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 a unit column vector, and α>0 represents an abnormal data weighting threshold, and the operators abs(·) and sgn(·) respectively calculate the absolute value and the sign function value of each element of the matrix. The adaptive weighted iterative solution algorithm for obtaining the vehicle-arm coordinated dynamics model parameters is as follows

[0090]

[0091] Based on the identification result of the composite robot dynamics model obtained by the above algorithm, formula (6) is used to design a test trajectory to verify the effectiveness and robustness of the model parameters.

[0092] Embodiment

[0093] The flow of the optimal excitation and high-precision identification of the vehicle-arm coordinated dynamics of the composite robot is as shown in Figure 1 The specific implementation object is a composite robot composed of a Mecanum wheel mobile platform and a six-degree-of-freedom mechanical arm. Each joint is configured with a brushless DC servo motor of Maxsine Company, and the joint angle is measured by a 17-bit encoder. A self-developed real-time controller based on Ubuntu14.0 RT kernel is used, and data processing and algorithm execution are realized through EtherCAT communication protocol and Beckhoff Acontis master station at a sampling frequency of 1 kHz. The position constraints of each joint of the composite robot are q 1min = q 2min = -2m, q 3min = -6.28 rad, q 4min = q 7min = q smin = q 9min = -3.14 rad, q 5min = q 6min = -1.57 rad, q 1max = q 2max = 2m, q 3max = 6.28 rad, q 4max = q 7max = q 8max = q 9max = 3.14 rad, q 5max = 1.57 rad, q 6max = 3.84 rad, and the velocity constraint is The acceleration constraint is

[0094] The optimization model for the optimal excitation design of the composite robot-arm coordinated dynamics of this invention is shown in formulas (5) and (6), where the fundamental frequency ω of the Fourier series is... f =0.1π and order N=5, with the weight coefficients of each optimization term taking values ​​of λ1=1, λ2=λ3=λ4=0.5, and λ5=0.2. Solving the above optimization problem yields the optimal synchronous excitation trajectory for the coordinated motion of the vehicle and arm, as follows: Figure 2 As shown, the trajectory is then run on the composite robot and the angle and torque information of each joint are collected in real time. Considering that the joint motor current and differential data processing will inevitably introduce high-frequency noise, the Butterworth filtering algorithm is used to preprocess the excitation data. The cutoff frequency slope is ignored to make the amplitude-frequency response curve in the passband as smooth as possible and to achieve the filtering requirement of smooth amplitude, so as to obtain more accurate data on the position, velocity, acceleration and torque of each joint.

[0095] The solution algorithm for high-precision identification of the composite robot-arm coordinated dynamic model of this invention is shown in formulas (7) and (8), wherein the number of sampling points in the experiment is m = 200, and the initial value of the weighted vector of the measurement data is W = 1. 2000×1 The abnormal data weighting threshold is set to α = 2.5, and the maximum number of iterations is 200. Based on this, the dynamic model parameters of the composite robot are obtained. The weight coefficients in formula (6) are further reset to λ1 = 1, λ2 = λ3 = λ4 = 0.8, and λ5 = 0.4. The optimized model is solved to obtain the test trajectory for verifying the model parameter identification results. The motion and torque data of each joint are collected on the composite robot. Then, the tracking accuracy of the calculated torque and actual driving torque of each joint dynamic model is analyzed. Finally, the test results are obtained as follows: Figure 3 As shown, the torque residual values ​​of each joint are calculated as Δτ. O1 =0.69Nm, Δτ O2 =0.75Nm, Δτ O3 =0.70Nm, Δτ O4 =0.62Nm, Δτ1=0.93Nm, Δτ2=0.60Nm, Δτ3=0.24Nm, Δτ4=0.19Nm, Δτ5=0.21Nm, Δτ6=0.15Nm. It can be seen that the calculated torque of the composite robot dynamic model can effectively track the actual driving torque of each joint, and finally realize the high-precision robust identification of the vehicle-arm coordinated dynamic model.

[0096] It is to be understood that the present application is described by way of example only, and that modifications or alterations can be made to the features and embodiments described without departing from the spirit and scope of the application. In addition, modifications can be made to the features and embodiments described to accommodate specific situations and materials without departing from the spirit and scope of the application. Accordingly, the application is not limited to the specific embodiments disclosed herein, but rather, the scope of the application includes all embodiments falling within the scope of the claims.

Claims

1. A composite robot vehicle arm coordination dynamics optimal excitation and high-precision identification method, characterized in that, It comprises the following steps: S1: Designing a linearized dynamics model of the compound robot arm coordination motion based on Newton-Euler recursion equation, obtaining a non-singular linear regression equation by QR decomposition, and realizing full-state dynamics modeling; S2: Constructing an optimization functional based on the weighted condition number of the regression matrix and dynamic sub-matrix of the compound robot linearized dynamics model, and designing an optimal synchronous excitation trajectory of the arm coordination dynamics using a finite Fourier series to improve the excitation effect of the robot dynamic characteristics and the physical consistency of the model parameters; S3: Designing an adaptive weighted iterative least squares solution algorithm for the arm coordination dynamics model parameters based on regularization of the measurement noise covariance matrix and weighted suppression of abnormal data, and designing a test trajectory to verify the effectiveness of the parameter identification results, realizing high-precision robust identification of the compound robot dynamics model.

2. The composite robot vehicle arm coordinated dynamics optimal excitation and high precision identification method according to claim 1, characterized in that, Step 1 includes constructing a linearized dynamics model of each link of the compound robot operating arm based on Newton-Euler recursion equation, as follows in equation (1) represents the required driving force of joint i when link i moves alone and torque and and represents the angular velocity of joint i, m i represents the mass of link i, represents the center of mass of link i, represents the inertia moment 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 respectively represent the rotational inertia, Coulomb friction, static friction and viscous friction coefficient of joint i, represents the friction torque observation matrix of joint i, where ω s > 0 represents the angular velocity threshold of static friction; the operator respectively represent the skew-symmetric matrix of the operation vector and the inertia moment left multiplication matrix; represents the linearized dynamics regression matrix of link i, Φ i = [m i , m i c i , I i , β i ] T represents the unknown inertia parameters of link i.

3. The composite robot vehicle arm coordinated dynamics optimal excitation and high precision identification method according to claim 2, characterized in that, Step 1 includes constructing a linearized dynamics model of the omnidirectional mobile platform of the compound robot, as follows In equation (2) denotes the Cartesian space coordinates of the mobile platform, denotes the driving torque of the Mecanum wheel set of the mobile platform, denotes the driving torque conversion matrix, m O , I O denote 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 denote the viscous and Coulomb friction coefficients of the mobile platform along the translational axes x O , y O and the rotational axis z O , where the operators and denote the mobile platform friction torque observation matrix; Φ O = [m O , I O , β O ] T denote the linearized dynamics regression matrix and the unknown inertia parameters of the mobile platform, respectively.

4. The method of claim 3, wherein, Step 1 includes designing a full-state linearized dynamics model of the compound robot car-arm coordination motion based on formula (1) and formula (2), as follows in equation (3) denote the driving torques of the joints of the manipulator and n denote the driving torques of the joints of the manipulator; denote the regression matrix and the unknown inertia parameters of the linearized dynamics model, respectively, denote the generalized joint space coordinates of the manipulator and n denote the joint angles of the manipulator, S O = E -1 (q O )A O , and where z i = [0, 0, 1] T , and. denote the pose rotation matrix and the origin translation vector of the coordinate frame of link i+1 with respect to the coordinate frame of link i, respectively; For the unknown inertia parameters Φ of the compound robot, including non-identifiable parameters, independently identifiable parameters and only combinable identifiable parameters, the linear dynamic regression matrix of the compound robot is There is a problem of not full rank phenomenon, which further affects the identification accuracy of the inertia parameters. First, the random sampling value is taken The dynamic regression matrix is calculated and the zero element column is deleted to obtain Where r is the random sampling point number and satisfies (n+4)r≥c, c≤(14n+8), so as to exclude the non-identifiable inertia parameters; matrix The rank *b* represents the number of elements in the smallest identifiable set of inertia parameters of the composite robot, obtained through QR decomposition. in For orthogonal matrices For an upper triangular matrix, if a small constant ε > 0 is set, then the diagonal elements |R ii |≤ε corresponds to inertia parameters that can only be identified by combination. and its regression matrix from Delete S c2 The corresponding columns yield the minimum inertia parameter set regression matrix. and its parameters Further QR decomposition get in equation (4) is the minimum inertia parameter set of the compound robot dynamics, represents the corresponding linear dynamics regression matrix; are the sub-regression matrices composed of the columns of S c1 , S c2 corresponding to S; thus obtaining a compound robot arm coordination full-state linearized dynamics model suitable for parameter identification.

5. The method of claim 3, wherein, Step 2 includes designing a periodic finite bandwidth excitation trajectory of the car-arm coordination motion using a finite Fourier series, as follows q j0 represents the initial bias of each joint excitation trajectory of the compound robot, ω f and N represent the fundamental frequency and order of the Fourier series respectively, let η = [η1, …, η n+3 ] T and η j = [a j1 , …, a jN , b j1 , …, b jN , q j0 ] T represents the to-be-optimized coefficients of each joint excitation trajectory, in order to make the dynamic regression matrix S b have a good condition number to suppress the adverse effects of measurement noise in the experiment on the parameter identification accuracy, the following optimization model is designed to solve the excitation trajectory coefficients s.t. Cond F Cond bg Cond bi Cond bf Cond b Cond bij Cond max Cond bij Cond min Cond b Cond b Cond λ1, λ2, λ3, λ4, λ5 represent weight coefficients; the optimization constraint term is used to limit the initial value of the excitation trajectory and the position, velocity, and acceleration of each joint to meet the physical constraints. Solving the above optimization problem obtains the optimal synchronous excitation trajectory of the compound robot car-arm coordination dynamics, improving the dynamic characteristic excitation effect and the physical consistency of the model parameters.

6. The method of claim 3, wherein, Step 3 includes the following specific steps: Let the number of sampling points in the experiment process be m, then the corresponding dynamic observation matrix and response force vector are Let the torque residual of dynamic model identification be and the initial value of measurement noise covariance matrix be Wherein represents the unit matrix, represents the deformation matrix; let the weighted regularized dynamic observation matrix and response force vector be Wherein ⊙ represents the point multiplication of matrix elements, represents the weight vector of measurement data, and the weight matrix Each column of the weight matrix is consistent with the vector W; the weighted least square solution algorithm of the composite robot dynamic model parameters is designed And the adaptive law of the measurement noise covariance matrix and the measurement data weighted vector is as follows In formulas (7) and (8) represents a block diagonal matrix, and diagonal block elements are covariance matrices ∑, k represents the number of iterations of the algorithm, represents a unit column vector, and α>0 represents an abnormal data weighting threshold value, the operators abs(·) and sgn(·) respectively calculate absolute values and sign function values of each element of a matrix, to obtain an adaptive weighted iterative solving algorithm for parameters of a coordinated dynamics model of a vehicle arm.

7. The method of claim 6, wherein, The adaptive weighted iterative solution algorithm for the arm coordination dynamics model parameters is an adaptive weighted iterative least squares algorithm, with the following steps: input: sampled data initial weights w, weighted threshold α; Output: kinetic model parameters Φ b ; Initialization of 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 executed in a loop: When condition two: the covariance matrix ∑ does not converge or is less than the maximum number of iterations; the following steps are executed in a loop: The kinetic model parameter Φ is calculated using equation (7) b ; Update the covariance matrix ∑ using formula (8); updating the weighted regularisation End condition two loop; Update the weight vector W using formula (8); End condition one loop; Verify the kinetic model parameters Φ b ; Based on the above algorithm, the identification results of the compound robot dynamics model are obtained, and formula (6) is used to design a test trajectory to verify the effectiveness and robustness of the model parameters.

Images