Indoor Unmanned Vehicle Localization Method Based on Adaptive Unscented Kalman Filter

Abstract

The present invention discloses an indoor unmanned vehicle positioning method based on adaptive unscented Kalman filtering, mainly aiming to solve the problem of poor positioning accuracy of unmanned vehicles in complex indoor environments in the prior art. The implementation scheme is as follows: determining the state coordinates of the adaptive unscented Kalman filtering at each positioning moment; determining the observation coordinates of the adaptive unscented Kalman filtering at each positioning moment; calculating the residual theoretical covariance and the actual covariance of the Kalman filtering system and the difference between the two variances according to the state and observation coordinates, and determining the adaptive coefficient of the adaptive unscented Kalman filtering at each positioning moment according to these two variances; determining the gain coefficient of the adaptive unscented Kalman filtering at each positioning moment according to the Kalman filtering gain formula; calculating the final positioning coordinates of the indoor unmanned vehicle at each positioning moment according to these two coefficients and the difference between the two variances. The present invention improves the robustness and average positioning accuracy of indoor positioning in complex environments and can be used in indoor positioning systems.

Description

Technical Field

[0001] The present invention belongs to the field of communication technologies, and further relates to an indoor unmanned vehicle positioning method, which can be used in an indoor positioning system to estimate the two-dimensional position of an indoor unmanned vehicle. Background Art

[0002] An indoor unmanned vehicle is a movable device that helps people transport goods indoors. With the rapid development of science and technology, there are more and more indoor unmanned vehicles. In order to facilitate the management of indoor unmanned vehicles, it is necessary to know the position of the indoor unmanned vehicle, that is, to perform indoor positioning on the unmanned vehicle. There are many methods applied to indoor positioning. Among them, the indoor positioning method based on a base station and the indoor positioning method based on inertial navigation are two of the most common indoor positioning methods. The indoor positioning method based on a base station uses a beacon to receive a short pulse signal sent by the base station to calculate the specific position of the target object indoors. The indoor positioning method based on inertial navigation uses the acceleration and angular velocity information collected by an inertial measurement unit IMU mounted on the target object to calculate the specific position of the target object indoors. Each of these two indoor positioning methods has its own advantages and disadvantages. Reasonably integrating these positioning methods will greatly improve the overall performance of the system and make the indoor unmanned vehicle positioning result more accurate.

[0003] Shenzhen University disclosed an indoor positioning method based on the combination of UWB and IMU in its patent document with the application number: CN201910504895.5. This method first obtains the position of the object to be positioned indoors through an ultra-wideband UWB system; then obtains the three-axis acceleration and three-axis angular velocity of the target object during movement through the inertial measurement unit IMU; then calculates the position of the target indoors through an inertial integration algorithm calculation unit; finally, through the Kalman filter algorithm, the calculation results of the UWB system and the IMU system are fused. Although this method can reduce the influence of the non-line-of-sight complex environment and improve the positioning accuracy. However, since the Kalman filter is only applicable to linear systems, and the complex and changeable indoor environment in reality is often non-linear, the positioning result obtained by directly using the Kalman filter fusion positioning method in a complex environment has poor accuracy. Summary of the Invention

[0004] The purpose of the present invention is to propose an indoor unmanned vehicle positioning method based on adaptive unscented Kalman filtering to improve the positioning accuracy of unmanned vehicles in a complex indoor environment in view of the defects existing in the above-mentioned prior art.

[0005] The technical idea of the present invention is as follows: By using the unscented transformation to linearize the non-linear positioning system, the defect that the direct use of Kalman filtering in the prior art will lead to low average positioning accuracy of indoor unmanned vehicles in complex environments is overcome; aiming at the problem that the gain parameter of the unscented Kalman filter will seriously affect the quality of system state estimation, an adaptive adjustment factor is added to the filter, and the unscented Kalman gain parameter is adjusted by adjusting the covariance of the state vector and the observation vector, so as to improve the robustness and average positioning accuracy of positioning.

