A robot positioning method based on background impulse Kalman filtering

Abstract

This invention provides a robot localization method based on background pulse Kalman filtering, comprising: constructing a linear dynamic system with multiple motion states and a Markov transition probability matrix; applying an interactive multi-model Kalman filter algorithm based on the linear dynamic system and the Markov transition probability matrix to obtain the estimated values ​​of the state vectors of the motion models and the model probabilities of the motion models; obtaining the estimated values ​​of the state vectors based on the estimated values ​​of the state vectors of the motion models and the model probabilities of the motion models; applying the background pulse Kalman filter algorithm to each motion model to obtain the estimated values ​​of the state vectors of the robot's motion models and the covariance matrix of the state vectors of the motion models; and iteratively solving to obtain the estimated values ​​of the state vectors of the motion models and the covariance matrix of the state vectors of the motion models at the next time step, thereby achieving high-precision localization of the robot.

Description

Technical Field

[0001] This invention relates to the field of robot localization technology, and more specifically, to a robot localization method based on background pulse Kalman filtering. Background Technology

[0002] Against the backdrop of innovation and technological transformation, autonomous localization technology has become a key research focus in the robotics industry, attracting increasing attention. However, current robot localization technologies suffer from significant shortcomings in stability and accuracy due to the influence of non-Gaussian noise, greatly hindering the development of the intelligent robot industry. In real-world conditions, a robot's motion state is often variable rather than singular, and the timing of these changes is extremely difficult to predict. Therefore, accurately estimating the robot's state under non-Gaussian noise conditions is a major research focus. The Kalman filter (KF) method, commonly used for robot localization, is only applicable to Gaussian noise under a single motion state; while the maximum correlation entropy Kalman filter (MCKF) and minimum error entropy Kalman filter (MEEKF) methods are only applicable to non-Gaussian noise under a single motion state; the interactive multiple model Kalman filter (IMKF) method is suitable for estimating the robot's state under multiple motion states, but it only achieves good localization accuracy under Gaussian noise conditions and is not applicable to non-Gaussian noise conditions.

[0003] Based on the above, this invention proposes a robot localization method based on background pulse Kalman filtering to process the state information (position, velocity, acceleration, etc.) of a robot in multiple motion states under non-Gaussian noise conditions, thereby significantly improving the robot's localization accuracy. Summary of the Invention

[0004] The purpose of this invention is to provide a robot localization method based on background pulse Kalman filtering, comprising: constructing a linear dynamic system with multiple motion states and a Markov transition probability matrix; applying an interactive multi-model Kalman filter algorithm based on the linear dynamic system and the Markov transition probability matrix to obtain estimated values ​​of the state vectors of the motion models and model probabilities of the motion models; obtaining estimated values ​​of the state vectors based on the estimated values ​​of the state vectors of the motion models and the model probabilities of the motion models; applying a background pulse Kalman filter algorithm to each motion model to obtain estimated values ​​of the state vectors of the robot's motion models and the covariance matrix of the state vectors of the motion models; and iteratively solving for the estimated values ​​of the state vectors of the motion models at the next time step based on the estimated values ​​of the state vectors of the motion models at the previous time step and the covariance matrix of the state vectors of the motion models, thereby achieving robot localization.

[0005] Furthermore, the expression for the linear dynamical system is:

[0006]

[0007] Where X(k+1) represents the p-dimensional state vector at time k+1; F m This represents the state transition matrix of the m-th motion model; m = 1, 2, ..., n. It represents the m-th motion model of a system with n motion states; X(k) ∈ R p w represents a p-dimensional state vector at time k; m (k)∈R p Z(k) represents the state noise of the m-th motion model at time k; q H represents the q-dimensional observation vector at time k; m r represents the observation transition matrix of the m-th motion model; m (k)∈R q This represents the observation noise at time k for the m-th motion model;

[0008] The expression for the Markov transition probability matrix is:

[0009]

[0010] Where P represents the Markov transition probability matrix; P im This represents the transition probability from motion model i to motion model m.

[0011] Furthermore, the step of applying the interactive multi-model Kalman filter algorithm based on the linear dynamical system and the Markov transition probability matrix to obtain the estimated values ​​of the state vectors of the motion model and the model probabilities of the motion model includes: calculating the mixed normalized model probabilities from motion model i to motion model m based on the Markov transition probability matrix; calculating the initial fused state vector of motion model m based on the mixed normalized model probabilities; calculating the mixed covariance estimation matrix of motion model m based on the mixed normalized model probabilities and the initial fused state vector; and obtaining the predicted values ​​of the state vectors of motion model m at each time step, the predicted values ​​of the error covariance matrix of the state vectors of motion model m at each time step, the error between the observed vectors and the predicted observed vectors of motion model m at each time step, and the estimated values ​​of the state vectors of motion model m at each time step through Kalman filtering based on the linear dynamical system, the initial fused state vectors, and the mixed covariance estimation matrix. The covariance matrix of the observation vector of the motion model m and the Kalman gain of the motion model m at each time step are calculated. Based on the predicted value of the state vector of the motion model m at each time step, the error between the observed vector and the predicted observed vector at each time step, and the Kalman gain of the motion model m at each time step, the estimated value of the state vector of the motion model is calculated. Based on the covariance matrix of the observation vector of the motion model m at each time step and the error between the observed vector and the predicted observed vector at each time step, the likelihood function of the observation vector at each time step is obtained. Based on the predicted value of the error covariance matrix of the state vector of the motion model m at each time step, the Kalman gain of the motion model m at each time step, and the covariance matrix of the observation vector of the motion model m at each time step, the covariance matrix of the state vector of the motion model m at each time step is updated. Based on the Markov transition probability matrix and the likelihood function, the model probability of the motion model is updated.

