A robot tracking method based on Kalman filtering under random measurement loss

By modeling measurement noise as a Student t-distribution and introducing Bernoulli random variables in a nonlinear robot tracking system, and combining variational Bayesian theory and Kalman filtering, the problem of state estimation divergence caused by random measurement loss is solved, and higher-precision robot tracking is achieved.

CN114815619BActive Publication Date: 2024-11-15CHONGQING UNIV OF POSTS & TELECOMM
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210484338.3
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-05-06
Publication Date
2024-11-15
Estimated Expiration
2042-05-06

AI Technical Summary

Technical Problem

In nonlinear robot tracking systems, random measurement loss and the heavy-tailed distribution of measurement noise can cause state estimation to diverge, and existing algorithms are not robust enough in this case.

Method used

The prior distribution of measurement noise is modeled as a Student's t-distribution. A Bernoulli random variable is introduced to describe the random loss of measurement. The state of the nonlinear system and the approximate posterior distribution of the parameters to be estimated are obtained through variational Bayesian theory. The measurement noise covariance matrix is ​​corrected, and iterative updates are performed using volumetric sampling rules and Kalman filtering.

Benefits of technology

It effectively reduces the error caused by random loss of measurement signals in nonlinear robot tracking systems, maintains the stability and accuracy of the tracking process, and outperforms existing algorithms in terms of root mean square error in position, velocity, and turning rate.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114815619B_ABST
    Figure CN114815619B_ABST
Patent Text Reader

Abstract

The invention belongs to the field of robot tracking, and specifically relates to a robot tracking method based on Kalman filtering under random measurement loss. A nonlinear robot tracking system model under measurement loss is constructed, an auxiliary parameter obeying the Gamma distribution is introduced, a measurement noise prior distribution is modeled as a Student t distribution, a random variable obeying the Bernoulli distribution is used to model a state space model of the measurement loss situation, and then a traditional measurement noise covariance matrix is ​​defined as a modified measurement noise covariance matrix, and the state of the nonlinear system and the approximate posterior distribution of the estimated parameters are obtained based on the variational Bayesian theory, and finally the measurement loss probability and the filtering result estimated by the filter are output. The invention can estimate the measurement loss probability, reduce the performance error of the robot tracking, and also reduce the influence of wild value interference in the tracking process.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of robot tracking, and in particular to a robot tracking method based on Kalman filtering under random measurement loss. Background Art

[0002] Robot tracking refers to the process in which the sensor captures the robot's motion characteristics based on the collected measurement data while eliminating various noises and interferences as much as possible, thereby estimating the robot's current motion state and predicting the subsequent motion state. The Kalman filter method is an optimal autoregressive data processing algorithm that is widely used in the field of robot tracking. In actual engineering applications, due to the inevitable influence of noise, instability of signal transmission, etc., there are situations where the measurement signal is randomly lost and the measurement noise is interfered by wild values. When tracking such complex systems, the traditional Kalman filter method is very likely to fail to track and cause state estimation to diverge. Therefore, it is of great engineering significance to study the filtering method of systems with thick-tailed noise under random measurement loss.

[0003] The Chinese patent (CN105931130A) discloses an improved ensemble Kalman filter estimation method that takes into account measurement signal loss. This method first uses a Bernoulli distribution sequence to model the state space model of the measurement signal loss situation, and then uses the traditional ensemble Kalman filter as a basic tool to derive an improved ensemble Kalman filter for solving the state estimation problem in the case of measurement signal loss. However, this method cannot estimate the measurement loss probability, and the error in the final robot tracking performance is large. Jia Guangle et al. proposed an adaptive Kalman filter with unknown measurement loss probability in a linear system (G. Jia, Y. Huang, Y. Zhang and J. Chambers. A Novel Adaptive Kalman Filter With Unknown Probability of Measurement Loss [J]. IEEE Signal Processing Letters, 2019, 26 (12): 1862-1866). This method uses the Bernoulli distribution sequence to model the state space model of measurement signal loss in a linear system, and uses the variational Bayesian theory to find the approximate posterior distribution, thereby estimating the measurement loss probability and reducing the filtering error. However, the above method is not robust enough in complex nonlinear robot tracking systems where the measurement noise has a thick-tailed distribution, which leads to the problem of estimated parameter divergence. Summary of the invention

