A robot joint state estimation method based on ellipsoidal contour distribution

Abstract

The present invention relates to the field of robotics, and in particular to a robot joint state estimation method based on ellipsoidal contour distribution. The robot joint state estimation method based on ellipsoidal contour distribution proposed by the present invention has the characteristics of accurate estimation, fast solution, high robustness, etc. compared with existing estimation methods under the condition of heavy-tailed process noise, and meets the needs of the robotics field for real-time performance and accuracy. The robot joint state estimation method based on ellipsoidal contour distribution proposed by the present invention does not need to provide a priori distribution of scale parameters of ellipsoidal contour distribution under the variational Bayesian solution method, and solves the expectation of scale parameters through reasonable approximation. The robot joint state estimation method based on ellipsoidal contour distribution proposed by the present invention realizes fast and accurate estimation of velocity and acceleration based on position measurement values, and can also be extended to applications based on position and velocity measurement values, and has good scalability.

Description

Technical Field

[0001] The present invention belongs to the field of robots, and in particular relates to a robot joint state estimation method based on ellipsoidal contour distribution. Background Art

[0002] In the field of robot control, especially for multi-joint robots, both model-based and learning-based control methods rely on the robot's current state information, such as joint position, velocity, and acceleration. However, the joint ends are generally only equipped with angle encoders for measuring position, and cannot accurately obtain velocity and acceleration information.

[0003] In the field of state estimation, the Kalman filter is the optimal filter for minimizing variance. It estimates the state vector through prediction and measurement updates, but this optimality is based on the Gaussian assumption of noise. Compared to the more stable movements of traditional manipulators, legged robots require rapid limb movements to maintain balance after external disturbances. These limb movements exhibit high maneuverability, leading to dramatic variations in process noise and an increased probability of large noise values. This violates the Gaussian assumption of the process noise, resulting in non-Gaussian noise with heavy tails. Within the traditional Kalman filter framework, if a fixed process noise covariance matrix is ​​used, a smaller process noise covariance matrix will not be able to quickly track corresponding state changes, resulting in reduced estimation accuracy. While increasing the process noise covariance matrix can achieve some improvement, it may cause filter divergence, rendering the estimation ineffective.

[0004] Variational inference-based filtering methods can achieve relatively accurate state estimation in the presence of heavy-tailed noise by selecting a suitable distribution form. However, these methods require conjugate priors for the relevant parameters and take a long time to solve, making them incapable of meeting the real-time requirements of legged robots with high control frequencies. Currently, no relevant research has been conducted on this issue. Therefore, it is necessary to develop fast and robust filtering methods for heavy-tailed process noise to improve the accuracy of robot joint state estimation in the presence of heavy-tailed process noise.

[0005] In summary, the existing robot joint state estimation methods cannot achieve fast and robust state estimation for highly maneuverable joints with heavy-tail process noise, and there is a problem of poor filtering accuracy leading to low accuracy of robot joint state estimation. Summary of the Invention

[0006] The purpose of this invention is to solve the problem of low accuracy of joint state estimation for high-mobility robots in existing robot joint state estimation methods. We propose a robot joint state estimation method based on ellipsoidal isohymetric distribution.

[0007] S1, constructing an interactive IMM filter; the interactive IMM filter includes M1 filters;

[0008] S2, use sensors to collect the robot joint position data at the current time step k, and obtain the observation vector z at the current time step k based on the robot joint position data at the time step k k ;

[0009] S3, obtaining the model probability, posterior probability density function, posterior state mean and posterior covariance matrix of M1 filters in the IMM filter at the previous time step k-1; the posterior state mean includes: joint position, velocity and acceleration;

[0010] S4, calculating the initial probability density function of the M1 filters at the current time step k based on the model probability, posterior probability density function, posterior state mean, and posterior covariance matrix of the M1 filters in the IMM filter at the previous time step k-1 obtained in S3; and obtaining the initial state mean and initial covariance matrix of the M1 filters based on the initial probability density function of the M1 filters;

[0011] S5, the observation vector z of the current time step k obtained in S2 k The initial state mean and initial covariance matrix of the M1 filters at the current time step k obtained in S4 are input into the interactive IMM filter, and the M1 filters are run in parallel to obtain the measurement likelihood probability density function, posterior state mean and posterior covariance matrix of the M1 filters at the current time step k;

[0012] S6, update the model probability at the current time step k according to the measurement likelihood probability density function of the M1 filters obtained in S5;

[0013] S7. Calculate the fusion estimation result at the current time step k based on the model probability of the M1 filters at time step k obtained by S6 and the posterior state mean and posterior covariance matrix of the M1 filters at the current time step k obtained by S5.

[0014] The beneficial effects of the present invention are:

[0015] The robot joint state estimation method based on ellipsoid contour distribution proposed in this invention has the characteristics of accurate estimation, fast solution and high robustness compared with existing estimation methods in the case of heavy-tailed process noise, meeting the real-time and accuracy requirements of the robotics field.

[0016] 2. The robot joint state estimation method based on ellipsoidal contour distribution proposed in the present invention does not need to provide a priori distribution of the scale parameters of the ellipsoidal contour distribution under the variational Bayesian solution method, and solves the expectation of the scale parameters through reasonable approximation.

[0017] 3. The robot joint state estimation method based on ellipsoidal contour distribution proposed in the present invention can achieve fast and accurate estimation of velocity and acceleration based on position measurement values, and can also be extended to applications based on position and velocity measurement values, with good scalability.

[0018] 4. All formulas contained in the robot joint state estimation method based on ellipsoidal contour distribution proposed in the present invention have been analytically given, are suitable for computer programming, and have good applicability. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 Schematic diagram of the state estimation algorithm framework based on ellipsoidal contour distribution of the present invention;

[0020] Figure 2 It is a flow chart of the interactive IMM algorithm based on ellipsoidal contour distribution of the present invention;

[0021] Figure 3 is a schematic diagram of parameter settings for an example study of the present invention;

[0022] Figure 4 This is a schematic diagram showing the comparison of RMSE curves of position estimation under different state estimation methods.

[0023] Figure 5 This is a schematic diagram showing the comparison of RMSE curves of speed estimation under different state estimation methods;

[0024] Figure 6 Here is a comparison diagram of the acceleration estimation RMSE curve under different state estimation methods.

[0025] Figure 7 This is a schematic diagram comparing the RMSE curves of position estimation under different state estimation methods in Example 2;

[0026] Figure 8 This is a schematic diagram comparing the RMSE curves of speed estimation under different state estimation methods in Example 2;

[0027] Figure 9 This is a schematic diagram comparing the RMSE curves of acceleration estimation under different state estimation methods in Example 2. DETAILED DESCRIPTION

[0028] Specific implementation method 1: Combination Figure 1 and Figure 2 The present invention is described, comprising:

[0029] S1, constructing an interactive IMM filter; the interactive IMM filter includes M1 filters;

[0030] S2, use sensors to collect the robot joint position data at the current time step k, and obtain the observation vector z at the current time step k based on the robot joint position data at the time step k k ;

[0031] S3, obtaining the model probability, posterior probability density function, posterior state mean and posterior covariance matrix of M1 filters in the IMM filter at the previous time step k-1; the posterior state mean includes: joint position, velocity and acceleration;

[0032] S4, calculating the initial probability density function of the M1 filters at the current time step k based on the model probability, posterior probability density function, posterior state mean, and posterior covariance matrix of the M1 filters in the IMM filter at the previous time step k-1 obtained in S3; and obtaining the initial state mean and initial covariance matrix of the M1 filters based on the initial probability density function of the M1 filters;

[0033] S5, the observation vector z of the current time step k obtained in S2 k The initial state mean and initial covariance matrix of the M1 filters at the current time step k obtained in S4 are input into the interactive IMM filter, and the M1 filters are run in parallel to obtain the measurement likelihood probability density function, posterior state mean and posterior covariance matrix of the M1 filters at the current time step k;

[0034] S6, update the model probability at the current time step k according to the measurement likelihood probability density function of the M1 filters obtained in S5;

