一种低成本的个人导航仪

As a type of novel autonomous navigation technology, personal navigation is widely used in the military and civilian fields, especially for tasks including individual combat, emergency rescue, reconnaissance and expedition, etc. Although the most common algorithm for personal navigation is MIMU/GPS integrated navigation, it is unable to continue positioning in the locations where the satellites are sheltered such as cities, valleys, and forests[1]. Besides, the SINS algorithm with low-accuracy MEMS sensors are unreliable for the significant accumulative error. Therefore, additional sensors are necessary in the process of positioning and compassing based on MIMU/GPS integrated navigation. Xu[2] put forward a kind of MIMU/GPS/magnetometer integrated navigation system in 2014, which behaved better than MIMU/GPS integrated navigation outdoors. Aggarwal et al. [3] suggested heuristic elimination of gyro drift in GPS-denied conditions, estimating heading angle by matching IMU-derived heading to the direction of the streets map in the database, whose precision is 1%. Guo et al.[4] suggested a personal navigation system using an inertial measurement unit and a MEMS-based ground reaction sensor array (GRSA), which provides a zero-velocity update for the IMU, and the system showed high accuracy both in vertical and in-plane positioning.

In terms of indoor location, the positioning algorithm can be divided into four categories: 1) ZUPT (zero velocity update), using velocity error at stance phase as an observation measurement in the Kalman filter to correct position errors, velocity errors and attitude errors[5]; 2) Wireless positioning, usually based on RSSI or TDOA, calculating the receiver’s location through the estimated distance between targets and the receiver; 3)Database matching, for instance, a raised typical indoor location system using Wi-Fi fingerprint matching technique[6]; 4) Dead reckoning, identifying pedestrian steps through inertial sensors[7]. Recent research results show a tendency toward combining different methods, for instance, a combination of ZUPT and indoor map matching[8], a combination of ZUPT and Wi-Fi received signal strength indicator[9], a combination of dead reckoning and RFID[10], and a combination of dead reckoning and map matching[11-12]. The methods above have a common point that they are based on the autonomous navigation algorithm, with external information (map, Wi-Fi, RFID, etc.) as an auxiliary. The addition of external information helps limit errors and improves positioning, which produces a better positioning effect indoors.

This paper proposes a kind of low-cost personal navigation unit which works in both outdoor and indoor locations. In the event of qualified GPS signals, the MIMU/GPS algorithm is adopted, while on other occasions the MIMU/magnetometer and pedometer are integrated. The experiments show that the unit operates effectively and switches between the two modes seamlessly.

1 Description of Hardware

The hardware includes a navigation computer and data acquisition module. Fig.1(a) shows that the navigation computer consists of a DSP and a FPGA, which are connected through EMIFA. The FPGA connects peripherals and sends calculation results to the PC through UART, while DSP runs the navigation algorithm. The data acquisition module is composed of an accelerometer, a gyroscope, a magnetometer and a GPS board. The frequencies of the inertial data and GPS are 100 and 1 Hz, respectively. The personal navigation unit is immobilized on the pedestrian’s waist while walking, and the result is shown on the PC monitor. Fig.1(b) is the picture of the unit.

2 Mathematical Model of Outdoor Positioning

In terms of outdoor positioning, the MIMU, GPS and magnetometer are integrated.The initial attitude is determined by the accelerometer and magnetometer. In the algorithm, the extended Kalman filter fuses GPS, magnetometer and positioning data, and then the errors of the accelerometer, gyroscope and navigator can be estimated. After the data is corrected, the navigation results are outputted.

2.1 The system state equation

The extended Kalman filter state equation is

where X is the system’s state vector; W is the noise vector; F is the state transition vector; G is the noise conversion vector. The form of X and W is X=δL δλ δh δVE δVN δVU δφE δφN δφU εbx εby εbzT, and

The components δL, δλ, δh are the errors of longitude, latitude and height; δVE, δVN, δVU are the errors of velocity in east, north and upward directions; δφE, δφN, δφU are the errors of attitude in east, north and upward directions; εbx, εby, εbz are the zero drift errors of the gyroscope; ωgx, ωgy, ωgz are the white noises of the gyroscope; ωax, ωay, ωaz are the white noises of the accelerometer.

