A method for joint estimation of contact force and environment stiffness of a robot arm based on iterative optimization

Abstract

This invention relates to a joint estimation method for contact force and environmental stiffness of a robotic arm based on iterative optimization. Addressing the problem of environmental stiffness sensing and contact force estimation for a sensorless robotic arm, the method firstly uses an iterative optimization (RTS) smoothing approach to obtain the posterior values ​​of the data sequence, based on a coupled dynamic model of the sensorless robotic arm and its environment, combined with the current measured values ​​of the system state and the environmental stiffness estimate obtained in the previous step. Secondly, based on the RTS smoothing result, an optimization function with environmental stiffness as a parameter is established using the maximum likelihood estimation method. Then, the environmental stiffness estimate is updated by solving for the maximum point of the optimization function, thereby obtaining the estimated contact force at the end of the robotic arm. This method enables online sensorless force estimation, improves the effectiveness of the interaction between the robotic arm and the unknown environment, and is applicable to robotic arm control systems in sensorless situations.

Description

TECHNICAL FIELD

[0001] The present application relates to a mechanical arm contact force and environment stiffness joint estimation method based on iterative optimization, which is suitable for a mechanical arm control system in a sensorless situation and belongs to the field of mechanical arm force tactile feedback and estimation. BACKGROUND

[0002] Mechanical arm tactile feedback refers to the interaction and interaction between the mechanical arm and the environment by sensing and processing the force and tactile information in the external environment. The traditional implementation method of mechanical arm tactile feedback is mainly to obtain the information of external force by relying on tactile sensors and other devices, among which the tactile sensors are usually installed at the end effector or joint of the mechanical arm to obtain the force, contact area distribution, object shape and other information when the mechanical arm contacts with the object. Force tactile sensors can usually provide accurate tactile force data, but usually have problems such as high cost, large size and long delay. Therefore, it is crucial to accurately estimate and feedback force and tactile in a sensorless situation.

[0003] A tactile stimulation system and method based on vibration array are designed in Chinese invention patent CN202011528330.X. The original data point array displaying the original data profile is generated according to the original conveying data and the initialization parameters, and the information data in the original data point array is converted into control instructions by using the vibration array instruction generation device to send to the tactile stimulation device, so that the corresponding stimulation array of the vibration array is excited, and the original conveying data profile is displayed to realize information transmission. However, the vibration mechanism used in this method may introduce additional interference, and in the case that the system itself external interference is not filtered out, the accuracy of tactile information measurement may be affected.

[0004] A mechanical arm external force estimation method and device are introduced in Chinese invention patent CN201710834869.X. The use of Kalman filter for external force estimation is avoided by modeling the dynamics of the mechanical arm, which avoids the use of additional force sensors; the mechanical arm dynamics model error is significantly reduced by using a supervised learning model for compensation; the robustness of the external force estimation in the presence of observation noise and model error is significantly improved by introducing Kalman filter. However, the Kalman filter used in this method depends on a single measurement value, which may be affected by measurement noise to a large extent, and the accuracy of the mechanical arm external force estimation may not be guaranteed.

[0005] The above-mentioned sensorless force estimation methods are mostly based on model, algorithm or rule inference. In the case of noise in data, unknown environment stiffness and other disturbances, the accuracy and accuracy are usually lacking, and the stability of the algorithm needs to be discussed. SUMMARY

[0006] Therefore, the present application provides a feasible solution to the problem.

[0007] The technical solution of the present application is to overcome the shortcomings of the prior art and provide an iterative optimization-based joint estimation method for contact force and environmental stiffness of a mechanical arm, to establish an optimization function with environmental stiffness as a parameter, to solve the state posteriori by RTS smoothing, and to update the environmental stiffness estimation value according to the optimization function batch processing, so as to realize sensorless force estimation and improve the interaction between the mechanical arm and the unknown environment.

[0008] The technical solution of the present application is to overcome the shortcomings of the prior art and provide an iterative optimization-based joint estimation method for contact force and environmental stiffness of a mechanical arm, to establish an optimization function with environmental stiffness as a parameter, to solve the state posteriori by RTS smoothing, and to update the environmental stiffness estimation value according to the optimization function batch processing, so as to realize sensorless force estimation and improve the interaction between the mechanical arm and the unknown environment.

