sensors Article
An Adaptive Zero Velocity Detection Algorithm Based on Multi-Sensor Fusion for a Pedestrian Navigation System Ming Ma *, Qian Song, Yang Gu, Yanghuan Li and Zhimin Zhou School of Electronic Science, National University of Defense Technology, Changsha 410073, China;
[email protected] (Q.S.);
[email protected] (Y.G.);
[email protected] (Y.L.);
[email protected] (Z.Z.) * Correspondence:
[email protected]; Tel.: +86-156-1619-5140 Received: 30 August 2018; Accepted: 26 September 2018; Published: 28 September 2018
Abstract: The zero velocity update (ZUPT) algorithm is an effective way to suppress the error growth for a foot-mounted pedestrian navigation system. To make ZUPT work properly, it is necessary to detect zero velocity intervals correctly. Existing zero velocity detection methods cannot provide good performance at high gait speeds or stair climbing. An adaptive zero velocity detection approach based on multi-sensor fusion is proposed in this paper. The measurements of an accelerometer, gyroscope and pressure sensor were employed to construct a zero-velocity detector. Then, the adaptive threshold was proposed to improve the accuracy of the detector under various motion modes. In addition, to eliminate the height drift, a stairs recognition method was developed to distinguish staircase movement from level walking. Detection performance was examined with experimental data collected at varying motion modes in real scenarios. The experimental results indicate that the proposed method can correctly detect zero velocity intervals under various motion modes. Keywords: pedestrian navigation system; ZUPT; zero velocity detection; adaptive threshold; stairs recognition
1. Introduction With the rapid development of micro-electro-mechanical system (MEMS) technology, MEMS inertial sensor-based pedestrian navigation systems play an important role for personal positioning indoors [1–6]. However, low-cost inertial sensors suffer from sensor drift and due to the integrative nature of the inertial navigation systems (INS), any small bias will accumulate and grow with time boundlessly [7,8]. Fortunately, a foot-mounted INS aided by the zero velocity update (ZUPT) technique has shown its ability to suppress navigation errors [9–13]. The performance of the ZUPT technique highly relies on the accuracy of zero velocity detection. A range of detectors that detect the zero velocity intervals from the output of the accelerometers or gyroscopes have been proposed [14–16]. In Reference [17], the zero velocity detection was formalized as a binary hypothesis testing problem and four likelihood ratio test (LRT) detectors were proposed. The detectors provided good performance at slow speeds (approximately 3 kph) and normal speeds (approximately 5 kph). The methods mentioned above are all threshold-based. These threshold values could differ significantly when a person is walking and running. The outputs of inertial sensors tend to become large and the zero velocity intervals tend to become shorter when a person is running. If the predefined threshold values are too small, then the zero velocity intervals cannot be detected when a person is running. To solve this problem, extensive research has been performed. In Reference [18], an adaptive stance-phase detection method was proposed. In this method, an additional accelerometer was
Sensors 2018, 18, 3261; doi:10.3390/s18103261
www.mdpi.com/journal/sensors
Sensors 2018, 18, 3261
2 of 16
Sensors 2018, 18, x FOR PEER REVIEW 2 of 16 attached to the chest and the difference of maximum acceleration change extracted from the chest acceleration was adopted to update the corresponding threshold for zero velocity detection. attached to the chest and the difference of maximum acceleration change extracted from the chest Inacceleration Reference [19], magnitude peakthe of y-axis gyroscope was used update the threshold wasthe adopted to update corresponding threshold fortozero velocity detection.based In onReference the pre-defined threshold function. In Reference [20], an algorithm based on a Markov [19], the magnitude peak of y-axis gyroscope was used to update the threshold basedmodel on was onlyfunction. uses theInsegmentation ofan the y-axis gyroscope of the thedeveloped pre-definedwhich threshold Reference [20], algorithm based on aoutputs Markovinstead model was three-axis output. in References showed good performance under walking developed whichThe onlymethod uses theused segmentation of the[18–20] y-axis gyroscope outputs instead of the three-axis and running modes. However, none of these methods explored zero velocity detection when a output. The method used in References [18–20] showed good performance under walking person and ascends or modes. descends stairs. Innone Reference [21], a detection method constructed using thea variations running However, of these methods explored zerowas velocity detection when person in ascends speed during a gait cycle. This method well under stair descending and ascending or descends stairs. In Reference [21], performed a detection method was constructed using the variations in speed a gait cycle. Thismethod methodwas performed stair descending and ascending modes. Theduring main limitation of this that thewell zerounder velocity detection of previous steps can modes. The main limitation of this method was that the zero velocity detection of previous steps can affect the detection of current step. Extra non-inertial sensors have been used to aid zero velocity affect thesuch detection of current step. sensors[22] have been used to aid zerosensors velocity detection, as magnetometers [7], Extra radio non-inertial frequency sensors and electromyography [23]. detection, suchextra as magnetometers radio frequency [22]these and electromyography [23].and Although using sensors might[7], improve detectionsensors accuracy, methods require sensors expensive Although equipment. using extra sensors might improve detection accuracy, these methods require expensive specialized andInspecialized this paper,equipment. we propose a novel zero velocity detection method based on multi-sensor fusion. In this paper, weaccelerometer, propose a novel zero velocity detectionsensor method based on multi-sensor The measurements of gyroscope and pressure were combined to detectfusion. the zero The measurements of accelerometer, gyroscope and pressure sensor were combined to detect the zero velocity intervals. Then, an adaptive threshold and stairs recognition were developed to improve the velocity intervals. Then, an adaptive threshold and stairs recognition were developed to improve the accuracy and reliability of detection. The new detector provided good performance under various accuracy and reliability of detection. The new detector provided good performance under various motion modes. motion modes.
2. Zero Velocity Detection 2. Zero Velocity Detection 2.1. Problem Description 2.1. Problem Description In most methods, detection thresholds must be assigned prior to detection. These thresholds In most methods, detection thresholds must be assigned prior to detection. These thresholds differ significantly under various motion modes, such as walking, running and ascending stairs. If the differ significantly under various motion modes, such as walking, running and ascending stairs. If threshold values are too large, false zero velocity intervals could be detected. Conversely, if we choose the threshold values are too large, false zero velocity intervals could be detected. Conversely, if we the threshold to be too small, the zero velocity intervals cannot be detected when a pedestrian is choose the threshold to be too small, the zero velocity intervals cannot be detected when a pedestrian running. We We show thisthis in in Figure 1. 1.InIngeneral, ofthe thestance stancehypothesis hypothesis optimal is running. show Figure general, the the performance performance of optimal detection (SHOE) algorithm in Reference [17] is relatively better than many other methods. Thus, detection (SHOE) algorithm in Reference [17] is relatively better than many other methods. Thus, we weused usedthis thismethod methodtotoverify verify that it is difficult to find a widely applicable threshold. Figure 1a depicts that it is difficult to find a widely applicable threshold. Figure 1a depicts that although the SHOE method performs wellwell when the person is walking, onlyonly half of theofzero that although the SHOE method performs when the person is walking, half the velocity zero intervals are detected during running with a small threshold. Figure 1b shows that the SHOE method velocity intervals are detected during running with a small threshold. Figure 1b shows that the SHOEcan detect most of detect the zero velocity of running using large threshold. However, it produces method can most of the intervals zero velocity intervals of running using large threshold. However, false it detection forfalse walking and for running targets, which degrades the accuracy positioning. produces detection walking and running targets, which degradesofthe accuracy of positioning. 7
7
x 10
x 10 15
angular rate energy
angular rate energy
15
10
5
0
10
5
0
2
4
6 8 sample time[s]
(a)
10
12
14
2
4
6
8
10
12
14
sample time[s]
(b)
Figure1.1.The The zero zero velocity of of thethe SHOE method whenwhen a pedestrian is walking and Figure velocitydetection detectionresults results SHOE method a pedestrian is walking running. (a) (a) TheThe detection result using a small threshold. (b) (b) TheThe detection result using a large and running. detection result using a small threshold. detection result using a large threshold. The denotes stationary moving values indicate the threshold. The redred lineline denotes the the stationary statestate and and moving state.state. SmallSmall values indicate the moving moving state, while large values indicate the stationary state. state, while large values indicate the stationary state.
Sensors 2018, 18, x FOR PEER REVIEW
3 of 16
Sensors Sensors 2018, 2018, 18, 18, 3261 x FOR PEER REVIEW
of 16 16 33 of
To solve this problem, we propose a novel zero velocity detection approach based on the multisensor Besides accelerometers gyroscopes, we also adopted pressure sensors to multiassist Tofusion. solve this problem, we proposeand a novel zero velocity detection approach based on the To solve this problem, we propose a novel zero velocity detection approach based on the detection. Figure 2 shows the output of and the pressure sensor the person is walking and running. sensor fusion. Besides accelerometers gyroscopes, wewhen also adopted pressure sensors to assist multi-sensor fusion. Besides accelerometers and gyroscopes, we also adopted pressure sensors to As the output of 2pressure sensor is of almost independent the motion mode, we considered only detection. Figure shows the output the pressure sensorofwhen the person is walking and running. assist adetection. Figure 2toshows the output of the pressure sensor when the person is walking and using pressure velocity intervals atofthe of the research. However, As the output ofsensor pressure detect sensorzero is almost independent thebeginning motion mode, we considered only running. As the output of pressure sensor is almost independent of the motion mode, we considered the detection accuracy sensorintervals was notatsatisfactory. Theofresults of Reference [24] using a pressure sensoroftousing detectpressure zero velocity the beginning the research. However, only using a pressure sensor tooutperformed detect zero velocity intervalssensor-based at the beginning of theWe research. However, show that the SHOE detector the pressure detector. also conducted the detection accuracy of using pressure sensor was not satisfactory. The results of Reference [24] the detectiontoaccuracy of using pressure of sensor was not satisfactory. The results of along Reference [24] experiments compare the performance the detectors. The pedestrian walked a square show that the SHOE detector outperformed thetwo pressure sensor-based detector. We also conducted show that the SHOE detector outperformed the pressure sensor-based detector. We also conducted trajectory, 6m for eight loops. The experiment was repeated 10 timeswalked and 10along datasets were experiments tolong, compare the performance of the two detectors. The for pedestrian a square experiments todetection compare results the performance ofinthe two detectors. Thebe pedestrian walked along avelocity square collected. The are shown Figure 3 and it can observed that the zero trajectory, 6 m long, for eight loops. The experiment was repeated for 10 times and 10 datasets were trajectory,detected 6 m long, loops.sensor-based The experiment was repeated for 10 earlier times and 10 datasets were intervals byfor theeight pressure detector always than collected. The detection results are shown in Figure 3 and it can started be observed that thethose zerodetected velocity collected. The detection results are shown in Figure 3 and it can be observed that the zero velocity by the SHOE detector. The mean positioning errors of thealways SHOEstarted detector and the pressure sensorintervals detected by the pressure sensor-based detector earlier than those detected intervals detected by the pressure sensor-based detector alwaysindicate started earlier than thosethe detected by based detector are 0.8 and 1.5 m, respectively. These results that only using pressure by the SHOE detector. The mean positioning errors of the SHOE detector and the pressure sensorthe SHOE detector. The mean positioning errors of the SHOE detector and the pressure sensor-based sensor for detection is not choice. Hence, propose a detector based on multi-sensor based detector are 0.8 andan 1.5optimal m, respectively. Thesewe results indicate that only using the pressure detector arethe 0.8proposed and 1.5 m, respectively. These results indicate that only using the pressure sensor for fusion and detector is introduced as follows. sensor for detection is not an optimal choice. Hence, we propose a detector based on multi-sensor detection is not an optimal choice. Hence, we propose a detector based on multi-sensor fusion and the fusion and the proposed detector is introduced as follows. proposed detector is introduced as follows. 4
pressure sensor output zero velocity interval
3.54
pressure sensor output running zero velocity interval
walking 3.5 3
walking
running
2.53 2.5 2 1.52 1.5 1 0.51 0.5 0 0
1
2
3
4
5 6 sample time(s)
7
8
9
10
1
2
3
4
5 6 sample time(s)
7
8
9
10
0 0
Figure 2. The measurements of the pressure sensor during walking and running. Figure 2. 2. The The measurements measurements of of the the pressure pressure sensor Figure sensor during during walking walking and and running. running. 4 3.54
pressure sensor output pressure sensor only SHOE(accelerometer+gyro) pressure sensor output
3.5 3
pressure sensor only SHOE(accelerometer+gyro)
2.53 2.5 2 1.52 1.5 1 0.51 0.5 0 0 0
0
1
2 3 sample time(s)
4
5
1
2
4
5
3
sample time(s) Figure 3. 3. The The detection detection results results of zero zero velocity velocity intervals. intervals. Figure of
3. Thewith detection results of zero velocity intervals.and three-axis gyroscopes) Assume that the INSFigure assembled an IMU (three-axis accelerometers Assume that the INS assembled with an IMU (three-axis accelerometers and three-axis and a pressure sensor, has been installed into the heel of a foot. Let mk ∈ R7 denote the7output from the gyroscopes) and a pressure has been installed into(three-axis the heel of accelerometers a foot. Let mk ∈and denote Assume that thesensor. INSsensor, assembled with an IMU three-axis the IMU and pressure 7 h i output from and the IMU and pressure denote the gyroscopes) a pressure sensor, sensor. has been installed into ptheTheel of a foot. Let mk ∈ mk = mka mω (1) mk k T output from the IMU and pressure sensor. p ω a mk = mk mk mk (1) T p mk = mka mkω mk (1)
Sensors 2018, 18, 3261
4 of 16
3 where mka , mω k ∈ R denote the measured specific force vector and the angular rate vector at time k, ρ respectively. mk ∈ R represents the measurement of the pressure sensor. The purpose of the zero velocity detection is to decide whether the IMU is stationary or moving between the time instants + N −1 n and n + N − 1, given the measurement sequence qn = {mk }nk= . The false-alarm probability n (i.e., the probability of deciding that the IMU is stationary when it is not) should be kept low. This is because the effect of imposing a false zero velocity constraint on the positioning system can greatly deteriorate the positioning accuracy. Furthermore, as imposing the true zero velocity constraints is the key to eliminating cubic error accumulation, the probability of detecting the zero velocity intervals should be maximized given a certain false-alarm probability. Mathematically, the zero velocity detection problem can be formalized as a binary hypothesis testing problem. The two hypotheses, H0 and H1 , can be defined as follows.
H0 : IMU is non-stationary H1 : IMU is stationary
(2)
The performance of the detector is determined by the probability of detection ( PD = p( H0 | H1 ) ) and the false-alarm probability ( PFA = p( H1 | H0 ) ). Based on the Neyman–Pearson theorem, for a given PFA = α, decide on H1 to maximize PD if T (qn ) =
p(qn ; H1 ) >γ p(qn ; H0 )
(3)
p(qn ; H1 ) and p(qn ; H0 ) denote the probability density functions of the observations for the hypothesis H1 and H0 , respectively and the threshold γ is determined by PFA =
Z {qn :T (qn )>γ}
p(qn ; H0 )dqn = α
(4)
The test in Equation (3) is referred to as the likelihood ratio test (LRT). 2.2. Sensor and Signal Model The probability density functions of the observed data depend on the true signal and the sensor noise. Therefore, the signal and sensor model should be specified to find probability density functions. For IMU sensors, the IMU measurement can be described as follows. mk = s k + v k
(5)
where ρ
iT
ρ
iT
sk =
h
ska
sω k
sk
vk =
h
vka
vω k
vk
(6)
ρ
3 here ska ∈ R3 , sω k ∈ R and sk ∈ R denote the IMU-experienced acceleration, the angular rate ρ 3 and pressure at time k, respectively. Moreover,vka ∈ R3 , vω k ∈ R and vk ∈ R represent the measurement noise of the accelerometer, gyroscope and pressure sensor, respectively. Here, we assume that the measurement noise is independent, identically-distributed white Gaussian noise with the covariance matrix σa2 I3 03 03×1 σω2 I3 03×1 C = E{vk vkT } = 03 (7) 01×3 01×3 σρ2
where I3 denotes an identity matrix of three orders, 03 denotes a zero matrix of three orders and 03×1 (01×3 ) denotes a zero matrix of size 3 × 1(1 × 3). σa2 ∈ R and σρ2 ∈ R denote the noise variance of
Sensors 2018, 18, x FOR PEER REVIEW
5 of 16
variance of the accelerometer, gyroscope and pressure sensor, respectively. Ε { ⋅} and {⋅}T denote the expectation operation and transpose operation, respectively. SensorsUnder 2018, 18, the 3261 hypothesis
H 0 , modeling the signal in a consistent way is difficult. Under 5 ofthe 16
hypothesis H 1 , gravitation acceleration is the only specific force observed by the accelerometer and theaccelerometer, angular rate experienced by the gyroscope is zero. Moreover, of the pressure the gyroscope and pressure sensor, respectively. andmeasurements expectation {·}the {·} T denote the senor willand reach the maximum value ( ρ max ) when the IMU is stationary. In other words, for the two operation transpose operation, respectively. Under the hypothesis H0 , modeling the conditions signal in a consistent way is difficult. Under the hypothesis hypotheses, the signal fulfills the restraint H1 , gravitation acceleration is the only specifica force observed by the accelerometer and the angular H 0 : ∃k ∈ Ω n , s.t. sk ≠ gun or sωk ≠ 0 or skρ ' ≠ ρ max rate experienced by the gyroscope is zero. Moreover, the measurements of the pressure senor will (8) ' Hmax ∈ Ω n , the then ska is = gstationary. un and sωk =In0 other and skρwords, = ρ maxfor the two hypotheses, reach the maximum value (ρ IMU 1 : ∀)kwhen the signal fulfills the restraint conditions Here, un ∈ 3 , un = 1 and Ω n = { ∈ : n ≤ < N − 1} . Using the mathematical method in ρ0
a ω 6 = 0 or s 6 = ρ gun or sas Reference [17], we canHderive theΩproposed n , s.t. sk 6 =detector max 0 : ∃k ∈ k follows:k (8) ρ0 a H1 : ∀k ∈ Ωn , then sk = 2gun and sω = 0 and s k k = ρmax a 2 2 3 1 Ωn a= {` m 1 `
