Mechanical arm dynamics identification method based on data weighting and friction separation

Abstract

The invention discloses a mechanical arm dynamics identification method based on data weighting and friction separation, and belongs to the field of robot dynamics parameter calibration. The problem that the identification accuracy is affected due to discontinuous identification data and large modeling error near zero speed in the existing dynamic identification method is solved. According to the method, on one hand, the zero-speed continuous nonlinear friction model is integrated and applied to dynamic identification, so that the accuracy of the dynamic parameter identification process is improved; and on the other hand, the data weighting method provided by the invention is different from the existing method for directly deleting the abnormal value, and the outlier is replaced by the weighted average value of the adjacent point data, so that the integrity of the data is reserved, and the accuracy of the identification result is further improved. The method is mainly used for mechanical arm dynamic parameter identification.

Description

Technical Field

[0001] The invention belongs to the field of robot dynamics parameter calibration. Background Art

[0002] Accurate dynamic parameters are crucial for achieving accurate model-based robot control, such as trajectory tracking, collision detection, speed optimization, and impedance control. However, existing dynamic identification methods have three main problems: data weighting methods will cause data loss and data discontinuity, friction prediction near zero speed will have large errors, and the nonlinear friction optimization identification process has a long search time and low efficiency. Therefore, scholars from various countries have tried many ways to optimize the robot dynamic parameter identification technology.

[0003] For dynamic identification, the commonly used methods are statistical methods and physical constraint-based optimization methods. The former mainly includes ordinary least squares (OLS), weighted least squares (WLS) and maximum likelihood estimation. The WLS method introduces a weighted matrix to consider the variance of the predicted torque to overcome the problem that the OLS method is sensitive to the singularity of the regressor. The maximum likelihood estimation method fully considers the measurement noise of the actuator torque and joint angle. Reference (J. Swevers, C. Ganseman, DB Tukel, et al. Optimal robot excitation and identification [J]. IEEE Trans. Robot. Autom., 1997, 13 (5): 730–740.), and has a certain robustness to noise. However, the above statistical methods are sensitive to outliers, easily fall into local optimal solutions, and have difficulty converging for high-dimensional problems. A classic work based on physical constraint optimization method is based on linear matrix inequality (LMI)-semidefinite programming (SDP) framework, reference (CDSousa, R.Cortesao. Physical feasibility of robot base inertial parameter identification: A linear matrix inequality approach [J]. The International Journal of Robotics Research, 2014, 33 (6): 931–944), which takes into account the triangle inequality. However, measurement noise and model uncertainty, especially in friction models, usually affect the accuracy of dynamic identification.

[0004] In order to improve the estimation accuracy, on the one hand, a data weighting method is proposed to reduce the impact of abnormal data on the identification results; on the other hand, a complex nonlinear friction model is used to improve the modeling accuracy. However, the existing data weighting method directly deletes the outliers after identification, which may inadvertently remove the internal point data and damage the integrity of the identification data, thus affecting the final identification accuracy. The friction model used in most literatures has discontinuous friction at zero speed, which increases the modeling error near zero speed. In addition, using an arbitrary initial value of the nonlinear friction coefficient as the optimization search starting point will reduce the overall identification efficiency. Therefore, the existing dynamic identification method needs to solve the problem of affecting the identification accuracy due to discontinuous identification data and large modeling errors near zero speed. Summary of the invention

[0005] The purpose of the present invention is to solve the problem that the existing dynamic identification method affects the identification accuracy due to discontinuous identification data and large modeling errors near zero speed. The present invention provides a robot arm dynamic identification method based on data weighting and friction separation.

[0006] A method for dynamic identification of a manipulator based on data weighting and friction separation comprises the following steps:

[0007] S1. Obtain N sets of data based on the symmetrical excitation trajectory of the robot arm, and each set of data includes the position vector q and velocity vector of the joint on the robot arm. and the acceleration vector And the velocity vector of the motor on the robot arm Acceleration vector and the moment vector τ m ; According to q, τ m And the given joint nonlinear friction coefficient vector α, Stribeck velocity vector K v and the inverse tangent coefficient vector V s The initial value of constructs the regression matrix L under this set of data; α, K v and V s All are nonlinear friction parameters;

[0008] S2, initialize the joint data weight vector P = ones (Nn, 1) and the stacked joint data weight vector P e =P·ones(1,r); n is the total number of links or joints, ones(Nn,1) is a column vector of N×n rows, and all elements of the column vector are 1; ones(1,r) is a row vector of r columns, and all elements of the row vector are 1; r is the number of columns of the matrix L;

[0009] S3, according to τ under N groups of data mand L construct the corresponding intermediate variables and according to and Solving the OLS solution Ψ of the basis parameter set Ψ in the dynamic differential equation ols , and according to ols Solve for the standardized moment residual vector R # The initial value of R # The initial value of the joint covariance matrix σ is solved;

[0010] S4, respectively according to and Construct the corresponding normalized motor torque vector and the normalized regression matrix according to and WLS solution of the basis parameter set Ψ in solving dynamic differential equations And according to Update R # and σ;

[0011] S5. According to R # and σ, update P and P e ;

[0012] S6, determine whether P converges, if yes, execute step S7, if no, execute step S4;