[0009] First, based on the coupling dynamics model of the force sensorless mechanical arm system and the environment, the current measurement value of the system state and the environmental stiffness estimation value are combined, and the posteriori estimation value of the state variable and its covariance matrix is solved by RTS smoothing.

[0010] The extended state is where p k is the generalized momentum of the mechanical arm at the kth moment, ω k is the disturbance state at the kth moment, and the following environment coupling dynamics model of the force sensorless mechanical arm is established:

[0011]

[0012] The following parameter matrix is:

[0013]

[0014] In the formula, respectively represent the augmented system state matrix, input matrix and output matrix at the kth moment, which is related to the environmental stiffness parameter θ; y k represents the measurement signal at the kth moment; u k represents the system input vector at the kth moment; A k ,W k ,C k respectively represent the state transition function and the measurement function of the generalized momentum and the disturbance state; E k represents the coefficient matrix between the generalized momentum p k and the environmental stiffness matrix K θ ; G k represents the coefficient matrix between the disturbance state ω k and the generalized momentum p k ; B k represents the input matrix between the generalized momentum and the input vector, and has:

[0015] Ak = I n , W k = I nd , B k = ΔT · I n , C k = I n .

[0016] where ΔT, J k and are the sampling time interval, the k-th time instant Jacobian matrix of the manipulator and the inverse of the k-th time instant system inertia matrix, respectively, denotes the pseudo-inverse of the k-th time instant Jacobian matrix, denotes the n-dimensional identity matrix, denotes the nd-dimensional identity matrix; is the environmental stiffness matrix, θ n. denotes the stiffness parameters in the stiffness matrix to be estimated; where diag{·} denotes the diagonal matrix composed of the vector; the dimensions of each matrix in the formula match;

[0017] denote the process noise and the measurement noise at the k-th time instant, respectively, and their distributions are where are the covariance matrices of the process noise and the measurement noise, respectively, denotes that the random variable obeys the Gaussian distribution, and the specific structure is:

[0018]

[0019] where w k , v k , ξ k denote the process noise of the manipulator system, the output noise of the disturbance subsystem, and the process noise of the disturbance subsystem at the k-th time instant, respectively, and v k denotes the measurement noise of the manipulator system at the k-th time instant, and has:

[0020]

[0021] where denote the process noise covariance of the manipulator system, the output noise covariance of the disturbance subsystem, and the process noise covariance of the disturbance subsystem at the k-th time instant, respectively, denotes the measurement noise covariance of the manipulator system at the k-th time instant.

[0022] On this basis, combined with the current measurement value, the RTS smoothing is used to solve the posteriori estimation value of the state variable and its covariance matrix from time instant k-N to time instant k, and the steps are as follows:

[0023] ​(1) Initialize the forward Kalman filter:

[0024]

[0025]

[0026] where, denotes the mean of the state estimate, denotes the covariance of the state estimate, denotes the forward filtered posterior estimate at the k - N step, m denotes the mean symbol, P denotes the error covariance matrix symbol, denotes the expectation symbol with respect to a random variable, T denotes the transpose of a vector.

[0027] (2) For the i-th step, i = k - N + 1,..., k (where N is the last time), perform the forward Kalman filter:

[0028]

[0029]

[0030]

[0031]

[0032]

[0033] where, is the measurement noise covariance matrix of the extended state system, K f,i is the Kalman gain at the i-th time, is the state estimate prior value at the i-th time obtained by the forward Kalman filter, is the state estimate posterior value at the i-th time obtained by the forward Kalman filter, * is the parameter estimate value obtained using the measurements from the k - N - 1-th time to the k - 1-th time, is the system matrix obtained using *

[0034] (3) Initialize the RTS smoother:

[0035]

[0036]

[0037] where, k|k denotes the state estimate value at the k-th time obtained by RTS smoothing using the measurements from the k - N-th time to the k-th time, m k|k ​P k|k is the smoothed state estimation covariance;

[0038] (4) For the i-th step, i = k-1,...,k-N+1, perform RTS smoother:

[0039]

[0040]

[0041]

[0042]

[0043]

[0044] By the above steps, the state variable posterior mean estimate value at the i-th moment the state variable covariance posterior estimate value at the i-th moment P i|k the state variable mutual covariance posterior estimate value at the i-th and i-1-th moments P i,i-1|k From which the optimization function about the to-be-estimated parameter θ can be derived.