[0004] In order to solve the problem that the robot tracking state estimation is prone to divergence under random loss of measurement in a nonlinear robot tracking system, the present invention proposes a robot tracking method based on Kalman filtering under random loss of measurement, models the prior distribution of sensor measurement noise as Student's t distribution, uses Bernoulli random variables to describe the situation of random loss of measurement, and finally obtains the approximate posterior distribution of the state of the nonlinear system and the parameters to be estimated based on the variational Bayesian theory. The invention can effectively reduce the error of the nonlinear robot tracking system under random loss of measurement signals.

[0005] A robot tracking method based on Kalman filtering under random measurement loss, characterized by comprising:

[0006] S1. Constructing a nonlinear robot tracking system model under measurement loss, wherein the system model includes a measurement loss model, a robot tracking state equation and a measurement equation;

[0007] S2. Initialize the parameters of the system model, set the time interval T=1, the sampling time k, the overall running time t, and determine the state vector of the robot and its corresponding error covariance matrix;

[0008] S3. Obtain the posterior state vector of the robot at the previous moment, and predict the prior state vector and prediction error covariance matrix of the robot at the current moment according to the state equation;

[0009] S4. Obtain the distribution parameters of the measurement loss probability at the previous moment, and update the distribution parameters of the measurement loss probability by the forgetting factor;

[0010] S5. Perform N iterations based on the prediction update results of S3 and S4, iteratively update the corrected measurement noise covariance matrix, and calculate the state estimation vector and the estimation error covariance matrix according to the updated corrected noise covariance matrix;

[0011] S6. After N iterations, the optimal state estimation vector and estimated error covariance matrix are output as the posterior state vector and error covariance matrix at the current moment, and the program sampling time is updated to k=k+T; determine whether k≥t-1 is satisfied. If so, the robot tracking process ends, otherwise, the current moment is recorded as the previous moment and returns to step S3.

[0012] Furthermore, the nonlinear robot tracking system model constructed in step S1 includes:

[0013] State equations and measurement equations for robot tracking:

[0014] Among them, k is the sampling time, x k ∈R n×n is the state vector of the robot, f k-1(·) is the state transition function of the robot, w k-1 is the process noise of the robot, and w k-1 The mean is 0 and the covariance matrix is ​​Q k-1 Gaussian distribution, denoted by w k-1 ~N(w k-1 ; 0,Q k-1 ), where Q k-1 represents the covariance matrix of the process noise at time k-1, y k ∈R m×m is the sensor measurement vector, h k (·) is the measurement function of the sensor, v k For measurement noise, the measurement loss model is expressed as:

[0015]

[0016] Among them, ξ k is a Bernoulli random variable, whose value is 0 or 1, τ k is the measurement loss probability of the sensor.

[0017] Furthermore, the measurement noise follows the Student's t distribution, which can be expressed as:

[0018]

[0019] Among them, R k is the covariance matrix of the measurement noise of sensor k, λ k is the auxiliary parameter introduced, v represents the degree of freedom parameter of Student's t distribution, G(λ k ; v / 2,v / 2) represents the auxiliary parameter λ k It obeys the Gamma distribution with distribution parameter v / 2, N(v k ; 0,R k / λ k ) represents the measurement noise v k The mean is 0 and the covariance matrix is ​​R k / λ k Gaussian distribution.

[0020] Furthermore, in step S3, the posterior state vector of the robot at time k-1 is volumetrically sampled by the volume sampling rule, and the prior state vector and prediction error covariance of the robot at time k are obtained by combining the state equation of robot tracking, which is expressed as:

[0021]

[0022]

[0023]

[0024]