[0006] According to the above idea, the implementation steps of the present invention are as follows:

[0007] (1) Determine the state coordinates of the adaptive unscented Kalman filter at each positioning moment:

[0008] (1a) Use the inertial navigation integration algorithm to obtain the initial horizontal and vertical coordinates of the center point of the unmanned vehicle base at each positioning moment;

[0009] (1b) Perform unscented transformation on the initial horizontal and vertical coordinates of the center point of the unmanned vehicle base at each positioning moment to obtain the state coordinates of the adaptive unscented Kalman filter;

[0010] (2) Determine the observation coordinates of the adaptive unscented Kalman filter at each positioning moment:

[0011] (2a) Use the ultra-wideband UWB positioning algorithm to obtain the observed horizontal and vertical coordinates of the center point of the unmanned target vehicle base at each positioning moment;

[0012] (2b) Calculate the observation coordinates of the adaptive unscented Kalman filter at each positioning moment:

[0013]

[0014] Among them, and respectively represent the observed horizontal and vertical coordinates of the center point of the indoor unmanned vehicle base at the k-th positioning moment, Z k represents the observation coordinates of the adaptive unscented Kalman filter at the k-th positioning moment, k ∈ [1, ∞);

[0015] (3) Determine the adaptive coefficient of the adaptive unscented Kalman filter at each positioning moment:

[0016] (3a) Calculate the system residual r k of the adaptive unscented Kalman filter at the k-th positioning moment:

[0017] r k = Z k - X k

[0018] Among them, X kis the adaptive unscented Kalman filter state coordinate at the k-th positioning moment;

[0019] (3b) Calculate the theoretical covariance C of the adaptive unscented Kalman filter system residual at the k-th positioning moment k :

[0020]

[0021] where is the set of adaptive unscented Kalman filter state coordinates at the k-th positioning moment, N ∈ [1, ∞) is the expansion coefficient of the unscented transform, and T represents the transpose symbol;

[0022] (3c) Calculate the actual covariance C' of the adaptive unscented Kalman filter system residual at the k-th positioning moment k ':

[0023]

[0024] (3d) Calculate the difference d between the theoretical covariance and the actual covariance of the adaptive unscented Kalman filter system residual at the k-th positioning moment k :

[0025] d k = |tr(C k ) - tr(C k ')|,

[0026] (3e) Determine the adaptive coefficient ρ of the adaptive unscented Kalman filter at the k-th positioning moment according to the theoretical covariance C k and the actual covariance C k ': k :

[0027]

[0028] (4) Determine the gain coefficient K of the adaptive unscented Kalman filter at each positioning moment according to the Kalman filter gain formula k ;

[0029] (5) Calculate the final positioning coordinates of the indoor unmanned vehicle at each positioning moment according to the above parameters:

[0030]

[0031] where represents the final positioning coordinates of the center point of the base of the indoor unmanned vehicle at the k-th positioning moment.

[0032] Compared with the prior art, the present invention has the following advantages:

[0033] First, in view of the fact that complex indoor environments are mostly non-linear systems, the present invention linearizes complex non-linear systems by using unscented transformation, overcomes the defect that the direct use of Kalman filtering in the prior art results in low average positioning accuracy of indoor unmanned vehicles in complex environments, and improves the average positioning accuracy of unmanned vehicles in complex indoor environments.

[0034] Second, in view of the fact that the gain parameters of unscented Kalman filtering will seriously affect the quality of system state estimation, the present invention adds an adaptive adjustment factor to the filter and adjusts the unscented Kalman gain parameters by adjusting the covariance of the state vector and the observation vector, further improving the robustness and average positioning accuracy of indoor positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 is a flowchart for the implementation of the present invention;

[0036] Figure 2 is a comparison chart of simulation results of positioning errors between the present invention and the prior art. DETAILED DESCRIPTION OF THE INVENTION

[0037] The present invention will be further described in detail below with reference to the drawings and specific embodiments.

[0038] In this embodiment, the device to be positioned is an indoor unmanned vehicle equipped with an MPU6050 inertial sensor and a UWB beacon. The main control unit of the unmanned vehicle is an stm32 microcontroller, and the MPU6050 inertial sensor and the UWB beacon are connected to the stm32 microcontroller on the vehicle in a serial connection manner.

[0039] Refer to Figure 1 , and the implementation steps of the embodiment are as follows.

[0040] Step 1, determine the state coordinates of the adaptive unscented Kalman filter at each positioning moment.

[0041] 1.1) Obtain the initial horizontal and vertical coordinates of the center point of the unmanned vehicle base at each positioning moment:

[0042] The microcontroller mounted on the unmanned vehicle controls the MPU6050 inertial sensor thereon to collect the acceleration and angular velocity data of the vehicle running every 1 second by means of interrupt control, and then uses the inertial navigation integration algorithm to obtain the initial horizontal coordinate and the initial vertical coordinate of the center point of the unmanned vehicle base at each positioning moment:

[0043]

[0044] Among them, and respectively represent the horizontal coordinate state value and the vertical coordinate state value of the center of the indoor unmanned vehicle base at the k-th positioning moment, ωk Denote the angular velocity of the driverless vehicle collected by the inertial sensor mounted on the driverless vehicle at the k-th positioning moment as a k and denote the acceleration of the driverless vehicle collected by the inertial sensor mounted on the driverless vehicle at the k-th positioning moment;

[0045] 1.2) Perform unscented transformation on the abscissa state value and the ordinate state value of the center point of the driverless vehicle base at each positioning moment to obtain the state coordinate X k :

[0046] 1.2.1) Calculate the i-th state abscissa at the k-th positioning moment after unscented transformation for the abscissa state value of the center of the driverless vehicle base indoors

[0047]

[0048] where λ is the scaling ratio parameter; i is the state serial number, i ∈ [0, 2N]; N is the unscented transformation expansion coefficient, N ∈ [1, ∞);

[0049] 1.2.2) Calculate the i-th state ordinate at the k-th positioning moment after unscented transformation for the ordinate state value of the center of the driverless vehicle base indoors

[0050]

[0051] 1.2.3) Obtain the i-th adaptive unscented Kalman filter state coordinate at the k-th positioning moment according to the i-th state abscissa and the i-th state ordinate at the k-th positioning moment obtained by unscented transformation

[0052]

[0053] 1.2.4) Calculate the final adaptive unscented Kalman filter state coordinate X at the k-th positioning moment according to the 2N + 1 adaptive unscented Kalman filter state coordinates at the k-th positioning moment k :

[0054]

[0055] Step 2, determine the observation coordinates of the particle filter at each positioning moment.

[0056] 2.1) The microcontroller mounted on the unmanned vehicle uses the method of interrupt control to control the UWB beacon on it to collect the flight time of the short pulse signal and the arrival angle of the short pulse signal emitted by the ultra-wideband UWB base station every 1 second, and then uses the ultra-wideband UWB positioning algorithm to obtain the horizontal and vertical coordinate observations of the center point of the base of the unmanned target vehicle at each positioning moment:

[0057]

[0058] Among them, and respectively represent the horizontal and vertical coordinate observations of the center point of the base of the unmanned target vehicle at the k-th positioning moment, c0 represents the propagation speed of electromagnetic waves in the air, τ k represents the flight time of the short pulse signal emitted by the ultra-wideband UWB base station received by the ultra-wideband UWB beacon mounted on the unmanned target vehicle at the k-th positioning moment, θ k represents the arrival angle of the short pulse signal, X U and Y U respectively represent the abscissa value and ordinate value of the center point of the base of the indoor ultra-wideband UWB base station;