[0013] S7, use the adjacent point data to and After replacing the outlier data with a weighted average, the optimization goal is set to solve the optimization equation solution of the dynamic parameter set χ and the basic parameter set Ψ in χ according to Solving for the estimated friction torque vector Thus, according to the estimated friction torque vector And the friction torque vector τ constructed f,m Update α, K in the regression matrix L v and V s ;

[0014] S8. Determine whether α converges. If yes, set α and K v 、V s , And x is output as the result, if the result is no, execute step S2.

[0015] Preferably, in step S3, according to τ under N sets of data m and L construct the corresponding intermediate variables and The implementation method is: the motor torque vector τ under N sets of datam and the regression matrix L are stacked column by column, and the corresponding and

[0016] Preferably, in step S3,

[0017] Preferably, in step S3, R # The initial value R0 = τ m -LΨ ols , the initial value of the joint covariance matrix σ

[0018] Preferably, in step S4, respectively according to and Construct the corresponding normalized motor torque vector and the normalized regression matrix according to and WLS solution of the basis parameter set Ψ in solving dynamic differential equations The implementation is:

[0019]

[0020] in, Re Nn×1 Represents a real number space with dimension Nn×1, Re Nn×r represents the real number space with dimension Nn×r, and All are intermediate variables.

[0021] Preferably, in step S4, according to Update R # The implementation of and σ is:

[0022]

[0023] Preferably, in step S5, according to R # and σ, update P and P e The implementation is:

[0024]

[0025] P e,j =P j ones(1,r);

[0026] Among them, P j is the element corresponding to joint j in P, j = 1...n, For R # The element corresponding to joint j in the jis the threshold value corresponding to joint j in the threshold vector δ, P e,j P e The element corresponding to the joint j in .

[0027] Preferably, in S7, the adjacent point data is used to and The way to replace the outlier data with the weighted average is:

[0028]

[0029] Among them, s i and e i are the first and last element positions of the i-th group of abnormal data, for The position s of the middle joint j i The corresponding data, for The position s of the middle joint j i The data corresponding to the previous position of is the position e of joint j i The data corresponding to the next position for The position s of the middle joint j i The data corresponding to the row, for The position s of the middle joint j i All the data corresponding to the previous row of the row, for The position of the middle joint j i All data corresponding to the next row of the row; 0≤i<N;

[0030] Set the optimization goal to solve the optimization equation solution of the dynamic parameter set χ and the basis parameter set Ψ The implementation is:

[0031]

[0032] in, Inertia parameter section is a real number space with dimension (r-4n)×1, and the linear friction parameter part Re 4n×1 is a real number space with dimension (r-4n)×1, Ψ(·) is the mapping of χ to function, χ∈Re 15n×1 , Re 15n×1 Represents the real number space with dimension 15n×1;

[0033]

[0034] χ j =[l j m j r j m j I a,j F c,j F v,j F s,j F b,j ], j = 1...n;

[0035] χ j is the dynamic parameter of the jth joint, l j is the inertia tensor vector of joint j, m j r j is the first-order moment of joint j, m j is the mass of connecting rod j, I a,j is the equivalent inertia of the rotor and transmission system at joint j, F c,j is the Coulomb friction coefficient of joint j, F v,j is the viscous friction coefficient of joint j, F s,j is the static friction coefficient of joint j, F b,j is the friction offset coefficient of joint j; there is a one-to-one correspondence between the connecting rod and the joint;

[0036] l j =[I xx,j I xy,j I xz,j I yy,j I yz,j I zz,j ], and I xx,j ,I xy,j ,I xz,j ,I yy,j ,I yz,j and I zz,j respectively

[0037] Represents the moment of inertia of joint j around the x-axis, the product of inertia in the x-axis and y-axis directions, the product of inertia in the x-axis and z-axis directions, the inertia tensor around the y-axis, the product of inertia in the y-axis and z-axis directions, and the moment of inertia around the z-axis;

[0038] m j r j =[m j r x,j m j r y,j m j r z,j ], and m j r x,j 、m j r y,j and m j ry,j They represent the first-order distance of joint j in the x direction, the first-order distance in the y direction, and the first-order distance in the z direction, respectively.

[0039] Preferably, the inequality constraints on the kinetic parameter set x include:

[0040] The quality constraints are:

[0041]

[0042] In the formula, m j,min and m j,max are the lower and upper limits of the mass of connecting rod j, m min and m max They are the lower and upper limits of the total mass of the manipulator, respectively;

[0043] The first-order moment constraint is expressed as:

[0044] (m j r j ) min ≤m j r j ≤(m j r j ) max ;

[0045] In the formula, (m j r j ) min and (m j r j ) max They represent the lower and upper limits of the first-order moment of connecting rod j respectively;

[0046] The LMI-triangle inequality constraint is expressed as:

[0047]

[0048] Where LMI is the expression of the LMI-triangle inequality constraint, 1 3×3 ∈Re 3×3 is the identity matrix, Re 3×3 is a real number space with a dimension of 3×3, tr(·) represents the function of solving the trace of the input matrix, I j represents the inertia tensor matrix of joint j;

[0049] The equivalent inertia constraint of the transmission system is expressed as:

[0050] I a,j >0;

[0051] The linear friction coefficient constraint is expressed as:

[0052] F c,j>0,F v,j >0,F s,j >0.

[0053] Preferably, in step S7,

[0054] In the formula, 0 4n×1 ∈Re 4n×1 is the zero vector, Re Nn×1 is a real number space with dimension Nn×1, Re 4n×1 is a real number space with dimension 4n×1;

[0055] In step S7, according to the estimated friction torque vector And the friction torque vector τ constructed f,m Update α, K in the regression matrix L v and V s The implementation is:

[0056] S71, from Extract is the estimated friction torque vector The element in the jth column of Re N×1 is a real number space with dimension N×1,

[0057] S72, according to Optimize the solution α j 、V s,j and K v,j , so as to realize the v and V s The updates are as follows:

[0058]

[0059] in,

[0060]

[0061] The friction torque vector τ is constructed f,m The friction torque vector of joint j corresponding to the N groups of data, Re N×1 is a real number space with dimension N×1, τ f,mjw for The friction torque of joint j corresponding to the wth group of data, w = 1, 2 ... N, θ j is the optimized variable set of joint j, θ j =[F c,j F v,j Fs,j F b,j α j K v,j V s,j ], and θ j F c,j 、F v,j 、F s,j 、F b,j Obtained from χ, α j 、V s,j and K v,j are the nonlinear friction coefficient, Stribeck velocity and inverse tangent coefficient of joint j, is the motor speed vector The wth group of data corresponds to the speed of the motor corresponding to the joint j, and e is a natural constant.

[0062] Advantages of the present invention:

[0063] The method for dynamic identification of a manipulator based on data weighting and friction separation described in the present invention is a nonlinear friction separation method based on symmetric excitation. On the one hand, the present invention integrates a zero-speed continuous nonlinear friction model and applies it to dynamic identification to improve the accuracy of the dynamic identification process. Specifically, the estimated friction torque vector is obtained by using the separation through data fitting. And the friction torque vector τ constructed f,m , to identify the nonlinear friction parameters α, K v and V s , and updated it to improve the overall iteration efficiency and recognition accuracy of the algorithm;

[0064] On the other hand, the present invention proposes a new data weighting method which is different from the existing method of directly deleting outliers. The present invention replaces outliers with the weighted average of adjacent point data, retains the integrity of the data, and further improves the accuracy of the identification results. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 is a flow chart of the method for identifying the dynamics of a manipulator based on data weighting and friction separation according to the present invention;

[0066] Figure 2 Schematic diagram of three sets of symmetrical excitation trajectories of the robot arm; Figure 2 a is the trajectory diagram of the forward trajectory 1, Figure 2 b is the trajectory diagram of reverse trajectory 1, Figure 2 c is the trajectory diagram of the forward trajectory 2, Figure 2 d is the trajectory diagram of reverse trajectory 2, Figure 2 e is the trajectory diagram of the forward trajectory 3, Figure 2 f is the trajectory diagram of reverse trajectory 3;

[0067] Figure 3 Schematic diagram of the data weighting technology in the method of the present invention and the traditional data weighting method for processing the measured joint torque; wherein, Figure 3 a is a schematic diagram of the joint torque measured by the traditional data weighting method. Figure 3 b is a schematic diagram of the joint torque measured by processing the data weighting technology in the method of the present invention.

[0068] Figure 4 This is a comparison chart of the results of three different friction models; Figure 4 The angle of the mid-joint is the joint position;

[0069] Figure 5 Schematic diagram of the parameters of the robot arm links and joints. DETAILED DESCRIPTION

[0070] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0071] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0072] The source of the concept of the present invention is: first, a zero-speed continuous nonlinear friction model is constructed, which is integrated and applied to the dynamic parameter identification to obtain the linear form of the dynamic differential equation τ m =LΨ, based on this linear form, the subsequent dynamics identification of the robot arm is carried out; secondly, the weighted average value of the outlier data is replaced, and the linear form of the dynamic differential equation is used to identify the dynamic parameter set and the optimization equation solution of the basic parameter set in the dynamic parameter set; finally, the nonlinear friction parameters α and K of the joint are realized based on the obtained optimization equation solution of the basic parameter set. v and V s separation and fitting.

[0073] In order to better describe the method of the present invention, see the following specific implementation method:

[0074] Specific implementation method 1. Figure 1 The present embodiment is described. The method for identifying the dynamics of a manipulator based on data weighting and friction separation described in the present embodiment is characterized in that the method comprises the following steps:

[0075] S1. Obtain N sets of data based on the symmetrical excitation trajectory of the robot arm, and each set of data includes the position vector q and velocity vector of the joint on the robot arm. and the acceleration vector And the velocity vector of the motor on the robot arm Acceleration vector and the moment vector τ m ; According to q, τ m And the given joint nonlinear friction coefficient vector α, Stribeck velocity vector K v and the inverse tangent coefficient vector V s The initial value of constructs the regression matrix L under this set of data; α, K v and V s All are nonlinear friction parameters;

[0076] S2, initialize the joint data weight vector P = ones (Nn, 1) and the stacked joint data weight vector P e =P·ones(1,r); n is the total number of links or joints, ones(Nn,1) is a column vector of N×n rows, and all elements of the column vector are 1; ones(1,r) is a row vector of r columns, and all elements of the row vector are 1; r is the number of columns of the matrix L;

