Robot dynamic parameter identification method and system based on projective geometry algebra

Abstract

The present invention provides a method and system for identifying robot dynamic parameters based on projective geometric algebra, comprising: an acquisition step: establishing dynamic equations based on the robot kinematic model and organizing them into linear equations on a positive definite matrix manifold; generating an optimal excitation trajectory based on the condition number of the linear equation coefficient matrix and acquiring data; a processing step: preprocessing the acquired data, filtering and identifying friction parameters to eliminate the influence of friction on joint torque; and a solution step: solving the processed data using optimization techniques on a symmetric positive definite matrix manifold to obtain physically achievable dynamic parameters of each rigid body. The present invention can solve the dynamic parameter identification problem of a general robot model when passive joint friction is negligible, obtain the inertia matrix of each rigid body and the friction parameters of the active joints, and strictly maintain the physical achievability of the dynamic parameters.

Description

Technical Field

[0001] The present invention relates to the field of robot dynamics parameter identification, and in particular to a robot dynamics parameter identification method and system based on projective geometry algebra. Background Art

[0002] A robot's dynamic parameters generally include inertial parameters such as the mass, center of mass position, and moment of inertia of each link, as well as joint friction parameters. Accurately identifying these parameters is the foundation for further model-based motion planning and control of the system. Because the generalized forces on the robot's active joints are linear with respect to the inertial parameters, the robot's inverse dynamics equations can always be formulated as linear equations relating the generalized forces to the inertial parameters. Currently, commonly used dynamic parameter identification methods are all based on this linear equation, primarily including least squares methods, semidefinite programming methods, and neural network methods. There are still some problems with the existing methods: 1) The least squares method can only estimate the minimum linearly independent parameter set of inertia parameters, and cannot guarantee the physical feasibility of the dynamic parameters, that is, it cannot guarantee that the solved mass is positive, the inertia tensor is positive definite, and satisfies the triangle inequality; 2) The semi-definite programming method can obtain physically realizable inertia parameters, but when there is a lack of prior knowledge or the confidence level of the prior is not high, the solution often results in a semi-definite result, that is, the mass is 0 or the inertia tensor is semi-positive definite; 2) Neural network methods lack the support of physical models, have poor interpretability, and require a lot of time to train. The resulting model is difficult to apply in actual engineering scenarios.

[0003] Chinese patent document CN114800519A, "A friction-considered method for identifying dynamic parameters of a six-degree-of-freedom industrial robot," uses a least-squares approach to identify dynamic parameters, taking into account joint friction and motor parameters. It also leverages a heuristic algorithm to further improve the accuracy of dynamic parameter identification. However, this method fails to consider the physical feasibility of rigid body inertia parameters, which can cause problems in motion planning and control scenarios involving flexible joints.

[0004] Chinese patent document CN109773794A, "A Method for Identifying Dynamic Parameters of a Six-Axis Robot Based on a Neural Network," uses a neural network approach to identify dynamic parameters. The neural network input is an observation matrix derived from a physical model, and the output is torque. This method can identify the dynamic parameters of a robot system with complex joints. However, this method also fails to consider the physical feasibility of rigid body inertia parameters, and the introduction of a neural network makes the model less interpretable. Summary of the Invention

[0005] In view of the defects in the prior art, the purpose of the present invention is to provide a robot dynamic parameter identification method and system based on projective geometry algebra.

[0006] According to the present invention, a robot dynamic parameter identification method based on projective geometric algebra is provided, comprising:

[0007] Acquisition steps: Based on the robot kinematic model, the dynamic equation is established and organized into a linear equation on a positive definite matrix manifold. The optimal excitation trajectory is generated based on the condition number of the linear equation coefficient matrix and data is collected;

[0008] Processing steps: pre-process the collected data, filter and identify friction parameters, and eliminate the influence of friction on joint torque;

[0009] Solution steps: Based on the processed data, use the optimization technology on the manifold to solve the problem on the symmetric positive definite matrix manifold to obtain the physically achievable dynamic parameters of each rigid body.

[0010] Preferably, the collecting step includes:

[0011] Step 1.1: Based on the robot's kinematic model, calculate the position, velocity, and acceleration of each rigid body, as well as the position, velocity, and acceleration of the triple basis vectors attached to the rigid body. The rigid body's position is an even-numbered unit multivector, its velocity and acceleration are both two-dimensional vectors, and the position, velocity, and acceleration of the triple basis vectors are all triple vectors.