[0012] Furthermore, the predicted values ​​of the state vector of motion model m at each time step, the predicted values ​​of the error covariance matrix of the state vector of motion model m at each time step, the error between the observed vector and the predicted observed vector of motion model m at each time step, the covariance matrix of the observed vector of motion model m at each time step, the Kalman gain of motion model m at each time step, and the expression for calculating the estimated value of the state vector of the motion model are obtained as follows:

[0013]

[0014]

[0015] in, This represents the estimated state vector of model m at time k; K represents the predicted state vector of model m at time k; m (k) represents the Kalman gain of model m at time k; vm (k) represents the error between the observed vector of model m at time k and the predicted observed vector; F m X represents the state transition matrix of the m-th motion model; pre_ (k-1|k-1) represents the initial fusion state vector at time k-1; P m (k|k-1) represents the predicted value of the error covariance matrix of the state vector of motion model m at time k; P pre_ (k-1|k-1) represents the mixture covariance estimation matrix of model m at time k-1; Q m Z(k) ∈ R represents the covariance matrix of the state noise of model m; q H represents the q-dimensional observation vector at time k; m S represents the observation transition matrix of the m-th motion model; m (k) represents the covariance matrix of the observation vector of model m at time k; R represents the covariance matrix of the observation noise of model m; * T Indicates the transpose operation; * -1 This indicates the inverse operation.

[0016] Furthermore, the expression for updating the covariance matrix of the state vector of the motion model m at each time step is:

[0017]

[0018] Among them, P m (k|k) represents the covariance matrix of the state vector of model m at time k; P m (k|k-1) represents the predicted value of the error covariance matrix of the state vector of motion model m at time k; K m (k) represents the Kalman gain of model m at time k; S m (k) represents the covariance matrix of the observation vectors of model m at time k; K m (k) represents the Kalman gain of model m at time k; * T This indicates the transpose operation.

[0019] Furthermore, the expression for updating the model probability of the motion model is:

[0020]

[0021] Where, μ m (k) represents the model probability of model m at time k; Λ m (k) represents the likelihood function of model m at time k; P im μ represents the transition probability from motion model i to motion model m; m = 1, 2, ..., n. μ represents the m-th motion model of a system with n motion states. i(k-1) represents the probability of model i at time k-1.

[0022] Furthermore, the expression for obtaining the estimated value of the state vector is:

[0023]

[0024] in, This represents the estimated value of the state vector at time k obtained after processing using the IMMBIKF method; μ represents the estimated state vector of motion model m at time k; m = 1, 2, ..., n, representing the m-th motion model of a system with n motion states; μ m (k) represents the model probability of motion model m at time k.

[0025] Furthermore, the step of applying the background impulse Kalman filter algorithm to each motion model to obtain the estimated value of the state vector of the robot's motion model and the covariance matrix of the state vector of the motion model includes: processing the observation noise—Gaussian mixture noise—of the motion model using the expectation-maximization algorithm to obtain the background noise model probability and the impulse noise model probability; and obtaining the Markov transition probability matrix of the noise model by modeling the state vector of each motion model as a weighted sum of the state vectors of two identical motion models respectively affected by background noise and impulse noise; initializing the noise model probability of the noise model, the estimated value of the state vector corresponding to the noise model, and the prior covariance matrix of the state vector of the noise model; based on the... Given the Markov transition probability matrix of the noise model and the noise model probability, calculate the probability of transitioning from noise model i to noise model j; based on the estimated value of the state vector corresponding to the noise model and the probability of transitioning from noise model i to noise model j, obtain the mixed state estimation vector of noise model j; based on the estimated value of the state vector corresponding to the noise model, the prior covariance matrix of the state vector of the noise model, the probability of transitioning from noise model i to noise model j, and the mixed state estimation vector of noise model j, obtain the mixture covariance matrix of the state vector of noise model j; based on the mixed state estimation vector of noise model j and the mixture covariance matrix of the state vector of noise model j, obtain... This represents the predicted value of the state vector of the noise model j at time k. This represents the predicted value of the error covariance matrix of the state vector of the noise model j at time k. The error of the observation vector of the noise model j at time k is represented by... The sum of the covariance matrices of the observation vectors of the noise model at time k. Let K represent the Kalman gain of the noise model j at time k; and based on the predicted value of the state vector of the noise model j, the Kalman gain of the noise model j, and the error of the observation vector of the noise model j, obtain the estimated value of the state vector of the noise model j; based on the predicted value of the Kalman gain of the noise model j and the error covariance matrix of the state vector of the noise model j, obtain the posterior covariance matrix of the noise model j; based on the Markov transition probability matrix of the noise model, the noise model probability of the noise model, the covariance matrix of the observation vector of the noise model j, and the error of the observation vector of the noise model j, obtain the likelihood function of the noise model j; based on the likelihood function of the noise model j, update the noise model probability of the noise model j; based on the noise model probability of the noise model j and the estimated value of the state vector of the noise model j, obtain the estimated value of the state vector of the motion model; based on the estimated value of the state vector of the noise model j, the posterior covariance matrix of the noise model j, and the estimated value of the state vector of the motion model m, obtain the covariance matrix of the state vector of the motion model.