[0035] S7. Calculate the fusion estimation result at the current time step k based on the model probability of the M1 filters at time step k obtained by S6 and the posterior state mean and posterior covariance matrix of the M1 filters at the current time step k obtained by S5.

[0036] Specific embodiment 2: The difference between this embodiment and specific embodiment 1 is that:

[0037] The specific process of constructing the m1∈M1 filter in the interactive IMM filter in S1 is:

[0038] S1.1: Construct a discrete-time state-space model of the robot joint system; the specific process is as follows:

[0039] S1.1.1: Construct the dynamic and measurement equations of the robot joint system;

[0040] S1.1.2: Construct a formula for calculating the predicted value. Calculate the predicted value of the state at time step k based on the mean of the posterior state at time step k-1, and use it as the distribution parameter of the prior distribution.

[0041] S1.1.3: Construct a priori probability density function p(x k |z 1:k-1 ,ξ k ), describes the prior distribution of the state at time step k, calculated based on the observation vectors of the previous k-1 time steps before the measurement value at time step k is obtained;

[0042] S1.1.4: Construct the probability density function p(Σ k ), describing the prior probability density function p(x k |z 1:k-1 ,ξ k ) the distribution of the covariance matrix parameters in ;

[0043] S1.1.5: Construct the likelihood probability density function p(z k |x k ), describing the probability of observation data given the model parameters;

[0044] S1.1.6: The prior probability density function p(x k |z 1:k-1 ,ξ k ), the probability density function of the ellipsoidal height distribution characteristic matrix p(Σ k ) and the likelihood probability density function p(z k |x k ) constitutes the discrete-time state-space model of the robot joint system at time step k.

[0045] S1.2: Construct the parameters to be estimated Θ based on the discrete-time state-space model of the robot joint system obtained in S1.1, and construct the m1-th filter based on the parameters to be estimated Θ;

[0046] S1.3: Set the initial state mean, covariance matrix, model probability, and model transfer matrix of the m1-th filter at time step 0, as well as the model transfer matrix in the IMM filter. Other steps and parameter settings are the same as those in the first embodiment.

[0047] Specific embodiment 3: This embodiment differs from specific embodiments 1 and 2 in that:

[0048] The dynamic equations and measurement equations of the robot joint system in S1.1.1 are expressed as follows:

[0049] x k =F k-1 x k-1 +w k-1 (3-1)

[0050] z k =Hk x k +v k (3-2)

[0051] Where k = 1, 2, ... represents the time step, is the n-dimensional state vector at time step k, is the m-dimensional observation vector at time step k, is the n×n dimensional state transfer matrix at time step k-1, is the m×m dimensional observation matrix at time step k, is the n-dimensional process noise vector at time step k-1, is the m-dimensional measurement noise vector at time step k;

[0052] The process noise vector w at time step k-1 k-1 The mean is zero, and the process noise vector w at time step k-1 is k-1 The n×n dimensional nominal covariance matrix is

[0053] The measurement noise vector v at time step k-1 k The mean of the measurement noise vector v at time step k-1 is zero. k The m×m dimensional covariance matrix is

[0054] n represents the state vector dimension, m represents the observation vector dimension, represents the set of real numbers;

[0055] The prediction value formula in S1.1.2 includes:

[0056] The state vector x k One-step forecast value The calculation formula and observation vector z k The predicted value of The calculation formula is: k|k-1 Indicates that the predicted value at time step k is calculated based on the value at time step k-1;

[0057] The state vector x k One-step forecast value The calculation formula is:

[0058]

[0059] Where, represents the mean of the posterior state when the time step is k-1;

[0060] The observation vector z k The predicted value of The calculation formula is:

[0061]

[0062] In the formula, the one-step prediction value is the k-1-step posterior state mean For the k-step state vector x k The predicted value for which the prediction is made;

[0063] The prior probability density function p(x k |z 1:k-1 ,ξ k )The specific process is:

[0064] S1.1.3.1: Determine the prior probability density function p(x k |z 1:k-1 ) obeys the ellipsoidal iso-height distribution form, and the prior probability density function p(x k |z 1:k-1 ) is modeled as an ellipsoidal contour distribution; a p×1-dimensional random vector x is said to obey an ellipsoidal contour distribution EC(x; μ, Σ, g(·)) if it has the probability density function shown in formula (2-1).

[0065] p(x)=k g |Σ| -1 / 2 Exp{g((x-μ) T Σ -1 (x-μ))} (2-1)

[0066] Where Exp(·) represents the natural exponential operation and g(·) is called the density generating function

[0067] Furthermore, the ellipsoidal contour distribution can be expressed as the following Gaussian scale mixture form

[0068]

[0069] Among them, ω(t) is called the weight function, Σ is called the feature matrix, and t is called the scale parameter;

[0070] S1.1.3.2: The ellipsoid isometric prior probability density function p(x k |z 1:k-1 ) rewrite the layered Gaussian form ellipsoidal contour distribution prior probability density function p(x k |z 1:k-1 ,ξ k );ξ k Represents the scale parameter of the ellipsoidal contour distribution.

[0071] The other steps and parameters are the same as those in the first and second embodiments.

[0072] Specific embodiment 4: This embodiment differs from specific embodiments 1 to 3 in that:

[0073] The prior probability density function p(x k |z 1:k-1 ) obeys the ellipsoidal contour distribution form, including: Gaussian ellipsoidal contour distribution (GEC), logarithmic ellipsoidal contour distribution (LEC), exponential ellipsoidal contour distribution (EEC);

[0074] The density generating function of the Gaussian ellipsoid isohymetric distribution is:

[0075] g GEC (s)=-0.5s (2-3)

[0076] Where s represents the auxiliary variable, s=(x-μ) T Σ -1 (x-μ), is a scalar, g (·) (·) is a scalar function representing the density generating function of the ellipsoidal contour distribution;

[0077] The density generating function of the logarithmic ellipsoidal contour distribution is:

[0078]

[0079] Where p is the dimension of the random variable that obeys the ellipsoidal isohymetric distribution, v is the degree of freedom parameter, and v=5 is selected;

[0080] The density generating function of the exponential ellipsoid isohymetric distribution is:

[0081]

[0082] Where p is the dimension of the random variable that follows the ellipsoidal isohymetric distribution, and σ = 5 represents the distribution parameter of EEC;

[0083] The prior probability density function p(x k |z 1:k-1 ) is modeled as an ellipsoidal equal height distribution, and the specific process is:

[0084] When it is determined to be a Gaussian ellipsoid with equal height distribution, the prior probability density function p(x k |z 1:k-1 ) is modeled as a Gaussian ellipsoid with equal height distribution, which can be expressed as:

[0085] p(x k |z 1:k-1 )=EC GEC Σ

[0086]

[0087] Where, Meaning is defined as; EC GEC () represents the probability density function of the Gaussian ellipsoid and the height distribution, Σ k represents the feature matrix; z 1:k-1 represents the set of observation vectors from time step 1 to time step k-1;

[0088] When the logarithmic ellipsoid is determined to be a height distribution, the prior probability density function p(x k |z 1:k-1 ) is modeled as a logarithmic ellipsoidal equal-height distribution, which can be expressed as:

[0089]

[0090] Where, EC LEC () represents the probability density function of the logarithmic ellipsoidal isohymetric distribution;

[0091] When it is determined to be an exponential ellipsoidal isoheight distribution, the prior probability density function p(x k |z 1:k-1 ) is modeled as an exponential ellipsoid with equal height distribution, which can be expressed as:

[0092]

[0093] Where, EC EEC () represents the probability density function of the exponential ellipsoidal isohymetric distribution;

[0094] In S1.1.3.2, the ellipsoidal contour distribution prior probability density function p(x k |z 1:k-1 ) is rewritten as layered Gaussian form p(x k |z 1:k-1 ,ξ k ), hierarchical Gaussian prior probability density function p(x k |z 1:k-1 ,ξ k ) is expressed as:

[0095] p(x k |z 1:k-1 ,ξ k )=N(x k ;μ k ,Σ k / ξ k ) (3-7)

[0096] p(ξ k )=p g (ξ k ) (3-8)

