A Heading Drift Correction Method for Pedestrian Inertial Positioning ()
1. Introduction
With the mature development of global satellite navigation technology, satellite navigation systems can provide continuous, stable, and reliable position services to users worldwide [1] . However, satellite navigation signals are susceptible to interference and obscuration, and the need for navigation and positioning in underground, underwater and other obscured spaces is becoming increasingly urgent [2] . Among these, autonomous positioning of pedestrians based on Micro-Electro-Mechanical System (MEMS) inertial/geomagnetic sensors has received a lot of attention due to their small size, lightweight, and low cost. The MEMS inertial/geomagnetic based pedestrian autonomous positioning consists of two main architectures, a Strapdown Inertial Navigation System (SINS) + Zero Velocity Updating (ZUPT) system and a Pedestrian Dead Reckoning (PDR) system. Although PDR has no special requirements for the installation position of the sensor, the lack of effective observation of position, velocity, attitude, and other information leads to large positioning errors in PDR, which makes it difficult to meet the actual application requirements [3] .
The error sources of PDR mainly include step counting error, step length error and heading error, among which the continuous accumulation of heading error will lead to rapid dispersion of positioning error. The current heading error correction methods can be divided into three categories, namely heuristic correction method, zero-angle speed update correction method and geomagnetic assisted correction method. Among them, the heuristic correction method is to construct the heading angle error observation based on the linear motion pattern of pedestrians, to estimate and correct the heading error in combination with the Kalman filter method, as in the literature [4] . The zero angular velocity update correction method is mainly based on the time period when the human body is stationary or the foot is on the ground, and the correction of the heading angle is achieved by constructing the heading angle error and the angular rate measurement error since the heading angle is constant and the three-axis angular rate output is zero, as in the literature [5] . The geomagnetic-assisted correction method achieves an accurate estimate of the pedestrian heading angle by fusing geomagnetic and gyroscopic measurements, as in the literature [6] , and the well-known Mahony algorithm [7] and Madgwick algorithm [8] .
In addition to the three methods mentioned above alone for heading error correction, the combination correction scheme of multiple methods is increasingly favored by researchers. Xu Yulong et al. [9] incorporated a zero-angle velocity correction method based on the heuristic correction method, and the positioning error could be within 2%. Yu Pei et al. [10] used heuristic correction when the pedestrian moved in a straight line and geomagnetic-assisted correction when the pedestrian moved in a curve based on zero angular velocity correction, with an optimal positioning error of up to 0.3%. Li Dongyang et al. [11] also used a combination of zero-angle velocity correction and geomagnetic assisted correction, and added geomagnetic interference detection on top of this, with a positioning error of up to 0.8%.
After a comprehensive analysis, this paper proposes a two-axis geomagnetic heading error spin correction method for autonomous pedestrian positioning, which first calibrates the geomagnetic measurement error, then uses only the calibrated two-axis geomagnetic measurement information to solve the pedestrian heading information, and finally combines motion period division and step estimation to achieve autonomous pedestrian positioning. Unlike the existing geomagnetic ellipsoid calibration algorithm [12] , this paper constructs a pedestrian spin geomagnetic calibration mechanism that enables the correction of biaxial geomagnetic measurement errors at any time, at any place and in any environment.
2. Conventional Pedestrian Dead Reckoning Framework
The traditional pedestrian heading projection mainly uses the body-worn gyroscope and accelerometer to collect the angular and linear motion information of the human body. After three steps of stride detection, heading solution and step estimation, the human body position at the current moment is obtained recursively based on the position at the previous moment, as shown in Figure 1.
2.1. Stride Detection
Due to the periodic nature of human movement, such as walking and running, the model values of tri-axial acceleration will also show periodic characteristics, so that stride detection can be achieved by counting the period of tri-axial acceleration modal values, the main detection methods are over-zero method, peak method, and correlation method.
2.2. Heading Solutions
The heading solution can be divided into inertial and geomagnetic solution algorithms. The inertial solution algorithm mainly uses the angular velocity information measured by the gyroscope to realize the real-time update of heading information by approximating the quaternion differential equation or equivalent rotation vector differential equation. The process of attitude update using the equivalent rotation vector method is divided into three main steps: 1) solving the equivalent rotation vector using the angular increment; 2) solving the change quaternion using the equivalent rotation vector; 3) updating the attitude quaternion using the change quaternion to achieve attitude update.
The equivalent rotation vector of the form “single subsample + previous period subsample” is given by:
(1)
where,
denotes the rotation vector,
denotes the angular increment at the current sampling moment,
denotes the angular increment of the previous sampling moment.
Figure 1. Conventional pedestrian dead reckoning framework.
The attitude change quaternion solution formula is
(2)
Attitude quaternion update:
(3)
The solution of the heading angle can be achieved by converting the attitude quaternion to an Eulerian angle.
Based on the geomagnetic sensor mounting axes shown in Figure 1, the following equation can be used to directly solve for heading angle information.
(4)
where,
denotes the heading angle,
denotes the geomagnetic measurements in the y-axis in the carrier coordinate system,
denotes the geomagnetic measurements in the z-axis in the carrier coordinate system.
2.3. Step-Length Estimation
The step length information of pedestrian movement is mainly estimated using acceleration. Commonly used step length estimation models are constant value method, linear model method, non-linear model method and neural network method. In this paper, the linear model method will be used for the estimation of pedestrian step length, as shown in the following equation:
(5)
where, L denotes the step length,
is the maximum value of the modulus of acceleration in the three axes,
is the minimum value of the modal value of the triaxial acceleration, k is empirical constant, obtained by pre-fitting.
3. Two-Axis Geomagnetic Heading Error Correction Algorithm
Due to the cumulative errors in the inertial attitude solution method, the long-time attitude accuracy is low; the geomagnetic attitude solution method is easily disturbed by the external environment and the stability is poor. In order to achieve reliable long-time pedestrian heading information acquisition, this paper proposes a two-axis geomagnetic heading error spin correction method, which can significantly improve the accuracy and stability of geomagnetic attitude solution by compensating external geomagnetic interference through the active rotation of the human body.
According to the literature [12] , the geomagnetic sensor output is modeled as:
(6)
where,
is the actual output of an uncalibrated geomagnetic sensor,
is the true magnetic field vector,
is a scaling factor,
is the non-orthogonal error matrix,
is soft magnetic interference,
is hard magnetic interference,
is zero bias error,
is random noise.
Expanding the geomagnetic sensor output model above as:
(7)
(8)
(9)
Random noise can be eliminated by averaging multiple measurement, thus the simplified geomagnetic sensor output model is:
(10)
The error compensation model for the geomagnetic sensor can be obtained from the above equation as:
(11)
The ellipse is fitted using the collected two-axis geomagnetic data, and the general form of the ellipse is:
(12)
(13)
Once the elliptic parameter vector is obtained after the above equation, the elliptic equation can be obtained according to Equation (13). The elliptic equation can be rewritten in the following form:
(14)
(15)
By comparing Equations (12) and (14), it follows that
(16)
4. Autonomous Pedestrian Positioning Method with Self-Correction of Heading Errors
Based on the above sections, this paper uses a two-axis geomagnetic sensor and a three-axis acceleration sensor mounted on the human chest or waist for autonomous pedestrian positioning, and proposes an autonomous pedestrian positioning method with self-correction of heading error, the overall structure of which is shown in Figure 2.
Figure 2. Architecture for autonomous pedestrian positioning methods with self-correction of heading errors.
The steps of the pedestrian autonomous positioning method proposed in this paper are as follows:
1) Adopt a two-axis geomagnetic heading error spin correction algorithm for the compensation of geomagnetic error, i.e. the positioner needs to spin in place for 3 to 5 revolutions, and use the collected two-axis geomagnetic sensor data to fit the elliptical error compensation coefficient for the compensation of geomagnetic measurement error.
2) Filter and noise reduction and mode-taking processing of the three-axis acceleration data collected from the human body. The collected tri-axial acceleration data is filtered and noise-reduced, mode-taking processed, and the step detection is performed using the peak method, and the step length information is estimated using a linear model.
3) The compensated dual-axis geomagnetic data is used to calculate the pedestrian heading information.
4) The pedestrian position information at the current moment is derived from the pedestrian position information at the previous moment, combined with the pedestrian heading information and the step length information.
5. Experiment
To verify the effectiveness of the algorithm proposed in this paper, several sets of experiments on autonomous pedestrian positioning were conducted using the inertial/geomagnetic measurement system Ellipse-N developed by the French company SBG. The index parameters of the Ellipse-N inertial/geomagnetic measurement system are shown in Table 1, and the human wearing method is shown in Figure 3.
The tester wore the inertial/geomagnetic measurement system on the chest in the manner of Figure 3 and walked the route indicated in Figure 4 for a total distance of 150 meters. In this case, the tester rotates in situ 3 times before the movement, which is used to correct for geomagnetic interference errors.
During the experiment, the inertial/geomagnetic measurement system was used to collect real-time three-axis acceleration, three-axis angular velocity and three-axis geomagnetic field strength information of human motion with a sampling frequency of 100 Hz. The acceleration, angular velocity, and geomagnetic field strength data of one set of pedestrian autonomous positioning experiments are shown in Figures 5-7.
Figure 3. Inertial/geomagnetic measurement system human body wearing method.
Figure 4. Pedestrian autonomous positioning experimental movement route.
Figure 5. Three-axis motion acceleration data.
Table 1. Ellipse-N inertial/geomagnetic measurement system index parameters.
Figure 6. Tri-axis motion angular velocity data.
Figure 7. Tri-axis geomagnetic field strength data.
First, according to the collected tri-axial acceleration information for personnel stride detection and step estimation, this paper uses the peak method for stride detection, i.e., detecting the local extreme values of tri-axial acceleration mode values, in order to eliminate the influence of high frequency noise, a low-pass filter with a cut-off frequency of 5 Hz is used to filter the tri-axial acceleration data, and the results of stride detection for some time periods are shown in Figure 8.
From Figure 8, there are more anomalies in the peak method stride detection. In order to filter out these anomalies, two constraints are set in this paper: 1) the peak must be greater than 11 m/s2; 2) the time interval between adjacent peak points must be greater than 0.4 s.
After completing the cross-step detection, a linear step model was used to estimate the person step based on the extreme value of the accelerometer modulus, and the coefficient K of the linear model obtained from the pre-fitting was 0.052.
Based on the collected acceleration, angular velocity and geomagnetic field strength information, the motion heading angle is calculated using the algorithm
Figure 8. Results of peak points detection.
proposed in this paper, the rotation vector algorithm, the heuristic heading algorithm, the geomagnetic heading solution algorithm, the Madgwick algorithm and the Mahony algorithm, where the rotation vector algorithm adopts the “single subsample + previous period subsample”. The Madgwick algorithm has a parameter α of 0.005 and the Mahony algorithm has a parameter β of 0.03.
The algorithm proposed in this paper requires the tester to actively rotate prior to movement for geomagnetic measurement error compensation. The biaxial geomagnetic data during active rotation and the rotation-corrected biaxial geomagnetic data are shown in Figure 9.
It is clear that the spin correction has suppressed the effect of hard magnetic interference on the accuracy of the magnetic field measurement. In addition, the flatness of the ellipse indicates the interference of soft magnetic errors, however, the flatness of the biaxial geomagnetic fitted ellipse does not change significantly before and after the correction, indicating that the geomagnetic measurement errors exist currently are mainly hard magnetic errors. Therefore, the spin correction algorithm in this paper can better suppress the influence of external errors on the accuracy of geomagnetic measurement, to obtain accurate personnel heading information.
The heading information obtained by using the algorithm proposed in this paper, the rotation vector binary-like algorithm, the heuristic heading algorithm, the geomagnetic heading solution algorithm, the Madgwick algorithm and the Mahony algorithm are shown in Figure 10.
Due to the lack of absolute heading reference information, it is impossible to quantitatively evaluate the heading solution accuracy of various algorithms. In the following, the heading information obtained by the algorithms in this paper is used as a reference to calculate the average error of heading angle of the other five types of algorithms relative to the algorithms in this paper, as shown in Table 2, to achieve a quantitative assessment of the relative accuracy of each type of algorithm.
Figure 9. Results of dual-axis magnetometer correction.
Figure 10. Six algorithmic heading solution results.
Table 2. The evaluation of relative accuracy for heading.
Based on the heading angle information calculated by the six algorithms, the same step size model is used to recursively calculate the person’s movement trajectory information in combination with the peak method stride detection method, as shown in Figure 11.
Due to the lack of reference of absolute position and absolute heading, the deviation of the starting point and the ending point is used in this paper to evaluate the positioning accuracy of the algorithms. The total distance travelled in this experiment was 150 m. The deviations between the starting and ending points of the six algorithms are shown in Table 3.
Figure 11. Results of motion trajectory during test.
Table 3. Six algorithms to locate track start and end point deviations.
According to the positioning results shown in Figure 11 and Table 3, the algorithms in this paper have higher positioning accuracy and better algorithm stability. The comparison results of the positioning accuracy of each type of algorithm are consistent with the comparison results of the heading solution accuracy in Table 2. In the absence of any correction measures, the geomagnetic measurement accuracy is easily disturbed by the external environment, causing large heading and positioning errors. Since both the Madgwick and Mahony algorithms solve the heading angle by fusing angular velocity and geomagnetic information, they are also susceptible to the influence of external geomagnetic interference. In this paper, by introducing a spin correction mechanism to correct the geomagnetic measurement information, the interference from the external environment is suppressed and the error accumulation problem of inertial heading solution does not exist, thus providing higher heading solution accuracy and positioning accuracy.
6. Conclusion
To address the heading error dispersion problem of autonomous pedestrian positioning, this paper proposes a two-axis geomagnetic heading error spin correction method, which achieves effective suppression of external geomagnetic interference by introducing active rotation before positioning, and then directly solves to obtain accurate heading information, and combines stride detection and step estimation to obtain the position information of the person. By comparing with five mainstream algorithms, the positioning accuracy of this algorithm can reach 0.95% of the total travel. The comparison experiments show that the algorithm has better geomagnetic interference suppression performance and no heading error accumulation problem, which has great potential for solving the positioning orientation problem in a satellite denial environment. In future research work, the fusion of complex step estimation methods will be considered to reduce the impact of step estimation errors on pedestrian autonomous positioning accuracy.
Acknowledgements
This work was funded by the Science and Technology Project of State Grid Shanxi Electric Power Company, “Research and application of key technologies of elastic fusion security operation in digital region based on 5G and deep learning”, 5205C0220001.