[0026] Furthermore, the expressions for the predicted value of the state vector of the noise model j, the predicted value of the error covariance matrix of the state vector of the noise model j, the error of the observation vector of the noise model j, the covariance matrix of the observation vector of the noise model j, and the Kalman gain of the noise model j, as well as the expression for the estimated value of the state vector of the noise model j, are obtained as follows:

[0027]

[0028]

[0029] in, This represents the estimated value of the state vector of model j at time k; This represents the predicted value of the state vector of model j at time k; This represents the Kalman gain of model j at time k; A represents the error of the observation vector of model j at time k; k This represents the state transition matrix of motion model m at time k; This represents the mixed state estimation vector of model j at time k-1; This represents the predicted value of the error covariance matrix of the model j state vector at time k; Q represents the mixture covariance matrix of the model j-state vector at time k-1; k y represents the covariance matrix of the state noise of the motion model m at time k; k C represents the observed value of the state vector at time k; k Let represent the observation transition matrix of motion model m at time k; This represents the covariance matrix of the model's observation vectors at time k; This represents the covariance of the noise in model j at time k; * T Indicates the transpose operation; * -1 This indicates the inverse operation.

[0030] Furthermore, the expression for obtaining the estimated value of the state vector of the motion model is:

[0031]

[0032] in, This represents the estimated state vector of motion model m at time k; This represents the probability of the noisy model of model j at time k; This represents the estimated value of the state vector of model j at time k;

[0033] The expression for the covariance matrix of the state vector of the motion model is as follows:

[0034]

[0035] Among them, P k| The covariance matrix represents the state vector of motion model m at time k; This represents the probability of being model j at time k; Let represent the posterior covariance matrix of model j at time k; This represents the estimated value of the state vector of model j at time k; This represents the estimated state vector of motion model m at time k; T This indicates the transpose operation.

[0036] The technical solutions of the embodiments of the present invention have at least the following advantages and beneficial effects:

[0037] This invention proposes a novel IMMBI KF method by combining interactive multi-model theory with the BI KF method. By applying the IMMBI KF method to robot localization technology, the accuracy of robot localization is greatly improved. Attached Figure Description

[0038] Figure 1 An exemplary flowchart of a robot localization method based on background pulse Kalman filtering provided for some embodiments of the present invention;

[0039] Figure 2 The actual state information X(k) of the robot and the estimated value of the state information obtained by the IMMKF and IMMBIKF methods. The root mean square error of displacement information in the X direction;

[0040] Figure 3The actual state information X(k) of the robot and the estimated value of the state information obtained by the IMMKF and IMMBIKF methods. The root mean square error of displacement information in the Y direction. Detailed Implementation

[0041] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0042] Figure 1 This is an exemplary flowchart illustrating a robot localization method based on background pulse Kalman filtering, provided for some embodiments of the present invention. Figure 1 As shown, process 100 may include the following:

[0043] Step 110: Construct a linear dynamic system with multiple motion states and a Markov transition probability matrix.

[0044] In some embodiments, the expression for the linear dynamical system is:

[0045]

[0046] Where X(k+1) represents the p-dimensional state vector at time k+1; F m This represents the state transition matrix of the m-th motion model; m = 1, 2, ..., n. It represents the m-th motion model of a system with n motion states; X(k) ∈ R p w represents a p-dimensional state vector at time k; m (k)∈R p Z(k) represents the state noise of the m-th motion model at time k; q H represents the q-dimensional observation vector at time k; m r represents the observation transition matrix of the m-th motion model; m (k)∈R q This represents the observation noise at time k for the m-th motion model;

[0047] In some embodiments, the expression for the Markov transition probability matrix is:

[0048]

[0049] Where P represents the Markov transition probability matrix; P im This represents the transition probability from motion model i to motion model m.