[0097] Among them, ξk represents the scale parameter of the ellipsoidal contour distribution; p(ξ k ) represents the probability density function of the scale parameter of the ellipsoidal isoheight distribution, p g (·) represents the probability density function obtained from the ellipsoidal contour distribution density generating function g(·);

[0098] Set the ellipsoid height distribution characteristic matrix Σ k The density function p(Σ k ) obeys the inverse Wishart distribution; it can be expressed as:

[0099] p(Σ k )=IW(Σ k ;u k ,U k ) (3-9)

[0100] Among them, p(Σ k ) represents the feature matrix Σ k The probability density function of the inverse Wissart distribution is IW(), and u k represents the degrees of freedom parameter of the inverse Wishart distribution, U k The other steps and parameters are the same as those in the first to third embodiments.

[0101] Specific embodiment 5: This embodiment differs from specific embodiments 1 to 4 in that:

[0102] The likelihood probability density function p(z k |x k ), the specific process is:

[0103] The likelihood probability density function p(z k |x k ) is modeled as a Gaussian distribution and is expressed as:

[0104] p(z k |x k )=N(z k ;H k x k ,R k ) (3-10)

[0105] Where: N() represents the probability density function of Gaussian distribution;

[0106] The joint posterior probability density function p(x k ,Σ k ,ξ k |z 1:k )The specific process is:

[0107] p(x k ,Σ k ,ξ k |z 1:k )∝p(z k |x k )p(x k |Σ k ,ξ k ,z 1:k-1 )p(Σ k )p(ξ k )(3-12)

[0108] In the formula, p(x k ,Σ k ,ξ k |z 1:k ) represents the joint posterior probability density function, ∝ represents proportional to, p(z k |x k ) represents the likelihood probability density function, p(x k |Σ k ,ξ k ,z 1:k-1 ) represents x k The joint prior probability density function of .

[0109] The other steps and parameters are the same as those in the first to fourth embodiments.

[0110] Specific embodiment 6: This embodiment differs from specific embodiments 1 to 5 in that:

[0111] In S1.2, the parameter to be estimated Θ is constructed based on the state space model obtained in S1.1, and the m1-th filter is constructed based on the parameter to be estimated Θ. The specific process is:

[0112] S1.2.1: Use the variational Bayesian method to construct an approximate solution for the joint posterior probability density function p(x k ,Σ k ,ξ k |z 1:k ) is expressed as:

[0113] p(x k ,Σ k ,ξ k |z 1:k )≈q(x k )q(Σ k )q(ξ k )(4-1)

[0114] Where q(·) represents the approximate posterior probability density function constructed based on the variational Bayesian method;

[0115] The variational Bayesian method is used to construct an approximate posterior probability density function {q(x k ),q(Σ k ),q(ξ k )}, expressed by formula (4-2):

[0116] {q(x k ),q(Σ k ),q(ξ k )}=argminKLD(q(x k )q(Σ k )q(ξ k )||p(x k ,Σ k ,ξ k |z 1:k )) (4-2)

[0117] Where KLD(·||·) represents the Kullback-Leibler divergence function; argmin(·) represents finding the minimum point;

[0118] S1.2.2: Construct the set of parameters to be estimated And based on the set of parameters to be estimated Θ, the optimal solution logq(φ) of the variational Bayes problem expression (4-2) is constructed, which can be expressed as follows:

[0119]

[0120] Where φ represents any element in the parameter set Θ to be estimated, Θ( -φ ) represents the subset consisting of the remaining elements in the parameter set Θ to be estimated except φ; c φ represents a constant associated with the element φ;

[0121] It means to find the expectation of all elements except φ in the estimated parameters.

[0122] S1.2.3: Based on the discrete-time state-space model of the robot joint system obtained in S1.1, construct the joint probability density function p(Θ,z 1:k ), which can be expressed as:

[0123]

[0124] S1.2.4: For the joint probability density function p(Θ,z 1:k ) takes the logarithm and obtains the logarithmic joint probability density function logp(Θ,z 1:k ), which can be expressed as:

[0125]

[0126] Where: n represents the dimension of the state vector, tr() represents the matrix trace operation, Represents the inverse matrix of the ellipsoidal height distribution characteristic matrix, u k represents the degrees of freedom parameter of the inverse Wishart distribution, U k represents the inverse scaling matrix of the inverse Wishart distribution;

[0127] S1.2.5: Initialize the parameters in the variational Bayes problem expression to obtain the m1-th filter. The specific process is:

[0128]

[0129] Among them, P k|k-1 is the prediction error covariance matrix from time step k-1 to time step k, n is the state vector dimension, τ is the adjustment parameter, and τ=5 is selected. represents the initial posterior state mean at time step k; represents the initial posterior covariance matrix at time step k, is the initial value of the ellipsoidal height distribution scale parameter at time step k, F k-1 represents the state transition matrix at time step k-1, P k-1|k-1 represents the posterior covariance matrix at time step k-1; u k represents the degrees of freedom parameter of the inverse Wishart distribution, U k The other steps and parameters of the inverse scale matrix representing the inverse Wishart distribution are the same as those of any one of the first to fifth embodiments.

[0130] Specific embodiment 7: This embodiment differs from specific embodiments 1 to 6 in that:

[0131] In S3, the model probability, posterior probability density function, posterior state mean and posterior covariance matrix of M1 filters in the IMM filter at the previous time step k-1 are obtained; the formula is expressed as follows:

[0132] The posterior state mean of the m1th filter at time step k-1 is expressed as The posterior covariance matrix is ​​expressed as The probability of the m1th filter model at time step k-1 is expressed as

[0133] The posterior probability density function p(x k-1 |η k-1 =m1,z 1:k-1 ), which can be expressed as:

[0134] p(x k-1 |η k-1=m1,z 1:k-1 )

[0135] Among them, η k-1 Indicates the filter number selected at time step k-1;

[0136] In S4, the model probability, posterior probability density function, posterior state mean and posterior covariance matrix of the M1 filters in the IMM filter at the previous time step k-1 obtained in S3 are used to calculate the initial probability density function of the M1 filters at the current time step k; and the initial state mean and initial covariance matrix of the M1 filters are obtained based on the initial probability density function of the M1 filters. The specific process is:

[0137] S4.1, construct the initial probability density function p(x k-1 |η k =m1,z 1:k-1 ),

[0138] It can be expressed as follows using formula (8-2):

[0139]

[0140] Among them, arg inf means finding the point that reaches the lower bound, m2∈M1,m2≠m1 means the m2th filter, Auxiliary variables representing intermediate calculations, is the mixing probability of filter m1 and filter m2, is the model transfer matrix The child element of represents the transition probability from model m2 to model m1;

[0141] S4.2, bring the model probability, posterior state mean and posterior covariance matrix of the M1 filters at time k-1 obtained in S3 into the initial probability density function p(x k-1 |η k =m1,z 1:k-1 ) is calculated using formula (8-2) to obtain the initial probability density function p(x k-1 |η k =m1,z 1:k-1 ), the formula is:

[0142]

[0143] represents the initial state mean of the m1th filter, represents the initial covariance matrix of the m1-th filter,

[0144] S4.3: Repeat S4.1 to S4.2 to obtain the initial probability density function of M1 filters, and obtain the initial state mean and initial covariance matrix of M1 filters based on the initial probability density function of M1 filters.

[0145] The initial state mean of the m1th filter is and the initial covariance matrix The calculation formula is:

[0146]

[0147] In S5, the observation vector z of the current time step k obtained in S2 is converted to k The initial state mean and initial covariance matrix of the M1 filters at the current time step k obtained by S4 are input into the interactive IMM filter, and the M1 filters are run in parallel to obtain the measurement likelihood probability density function and the posterior state mean and posterior covariance matrix of the M1 filters at the current time step k; the specific process is:

[0148] S5.1: Assign the initial parameters required to run the variational Bayes method at time step k set in S1.2.5 to each of the M1 filters;