[0059] 2.2) According to the horizontal and vertical coordinate observations and the vertical coordinate observation of the center point of the base of the unmanned target vehicle at the k-th positioning moment, obtain the observation coordinates of the adaptive unscented Kalman filter at each positioning moment:

[0060]

[0061] Among them, Z k represents the observation coordinates of the adaptive unscented Kalman filter at the k-th positioning moment.

[0062] Step 3, determine the adaptive coefficient of the adaptive unscented Kalman filter at each positioning moment.

[0063] (3.1) According to the observation coordinates Z k and the state coordinates X k of the adaptive unscented Kalman filter at the k-th positioning moment, calculate the system residual r k of the adaptive unscented Kalman filter at the k-th positioning moment:

[0064] r k =Z k -X k ;

[0065] (3.2) According to the set of adaptive unscented Kalman filter state coordinates at the k-th positioning moment and the observation coordinates Z kCalculate the theoretical covariance C of the residuals of the adaptive unscented Kalman filter system at the k-th positioning moment k :

[0066]

[0067] where N is the expansion coefficient of the unscented transform, N ∈ [1, ∞); T represents the transpose symbol;

[0068] (3.3) Calculate the actual covariance C k ' of the residuals of the adaptive unscented Kalman filter system at the k-th positioning moment according to the system residual r k ':

[0069]

[0070] (3d) Calculate the theoretical covariance C k of the residuals of the adaptive unscented Kalman filter system at the k-th positioning moment and the difference d k between the actual covariance C k ':

[0071] d k = |tr(C k ) - tr(C k ')|;

[0072] (3e) Determine the adaptive coefficient ρ k of the adaptive unscented Kalman filter at the k-th positioning moment according to the theoretical covariance C k and the actual covariance C k ':

[0073]

[0074] Step 4, calculate the gain coefficient of the adaptive unscented Kalman filter at each positioning moment.

[0075] 4.1) Calculate the state covariance matrix of the adaptive unscented Kalman filter at the k-th positioning moment

[0076]

[0077] where is the set of adaptive unscented Kalman filter state coordinates at the k-th positioning moment; X k is the adaptive unscented Kalman filter state coordinate at the k-th positioning moment;

[0078] 4.2) Calculate the measurement covariance matrix of the adaptive unscented Kalman filter at the k-th positioning moment

[0079]

[0080] 4.3) According to the state covariance matrix of the adaptive unscented Kalman filter and the measurement covariance matrix obtain the gain coefficient K of the adaptive unscented Kalman filter at the k-th positioning moment k :

[0081]

[0082] wherein, is the inverse operation.

[0083] Step 5, calculate the final positioning coordinates of the indoor unmanned vehicle at each positioning moment to complete the positioning of the indoor unmanned vehicle.

[0084] Since the gain parameter of the unscented Kalman filter will seriously affect the quality of the system state estimation, in this example, an adaptive adjustment coefficient ρ k and the gain coefficient K k are added to the filter to adjust the covariance of the state vector and the observation vector, realize the adjustment of the unscented Kalman gain, and obtain the final positioning coordinates of the indoor unmanned vehicle at each positioning moment:

[0085]

[0086] wherein, the final positioning coordinates of the center point of the base of the indoor unmanned vehicle at the k-th positioning moment.

[0087] The following further illustrates the effect of the present invention in combination with simulation experiments:

[0088] 1. Simulation conditions.

[0089] The hardware platform for the simulation experiment of the present invention is: an indoor unmanned vehicle of model FW-001, a UWB base station, a laptop computer with a processor of Intel i7-6700, and an indoor unmanned vehicle equipped with an MPU6050 sensor and a UWB beacon.

[0090] Simulation software and hardware environment: Windows 10, MATLAB R2019a.