[0012] Step 1.2: Calculate the inertial force regression matrix for each rigid body. Define the inertial force regression matrix as a 4th-order matrix where each element is a two-dimensional vector and is only related to the kinematic parameters. Define the inertia matrix as a 4th-order symmetric positive definite matrix over the real number field. According to the relevant formulas of projective geometry algebra, the inertial force spinor of each rigid body is calculated by the matrix inner product of these two 4th-order matrices.

[0013] Step 1.3: Calculate the generalized force regression matrix for each rigid body corresponding to each generalized force, and organize the dynamic equations of the robot system into linear equations defined on the positive definite matrix manifold. The contribution of the inertial force of a link of the robot system to a generalized force is also calculated by the inner product of two 4th-order matrices, one of which is the inertia matrix of the rigid body, and the other is defined as the generalized force regression matrix, which is calculated from the kinematic model of the rigid body and the inertial force regression matrix. It is a 4th-order matrix defined on the real number field. After organization, the dynamic equation of the robot system is Q= R * N The expression of , where Q is the generalized force vector, defined on the real number field; R is a special matrix, the i-th row and j-th column of the matrix are the generalized force regression matrix of the inertia of connecting rod j to the generalized force i; N is a special vector, the jth element is the inertia matrix of link j;

[0014] Step 1.4: Parameterize the robot joint space trajectory and transform the special matrix of step 1.3 into R Reshape into a regression matrix on the real field Y , analyze the minimum parameter set and obtain the minimum parameter set regression matrix Y b , regression matrix with the minimum parameter set Y b The condition number of is taken as the objective function, and the optimal excitation trajectory for dynamic parameter identification is obtained by optimization.

[0015] Preferably, the gradient of the objective function is computed analytically using an algorithm in projective geometry algebra.

[0016] Preferably, the processing step comprises:

[0017] Use Butterworth filtering or other filtering methods to remove noise generated during the signal acquisition process in the joint motion data and joint torque data;

[0018] The least squares method is used to preliminarily identify the system's dynamic parameters, including inertia parameters and friction parameters.

[0019] Preferably, the solving step includes: based on the linear equation, summarizing the physical feasibility of the dynamic parameters as the inertia matrix of the rigid body on the symmetric positive definite matrix manifold, the inverse dynamics of the rigid body is a linear mapping from the manifold to the generalized force space, and there is an expression given in step 1.3, in the embedded space of the symmetric positive definite matrix manifold, analytically writing the gradient and Hessian transformation, and solving by a second-order optimization algorithm on the manifold.

[0020] According to the present invention, a robot dynamic parameter identification system based on projective geometric algebra is provided, comprising:

[0021] Acquisition module: Based on the robot kinematic model, the dynamic equation is established and organized into a linear equation on the positive definite matrix manifold. The optimal excitation trajectory is generated based on the condition number of the linear equation coefficient matrix and data is collected;

[0022] Processing module: pre-processes the collected data, filters and identifies friction parameters, and eliminates the influence of friction on joint torque;

[0023] Solution module: Based on the processed data, the optimization technology on the manifold is used to solve the problem on the symmetric positive definite matrix manifold to obtain the physically achievable dynamic parameters of each rigid body.

[0024] Preferably, the acquisition module includes:

[0025] Module M1.1: Based on the robot's kinematic model, calculate the position, velocity, and acceleration of each rigid body, as well as the position, velocity, and acceleration of the triple basis vectors attached to the rigid body. The position of the rigid body is an even-numbered unit multivector, the velocity and acceleration are both two-way vectors, and the position, velocity, and acceleration of the triple basis vectors are all triplets.

[0026] Module M1.2: Calculate the inertial force regression matrix for each rigid body. Define the inertial force regression matrix as a 4th-order matrix where each element is a two-dimensional vector and is only related to the kinematic parameters. Define the inertia matrix as a 4th-order symmetric positive definite matrix over the real number field. According to the relevant formulas of projective geometry algebra, the inertial force spinor of each rigid body is calculated as the matrix inner product of these two 4th-order matrices.

[0027] Module M1.3: Calculate the generalized force regression matrix for each rigid body corresponding to each generalized force, and organize the dynamic equations of the robot system into linear equations defined on the positive definite matrix manifold. The contribution of the inertial force of a link in the robot system to a generalized force is also calculated by the inner product of two 4th-order matrices, one of which is the inertia matrix of the rigid body, and the other is defined as the generalized force regression matrix, which is calculated from the kinematic model of the rigid body and the inertial force regression matrix. It is a 4th-order matrix defined on the real number field. After organization, the dynamic equation of the robot system is Q= R * N The expression of , where Q is the generalized force vector, defined on the real number field; R is a special matrix, the i-th row and j-th column of the matrix are the generalized force regression matrix of the inertia of connecting rod j to the generalized force i; N is a special vector, the jth element is the inertia matrix of link j;