[0025] in, and P k|k-1 are the prior state vector and prediction error covariance of the robot at time k, X j,k-1 is the volume sampling point, For X j,k-1 The sampling point after the state transfer function transformation, S k-1 is the decomposition matrix of the robot's error covariance matrix at time k-1 obtained by Cholesky decomposition, r j is the jth column of the set of vectors with 1 as the generator in the n-dimensional space of the fully symmetric region, j = 1, 2, ... 2n, is the robot's posterior state vector at time k-1, Q k represents the covariance matrix of the process noise at time k.

[0026] Furthermore, the process of the i, i={1, 2, ..., N}th iteration in step S5 is:

[0027] S11. Get the expectation E of the Bernoulli random variable after the i-1th iteration update (i-1) [ξ k ] and auxiliary parameter expectation E (i-1) [λ k ], through E (i-1) [ξ k ] and E (i-1) [λ k ] Update the corrected measurement noise covariance matrix

[0028] S12. Based on the updated corrected measurement noise covariance matrix Calculate the state estimation vector of the robot at the current moment in the i-th iteration and the estimated error covariance matrix

[0029] S13. Calculate and update the expectation E of the Bernoulli random variable (i) [ξ k ] and auxiliary parameter expectation E (i) [λ k ].

[0030] Furthermore, the formula for updating the corrected measurement noise covariance matrix in step S11 is:

[0031]

[0032] in, represents the corrected measurement noise covariance matrix after the i-th iteration update at time k, R k represents the covariance matrix of the sensor's measurement noise at time k.

[0033] Furthermore, in step S12, the robot's current state estimation vector and the estimated error covariance matrix The update formula is:

[0034]

[0035]

[0036] in, is the prior state vector of the robot at time k, P k|k-1 is the prediction error covariance matrix of the robot at time k, y k is the measurement vector directly measured by the sensor, is the predicted measurement vector at time k, is the measurement error covariance matrix, is the Kalman coefficient, and its update formula is:

[0037]

[0038] in, is the cross-covariance matrix.

[0039] Furthermore, the update formulas of the measurement error covariance matrix and the cross-covariance matrix are:

[0040]

[0041]

[0042] in, is the measured function h k (·) Transformed sampling points, is the measured function h k (·) The transformed prediction measurement vector, is the volume sampling point, represents the corrected measurement noise covariance matrix after the i-th iteration update at time k.

[0043] Furthermore, in step S13, the expected E updated in the i-th iteration is (i) [ξ k The calculation formula of ] is:

[0044]

[0045] Among them, q (i)(ξ k =1),q (i) (ξ k =0) are Bernoulli random variables ξ k When the posterior probability density function is 1 or 0, the update formula for the i-th iteration is:

[0046] q (i) (ξ k =1) = c1exp{E (i-1) [log(1-τ k )]-0.5E (i-1) [λ k ]tr(A k (i) R k -1 )+0.5mE (i-1) [logλ k ]}

[0047] q (i) (ξ k =0) = c2exp{E (i-1) [logτ k ]-0.5E (i-1) [λ k ]tr(B k (i) R k -1 )+0.5mE (i-1) [logλ k ]}

[0048] Among them, c1 and c2 are constants in the i-th iteration, A k (i) , B k (i) represents the process parameter, τ k To measure the loss probability, E (i-1) [logτ k ] is logτ after the i-1th iteration update k Expectations, E (i-1) [log(1-τ k )] is the log(1-τ after the i-1th iteration update k ) expectations, E (i-1) [logλ k ] is the logλ after the i-1th iteration update k The expectation of k , log(1-τ k ), logλ k The update formulas of the expectation at the i-th iteration are:

[0049]

[0050]

[0051]

[0052] where ψ(·) represents the digamma function, τ is the i-th iteration k The distribution parameters of When λ is the i-th iteration k The update formulas of these four distribution parameters in the i-th iteration are:

[0053]

[0054]

[0055]

[0056]

[0057] Where m represents the dimension of the measurement vector, v represents the degree of freedom parameter of the Student's t distribution, and tr(·) represents the matrix trace operation. At the i-th iteration, according to the updated Calculate and update the auxiliary parameter expectation E (i) [λ k ], expressed as:

[0058]

[0059] Beneficial effects of the present invention:

[0060] The present invention fully considers the situation that the measurement is randomly lost and the measurement noise is distributed in a thick-tailed manner in the nonlinear robot tracking system, introduces auxiliary parameters that obey the Gamma distribution, and models the measurement noise prior distribution as the convolution of the Gaussian distribution and the Gamma distribution to reduce the influence of wild value interference in the tracking process, and introduces random variables that obey the Bernoulli distribution to model the state space model of the measurement loss situation, so as to obtain the probability of measurement loss more accurately. Secondly, the traditional measurement noise covariance matrix is ​​defined as the modified measurement noise covariance matrix, and the approximate posterior distribution of the state of the nonlinear system and the parameters to be estimated is obtained based on the variational Bayesian theory, and finally the measurement loss probability and filtering result estimated by the filter are output.

[0061] In the field of robot tracking, existing algorithms may experience divergence due to tracking failure due to the influence of random loss of measurement and thick-tailed distribution of measurement noise. In the nonlinear robot tracking simulation experiment, the algorithm proposed in this paper is superior to other algorithms in terms of root mean square error of position, speed and turning rate. When other algorithms diverge, this algorithm can still maintain convergence and successfully track the robot motion, demonstrating the superiority of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 It is a schematic diagram of a robot tracking method based on Kalman filtering under random measurement loss of the present invention;

[0063] Figure 2 A flow chart of a robot tracking method according to an embodiment of the present invention;

[0064] Figure 3 A robot tracking algorithm based on Kalman filtering under random loss of one-step measurement according to an embodiment of the present invention;

[0065] Figure 4 is a comparison diagram of the root mean square error of the robot position according to an embodiment of the present invention;

[0066] Figure 5 is a comparison diagram of the root mean square error of the robot speed according to an embodiment of the present invention;

[0067] Figure 6 A comparison diagram of the root mean square error of the turning rate of the robot according to an embodiment of the present invention;

[0068] Figure 7 A comparison diagram of measurement loss probability estimation according to an embodiment of the present invention;

[0069] Figure 8 4 is a thick-tailed probability graph of an embodiment of the present invention. DETAILED DESCRIPTION

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

[0071] A robot tracking method based on Kalman filtering under random measurement loss, such as Figure 1-2 As shown, the following steps are included:

[0072] S1. Constructing a nonlinear robot tracking system model under measurement loss, wherein the system model includes a measurement loss probability that obeys a binomial distribution, a state equation for robot tracking, and a measurement equation;

[0073] S2. Initialize the parameters of the system model, set the time interval T=1, the sampling time k, the overall running time t, and determine the state vector of the robot and its corresponding error covariance matrix;

[0074] S3. Obtain the posterior state vector of the robot at the previous moment, and predict the prior state vector and prediction error covariance matrix of the robot at the current moment according to the state equation;

[0075] S4. Obtain the distribution parameters of the measurement loss probability at the previous moment, and update the distribution parameters of the measurement loss probability by the forgetting factor;

[0076] S5. Perform N iterations based on the prediction update results of S3 and S4, iteratively update the corrected measurement noise covariance matrix, and calculate the state estimation vector and the estimation error covariance matrix according to the updated corrected noise covariance matrix;

[0077] S6. After N iterations, output the optimal state estimation vector and estimated error covariance matrix as the posterior state vector and error covariance matrix at the current moment; determine whether k≥t-1 is satisfied. If so, the robot tracking process ends; otherwise, record the current moment as the previous moment and return to step S3.

[0078] In one embodiment, a nonlinear robot tracking system model under random measurement loss is constructed, which includes a measurement loss model, a state equation of robot tracking, and a measurement equation:

[0079] The measurement loss model is expressed as:

[0080]