[0149] S5.2: The observation vector z of the current time step k obtained in S2 k The initial state mean and initial covariance matrix of the M1 filters at the current time step k obtained in S4 are respectively input into the M1 filters of the interactive IMM filter; the M1 filters are run in parallel to obtain the measurement likelihood probability density function and the posterior state mean and the posterior covariance matrix of the M1 filters at the current time step k. The other steps and parameters are the same as those in one of the specific implementation methods one to six.

[0150] Specific embodiment eight: This embodiment differs from specific embodiments one to seven in that:

[0151] The process of obtaining the measurement likelihood probability density function, the posterior state mean, and the posterior covariance matrix of the m1-th filter at time step k in S5.2 is:

[0152] S5.2.1: Construct an approximate form of the joint posterior probability density function

[0153] S5.2.2: Use the results from S4.3 and Update the filter's posterior state mean at time step k-1 And the posterior covariance matrix P k-1|k-1 , expressed as:

[0154]

[0155] Update the initial parameters required to run the variational Bayesian method for the m1th filter at time step k set in S1.2.5, run the variational Bayesian iterative algorithm, and record the number of iterations as i, and set the maximum number of iterations to N max , i is [0, N max ) is an integer between .

[0156] S5.2.3: Compute the scale parameter ξ of the ellipsoidal contour distribution in the i+1th variational Bayesian iteration k Expectations (i+1) [ξ k ];

[0157] S5.2.4: Compute the characteristic matrix Σ of the ellipsoidal contour distribution in the i+1th variational Bayesian iteration k Expectations

[0158] S5.2.5: Update the mean of the posterior states in the i+1th variational Bayesian iteration and the posterior covariance matrix

[0159] S5.2.6: Repeat S5.2.3 to S5.2.5; end the iteration when the number of iterations i reaches the maximum value or the iteration stop condition is met, and obtain the final posterior state mean and the posterior covariance matrix

[0160] Calculate and record the measurement likelihood probability density function

[0161] The joint posterior probability density function of the approximate form is constructed in S5.2.1 The specific process is:

[0162] S5.2.1.1: Based on the scale parameter ξ k S1.1.4 The expected and likelihood probability density function p(z k |x k ), we get the approximate likelihood probability density function expression It can be expressed as:

[0163]

[0164] Where θ k represents an auxiliary variable, E[·] represents an operator for obtaining the expectation, and st represents subjection to the condition;

[0165] S5.2.1.2: Likelihood probability density function according to the approximate form and Bayesian formula to obtain the approximate joint posterior probability density function It can be expressed as:

[0166]

[0167] Where, Represents the joint posterior probability density function of the state quantity, p(ξ k |z 1:k ) represents the posterior probability density function of the scale parameter, represents the measurement likelihood probability density function;

[0168] The scale parameter ξ of the i+1th ellipsoid contour distribution is calculated in S5.2.3 k Expectations (i+1) [ξ k ]; the specific process is:

[0169] S5.2.3.1: Based on the joint posterior probability density function Get the measurement likelihood probability density function It can be expressed as:

[0170]

[0171] S5.2.3.2: Joint posterior probability density function Integrate and get The specific process is:

[0172]

[0173] Where, Indicated by auxiliary variable θ k is the covariance matrix of the parameters; E( i) [·] represents the expected value obtained in the i-th variational Bayesian iteration, Represents the ellipsoidal height distribution characteristic matrix Σ k The inverse matrix, H k represents the measurement matrix at time step k, Indicates H k The transposed matrix of

[0174] g(·) represents the density generating function of the ellipsoid contour distribution, which is determined according to the ellipsoid contour distribution form obeyed by the selected filter, including: Gaussian ellipsoid contour distribution (GEC) density generating function g GEC (·), logarithmic ellipsoidal contour distribution (LEC) density generating function g LEC (·), the exponential ellipsoidal contour distribution (EEC) density generating function g EEC (·);

[0175] S5.2.3.3: Change p(ξ k |z 1:k ) is approximately a Dirac delta function, which can be expressed as:

[0176] p(ξ k |z 1:k )≈δ(ξ k -E[ξ k |z 1:k ])(5-5)

[0177] In formula (5-5), δ() represents the Dirac delta function;

[0178] S5.2.3.4: Combine equation (5-5) and calculate z in equations (5-2) and (5-3). k Derivative to obtain auxiliary variable θ k Calculation formula:

[0179]

[0180] in, represents the innovation vector, Indicates d k The transpose of represents the first derivative of the density generating function of the ellipsoidal contour distribution;

[0181] S5.2.3.5: Set the maximum number of fixed point iterations to M max ; The result of the 0th fixed point iteration is set to Solve Equation 5-6 using the fixed point iteration method to obtain θ k The value of

[0182] When the j-th generation fixed point iteration method is used to solve the problem, the formula is expressed as:

[0183]

[0184] In formula (5-7), j represents the number of fixed point iterations, represents θ k The result of the jth fixed point iteration, Denoted as θ k The result of the j+1th fixed point iteration,

[0185] In the i+1th variational Bayesian iteration, E (i+1) [ξ k The updated result of ] is

[0186]

[0187] The characteristic matrix Σ in the ellipsoid contour distribution in the i+1th variational Bayesian iteration is calculated in S5.2.4 k Expectation of the inverse matrix The specific process is:

[0188] S5.2.4.1: The update method for solving the characteristic matrix of the ellipsoidal contour distribution is as follows:

[0189] Let φ = Σ k , according to the logarithmic joint probability density function logp(Θ,z 1:k ) and the optimal solution of the variational Bayes problem logq(φ), and the optimal solution of the variational Bayes problem in the i+1th iteration logq (i+1) (Σ k )

[0190]

[0191] Where i<N max represents the number of iterations of variational Bayes, N max Indicates the maximum number of variational Bayesian iterations, Represents the same as Σ k Related constants; represents the auxiliary parameters of the i-th variational Bayesian iteration; and Respectively represent the posterior state mean and posterior covariance matrix of the i-th variational Bayesian iteration;

[0192] According to logq (i+1) (Σ k ) can get the ellipsoidal height distribution characteristic matrix Σ k Update method: according to the ellipsoid height distribution feature matrix Σ k Update method to calculate the ellipsoid height distribution characteristic matrix Σ k expectations;

[0193] The ellipsoid contour distribution characteristic matrix Σ k The update method is expressed by the formula:

[0194]

[0195] in,

[0196]

[0197] in and They represent Σ in the i+1th variational Bayesian iteration respectively. k The degrees of freedom parameter and inverse scaling matrix of the distribution; u k and U kare the initial parameters for running Variational Bayes at the current time step k;

[0198] S5.2.4.2: Characteristic matrix Σ based on ellipsoidal contour distribution k The update method calculates the characteristic matrix Σ in the ellipsoidal height distribution in the i+1th variational Bayesian iteration k The expectation of the inverse matrix is:

[0199]

[0200] Where, Represents the feature matrix Σ k The other steps and parameters are the same as those in the first to seventh embodiments.

[0201] Specific embodiment 9: This embodiment differs from specific embodiments 1 to 8 in that:

[0202] The S5.2.5 updates the posterior state mean of the i+1 variational Bayesian iteration and the posterior covariance matrix The specific process is:

[0203] S5.2.5.1: Let φ = x k , according to the logarithmic joint probability density function logp(Θ,z 1:k )(4-6) and the optimal solution to the variational Bayes problem logq(φ) formula (4-3), we get the optimal solution to the variational Bayes problem logq in the i+1th iteration (i+1) (x k ) is expressed as:

[0204]

[0205] in, Indicates that x k Related constants;

[0206] S5.2.4.2: According to the scale parameter ξ of the ellipsoid contour distribution in the i+1th variational Bayesian iteration k Expectations (i +1) [ξ k ] and the characteristic matrix Σ in the ellipsoidal height distribution in the i+1th variational Bayesian iteration k Expectation of the inverse matrix Get the modified prior probability density function p (i+1) (x k |z 1:k-1 ), likelihood probability density function p (i+1) (z k |x k), and the modified forecast error covariance matrix Expressed as:

[0207]

[0208] p (i+1) (z k |x k )=N(z k ;H k x k ,R k ) (7-3)

[0209]

