Self-adaptive UWB / IMU combined positioning method based on innovation change

Abstract

The invention discloses a UWB / IMU combined positioning method in an NLOS environment. In order to solve the problem that an ultra-wideband positioning system cannot accurately position in a complex environment with large noise change and unknown noise statistical characteristics, a nonlinear error model is established according to the characteristics of a UWB / IMU combined positioning system. In order to improve the stability and precision of UWB / IMU combined positioning, a robust adaptive EKF algorithm is provided, an adaptive factor is introduced to update a covariance matrix of measurement noise in real time, a fading factor is added to suppress filtering divergence, and the influence of historical noise on current positioning can be effectively adjusted. The method mainly comprises the steps of spatial model establishment, filtering state updating, filtering gain calculation and measurement updating. In a complex non-line-of-sight (NLOS) scene, experimental verification is carried out on the algorithm. Experimental results show that the x-axis root-mean-square errors of the scheme are respectively reduced by 50.37%, 59.65% and 76.44% compared with those of independent UWB, KF and EKF, the y-axis root-mean-square errors of the scheme are respectively reduced by 33.95%, 53.88% and 69.04%, the positioning error of 90% is kept within 5.08 cm, and the positioning precision in a non-line-of-sight environment is kept within an acceptable range.

Description

Technical Field

[0001] The present invention belongs to the technical field of indoor positioning, and in particular relates to an indoor positioning method based on combined positioning of UWB and IMU in a non-line-of-sight environment. Background Art

[0002] Ultra Wideband Band (UWB) is a wireless carrier communication technology that does not use a sinusoidal carrier but uses nanosecond non-sinusoidal pulses to transmit data. It is widely used in indoor positioning because of its advantages such as high time resolution, low power consumption and strong anti-multipath effect. However, the indoor environment is complex and changeable, and UWB signals are easily blocked by factors such as people, objects or walls, resulting in non-line of sight (NLOS) propagation, causing the positioning results to deviate from the actual position. Therefore, it is difficult to achieve high-precision positioning in complex environments relying solely on UWB. The inertial navigation system uses an inertial measurement unit (IMU), which can independently estimate the target position at high frequency and maintain reliable accuracy in a short period of time, but the error will accumulate over time. Therefore, the algorithm that combines the advantages of UWB and IMU for fusion positioning has been widely studied.

[0003] The present invention is based on the Kalman filter algorithm of UWB / IMU combined positioning, proposes a method for determining an adaptive factor according to the size of the new information change trend, adjusts the new information error correction scheme in real time under the influence of different non-line-of-sight environments, and adaptively adjusts the state estimation covariance matrix and the noise covariance matrix, thereby improving the positioning accuracy and stability of the UWB / IMU combined positioning system in complex environments. Summary of the invention

[0004] In view of the shortcomings of the prior art, the technical problem that the present invention intends to solve is that the ultra-wideband positioning system cannot accurately locate in an environment with large noise changes and unknown statistical characteristics, and proposes a UWB / IMU combined positioning method based on an adaptive Kalman filter algorithm based on new information changes. In a complex environment, when the UWB signal is in an NLOS environment, the IMU is used to assist in solving the problem of positioning distortion when the UWB signal is blocked.

[0005] The technical solution adopted by the present invention is as follows:

[0006] Step 1: By using the DWM1000 module as the transmitting and receiving module of the ultra-wideband signal and the icm42605 module as the inertial sensor module, design a program to realize the communication between the base station and the tag, and collect the acceleration and angular velocity information of the tag movement.

[0007] Step 2: After time synchronization, the collected original timestamp information is used to obtain the arrival time difference, and the TDOA positioning algorithm is used to calculate the position. The collected IMU information is solved by strapdown inertial navigation to obtain the carrier's position, speed, attitude and other information.

[0008] Step 3: Use the position, velocity, and attitude information as the state vector to construct the state equation, and use the arrival distance difference to construct the measurement equation. Feed the error state estimate back to the strapdown inertial navigation system for correction processing to obtain the final position, velocity, and attitude information.

[0009] Step 4: The adaptive Kalman filter algorithm based on the trend of new information changes predicts and updates the object's motion state at the next moment.

[0010] The beneficial effects of the present invention are as follows:

[0011] 1. The UWB / IMU combined positioning system in the present invention adopts a tight combination positioning method, which has higher positioning accuracy and better environmental adaptability than a loose combination system.

[0012] 2. The present invention adopts the exponential decay memory method to estimate the system measurement noise, which can reduce the influence of historical noise on the current filtering.