[0081] Introduce a Bernoulli random variable ξ that follows a Bernoulli distribution k , whose value is 0 or 1, using the Bernoulli random variable ξ k Describes the situation where measurements are randomly lost, τ k is the measurement loss probability of the sensor.

[0082] State equations and measurement equations for robot tracking:

[0083] Among them, k is the sampling time, x k ∈R n×n is the state vector of the robot, f k-1 (·) is the state transition function of the robot, w k-1 is the process noise of the robot, and wk-1 The mean is 0 and the covariance matrix is ​​Q k-1 Gaussian distribution, denoted by w k-1 ~N(w k-1 ; 0,Q k-1 ), y k ∈R m×m is the sensor measurement vector, h k (·) is the measurement function of the sensor, v k is the measurement noise, that is, uncertain thick-tailed noise.

[0084] Introduce the auxiliary parameter λ that follows the Gamma distribution k , the measurement noise prior distribution is modeled as the convolution of Gaussian distribution and Gamma distribution, that is, Student's t distribution, expressed as:

[0085]

[0086] Among them, R k is the covariance matrix of the measurement noise of sensor k, v represents the degree of freedom parameter of Student's t distribution, G(λ k ; v / 2,v / 2) represents the auxiliary parameter λ k It obeys the Gamma distribution with distribution parameter v / 2, N(v k ; 0,R k / λ k ) represents the measurement noise v k The mean is 0 and the covariance matrix is ​​R k / λ k Gaussian distribution.

[0087] In one embodiment, target tracking is performed based on a constructed nonlinear robot tracking system model, such as Figure 2 As shown, first initialize the model parameters at time zero (k = 0) to obtain the robot's state vector Error covariance matrix P 0|0 , the covariance matrices Q0 and R0 of process noise and measurement noise, set the measurement loss probability of the sensor at the initial moment to τ0, the time interval to T, and the overall running time to t.

[0088] Assume that the estimated value of the robot state vector at time k-1 (posterior state vector) is The state error covariance matrix is ​​P k-1|k-1 , through the volume sampling rule By performing volume sampling, we can obtain the robot's prior state vector at time k and the prediction error covariance P k|k-1 , the specific process is expressed as:

[0089]

[0090]

[0091]

[0092]

[0093]

[0094] Among them, S k-1 is the error covariance matrix P of the robot at time k-1 k-1|k-1 The decomposition matrix obtained by Cholesky decomposition, YesS k-1 The transposed matrix, r j is the jth column of the set of vectors with 1 as the generator in the n-dimensional space of the fully symmetric region, j = 1, 2, ... 2n, n is the dimension of the state vector; X j,k-1 is the volume sampling point, For X j,k-1 The sampling points after transformation by the state transfer function f(·).

[0095] At the same time, at time k, the loss probability τ at time k-1 is adjusted by the forgetting factor ρ∈(0,1] k-1 The distribution parameters of and Perform time update, expressed as:

[0096]

[0097]

[0098] In this embodiment, the full set Ω is set to {x k ,λ k ,ξ k ,τ k}, subset θ∈Ω, according to variational Bayes theory:

[0099] p(Ω,y 1:k )≈q(x k )q(ξ k )q(τ k )q(λ k )

[0100] This formula represents the robot state vector x k , Bernoulli random variable ξ k , measurement loss probability τ k And the auxiliary parameter λ k The joint posterior distribution of q(x k ),q(ξ k ), q(τk ),q(λ k ) are x k , k , τ k , k The approximate posterior distribution of , where the joint posterior probability density function of the joint posterior distribution is expressed as:

[0101]

[0102] The measurement process of this embodiment is based on variational Bayesian, and the measurement noise covariance matrix is ​​updated and corrected through N iterations, so that E (0) [ξ k ]=1,E (0) [λ k ]=1, where γ (i) is the i-th iteration of γ, E(·) is the expectation operator, and all c (·) All represent constants.

[0103] Preferably, in the i-th iteration, let the process parameters be:

[0104] A k (i) =[y k -h k (x k )][y k -h k (x k )] T

[0105] B k (i) =y k y k T