[0045] Second step, based on the environment coupling dynamics model of the sensorless robot arm, an optimization function with the to-be-estimated environmental stiffness as the parameter at the k-th moment is established. First, the log-likelihood function, that is, the conditional expectation of the log joint probability, is calculated:

[0046]

[0047] where x k-N:k is the state variable from the k-N-th moment to the k-th moment, y 1:k is the measurement value from the initial moment to the k-th moment; p(a|b;c) is the probability of event a occurring under the condition that event b occurs given that the parameter value is c; p(a,b;c) is the probability of events a and b occurring simultaneously with the parameter c; ∫· is the integral symbol, and d· is the total differential symbol; represents the likelihood function, which is used to describe the fitting degree of the parameter to the observation data; log(·) is the logarithm symbol; p(·) represents the probability.

[0048] From the Bayesian formula and the non-persistence of Markov chain, the auxiliary recursive form expression of the joint probability can be further calculated:

[0049]

[0050] where p(a|b) is the probability of event a occurring under the condition that event b occurs; Σ(·) is the summation symbol; Mk p(x) represents the joint probability of the state variables and measurements up to time k, with θ as the unknown parameter. k-N:k ,y 1:k The logarithm of θ), M k-N p(x) represents the joint probability of the state variables and measurements up to time kN, with θ as the unknown parameter. k-N ,y 1:k-N The logarithm of θ is:

[0051] M k-N =log p(y k-N |x k-N )+log p(x k-N |y 1:k-N-1 ;θ)+log p(y 1:k-N-1 )

[0052] By retaining only the terms related to the parameter θ, the objective function used for optimization can be rewritten as:

[0053]

[0054] From the time update formula for the state variables and the fact that the process noise follows a normal distribution, we know that the conditional probability satisfies:

[0055]

[0056]

[0057] Where, m i|i-1 Indicates that θ is a parameter and x i-1 Given that, x i The mean, m k-N|k-N-1 This indicates that y is a parameter with θ as the parameter. 1:k-N-1 Given the premise, x k-N The mean, P k-N|k-N-1 This indicates that θ is a parameter and y 1:k-N-1 Given the premise, x k-N The covariance matrix is:

[0058]

[0059] in, This is the system matrix corresponding to the unknown parameter θ. Combining this with the probability density function of the normal distribution, the optimization function is expressed as:

[0060]

[0061] in, |·| represents the determinant of a matrix. Substituting the smoothed RTS solution into the optimization function makes the environmental stiffness the only variable in the optimization function.

[0062] The third step involves finding the maximum point of the optimization function obtained in the second step, using it as the current environmental stiffness estimate, and then returning to the first step until the iteration converges. The current environmental stiffness estimate and the corresponding contact force estimate are then output. The specific implementation process is as follows:

[0063] (1) Solve for the maximum likelihood estimate at time k and step m:

[0064] Let the current step be the m-th step. Solve for the maximum likelihood estimate of the stiffness parameter at time k and step m using the gradient descent method. Since the objective function used for optimization is a concave function and its inverse is a convex function, the closed-form solution can be obtained by solving the following equation:

[0065]

[0066] in, tr(·) represents the partial derivative of the function with respect to θ, and is the matrix trace symbol. θ,i|k This means taking θ as a parameter and y as the parameter. 1:k As a premise and Terms related to the mean, M θ,k-N This means taking θ as a parameter and y as the parameter. 1:k-N-1 As a premise and The relevant items are:

[0067]

[0068]

[0069] (2) Iteratively solve for the parameter estimates at time k:

[0070] Stiffness parameter estimates based on time k and step m Update the system matrix as follows It then returns to the first iteration to calculate, continuing until the preset maximum number of iterations is reached or the error between two adjacent iterations is less than a given threshold ε, at which point the process terminates. and the current iteration result As the environmental stiffness estimate at time k

[0071] (3) Solve for the estimated external force at time k:

[0072] Based on the above process, the estimated environmental stiffness value at time k can be output. Then, the estimated value of the tactile force at the end of the robotic arm at time k is calculated and output.

[0073]

[0074] in, express The n1th component. Furthermore, based on the estimated stiffness parameter at time k. Update the system matrix as follows and as Return to the first step to participate in the iteration and solution at time k+1.