[0028] Module 1.4: Parameterize the robot joint space trajectory and transform the special matrix of module M1.3 into R Reshape into a regression matrix on the real field Y , analyze the minimum parameter set and obtain the minimum parameter set regression matrix Y b , regression matrix with the minimum parameter set Y b The condition number of is taken as the objective function, and the optimal excitation trajectory for dynamic parameter identification is obtained by optimization.

[0029] Preferably, the gradient of the objective function is computed analytically using an algorithm in projective geometry algebra.

[0030] Preferably, the processing module includes:

[0031] Use Butterworth filtering or other filtering methods to remove noise generated during the signal acquisition process in the joint motion data and joint torque data;

[0032] The least squares method is used to preliminarily identify the system's dynamic parameters, including inertia parameters and friction parameters.

[0033] Preferably, the solution module includes: based on the linear equation, the physical feasibility of the dynamic parameters is summarized as the inertia matrix of the rigid body on the symmetric positive definite matrix manifold, the inverse dynamics of the rigid body is a linear mapping from the manifold to the generalized force space, and there is an expression given in module 1.3. In the embedded space of the symmetric positive definite matrix manifold, the gradient and Hessian transformation are analytically written, and the solution is obtained by the second-order optimization algorithm on the manifold.

[0034] Compared with the prior art, the present invention has the following beneficial effects:

[0035] The present invention can solve the problem of dynamic parameter identification of general robot models when the passive joint friction is negligible, obtain the inertia matrix of each rigid body and the friction parameters of the active joint, and strictly maintain the physical feasibility of the dynamic parameters. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Other features, objects and advantages of the present invention will become more apparent upon reading the detailed description of non-limiting embodiments with reference to the following drawings:

[0037] Figure 1 Flowchart of the present invention;

[0038] Figure 2 Compute reference graphs for projective geometric algebra;

[0039] Figure 3 This is the initial state diagram of the 6-DOF serial industrial robot PUMA560;

[0040] Figure 4 is the optimal excitation trajectory curve in joint space;

[0041] Figure 5 is the joint torque curve after adding noise;

[0042] Figure 6 A comparison chart of the torque estimation results of the three methods. DETAILED DESCRIPTION

[0043] The present invention will be described in detail below with reference to specific embodiments. The following examples will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those skilled in the art, several changes and improvements can be made without departing from the scope of the present invention. These all fall within the scope of protection of the present invention.

[0044] like Figure 1As shown in the figure, a method for identifying robot dynamic parameters based on projective geometric algebra includes the following steps: 1. Based on the robot's kinematic model, the dynamic equations are established and organized into linear equations on a positive definite matrix manifold. Based on the condition number of the linear equation coefficient matrix, the optimal excitation trajectory is generated and data is collected. 2. The data is preprocessed, filtered, and friction parameters are identified to eliminate the influence of friction on the generalized force. 3. On the symmetric positive definite matrix manifold, optimization techniques are used to solve the problem and obtain the physically achievable dynamic parameters of each rigid body.

[0045] Step 1 includes:

[0046] Step 1.1: Based on the robot's kinematic model, calculate the position, velocity, and acceleration of each rigid body, as well as the position, velocity, and acceleration of the triple basis vectors attached to the rigid body. The rigid body's position is an even-multiplicity unit multivector, its velocity and acceleration are both two-multiplicity vectors, and the position, velocity, and acceleration of the triple basis vectors are all triplets. An n-multiplicity vector is a concept from projective geometry algebra.

[0047] According to the robot's kinematic parameters, the robot's kinematic model is established. Assuming that each link of the robot is a rigid body, the Jacobian between the velocity of each rigid body and the joint velocity is analyzed, and the position, velocity, and acceleration of each rigid body are calculated. Indicated in. It is a 16-dimensional linear space, and its basis vectors are distinguished by multiplicity, including 0-multiplicity vectors (scalars), vectors, bi-multiplicity vectors, triplicate vectors, and quadruple vectors (pseudo-scalars). In this linear space, a bilinear operator, the geometric product, is defined between basis vectors. The geometric product operation between bases is as follows: Figure 2 As shown. The pose M of rigid body i i is the sum of a scalar, a bivector, and a pseudoscalar, and is an even-multiplicity multivector satisfying the constraint M i The double vector part of is negative. The velocity V i With acceleration are both expressed as two vectors. The Jacobian of the rigid body velocity with respect to the generalized coordinate velocity can be written as follows:

[0048]

[0049] Where n is the number of degrees of freedom of the system, L j is a double vector.

[0050] In projective geometry algebra In , points are represented by triple vectors, and the coordinates of the points in Euclidean space are expressed as:

[0051] P=xE1+yE2+zE3+E0 (2)

[0052] Where E1, E2, E3, and E0 are the basis vectors of the triple vector shown in Table 1, and the coordinate matrix of the point is

[0053] P =[x,y,z,1] T (3)

[0054] The position, velocity, and acceleration of a point after it moves with the rigid body are expressed as:

[0055]

[0056]

[0057]

[0058] According to formulas (4)-(6), calculate the position E′ of the triple vector basis vector Ek after the rigid body i moves i，k ,speed acceleration In the above steps, i=1, 2, ..., N B , N B is the number of rigid bodies; k = 0, 1, 2, 3.

[0059] Step 1.2: Calculate the inertial force regression matrix for each rigid body. Define the inertial force regression matrix as a special 4th-order matrix where each element is a two-dimensional vector and is related only to the kinematic parameters. Define the inertia matrix as a 4th-order symmetric positive definite matrix over the real number field. According to the relevant formulas in projective geometry algebra, the inertial force spinor of each rigid body is calculated as the inner product of these two 4th-order matrices.

[0060] The dynamic equation of rigid body i is expressed as

[0061] w=tr(N T A):=N*A (7)

[0062] in

[0063] w=f x e 23 +f y e 31 +f z e 12 +m x e 01 +m y e 02 +m z e 03 (8)

[0064] is the sum of all the forces acting on the rigid body, [f x , fy , f z ] T is the principal vector of all external forces acting on the rigid body, [m x , m y , m z ] T is the principal moment of the rigid body relative to the origin of the inertial system;

[0065]

[0066] is the inertia matrix of the rigid body, where Ω is the spatial extent of the rigid body; m is the mass of the rigid body; c is the position of the center of mass of the rigid body; and ∑ is the moment of inertia of the rigid body. The physical realizability of the inertia parameters of the rigid body means that the matrix is ​​positive definite, that is, N is on the 4th-order symmetric positive definite matrix manifold SPD(4).

[0067] A i It is a special matrix related only to the motion of rigid body i, defined as the inertial force regression matrix, where the element in the kth row and lth column is a two-vector, regarded as a number, and the expression is,

[0068]

[0069] Wherein, the operation V is defined as:

[0070]

[0071] Among them, P1 and P2 are triple vectors; is a one-to-one mapping from triple vectors to vectors, satisfying

[0072] J(E i )=e i (12)

[0073] The operation ∧ is the outer product. The outer product of two vectors a and b is defined as

[0074] a∧b=0.5(ab-ba) (13)

[0075] When the outer product is generalized to multivectors, it represents the vector component with the highest multiplicity in the geometric product of two multivectors. For example, the outer product of two binary vectors is the pseudo-scalar component of the multivector obtained by performing the geometric product operation on the two vectors.

[0076] Step 1.3: Calculate the generalized force regression matrix for each rigid body corresponding to each generalized force, and organize the dynamic equations of the robot system into linear equations defined on a positive definite matrix manifold. The contribution of the inertial force of a certain link of the robot system to a certain generalized force is also calculated by the inner product of two 4th-order matrices, one of which is the inertia matrix of the rigid body, and the other is defined as the generalized force regression matrix, which is calculated from the kinematic model of the rigid body and the inertial force regression matrix and is a 4th-order matrix defined on the real number field. After organization, the dynamic equations of the robot system are of the form Q= R * N Where Q is the generalized force vector, defined in the real field; R is a special matrix, the i-th row and j-th column of the matrix are the generalized force regression matrix of the inertia of connecting rod j to the generalized force i; N is a special vector, the jth element of which is the inertia matrix of link j.

[0077] According to the D'Alembert principle and the virtual power principle, the dynamic equation of the robot system is obtained:

[0078]

[0079] in, The main driving force, the virtual velocity of connecting rod i ΔV i and generalized coordinate imaginary velocity Satisfaction relationship

[0080]

[0081] After substituting, we get

[0082]