2.2 The system observation equation

The MIMU output parameters are corrected with the help of GPS and magnetometer data, that is, on the occasions when qualified GPS signals, GPS velocity, the position and heading angle are used and on other occasions, the magnetometer provides a heading angle. The observation equation is

where LS, λS, hS, VSE, VSN, VSU, φS are the position, velocity and heading angle calculated through the SINS navigation algorithm; LG, λG, hG, VGE, VGN, VGU, φG are the GPS positions, velocity and heading angle; φM is the magnetic heading angle obtained from magnetometer; δL, δλ, δh, δVE, δVN, δVU, δφ are the errors of MIMU positions, velocity and heading angle; δLG, δλG, δhG, δVGE, δVGN, δVGU, δφG are the errors of GPS positions, velocity and heading angle; δφM is the error of magnetic heading angle; H and V are the system’s measurement matrix and measurement noise matrix, respectively.

3 Mathematical Model of Indoor Positioning

In terms of indoor positioning,the dead reckoning algorithm is adopted. According to accelerometer data, the adaptive threshold is calculated and the step confidence is estimated, and then the step can be detected in the adaptive window. In addition, step length can be estimated according to the step frequency based on the step frequency-length linear model. Finally, indoor positioning is realized with step number and step length.

3.1 Analysis of step data

The output of the 3-axis accelerometer mobilized on the pedestrians’ waist shows that the acceleration data changes cyclically, especially the vertical and forward accelerations. However, multiple peaks and valleys exist in each cycle because of factors including noise and error. Therefore, the acceleration data of both axes should be taken into account for the issue of steps detection.

3.2 Adaptive threshold

The vertical acceleration fluctuates periodically, one cycle matching one step. Here is the detection method, first we compare vertical acceleration to a predetermined threshold, and then we regard the time point that acceleration passes through threshold from bottom to up as the beginning of a step. Considering that multiple extremums may appear in one period since the raw data rises and falls sharply, the method may miss a step or overcount.

In each time window, the high threshold TH, middle threshold TM, and low threshold TL can be written as

Suppose that there are i local maximums, p1,p2,…,pi, and j local minimums, v1,v2,…,vj, in the time window. TH,t-1 and TL,t-1 are the high threshold and low threshold in the previous time window. The three thresholds change constantly with the pedestrian’s walking steps. In particular, the methods calculating adaptive thresholds on the vertical and forward axes are the same. It is relatively reliable to use the middle threshold as a standard, so the unit regards the time point when the vertical acceleration passes through middle threshold upward as the beginning of a step.

3.3 Estimation of degree of step confidence

The unit distinguishes between two states, walking and rest, by estimating the degree of step confidence twice. The purpose of the first estimation is to judge the trend of data in the window roughly and ignore the windows in which steps almost impossibly exist. Standard deviation is a typical statistical argument reflecting the degree of dispersion. Greater standard deviation matches sharper fluctuation of the acceleration data in a window, that is, a step is more likely to exist. However, the large amount of computation in calculating standard deviation affects the efficiency of the algorithm, so the fluctuation is represented by

The calculation of sy is similar to Eq.(6). It is certain that sz and sy approach zero when a pedestrian rests or barely moves. From this, the degrees of step confidence on the two axes, cz1 and cy1, will be set to be 1 or clear to 0 by the rule

The second estimation is executed if both cz1 and cy1 meet the condition, and the degrees, cz2 and cy2, are defined by the difference between high and low thresholds. The rule can be written as

wher TH and TL are the high threshold and low threshold of acceleration in the time window, respectively. The thresholds 0.9 and 1.1 are empirical values. Then, the window in which the number of steps changes can be detected. Fig.2(a) and Fig.2(b) show the degree of confidence of vertical and forward accelerations.

If both cz2 and cy2 are set, the vertical acceleration data will be traversed and the step will be detected further. The condition of the step can be written as

where the sampling point k+1 is the time when vertical acceleration passes through the middle threshold upward, that is, the beginning of a step.

3.4 Adaptive time window