[0075] The advantages of this invention compared to the prior art are:

[0076] This invention employs expectation-maximization and RTS smoothing algorithms to estimate tactile force without requiring an external tactile force sensor. Compared to traditional external tactile force sensors, the reliability of this method is unaffected by the manufacturing process of the external sensor, and it filters out interference from process noise and measurement noise, fully utilizing effective measurement information to improve the estimation accuracy of force and tactile sensation. This invention can be applied to fields such as rehabilitation robotic arms and remote medical surgical robotic arms. Attached Figure Description

[0077] Figure 1 This is a flowchart illustrating the implementation of the method of the present invention. Detailed Implementation

[0078] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0079] like Figure 1 As shown, this invention proposes a joint estimation method for robotic arm contact force and environmental stiffness based on iterative optimization. The specific implementation steps are as follows:

[0080] 1. Based on the coupled dynamic model of the powerless sensor robotic arm system and the environment, and combining the current measured values ​​of the system state and the estimated values ​​of the environmental stiffness, the posterior estimates of the state variables and their covariance matrices are solved by using RTS smoothing.

[0081] by For the extended state, where p k Let ω be the generalized momentum of the robotic arm's end effector at time k. k For the perturbation state at time k, the following dynamic model of the sensorless robotic arm coupled with the environment is established:

[0082]

[0083] The following parameter matrix exists:

[0084]

[0085] In the formula, Let these represent the state matrix, input matrix, and output matrix of the augmented system at time k, respectively. Related to the environmental stiffness parameter θ; y k Represents the measurement signal at time k; u k A represents the system input vector at time k; k W k C k Let E represent the state transition function and measurement function for generalized momentum and perturbation state, respectively; k Generalized momentum p k With environmental stiffness matrix K θ The coefficient matrix between; G k Indicates the disturbance state ω k With generalized momentum p k The coefficient matrix between; B k The input matrix representing the relationship between the generalized momentum and the input vector is:

[0086] A k =I n , W k =I nd B k =ΔT·I n C k =I n .

[0087] Among them, ΔT, J k and These are the sampling time interval, the inverse of the robotic arm Jacobian matrix at time k, and the inverse of the system inertia matrix at time k, respectively. Denotes the pseudo-inverse of the Jacobian matrix at time k. Describes an n-dimensional identity matrix. Represents an nd-dimensional identity matrix; Let θ be the environmental stiffness matrix. n· Let represent the stiffness parameters of each dimension in the stiffness matrix to be estimated; where diag{·} represents a diagonal matrix composed of vectors; and the dimensions of each matrix are matched in the formula.

[0088] In the coupled dynamics model Let the process noise and measurement noise at time k be represented respectively, and their distributions are as follows: in, These are the covariance matrices of process noise and measurement noise, respectively. This indicates that the random variable follows a Gaussian distribution, and the specific structures are:

[0089]

[0090] Among them, w k ,ν k ξ kLet v represent the process noise of the robotic arm system, the output noise of the interference subsystem, and the process noise of the interference subsystem at time k, respectively. k The measurement noise of the robotic arm system at time k is:

[0091]

[0092] in, Let these represent the process noise covariance of the robotic arm system, the output noise covariance of the interference subsystem, and the process noise covariance of the interference subsystem at time k, respectively. Let represent the measurement noise covariance of the robotic arm system at time k.

[0093] Based on this, and combined with the current measurement values, the posterior estimates of the state variables and their covariance matrices over the time interval from time kN to time k are obtained using RTS smoothing. The steps are as follows:

[0094] (1) Initialize the forward Kalman filter:

[0095]

[0096]

[0097] In the formula, This represents the mean of the state estimates. The covariance of the state estimate is represented by the following: This represents the forward filtering posterior estimate at the kN-th step, where m represents the mean sign and P represents the error covariance matrix sign. The sign of the expectation of a random variable is expressed as (·). T This represents the transpose of a vector.

[0098] (2) For the i-th step, i = k - N + 1, ..., k (where N is the last time step), execute the forward Kalman filter:

[0099]

[0100]

[0101]

[0102]

[0103]