[0083] in, is the corresponding generalized coordinate q j The generalized force is the resultant torque of the joint torque and the friction torque. The linear independence of the generalized coordinates and the linearity of the matrix trace give n equations:

[0084]

[0085] Among them, the generalized force regression matrix is ​​defined as R ji =L j,i ∧A i , which represents the coefficient matrix of the contribution of the inertia of link i to the generalized force j. j,i ∧A i Indicates L j,i With A i Each double vector in the outer product operation is performed, and the result is a pseudo scalar. In actual operation, the scalar is directly mapped to the scalar space, so R jiEach element of is a scalar real number, and the expression of the kth row and lth column is

[0086]

[0087] By combining and arranging the set of equations, we can obtain the dynamic equation of the robot system:

[0088]

[0089] If the robot system moves along a certain trajectory, N measurements are performed, and the motion data q1, q2, ...q of the active joint are obtained through sensor measurement. N With torque data Q1, Q2…Q N , each measurement satisfies the kinetic equation

[0090] Q= R * N +ε (20)

[0091] Where ε is the error caused by measurement noise, etc.

[0092] After grouping, we get the equation

[0093]

[0094] in,

[0095] The problem of dynamic parameter identification is summarized as manifold SPD(4) n Unconstrained optimization problem on

[0096]

[0097] where ||·||2 is the 2-norm of the vector.

[0098] Step 1.4: Parameterize the robot joint space trajectory and transform the special matrix of step (1.3) R Reshape into a regression matrix on the real field Y , analyze the minimum parameter set and obtain the minimum parameter set regression matrix Y b The optimal excitation trajectory for dynamic parameter identification is obtained by optimizing the condition number of this matrix. The gradient of the objective function is calculated analytically using an algorithm in projective geometry algebra.

[0099] In order to obtain the optimal excitation trajectory, the original dynamic equation is rearranged as

[0100]

[0101] where vecN i N i Vectorization, due to Ni Symmetric, only 10 independent parameters, with vecN i =[(N i ) 11 ,(N i ) 12 ,(N i ) 13 ,(N i ) 22 ,(N i ) 23 ,(N i ) 33 ,(N i ) 14 ,(N i ) 24 ,(N i ) 34 ,(N i ) 44 ] T :=[MXX,MXY,MXZ,MYY,MYZ,MZZ,MX,MY,MZ,M] T (twenty four)

[0102] vecR ji R ji Vectorized, also for a 10-dimensional vector, there is

[0103] vecR ji =[(R ji ) 11 ,(R ji ) 12 +(R ji ) 21 ,(R ji ) 13 +(R ji ) 31 ,(R ji ) 22 ,(R ji ) 23 +(R ji ) 32 ,(R ji ) 33 ,(R ji ) 14 +(R ji ) 41 ,(R ji ) 24 +(R ji ) 42 ,(R ji ) 34 +(R ji ) 43 ,(R ji )44 ] T (25)

[0104] Given N times of measurement data, the kinetic equation is:

[0105]

[0106] The robot joint space trajectory is parameterized using Fourier series. For each active joint, we have

[0107]

[0108] The parameter vector of the joint is p = [a1, b1, a2, b2, ..., a K , b K ,c], the parameter set of all joint trajectories is a vector p ;ω k Select according to the sensor performance and motor performance. At this time, the regression matrix Y = Y ( p ).

[0109] The generation of the optimal excitation trajectory is reduced to the optimization problem of the parameter space of the trajectory.

[0110]

[0111] subjectto p ∈C

[0112] in, Y b is the regression matrix of the minimum parameter set, the matrix is ​​full rank and satisfies Y b π b = Y π; C is the set of constraints that the parameters must satisfy, including joint motion range, velocity range, acceleration range, start and end point constraints, etc.

[0113] In order to quickly solve the optimization problem, it is necessary to give the objective function cond( Y b ), the more difficult step is to find Y right According to the analytical expression of each term of the regression matrix provided by the above dynamic model, we can make full use of the calculation rules of projective geometric algebra to obtain a more efficient algorithm. The specific algorithm can be referred to the relevant papers.

[0114] Step 2: Data preprocessing is carried out in two steps. In the first step, the noise generated by the signal acquisition process in the joint motion data and joint torque data is removed by Butterworth filtering or other filtering methods. In the second step, the least squares method is used to preliminarily identify the dynamic parameters of the system, including inertia parameters and friction parameters. Among them, the inertia parameters are the parameters of the minimum parameter set and are not necessarily physically achievable parameters. The torque generated by inertia is optimally estimated in the sense of least squares. The friction parameters are optimally estimated in the sense of least squares for the joint friction force. By removing the friction component in the joint torque data, the torque data related only to the robot inertia is obtained, that is, the data T in formula (21). If the joint friction force can be ignored by measurement or other methods, this step can be omitted.