Although the algorithm does not miss a step if the passing point is in the time window, the problem is that the step will be missed if the point is exactly right on the boundary of the time window. Therefore, an adaptive time window is adopted to make the step point appear in the middle as much as possible, which means that a time window with changing steps will be terminated if the condition

is met. The point k+1 is the time when vertical acceleration passes through the low threshold downward, and a new time window will be fit from data az,k+1 and ay,k+1. The data in the new time window is {az,k+1,…,az,N,…,az,N+k} and{ay,k+1,…,ay,N,…,ay,N+k}, where N is the size of the window.

In Fig.3, the blue curve is the vertical acceleration data acquired while walking, the black polyline represents middle threshold, and the red split line represents the detected step. Since the time window is adaptive, the width of the window varies. The figure shows that the result of the algorithm is quite effective.

3.5 Dead reckoning

According to the dead reckoning theory, the walking process can be separated into steps, each one projecting to east and north under the heading angle. As the duration of a step is quite short, it is supposed to be a linear motion, and the pedestrians’ position is obtained by accumulating all of the steps.

The position on the next moment, (Ln+1,λn+1), is calculated on the basis of dead reckoning according to the previous position, (Ln,λn), which can be modeled by

where φM is the magnetic heading angle; rn is the curvature radius of the earth on the north-axis; re is the curvature radius of the earth on the east-axis; l is the length of a step which is estimated by the frequency. There is a linear relationship between the length l and frequency f of a step, which can be written as

where α and β are coefficients of the linear equation, which are obtained from the fitting curve by several tests. As Fig.4 shows, the vertical and forward acceleration data are put into two 50-length buffer arrays, and the algorithm is executed when the arrays are full. The first estimation of the confidence degree is obtained according to Eqs.(7) and (8), and then the adaptive thresholds are obtained according to Eqs. (3) to (5). The number of steps is probably changing in the current time window if both of the second estimations meet the condition, and the changing point can be detected when vertical acceleration passes through TM upward. Finally, the current position is calculated based on the step number, length and magnetic heading angle.

4 Switch Between Two Modes

The instrument chooses GPS/SINS integrated navigation mode when outdoors, and it changes to the dead reckoning algorithm when indoors, that is, there is no GPS signal. Fig.5 shows the switching method between outdoor and indoor algorithms.

The GPS signal is checked every second (GPS frequency is 1 Hz). Once the signal is not available, there is a variable K counting time. If K is less than 5 which means that missing time is less than 5 s, the GPS/SINS integrated navigation algorithm can still work. Otherwise, the system changes to the dead reckoning algorithm.

As Fig.6 shows, the participant walks from an open environment into a building, and wanders for a while indoors. The diagram can roughly indicate the person’s path, so the switching algorithm is acceptable.

5 Results and Discussion

Before testing the navigation algorithm, the pedometer was tested since the accuracy of the counting steps is essential for the navigation result. The experiment participant wearing the unit walked on the path at different lengths, and the steps were counted and compared to the steps outputted from the pedometer.

The results in Tab.1 show that the algorithm counts steps correctly and the precision is greater than 95%.

Afterwards, an experiment testing the indoor positioning effect was performed, in which the participant wearing the unit walked indoors along a specified route. The participant started from point (0, 0), and the route shaped a rectangle as Fig.7(a) shows. The error during the route can be seen in Fig.7(b). The walking distance is about 80 m, and Tab.2 shows that the root square errors in east and north directions are 0.71 and 1.34 m, respectively.

The comprehensive experiment was performed on campus. Both indoor and outdoor algorithms could be tested in this situation, because there were buildings and tree shade and GPS signal could be sheltered. The participant wearing the unit walked around campus, and Fig.8(a) is the trace plot on the Google map.

According to the experiment, GPS could not position during loss of a satellite signal while the pedestrian passed across the courtyard, under tall buildings or tree shade. However, the unit was able to adapt to indoor situation and position with the help of the accelerometer and magnetometer. The errors during the experiment can be seen in Fig.8(b), and the maximum error in east and north directions is 8 m.

