UWB-PDOA and MEMS inertial sensor combined positioning method

Abstract

The invention discloses a UWB-PDOA and MEMS inertial sensor combined positioning method. The invention is characterized in that an adaptive error Kalman integrated positioning method of a single base station is designed. In the aspect of hardware, a UWB device and an MEMS inertial sensing device which support PDOA are selected to collect and calculate UWB information and inertial navigation information; in the aspect of software, in order to improve the precision and stability of UWB and MEMS inertial sensor combined positioning, a self-adaptive error Kalman combined positioning algorithm based on the chi-square test is provided, the chi-square test is introduced to carry out dynamic evaluation on system noise, and a Kalman combined positioning algorithm of self-adaptive process noise is obtained. The advantages and disadvantages of the two independent positioning systems are fully fused, and the positioning precision and robustness of the UWB-PDOA positioning system are improved.

Description

Technical Field

[0001] The present invention relates to the field of indoor positioning technology, and in particular to a positioning method combining UWB-PDOA and MEMS inertial sensor. Background Art

[0002] With the rapid development of navigation technology, indoor positioning technology based on UWB has broad application prospects in the field of indoor positioning with centimeter-level positioning accuracy. UWB-PDOA (Phase Difference of Arrival) positioning technology is a high-precision positioning method based on the phase difference of ultra-wideband (UWB) signals for position measurement. UWB-PDOA indoor positioning technology can achieve relatively accurate two-dimensional indoor positioning with only a single base station. However, UWB signals are easily affected by factors such as multipath effects and non-line-of-sight propagation during transmission, which greatly affects the positioning accuracy. The micro-inertial navigation system of MEMS inertial sensors has strong interference ability and high short-term accuracy, but there are cumulative errors and it cannot be positioned for a long time. Therefore, MEMS inertial positioning is introduced as an auxiliary independent observation: accurately estimating the attitude and motion trajectory of the carrier, which has good complementary characteristics with UWB-PDOA positioning.

[0003] The present invention proposes a combined positioning system based on UWB-PDOA and MEMS inertial sensors, and also proposes an adaptive error Kalman filter algorithm using the Kalman method to fuse the PDOA positioning information measured by UWB with the attitude position information measured by the MEMS inertial system. This effectively improves the positioning accuracy and stability of the combined positioning system of UWB-PDOA and MEMS inertial sensors. Summary of the invention

[0004] In view of the problem that the existing UWB-PDOA indoor positioning technology is still unstable and prone to large errors. The present invention designs a UWB-PDOA / MEMS inertial sensor combined positioning system. In terms of hardware, UWB devices and MEMS inertial sensor devices that support PDOA are selected to collect UWB and inertial navigation information; in terms of software, an adaptive error Kalman combined positioning algorithm based on the chi-square test is proposed, which fully integrates the advantages and disadvantages of the two independent positioning systems and solves the problem of unstable positioning accuracy when using UWB-PDOA positioning alone.

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

[0006] Step 1: Design the indoor positioning system. The base station main control module uses the STM32F407VET6 chip, which is very suitable for the development of UWB positioning systems due to its high performance, low power consumption and high integration. The ultra-wideband positioning module uses the DWM3000 module, which supports dual-antenna configuration and is very suitable for the development of PDOA positioning systems. The power module uses USB for power supply, and uses the power management IC (AMS1117-3.3) to stabilize the input voltage at 3.3V. The CH340X chip is selected to implement the USB to serial port module function, and is connected to the computer host through serial port communication for system debugging and data error analysis. The MEMS inertial sensor module selects the ICM42605 module to collect inertial data.

[0007] Step 2: Design a program to collect UWB's PDOA information, timestamp information, and inertial sensor information. Use the PDOA+DS-TWR method to calculate the UWB position, and use the strapdown inertial navigation algorithm to calculate the collected inertial sensor information to obtain the carrier's position, velocity, and attitude information.

[0008] Step 3: Use the error Kalman filter algorithm to construct the state equation with position error, velocity error, and attitude error information as the state vector, and construct the measurement equation with the position error. The error state estimation is compensated to the UWB positioning system for correction processing to obtain the final position, velocity, and attitude information.

[0009] Step 4: Based on the original UWB positioning model, the adaptive error Kalman filter algorithm with chi-square test is adopted, and the chi-square test is introduced to dynamically evaluate the system noise to determine the accuracy of the positioning system model, based on which the covariance matrix of the process noise is further estimated and corrected in real time.

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