[0091] The data used in the simulation experiment of the present invention was collected from the Xibuilding laboratory and corridor of Xidian University in October 2022. The content of the sample set is the indoor positioning data every second within 100 seconds. Among them, the data in the first 50 seconds was collected in the corridor and can be regarded as an experiment in a simple environment, and the data in the last 50 seconds was collected in the laboratory and can be regarded as an experiment in a complex environment.

[0092] 2. Simulation content and its result analysis.

[0093] Under the above conditions, the position calculation of the indoor positioning data for each second within 100 seconds collected by the simulation is respectively performed using the present invention and the existing "indoor positioning and navigation system based on the fusion of IMU and UWB" to obtain the positioning results of the indoor unmanned vehicle. Then, this positioning result is divided by the actual positioning result to obtain the positioning accuracy and draw a positioning accuracy graph, as Figure 2 shown. Among them Figure 2 the abscissa represents the positioning time, and the ordinate represents the positioning accuracy. It can be seen from Figure 2 that within the first 50 seconds of positioning time, the positioning accuracy of the present invention is stable at around 97%, and the positioning accuracy of the existing technology is around 93%. In the subsequent 50s of positioning time, the positioning accuracy of the present invention is stable at around 95%; the positioning accuracy of the existing technology is only around 86%.

[0094] The simulation experiment shows that: in the complex environment in the subsequent 50s, since the present invention linearizes the non-linear complex environment through unscented transformation, its average positioning accuracy is significantly improved compared with the existing method. At the same time, whether in the simple environment in the first 50s or in the complex environment in the subsequent 50s, since the present invention adds an adaptive adjustment coefficient to the filter, the covariance of the state vector and the observation vector can be adjusted to adjust the unscented Kalman gain parameter, resulting in an improvement in the average positioning accuracy.

Claims

1. An indoor unmanned vehicle positioning method based on adaptive unscented Kalman filtering, characterized in that Including the following steps: (1) Determine the state coordinates of the adaptive unscented Kalman filter at each positioning moment: (1a) Using the inertial navigation integration algorithm, obtain the initial value of the abscissa of the center point of the unmanned vehicle base at each positioning moment and the initial value of the ordinate (1b) The initial value of the abscissa of the center point of the unmanned vehicle base at each positioning moment and the initial value of the ordinate perform unscented transformation to obtain the state coordinates X of the adaptive unscented Kalman filter k ; (2) Determine the observation coordinates of the adaptive unscented Kalman filter at each positioning moment: (2a) Use the ultra-wideband (UWB) positioning algorithm to obtain the horizontal and vertical coordinate observations of the center point of the base of the unmanned target vehicle at each positioning moment; (2b) Calculate the observation coordinates of the adaptive unscented Kalman filter at each positioning moment: Among them, and respectively represent the horizontal and vertical coordinate observation values of the center point of the indoor unmanned vehicle base at the k-th positioning moment, and Z k represents the observation coordinate of the adaptive unscented Kalman filter at the k-th positioning moment, where k ∈ [1, ∞); (3) Determine the adaptive coefficient of the adaptive unscented Kalman filter at each positioning moment: (3a) Calculate the system residual r of the adaptive unscented Kalman filter at the k-th positioning moment k : r k = Z k - X k where, X k is the state coordinate of the adaptive unscented Kalman filter at the k-th positioning moment; (3b) Calculate the theoretical covariance C of the residual of the adaptive unscented Kalman filter system at the k-th positioning moment k : wherein is the i-th adaptive unscented Kalman filter state coordinate at the k-th positioning moment, N ∈ [1, ∞) is the expansion coefficient of the unscented transform, and T represents the transpose symbol; (3c) Calculate the actual covariance C k ′ of the residual of the adaptive unscented Kalman filter system at the k-th positioning moment (3d) Calculate the difference d between the theoretical covariance and the actual covariance of the residuals of the adaptive unscented Kalman filter system at the k-th positioning moment k : d k = |tr(C k ) - tr(C k )|, (3e) Determine the adaptive coefficient ρ of the adaptive unscented Kalman filter at the k-th positioning moment according to the theoretical covariance C of the residual of the Kalman filter system k and the actual covariance C k ′, k : (4) Determine the gain coefficient K of the adaptive unscented Kalman filter at each positioning moment according to the Kalman filter gain formula k ; (5) Calculate the final positioning coordinates of the indoor unmanned vehicle at each positioning moment according to the following formula: Among them, represents the final positioning coordinates of the center point of the indoor unmanned vehicle base at the k-th positioning moment.