[0077] S3, according to τ under N groups of data m and L construct the corresponding intermediate variables and according to and Solving the OLS solution Ψ of the basis parameter set Ψ in the dynamic differential equation ols , and according to ols Solve for the standardized moment residual vector R # The initial value of R # The initial value of the joint covariance matrix σ is solved;

[0078] S4, respectively according to and Construct the corresponding normalized motor torque vector and the normalized regression matrix according to and WLS solution of the basis parameter set Ψ in solving dynamic differential equations And according to Update R # and σ;

[0079] S5. According to R # and σ, update P and P e ;

[0080] S6, determine whether P converges, if yes, execute step S7, if no, execute step S4;

[0081] S7, use the adjacent point data to and After replacing the outlier data with a weighted average, the optimization goal is set to solve the optimization equation solution of the dynamic parameter set χ and the basic parameter set Ψ in χ according to Solving for the estimated friction torque vector Thus, according to the estimated friction torque vector And the friction torque vector τ constructed f,m Update α, K in the regression matrix L v and V s ;

[0082] S8. Determine whether α converges. If yes, set α and K v 、V s , And x is output as the result, if the result is no, execute step S2.

[0083] In this embodiment, the robot arm dynamics identification method based on data weighting and friction separation is different from the existing method of directly deleting outliers. The new data weighting method proposed in the present invention replaces outlier data with the weighted average of nearby non-outlier data points, thereby retaining the integrity of the measured data and improving the identification accuracy. m Separate the estimated friction torque vector on the motor side And according to the estimated friction torque vector And the friction torque vector τ constructed f,m , to achieve nonlinear friction parameters α, K v The identification of and significantly improves the identification efficiency of the proposed algorithm. OLS is the traditional least squares method, WLS is the weighted least squares method, and the condition for judging the convergence of P and α is that the difference between two consecutive values ​​is less than the threshold.

[0084] In specific application, the expression of the dynamic differential equation based on which the present invention is based is: τ m =LΨ, and the idea is as follows:

[0085] For a serial robot with n rigid links, where there is a one-to-one correspondence between links and joints, see Figure 5 , mapping the dynamic equation of the motor end of the robot arm to the connecting rod end, the dynamic differential equation can be described in the following concise form:

[0086]

[0087] In the formula, q, are the joint coordinates of the robot, respectively representing the position vector, velocity vector and acceleration vector of the robot joint, M(q) is the mass matrix of the connecting rod, I a is the equivalent inertia of the rotor and transmission system, is the matrix composed of the centrifugal force and Coriolis force of the connecting rod, g(q) is the gravity vector of the connecting rod, and for the jth connecting rod, its mass, centrifugal force, Coriolis force, and gravity are M respectively. j , C j and g j . are the motor speed and acceleration vector reflected by the transmission ratio, τ f,m is the motor side friction torque vector reflected by the transmission ratio, τ m is the measured motor torque vector reflected by the transmission ratio. For the jth connecting rod, the measured motor torque vector is τ m,j .

[0088] Considering only the measured data of a single motor speed reflected by the transmission ratio, for the joint transmission system equipped with harmonic drive, the Stribeck friction model with speed exponential term is used to characterize the viscous friction phenomenon, and the inverse tangent function atan(·) is used instead of the sign function to improve the discontinuity of the friction model at the speed reversal point. For the j-th joint under a set of sampling data, a combined friction model for calculating the friction torque on the motor side is proposed and used as follows:

[0089]

[0090] In the formula, F s,j 、F c,j 、F v,j and F b,j They respectively represent the static friction coefficient, Coulomb friction coefficient, viscous friction coefficient and friction offset coefficient of the jth joint. The above four are all linear friction coefficients. is the motor speed reflected by the transmission ratio, the jth joint speed, τ f,mj is the friction torque of the jth joint, α j is the nonlinear friction coefficient of the jth joint, V s,j is the Stribeck velocity of the jth joint, K v,j is the inverse tangent coefficient of the jth joint.

[0091] Formula (2) is a scalar equation. For a robot with n degrees of freedom, the friction scalar equation of each joint can be combined into a matrix form (j = 1, ..., n), that is, [τ f,m1 ,τ f,m2 ,…,τ f,mj], and then substitute formula (2) into formula (1) and perform linearization, that is, the dynamic differential equation of formula (1) can be rewritten into the following linear form:

[0092] τ m =LΨ (3);

[0093] Where Ψ∈Re r×1 is the basis parameter set, L∈Re n×r is the regression matrix, is q, α,K v ,V s The function of Here, α, K v 、V s They are α j 、V s,j , K v,j The stacked column vectors are collectively referred to as the combined friction coefficients. r represents the number of columns of the L matrix, that is, the number of types of parameters contained in the L matrix.

[0094] Further, in step S3, according to τ under N groups of data m and L construct the corresponding intermediate variables and The implementation method is: the motor torque vector τ under N sets of data m and the regression matrix L are stacked column by column, and the corresponding and Specifically, it can be: Among them, τ m,1 to τ m,N Respectively represent the motor torque vectors under the 1st to Nth groups of data; L1 to L N Represent the regression matrices for the 1st to Nth groups of data respectively.

