Dynamic Target Tracking Method Based on Cascade Robust Optimal Hybrid Filtering

Abstract

A dynamic target tracking method based on cascaded robust optimal hybrid filtering, comprising: establishing a continuous kinematic model for the motion of a mobile robot in a warehouse; considering the hybrid disturbances in the complex environment of the warehouse and the inaccuracy of the model, establishing a system state equation and an observation equation of the sensor; designing a corresponding cascaded robust optimal hybrid filter according to the observed values of the sensor; giving an autonomous error model of the system, designing and solving the Riccati equation through an iterative algorithm to obtain the gain of each filter; substituting the gain of the above filter to obtain a real-time estimated value, and realizing real-time position tracking of the mobile robot. The present invention can perform real-time high-precision estimation of the position of the mobile robot.

Description

Technical Field

[0001] The present invention relates to the technical field of dynamic target tracking, and specifically relates to a dynamic target tracking method based on cascaded robust optimal hybrid filtering. Background Art

[0002] Target tracking refers to obtaining observation information related to a dynamic target in the environment through one or more sensors, and then determining the state information of the target through a certain estimation method. These observation information may come from the dynamic target itself, the relevant environmental background or system noise, etc. Using the information in all observations to estimate the state of the dynamic target (such as position, speed, etc.) is the target tracking filtering method. Target tracking methods can be widely applied to fields related to navigation, transportation, and military. In a warehouse, the emergence of intelligent robots has liberated a large amount of labor and production costs. The intelligent robots move along a trajectory and need to be tracked. However, due to the occlusion of some equipment in the warehouse, insufficient signal accuracy may result in the inability to receive enough signals for tracking.

[0003] In the target tracking filtering method, the commonly used one is the Kalman filtering algorithm. Since the Kalman filter can process estimates in real time, the Kalman filter is widely used in dynamic data processing. At the same time, the Kalman filter is an optimal linear filter based on the principle of minimum mean square error, so it has high accuracy. The Kalman filtering algorithm has very high requirements for the accuracy of the model. However, when tracking a target, the established model is often inaccurate, and there will be some external uncertainties and interferences, which will all lead to a decline in the estimation effect of the Kalman filtering algorithm. Summary of the Invention

[0004] To solve the above problems, the present invention provides a target tracking method based on cascaded robust optimal hybrid filtering.

[0005] The working principle of the present invention is as follows: Assume that there is a mobile robot in the warehouse. First, establish a continuous kinematic model for the mobile robot to simulate the actual movement situation; then consider the inaccuracy and interference of the mobile robot model as a hybrid disturbance signal composed of random disturbance and uncertainty disturbance; further use sensors to observe the mobile robot and perform data fusion processing by adopting cascaded robust optimal hybrid filtering, which improves the estimation accuracy on the premise of ensuring the robustness and anti-interference ability of the system.

[0006] The target tracking estimation method based on cascaded robust optimal hybrid filtering specifically includes the following steps:

[0007] 1) Establish a continuous kinematic model for the movement of the mobile robot in the warehouse;

[0008] 2) Consider the hybrid disturbances in the complex warehouse environment and the inaccuracy of the model, and establish the system state equation and the sensor observation equation;

[0009] 3) Design the corresponding cascaded robust optimal hybrid filter according to the sensor observations;

[0010] 4) Give the autonomous error model of the system, design and solve the Riccati equation through an iterative algorithm to obtain the gain of each filter;

[0011] 5) Substitute the gain of the above filter to obtain the real-time estimation value and achieve real-time position tracking of the mobile robot.

[0012] Furthermore, in step 1), establish a continuous kinematic model for the motion of the mobile robot in the warehouse. We establish the ground environment in the warehouse as a plane rectangular coordinate system, and then use to represent the position of the mobile robot, where P x represents the abscissa of the mobile robot, and P y represents the ordinate of the mobile robot.

[0013] Furthermore, in step 2), consider the hybrid disturbances in the complex warehouse environment and the inaccuracy of the model, and establish the system state equation and the sensor observation equation. Considering the hybrid disturbances and model inaccuracy of the ground environment in the warehouse, establishing the system state equation and the sensor observation equation includes the following steps:

[0014] (2.1) Establish the state equation of the system. The state equation of the system is:

[0015] x(k + 1) = Ax(k) + B 1 ω 1 (k) + B 0 ω 0 (k) (1)

[0016] where k represents the current discretization time, k + 1 represents the next discretization time, x represents the position of the mobile robot, x = [P x P y T , P x represents the abscissa of the mobile robot, P y represents the ordinate of the mobile robot, the superscript "T" represents the transpose of the matrix, A represents the state transition matrix of x, ω 1 represents the uncertainty disturbance signal, B 1 represents the input matrix of the uncertainty disturbance signal ω 1 , ω 0 represents white noise with a mean of 0 and a variance of 1, and B 0 represents white noise ω0 Input matrix

[0017] (2.2) Establish the observation equation of the sensor. The observation equation of the sensor is:

[0018] z(k) = Hx(k) + D 1 ω 1 (k) + D 0 ω 0 (k) (2)

[0019] where k represents the current discretization time, z represents the observation vector of the sensor, x represents the position of the mobile robot, x = [P x P y T , P x represents the abscissa of the mobile robot, P y represents the ordinate of the mobile robot, H represents the observation matrix of the sensor, ω 1 represents the uncertainty disturbance signal, D 1 represents the observation matrix of the uncertainty disturbance signal of the sensor, ω 0 represents white noise with a mean of 0 and a variance of 1, D 0 represents the observation matrix of the white noise ω 0 of the sensor.

[0020] Furthermore, in step 3), design the corresponding cascaded robust optimal hybrid filter according to the observation value of the sensor. When tracking the mobile robot in the warehouse, the data transmission is carried out in the wireless sensor network. For the sensor, it can receive the observation value through the wireless sensor network, but it is often disturbed during the transmission process, resulting in data loss. Assume that the actual observation value received by the sensor is denoted as:

[0021] s(k) = Φ(k)z(k) (3)

[0022] where k represents the discretization time, s represents all the data actually received by the sensor, Φ represents whether the data of the observation value z of the sensor is lost, and z represents the observation vector of the sensor.

[0023] Design the cascaded robust optimal filters F and Q of the sensor:

[0024]

[0025]

[0026] where k represents the current discretization time, k + 1 represents the next discretization time. A represents the state transition matrix of x, represents the estimated value of x, e z ​Indicates the difference between x and the corresponding estimated value , Indicates the estimated value of the estimation object e z , L 0 Indicates the F filter gain of the sensor, L 1 Indicates the Q filter gain of the sensor. Φ indicates whether the data of the observed value z of the sensor is lost, z indicates the observation vector of the sensor, H indicates the observation matrix of the sensor, and s(k) indicates all the data actually received by the sensor.

[0027] The role of the cascaded robust optimal filters F and Q is to make the position estimated value of the sensor for the mobile robot as close as possible to the actual position x(k) of the mobile robot, realizing real-time high-precision estimation of the position of the mobile robot.

[0028] Furthermore, in step 4), an autonomous error model of the system is given, and the Riccati equation is designed and solved by an iterative algorithm to obtain the gain of each filter. An autonomous error model of the system is given, and the gains L 0 and L 1 of the F and Q filters are designed and solved by an iterative algorithm, specifically including the following steps:

[0029] (4.1) Give the autonomous error model of the system. The autonomous error model of the system is obtained through equations (1), (2), (4), and (5) respectively:

[0030] e z (k + 1) = (A + L 0 Φ(k)H)e z (k) + [B 0 + L 0 Φ(k)D 0 ω 0 (k) (6)

[0031]

[0032] where k represents the current discrete time, and k + 1 represents the next discrete time., e z Indicates the difference between x and the corresponding estimated value , Indicates the difference between e z and the corresponding estimated value , A represents the state transition matrix of x, B 0 represents the input matrix of white noise ω 0 , H represents the observation matrix of the sensor, D 0 represents the observation matrix of white noise ω 0 of the sensor, ω 1 represents the uncertainty perturbation signal, ω0 denotes white noise with a mean of 0 and a variance of 1, Φ represents whether the data of the observed value z of the sensor is lost, z represents the observation vector of the sensor, and are both intermediate matrices related to L 0 and L 1 respectively, L 0 represents the F filter gain of the sensor, L 1 represents the Q filter gain of the sensor.

[0033] (4.2) gives the expression of the F filter gain L 0 . Based on the autonomous error model of the system in Equation (6), the expression of the F filter gain L 0 is obtained through the Kalman filter algorithm:

[0034]

[0035] where k represents the current discretization time, A represents the state transition matrix of x, the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix, B 0 represents the input matrix of the white noise ω 0 , H represents the observation matrix of the sensor, D 0 represents the observation matrix of the white noise ω 0 of the sensor, and O is an intermediate matrix.

[0036] (4.3) gives the expression of the Q filter gain L 1 . Based on the autonomous error model of the system in Equation (7), the expression of the Q filter gain L ∞ is obtained through the H 1 filter algorithm:

[0037] L 1 =-uw -1 (9)

[0038] where both u and w are intermediate matrices containing M, and M is an intermediate matrix.

[0039] (4.4) gives the initial values of the intermediate matrices O and M. The intermediate matrices O and M respectively represent the covariance forms of the autonomous error models (6) and (7) of the above system. When k = 0, the initial values are assigned to the intermediate matrices O and M, that is

[0040] O(0), M(0) (10)

[0041] (4.5) gives the Riccati equation of the intermediate matrix O and obtains the intermediate matrix O(1). The intermediate matrix O satisfies the following Riccati equation:

[0042]

[0043] Therefore, the intermediate matrix O(1) is obtained:

[0044]

[0045] where k represents the current discretization time, k + 1 represents the next discretization time, A represents the state transition matrix of x, the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix, B 0 represents the input matrix of the white noise ω 0 H represents the observation matrix of the sensor, D 0 represents the observation matrix of the white noise ω 0 of the sensor, μ represents the mathematical expectation of Φ, Φ represents whether the data of the observed value z of the sensor is lost, z represents the observation vector of the sensor, and O is the intermediate matrix.

[0046] (4.6) gives the Riccati equation of the intermediate matrix M, and the intermediate matrix M(1) is obtained. The intermediate matrix M satisfies the following Riccati equation:

[0047]

[0048] Therefore, the intermediate matrix M(1) is obtained:

[0049]

[0050] where k represents the current discretization time, k + 1 represents the next discretization time, γ represents the preset H ∞ parameter, the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix, the superscript "2" represents the square of the parameter, I represents the identity matrix of a certain dimension, L 0 represents the F filter gain of the sensor, H represents the observation matrix of the sensor, μ represents the mathematical expectation of Φ, Φ represents whether the data of the observed value z of the sensor is lost, z represents the observation vector of the sensor, D 1 represents the observation matrix of the uncertainty perturbation signal of the sensor, B 1 represents the input matrix of the uncertainty perturbation signal ω 1 of the uncertainty perturbation signal ω, and M are both intermediate matrices, and u and w are both intermediate matrices containing M.

[0051] (4.7) Under the condition of satisfying the given error, the intermediate matrix O and the intermediate matrix M are iteratively solved. Repeat steps (4.5)(4.6).

[0052] If \(k = T\), and the two - norm of the difference between matrix \(O(T)\) and matrix \(O(T - 1)\) is less than the given error, we get:

[0053] \(O = O(T)=O(T - 1)\quad(15)\)

[0054] Similarly, if \(k = T\), and the two - norm of the difference between matrix \(M(T)\) and matrix \(M(T - 1)\) is less than the given error, we get:

[0055] \(M = M(T)=M(T - 1)\quad(16)\)

[0056] where \(O\) and \(M\) are both intermediate matrices.

[0057] Substitute the intermediate matrices \(O\) and \(M\) into (4.8) to solve for the gain matrices \(L\) 0 and \(L\) 1 of the filters \(F\) and \(Q\). Substitute the intermediate matrices \(O\) and \(M\) into equations (8) and (9) respectively to obtain the gain matrices \(L\) 0 and \(L\) 1 of the cascaded robust optimal filters \(F\) and \(Q\).

[0058] Furthermore, in step 5), substitute the gains of the above - mentioned filters to obtain the real - time estimated value, and achieve real - time position tracking of the mobile robot. Substitute the gain matrices \(L\) 0 and \(L\) 1 obtained in step (4.8) into the cascaded robust optimal filters \(F\) in equation (4) and \(Q\) in equation (5) to obtain the real - time estimated value of the position of the mobile robot, and achieve the tracking of the mobile robot.