[0210] S5.2.4.3: According to the standard Kalman filter method, substitute equations (7-2) to (7-4) into equation (7-1) to obtain the approximate probability density function q of the state vector in the i+1th variational Bayesian iteration (i+1) (x k )for:

[0211]

[0212] in, is the normalization constant.

[0213] S5.2.5.4: According to equations (7-2) to (7-5), update q using the Gaussian distribution (i+1) (x k )The specific process is:

[0214]

[0215] Among them, the posterior state mean of the i+1th variational Bayesian iteration is and the covariance matrix Calculate as follows:

[0216]

[0217] Where i<N max represents the number of iterations of variational Bayes, N max Indicates the maximum number of iterations set; is the Kalman gain in the i+1th variational Bayesian iteration;

[0218] The iteration stopping condition in S5.2.6 is:

[0219]

[0220] In formula (7-10), ζ represents the threshold of iterative convergence, which is set to ζ = 10 -10 ;

[0221] Assuming that the iteration stopping condition is met in the i+1 generation, the final posterior state mean and the posterior covariance matrix As the posterior state mean and posterior covariance matrix of the m1-th filter at the current time step k, it is expressed as:

[0222]

[0223] Where, represents the posterior state mean of the m1th filter, The other steps and parameters are the same as those in the first to eighth embodiments.

[0224] Specific embodiment 10: This embodiment differs from specific embodiments 1 to 9 in that:

[0225] In S6, the model probability at the current time step k is updated according to the measurement likelihood probability density function of the M1 filters obtained in S5. The specific process is:

[0226] The model probability of the update time step k of the m1th filter is The formula is expressed as:

[0227]

[0228] in, is the auxiliary variable of the m1th filter calculated by formula (8-4), a represents the filter number, The auxiliary variable of the ath filter calculated by expression (8-4), p(z k |η k =m1,z 1:k-1 ) represents the measurement likelihood probability density function of the m1-th filter, which is calculated as follows:

[0229]

[0230] in, represents the normalization constant, Auxiliary variables for the convenience of formula expression; Indicated by auxiliary variable θ k The covariance matrix of the parameters is obtained by formula (5-4), is the predicted value of the observation vector, and Both come from the measurement likelihood probability density function of the m1-th filter;

[0231] In S7, the model probability of the M1 filters at time step k obtained by updating S6 and the posterior state mean and posterior covariance matrix of the M1 filters obtained in S5 are used to calculate the fusion estimation result at the current time step k. The specific process is:

[0232]

[0233] Where, represents the posterior state mean of the a-th filter, represents the posterior covariance matrix of the a-th filter, represents the model probability of the ath filter at time step k;

[0234] The fusion estimation result at the current time step k includes: the posterior state mean at the current time step k The posterior covariance matrix P at the current time step k k|k The other steps and parameters are the same as those in Specific Embodiments 1 to 9.

[0235] The robot joint state estimation method based on ellipsoid contour distribution proposed in this invention has the characteristics of accurate estimation, fast solution and high robustness compared with existing estimation methods in the case of heavy-tailed process noise, meeting the real-time and accuracy requirements of the robotics field.

[0236] 2. The robot joint state estimation method based on ellipsoidal contour distribution proposed in the present invention does not need to provide a priori distribution of the scale parameters of the ellipsoidal contour distribution under the variational Bayesian solution method, and solves the expectation of the scale parameters through reasonable approximation.

[0237] 3. The robot joint state estimation method based on ellipsoidal contour distribution proposed in the present invention can achieve fast and accurate estimation of velocity and acceleration based on position measurement values, and can also be extended to applications based on position and velocity measurement values, with good scalability.

[0238] 4. The various formulas contained in the robot joint state estimation method based on ellipsoidal contour distribution proposed in the present invention have been analytically given, which is suitable for computer programming and has good applicability. In order to better understand the above invention content, the simulation analysis of the specific embodiments 1 to 10 is combined with two typical examples and the attached Figure 1 To the attached Figure 9 , illustrating the actual effects of the ellipsoidal contour distribution filtering method and the interactive multi-model filtering method invented in this patent.

[0239] In the embodiment, the state vector includes three elements: position, velocity, and acceleration. The parameters in the linear discrete-time system state space model are set as follows:

[0240]

[0241] Where r is the measurement noise amplitude. To reflect the real-time requirements of robot control, the time step / filter interval is set to Δt = 2 ms. The nominal value of the process noise is set to q = 10000 rad based on the motion characteristics of high-mobility joints in actual applications. 2 / s 3 , according to the 17-bit encoder accuracy value, the nominal value of the measurement noise is set to r = 0.0001rad 2 The heavy-tailed non-Gaussian process noise and Gaussian measurement noise are generated by the following equation:

[0242]

[0243] Among them, wp means "with probability". At time step k, a random number that obeys the 0-1 uniform distribution is generated. If it is less than p, the nominal process noise increases. Figure 3 The corresponding parameter settings for two case studies are given.

[0244] The experimental study is set up as follows: In the following case study, the initial conditions of each filter are kept the same; the root mean square error is calculated as

[0245]

[0246] Among them, the number of Monte Carlo experiments M = 1000, and They represent the i={1,2,3}th components of the estimated value and the true value of the state vector in the sth Monte Carlo experiment respectively.

[0247] The following filtering methods were selected for comparison in Case Study 1 and Case Study 2:

[0248] KFTCM, the standard Kalman filter with accurate process noise covariance matrix, is used as the evaluation reference;

[0249] KFNCM, standard Kalman filtering with nominal process noise covariance matrix;

[0250] STKF, which models the state prior as Student's t-distribution and uses a mixture of Gaussians for representation;

[0251] RSTKF, which models the forecast error covariance matrix as an inverse Wishart distribution and the mixture probability as a gamma distribution;

[0252] GGM-VBAKF, which uses a Gaussian-gamma mixture distribution to model process noise;

[0253] IW-VBAKF, which models the forecast error covariance matrix as an inverse Wishart distribution;

[0254] LEC-VBAKF, this algorithm is a filter invented by this patent using LEC distribution;

[0255] EEC-VBAKF, this algorithm is a filter invented by this patent using EEC distribution;

[0256] EC-IMM, this algorithm is an interactive multi-model filtering method based on ellipsoidal contour distribution invented by this patent.

[0257] Combined with attachment Figure 1 With attached Figure 2 Write the filtering code and get the following Figure 4 To the attached Figure 9 The experimental results are shown in the attached Figure 4 To the attached Figure 6 It can be seen that the filtering method invented by this patent has excellent position, velocity and acceleration estimation accuracy in the research example 1; Figure 7 To the attached Figure 9 It can be seen that the filtering method invented in this patent has excellent position, velocity and acceleration estimation accuracy in the second study example, especially in the case of large process noise values.

[0258] The above only describes the preferred embodiments of the present invention. It should be understood that the present invention is not limited to the above-mentioned specific implementation methods. Although the present invention has been disclosed as above with preferred embodiments, it is not intended to limit the present invention. Any technician familiar with this profession can make some changes or modifications to equivalent embodiments of equivalent changes using the technical content disclosed above without departing from the scope of the technical solution of the present invention. However, any simple modifications, equivalent replacements and improvements made to the above embodiments without departing from the content of the technical solution of the present invention, based on the technical essence of the present invention, within the spirit and principles of the present invention, still fall within the scope of protection of the technical solution of the present invention.

Claims