[0095] Furthermore, in step S3, Ψ ols ∈Re r×1 is the OLS solution of Ψ, which can be obtained by Identification; Normalized moment residual vector R # The initial value R0∈Re Nn×1 , can be obtained by R0 = τ m -LΨ ols Calculate the joint covariance matrix σ∈Re Nn×Nn , and the initial value of σ is σ0, through Calculate. Among them, Re r×1 Re represents the real number space with dimension r×1, Nn ×1 Represents a real number space with dimension Nn×1, Re Nn×Nn Represents the real number space of dimension Nn×Nn.

[0096] Further, in step S4, according to and Construct the corresponding normalized motor torque vector and the normalized regression matrix according to and WLS solution of the basis parameter set Ψ in solving dynamic differential equations The implementation is:

[0097]

[0098] in, Re Nn×1 Represents a real number space with dimension Nn×1, Re Nn×r represents the real number space with dimension Nn×r, and All are intermediate variables.

[0099] In this preferred embodiment, due to the existence of measurement noise and model uncertainty, the measured data may have outliers. In order to find the location of the outliers, the present invention uses the traditional data weighting method to apply and After weighted and Then the weighted motor torque vector and the regression matrix For parameter identification, calculation This can reduce the impact of abnormal measurement data on the identification results.

[0100] In formula (6), It is a WLS solution based on the traditional data weighting method. # ∈Re Nn×1 is the normalized moment residual vector. Therefore, further, in step S4, according to Update the normalized moment residual vector R # The implementation of the joint covariance matrix σ is:

[0101]

[0102] Then, based on the standardized moment residual vector R # , the position of the outlier can be identified and recorded. However, unlike the method that uses only one threshold, the present invention sets a separate threshold for each joint to filter outliers, that is, n thresholds are set to achieve this. Therefore, the data weight vector of the joint can be updated in the following way, specifically:

[0103] In step S5, according to R# and σ, update P and P e The implementation is:

[0104]

[0105] P e,j =P j ones(1, r) (10);

[0106] Among them, P j is the element corresponding to joint j in P, j = 1...n, For R # The element corresponding to joint j in the j is the threshold value corresponding to joint j in the threshold vector δ. As a preference, all elements of the threshold vector δ are set to 3. j ∈Re N×1 The initial value of is a vector with all elements equal to 1. Therefore, the data weight vector P of joint j is j The column-wise expansion matrix P e,j ∈Re N×r The updated P j To update P e,j , where P e,j P e The element corresponding to the joint j in .

[0107] In this preferred embodiment, P and P are updated e The implementation method is to update the data weighted vector of each joint separately. Different thresholds can be set for the data weighted vector of each joint, which is conducive to better eliminating outliers of different joints.

[0108] Furthermore, in S7, and The way to replace the outlier data with the weighted average is:

[0109]

[0110] Among them, s i and e i are the first and last element positions of the i-th group of abnormal data, for The position s of the middle joint j i The corresponding data, for The position s of the middle joint j i The data corresponding to the previous position of is the position e of joint j i The data corresponding to the next position for The position s of the middle joint j i The data corresponding to the row, for The position s of the middle joint j i All the data corresponding to the previous row of the row, for The position of the middle joint j i All data corresponding to the next row of the row; 0≤i<N;

[0111] In this preferred embodiment, a new data weighting method is used to and The outlier data is replaced instead of being deleted directly, which protects the integrity of the measurement data and ensures the accuracy of the final identification result.

[0112] Due to the weighted and Can be used for parameter identification, such as the WLS solution of Ψ based on a new data weighting method It can be obtained by the following formula:

[0113]

[0114] is the WLS solution of Ψ based on the new data weighting method. However, due to is not physically feasible. Therefore, in order to obtain a physically feasible solution for Ψ, the new data weighting method is used to obtain and It can be used to solve the following optimization problems, specifically:

[0115] In S7, the optimization equations of the dynamic parameter set χ and the basis parameter set Ψ are solved by setting the optimization target. The implementation is:

[0116]

[0117] in, Inertia parameter section Re (r-4n ) ×1 is a real number space with dimension (r-4n)×1, and the linear friction parameter part Re 4n×1 is a real number space with dimension (r-4n)×1, Ψ(·) is the mapping of χ to function, χ∈Re 15n×1 , Re 15n×1 Represents the real number space with dimension 15n×1;

[0118]

[0119] χ j =[l j m j r j m j I a,j F c,j F v,j F s,j F b,j ], j = 1...n;

[0120] χ j is the dynamic parameter of the jth joint, l j is the inertia tensor vector of joint j, m j r j is the first-order moment of joint j, m j is the mass of connecting rod j, I a,j is the equivalent inertia of the rotor and transmission system at joint j, F c,j is the Coulomb friction coefficient of joint j, F v,j is the viscous friction coefficient of joint j, F s,j is the static friction coefficient of joint j, F b,j is the friction offset coefficient of joint j; there is a one-to-one correspondence between the connecting rod and the joint;

[0121] l j =[I xx,j I xy,j I xz,j I yy,j I yz,j I zz,j ], and I xx,j ,I xy,j ,I xz,j ,I yy,j ,I yz,j and I zz,j respectively