[0115] Step 3, use the positive definite matrix manifold SPD (4) n The optimization algorithm on , solves the inertia matrix of each rigid body, that is

[0116]

[0117] Thanks to the explicit expression of the robot dynamics equation obtained in step 1, the analytical expressions of the gradient and Hessian transform of the objective function on the manifold embedded in the Euclidean space can be written, where

[0118]

[0119]

[0120] in, U For N The matrix vector of the same size, that is, each element in the vector is a 4-order matrix. In the above operations involving transposition and vector inner product, the generalized force regression matrix and the inertia matrix N The 4th-order matrix as an element in is treated as a single number in the operation, rather than a block matrix, such as

[0121]

[0122]

[0123] The following describes the implementation of the present invention in detail by taking the 6-DOF serial industrial robot PUMA560 as an example in conjunction with the accompanying drawings, but the protection scope of the present invention is not limited to the following embodiments.

[0124] The specific process of this embodiment includes: first, establishing a kinematic model based on the kinematic parameters of the robot model; then, randomly generating data on joint position, velocity, and acceleration, calculating the regression matrix and analyzing the minimum parameter set; secondly, taking the condition number of the minimum parameter set regression matrix as the objective function, parameterizing the joint space trajectory with the Fourier series, optimizing and obtaining the optimal excitation trajectory, and outputting the joint position, velocity, acceleration, and torque when moving along the excitation trajectory from the robot simulation model, and adding white noise to the torque data; finally, calculating the regression matrix and the gradient and Hessian transform of the dynamic parameter identification objective function based on the data from the previous step, and solving the dynamic parameters using the optimization algorithm on the manifold.

[0125] In the following examples, the robot model used is the 6-DOF serial industrial robot PUMA560, and the initial state is as follows: Figure 3 According to the MDH parameters of the robot, when the robot is in the initial state, the six joint axes are expressed as two vectors:

[0126]

[0127]

[0128]

[0129]

[0130]

[0131]

[0132] The rigid body motion caused by joint i is,

[0133]

[0134] The rigid body motion of connecting rod i is

[0135]

[0136] After the rigid body motion, the joint axis i is

[0137]

[0138] The speed of connecting rod i is

[0139]

[0140] The acceleration of connecting rod i is

[0141]

[0142] Where g is the acceleration due to gravity, which is 9.81e 03 .

[0143] According to the above formula, the dynamic equation of the robot system is calculated as follows:

[0144]

[0145] According to formulas (24)-(25), the rearranged form is

[0146] τ=Yπ=Y b π b

[0147] Among them, the minimum parameters are 36, namely π b is a 36-dimensional vector.

[0148] The joint space trajectory is parameterized using trigonometric functions with frequencies of 0.1Hz, 0.2Hz, and 0.4Hz, and an optimized excitation trajectory is obtained using the dynamic model proposed in this invention. The trajectory parameters of the six joints are

[0149] Table 1 Parameters of optimal excitation trajectory

[0150]

[0151] This trajectory is a periodic trajectory with a period of 10s. The joint trajectory is as follows Figure 4 As shown. Take the sampling rate of 1kHz and collect joint position q and joint velocity along the excitation trajectory. Joint acceleration Set the inertia matrix of each link to be. Use the inverse dynamics algorithm to obtain the joint torque, add white noise, and the white noise of each joint is independent and has a mean of 0 and a variance of σ. 2 Gaussian distribution, σ 2 =20, the simulation torque obtained is as follows Figure 5 shown.

[0152] Using the method of the present invention, take σ 2 = 0, 1, 20. Based on the generated data, dynamic parameter identification was performed to obtain the dynamic parameters of each rigid body. A random trajectory was then generated, and the identified parameters were used to predict joint torques. The results were compared with the actual torques to evaluate the identification results. Furthermore, the results were compared with those based on iterative least squares and semi-positive definite programming, as shown in Table 2.

[0153] It can be seen that the method of the present invention can be used to identify dynamic parameters. Under the same noise level, the same torque estimation accuracy as other methods can be guaranteed. 2 =20, the three methods estimate the torque as follows Figure 6 As shown in the figure, it can be seen that the three methods have comparable prediction effects on torque; the dynamic parameters obtained by this method are strictly physically feasible and closer to the true values ​​of each rigid body; when the measurement noise increases, this method is more robust to parameter identification, which is reflected in the fact that the distance between the identification result and the true value does not change much.