1. A robot joint state estimation method based on ellipsoidal contour distribution, characterized in that: The following steps are involved: S1, constructing an interactive IMM filter; the interactive IMM filter includes M1 filters; S2, use sensors to collect the robot joint position data at the current time step k, and obtain the observation vector z at the current time step k based on the robot joint position data at the time step k k ; S3, obtaining the model probability, posterior probability density function, posterior state mean and posterior covariance matrix of M1 filters in the IMM filter at the previous time step k-1; the posterior state mean includes: joint position, velocity and acceleration; S4, calculating the initial probability density function of the M1 filters at the current time step k based on the model probability, posterior probability density function, posterior state mean, and posterior covariance matrix of the M1 filters in the IMM filter at the previous time step k-1 obtained in S3; and obtaining the initial state mean and initial covariance matrix of the M1 filters based on the initial probability density function of the M1 filters; S5, the observation vector z of the current time step k obtained in S2 k The initial state means and initial covariance matrix of the M1 filters at the current time step k obtained in S4 are input into the interactive IMM filter, and the M1 filters are run in parallel to obtain the measurement likelihood probability density function, posterior state mean and posterior covariance matrix of the M1 filters at the current time step k; S6. Update the model probability at the current time step k according to the measurement likelihood probability density function of the M1 filters obtained in S5; S7, calculate the fusion estimation result at the current time step k based on the model probability of the M1 filters at the time step k obtained by S6 and the posterior state mean and posterior covariance matrix of the M1 filters at the current time step k obtained by S5; The specific process of constructing the m1∈M1 filter in the interactive IMM filter in S1 is: S1.1: Construct a discrete-time state-space model of the robot joint system; the specific process is as follows: S1.1.1: Construct the dynamic and measurement equations of the robot joint system; S1.1.2: Construct a formula to calculate the predicted value; S1.1.3: Based on the obtained dynamic equations, measurement equations and predicted values ​​of the robot joint system, a hierarchical Gaussian ellipsoidal contour distribution prior probability density function p(x k |z 1:k-1 ,ξ k ); S1.1.4: Construct the probability density function p(Σ k ); S1.1.5: Construct the likelihood probability density function p(z k |x k ); construct the joint posterior probability density function p(x k ,Σ k ,ξ k |z 1:k ); S1.1.6: The layered Gaussian ellipsoid isotopic distribution prior probability density function p(x k |z 1:k-1 ,ξ k ), the probability density function of the ellipsoidal height distribution characteristic matrix p(Σ k ) and the likelihood probability density function p(z k |x k ) are combined into a discrete time state space model of the robot joint system; S1.2: Construct the parameters to be estimated Θ based on the discrete-time state-space model of the robot joint system obtained in S1.1, and construct the m1-th filter based on the parameters to be estimated Θ; S1.3: Set the initial state mean, initial covariance matrix, initial model probability of the m1th filter at time step 0, and the model transfer matrix in the IMM filter; In said S1.1.3, based on the obtained dynamic equations of the robot joint system, the measurement equations and the predicted values, a hierarchical Gaussian ellipsoidal contour distribution prior probability density function p(x k |z 1:k-1 ,ξ k )The specific process is: S1.1.3.1: Determine the prior probability density function p(x k |z 1:k-1 ) obeys the ellipsoidal iso-height distribution form, and the prior probability density function p(x k |z 1:k-1 ) is modeled as an ellipsoidal contour distribution; S1.1.3.2: The ellipsoid isometric prior probability density function p(x k |z 1:k-1 ) rewrite the layered Gaussian form ellipsoidal contour distribution prior probability density function p(x k |z 1:k-1 ,ξ k );ξ k Represents the scale parameter of the ellipsoidal contour distribution.

2. The robot joint state estimation method based on ellipsoidal contour distribution according to claim 1, characterized in that: The dynamic equations and measurement equations of the robot joint system in S1.1.1 are expressed as follows: x k =F k-1 x k-1 +w k-1 (3-1) z k =H k x k +v k (3-2) Where k = 1, 2, ... represents the time step, is the n-dimensional state vector at time step k, is the m-dimensional observation vector at time step k, is the n×n dimensional state transfer matrix at time step k-1, is the m×m dimensional observation matrix at time step k, is the n-dimensional process noise vector at time step k-1, is the m-dimensional measurement noise vector at time step k; The process noise vector w at time step k-1 k-1 The mean is zero, and the process noise vector w at time step k-1 is k-1 The n×n dimensional nominal covariance matrix is The measurement noise vector v at time step k-1 k The mean of the measurement noise vector v at time step k-1 is zero. k The m×m dimensional covariance matrix is n represents the state vector dimension, m represents the observation vector dimension, represents the set of real numbers; The prediction value formula in S1.1.2 includes: The state vector x at time step k k One-step forecast value The calculation formula and the observation vector z at time step k k The predicted value of The calculation formula is: k|k-1 Indicates that the predicted value at time step k is calculated based on the value at time step k-1; The state vector x at time step k k One-step forecast value The calculation formula is: Where, represents the mean of the posterior state when the time step is k-1; The observation vector z at time step k k The predicted value of The calculation formula is: In the formula, the one-step prediction value is the mean of the posterior state at time step k-1 The state vector x for time step k k The predicted value for which to make a prediction.

3. The robot joint state estimation method based on ellipsoidal contour distribution according to claim 2, characterized in that: The prior probability density function p(x k |z 1:k-1 ) obeys the ellipsoidal contour distribution form, including: Gaussian ellipsoidal contour distribution, logarithmic ellipsoidal contour distribution and exponential ellipsoidal contour distribution; Each ellipsoidal contour distribution has a corresponding ellipsoidal contour distribution density generating function g (·) (·); The density generating function of the Gaussian ellipsoid isohymetric distribution is: g GEC (s)=-0.5s (2-3) In the formula, s represents the auxiliary variable, The density generating function of the logarithmic ellipsoidal contour distribution is: Where p is the dimension of the random variable that obeys the ellipsoidal isohymetric distribution, v is the degree of freedom parameter, and v=5 is selected; The density generating function of the exponential ellipsoid isohymetric distribution is: Where p is the dimension of the random variable that obeys the ellipsoidal isohymetric distribution, σ = 5 represents the distribution parameter of EEC; The prior probability density function p(x k |z 1:k-1 ) is modeled as an ellipsoidal equal height distribution, and the specific process is: When it is determined to be a Gaussian ellipsoid with equal height distribution, the prior probability density function p(x k |z 1:k-1 ) is modeled as a Gaussian ellipsoid with equal height distribution, which can be expressed as: Where, Meaning is defined as; EC GEC () represents the probability density function of the Gaussian ellipsoid and the height distribution, Σ k represents the feature matrix; z 1:k-1 represents the set of observation vectors from time step 1 to time step k-1; When the logarithmic ellipsoid is determined to be a height distribution, the prior probability density function p(x k |z 1:k-1 ) is modeled as a logarithmic ellipsoidal equal-height distribution, which can be expressed as: Where, EC LEC () represents the probability density function of the logarithmic ellipsoidal isohymetric distribution; When it is determined to be an exponential ellipsoidal isoheight distribution, the prior probability density function p(x k |z 1:k-1 ) is modeled as an exponential ellipsoid with equal height distribution, which can be expressed as: Where, EC EEC () represents the probability density function of the exponential ellipsoidal isohymetric distribution; In S1.1.3.2, the ellipsoidal contour distribution prior probability density function p(x k |z 1:k-1 ) is rewritten as layered Gaussian form p(x k |z 1:k-1 ,ξ k ), hierarchical Gaussian prior probability density function p(x k |z 1:k-1 ,ξ k ) is expressed as: p(x k |z 1:k-1 ,x k )=N(x k ;m k ,S k / x k ) (3-7) p(ξ k )=p g (x) k ) (3-8) Among them, ξ k represents the scale parameter of the ellipsoidal contour distribution; p(ξ k ) represents the probability density function of the scale parameter of the ellipsoidal isoheight distribution, p g (·) represents the probability density function obtained from the ellipsoidal contour distribution density generating function g(·); Set the ellipsoid height distribution characteristic matrix Σ k The density function p(Σ k ) obeys the inverse Wishart distribution; it can be expressed as: p(Σ k )=IW(Σ k ;u k ,U k ) (3-9) Among them, p(Σ k ) represents the ellipsoidal height distribution characteristic matrix Σ k The probability density function of the inverse Wissart distribution is IW(), and u k represents the degrees of freedom parameter of the inverse Wishart distribution, U k represents the inverse scaling matrix of the inverse Wishart distribution.