[0013] 3. According to the changing trend of the new information error, an adaptive factor is introduced to update the covariance of the measurement noise in real time to update the measurement noise, and a fading factor is added to suppress the filter divergence. During data fusion, the improved EKF algorithm can adaptively adjust the fading factor according to the environment to achieve accurate estimation of the noise covariance matrix, maintain strong tracking of the noise, and improve the stability and accuracy of the UWB / IMU combined positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] The present invention is further described below with reference to the accompanying drawings and examples.

[0015] Figure 1 It is the overall architecture of the positioning system;

[0016] Figure 2 It is a schematic diagram of the strapdown inertial navigation system;

[0017] Figure 3 It is based on a tight combination positioning flow chart;

[0018] Figure 4 It is the positioning scene graph

[0019] Figure 5 Walking positioning trajectory along a rectangular path in a non-line-of-sight environment

[0020] Figure 6 It is the CDF of the combined positioning error in non-line-of-sight environment Specific implementation methods

[0022] In order to enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0023] like Figure 1 In the overall architecture diagram of the positioning system shown in FIG, the positioning system includes the following two parts: one is the positioning node, which consists of a tag with an unknown position and four base stations with known positions; the other is the PC end equipped with a host computer. The base station and the PC end are connected by a network cable through a switch, and the base station and the tag are connected wirelessly. During the positioning process, the main base station sends a clock synchronization frame signal to the slave base station, and the tag sends a positioning frame signal to all base stations. The slave base station only receives the clock synchronization frame signal from the main base station and the positioning frame signal from the tag. At the same time, the mobile tag collects the acceleration and angular velocity collected by the inertial measurement unit through the UWB communication module to the main base station, and then the base station transmits the received data to the host computer through the user datagram protocol. The PC end converts the received data into motion information for display.

[0024] like Figure 2 The figure shows the principle diagram of strapdown inertial navigation system solution. The acceleration and angular velocity information after zero bias correction collected by the icm42605 module are integrated and transformed into coordinates to obtain the position, velocity, attitude and other information of the tag.

[0025] like Figure 3 As shown in the figure, in the tightly coupled positioning mode, the UWB system outputs the distance difference from the tag to each base station without calculating the position. The IMU calculates the position based on the inertial sensor measurement information and converts it into the distance information to each base station. The filter fuses the information of the two and outputs the optimal error estimate of the position, speed and attitude to correct the IMU positioning result. In the ESKF system, the real state x is involved. t , nominal state x and error state δx, the real state can be obtained by a certain combination (such as linear addition or matrix multiplication) of the nominal state and the error state. For the UWB and IMU combined positioning system implemented by the present invention, the nominal state is composed of the position, velocity, attitude of the tag and the acceleration bias and angular velocity bias of the inertial sensor. The error state corresponding to the nominal state can be expressed as:

[0026]

[0027] Discretizing the system error state equation, the state transfer formula is as follows:

[0028]

[0029] In the UWB position calculation method of the present invention, the TDOA algorithm is adopted, and antenna delay calibration and clock synchronization are carried out. Here, for the convenience of calculation, the time difference of arrival data is multiplied by the speed of light to obtain the range difference of arrival RDOA, and the range difference of arrival is used as the observable, and the observation equation is established as follows:

[0030]

[0031] Due to the non-linearity of the observation equation, it is necessary to find the Jacobian matrix of h i (x k+1 )

[0032]

[0033] In the formula:

[0034]