The root mean square errors in east and north directions are listed in Tab.3. The results show that the error rate of the unit is less than 0.2% of the total route distance, which is about 1 600 m. This proves that the unit is able to position correctly in most occasions.

6 Conclusions

1) The personal navigation unit realizes long-term positioning indoors and outdoors for a pedestrian, and is able to switch between two modes when the situation changes.

2) The outdoor positioning algorithm is MIMU/GPS/magnetometer integrated navigation, using the position, velocity and heading angle as an observation vector in the extended Kalman filter.

3) The indoor positioning operating mode integrates the MIMU/magnetometer, and the algorithm is dead reckoning. With the vertical and forward acceleration data, the algorithm is able to detect the steps according to the adaptive threshold and degree estimation of step confidence. Finally, positioning is realized by magnetometer determining heading.

4) The experiment shows that the unit’s error rate is less than 0.2% of the total route distance, which proves that the realization is effective and beneficial for research in fields of personal navigation.

[1]Sun C, Guo S Q, Li D. The technology of MIMU/GPS navigation [J]. Mechanical Management and Development, 2006(2): 83-84,86. (in Chinese)

[2]Xu N. The design and realization of MIMU/GPS/magnetic compass integrated personal navigation system [D]. Nanjing: Southeast University, 2014. (in Chinese)

[3]Aggarwal P, Thomas D, Ojeda L, et al. Map matching and heuristic elimination of gyro drift for personal navigation systems in GPS-denied conditions[J].Measurement Science and Technology, 2011, 22(2): 025205. DOI:10.1088/0957-0233/22/2/025205.

[4]Guo Q, Bebek O, Cavusoglu M C, et al. A personal navigation system using MEMS-based high-density ground reaction sensor array and inertial measurement unit[C]//2015 18th International Conference on Solid-State Sensors, Actuators and Microsystems. Anchorage, AK, USA, 2015: 1077-1080. DOI:10.1109/TRANSDUCERS.2015.7181113.

[5]Zhang J L, Qin Y Y, Mei C B. Shoe-mounted personal navigation system based on MEMS inertial technology [J]. Journal of Chinese Inertial Technology, 2011, 19(3):253-256. (in Chinese)

[6]Li Y, He Z, Nielsen J, et al. Using Wi-Fi/magnetometers for indoor location and personal navigation[C]// IEEE International Conference on Indoor Positioning and Indoor Navigation. Banff, AB, Canada, 2015:1-7. DOI:10.1109/IPIN.2015.7346764.

[7]Kang W, Han Y. SmartPDR:Smartphone-based pedestrian dead reckoning for indoor localization[J]. IEEE Sensors Journal, 2015, 15(5): 2906-2916. DOI:10.1109/jsen.2014.2382568.

[8]Zhang X D, Ren M R,Wang P, et al. A new zero velocity update algorithm for the shoe-mounted personal navigation system based on IMU[C]//2015 34th Chinese Control Conference. Hangzhou, China, 2015: 5297-5302. DOI:10.1109/ChiCC.2015.7260466.

[9]Zhuang Y, Lan H Y, Li Y, et al. PDR/INS/WiFi integration based on handheld devices for indoor pedestrian navigation[J].Micromachines, 2015, 6(6): 793-812. DOI:10.3390/mi6060793.

[10]Ho C C, Lee R. Real-time indoor positioning system based on RFID heron-bilateration location estimation and IMU inertial-navigation location estimation[C]//2015 IEEE 39th Annual Computer Software and Applications Conference. Taichung, China, 2015: 481-486. DOI:10.1109/COMPSAC.2015.317.

[11]Racko J, Brida P, Perttula A, et al. Pedestrian dead reckoning with particle filter for handheld smartphone[C]//2016 International Conference on Indoor Positioning and Indoor Navigation. Alcala de Henares, Spain, 2016: 1-7. DOI:10.1109/IPIN.2016.7743608.

[12]Inderst F, Pascucci F, Renaudin V. PDR and GPS trajectory parts matching for an improved self-contained personal navigation solution with handheld device[C]//2017 European Navigation Conference. Lausanne, Switzerland, 2017: 100-107. DOI:10.1109/EURONAV.2017.7954198.

A low-cost personal navigation unit