[0106] Specifically, when θ = x k When:

[0107]

[0108] The above formula can be updated Represent the state estimation vector and estimated error covariance matrix of the robot at time k in the i-th iteration, respectively. They are updated by cubature Kalman. The update process of the i-th iteration includes: performing Cholesky decomposition on the error covariance matrix of the robot at time k-1, and the decomposition matrix S k-1 , through the volume sampling rule, we get the predicted measurement vector after h(·) transformation Cross-covariance matrix and the measurement error covariance matrix Then update the Kalman coefficient K k (i) , calculate the robot state estimation vector and the estimated error covariance matrix The specific process is expressed as:

[0109]

[0110]

[0111]

[0112]

[0113]

[0114]

[0115]

[0116]

[0117]

[0118] Specifically, when θ = λ k When:

[0119]

[0120] The above formula can be updated in is the corresponding i-th iteration cycle k The distribution parameters can be updated by the following formula:

[0121]

[0122]

[0123] Specifically, when θ = ξ k When:

[0124]

[0125] Bernoulli random variable ξ k The posterior probability density function when it is 0 or 1 is updated by the following formula:

[0126] q (i) (ξ k =1) = c1exp{E (i-1) [log(1-τ k )]-0.5E (i-1) [λk ]tr(A k (i) R k -1 )+0.5mE (i-1) [logλ k ]}

[0127] q (i) (ξ k =0) = c2exp{E (i-1) [logτ k ]-0.5E (i-1) [λ k ]tr(B k (i) R k -1 )+0.5mE (i-1) [logλ k ]}

[0128] Where c1 and c2 represent constants at the i-th iteration.

[0129] Specifically, when θ=τ k When:

[0130]

[0131] is the corresponding variation iteration cycle τ k The distribution parameters of are updated by:

[0132]

[0133]

[0134] Specifically, each expectation involved in the above formula needs to be updated every iteration, and the update formulas are:

[0135]

[0136]

[0137]

[0138]

[0139]

[0140] where ψ(·) represents the digamma function.

[0141] In one embodiment, a robot tracking algorithm based on Kalman filtering under random loss of one-step measurement, such as Figure 3As shown, including:

[0142] S11. Input

[0143] S12. Predict the prior state vector at time k and the prediction error covariance matrix P k|k-1 , and update the measurement loss probability τ through the forgetting factor ρ k-1 The distribution parameters of and get

[0144] S13. Iteration initialization, initializing the number of iterations i to 0;

[0145] S14. Perform N variational iterations, and output the optimal state estimation vector at the current k moment after N iterations. and the estimated error covariance matrix, which are recorded as the posterior state vector and error covariance matrix at time k respectively.

[0146] The specific process of each variational iteration is as follows:

[0147] S101. Obtain the expectation E of the Bernoulli random variable after the i-1th iteration update (i-1) [ξ k ] and auxiliary parameter expectation E (i-1) [λ k ], through E (i-1) [ξ k ] and E (i-1) [λ k ] Update the corrected measurement noise covariance matrix

[0148] S102. Based on the updated corrected measurement noise covariance matrix Update the robot's current state estimation vector at the i-th iteration and the estimated error covariance matrix

[0149] S103. Calculate and update the expectation E of the Bernoulli random variable (i) [ξ k ] and auxiliary parameter expectation E (i) [λ k ].

[0150] In one embodiment, a simulation system based on a nonlinear robot tracking system is designed, wherein the target robot is located in a Cartesian coordinate system. xy Coordinate system movement. The robot state vector x = [s x v x s y vy w] T ,s x 、s y Respectively represent the position in the x and y directions, v x 、v y Represent the speed in the x and y directions respectively, w is the turning rate, and T is the sampling period, which is 1s, and t is the total simulation time, which is 1000s.

[0151] The state transition function of the robot is

[0152] The covariance matrix is

[0153] Assuming that the sensor is located at the origin of the coordinate system, the measurement equation can be expressed as:

[0154]

[0155] in,