[0059] An object - tracking algorithm based on cascaded robust optimal hybrid filtering designed by the present invention solves two sets of uncoupled Riccati equations through an iterative algorithm, and then solves for the gains of each filter, and constructs multiple filters to achieve real - time high - precision estimation of the position of a mobile robot under white noise and uncertain interference signals.

[0060] The advantages of the present invention are as follows: considering the influence of the actual complex environment, establishing the system state equation and the observation equation for the inaccurate mobile robot model, further constructing a cascaded robust optimal hybrid filter, and achieving real - time high - precision estimation of the position of the mobile robot on the premise of ensuring the robustness and anti - interference ability of the system. The estimation results can meet the accuracy and real - time requirements of practical applications, and the algorithm can fit well regardless of the target used, meeting the requirements of target tracking. Brief Description of the Drawings

[0061] Figure 1 is the experimental tracking effect diagram of the present invention.

[0062] Figure 2It is the experimental tracking error effect diagram of the present invention. Detailed implementation manners

[0063] To make the objectives, technical solutions and specific effects of the present invention clearer, the technical solutions of the present invention will be further described below in combination with actual experimental data.

[0064] The present invention provides a target tracking method based on cascaded robust optimal hybrid filtering. Its working principle is as follows: Assume that there is a mobile robot in a warehouse. First, a continuous kinematic model of the mobile robot is established to simulate its actual movement; then, the inaccuracy and interference of the mobile robot model are considered as a hybrid disturbance signal composed of random disturbance and uncertainty disturbance; further, the mobile robot is observed by sensors, and cascaded robust optimal hybrid filtering is adopted for data fusion processing, which improves the estimation accuracy on the premise of ensuring the robustness and anti-interference of the system.

[0065] The target tracking estimation method based on cascaded robust optimal hybrid filtering specifically includes the following steps:

[0066] 1) Establish a continuous kinematic model for the movement of the mobile robot in the warehouse;

[0067] 2) Considering the hybrid disturbance and model inaccuracy in the complex environment of the warehouse, establish the system state equation and the observation equation of the sensor;

[0068] 3) Design the corresponding cascaded robust optimal hybrid filter according to the observation value of the sensor;

[0069] 4) Give the autonomous error model of the system, design and solve the Riccati equation through an iterative algorithm to obtain the gain of each filter;

[0070] 5) Substitute the gain of the above filter to obtain the real-time estimation value and realize the real-time position tracking of the mobile robot.

[0071] Further, in step 1), a continuous kinematic model for the movement of the mobile robot in the warehouse is established. We establish the ground environment in the warehouse as a plane rectangular coordinate system, and then use to represent the position of the mobile robot, where P x represents the abscissa of the mobile robot, and P y represents the ordinate of the mobile robot.

[0072] Further, in step 2), considering the hybrid disturbance and model inaccuracy in the complex environment of the warehouse, establish the system state equation and the observation equation of the sensor. Considering the hybrid disturbance and model inaccuracy of the ground environment in the warehouse, establishing the system state equation and the observation equation of the sensor includes the following steps:

[0073] (2.1) Establish the state equation of the system. The state equation of the system is:

[0074] x(k + 1) = Ax(k) + B 1 ω 1 (k) + B 0 ω 0 (k) (1)