[0050] Step 120: Based on the linear dynamic system and the Markov transition probability matrix, apply the interactive multi-model Kalman filter algorithm to obtain the estimated value of the state vector of the motion model and the model probability of the motion model.

[0051] In some embodiments, the process of obtaining the estimated state vector of the motion model and the model probability of the motion model may include the following:

[0052] Step 120-1: Based on the Markov transition probability matrix, calculate the mixed normalized model probability from motion model i to motion model m.

[0053] In some embodiments, the mixed normalized model probability μ of models i to m is calculated. im The expression is:

[0054]

[0055] Where, μ im (k-1|k-1) represents the mixture normalized model probability from model i to model m at time k-1, μ i (k-1) represents the probability of model i at time k-1.

[0056] Step 120-2: Calculate the initial fusion state vector of the motion model m based on the hybrid normalized model probability.

[0057] In some embodiments, the initial fusion state vector X of the computation model m is calculated. pre_m The expression is:

[0058]

[0059] Among them, X pre_m (k-1|k-1) represents the initial fusion state vector at time k-1. This represents the estimated state vector of motion model i at time k-1.

[0060] Step 120-3: Based on the hybrid normalized model probability and the initial fused state vector, calculate the hybrid covariance estimation matrix of the motion model m.

[0061] In some embodiments, the mixture covariance estimation matrix P of model m is calculated. pre_m The expression is:

[0062]

[0063] Among them, P pre_m(k-1|k-1) represents the mixture covariance estimation matrix of model m at time k-1, P i (k-1|k-1) represents the error covariance matrix of the state vector of model i at time k-1.

[0064] Step 120-4: Based on the linear dynamic system, the initial fused state vector, and the hybrid covariance estimation matrix, Kalman filtering is used to obtain the predicted value of the state vector of the motion model m at each time step, the predicted value of the error covariance matrix of the state vector of the motion model m at each time step, the error between the observed vector and the predicted observed vector of the motion model m at each time step, the covariance matrix of the observed vector of the motion model m at each time step, and the Kalman gain of the motion model m at each time step. Based on the predicted value of the state vector of the motion model m at each time step, the error between the observed vector and the predicted observed vector of the motion model m at each time step, and the Kalman gain of the motion model m at each time step, the estimated value of the state vector of the motion model is calculated.

[0065] In some embodiments, the predicted values ​​of the state vectors of the motion model m at each time step, the predicted values ​​of the error covariance matrix of the state vectors of the motion model m at each time step, the error between the observed vectors and the predicted observed vectors of the motion model m at each time step, the covariance matrix of the observed vectors of the motion model m at each time step, the Kalman gain of the motion model m at each time step, and the expression for calculating the estimated values ​​of the state vectors of the motion model are as follows:

[0066]

[0067]

[0068] in, This represents the estimated state vector of model m at time k; K represents the predicted state vector of model m at time k; m (k) represents the Kalman gain of model m at time k; v m (k) represents the error between the observed vector of model m at time k and the predicted observed vector; F m X represents the state transition matrix of the m-th motion model; pre_ (k-1|k-1) represents the initial fusion state vector at time k-1; P m (k|k-1) represents the predicted value of the error covariance matrix of the state vector of motion model m at time k; P pre_ (k-1|k-1) represents the mixture covariance estimation matrix of model m at time k-1; Q m Z(k) ∈ R represents the covariance matrix of the state noise of model m; q H represents the q-dimensional observation vector at time k; mS represents the observation transition matrix of the m-th motion model; m (k) represents the covariance matrix of the observation vector of model m at time k; R represents the covariance matrix of the observation noise of model m; * T Indicates the transpose operation; * -1 This indicates the inverse operation.

[0069] Step 120-5: Based on the covariance matrix of the observation vector of the motion model m at each time moment and the error between the observation vector of the motion model m at each time moment and the predicted observation vector, the likelihood function of the observation vector at each time moment is obtained.

[0070] In some embodiments, the expression for the likelihood function Λ of Z(k) is:

[0071]

[0072] Among them Λ m (k) represents the likelihood function of model m at time k.

[0073] Step 120-6: Based on the predicted value of the error covariance matrix of the state vector of the motion model m at each time moment, the Kalman gain of the motion model m at each time moment, and the covariance matrix of the observation vector of the motion model m at each time moment, update the covariance matrix of the state vector of the motion model m at each time moment.

[0074] In some embodiments, the expression for updating the covariance matrix of the state vector of the motion model m at each time step is:

[0075]

[0076] Among them, P m (k|k) represents the covariance matrix of the state vector of model m at time k; P m (k|k-1) represents the predicted value of the error covariance matrix of the state vector of motion model m at time k; K m (k) represents the Kalman gain of model m at time k; S m (k) represents the covariance matrix of the observation vectors of model m at time k; K m (k) represents the Kalman gain of model m at time k; * T This indicates the transpose operation.

[0077] Step 120-7: Update the model probability of the motion model based on the Markov transition probability matrix and the likelihood function.