[0156] v k The covariance matrix of k and 100R k The probabilities are 0.98 and 0.02 respectively. The state vector x0 and the state error covariance matrix P0 at the initial moment are:

[0157]

[0158]

[0159] The algorithm proposed in the present invention and the AKF and BKF-1 algorithms were used to conduct experiments, and the root mean square error (RMSE) results of position, speed and rotation rate were shown in Table 1:

[0160] Table 1 Comparison of algorithm results

[0161] algorithm Position RMSE(m) Speed ​​RMSE (m / s) Rotation rate RMSE (rad / s) This article's algorithm 10.8679 3.7354 1.9507 BKF-I 19.6880 6.4195 2.5238 GA 23.2146 6.7851 2.5820

[0162] exist Figure 4-6 In the figure, the horizontal axis represents time, and the vertical axis represents the magnitude of the root mean square error. Figure 4-8From the results shown, it can be seen that the algorithm proposed in the present invention is superior to the existing filtering methods in the root mean square errors of the position, velocity and turning rate of the robot state estimation when the measurement is randomly lost and the measurement noise is thick-tailed. The reason is that in the AKF and BKF-1 filters, the measurement noise is modeled as an ordinary Gaussian distribution, which cannot well fit the thick-tailed noise in the case of wild value interference in the sensor. Therefore, when wild value interference occurs, the filter performance is severely degraded, which ultimately leads to a large root mean square error.

[0163] The algorithm proposed in the present invention models the thick-tailed measurement noise as a Student's t distribution by introducing auxiliary parameters. In the variational iteration process, the modified measurement noise covariance matrix is ​​used for calculation to obtain the optimal estimate of the joint distribution. This can effectively deal with the situations of random loss of measurement and wild value interference in the presence of noise, reflecting the superiority of the algorithm proposed in the present invention.