[0154] Table 2 Comparison of kinetic parameter identification results

[0155]

[0156] The present invention also provides a robot dynamic parameter identification system based on projective geometric algebra. The robot dynamic parameter identification system based on projective geometric algebra can be implemented by executing the process steps of the robot dynamic parameter identification method based on projective geometric algebra, that is, those skilled in the art can understand the robot dynamic parameter identification method based on projective geometric algebra as a preferred implementation of the robot dynamic parameter identification system based on projective geometric algebra.

[0157] A robot dynamic parameter identification system based on projective geometry algebra, comprising:

[0158] Acquisition module: Based on the robot kinematic model, the dynamic equation is established and organized into a linear equation on the positive definite matrix manifold. The optimal excitation trajectory is generated and data is collected based on the condition number of the linear equation coefficient matrix.

[0159] Processing module: pre-processes the collected data, filters and identifies friction parameters, and eliminates the influence of friction on joint torque.

[0160] Solution module: Based on the identified friction parameters, the optimization technology on the symmetric positive definite matrix manifold is used to solve and obtain the physically achievable dynamic parameters of each rigid body.

[0161] Those skilled in the art will appreciate that, in addition to implementing the system and its various devices, modules, and units provided by the present invention in purely computer-readable program code, it is entirely possible to implement the same functions of the system and its various devices, modules, and units provided by the present invention in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; the devices, modules, and units for implementing various functions can also be considered as both software modules implementing the method and structures within the hardware component.

[0162] The above describes specific embodiments of the present invention. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art may make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. The embodiments of this application and the features in the embodiments may be combined with each other in any manner unless there is a conflict.

Claims

1. A robot dynamic parameter identification method based on projective geometry algebra, characterized in that: include: Acquisition steps: Based on the robot kinematic model, the dynamic equation is established and organized into a linear equation on a positive definite matrix manifold. The optimal excitation trajectory is generated based on the condition number of the linear equation coefficient matrix and data is collected; Processing steps: pre-process the collected data, filter and identify friction parameters, and eliminate the influence of friction on joint torque; Solution steps: Based on the processed data, use optimization techniques on the symmetric positive definite matrix manifold to solve and obtain the physically achievable dynamic parameters of each rigid body; The collection step includes: Step 1.1: Based on the robot's kinematic model, calculate the position, velocity, and acceleration of each rigid body, as well as the position, velocity, and acceleration of the triple basis vectors attached to the rigid body. The rigid body's position is an even-numbered unit multivector, its velocity and acceleration are both two-dimensional vectors, and the position, velocity, and acceleration of the triple basis vectors are all triple vectors. Step 1.2: Calculate the inertial force regression matrix for each rigid body. Define the inertial force regression matrix as a 4th-order matrix where each element is a two-dimensional vector and is only related to the kinematic parameters. Define the inertia matrix as a 4th-order symmetric positive definite matrix over the real number field. According to the relevant formulas of projective geometry algebra, the inertial force spinor of each rigid body is calculated by the matrix inner product of these two 4th-order matrices. Step 1.3: Calculate the generalized force regression matrix for each rigid body corresponding to each generalized force, and organize the dynamic equations of the robot system into linear equations defined on the positive definite matrix manifold. The contribution of the inertial force of a link of the robot system to a generalized force is also calculated by the inner product of two 4th-order matrices, one of which is the inertia matrix of the rigid body, and the other is defined as the generalized force regression matrix, which is calculated by the kinematic model of the rigid body and the inertial force regression matrix. It is a 4th-order matrix defined on the real number field. After organization, the dynamic equations of the robot system are: The expression of , where is a generalized force vector, defined over the real number field; is a special matrix, the matrix Rank Classified as connecting rod The inertia of the generalized force The generalized force regression matrix of ; is a special vector, The connecting rod The inertia matrix of Step 1.4: Parameterize the robot joint space trajectory and transform the special matrix of step 1.3 into Reshape into a regression matrix on the real field , analyze the minimum parameter set and obtain the minimum parameter set regression matrix , regression matrix with the minimum parameter set The condition number of is taken as the objective function, and the optimal excitation trajectory for dynamic parameter identification is obtained by optimization.

2. The robot dynamic parameter identification method based on projective geometry algebra according to claim 1, characterized in that: The gradient of the objective function is computed analytically by an algorithm in projective geometric algebra.