[0104] in, To expand the measurement noise covariance matrix of the system, K f,i Let be the Kalman gain at time i. The prior value of the state estimate at time i is obtained by the forward Kalman filter. Let θ be the posterior value of the state estimate at time i obtained by the forward Kalman filter. * The parameter estimates are obtained using measurements from time kN-1 to time k-1. For using θ * The system matrix is ​​obtained.

[0105] (3) Initialize the RTS smoother:

[0106]

[0107]

[0108] in,(·) k|k m represents the state estimate at time k obtained by RTS smoothing using measurements from time kN to time k. k|k P is the mean of the smoothed state estimate. k|k Estimate the covariance of the smoothed state;

[0109] (4) For the i-th step, i = k-1,...,k-N+1, execute the RTS smoother:

[0110]

[0111]

[0112]

[0113]

[0114]

[0115] By following the steps above, we can obtain the posterior mean estimate of the state variable at time i. The posterior estimate of the covariance of the state variable at time i is P. i|k The posterior estimate of the cross-covariance P of the state variables at time i and time i-1. i,i-1|k Based on this, an optimization function for the parameter θ to be estimated can be derived.

[0116] 2. Based on the results of the RTS smooth solution of the state variables obtained in the first step, an optimization function with environmental stiffness as a parameter is established. First, the log-likelihood function, i.e., the conditional expectation of the joint probability logarithm, is calculated:

[0117]

[0118] Where, x k-N:ky is the state variable from time kN to time k. 1:k is the measured value from the initial time to the k-th time; p(a|b;c) is the probability of event a occurring given that event b occurs, when the parameter value is c; p(a,b;c) is the probability of events a and b occurring simultaneously when the parameter is c; ∫· is the integral symbol, and d· is the total differential symbol; The likelihood function is used to describe how well the parameters fit the observed data; log(·) is the logarithmic symbol; p(·) represents the probability.

[0119] Based on Bayes' theorem and the aftereffect-free property of Markov chains, we can further calculate the auxiliary recursive form of the joint probability:

[0120]

[0121] Where p(a|b) is the probability of event a occurring given that event b has occurred; Σ(·) is the summation symbol; M k p(x) represents the joint probability of the state variables and measurements up to time k, with θ as the unknown parameter. k-N:k ,y 1:k The logarithm of θ), M k-N p(x) represents the joint probability of the state variables and measurements up to time kN, with θ as the unknown parameter. k-N ,y 1:k-N The logarithm of θ is:

[0122] M k-N =logp(y k-N |x k-N )+logp(x k-N |y 1:k-N-1 ;θ)+logp(y 1:k-N-1 )

[0123] By retaining only the terms related to the parameter θ, the objective function used for optimization can be rewritten as:

[0124]

[0125] From the time update formulas for the state variables in the system equations, and the fact that the process noise follows a normal distribution, we know that the conditional probability satisfies:

[0126]

[0127]

[0128] Where, m i|i -1 indicates that θ is a parameter and x i-1 Given the premise, x i The mean, mk-N|k-N-1 This indicates that θ is a parameter and y 1:k-N-1 Given the premise, x k-N The mean, P k-N|k-N-1 This indicates that θ is a parameter and y 1:k-N-1 Given the premise, x k-N The covariance matrix is:

[0129]

[0130] in, This is the system matrix corresponding to the unknown parameter θ. Combining this with the probability density function of the normal distribution, the optimization function is expressed as:

[0131]

[0132] in, |·| represents the determinant of a matrix. Substituting the smoothed RTS solution into the optimization function makes the environmental stiffness the only variable in the optimization function.

[0133] 3. Find the maximum point of the optimization function obtained in step 2, use it as the current environmental stiffness estimate, and return to step 1 until the iteration converges; output the current environmental stiffness estimate and the corresponding contact force estimate. The specific implementation process is as follows:

[0134] (1) Solve for the maximum likelihood estimate at time k and step m:

[0135] Let the current step be the m-th step. Solve for the maximum likelihood estimate of the stiffness parameter at time k and step m using the gradient descent method. Since the objective function used for optimization is a concave function and its inverse is a convex function, the closed-form solution can be obtained by solving the following equation:

[0136]

[0137] in, tr(·) represents the partial derivative of the function with respect to θ, and is the matrix trace symbol. θ,i|k This means taking θ as a parameter and y as the parameter. 1:k As a premise and Terms related to the mean, M θ,k-N This means taking θ as a parameter and y as the parameter. 1:k-N-1 As a premise and The relevant items are:

[0138]

[0139]

[0140] (2) Iteratively solve for the parameter estimates at time k:

[0141] Stiffness parameter estimates based on time k and step m Update the system matrix as follows It then returns to the first iteration to calculate, continuing until the preset maximum number of iterations is reached or the error between two adjacent iterations is less than a given threshold ε, at which point the process terminates. and the current iteration result As the environmental stiffness estimate at time k

[0142] (3) Solve for the estimated external force at time k:

[0143] Based on the above process, the estimated environmental stiffness value at time k can be output. Then, the estimated value of the tactile force at the end of the robotic arm at time k is calculated and output.

[0144]

[0145] in, express The n1th component. Furthermore, based on the estimated stiffness parameter at time k. Update the system matrix as follows and as Return to the first step to participate in the iteration and solution at time k+1.

[0146] The contents not described in detail in this specification are existing technologies known to those skilled in the art.

Claims

1. A method for jointly estimating the contact force and environmental stiffness of a robotic arm based on iterative optimization, characterized in that: Includes the following steps: The first step is to use the coupled dynamics model of the powerless sensor robotic arm system and the environment, combined with the current measured values ​​of the system state and the estimated values ​​of the environmental stiffness, to use RTS to smoothly solve the posterior estimates of the state variables and their covariance matrices. The second step is to use the maximum likelihood estimation method to calculate the optimization function with environmental stiffness as a parameter, based on the posterior estimates of the state variables and their covariance matrices obtained in the first step through RTS smoothing. The third step is to find the maximum point of the optimization function obtained in the second step, which is used as the current environmental stiffness estimate, and then return to the first step until the iteration converges; output the current environmental stiffness estimate and the corresponding contact force estimate. The first step is implemented as follows: by In the expansion state, where Let be the generalized momentum of the robotic arm's end effector at time k. Given the perturbation state at time k, the following environmental coupled dynamics model of the sensorless robotic arm is established: The following parameter matrix exists: 、 、 In the formula, They represent the first The augmented system state matrix, input matrix, and output matrix at time t. With environmental stiffness parameters related; Indicates the first The measurement signal at time; Indicates the first The system input vector at time t; Let these represent the state transition function and measurement function for generalized momentum and perturbation state, respectively; Generalized momentum With environmental stiffness matrix The coefficient matrix between them; Indicates the state of disturbance With generalized momentum The coefficient matrix between them; The input matrix representing the relationship between the generalized momentum and the input vector is: ， ， ， ， ， in, , and These are the sampling time interval and the first... The Jacobian matrix of the robotic arm at time and the first The inverse of the system's inertia matrix at any given time. Indicates the first The pseudo-inverse of the Jacobian matrix at time. express 1D identity matrix express 1D identity matrix; Here is the environmental stiffness matrix. Denotes the stiffness parameters of each dimension in the stiffness matrix to be estimated; where This represents a diagonal matrix composed of vectors; the dimensions of each matrix are matched in the formula. In the coupled dynamics model They represent the first The distribution of process noise and measurement noise at each moment is as follows: , ,in , These are the covariance matrices of process noise and measurement noise, respectively. This indicates that the random variable follows a Gaussian distribution, and the specific structures are: ， ； in, They represent the first The process noise of the robotic arm system, the output noise of the interference subsystem, and the process noise of the interference subsystem are constantly monitored. Indicates the first The robotic arm system measures noise at all times, including: 、 ， ， in, They represent the first The process noise covariance of the robotic arm system at any given time, the output noise covariance of the interference subsystem, and the process noise covariance of the interference subsystem. Indicates the first Measurement noise covariance of the robotic arm system at any given time; Based on this, and combined with the current measurement values, the RTS smoothing solution is used to solve for the time step. At the time The steps to obtain the posterior estimates of the state variables and their covariance matrix over the time period are as follows: (1) Initialize the forward Kalman filter: In the formula, This represents the mean of the state estimates. The covariance of the state estimate is represented by the following: Indicates the first Forward filtering posterior estimation step Indicates the sign of the mean. Indicates the sign of the error covariance matrix. This represents the expectation of a random variable. Represents the transpose of a vector; (2) For the first step, ,in At the last moment, the forward Kalman filter is executed: in, The measurement noise covariance matrix of the extended state system. For the first Kalman gain at time step The first one obtained by the forward Kalman filter Prior values ​​for state estimation at time step The first one obtained by the forward Kalman filter The posterior value of the state at time step is estimated. To utilize the first Time to the The parameter estimates obtained from measurements at time [time]. For the reason The obtained system matrix; (3) Initialize the RTS smoother: in, Indicates the use of the first Time to the The measurement at time 1000 is obtained after RTS smoothing. Time-state estimate To estimate the mean of the smoothed state, Estimate the covariance of the smoothed state; (4) For the first step, Execute the RTS smoother: Through the above steps, we can obtain the first... Posterior mean estimate of the state variable at time t , No. Posterior estimate of the covariance of the state variable at time 1 , No. Time and the Posterior estimate of the cross-covariance of state variables at time 1 Based on this, we can derive the environmental stiffness parameters to be estimated. The optimized function.