[0078] In some embodiments, the expression for updating the model probability of the motion model is:

[0079]

[0080] Where, μ m (k) represents the model probability of model m at time k; Λ m (k) represents the likelihood function of model m at time k; P im μ represents the transition probability from motion model i to motion model m; m = 1, 2, ..., n. μ represents the m-th motion model of a system with n motion states. i (k-1) represents the probability of model i at time k-1.

[0081] Step 130: Based on the estimated value of the state vector of the motion model and the model probability of the motion model, obtain the estimated value of the state vector.

[0082] In some embodiments, the expression for obtaining the estimated value of the state vector is:

[0083]

[0084] in, This represents the estimated value of the state vector at time k obtained after processing using the IMMBIKF method; μ represents the estimated state vector of motion model m at time k; m = 1, 2, ..., n, representing the m-th motion model of a system with n motion states; μ m (k) represents the model probability of motion model m at time k.

[0085] Step 140: Apply the background pulse Kalman filter algorithm to each motion model to obtain the estimated value of the state vector of the robot's motion model and the covariance matrix of the state vector of the motion model.

[0086] In some embodiments, obtaining the estimated value of the state vector of the robot's motion model and the covariance matrix of the state vector of the motion model includes:

[0087] Step 140-1: The observation noise of the motion model—mixed Gaussian noise—is processed by the expectation-maximization algorithm to obtain the background noise model probability and the impulse noise model probability; and the Markov transition probability matrix of the noise model is obtained by modeling the state vector of each motion model as a weighted sum of the state vectors of two identical motion models that are affected by the background noise and the impulse noise, respectively.

[0088] For example, the Expectation-Maximization (EM) algorithm is used to process the observation noise of the motion model—Gaussian mixture noise—resulting in two noise models: background noise covariance R0. 1 Background noise model probability β 1 Impulse noise covariance R 2 Impulse noise model probability β2 For the state vector of each motion model It can be modeled as the state vectors of two identical motion models, one affected by background noise and the other by impulse noise. The weighted sum. In some embodiments, the Markov transition probability matrix of the noise model is obtained. The expression is:

[0089]

[0090] Step 140-2: Initialize the noise model probability, the estimated value of the state vector corresponding to the noise model, and the prior covariance matrix of the state vector of the noise model.

[0091] In some embodiments, the expressions for the initial noise model probability, the estimated value of the state vector corresponding to the noise model, and the prior covariance matrix of the state vector of the noise model are:

[0092]

[0093] in, Let represent the probability of the i-th noise model at time k-1. Let xi represent the estimated value of the state vector corresponding to the i-th noise model at time k-1, and let x0 represent the initial state vector of the nonlinear system determined by formula (1). Let Pi represent the prior covariance matrix of the state vector of the i-th noise model at time k-1, and P0 represent the initial error matrix of the state vector.

[0094] Step 140-3: Based on the Markov transition probability matrix of the noise model and the noise model probability of the noise model, calculate the probability of transitioning from noise model i to noise model j.

[0095] In some embodiments, the probability of transitioning from noise model i to noise model j at time k-1 is calculated. The expression is:

[0096]

[0097] in, express The probability value in the i-th row and j-th column.

[0098] Step 140-4: Based on the estimated value of the state vector corresponding to the noise model and the probability from noise model i to noise model j, obtain the mixed state estimation vector of noise model j.

[0099] In some embodiments, the mixed state estimation vector of model j at time k-1 is obtained. The expression is:

[0100]

[0101] Step 140-5: Based on the estimated value of the state vector corresponding to the noise model, the prior covariance matrix of the state vector of the noise model, the probability from noise model i to noise model j, and the mixed state estimation vector of noise model j, the mixed covariance matrix of the state vector of noise model j is obtained.

[0102] In some embodiments, the mixture covariance matrix of the model j state vector at time k-1 is obtained. The expression is:

[0103]

[0104] Step 140-6: Based on the mixed state estimation vector of the noise model j and the mixed covariance matrix of the noise model j's state vector, obtain the predicted value of the noise model j's state vector, the predicted value of the error covariance matrix of the noise model j's state vector, the error of the noise model j's observation vector, the covariance matrix of the noise model j's observation vector, and the Kalman gain of the noise model j; and based on the predicted value of the noise model j's state vector, the Kalman gain of the noise model j, and the error of the noise model j's observation vector, obtain the estimated value of the noise model j's state vector.

[0105] In some embodiments, the expressions for the predicted value of the state vector of noise model j, the predicted value of the error covariance matrix of the state vector of noise model j, the error of the observation vector of noise model j, the covariance matrix of the observation vector of noise model j, and the Kalman gain of noise model j, as well as the expression for the estimated value of the state vector of noise model j, are as follows:

[0106]

[0107]