3. The robot dynamic parameter identification method based on projective geometry algebra according to claim 1, characterized in that: The processing steps include: Use Butterworth filtering or other filtering methods to remove noise generated during the signal acquisition process in the joint motion data and joint torque data; The least squares method is used to preliminarily identify the system's dynamic parameters, including inertia parameters and friction parameters.

4. The robot dynamic parameter identification method based on projective geometry algebra according to claim 1, characterized in that: The solution steps include: based on the linear equation, summarizing the physical feasibility of the dynamic parameters as the inertia matrix of the rigid body on the symmetric positive definite matrix manifold, the inverse dynamics of the rigid body is a linear mapping from the manifold to the generalized force space, and there is an expression given in step 1.

3. In the embedded space of the symmetric positive definite matrix manifold, the gradient and Hessian transformation are analytically written, and the solution is obtained through a second-order optimization algorithm on the manifold.

5. A robot dynamic parameter identification system based on projective geometry algebra, characterized by: include: Acquisition module: Based on the robot kinematic model, the dynamic equation is established and organized into a linear equation on the positive definite matrix manifold. The optimal excitation trajectory is generated based on the condition number of the linear equation coefficient matrix and data is collected; Processing module: pre-processes the collected data, filters and identifies friction parameters, and eliminates the influence of friction on joint torque; Solving module: Based on the processed data, the optimization technology on the symmetric positive definite matrix manifold is used to solve and obtain the physically achievable dynamic parameters of each rigid body; The acquisition module includes: Module M1.1: Based on the robot's kinematic model, calculate the position, velocity, and acceleration of each rigid body, as well as the position, velocity, and acceleration of the triple basis vectors attached to the rigid body. The position of the rigid body is an even-numbered unit multivector, the velocity and acceleration are both two-way vectors, and the position, velocity, and acceleration of the triple basis vectors are all triplets. Module M1.2: Calculate the inertial force regression matrix for each rigid body. Define the inertial force regression matrix as a 4th-order matrix where each element is a two-dimensional vector and is only related to the kinematic parameters. Define the inertia matrix as a 4th-order symmetric positive definite matrix over the real number field. According to the relevant formulas of projective geometry algebra, the inertial force spinor of each rigid body is calculated as the matrix inner product of these two 4th-order matrices. Module M1.3: Calculate the generalized force regression matrix for each rigid body corresponding to each generalized force, and organize the dynamic equations of the robot system into linear equations defined on the positive definite matrix manifold. The contribution of the inertial force of a link of the robot system to a generalized force is also calculated by the inner product of two 4th-order matrices, one of which is the inertia matrix of the rigid body, and the other is defined as the generalized force regression matrix, which is calculated by the kinematic model of the rigid body and the inertial force regression matrix. It is a 4th-order matrix defined on the real number field. After organization, the dynamic equations of the robot system are: The expression of , where is a generalized force vector, defined over the real number field; is a special matrix, the matrix Rank Classified as connecting rod The inertia of the generalized force The generalized force regression matrix of ; is a special vector, The connecting rod The inertia matrix of Module 1.4: Parameterize the robot joint space trajectory and transform the special matrix of module M1.3 into Reshape into a regression matrix on the real field , analyze the minimum parameter set and obtain the minimum parameter set regression matrix , regression matrix with the minimum parameter set The condition number of is taken as the objective function, and the optimal excitation trajectory for dynamic parameter identification is obtained by optimization.

6. The robot dynamic parameter identification system based on projective geometry algebra according to claim 5, characterized in that: The gradient of the objective function is computed analytically by an algorithm in projective geometric algebra.

7. The robot dynamic parameter identification system based on projective geometry algebra according to claim 5, characterized in that: The processing module includes: Use Butterworth filtering or other filtering methods to remove noise generated during the signal acquisition process in the joint motion data and joint torque data; The least squares method is used to preliminarily identify the system's dynamic parameters, including inertia parameters and friction parameters.

8. The robot dynamic parameter identification system based on projective geometry algebra according to claim 5, characterized in that: The solution module includes: based on the linear equation, the physical feasibility of the dynamic parameters is summarized as the inertia matrix of the rigid body on the symmetric positive definite matrix manifold, the inverse dynamics of the rigid body is a linear mapping from the manifold to the generalized force space, and there is an expression given by module M1.

3. In the embedded space of the symmetric positive definite matrix manifold, the gradient and Hessian transformation are analytically written, and the solution is obtained through the second-order optimization algorithm on the manifold.

Images