2. The method for jointly estimating the contact force and environmental stiffness of a robotic arm based on iterative optimization according to claim 1, characterized in that: The second step is specifically implemented as follows: Based on the sensorless robotic arm and environment coupled dynamics model, the first... The optimization function is defined with the environmental stiffness to be estimated as a parameter at all times; first, the log-likelihood function is established, which is the conditional expectation of the joint probability logarithm: ， in, It is the first Time to the The state variable at time t, From the initial moment to the th The measured value at that moment; The parameter takes the value of At the time, in a given event Under the conditions that the event occurs The probability of occurrence; Therefore When parameters are used, events The probability of them happening simultaneously; The integral symbol is used. The symbol for total differential; This represents the likelihood function, used to describe how well the parameters fit the observed data; The symbol is for logarithms; Represents probability; Based on Bayes' theorem and the aftereffect-free property of Markov chains, we can further calculate the auxiliary recursive form of the joint probability: in, In a given event Under the conditions that the event occurs The probability of occurrence; It is the summation symbol; Indicates For unknown parameters, state variables, and measurements, the cutoff point is the [number]th [number]. Joint probability at time step The logarithm of Indicates For unknown parameters, state variables, and measurements, the cutoff point is the [number]th [number]. Joint probability at time step The logarithm of the equation is: Only parameters are retained. The relevant terms will be used to rewrite the objective function for optimization as follows: From the time update formula for the state variables and the fact that the process noise follows a normal distribution, we know that the conditional probability satisfies: in, Indicates For parameters, When it is a premise, The mean, Indicates For parameters, When it is a premise, The mean, Indicates For parameters, When it is a premise, The covariance matrix is: ； in, It is an unknown parameter The corresponding system matrix; combined with the probability density function of the normal distribution, the optimization function is expressed as: in, , It is the symbol for finding the trace of a matrix. ; Represent the determinant of the matrix; substitute the RTS smoothing solution result into the optimization function to make the environmental stiffness parameters... It is the only variable in the optimization function.

3. The method for jointly estimating the contact force and environmental stiffness of a robotic arm based on iterative optimization according to claim 2, characterized in that: The third step is implemented as follows: (1) Solve the first Time, Number The maximum likelihood estimate of the step: Let the current step be the th step. Step 1: Solve the first step using gradient descent. Time, Number Maximum likelihood estimates of environmental stiffness parameters Since the objective function used for optimization is a concave function and its inverse is a convex function, the closed-form solution can be obtained by solving the following equation: in, Representing the relative function The partial derivatives, Indicates As parameters, with As a premise and Terms related to the mean, Indicates As parameters, with As a premise and The relevant items are: , ； (2) Iterative solution of the first Parameter estimates at time: Based on the Time, Number estimated environmental stiffness parameters of the step Update the system matrix to It then returns to the first iteration loop to calculate until the preset maximum number of iterations is reached or the error between two adjacent iterations is less than a given threshold. That is, there is a termination condition. and the current iteration result As the first Environmental stiffness estimate at time t ; (3) Solve the first Estimated external force at time: Based on the above process, the output is the first... Environmental stiffness estimate at time t Then solve and output the first... Estimated value of tactile force at the end of the robotic arm at any moment : in, express The The first component; and further, based on the first... Estimates of stiffness parameters at time t. Update the system matrix to and take it as Return to step one to participate in the first step. Iteration and solution at each moment.

Images