[0035] Where p i =[p ix p iy p iz is the UWB base station coordinate, and O1 and O2, which are the IMU solution positions at the current moment, are respectively the zero matrices of N×3 and N×9.

[0036] Then:

[0037] H k+1 =[H k+1，2 H k+1，3 …H k+1，N T

[0038] Define the innovation and innovation error covariance at time k as:

[0039]

[0040]

[0041] It can be seen from the above formula that as long as the innovation covariance can be accurately estimated, the measurement noise covariance can be deduced. The present invention estimates the current innovation error by exponential decay:

[0042]

[0043] Where d k =(1-b) / (1-b k ), 0 < b < 1. The forgetting factor b is generally taken as 0.95 - 0.99 according to experience. Therefore, the noise covariance matrix of UWB is:

[0044] ​

[0045] The environment in which the combined positioning system is located is changing, and the statistical characteristics of noise are also uncertain. According to different good line-of-sight environments, slight non-line-of-sight environments, and severe non-line-of-sight environments, in order to accurately estimate the noise covariance matrix, the adaptive factors are set as follows:

[0046]

[0047] in In order to ensure the stability of the algorithm, the fading factor λ is introduced k , the system prediction covariance matrix becomes:

[0048]

[0049]

[0050]

[0051] in, R base They are k =1 when the innovation covariance and noise covariance, L k The size can be set by yourself and can be set between 5 and 10.

[0052] Combining the above formula, we get the improved innovation covariance estimate:

[0053]

[0054] Therefore, when performing data fusion, by reasonably adjusting the adaptive factor and the fading factor, the influence of the state estimation value and the measurement value on the filtering can be more accurately controlled, thereby improving the combined positioning accuracy and robustness.

[0055] like Figure 4 As shown in the figure, in order to verify the positioning performance of the combined positioning algorithm in a non-line-of-sight environment, wooden boards, metals, cartons and other objects are set as static occlusions, and pedestrians are arranged to walk randomly to create a dynamic occlusion environment.

[0056] like Figure 5As shown in the figure, the IMU positioning shows a rapid divergence phenomenon, and the UWB positioning near the obstacle shows scattered points that deviate from the true trajectory. The black UWB scattered points at the ordinate of 300-400cm and the abscissa of 150cm-250cm show a large trajectory deviation, which is mainly due to the non-line-of-sight measurement error caused by the occlusion of metal and wooden boards, while the effect of the carton is relatively small. The deviation of the remaining UWB positioning is caused by pedestrians blocking the base station when walking. However, it can be seen that the positioning trajectory after fusion of several different filtering algorithms has been smoothed. The robust adaptive EKF is based on the increase of new information caused by abnormal measurement values ​​and inaccurate state prediction, and establishes an adaptive factor to dynamically correct the measurement noise covariance and state prediction covariance. Therefore, the improved robust adaptive EKF algorithm has better convergence in the non-line-of-sight environment and higher positioning accuracy. According to Table 1, the x-axis root mean square error of the robust adaptive EKF solution is reduced by 50.37%, 59.65%, and 76.44% respectively compared with the use of UWB, KF, and EKF alone, and the y-axis root mean square error is reduced by 33.95%, 53.88%, and 69.04% respectively. The maximum positioning error is reduced by 83.14cm, 16.92cm, and 6.42cm respectively.

[0057] Table 1 Position error of walking along a rectangle in non-line-of-sight environment

[0058]

[0059] like Figure 6 As shown, 90% of the positioning errors of UWB positioning, KF, EKF, and robust adaptive EKF are kept within 16.19cm, 10.23cm, 8.04cm, and 5.08cm, respectively, and the positioning accuracy in non-line-of-sight environments remains within an acceptable range.

[0060] Any matters not described in the present invention are applicable to the prior art.

Claims

1. A UWB / IMU combined positioning system in a NLOS environment, characterized by: The UWB / IMU combined positioning system is composed of a UWB positioning module, an inertial sensor module and a main controller module. The UWB / IMU combined positioning system in the present invention adopts a tight combination positioning method, which has higher positioning accuracy and better environmental adaptability than a loose combination system. The system measurement noise is estimated by using the exponential decay memory method to reduce the impact of historical noise on the current positioning. An adaptive factor is introduced to update the covariance matrix of the measurement noise in real time, and a fading factor is added to suppress the filter divergence, thereby improving the positioning accuracy, robustness and stability of the UWB / IMU combined positioning system in the NLOS environment.

2. The UWB / IMU combined positioning system according to claim 1, characterized in that: The combined positioning system adopts a tight combination method and uses the arrival distance difference of the UWB signal as the observation quantity:

3. The method for estimating system measurement noise using the exponential decay memory method according to claim 1, characterized in that:

4. The robust adaptive EKF algorithm according to claim 1, characterized in that: The environment in which the combined positioning system is located is changing, and the statistical characteristics of noise are also uncertain. According to different good line-of-sight environments, slight non-line-of-sight environments, and severe non-line-of-sight environments, in order to accurately estimate the noise covariance matrix, the adaptive factors are set as follows: in In order to ensure the stability of the filter, a fading factor is added to modify the weight of the adjacent measurement data: Combining the above formula, the new information error is divided into two parts for adaptive compensation, which can correct the influence of measurement noise in time. Therefore, when performing data fusion, by reasonably adjusting the adaptive factor and the fading factor, the influence of the state estimation value and the measurement value on the filtering can be more accurately controlled, thereby improving the combined positioning accuracy and robustness.

Images