[0122] Represents the moment of inertia of joint j around the x-axis, the product of inertia in the x-axis and y-axis directions, the product of inertia in the x-axis and z-axis directions, the inertia tensor around the y-axis, the product of inertia in the y-axis and z-axis directions, and the moment of inertia around the z-axis;

[0123] m j r j =[m j r x,j m j r y,j m j r z,j ], and m j r x,j 、m j r y,j and mj r y,j They represent the first-order distance of joint j in the x direction, the first-order distance in the y direction, and the first-order distance in the z direction, respectively.

[0124] Furthermore, inequality constraints are imposed on the dynamic parameter set χ, including mass constraints, first-order moment constraints, LMI-triangular inequality constraints, equivalent inertia constraints, and linear friction parameter constraints, as follows:

[0125] The quality constraints are:

[0126]

[0127] In the formula, m j,min and m j,max are the lower and upper limits of the mass of connecting rod j, m min and m max They are the lower and upper limits of the total mass of the manipulator, respectively;

[0128] The first-order moment constraint is expressed as:

[0129] (m j r j ) min ≤m j r j ≤(m j r j ) max (15);

[0130] In the formula, (m j r j ) min and (m j r j ) max They represent the lower and upper limits of the first-order moment of connecting rod j respectively;

[0131] The LMI-triangle inequality constraint is expressed as:

[0132]

[0133] Where LMI is the expression of the LMI-triangle inequality constraint, 1 3×3 ∈Re 3×3 is the identity matrix, Re 3×3 is a real number space with a dimension of 3×3, tr(·) represents the function of solving the trace of the input matrix, I j represents the inertia tensor matrix of joint j;

[0134] The equivalent inertia constraint of the transmission system is expressed as:

[0135] I a,j >0 (18);

[0136] The linear friction coefficient constraint is expressed as:

[0137] F c,j >0,F v,j >0,F s,j >0 (19).

[0138] In this preferred embodiment, the problem of directly solving the linear form of the dynamic differential equation is transformed into an optimization problem for solving the deviation between the theoretical calculation and the measurement result. The solution to the optimization problem is the optimization equation solution of the dynamic parameter set χ and the basis parameter set Ψ. And solve for χ and The process takes into account the weight of the measurement data and continuously iterates and updates the calculation results, thereby improving the accuracy of the identification results.

[0139] Nonlinear friction parameters α, K v and V s The separation and fitting part of :

[0140] Furthermore, in order to identify α j 、V s,j and K v,j , the estimated friction torque vector can be calculated by the following formula Specifically:

[0141]

[0142] In the formula, 0 4n×1 ∈Re 4n×1 is the zero vector, Re Nn×1 is a real number space Re with dimension Nn×1 4n×1 is a real number space with dimension 4n×1;

[0143] Reuse Update α, K in the regression matrix L v and V s , the specific implementation methods include:

[0144] S71, from Extract is the friction torque vector The element in the jth column of Re N×1 is a real number space with dimension N×1,

[0145] S72, according to Optimize the solution α j 、V s,j and K v,j , so as to realize the v and Vs The updates are as follows:

[0146]

[0147] in,

[0148]

[0149] The friction torque vector τ is constructed f,m The friction torque vector of joint j corresponding to the N groups of data, Re N×1 is a real number space with dimension N×1, τ f,mjw for The friction torque of joint j corresponding to the wth group of data, w = 1, 2 ... N, θ j is the optimized variable set of joint j, θ j =[F c,j F v,j F s,j F b,j α j K v,j V s,j ], and θ j F c,j 、F v,j 、F s,j 、F b,j Obtained from χ, α j 、V s,j and K v,j are the nonlinear friction coefficient, Stribeck velocity and inverse tangent coefficient of joint j, is the motor speed vector The wth group of data corresponds to the speed of the motor corresponding to the joint j, and e is a natural constant.

[0150] In this preferred embodiment, α j 、V s,j and K v,j The optimization problem can be solved by formula 21, which can better handle the nonlinear parameter space and obtain a higher convergence speed. Based on the Stribeck friction model, the present invention uses the inverse tangent function atan instead of the sign function in the constructed friction torque vector, and uses the exponential function of the speed. Instead of the linear velocity function to describe the viscous friction, the modeling accuracy of the friction model is improved, and the identification accuracy is improved. The specific verification results are shown in Table 6. And through the forward and reverse excitation trajectories in the symmetrical trajectory, the measured motor torque vector τ mThe friction torque and the remaining part (inertia torque part) are separated from the original part, which significantly improves the recognition efficiency of the proposed algorithm.

[0151] Verification test:

[0152] The following verification tests are conducted to illustrate the effectiveness of the proposed method, which is superior to the prior art in terms of torque prediction accuracy and identification efficiency. Specifically:

[0153] First, the symmetrical excitation trajectory of the robot arm is set to obtain the various data that need to be measured during the identification process. In this example, three forward motion trajectories and three reverse motion trajectories are set. In each trajectory, the change of the joint angle of each joint over time is shown as follows: Figure 2 As shown in Figure 2, the results of identifying the dynamic parameters of the robot arm (α, K v 、V s , and χ) are shown in Table 1, and the optimal equation solution of the identified basis parameter set Ψ The results and standard deviations are shown in Table 2, and in Table 2, to Respectively The 1st to 64th elements in .

[0154] Table 1 Identification results of robot arm dynamic parameters