[0075] where k represents the current discretization time, k + 1 represents the next discretization time, x represents the position of the mobile robot, x = [P x P y T , P x represents the abscissa of the mobile robot, P y represents the ordinate of the mobile robot, the superscript "T" represents the transpose of the matrix, represents the state transition matrix of x, the uncertainty disturbance signal ω 1 is simulated by ω 1 (k) = |0.35 * sin(0.5 * k)|, represents the input matrix of the uncertainty disturbance signal ω 1 , ω 0 represents white noise with a mean of 0 and a variance of 1, represents the input matrix of white noise ω 0 .

[0076] (2.2) Establish the observation equation of the sensor. The observation equation of the sensor is:

[0077] z(k) = Hx(k) + D 1 ω 1 (k) + D 0 ω 0 (k) (2)

[0078] where k represents the current discretization time, z represents the observation vector of the sensor, x represents the position of the mobile robot, x = [P x P y T , P x represents the abscissa of the mobile robot, P y represents the ordinate of the mobile robot, H = [0.2 0.5] represents the observation matrix of the sensor, the uncertainty disturbance signal ω 1 is simulated by ω 1 (k) = |0.35 * sin(0.5 * k)|, D 1 = 2 represents the observation matrix of the uncertainty disturbance signal of the sensor, ω 0 represents white noise with a mean of 0 and a variance of 1, D 0 ​​= [1.2 1.5] represents the white noise ω of the sensor 0 of the observation matrix

[0079] Furthermore, in step 3), according to the observed values of the sensor, its corresponding cascaded robust optimal hybrid filter is designed. When tracking a mobile robot in a warehouse, the data transmission is carried out in a wireless sensor network. For the sensor, it can receive the observed values through the wireless sensor network, but during the transmission process, it is often disturbed, resulting in data loss. Assume the actually received observed value of the sensor is denoted as:

[0080] s(k) = Φ(k)z(k) (3)

[0081] where k represents the discretization time, s represents all the data actually received by the sensor, Φ represents whether the data of the observed value z of the sensor is lost, and z represents the observation vector of the sensor

[0082] Design the cascaded robust optimal filters F and Q of the sensor:

[0083]

[0084]

[0085] where k represents the current discretization time and k + 1 represents the next discretization time represents the state transition matrix of x, H = [0.2 0.5] represents the observation matrix of the sensor represents the estimated value of x, e z represents the difference between x and the corresponding estimated value represents the estimated value of the estimated object e z 0 represents the F filter gain of the sensor, L 1 represents the Q filter gain of the sensor, Φ represents whether the data of the observed value z of the sensor is lost, z represents the observation vector of the sensor, and s(k) represents all the data actually received by the sensor

[0086] The roles of the cascaded robust optimal filters F and Q are to make the estimated value of the position of the mobile robot by the sensor as close as possible to the actual position x(k) of the mobile robot, realizing real-time high-precision estimation of the position of the mobile robot

[0087] Furthermore, in step 4), give the autonomous error model of the system, design and solve the Riccati equation through an iterative algorithm to obtain the gain of each filter. Give the autonomous error model of the system, design and solve the gains L 0 and L​​1 , specifically including the following steps:

[0088] (4.1) Give the autonomous error model of the system. The autonomous error model of the system is obtained through Equations (1), (2), (4), and (5) respectively:

[0089] e z (k + 1) = (A + L 0 Φ(k)H)e z (k) + [B 0 + L 0 Φ(k)D 0 ω 0 (k) (6)

[0090]

[0091] where k represents the current discrete time, and k + 1 represents the next discrete time., e z represents the difference between x and the corresponding estimated value , represents the difference between e z and the corresponding estimated value , represents the state transition matrix of x, represents the input matrix of the uncertainty perturbation signal ω 1 , and the uncertainty perturbation signal ω 1 is simulated by ω 1 (k) = |0.35 * sin(0.5 * k)|, represents the input matrix of the white noise ω 0 , ω 0 represents white noise with a mean of 0 and a variance of 1, H = [0.2 0.5] represents the observation matrix of the sensor, D 0 = [1.2 1.5] represents the observation matrix of the white noise ω 0 of the sensor, Φ represents whether the data of the observed value z of the sensor is lost, z represents the observation vector of the sensor, and are both intermediate matrices related to L 0 and L 1 , L 0 represents the F filter gain of the sensor, and L 1 represents the Q filter gain of the sensor.

[0092] (4.2) Give the expression of the F filter gain L 0 . Based on the autonomous error model (6) of the system, the expression of the F filter gain L 0 is obtained through the Kalman filter algorithm:

[0093]

[0094] Among them, \(k\) represents the current discretization time, represents the state transition matrix of \(x\), the superscript "-1" represents the inverse of the matrix, and the superscript "T" represents the transpose of the matrix, represents the white noise \(\omega\) 0 input matrix of, \(H = [0.2\ 0.5]\) represents the observation matrix of the sensor, \(D\) 0 = [1.2\ 1.5] represents the observation matrix of the white noise \(\omega\) 0 of the sensor, and \(O\) is an intermediate matrix.

[0095] (4.3) gives the expression of the Q filter gain \(L\) 1 Based on the autonomous error model (7) of the system, through \(H\) ∞ filtering algorithm, the expression of the Q filter gain \(L\) 1 is obtained:

[0096] \(L\) 1 = -uw -1 (9)

[0097] Among them, both \(u\) and \(w\) are intermediate matrices containing \(M\), and \(M\) is an intermediate matrix.

[0098] (4.4) gives the initial values of the intermediate matrices \(O\) and \(M\). The intermediate matrices \(O\) and \(M\) respectively represent the covariance forms of the autonomous error models (6) and (7) of the above system. When \(k = 0\), the initial values are assigned to the intermediate matrices \(O\) and \(M\), that is

[0099] \(O(0), M(0)\) (10)

[0100] (4.5) gives the Riccati equation of the intermediate matrix \(O\) and obtains the intermediate matrix \(O(1)\). The intermediate matrix \(O\) satisfies the following Riccati equation:

[0101]

[0102] Therefore, the intermediate matrix \(O(1)\) is obtained:

[0103]

[0104] Among them, \(k\) represents the current discretization time, \(k + 1\) represents the next discretization time, represents the state transition matrix of \(x\), the superscript "-1" represents the inverse of the matrix, and the superscript "T" represents the transpose of the matrix, represents the white noise \(\omega\) 0 input matrix of, \(H = [0.2\ 0.5]\) represents the observation matrix of the sensor, \(D\) 0 = [1.2\ 1.5] represents the observation matrix of the white noise \(\omega\)0 The observation matrix, where μ = 0.8 represents the mathematical expectation of Φ, Φ represents whether the data of the sensor observation value z is lost, z represents the sensor observation vector, and O is an intermediate matrix.

[0105] (4.6) gives the Riccati equation of the intermediate matrix M to obtain the intermediate matrix M(1). The intermediate matrix M satisfies the following Riccati equation:

[0106]

[0107] Therefore, the intermediate matrix M(1) is obtained:

[0108]

[0109] where k represents the current discretization time, k + 1 represents the next discretization time, γ = 3.5 represents the preset H ∞ parameter, the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix, the superscript "2" represents the square of the parameter, I represents the identity matrix of a certain dimension, and L 0 represents the F filter gain of the sensor, H = [0.2 0.5] represents the sensor observation matrix, μ = 0.8 represents the mathematical expectation of Φ, Φ represents whether the data of the sensor observation value z is lost, z represents the sensor observation vector, and D 1 = 2 represents the observation matrix of the sensor's uncertainty disturbance signal, represents the input matrix of the uncertainty disturbance signal ω 1 Both and M are intermediate matrices, and both u and w are intermediate matrices containing M.

[0110] (4.7) Under the condition of meeting the given error, iteratively solve the intermediate matrix O and the intermediate matrix M. Repeat steps (4.5)(4.6).

[0111] If at k = T, the two-norm of the difference between the matrix O(T) and the matrix O(T - 1) is less than the given error, we get:

[0112]

[0113] Similarly, if at k = T, the two-norm of the difference between the matrix M(T) and the matrix M(T - 1) is less than the given error, we get:

[0114]

[0115] where both O and M are intermediate matrices.

[0116] (4.8) Substitute the intermediate matrix O and the intermediate matrix M to solve for the gain matrices L of the filters F and Q​0 and L 1 Substitute the intermediate matrices O and M into equations (8) and (9) respectively to obtain the gain matrices L of the cascaded robust optimal filters F and Q 0 and L 1 .

[0117] Furthermore, in step 5), substitute the gains of the above filters to obtain the real-time estimated value, and realize the real-time position tracking of the mobile robot. Substitute the gain matrix L 0 and L 1 into the cascaded robust optimal filters F in equation (4) and Q in equation (5) to obtain the real-time estimated value of the position of the mobile robot and realize the tracking of the mobile robot.

[0118] The content described in the embodiments of this specification is only a list of the implementation forms of the inventive concept. The protection scope of the present invention should not be regarded as limited to the specific forms stated in the embodiments. The protection scope of the present invention also extends to equivalent technical means that can be conceived by those skilled in the art based on the inventive concept of the present invention.

Claims