[0011] 1. The present invention proposes a positioning method of combining UWB-PDOA and MEMS inertial sensors of a single base station, which improves the positioning accuracy and stability compared with using UWB positioning alone.

[0012] 2. The present invention proposes an error Kalman filter combined positioning and navigation algorithm using a chi-square test, an error Kalman algorithm using a chi-square test, and utilizes the chi-square test to evaluate the accuracy of the system model.

[0013] 3. A three-segment fuzzy logic judgment function is introduced to dynamically evaluate and correct the system noise estimate. The upper and lower thresholds of the process noise covariance matrix are added to suppress filter divergence and weaken the influence of abnormal noise, thereby obtaining more accurate and stable positioning information. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0015] Figure 1 It is the circuit block diagram of the positioning system.

[0016] Figure 2 It is an overall schematic diagram of the positioning system.

[0017] Figure 3 It is the structural block diagram of the combined positioning algorithm.

[0018] Figure 4 It is the positioning trajectory diagram of the positioning system.

[0019] Figure 5 It is the cumulative distribution function (CDF) of positioning error. DETAILED DESCRIPTION

[0020] 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.

[0021] like Figure 1 In the circuit block diagram of the positioning system shown in FIG, the positioning system consists of an inertial sensor module, an ultra-wideband module, a USB-to-serial port module, a main controller module, and a power module. The inertial sensor module and the ultra-wideband module communicate with the microcontroller master through the SPI bus to realize the control communication between the inertial module and the ultra-wideband module; the USB-to-serial port module can realize the data conversion between the computer USB interface and the USART interface, and realize the data communication between the base station and the PC; the LDO in the system power module converts the external USB voltage into a stable 3.3V power supply voltage to realize the power supply function of the positioning system.

[0022] like Figure 2In the overall schematic diagram of the positioning system shown, the positioning system mainly includes a base station, a mobile tag and a PC. The base station and the PC are connected via a USB interface, and the base station and the tag are connected via a UWB signal. During a positioning process, the mobile tag exchanges data with the base station to obtain the data information required for positioning, including the timestamp information and phase difference information of the mobile tag, and the acceleration and angular velocity information collected by the mobile tag through the inertial measurement unit. The system is divided into two working states: static state and motion state. The switching of the working state is realized through the motion wake-up interrupt function of the MEMS inertial system. In the static state, only UWB's PDOA positioning is used, and in the motion state, a combined positioning method of UWB-PDOA and MEMS inertial system is adopted.

[0023] like Figure 3 In the structural block diagram of the combined positioning algorithm shown in the figure, the MEMS inertial sensor positioning system collects three-axis acceleration and three-axis angular velocity data, and obtains the position, velocity, attitude and other information of the tag after using the strapdown inertial navigation algorithm. The UWB positioning system collects the phase information and timestamp information of the positioning tag, and the position information solved by the UWB system can be obtained through the UWB-PDOA algorithm. The error between MEMS and UWB is estimated with the help of the Kalman filter, and the solution results of the navigation system are evaluated and optimized. The 15-dimensional system state vector composed of the three-axis position error, three-axis velocity error, three-axis attitude error, three-axis gyroscope offset error and three-axis accelerometer offset error is:

[0024]

[0025] The measurement equation is:

[0026] z=Hx+ζ,

[0027] H=[I 3*3 , O 3*12 ]

[0028] I 3*3 is a 3*3 unit matrix, O 3*12 is a 3*12 zero matrix, and ζ is the measurement noise vector.

[0029] In the adaptive error Kalman algorithm update of the chi-square test, the new information is:

[0030]

[0031] During the filtering process, if the observed value has no large deviation, the new information output of the Kalman filter obeys the standard normal distribution, and its covariance matrix is:

[0032]

[0033] The test statistic for constructing new information is:

[0034]

[0035] When the state changes suddenly, the estimated value of the filter deviates greatly from the actual value, and the process noise increases. The greater the degree of change, the greater the chi-square test value. The significance level α of the chi-square test can be set, and the critical value under the significance level can be found in the table. When the chi-square test value is greater than the critical value, it can be judged that the positioning model has a sudden change. Introduce a three-segment fuzzy logic judgment function to evaluate the process noise and set the upper limit threshold α of the state detection MAX and the lower threshold α MIN , respectively represent the chi-square test values ​​corresponding to the confidence probabilities of 99% and 90%, α MAX =11.35,α MIN =6.25, define the three-segment fuzzy judgment function as:

[0036]

[0037] Q MIN The covariance matrix Q is preset to the statistical characteristics of the process noise under normal circumstances. MAX Generally Q MIN An integer multiple of .

[0038] Therefore, the chi-square test can effectively and dynamically evaluate the system status, dynamically adjust the process noise, and improve the system's anti-interference ability and positioning accuracy.

[0039] like Figure 4 In the positioning trajectory diagram of the positioning system shown in the figure, the positioning trajectory is divided into 4 categories: the first is the positioning result of using UWB-PDOA alone, the second is the positioning result after Kalman filtering when using UWB-PDOA alone, the third is the positioning result of integrating UWB and MEMS inertial sensor system, and the last category is the positioning result of the adaptive error Kalman combination positioning algorithm using the chi-square test. It can be seen from the data in Table 1 that after using the adaptive error Kalman combination positioning algorithm using the chi-square test, the root mean square error in the X and Y directions is significantly reduced compared with using UWB alone, KF (UWB) and KF (UWB + MEMS).

[0040] Table 1 Positioning error analysis

[0041]

[0042] like Figure 5In the cumulative distribution function (CDF) of positioning error shown in the figure, 90% of the positioning accuracy using single UWB positioning, KF (UWB), KF (UWB + MEMS) and adaptive KF (UWB + MEMS) is within 28.11cm, 20.75cm, 14.98cm, and 11.2cm, meeting the positioning accuracy requirements of a single base station.

Claims

1. A UWB-PDOA and MEMS inertial sensor combined positioning system, characterized in that: The UWB-PDOA and MEMS inertial sensor combined positioning system is composed of a MEMS inertial sensor module, a main controller module, a UWB-PDOA positioning module and a power management module. The UWB-PDOA and MEMS inertial sensor combined positioning method adopts the error Kalman algorithm of chi-square test and utilizes chi-square test to evaluate the accuracy of the system model. A three-segment fuzzy logic judgment function is introduced to dynamically evaluate and correct the system noise estimation value. The upper and lower thresholds of the process noise covariance matrix are added to suppress the filter divergence, thereby improving the robustness and positioning accuracy of the positioning system.

2. The UWB-PDOA and MEMS inertial sensor combined positioning system according to claim 1, characterized in that: The system is divided into two working states: static state and moving state. The switching of working states is realized through the motion wake-up interrupt function of the MEMS inertial system. In the static state, only UWB's PDOA positioning is adopted, and in the moving state, the combined positioning method of UWB-PDOA and MEMS inertial system is adopted; the MEMS inertial sensor module and the UWB-PDOA module communicate with the microcontroller main control through the SPI bus to realize the collection and solution of inertial sensor data and ultra-wideband data. The three-axis acceleration and angular velocity information collected by the inertial sensor module are used to obtain the position, posture and speed of the mobile tag through navigation solution. The arrival phase information and timestamp information collected by the UWB module can be used to obtain the position settlement under the UWB system through the PDOA algorithm; the linear regulator module in the system power module converts the external USB voltage into a stable 3.3V power supply voltage to realize the power supply function of the positioning system.

3. The combined positioning method of UWB-PDOA and MEMS inertial sensor according to claim 1, characterized in that: The error Kalman algorithm of the chi-square test is used to evaluate the accuracy of the system model. The chi-square test value based on the new information is constructed. In the update of the error Kalman algorithm of the chi-square test, if the UWB module or the MEMS inertial sensor module is subject to large noise and causes a large deviation in the collected data, the chi-square test value will change accordingly. The chi-square test value is used to judge the accuracy of the system model.

4. The three-stage fuzzy logic judgment function according to claim 1 is characterized in that: A three-segment fuzzy logic judgment function is introduced to evaluate process noise and set the upper threshold α of state detection. MAX and the lower threshold α MIN , respectively represent the chi-square test values ​​corresponding to the confidence probabilities of 99% and 90%, α MAX =11.35,α MIN =6.25, define the three-segment fuzzy judgment function as: Q MIN The covariance matrix Q is preset to the statistical characteristics of the process noise under normal circumstances. MAX -Generally Q MIN An integer multiple of . Therefore, the chi-square test can effectively and dynamically evaluate the system status, dynamically adjust the process noise, and improve the system's anti-interference ability and positioning accuracy.

Images