4. The robot joint state estimation method based on ellipsoidal contour distribution according to claim 3 is characterized in that: The likelihood probability density function p(z k |x k The specific process is as follows: The likelihood probability density function p(z k |x k ) is modeled as a Gaussian distribution and is expressed as: p(z k |x k )=N(z k ;H k x k ,R k ) (3-10) Where: N() represents the probability density function of Gaussian distribution; The joint posterior probability density function p(x k ,Σ k ,ξ k |z 1:k )The specific process is: p(x k ,S k ,x k |z 1:k )∝p(z k |x k )p(x k |S k ,x k ,z 1:k-1 )p(Σ k )p(ξ k (3-12) In the formula, p(x k ,Σ k ,ξ k |z 1:k ) represents the joint posterior probability density function, ∝ represents proportional to, p(z k |x k ) represents the likelihood probability density function, p(x k |Σ k ,ξ k ,z 1:k-1 ) represents x k The joint prior probability density function of .

5. The robot joint state estimation method based on ellipsoidal contour distribution according to claim 4 is characterized in that: In S1.2, the parameter to be estimated Θ is constructed based on the discrete-time state space model of the robot joint system obtained in S1.1, and the m1-th filter is constructed based on the parameter to be estimated Θ. The specific process is: S1.2.1: Use the variational Bayesian method to construct an approximate solution for the joint posterior probability density function p(x k ,Σ k ,ξ k |z 1:k ) is expressed as: p(x k ,Σ k ,ξ k |z 1:k )≈q(x k )q(Σ k )q(ξ k )(4-1)where q(·) represents the approximate posterior probability density function constructed based on the variational Bayesian method; Based on the variational Bayesian method, an approximate posterior probability density function {q(x k ),q(Σ k ),q(ξ k )}, expressed by formula (4-2): {q(x k ),q(Σ k ),q(ξ k )}=argminKLD(q(x k )q(Σ k )q(ξ k )||p(x k ,Σ k ,ξ k |z 1:k )) (4-2) Where KLD(·||·) represents the Kullback-Leibler divergence operation; argmin(·) represents finding the minimum point; S1.2.2: Construct the parameters to be estimated And based on the set of parameters to be estimated Θ, the optimal solution logq(φ) of the variational Bayes problem expression (4-2) is constructed, which can be expressed as follows: Where φ represents any element in the parameter Θ to be estimated, Θ (-φ) represents the subset of the remaining elements in the parameter set Θ to be estimated except the element φ; c φ represents a constant associated with the element φ; It means to find the expectation of all elements except φ in the estimated parameters. S1.2.3: Based on the discrete-time state-space model of the robot joint system obtained in S1.1, construct the joint probability density function p(Θ,z 1:k ), which can be expressed as: S1.2.4: For the joint probability density function p(Θ,z 1:k ) takes the logarithm and obtains the logarithmic joint probability density function logp(Θ,z 1:k ), which can be expressed as: Where: n represents the dimension of the state vector, tr() represents the matrix trace operation, Represents the inverse matrix of the ellipsoidal height distribution characteristic matrix, u k represents the degrees of freedom parameter of the inverse Wishart distribution, U k represents the inverse scaling matrix of the inverse Wishart distribution; S1.2.5: Initialize the parameters in the variational Bayes problem expression to obtain the m1-th filter. The specific process is: Among them, P k|k-1 is the prediction error covariance matrix from time step k-1 to time step k, n is the state vector dimension, τ is the adjustment parameter, and τ=5 is selected. represents the initial posterior state mean at time step k; represents the initial posterior covariance matrix at time step k, is the initial value of the ellipsoidal height distribution scale parameter at time step k, F k-1 represents the state transition matrix at time step k-1, P k-1|k-1 represents the posterior covariance matrix at time step k-1; u k represents the degrees of freedom parameter of the inverse Wishart distribution, U k Represents the inverse scaling matrix of the inverse Wishart distribution.

6. The robot joint state estimation method based on ellipsoidal contour distribution according to claim 5, characterized in that: In S3, the model probability, posterior probability density function, posterior state mean and posterior covariance matrix of M1 filters in the IMM filter at the previous time step k-1 are obtained; the formula is expressed as follows: The posterior state mean of the m1th filter at time step k-1 is expressed as The posterior covariance matrix is ​​expressed as The probability of the m1th filter model at time step k-1 is expressed as The posterior probability density function p(x k-1 |η k-1 =m1,z 1:k-1 ), which can be expressed as: Among them, η k-1 Indicates the filter number selected at time step k-1; In S4, the model probability, posterior probability density function, posterior state mean and posterior covariance matrix of the M1 filters in the IMM filter at the previous time step k-1 obtained in S3 are used to calculate the initial probability density function of the M1 filters at the current time step k; and the initial state mean and initial covariance matrix of the M1 filters are obtained based on the initial probability density function of the M1 filters. The specific process is: S4.1, construct the initial probability density function p(x k-1 |η k =m1,z 1:k-1 ), It can be expressed as follows using formula (8-2): Among them, arg inf means finding the point that reaches the lower bound, m2∈M1,m2≠m1 means the m2th filter, Auxiliary variables representing intermediate calculations, is the mixing probability of filter m1 and filter m2, is the model transfer matrix The child element of represents the transition probability from model m2 to model m1; S4.2, bring the model probability, posterior state mean and posterior covariance matrix of the M1 filters at time step k-1 obtained in S3 into the initial probability density function p(x k-1 |η k =m1,z 1:k-1 ) is calculated using formula (8-2) to obtain the initial probability density function p(x k-1 |η k =m1,z 1:k-1 ), the formula is: represents the initial state mean of the m1th filter, represents the initial covariance matrix of the m1-th filter, S4.3: Repeat S4.1 to S4.2 to obtain the initial probability density function of M1 filters, and obtain the initial state mean and initial covariance matrix of M1 filters based on the initial probability density function of M1 filters. The initial state mean of the m1th filter is and the initial covariance matrix The calculation formula is: Where a represents the filter number; In S5, the observation vector z at the current time step k obtained in S2 is converted k The initial state mean and initial covariance matrix of the M1 filters at the current time step k obtained in S4 are input into the interactive IMM filter, and the M1 filters are run in parallel to obtain the measurement likelihood probability density function, posterior state mean and posterior covariance matrix of the M1 filters at the current time step k; The specific process is: S5.1: Assign the initial parameters required to run the variational Bayes method at time step k set in S1.2.5 to each of the M1 filters; S5.2: The observation vector z of the current time step k obtained in S2 k The initial state mean and initial covariance matrix of the M1 filters at the current time step k obtained by S4 are respectively input into the M1 filters of the interactive IMM filter; the M1 filters are run in parallel to obtain the measurement likelihood probability density function, posterior state mean and posterior covariance matrix of the M1 filters at the current time step k.

7. The robot joint state estimation method based on ellipsoidal contour distribution according to claim 6, characterized in that: In S5.2, the observation vector z of the current time step k obtained in S2 is converted to k The initial state mean and initial covariance matrix of the M1 filters at the current time step k obtained by S4 are respectively input into the M1 filters of the interactive IMM filter; the M1 filters are run in parallel to obtain the measurement likelihood probability density function, posterior state mean and posterior covariance matrix of the m1 filter at time step k: S5.2.1: Construct an approximate form of the joint posterior probability density function S5.2.2: Use the results from S4.3 and Update the filter's posterior state mean at time step k-1 And the posterior covariance matrix P k-1|k-1 , expressed as: Update the initial parameters required to run the variational Bayesian method for the m1th filter at time step k set in S1.2.5, run the variational Bayesian iterative algorithm, and record the number of iterations as i, and set the maximum number of iterations to N max , i is [0, N max ) S5.2.3: Compute the scale parameter ξ of the ellipsoidal contour distribution in the i+1th variational Bayesian iteration k Expectations (i+1) [ξ k ]; S5.2.4: Compute the characteristic matrix Σ of the ellipsoidal contour distribution in the i+1th variational Bayesian iteration k The expectation of the inverse matrix S5.2.5: Update the mean of the posterior states in the i+1th variational Bayesian iteration and the posterior covariance matrix S5.2.6: Repeat S5.2.3 to S5.2.5; end the iteration when the number of iterations i reaches the maximum value or the iteration stop condition is met, and obtain the final posterior state mean and the posterior covariance matrix Calculate and record the measurement likelihood probability density function The joint posterior probability density function of the approximate form is constructed in S5.2.1 The specific process is: S5.2.1.1: Based on the scale parameter ξ k S1.1.4 The expected and likelihood probability density function p(z k |x k ), we get the approximate form of the likelihood probability density function expression It can be expressed as: Where θ k represents an auxiliary variable, E[·] represents an operator for obtaining the expectation, and st represents subjection to the condition; S5.2.1.2: Likelihood probability density function according to the approximate form and Bayesian formula to obtain the approximate joint posterior probability density function It can be expressed as: Where, Represents the joint posterior probability density function of the state quantity, p(ξ k |z 1:k ) represents the posterior probability density function of the scale parameter, represents the measurement likelihood probability density function; The scale parameter ξ of the ellipsoid contour distribution in the i+1th variational Bayesian iteration is calculated in S5.2.3 k Expectations (i+1) [ξ k ]; the specific process is: S5.2.3.1: Joint posterior probability density function according to the approximate form Get the measurement likelihood probability density function It can be expressed as: S5.2.3.2: Approximate joint posterior probability density function Integrate and get The specific process is: Where, Indicated by auxiliary variable θ k is the covariance matrix of the parameters; E (i) [·] represents the expected value obtained in the i-th variational Bayesian iteration, Represents the ellipsoidal height distribution characteristic matrix Σ k The inverse matrix, H k represents the measurement matrix at time step k, Indicates H k The transposed matrix of g(·) represents the density generating function of the ellipsoid contour distribution, which is determined according to the ellipsoid contour distribution form obeyed by the selected filter, including: Gaussian ellipsoid contour distribution (GEC) density generating function g GEC (·), logarithmic ellipsoidal contour distribution (LEC) density generating function g LEC (·), the exponential ellipsoidal contour distribution (EEC) density generating function g EEC (·); S5.2.3.3: Change p(ξ k |z 1:k ) is approximately the Dirac delta function, which can be expressed as: p(ξ k |z 1:k )≈δ(ξ k -E[ξ k |z 1:k ])(5-5)In formula (5-5), δ() represents the Dirac delta function; S5.2.3.4: Combine equation (5-5) and calculate z in equations (5-2) and (5-3). k Derivative to obtain auxiliary variable θ k Calculation formula: in, represents the innovation vector, Indicates d k The transpose of represents the first derivative of the density generating function of the ellipsoidal contour distribution, S5.2.3.5: Set the maximum number of fixed point iterations to M max ; The result of the 0th fixed point iteration is set to Solve equation (5-6) by fixed point iteration method and get θ k The value of When the j-th generation fixed point iteration method is used to solve the problem, the formula is expressed as: In formula (5-7), j represents the number of fixed point iterations, represents θ k The result of the jth fixed point iteration, Denoted as θ k The result of the j+1th fixed point iteration, In the i+1th variational Bayesian iteration, E (i+1) [ξ k The updated result of ] is: The characteristic matrix Σ in the ellipsoid contour distribution in the i+1th variational Bayesian iteration is calculated in S5.2.4 k Expectation of the inverse matrix The specific process is: S5.2.4.1: The update method for solving the characteristic matrix of the ellipsoidal contour distribution is as follows: Let φ = Σ k , according to the logarithmic joint probability density function logp(Θ,z 1:k ) and the optimal solution of the variational Bayes problem logq(φ), and the optimal solution of the variational Bayes problem in the i+1th iteration logq (i+1) (Σ k ) Where i<N max represents the number of iterations of variational Bayes, N max Indicates the maximum number of variational Bayesian iterations, Represents the same as Σ k Related constants; represents the auxiliary parameters of the i-th variational Bayesian iteration; and Respectively represent the posterior state mean and posterior covariance matrix of the i-th variational Bayesian iteration; According to logq (i+1) (Σ k ) can get the ellipsoidal height distribution characteristic matrix Σ k Update method: according to the ellipsoid height distribution feature matrix Σ k Update method to calculate the ellipsoid height distribution characteristic matrix Σ k expectations; The characteristic matrix Σ k The update method is expressed by the formula: in, in and They represent Σ in the i+1th variational Bayesian iteration respectively. k the degrees of freedom parameter and inverse scaling matrix of the distribution; S5.2.4.2: Characteristic matrix Σ based on ellipsoidal contour distribution k The update method calculates the characteristic matrix Σ in the ellipsoidal height distribution in the i+1th variational Bayesian iteration k The expectation of the inverse matrix is: Where, Represents the feature matrix Σ k The inverse matrix of .

8. The robot joint state estimation method based on ellipsoidal contour distribution according to claim 7, characterized in that: The S5.2.5 updates the posterior state mean of the i+1 variational Bayesian iteration and the posterior covariance matrix The specific process is: S5.2.5.1: Let φ = x k , according to the logarithmic joint probability density function logp(Θ,z 1:k ) and the optimal solution of the variational Bayes problem logq(φ), and the optimal solution of the variational Bayes problem in the i+1th iteration logq (i+1) (x k ) is expressed as: in, Indicates that x k Related constants; S5.2.4.2: According to the scale parameter ξ of the ellipsoid contour distribution in the i+1th variational Bayesian iteration k Expectations (i+1) [ξ k ] and the characteristic matrix Σ in the ellipsoidal height distribution in the i+1th variational Bayesian iteration k Expectation of the inverse matrix Get the modified prior probability density function p (i+1) (x k |z 1:k-1 ), likelihood probability density function p (i+1) (z k |x k ), and the modified forecast error covariance matrix Expressed as: p (i+1) (z k |x k )=N(z k ;H k x k ,R k ) (7-3) S5.2.4.3: According to the standard Kalman filter method, substitute equations (7-2) to (7-4) into equation (7-1) to obtain the approximate probability density function q of the state vector in the i+1th variational Bayesian iteration (i+1) (x k )for: in, is the normalization constant; S5.2.5.4: According to equations (7-2) to (7-5), update q using the Gaussian distribution (i+1) (x k )The specific process is: Among them, the posterior state mean of the i+1th variational Bayesian iteration is and the covariance matrix Calculate as follows: Where i<N max represents the number of iterations of variational Bayes, N max Indicates the maximum number of iterations set; is the Kalman gain in the i+1th variational Bayesian iteration; The iteration stopping condition in S5.2.6 is: In formula (7-10), ζ represents the threshold of iterative convergence, which is set to ζ = 10 -10 ; Assuming that the iteration stopping condition is met in the i+1 generation, the final posterior state mean and the posterior covariance matrix As the posterior state mean and posterior covariance matrix of the m1-th filter at the current time step k, it is expressed as: Where, represents the posterior state mean of the m1th filter, represents the posterior covariance matrix of the m1-th filter.

9. The robot joint state estimation method based on ellipsoidal contour distribution according to claim 8, characterized in that: In S6, the model probability at the current time step k is updated according to the measurement likelihood probability density function of the M1 filters obtained in S5. The specific process is: The model probability of the update time step k of the m1th filter is The formula is expressed as: in, is the auxiliary variable of the m1th filter calculated by formula (8-4), a represents the filter number, The auxiliary variable of the ath filter calculated by expression (8-4), p(z k |η k =m1,z 1:k-1 ) represents the measurement likelihood probability density function of the m1-th filter, which is calculated as follows: in, represents the normalization constant, Auxiliary variables for the convenience of formula expression; Indicated by auxiliary variable θ k The covariance matrix of the parameters is obtained by formula (5-4), is the predicted value of the observation vector; In S7, the model probability of the M1 filters at time step k obtained by updating S6 and the posterior state mean and posterior covariance matrix of the M1 filters obtained in S5 are used to calculate the fusion estimation result at the current time step k. The specific process is: Where, represents the posterior state mean of the a-th filter, represents the posterior covariance matrix of the a-th filter, represents the model probability of the ath filter at time step k; The fusion estimation result at the current time step k includes: the posterior state mean at the current time step k The posterior covariance matrix P at the current time step k k|k .

Images