A method for identifying and estimating unknown external forces of a robotic arm based on residual dynamic learning
Through the residual dynamic learning method based on Gaussian process, a composite dynamic model and an adaptive Kalman filter were established, which solved the problem of tactile force estimation caused by inaccurate model in the robotic arm system, and achieved high-precision and anti-interference tactile external force estimate.
Patent Information
- Application Number
- CN202211289315.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-10-20
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2042-10-20
AI Technical Summary
In robotic arm systems, when the model and contact external force models are inaccurate, it is difficult for traditional methods to achieve high-precision sensorless haptic force estimation, especially in the presence of unmodeled residual dynamics and multiple interferences.
Using the residual dynamic learning method based on Gaussian process, establish a composite dynamic model and enhance the Gaussian process interactive force estimation system model, design an adaptive Kalman filter, and achieve approximation of the model noise characteristics through offline learning and online compensation, and improve touch sense. The accuracy of external force estimation and anti-interference ability.
It realizes high-precision tactile external force estimation when the robotic arm model and contact external force model are inaccurate, has good anti-interference ability and engineering practicality, and is suitable for multi-scene multi-type force interactive robotic arm systems.
Smart Images

Figure CN115488896B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of robot arm design, and specifically relates to a method for identifying and estimating unknown external forces of a robot arm based on residual dynamic learning, which solves the problem of estimating sensorless tactile forces of a robot arm when both the robot arm model and the contact external force model are inaccurate. Background Art
[0002] As an intelligent substitute for human interaction with the environment, robots have increasingly helped humans to remotely acquire and perceive environmental information. However, the tactile information perception of robots is mostly achieved through external force sensors, which greatly limits the application scenarios of robots with tactile perception. The sensorless external force estimation method based on the dynamic inversion solution of the measurement information and the dynamic model information of the manipulator itself has gained widespread attention. However, this model-based external force estimation method is limited by the assumption of the accuracy of the external force and the manipulator system model. In the face of the manipulator system that is difficult to accurately model and the external force model is not accurately known, the sensorless external force estimation method often cannot give satisfactory estimation results. Specifically, due to the presence of other multi-source and multi-type interferences coupled with the external force channel in the manipulator system, the tactile external force is no longer observable, so it is difficult to accurately separate and estimate the tactile external force. The existence of the above problems makes it difficult to directly apply the traditional unknown external force identification and estimation method. Therefore, combined with the existing technology, it is very important to design a sensorless tactile external force identification and estimation method based on an imprecise model, which has broad application prospects.
[0003] At present, the research on sensorless tactile force estimation is mainly focused on more accurate identification of robotic arm systems, and most of them do not consider modeling errors. There are few studies on effective estimation technology of tactile external forces in the case of residual dynamic coupling. The paper "Manipulator Load Estimation Based on Optimal Excitation Posture Sequence" uses the robot statics method to calculate the load parameter model, and identifies the joint load through the load parameter offline identification method of joint torque and position information, achieving good load force estimation results. The paper "Research and Application of Robot Arm External Force Estimation and Control Method" proposes a method based on H ∞ The above three methods are based on simplified robot arm models, ignoring many unmodeled nonlinear dynamics in the original system that are coupled with external forces, and it is difficult to ensure the estimation accuracy. Chinese invention patent CN201410112717.5 proposes a method based on H ∞ Aiming at the problem of estimating the external force of the actual robotic arm system, the analog low-pass filter is used to filter the signal in the dynamic equation, and the external force is expanded and then based on H ∞Filtering estimates external force without measuring acceleration signals, but this method does not consider the actual model uncertainty, which affects the accuracy of external force estimation. Chinese invention patent CN201710834869.X proposes an external force estimation method and device, which uses a multi-layer perceptron to model and compensate for modeling errors, and realizes external force estimation without external force sensors. By introducing a Kalman filter, it achieves a certain degree of robustness in the presence of observation noise and model errors, but does not consider the confidence of error modeling, and the modeling method using a multi-layer perceptron is not interpretable. The amount of data required for modeling parameterized models is very large, which is not suitable for actual engineering applications. Chinese invention patent CN202210279718.3 proposes a method for estimating tactile external force of a robotic arm based on a mechanism data hybrid model. The Gaussian process is used to model and compensate for modeling errors. The learned time-varying covariance matrix is introduced into the design of the Kalman filter gain, and the uncertainty information of Gaussian process modeling is used to improve the accuracy of force estimation. However, this method does not consider the prior information of the dynamic model of the contact external force, and relies on the assumption that the covariance matrix is accurately known. However, the actual covariance matrix cannot be accurately obtained in advance.
[0004] In summary, when the interaction force estimation problem of the sensorless robotic arm system depends on accurate model information, when the robotic arm system model and the contact external force model are inaccurate, the unmodeled residual dynamics of the system coupled with the external force make the external force unobservable. Existing methods lack high-precision tactile force separation estimation methods that can resist coupling interference for this problem. It is urgent to overcome the need to develop a sensorless tactile force estimation method in which the model can be adaptively adjusted online. Summary of the invention
[0005] In order to solve the problem of sensorless tactile force estimation of a robotic arm when both the robotic arm model and the contact external force model are inaccurate and to overcome the shortcomings of the prior art, the present invention provides a method for identifying and estimating unknown external forces of a robotic arm based on residual dynamic learning, which realizes offline learning and online compensation of modeled residual dynamics, adopts a set of tactile external force estimation strategies, does not require external sensors, and can iterate the adaptive robotic arm system model and tactile external force model online, thereby approximating the real noise characteristics of the model and improving the accuracy and anti-interference ability of tactile external force estimation.
[0006] In order to achieve the above object, the technical solution adopted by the present invention is:
[0007] A method for identifying and estimating unknown external forces of a robotic arm based on residual dynamic learning comprises the following steps:
[0008] The first step is to establish a composite dynamics model of the manipulator based on Gaussian process residual dynamic compensation;
[0009] The second step is to establish an enhanced Gaussian process interaction force estimation system model that includes an exogenous interaction force system model and virtual quantity measurement;
[0010] The third step is to design an interaction force estimator based on Gaussian process adaptive Kalman filter, in which the estimator gain is designed based on the process noise covariance matrix and the virtual measurement noise covariance matrix. The process noise covariance matrix is obtained by the variational Bayesian estimation algorithm, and the virtual measurement noise covariance matrix is obtained based on Gaussian process prediction.
[0011] Furthermore, the modeling process in the first step is as follows:
[0012] On the basis of the nominal dynamics model of the manipulator, the manipulator joint state is used as input, and the difference between the actual joint torque and the torque calculated by the nominal model is used as the residual measurement output. The residual dynamic compensator model is trained by Gaussian process regression, and then a composite dynamics model of the manipulator based on Gaussian process residual dynamic compensation is established. The specific implementation steps are as follows:
[0013] (1) In the non-contact scenario, for the n-link robot arm, a rigid body Euler-Lagrangian dynamics equation nominal model is established in the robot arm joint space:
[0014]
[0015] Among them, the system state variables include q, are the joint angle, angular velocity and angular acceleration respectively; M(q) is the inertia matrix, represents the Coriolis force and centrifugal force, G(q) represents the gravity term, τ EL represents the joint torque based on the nominal model.
[0016] The residual dynamic measurement is the difference between the actual joint torque and the nominal joint torque:
[0017] ε=τ m -τ EL ,
[0018] Among them, τ m is the actual joint torque measurement value, and ε is the residual dynamic measurement value. The residual dynamic measurement value is selected as the model output true value of Gaussian process regression, and the system state variable q, which represents the joint state, As model input, input and output data are collected in a contactless scenario to form a regression training set: D is the symbol of the training set;
[0019] (2) Using Gaussian process regression to train the residual dynamic compensator model: The residual dynamic compensator model is a random process, which consists of a training set D and hyperparameters θ; the hyperparameters include mean function, kernel function, likelihood function, inference method, parameter value of sparse approximation and function type. Based on the maximum a posteriori probability criterion, the inference is optimized and solved by the conjugate gradient method. The residual dynamic compensator model obtained by training is expressed as:
[0020]
[0021] Where p is the symbol of probability distribution; Input data vector for test points; τ Δ represents the predicted output torque of the residual dynamic compensator model; Represents τ Δ It follows a Gaussian distribution with a mean of μ and a variance matrix of Σ;
[0022] (3) In the environmental contact scenario, the joint momentum is used as the state variable of the robot system, and the state equation of the robot system is derived from the Euler-Lagrange dynamics equation as follows:
[0023]
[0024] in, represents the derivative of the state variable, The superscript T in the matrix indicates the transpose of the matrix, τ ext Represents the external torque of the contact force acting on the joint;
[0025] Discretize the state equation of the robotic arm system and express it as:
[0026] x k =Φ k x k-1 +F k u k +E 1,k d 1,k +E 2,k d 2,k
[0027] in,· k and· k-1 Respectively represent the sampling values of the matrix or vector at the kth and k-1th steps; Φ k ,F k ,E 1,k ,E 2,k They represent the system matrix of the discrete state equation of the manipulator system, which is obtained by discretizing the system matrix of the continuous state equation; u k represents the input of the robot system, d 1,k ,d 2,kare two additive interference torque terms, representing the contact torque and residual dynamic torque respectively. The specific calculation is as follows:
[0028] Furthermore, the modeling process of the enhanced Gaussian process interaction force estimation system in the second step is as follows:
[0029] Establish an exogenous contact force system model, introduce virtual measurement of contact force dynamics, directly measure contact force dynamics, and establish an enhanced Gaussian process interaction force estimation system model based on virtual measurement based on the composite dynamics model of the manipulator obtained in the first step; the specific steps are as follows:
[0030] (1) The contact torque model is described by an exogenous system including dynamics as follows:
[0031] ω k =S k ω k-1 +W d,k-1 ,
[0032] d 1,k =H k ω k ,
[0033] in, represents the dynamics of the contact force exogenous system, n is the degree of freedom of the manipulator, and l represents the order of the moment in the joint space; and is the system parameter matrix, which represents the system matrix and the output matrix respectively, and the parameter pair (S k ,E 1,k H k ) satisfies the observability condition; W d,k-1 Represents the exogenous system process noise term, which is used to describe the uncertainty of the exogenous system model and follows the distribution It means that the mean is zero and the time-varying variance matrix is Σ d,k-1 Gaussian distribution of
[0034] According to the residual dynamic compensator model trained in the first step, the residual dynamic torque model is described as follows:
[0035] d 2,k =μ Δ,k (z * )+W Δ,k (z * ),
[0036] Among them, μ Δ,k is the predicted mean of the residual dynamic torque, given by a Gaussian process; is a time-varying state covariance matrix Σ Δ,k (z *), and the residual dynamic torque of each joint is trained independently, satisfying is composed of n independent prediction variances The diagonal array composed of
[0037] (2) Define virtual measurement as follows:
[0038]
[0039] in, It is a virtual measurement, representing a direct measurement of the contact force dynamics; k ,y k-1 are the measurements of the momentum of the robotic system at time k and k-1 respectively. Since Gaussian process regression can learn the characteristics of measurement noise, y k =x k ;
[0040] (3) By introducing virtual measurement and combining the discrete system state equation of the manipulator in the first step, the enhanced Gaussian process contact force estimation system model based on virtual measurement is established as follows:
[0041]
[0042] in, represents the virtual measurement noise and is described as follows:
[0043] ν k =E 2,k W Δ,k ,
[0044] The virtual measurement noise is calculated by the Gaussian process residual dynamic moment model, which has a zero mean and a time-varying variance matrix of Σ ν,k =E 2,k Σ Δ,k Gaussian distribution.
[0045] Furthermore, the design of the interaction force estimator based on Gaussian process adaptive Kalman filter in the third step is specifically implemented as follows:
[0046] According to the enhanced Gaussian process interactive force estimation system model established in the second step, the robot arm adaptive Kalman filter tactile external force estimator is as follows:
[0047] (1) Status prediction:
[0048]
[0049] in, represents the prediction of the dynamics of the external force system at time k from time k-1, represents the a posteriori estimate of the dynamics of the external force system at time k-1;
[0050] (2) Error covariance matrix prediction:
[0051]
[0052] Among them, P k|k-1 represents the prediction of the error covariance matrix at time k from time k-1, P k-1|k-1 represents the posterior estimate of the covariance at time k-1, represents the transpose of the state matrix. Since Σ d,k-1 Inaccurate, resulting in P k|k-1 Inaccurate, it is necessary to introduce variational Bayesian iteration to approximate the accurate error covariance matrix;
[0053] (3) Based on the second step, update the virtual measurement and residual dynamics d 2,k ;
[0054] (4) Variational Bayesian iterative approximation error covariance matrix and contact force dynamic estimation:
[0055] Assuming that each dimension of the error covariance matrix obeys a Gaussian distribution, the prior distribution of the variational Bayesian approximation obeys an inverse entropy distribution:
[0056]
[0057] in, Indicates P k∣k-1 It obeys the inverse entropy distribution, q(·) represents the variational Bayesian approach symbol; λ k∣k-1 represents the degree of freedom parameter, represents a positive symmetric distance matrix, and η represents the matrix dimension; specifically, it includes:
[0058] a. Initialization:
[0059] λ k∣k-1 =η+τ+1
[0060] γ k∣k-1 =τP k∣k-1
[0061] Among them, τ is the adjustment parameter, which is used to describe the influence of the prior distribution on the predicted distribution;
[0062] b. The i-th iteration update, where i ranges from 0 to M-1 and M represents the number of iterations:
[0063] b.1 The error covariance matrix obeys the inverse entropy distribution:
[0064]
[0065] The inverse entropy distribution is calculated as follows:
[0066]
[0067]
[0068]
[0069] in, represents the auxiliary variable for the i-th iteration, (i) , · (i+1) Represent the i-th and i+1-th iteration values of the variable respectively.
[0070] b.2 Based on the error covariance matrix q obtained from the i-th iteration (i+1) (P k∣k-1 ), the i-th external force dynamic estimate is calculated as follows:
[0071]
[0072] The Gaussian distribution is calculated as follows:
[0073]
[0074]
[0075] in, To find the expected sign, represents the inverse of the error covariance matrix, represents the estimated value of the error covariance matrix at the i+1th iteration;
[0076] b.3 Calculate the optimal Kalman gain for the i-th iteration:
[0077]
[0078] in, represents the Kalman gain of the i+1th iteration at time k;
[0079] b.4 The i-th iteration measures and updates the state estimate:
[0080]
[0081] b.5 Update the covariance estimate:
[0082]
[0083] When the Mth iteration is reached, the variational Bayesian approximation iterative algorithm ends and the output is:
[0084]
[0085] in, Respectively P k∣k ,λ k ,γ k Mth iteration value;
[0086] (5) Estimation of external contact force at the end of the robotic arm:
[0087]
[0088]
[0089] in, represents the estimated value of the end contact external force, Indicates d 1,k The estimated value of , J is the Jacobian matrix of the robot arm.
[0090] The advantages of the present invention compared with the prior art are: the present invention relates to a method for identifying and estimating unknown external forces of a robotic arm based on residual dynamic learning. Aiming at the shortcomings of the prior methods that both the robotic arm model and the contact external force model are inaccurate and lack the ability to estimate the tactile force separation with high precision and resistant to coupling interference, a robotic arm modeling and compensation strategy based on residual dynamic learning is designed, and offline learning and online compensation of the modeled residual dynamics are realized. Furthermore, a set of tactile external force estimation strategies are designed, which do not require external sensors and can iterate the adaptive robotic arm system model and tactile external force model online, so as to approximate the real noise characteristics of the model and improve the accuracy and anti-interference ability of tactile external force estimation. BRIEF DESCRIPTION OF THE DRAWINGS
[0091] Figure 1 It is a design flow chart of a method for identifying and estimating unknown external forces of a robot arm based on residual dynamic learning of the present invention;
[0092] Figure 2 The block diagram of the method for identifying and estimating unknown external forces of a robotic arm based on residual dynamic learning of the present invention. DETAILED DESCRIPTION
[0093] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.
[0094] like Figure 1 As shown, the present invention relates to a method for estimating tactile external force of a robotic arm based on a mechanism data hybrid model, which specifically includes the following steps:
[0095] The first step is to establish a composite dynamics model of the manipulator based on Gaussian process residual dynamic compensation;
[0096] The second step is to establish an enhanced Gaussian process interaction force estimation system model based on virtual measurement;
[0097] The third step is to design an interaction force estimator based on Gaussian process adaptive Kalman filter. The estimator gain is designed based on the process noise covariance matrix and the virtual measurement noise covariance matrix. The process noise covariance matrix is optimized based on the variational Bayesian approximation algorithm, and the virtual measurement noise covariance matrix is optimized based on the Gaussian process prediction variance.
[0098] The present invention designs an offline learning modeling and compensation strategy for the unmodeled residual dynamics of the robotic arm dynamics model, and can adapt the contact external force model online, which can ensure that external force estimation can be achieved in different scenarios, in the presence of multiple unknown modeling errors, multi-source interference dynamics, and unknown contact external force noise characteristics. It has the characteristics of high precision, easy engineering implementation, explainability and anti-interference ability, and is suitable for multi-scenario and multi-type force interactive robotic arm systems that require high-precision external force estimation.
[0099] The specific implementation steps of the present invention are as follows:
[0100] In the first step, based on the Euler-Lagrangian dynamics formula, a nominal model of the rigid body dynamics of the robot arm is established. In a non-contact scenario, the difference between the actual joint torque and the torque calculated by the nominal model is used as the residual dynamic measurement value. The robot arm joint state is used as input and the residual dynamic measurement value is used as output. The residual dynamic compensator model is trained using Gaussian process regression to establish a composite dynamic model of the robot arm based on Gaussian process residual dynamic compensation. The specific steps are as follows:
[0101] (1) In the non-contact scenario, for the n-link robot arm, a rigid body Euler-Lagrangian dynamics equation nominal model is established in the robot arm joint space:
[0102]
[0103] Among them, the system state variables include q, are the joint angle, angular velocity and angular acceleration respectively; M(q) is the inertia matrix, represents the Coriolis force and centrifugal force, G(q) represents the gravity term, τ EL represents the joint torque based on the nominal model.
[0104] The residual dynamic measurement is the difference between the actual joint torque and the nominal joint torque:
[0105] ε=τ m -τ EL ,
[0106] Among them, τ m is the actual joint torque measurement value, and ε is the residual dynamic measurement value. The residual dynamic measurement value is selected as the model output true value of Gaussian process regression, and the system state variable q, which represents the joint state, As model input, input and output data are collected in a contactless scenario to form a regression training set: D is the symbol of the training set.
[0107] (2) The residual dynamic compensator model is trained using Gaussian process regression. The residual dynamic compensator model is a random process consisting of a training set D and hyperparameters θ. The hyperparameters θ include the mean function, kernel function, likelihood function, inference method, parameter values of sparse approximation, and function type. The inference is optimized based on the maximum a posteriori probability criterion and solved using the conjugate gradient method. The residual dynamic compensator model obtained through training is expressed as:
[0108]
[0109] Where p is the symbol of probability distribution; Input data vector for test points; τ Δ represents the predicted output torque of the residual dynamic compensator model; Represents τ Δ It follows a Gaussian distribution with mean μ and variance Σ.
[0110] (3) In the environmental contact scenario, the joint momentum is used as the state variable of the robot system, and the state equation of the robot system is derived from the Euler-Lagrange dynamics equation as follows:
[0111]
[0112] in, represents the derivative of the state variable, The superscript T in the matrix indicates the transpose of the matrix, τ ext Represents the external torque of the contact force acting on the joint. Discretizing the state equation of the robotic arm system can be expressed as:
[0113] x k =Φ k x k-1 +F k u k +E 1,k d 1,k +E 2,k d 2,k
[0114] in,· k and· k-1 Respectively represent the sampling values of the matrix or vector at the kth and k-1th steps; Φ k ,Fk ,E 1,k ,E 2,k They represent the system matrix of the discrete state equation of the manipulator system, which is obtained by discretizing the system matrix of the continuous state equation; u k represents the input of the robot system, d 1,k ,d 2,k are two additive interference torque terms, representing the contact torque and residual dynamic torque respectively. The specific calculation is as follows:
[0115]
[0116] d 1,k =τ ext,k ,d 2,k =τ Δ,k ,
[0117] The second step is to establish an exogenous contact force system model and introduce virtual measurement of contact force dynamics to directly measure the contact force dynamics. Based on the composite dynamics model in the first step, an enhanced Gaussian process interaction force estimation system model based on virtual measurement is established. The specific steps are as follows:
[0118] (1) The contact torque model is described by an exogenous system including dynamics as follows:
[0119] ω k =S k ω k-1 +W d,k-1 ,
[0120] d 1,k =H k ω k ,
[0121] in, represents the dynamics of the contact force exogenous system, n is the degree of freedom of the manipulator, and l represents the order of the moment in the joint space; and is the system parameter matrix, which represents the system matrix and the output matrix respectively, and the parameter pair (S k ,E 1,k H k ) satisfies the observability condition; W d,k-1 Represents the exogenous system process noise term, which is used to describe the uncertainty of the exogenous system model and follows the distribution It means that the mean is zero and the time-varying variance matrix is Σ d,k-1 Gaussian distribution.
[0122] According to the residual dynamic compensator model trained in the first step, the residual dynamic torque model is described as follows:
[0123] d 2,k =μΔ,k (z * )+W Δ,k (z * ),
[0124] Among them, μ Δ,k is the predicted mean of the residual dynamic torque, given by a Gaussian process; is a time-varying state covariance matrix Σ Δ,k (z * ), and the residual dynamic torque of each joint is trained independently, satisfying is composed of n independent prediction variances A diagonal array.
[0125] (2) Define virtual measurement as follows:
[0126]
[0127] in, It is a virtual measurement, representing a direct measurement of the contact force dynamics; k ,y k-1 are the measurements of the momentum of the robotic system at time k and k-1 respectively. Since Gaussian process regression can learn the characteristics of measurement noise, y can be realized k =x k .
[0128] (3) By introducing virtual measurement and combining the discrete system state equation of the manipulator in the first step, the enhanced Gaussian process contact force estimation system model based on virtual measurement is established as follows:
[0129]
[0130] in, represents the virtual measurement noise and is described as follows:
[0131] ν k =E 2,k W Δ,k ,
[0132] The virtual measurement noise is calculated by the Gaussian process residual dynamic moment model, which has a zero mean and a time-varying variance matrix of Σ ν,k =E 2,k Σ Δ,k Gaussian distribution.
[0133] The third step is to design an interaction force estimator based on Gaussian process adaptive Kalman filter. The design of the estimator gain is based on the process noise covariance matrix and the virtual measurement noise covariance matrix. The process noise covariance matrix is optimized based on the variational Bayesian approach algorithm, and the virtual measurement noise covariance matrix is optimized based on the Gaussian process prediction variance:
[0134] According to the enhanced Gaussian process interactive force estimation system model established in the second step, the robot arm adaptive Kalman filter tactile external force estimator is as follows:
[0135] (1) Status prediction:
[0136]
[0137] in, represents the prediction of the dynamics of the external force system at time k from time k-1, Represents the a posteriori estimate of the dynamics of the external force system at time k-1.
[0138] (2) Error covariance matrix prediction:
[0139]
[0140] Among them, P k|k-1 represents the prediction of the error covariance matrix at time k from time k-1, P k-1|k-1 represents the posterior estimate of the covariance at time k-1, represents the transpose of the state matrix. Since Σ d,k-1 Inaccurate, resulting in P k|k-1 It is inaccurate, and it is necessary to introduce variational Bayesian iteration to approximate the accurate error covariance matrix.
[0141] (3) Based on the second step, update the virtual measurement and residual dynamics d 2,k ;
[0142] (4) Variational Bayesian iterative approximation error covariance matrix and contact force dynamic estimation:
[0143] Assuming that each dimension of the error covariance matrix obeys a Gaussian distribution, the prior distribution of the variational Bayesian approximation obeys an inverse entropy distribution:
[0144]
[0145] in, Indicates P k∣k-1 It obeys the inverse entropy distribution, q(·) represents the variational Bayesian approach symbol; λ k∣k-1 represents the degree of freedom parameter, represents a positive definite symmetric distance matrix, and η represents the matrix dimension.
[0146] a. Initialization:
[0147] λ k∣k-1 =η+τ+1
[0148] γ k∣k-1 =τP k∣k-1
[0149] Among them, τ is the adjustment parameter, which is used to describe the influence of the prior distribution on the predicted distribution;
[0150] b. Update for the i-th iteration (i ranges from 0 to M-1, where M represents the number of iterations):
[0151] b.1 The error covariance matrix obeys the inverse entropy distribution:
[0152]
[0153] The inverse entropy distribution is calculated as follows:
[0154]
[0155]
[0156]
[0157] in, represents the auxiliary variable for the i-th iteration, (i) , · (i+1) Represent the i-th and i+1-th iteration values of the variable respectively.
[0158] b.2 Based on the error covariance matrix q obtained from the i-th iteration (i+1) (P k∣k-1 ), the i-th external force dynamic estimate is calculated as follows:
[0159]
[0160] The Gaussian distribution is calculated as follows:
[0161]
[0162]
[0163] in, To find the expected sign, represents the inverse of the error covariance matrix, represents the estimated value of the error covariance matrix at the i+1th iteration.
[0164] b.3 Calculate the optimal Kalman gain for the i-th iteration:
[0165]
[0166] in, represents the Kalman gain of the i+1th iteration at time k.
[0167] b.4 The i-th iteration measures and updates the state estimate:
[0168]
[0169] b.5 Update the covariance estimate:
[0170]
[0171] When the Mth iteration is reached, the variational Bayesian approximation iterative algorithm ends and the output is:
[0172]
[0173] in, Respectively P k∣k ,λ k ,γ k The Mth iteration value.
[0174] (5) Estimation of external contact force at the end of the robotic arm:
[0175]
[0176]
[0177] in, represents the estimated value of the end contact external force, Indicates d 1,k The estimated value of , J is the Jacobian matrix of the robot arm.
[0178] The method of the present invention is used to estimate the external force of the robotic arm, and can provide sensorless tactile force estimation of the robotic arm when both the robotic arm model and the contact external force model are inaccurate, thereby realizing offline learning and online compensation of unmodeled residual dynamics, and can iterate the adaptive robotic arm system model and tactile external force model online to approximate the real noise characteristics of the model, so as to improve the accuracy and anti-interference capability of the tactile external force estimation, thereby meeting the requirements of high precision and strong robustness.
[0179] The contents not described in detail in the specification of the present invention belong to the prior art known to the professional and technical personnel in the field. It is easy for the technical personnel in the field to understand that the above description is only the preferred embodiment of the present invention and is not intended to limit the present invention. Any modification, equivalent replacement and improvement made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.
Claims
1. A method for identifying and estimating unknown external forces of a robotic arm based on residual dynamic learning, characterized in that: The following steps are involved: The first step is to establish a composite dynamics model of the manipulator based on Gaussian process residual dynamic compensation; The modeling process in the first step is as follows: On the basis of the nominal dynamics model of the manipulator, the manipulator joint state is used as input, and the difference between the actual joint torque and the torque calculated by the nominal model is used as the residual measurement output. The residual dynamic compensator model is trained by Gaussian process regression, and then a composite dynamics model of the manipulator based on Gaussian process residual dynamic compensation is established. The specific implementation steps are as follows: (1) In the non-contact scenario, for the n-link robot arm, a nominal model of the rigid body Euler-Lagrangian dynamic equation is established in the robot arm joint space: ; The system state variables include , , , are joint angle, angular velocity and angular acceleration respectively; is the inertia matrix, represents the Coriolis force and centrifugal force terms, represents the gravity term, represents the joint torque based on the nominal model; The residual dynamic measurement is the difference between the actual joint torque and the nominal joint torque: ; in, is the actual joint torque measurement value, is the residual dynamic measurement value; the residual dynamic measurement value is selected as the model output true value of Gaussian process regression, representing the system state variable of the joint state , , As model input, input and output data are collected in a contactless scenario to form a regression training set: , is the symbol of the training set; (2) Using Gaussian process regression to train the residual dynamic compensator model: The residual dynamic compensator model is a random process, which is determined by the training set , hyperparameters Composition; hyperparameters include mean function, kernel function, likelihood function, inference method, parameter value and function type of sparse approximation. Based on the maximum a posteriori probability criterion, the inference is optimized and solved by the conjugate gradient method. The residual dynamic compensator model obtained by training is expressed as: ; in, is the probability distribution symbol; Enter the data vector for the test point; represents the predicted output torque of the residual dynamic compensator model; express The mean is , the variance matrix is Gaussian distribution of (3) In the environmental contact scenario, the joint momentum is used as the state variable of the robotic arm system, and the state equation of the robotic arm system is derived from the Euler-Lagrange dynamics equation as follows: ; in, represents the derivative of the state variable, Superscript in represents the transpose of a matrix, Represents the external torque of the contact force acting on the joint; Discretize the state equation of the robotic arm system and express it as: ; in, and Respectively represent matrices or vectors in The sampling value of the step; They represent the system matrices of the discrete state equations of the manipulator system, which are obtained by discretizing the system matrices of the continuous state equations; represents the input of the robot system, are two additive interference torque terms, representing the contact torque and residual dynamic torque respectively. The specific calculation is as follows: ; The second step is to establish an enhanced Gaussian process interaction force estimation system model that includes an exogenous interaction force system model and virtual quantity measurement; The third step is to design an interaction force estimator based on Gaussian process adaptive Kalman filter, in which the estimator gain is designed based on the process noise covariance matrix and the virtual measurement noise covariance matrix. The process noise covariance matrix is obtained by the variational Bayesian estimation algorithm, and the virtual measurement noise covariance matrix is obtained based on Gaussian process prediction.
2. According to claim 1, a method for identifying and estimating unknown external forces of a robotic arm based on residual dynamic learning is characterized in that: The modeling process of the enhanced Gaussian process interaction force estimation system in the second step is as follows: Establish an exogenous contact force system model, introduce virtual measurement of contact force dynamics, directly measure contact force dynamics, and establish an enhanced Gaussian process interaction force estimation system model based on virtual measurement based on the composite dynamics model of the manipulator obtained in the first step; the specific steps are as follows: (1) The contact torque model is described by an exogenous system containing dynamics as follows: ; in, represents the dynamics of the contact force exogenous system, is the degree of freedom of the robot arm, represents the order of the moment in the joint space; and are system parameter matrices, representing the system matrix and output matrix respectively, and the parameter pair Satisfy observability conditions; Represents the exogenous system process noise term, which is used to describe the uncertainty of the exogenous system model and follows the distribution ; It means that it obeys the mean of zero, and the time-varying variance matrix is Gaussian distribution of According to the residual dynamic compensator model trained in the first step, the residual dynamic torque model is described as follows: ; in, is the predicted mean of the residual dynamic torque, given by a Gaussian process; is a time-varying state covariance matrix Gaussian noise, and the residual dynamic torque of each joint is trained independently, satisfying is composed of n independent prediction variances The diagonal array composed of (2) Define virtual measurement as follows: ; in, For virtual measurement, characterization is performed for direct measurement of contact force dynamics; Respectively for The measurement of the momentum of the robotic arm system at the moment is realized because Gaussian process regression can learn the characteristics of measurement noise. ; (3) By introducing virtual measurement and combining the discrete system state equation of the robot in the first step, the enhanced Gaussian process contact force estimation system model based on virtual measurement is established as follows: ; in, represents the virtual measurement noise and is described as follows: ; The virtual measurement noise is calculated by the Gaussian process residual dynamic moment model, which has zero mean and a time-varying variance matrix of Gaussian distribution.
3. The method for identifying and estimating unknown external forces of a robotic arm based on residual dynamic learning according to claim 2 is characterized in that: The design of the interaction force estimator based on Gaussian process adaptive Kalman filter in the third step is specifically implemented as follows: According to the enhanced Gaussian process interactive force estimation system model established in the second step, the robot arm adaptive Kalman filter tactile external force estimator is as follows: (1) State prediction: ; in, Indicated by Always Prediction of the dynamics of the external force system at any given moment, express A posteriori estimates of the dynamics of the external force system at the moment; (2) Error covariance matrix prediction: ; in, Indicated by Always Prediction of the moment error covariance matrix, express The posterior estimate of the moment covariance, represents the transpose of the state matrix; since Inaccurate, resulting in Inaccurate, it is necessary to introduce variational Bayesian iteration to approximate the accurate error covariance matrix; (3) Based on the second step, update the virtual measurement and residual dynamics ; (4) Variational Bayesian iterative approximation error covariance matrix and contact force dynamic estimation: Assuming that each dimension of the error covariance matrix obeys a Gaussian distribution, the prior distribution of the variational Bayesian approximation obeys an inverse entropy distribution: ; in, express Obey the inverse entropy distribution, represents the variational Bayes approach symbol; represents the degree of freedom parameter, represents a positive definite symmetric distance matrix, Indicates the matrix dimensions; specifically includes: a. Initialization: ; in, It is a tuning parameter used to describe the influence of the prior distribution on the predicted distribution; b. The i-th iteration update, where i ranges from 0 to M-1 and M represents the number of iterations: b.1 The error covariance matrix follows the inverse entropy distribution: ; The inverse entropy distribution is calculated as follows: ; in, represents the auxiliary variable for the i-th iteration, Respectively represent the i-th and i+1-th iteration values of the variable; b.2 Error covariance matrix obtained based on the i-th iteration , the i-th external force dynamic estimate is calculated as follows: ; The Gaussian distribution is calculated as follows: ; in, To find the expected sign, represents the inverse of the error covariance matrix, represents the estimated value of the error covariance matrix at the i+1th iteration; b.3 Calculate the optimal Kalman gain for the i-th iteration: ; in, express Kalman gain at the i+1th iteration; b.4 The i-th iteration measures and updates the state estimate: ; b.5 Update the covariance estimate: ; When the Mth iteration is reached, the variational Bayesian approximation iterative algorithm ends and the output is: ; in, Respectively Mth iteration value; (5) Estimation of external contact force at the end of the robotic arm: ; in, represents the estimated value of the end contact external force, express The estimated value of is the Jacobian matrix of the robot arm.