[0108] in, This represents the estimated value of the state vector of model j at time k; This represents the predicted value of the state vector of model j at time k; This represents the Kalman gain of model j at time k; A represents the error of the observation vector of model j at time k; k This represents the state transition matrix of motion model m at time k; This represents the mixed state estimation vector of model j at time k-1; This represents the predicted value of the error covariance matrix of the model j state vector at time k; Q represents the mixture covariance matrix of the model j-state vector at time k-1; k y represents the covariance matrix of the state noise of the motion model m at time k; k C represents the observed value of the state vector at time k; k Let represent the observation transition matrix of motion model m at time k; This represents the covariance matrix of the model's observation vectors at time k; This represents the covariance of the noise in model j at time k; * T Indicates the transpose operation; * -1 Indicates the inverse operation

[0109] Step 140-7: Based on the Kalman gain of the noise model j and the predicted value of the error covariance matrix of the state vector of the noise model j, obtain the posterior covariance matrix of the noise model j.

[0110] In some embodiments, the posterior covariance matrix of model j at time k is obtained. The expression is:

[0111]

[0112] Step 140-8: Based on the Markov transition probability matrix of the noise model, the noise model probability of the noise model, the covariance matrix of the observation vector of the noise model j, and the error of the observation vector of the noise model j, obtain the likelihood function of the noise model j.

[0113] In some embodiments, the likelihood function of model j at time k is obtained. The expression is:

[0114]

[0115] Step 140-9: Update the noise model probability of noise model j based on the likelihood function of the noise model j.

[0116] In some embodiments, the noise model probability of model j at time k is updated. The expression is:

[0117]

[0118] Step 140-10: Based on the noise model probability of the noise model j and the estimated value of the state vector of the noise model j, obtain the estimated value of the state vector of the motion model.

[0119] In some embodiments, the expression for obtaining the estimated value of the state vector of the motion model is:

[0120]

[0121] in, This represents the estimated state vector of motion model m at time k; This represents the probability of the noisy model of model j at time k; This represents the estimated value of the state vector of model j at time k.

[0122] Steps 140-11: Based on the estimated value of the state vector of the noise model j, the posterior covariance matrix of the noise model j, and the estimated value of the state vector of the motion model m, the covariance matrix of the state vector of the motion model is obtained.

[0123] In some embodiments, the expression for the covariance matrix of the state vector of the motion model is:

[0124]

[0125] Among them, P k| The covariance matrix represents the state vector of motion model m at time k; This represents the probability of being model j at time k; Let represent the posterior covariance matrix of model j at time k; This represents the estimated value of the state vector of model j at time k; This represents the estimated state vector of motion model m at time k; T This indicates the transpose operation.

[0126] Step 150: Based on the estimated value of the state vector of the motion model at the previous moment and the covariance matrix of the state vector of the motion model, iteratively solve to obtain the estimated value of the state vector of the motion model at the next moment and the covariance matrix of the state vector of the motion model, thereby realizing the localization of the robot.

[0127] For example, let P m (k|k)=P k|k , k-1 = k, repeat steps 110-120, and 130 to obtain the positioning data. P represents the value of the state vector of motion model m at time k; m (k|k) represents the covariance matrix of the state vector of motion model m at time k.

[0128] Figure 2 The actual state information X(k) of the robot and the estimated value of the state information obtained by the IMMKF and IMMBIKF methods. The root mean square error of displacement information in the X direction; Figure 3The actual state information X(k) of the robot and the estimated value of the state information obtained by the IMMKF and IMMBIKF methods. The root mean square error of displacement information in the Y direction.

[0129] based on Figure 2 and Figure 3 The effectiveness of the robot localization method based on background pulse Kalman filtering proposed in this invention is verified.

[0130] Model a linear dynamic system consisting of uniform linear motion (CV) and uniform circular motion (CM):

[0131]

[0132] Initial position and velocity information (initial state vector) X(0) = [1000, 10, 1000, -10] T When t∈(0,350], w1(k), r1(k); when t∈(350,650], w2(k), r2(k); when t∈(650,1000], w1(k), r1(k).

[0133] Where w1(k) ~ G1N(0,q1), w2(k) ~ G2N(0,q2);

[0134] r1(k)~0.9N(0,0.01 2 )+0.1N(0,100 2 ), r2(k)~0.9N(0,0.01 2 )+0.1N(0,100 2 );

[0135]