[0164] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A robot tracking method based on Kalman filtering under random measurement loss, characterized in that: include: S1. Constructing a nonlinear robot tracking system model under measurement loss, wherein the system model includes a measurement loss model, a robot tracking state equation and a measurement equation; The nonlinear robot tracking system model constructed in step S1 includes: State equations and measurement equations for robot tracking: Among them, k is the sampling time, x k ∈R n×n is the state vector of the robot, f k-1 (·) is the state transition function of the robot, w k-1 is the process noise of the robot, and w k-1 The mean is 0 and the covariance matrix is ​​Q k-1 Gaussian distribution, denoted by w k-1 ~N(w k-1 ; 0,Q k-1 ), y k ∈R m×m is the sensor measurement vector, h k (·) is the measurement function of the sensor, v k For measurement noise, the measurement loss model is expressed as: Among them, ξ k is a Bernoulli random variable, whose value is 0 or 1, τ k is the measurement loss probability of the sensor; The measurement noise follows the Student's t distribution, expressed as: Among them, R k is the covariance matrix of the sensor measurement noise, λ k is the auxiliary parameter introduced, v represents the degree of freedom parameter of Student's t distribution, G(λ k ; v / 2,v / 2) represents the auxiliary parameter λ k It follows the Gamma distribution with distribution parameter v / 2; S2. Initialize the parameters of the system model, set the time interval T=1, the sampling time k, the overall running time t, and determine the initial state vector of the robot and its corresponding initial error covariance matrix; S3. Obtain the posterior state vector of the robot at the previous moment, and predict the prior state vector and prediction error covariance matrix of the robot at the current moment according to the state equation; In step S3, the volume sampling rule is used to perform volume sampling on the robot's posterior state vector at time k-1. Combined with the robot tracking state equation, the robot's prior state vector and prediction error covariance at time k are obtained, which can be expressed as: in, and P k|k-1 are the prior state vector and prediction error covariance of the robot at time k, X j,k-1 is the volume sampling point, For X j,k-1 The sampling point after the state transfer function transformation, S k-1 is the decomposition matrix of the robot's error covariance matrix at time k-1 obtained by Cholesky decomposition, r j is the jth column of the set of vectors with 1 as the generator in the n-dimensional space of the fully symmetric region, j = 1, 2, ... 2n, is the robot's posterior state vector at time k-1, Q k represents the covariance matrix of the process noise; S4. Obtain the distribution parameters of the measurement loss probability at the previous moment, and update the distribution parameters of the measurement loss probability by the forgetting factor; S5. Perform N iterations based on the prediction update results of S3 and S4, iteratively update the corrected measurement noise covariance matrix, and calculate the state estimation vector and the estimation error covariance matrix according to the updated corrected noise covariance matrix; The process of the i-th, i={1,2,...,N}th iteration in step S5 is: S11. Get the expectation E of the Bernoulli random variable after the i-1th iteration update (i-1) [ξ k ] and auxiliary parameter expectation E (i-1) [λ k ], through E (i-1) [ξ k ] and E (i-1) [λ k ] Update the corrected measurement noise covariance matrix S12. Based on the modified measurement noise covariance matrix Calculate the state estimation vector of the robot at the current moment in the i-th iteration and the estimated error covariance matrix S13. Calculate and update the expectation E of the Bernoulli random variable (i) [ξ k ] and auxiliary parameter expectation E (i) [λ k ]; The formula for updating the corrected measurement noise covariance matrix in step S11 is: in, represents the corrected measurement noise covariance matrix after the i-th iteration update at time k, R k represents the covariance matrix of the sensor's measurement noise at time k; In step S12, the robot's current state estimation vector and the estimated error covariance matrix The update formula is: in, is the prior state vector of the robot at time k, P k|k-1 is the prediction error covariance matrix of the robot at time k, y k is the sensor’s measurement vector, is the predicted measurement vector at time k, is the measurement error covariance matrix, is the Kalman coefficient, and its update formula is: in, is the cross-covariance matrix; The update formulas of the measurement error covariance matrix and the cross-covariance matrix are: in, is the measured function h k (·) Transformed sampling points, is the measured function h k (·) The transformed prediction measurement vector, is the volume sampling point, represents the corrected measurement noise covariance matrix after the i-th iteration update at time k; In step S13, the expected E of the i-th iteration update is (i) [ξ k The calculation formula of ] is: Among them, q (i) (ξ k =1),q (i) (ξ k =0) are Bernoulli random variables ξ k When the posterior probability density function is 1 or 0, the update formula for the i-th iteration is: q (i) (x) k =1)=c1exp{E (i-1) [log(1-τ k )]-0.5E (i-1) [l k ]tr(A k (i) R k -1 )+0.5mE (i-1) [logλ k ]} q (i) (x) k =0)=c2exp{E (i-1) [logτ k ]-0.5E (i-1) [l k ]tr(B k (i) R k -1 )+0.5mE (i-1) [logλ k ]} Among them, c1 and c2 are constants in the i-th iteration, A k (i) , B k (i) represents the process parameter, τ k To measure the loss probability, E (i-1) [logτ k ] is logτ after the i-1th iteration update k Expectations, E (i-1) [log(1-τ k )] is the log(1-τ after the i-1th iteration update k ) expectations, E (i-1) [logλ k ] is the logλ after the i-1th iteration update k The expectation of k , log(1-τ k ), logλ k The update formulas of the expectation at the i-th iteration are: where ψ(·) represents the digamma function, τ is the i-th iteration k The distribution parameters of When λ is the i-th iteration k The update formulas of these four distribution parameters in the i-th iteration are: Where m represents the dimension of the measurement vector, v represents the degree of freedom parameter of the Student's t distribution, and tr(·) represents the matrix trace operation. At the i-th iteration, according to the updated Calculate and update the auxiliary parameter expectation E (i) [λ k ], expressed as: S6. After N iterations, the optimal state estimation vector and estimated error covariance matrix are output as the posterior state vector and error covariance matrix at the current moment, and the program sampling time is updated to k=k+T; determine whether k≥t-1 is satisfied. If so, the robot tracking process ends, otherwise, the current moment is recorded as the previous moment and return to step S3.