1. Dynamic target tracking and estimation method based on cascaded robust optimal hybrid filtering, specific steps include: 1). Establish a continuous kinematic model for the motion of mobile robots in the warehouse; Establish the ground environment in the warehouse as a two-dimensional rectangular coordinate system, and then use to represent the position of the mobile robot, where P x represents the abscissa of the mobile robot, and P y represents the ordinate of the mobile robot, so as to obtain the position of the mobile robot at each moment; 2). Considering the hybrid disturbances in the complex warehouse environment and the inaccuracy of the model, establish the system state equation and the observation equation of the sensor; Considering the hybrid disturbances on the ground environment in the warehouse and the inaccuracy of the model, establishing the system state equation and the observation equation of the sensor includes the following steps: (2.1) Establish the state equation of the system; The state equation of the system is: x(k + 1) = Ax(k) + B 1 ω 1 (k) + B 0 ω 0 (k) (1) where k represents the current discretization time, k + 1 represents the next discretization time, x represents the position of the mobile robot, x = [P x P y T , P x represents the abscissa of the mobile robot, P y represents the ordinate of the mobile robot, the superscript "T" represents the transpose of the matrix, A represents the state transition matrix of x, ω 1 represents the uncertainty disturbance signal, B 1 represents the input matrix of the uncertainty disturbance signal ω 1 , ω 0 represents white noise with a mean of 0 and a variance of 1, B 0 represents the input matrix of white noise ω 0 ;​ (2.2) Establish the observation equation of the sensor; The observation equation of the sensor is: z(k) = Hx(k) + D 1 ω 1 (k) + D 0 ω 0 (k) (2) Among them, k represents the current discretization moment, z represents the observation vector of the sensor, x represents the position of the mobile robot, and x = [P x P y T , P x represents the abscissa of the mobile robot, and P y represents the ordinate of the mobile robot. H represents the observation matrix of the sensor, ω 1 represents the uncertainty perturbation signal, D 1 represents the observation matrix of the uncertainty perturbation signal of the sensor, ω 0 represents white noise with a mean of 0 and a variance of 1, and D 0 represents the observation matrix of the white noise ω 0 of the sensor;​ 3). Design its corresponding cascaded robust optimal hybrid filter according to the observed values of the sensor; When tracking the mobile robot in the warehouse, the data transmission is carried out in the wireless sensor network. For the sensor, it can receive the observed values through the wireless sensor network, but the data is often disturbed during the transmission process, resulting in data loss; Assume the actually received observed value of the sensor, denoted as: s(k) = Φ(k)z(k) (3) where, k represents the discretization time, s represents all the data actually received by the sensor, Φ represents whether the data of the observed value z of the sensor is lost, and z represents the observation vector of the sensor; Design the cascaded robust optimal filters F and Q of the sensor: where k represents the current discretization time, and k + 1 represents the next discretization time; A represents the state transition matrix of x, represents the estimated value of x, e z represents the difference between x and the corresponding estimated value of, represents the estimated value of the estimated object e z of, L 0 represents the F filter gain of the sensor, L 1 represents the Q filter gain of the sensor, Φ represents whether the data of the observed value z of the sensor is lost, z represents the observation vector of the sensor, H represents the observation matrix of the sensor, and s(k) represents all the data actually received by the sensor; The functions of the cascaded robust optimal filters F and Q are to make the position estimation value of the sensor for the mobile robot as close as possible to the actual position x(k) of the mobile robot, achieving real-time high-precision estimation of the position of the mobile robot; 4) Give the autonomous error model of the system, design and solve the Riccati equation through an iterative algorithm to obtain the gain of each filter; give the autonomous error model of the system, design and solve the gains L of the F and Q filters through an iterative algorithm 0 and L 1 , which specifically includes the following steps: (4.1) Give the autonomous error model of the system; Obtain the autonomous error model of the system through equations (1), (2), (4) and (5) respectively: e z (k + 1) = (A + L 0 Φ(k)H)e z (k) + [B 0 + L 0 Φ(k)D 0 ω 0 (k)(6) Among them, k represents the current discrete time, and k + 1 represents the next discrete time; e z represents the difference between x and the corresponding estimated value . represents the difference between e z and the corresponding estimated value . A represents the state transition matrix of x, B 0 represents the input matrix of white noise ω 0 . H represents the observation matrix of the sensor, D 0 represents the observation matrix of the white noise ω 0 of the sensor, ω 1 represents the uncertainty perturbation signal, ω 0 represents white noise with a mean of 0 and a variance of 1. Φ represents whether the data of the observed value z of the sensor is lost, z represents the observation vector of the sensor, and are both intermediate matrices related to L 0 and L 1 . L 0 represents the F filter gain of the sensor, L 1 represents the Q filter gain of the sensor; (4.2) gives the expression of the F filter gain L 0 ; based on the autonomous error model of the system in Equation (6), the expression of the F filter gain L 0 is obtained through the Kalman filtering algorithm: where k represents the current discretization time, A represents the state transition matrix of x, the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix, and B 0 represents the input matrix of white noise ω 0 , H represents the observation matrix of the sensor, D 0 represents the observation matrix of the white noise ω 0 of the sensor, and O is the intermediate matrix; (4.3) gives the expression of the Q filter gain L 1 ; based on the autonomous error model of the system in Equation (7), the expression of the Q filter gain L ∞ is obtained through the H 1 filtering algorithm: L 1 = -uw -1 (9) where, both u and w are intermediate matrices containing M, and M is the intermediate matrix; (4.4) Give the initial values of the intermediate matrices O and M; The intermediate matrices O and M respectively represent the covariance forms of the autonomous error models (6) and (7) of the above system; When k = 0, give the initial values to the intermediate matrices O and M, that is O(0), M(0) (10) (4.5) Give the Riccati equation of the intermediate matrix O to obtain the intermediate matrix O(1); The intermediate matrix O satisfies the following Riccati equation: Therefore, obtain the intermediate matrix O(1): Among them, \(k\) represents the current discretization moment, \(k + 1\) represents the next discretization moment, \(A\) represents the state transition matrix of \(x\), the superscript \(-1\) represents the inverse of the matrix, and the superscript \(T\) represents the transpose of the matrix, \(B\) 0 represents the white noise \(\omega\) 0 input matrix, \(H\) represents the observation matrix of the sensor, \(D\) 0 represents the white noise \(\omega\) 0 observation matrix of, \(\mu\) represents the mathematical expectation of \(\varPhi\), \(\varPhi\) represents whether the data of the observed value \(z\) of the sensor is lost, \(z\) represents the observation vector of the sensor, and \(O\) is the intermediate matrix; (4.6) Give the Riccati equation of the intermediate matrix M to obtain the intermediate matrix M(1); The intermediate matrix M satisfies the following Riccati equation: Therefore, obtain the intermediate matrix M(1): Among them, k represents the current discretization moment, k + 1 represents the next discretization moment, γ represents the preset H ∞ parameter, the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix, the superscript "2" represents the square of the parameter, I represents the identity matrix of a certain dimension, L 0 represents the F filter gain of the sensor, H represents the observation matrix of the sensor, μ represents the mathematical expectation of Φ, Φ represents whether the data of the observation value z of the sensor is lost, z represents the observation vector of the sensor, D 1 represents the observation matrix of the uncertainty disturbance signal of the sensor, B 1 represents the input matrix of the uncertainty disturbance signal ω 1 of, and M are both intermediate matrices, and both u and w are intermediate matrices containing M; (4.7) Under the condition of meeting the given error, iteratively solve the intermediate matrix O and the intermediate matrix M; Repeat steps (4.5)(4.6); If at k = T time, the two-norm of the difference between the matrix O(T) and the matrix O(T - 1) is less than the given error, obtain: O = O(T) = O(T - 1) (15) Similarly, if at k = T time, the two-norm of the difference between the matrix M(T) and the matrix M(T - 1) is less than the given error, obtain: M = M(T) = M(T - 1) (16) where, O and M are both intermediate matrices; Substitute the intermediate matrices O and M into (4.8) to solve for the gain matrices L of the filters F and Q 0 and L 1 ; Substitute the intermediate matrices O and M into equations (8) and (9) respectively to obtain the gain matrices L of the cascaded robust optimal filters F and Q 0 and L 1 ; 5), Substitute the gain of the above filter to obtain the real-time estimated value, and achieve the real-time position tracking of the mobile robot; Substitute the gain matrices L 0 and L 1 into the cascaded robust optimal filters F in Equation (4) and Q in Equation (5) to obtain the real-time estimated value of the mobile robot's position and achieve the tracking of the mobile robot.

Images