[0136] Where N(0,q1) represents a Gaussian distribution with mean 0 and covariance q1, and N(0,q2) represents a Gaussian distribution with mean 0 and covariance q2; 0.9N(0,0.01) 2 )+0.1N(0,100 2 ) indicates that 90% of the data follows N(0, 0.01). 2 The data and 10% conformity N(0,100) 2The data is a random mixture of Gaussian distributions; w1(k)~G1N(0,q1) and w2(k)~G2N(0,q2) represent weighted distributions of w1(k) and w2(k) with weights G1 and G2 respectively, and are Gaussian distributions with weights N(0,q1) and N(0,q2); r1(k)~0.9N(0,0.01) 2 )+0.1N(0,100 2 ), r2(k)~0.9N(0,0.01 2 )+0.1N(0,100 2 Let r1(k) and r2(k) respectively represent that r1(k) and r2(k) follow the condition 0.9N(0,0.01). 2 )+0.1N(0,100 2 The mixture of Gaussian distributions.

[0137] Take 1000 sample points of the state vector X(k), i.e., k = 0, 1, 2, ..., 1000, and run the simulation 100 times independently to obtain the simulation results as follows. Figure 2 and Figure 3 As shown.

[0138] Depend on Figure 2 and Figure 3 It can be known that:

[0139] The newly proposed IMMBIKF-based positioning method has a root mean square error (RMSE) that is much smaller than that of the traditional IMMBIKF method, resulting in a significant improvement in positioning accuracy. The theoretical values ​​of the algorithm match the simulation results.

[0140] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A robot localization method based on background pulse Kalman filtering, characterized in that, include: A linear dynamic system with uniform linear motion and uniform circular motion and a Markov transition probability matrix are constructed; the state vector of the linear dynamic system includes the robot's position and velocity information; the expression of the linear dynamic system is: ； in, express The p-dimensional state vector at time +1; Let m represent the state transition matrix of the m-th motion model; m = 1, 2, ..., n represents the m-th motion model of a system with n motion states. Represents a p-dimensional state vector at time k; This represents the state noise of the m-th motion model at time k; Represents the q-dimensional observation vector at time k; This represents the observation transition matrix of the m-th motion model; This represents the observation noise at time k for the m-th motion model; The expression for the Markov transition probability matrix is: ； Where P represents the Markov transition probability matrix; This represents the transition probability from motion model i to motion model m; Based on the linear dynamical system and the Markov transition probability matrix, the interactive multi-model Kalman filter algorithm is applied to obtain the estimated values ​​of the state vector of the motion model and the model probabilities of the motion model, including: Based on the Markov transition probability matrix, calculate the mixed normalized model probability from motion model i to motion model m; Based on the hybrid normalized model probability, the initial fusion state vector of the motion model m is calculated; Based on the hybrid normalized model probability and the initial fused state vector, calculate the hybrid covariance estimation matrix of the motion model m; Based on the linear dynamic system, the initial fused state vector, and the hybrid covariance estimation matrix, Kalman filtering is used to obtain the predicted values ​​of the state vector of motion model m at each time step, the predicted values ​​of the error covariance matrix of the state vector of motion model m at each time step, the error between the observed vector and the predicted observed vector of motion model m at each time step, the covariance matrix of the observed vector of motion model m at each time step, and the Kalman gain of motion model m at each time step. Based on the predicted values ​​of the state vector of motion model m at each time step, the error between the observed vector and the predicted observed vector of motion model m at each time step, and the Kalman gain of motion model m at each time step, the estimated values ​​of the state vector of the motion model are calculated. ； ； in, This represents the estimated state vector of model m at time k; This represents the predicted value of the state vector of model m at time k; This represents the Kalman gain of model m at time k; This represents the error between the observed vector of model m at time k and the predicted observed vector. This represents the state transition matrix of the m-th motion model; Indicates k The initial fusion state vector at time 1; This represents the predicted value of the error covariance matrix of the state vector of motion model m at time k; Indicates k The mixture covariance estimation matrix of model m at time 1; The covariance matrix represents the state noise of model m; Represents the q-dimensional observation vector at time k; This represents the observation transition matrix of the m-th motion model; R represents the covariance matrix of the observation vector of model m at time k; R represents the covariance matrix of the observation noise of model m. Indicates the transpose operation; This represents the inverse operation; Based on the covariance matrix of the observation vector of the motion model m at each time moment and the error between the observation vector of the motion model m at each time moment and the predicted observation vector, the likelihood function of the observation vector at each time moment is obtained. Based on the predicted value of the error covariance matrix of the state vector of the motion model m at each time step, the Kalman gain of the motion model m at each time step, and the covariance matrix of the observation vector of the motion model m at each time step, update the covariance matrix of the state vector of the motion model m at each time step: ； in, The covariance matrix represents the state vector of model m at time k; This represents the predicted value of the error covariance matrix of the state vector of motion model m at time k; This represents the Kalman gain of model m at time k; Let represent the covariance matrix of the observation vectors of model m at time k; This represents the Kalman gain of model m at time k; Indicates the transpose operation; The model probability of the motion model is updated based on the Markov transition probability matrix and the likelihood function. Based on the estimated value of the state vector of the motion model and the model probability of the motion model, the estimated value of the state vector is obtained: ； in, This indicates the result obtained after processing using the IMMBIKF method. The estimated value of the state vector at time step; express The estimated value of the state vector of the motion model at time m; m = 1, 2, ..., n represents the m-th motion model of a system with n motion states; This represents the model probability of motion model m at time k; Applying the background pulse Kalman filter algorithm to each motion model yields estimates of the robot's motion model's state vector and its covariance matrix, including: The observation noise of the motion model—Gaussian mixture noise—is processed by the expectation-maximization algorithm to obtain the background noise model probability and the impulse noise model probability; and the Markov transition probability matrix of the noise model is obtained by modeling the state vector of each motion model as a weighted sum of the state vectors of two identical motion models that are affected by background noise and impulse noise respectively. Initialize the noise model probability, the estimated value of the state vector corresponding to the noise model, and the prior covariance matrix of the state vector of the noise model; Based on the Markov transition probability matrix of the noise model and the noise model probability of the noise model, calculate the probability of transitioning from noise model i to noise model j; Based on the estimated value of the state vector corresponding to the noise model and the probability from noise model i to noise model j, the mixed state estimation vector of noise model j is obtained. Based on the estimated value of the state vector corresponding to the noise model, the prior covariance matrix of the state vector of the noise model, the probability from noise model i to noise model j, and the mixed state estimation vector of noise model j, the mixed covariance matrix of the state vector of noise model j is obtained. Based on the mixed state estimation vector of the noise model j and the mixed covariance matrix of the state vector of the noise model j, we obtain This represents the predicted value of the state vector of the noise model j at time k. This represents the predicted value of the error covariance matrix of the state vector of the noise model j at time k. The error of the observation vector of the noise model j at time k is represented by... The sum of the covariance matrices of the observation vectors of the noise model at time k. Let K represent the Kalman gain of the noise model j at time k; and based on the predicted value of the state vector of the noise model j, the Kalman gain of the noise model j, and the error of the observation vector of the noise model j, obtain the estimated value of the state vector of the noise model j. Based on the Kalman gain of the noise model j and the predicted value of the error covariance matrix of the state vector of the noise model j, the posterior covariance matrix of the noise model j is obtained. Based on the Markov transition probability matrix of the noise model, the noise model probability, the covariance matrix of the observation vector of the noise model j, and the error of the observation vector of the noise model j, the likelihood function of the noise model j is obtained. Based on the noise model The likelihood function is used to update the noise model. The probability of the noise model; Based on the noise model probability of the noise model j and the estimated value of the state vector of the noise model j, the estimated value of the state vector of the motion model is obtained. Based on the estimated value of the state vector of the noise model j, the posterior covariance matrix of the noise model j, and the estimated value of the state vector of the motion model m, the covariance matrix of the state vector of the motion model is obtained. Based on the estimated state vector of the motion model at the previous moment and the covariance matrix of the state vector of the motion model, the estimated state vector of the motion model at the next moment and the covariance matrix of the state vector of the motion model are obtained by iterative solution, thereby realizing the localization of the robot.

2. The robot localization method based on background pulse Kalman filtering according to claim 1, characterized in that, The expression for updating the model probability of the motion model is: ； in, This represents the model probability of model m at time k; Let m be the likelihood function of model m at time k; Let represent the transition probability from motion model i to motion model m; m = 1, 2, ..., n represents the m-th motion model of a system with n motion states; Indicates k The probability of model i at time 1.

3. The robot localization method based on background pulse Kalman filtering according to claim 1, characterized in that, The expressions for the predicted value of the state vector of the noise model j, the predicted value of the error covariance matrix of the state vector of the noise model j, the error of the observation vector of the noise model j, the covariance matrix of the observation vector of the noise model j, and the Kalman gain of the noise model j, as well as the expression for the estimated value of the state vector of the noise model j, are as follows: ； in, This represents the estimated value of the state vector of model j at time k; This represents the predicted value of the state vector of model j at time k; This represents the Kalman gain of model j at time k; This represents the error of the observation vector of model j at time k; This represents the state transition matrix of motion model m at time k; Indicates k The mixed state estimation vector of model j at time 1; This represents the predicted value of the error covariance matrix of the model j state vector at time k; Indicates k The mixture covariance matrix of the state vector of model j at time 1; Let represent the covariance matrix of the state noise of the motion model m at time k; This represents the observed value of the state vector at time k; Let represent the observation transition matrix of motion model m at time k; This represents the covariance matrix of the model's observation vectors at time k; Let represent the covariance of the noise in model j at time k; Indicates the transpose operation; This indicates the inverse operation.

4. The robot localization method based on background pulse Kalman filtering according to claim 1, characterized in that, The expression for obtaining the estimated value of the state vector of the motion model is: ； in, This represents the estimated state vector of motion model m at time k; This represents the probability of the noisy model of model j at time k; This represents the estimated value of the state vector of model j at time k; The expression for the covariance matrix of the state vector of the motion model is as follows: ； in, The covariance matrix represents the state vector of motion model m at time k; This represents the probability of being model j at time k; Let represent the posterior covariance matrix of model j at time k; This represents the estimated value of the state vector of model j at time k; This represents the estimated state vector of motion model m at time k; This indicates the transpose operation.

Images