[0155]

[0156]

[0157] Table 2 Basic parameter identification results and standard deviations of the robot arm dynamic parameters

[0158]

[0159]

[0160] Under the same conditions, the data weighting method proposed in the identification method of the present invention and the traditional data weighting method are used to process the measured joint torque signal after low-pass filtering. The results are as follows: Figure 3 The root mean square errors RMSE of the two methods are shown in Table 3. It can be found from Table 3 that the joint torque residual RMSE value of the data weighting method proposed by the present invention is smaller and the identification accuracy is higher, which proves the effectiveness of the present invention.

[0161] Table 3 RMSE of moment residuals using two different data weighting methods

[0162]

[0163]

[0164] Table 4 Comparison of RMSE and computation time of different identification methods using identification trajectories

[0165]

[0166] Table 5 RMSE comparison of different identification methods using validation trajectories

[0167]

[0168] Table 6 RMSE of the results of three different friction models

[0169]

[0170] In addition, the method proposed in the present invention is compared with method 1 (using a three-loop iteration method to identify the dynamic parameters of the robot arm) and method 2 (using a two-loop iteration method to identify the dynamic parameters of the robot arm), and the comparison results are shown in Tables 4 and 5. It can be found from Tables 4 and 5 that the joint torque residual RMSE value of the method proposed in the present invention is smaller, which proves that the identification effect of the present invention is better and the present invention is more effective.

[0171] The continuous nonlinear friction torque model constructed by the present invention (that is, the friction torque vector τ constructed) f,m ) are compared with the friction model using the sign function and velocity index and the Stribeck friction model. The results are shown in Figure 4 As shown in Table 6. From Table 6, it can be found that the joint torque residual RMSE value of the method proposed in the present invention is smaller, which proves the effectiveness of the present invention.

[0172] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the present invention. It should therefore be understood that many modifications may be made to the exemplary embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in a manner different from that described in the original claims. It should also be understood that features described in conjunction with individual embodiments may be used in other described embodiments.

Claims

1. A method for identifying the dynamics of a manipulator based on data weighting and friction separation, characterized in that: The method comprises the following steps: S1. Obtain N sets of data based on the symmetrical excitation trajectory of the robot arm, and each set of data includes the position vector q and velocity vector of the joint on the robot arm. and the acceleration vector And the velocity vector of the motor on the robot arm Acceleration vector and the moment vector τ m ; According to q, τ m And the given joint nonlinear friction coefficient vector α, Stribeck velocity vector K v and the inverse tangent coefficient vector V s The initial value of constructs the regression matrix L under this set of data; α, K v and V s All are nonlinear friction parameters; S2, initialize the joint data weight vector P = ones (Nn, 1) and the stacked joint data weight vector P e =P·ones(1,r); n is the total number of links or joints, ones(Nn,1) is a column vector of N×n rows, and all elements of the column vector are 1; ones(1,r) is a row vector of r columns, and all elements of the row vector are 1; r is the number of columns of the matrix L; S3, according to τ under N groups of data m and L construct the corresponding intermediate variables and according to and Solving the OLS solution Ψ of the basis parameter set Ψ in the dynamic differential equation ols , and according to ols Solve for the standardized moment residual vector R # The initial value of R # The initial value of the joint covariance matrix σ is solved; S4, respectively according to and Construct the corresponding normalized motor torque vector and the normalized regression matrix according to and WLS solution of the basis parameter set Ψ in solving dynamic differential equations And according to Update R # and σ; S5. According to R # and σ, update P and P e ; S6, determine whether P converges, if yes, execute step S7, if no, execute step S4; S7, use the adjacent point data to and After replacing the outlier data with a weighted average, the optimization goal is set to solve the optimization equation solution of the dynamic parameter set χ and the basic parameter set Ψ in χ according to Solving for the estimated friction torque vector Thus, according to the estimated friction torque vector And the friction torque vector τ constructed f,m Update α, K in the regression matrix L v and V s ; S8. Determine whether α converges. If yes, set α and K v 、V s , And x is output as the result, if the result is no, execute step S2.

2. The method for mechanical arm dynamics identification based on data weighting and friction separation according to claim 1, characterized in that: In step S3, according to τ under N groups of data m and L construct the corresponding intermediate variables and The implementation method is: the motor torque vector τ under N sets of data m and the regression matrix L are stacked column by column, and the corresponding and 3. The method for mechanical arm dynamics identification based on data weighting and friction separation according to claim 1, characterized in that: In step S3, 4. The method for mechanical arm dynamics identification based on data weighting and friction separation according to claim 1, characterized in that: In step S3, R # The initial value R0 = τ m -LΨ ols , the initial value of the joint covariance matrix σ 5. The method for mechanical arm dynamics identification based on data weighting and friction separation according to claim 1, characterized in that: In step S4, according to and Construct the corresponding normalized motor torque vector and the normalized regression matrix according to and WLS solution of the basis parameter set Ψ in solving dynamic differential equations The implementation is: in, Re Nn×1 Represents a real number space with dimension Nn×1, Re Nn×r represents the real number space with dimension Nn×r, and All are intermediate variables.

6. The method for mechanical arm dynamics identification based on data weighting and friction separation according to claim 5, characterized in that: In step S4, according to Update R # The implementation of and σ is:

7. The method for mechanical arm dynamics identification based on data weighting and friction separation according to claim 1, characterized in that: In step S5, according to R # and σ, update P and P e The implementation is: P e,j =P j ·ones(1,r); Among them, P j is the element corresponding to joint j in P, j = 1...n, For R # The element corresponding to joint j in the j is the threshold value corresponding to joint j in the threshold vector δ, P e,j P e The element corresponding to the joint j in .

8. The method for mechanical arm dynamics identification based on data weighting and friction separation according to claim 1, characterized in that: In S7, use the adjacent point data to and The way to replace the outlier data with the weighted average is: Among them, s i and e i are the first and last element positions of the i-th group of abnormal data, for The position s of the middle joint j i The corresponding data, for The position s of the middle joint j i The data corresponding to the previous position of is the position e of joint j i The data corresponding to the next position for The position s of the middle joint j i The data corresponding to the row, for The position s of the middle joint j i All the data corresponding to the previous row of the row, for The position of the middle joint j i All data corresponding to the next row of the row; 0≤i<N; Set the optimization goal to solve the optimization equation solution of the dynamic parameter set χ and the basis parameter set Ψ The implementation is: Inequality constraints on χ in, Inertia parameter section Re (r-4n)×1 is a real number space with dimension (r-4n)×1, and the linear friction parameter part Re 4n×1 is a real number space with dimension (r-4n)×1, Ψ(·) is the mapping of χ to function, χ∈Re 15n×1 , Re 15n×1 Represents the real number space with dimension 15n×1; χ j =[l j m j r j m j I a,j F c,j F v,j F s,j F b,j ],j=1……n; χ j is the dynamic parameter of the jth joint, l j is the inertia tensor vector of joint j, m j r j is the first-order moment of joint j, m j is the mass of connecting rod j, I a,j is the equivalent inertia of the rotor and transmission system at joint j, F c,j is the Coulomb friction coefficient of joint j, F v,j is the viscous friction coefficient of joint j, F s,j is the static friction coefficient of joint j, F b,j is the friction offset coefficient of joint j; there is a one-to-one correspondence between the connecting rod and the joint; l j =[I xx,j I xy,j I xz,j I yy,j I yz,j I zz,j ] and I xx,j , I xy,j , I xz,j , I yy,j , I yz,j Japanese I zz,j separation Represents the moment of inertia of joint j around the x-axis, the product of inertia in the x-axis and y-axis directions, the product of inertia in the x-axis and z-axis directions, the inertia tensor around the y-axis, the product of inertia in the y-axis and z-axis directions, and the moment of inertia around the z-axis; m j r j =[m j r x,j m j r y,j m j r z,j ], and m j r x,j 、m j r y,j and m j r y,j They represent the first-order distance of joint j in the x direction, the first-order distance in the y direction, and the first-order distance in the z direction, respectively.

9. The method for mechanical arm dynamics identification based on data weighting and friction separation according to claim 8, characterized in that: Inequality constraints are imposed on the dynamic parameter set χ, including: The quality constraints are: In the formula, m j,min and m j,max are the lower and upper limits of the mass of connecting rod j, m min and m max They are the lower and upper limits of the total mass of the manipulator, respectively; The first-order moment constraint is expressed as: (m j r j ) min ≤m j r j ≤(m j r j ) max ; In the formula, (m j r j ) min and (m j r j ) max denote the lower and upper limits of the first-order moment of connecting rod j, respectively; The LMI-triangle inequality constraint is expressed as: Where LMI is the expression of the LMI-triangle inequality constraint, 1 3×3 ∈Re 3×3 is the identity matrix, Re 3×3 is a real number space with a dimension of 3×3, tr(·) represents the function of solving the trace of the input matrix, I j represents the inertia tensor matrix of joint j; The equivalent inertia constraint of the transmission system is expressed as: I a,j ＞0； The linear friction coefficient constraint is expressed as: F c,j >0,F v,j >0,F s,j >0。 10. The method for mechanical arm dynamics identification based on data weighting and friction separation according to claim 9, characterized in that: In step S7, In the formula, 0 4n×1 ∈Re 4n×1 is the zero vector, Re Nn×1 is a real number space with dimension Nn×1, Re 4n×1 is a real number space with dimension 4n×1; In step S7, according to the estimated friction torque vector And the friction torque vector τ constructed f,m Update α, K in the regression matrix L v and V s The implementation is: S71, from Extract is the estimated friction torque vector The element in the jth column of Re N×1 is a real number space with dimension N×1, S72, according to Optimize the solution α j 、V s,j and K v,j , so as to realize the v and V s The updates are as follows: in, The friction torque vector τ is constructed f,m The friction torque vector of joint j corresponding to the N groups of data, Re N×1 is a real number space with dimension N×1, τ f,mjw for The friction torque of joint j corresponding to the wth group of data, w = 1, 2...N, is the optimized variable set of joint j, and F c,j 、F v,j 、F s,j 、F b,j Obtained from χ, α j 、V s,j and K v,j are the nonlinear friction coefficient, Stribeck velocity and inverse tangent coefficient of joint j, is the motor speed vector The wth group of data corresponds to the speed of the motor corresponding to the joint j, and e is a natural constant.

Images