2. The method according to claim 1, wherein In step (1a), the initial abscissa value of the center of the indoor unmanned vehicle base at each positioning moment determined by using the inertial integration algorithm and the initial ordinate value The formula is as follows: Among them, and respectively represent the horizontal and vertical coordinate state values of the center of the indoor unmanned vehicle base at the k-th positioning moment, ω k represents the angular velocity of the unmanned vehicle collected by the inertial sensor mounted on the unmanned vehicle at the k-th positioning moment, a k represents the acceleration of the unmanned vehicle collected by the inertial sensor mounted on the unmanned vehicle at the k-th positioning moment.

3. The method according to claim 1, wherein The initial abscissa value of the center point of the unmanned vehicle base at each positioning moment in step (1b) and the initial ordinate value perform unscented transformation to obtain the state coordinate X of the adaptive unscented Kalman filter k , which is implemented as follows: (1b1) Calculate the abscissa state value of the center of the indoor unmanned vehicle base The abscissa of the i-th state at the k-th positioning moment after unscented transformation where λ is the scaling ratio parameter; i is the state serial number, i ∈ [0, 2N]; N is the expansion coefficient of the unscented transform, N ∈ [1, ∞); (1b2) Calculate the vertical coordinate status value of the center of the indoor unmanned vehicle base The i-th state vertical coordinate at the k-th positioning moment after unscented transformation (1b3) The abscissa of the $i$-th state at the $k$-th positioning moment obtained according to the unscented transformation and the ordinate of the $i$-th state to obtain the $i$-th adaptive unscented Kalman filter state coordinate at the $k$-th positioning moment (1.2.4) Calculate the final adaptive unscented Kalman filter state coordinate X at the k-th positioning moment based on the 2N + 1 adaptive unscented Kalman filter state coordinates at the k-th positioning moment k :

4. The method according to claim 1, wherein In step (2a), the UWB positioning algorithm is used to obtain the horizontal and vertical coordinate measurements of the center point of the base of the unmanned vehicle at each positioning moment, which are expressed as follows: Among them, and respectively represent the horizontal and vertical coordinate measurement values of the center point of the base of the unmanned target vehicle at the k-th positioning moment. $c_0$ represents the propagation speed of electromagnetic waves in air, and $\theta$ k represents the arrival angle of the short pulse signal; X U and Y U respectively represent the horizontal and vertical coordinate values of the center point of the indoor ultra-wideband (UWB) base station pedestal; τ k represents the time of flight of the short pulse signal emitted by the ultra-wideband UWB base station received by the ultra-wideband UWB beacon mounted on the unmanned target vehicle at the k-th positioning moment.

5. The method according to claim 1, wherein In step (4), the gain coefficient K of the adaptive unscented Kalman filter at each positioning moment is determined using the Kalman filter gain formula as follows: k , which is implemented as follows: (4a) Calculate the state covariance matrix of the adaptive unscented Kalman filter at the k-th positioning moment Among them, is the adaptive unscented Kalman filter state coordinate set at the k-th positioning moment, X k is the adaptive unscented Kalman filter state coordinate at the k-th positioning moment; (4b) Calculate the measurement covariance matrix of the adaptive unscented Kalman filter at the k-th positioning moment (4c)According to the state covariance matrix of the adaptive unscented Kalman filter and the measurement covariance matrix obtain the gain coefficient K of the adaptive unscented Kalman filter at the k-th positioning moment k : Among them, is the inverse operation.

Images