CC BY
262
УДК [528:629.78]+519.688
РАЗРАБОТКА АЛГОРИТМА И СИСТЕМЫ ДЛЯ НЕПРЕРЫВНОЙ INOOR/OUTDOOR НАВИГАЦИИ С ПОМОЩЬЮ ИНТЕГРАЦИИ НЕДОРОГОСТОЯЩИХ GNSS И MEMS-СЕНСОРОВ, КРЕПИМЫХ НА НОГЕ
Ян Цвинер
Университет прикладных наук Карлсруэ, Мольтке штрассе. 30, 76133 Карлсруэ, Германия, магистр, научный сотрудник, тел: +49 (0)7219252319, e-mail: [email protected]
Аксель Лоренц
Университет прикладных наук Карлсруэ, Мольтке штрассе. 30, 76133 Карлсруэ, Германия, научный сотрудник, тел: +49 (0)7219252320, e-mail: [email protected]
Райнер Ягер
Университет прикладных наук Карлсруэ, Мольтке штрассе. 30, 76133 Карлсруэ, Германия, директор Института геоматики, профессор, доктор-инженер, тел: +49 (0)721 925 2620, факс: +49 (0)721 925 2597, email: [email protected]
В данной статье описывается outdoor/indoor навигационная система, оснащенная инерциальными измерительными блоками (IMU) с MEMS-сенсорами, крепимыми к ноге, ГНСС приемником с относительным положением антенны. Система может работать в течение длительного периода времени без GNSS сигнала и за счет нулевой скорости и алгоритмов обнаружения нулевого вращения. Представляемая система была разработана авторами в Университете прикладных наук Карлсруэ (HSKA). Далее описывается принцип действия системы, ее сравнение с IMU смартфона (с такими же алгоритмами). Кроме того, предлагаются два подхода к увеличению производительности инерциальных навигационных систем: а) калибровка наклона гироскопа с обновлениями нулевого вращения и б) ограничение ошибки по высоте на плоских поверхностях посредством метода стабилизации высоты, что помогает при оценке наклона акселерометра, а также повышает точность позиционирования в плане.
Ключевые слова: коррекция по нулевой скорости, ZRU, Indoor, бортовая навигационная система, ГНСС, инерциальный измерительный блок, барометр, мультисенсорная навигация, плечо внутренней пары сил, самокалибровка.
ALGORITHM AND SYSTEM DEVELOPMENT FOR SEAMLESS OUT-/INDOOR NAVIGATION WITH LOWCOST GNSS AND FOOT-MOUNTED MEMS SENSORS
Jan Zwiener
University of Applied Sciences Karlsruhe, Germany, Moltkestr. 30, 76133 Karlsruhe, M.Sc., Research Associate, tel: +49 (0)7219252319, e-mail: [email protected]
Axel Lorenz
University of Applied Sciences Karlsruhe, Germany, Moltkestr. 30, 76133 Karlsruhe, B.Sc., Research Associate, tel: +49 (0)7219252320, e-mail: [email protected]
Reiner Jäger
University of Applied Sciences Karlsruhe, Germany, Moltkestr. 30, 76133 Karlsruhe, Prof. Dr.-Ing., Head of Geomatics Institute at IAF, tel: +49 (0)7219252620, e-mail: [email protected]
This article describes an outdoor/indoor navigation system based on foot-mounted MEMS inertial measurement units (IMU) combined with a GNSS receiver and a relative antenna position. The system can operate for longer periods without a GNSS signal due to the zero-velocity and zero-rotation detection algorithms. The presented system was developed at the Hochschule Karlsruhe by the authors. The performance of this system is shown. It is also compared to a smartphone IMU (with the same algorithms). Additionally two approaches are shown to increase the performance of foot-mounted inertial navigation systems: a) the calibration of the gyroscope bias with zero-rotation updates and b) limit the height error on flat surfaces with a height stabilization method, which helps in estimating the accelerometer bias and also improves the horizontal positioning accuracy.
Key words: ZUPT, ZRU, Indoor, IMU, GNSS, INS, barometer, multi-sensor navigation, lever arms, self-calibration.
Dieser Beitrag beschreibt ein Indoor-Navigationssystem auf Basis von MEMS Inertialsensoren (IMUs) am Fuß in Kombination mit einem GNSS Empfänger mit verorteter GNSS Antenne. Das System kann einen GNSS-Ausfall mit Hilfe von Stillstandserkennungsalgorithmen (Zero-Velocity Detection) überbrücken. Dabei wird zum einen ein System vorgestellt, das an der Hochschule Karlsruhe von den Autoren entwickelt wurde, zum anderen wird gezeigt, was mit den Sensoren aus einem handelsüblichen Smartphone möglich ist, da beide Systeme im Kern eine vergleichbare Konfiguration enthalten. Zusätzlich werden zwei Modelle gezeigt, um die Genauigkeit zu steigern. Zum einen über eine Null-Rotationserkennung (diese muss getrennt von der Stillstanderkennung gerechnet werden), zum anderen wird ein Höhenfixierungsalgorithmus vorgestellt, der innerhalb von Gebäuden den Höhenfehler limitiert, die BeschleunigungsmesserFehlerschätzung stützt und damit auch den horizontalen Fehler reduziert.
1. Introduction
While the estimation of the navigation state (position, velocity and orientation) with GNSS receivers and MEMS-based inertial sensors is not difficult, the outage of GNSS renders such a system unusable (at least for position and velocity) after a few hundred milliseconds or seconds (depending on the quality of the MEMS sensors). This problem can be mitigated with zero-velocity updates as shown by [3] with the help of foot-mounted IMUs. The detection of zero-velocity phases can be based on the IMU readings, see e. g. [7] for an evaluation of methods. In [1] it is shown that a dedicated pressure sensor (to detect a contact with the ground) inserted in the insole of a shoe in order to identify stance phases can outperform IMU-based zero-velocity detection algorithms. A combination of map-matching and particle-filtering can improve the solution further, as shown in [5]. This requires a geo-referenced model (figure 1). There are a couple of different sensors to install in a building to augment indoor navigation, e. g. infrared sensors, ultrasonic tags and crickets, RSSI systems (Received Signal Strength Information) and computer vision based systems (e. g. cameras with barcode-like markers); a good overview can be found in [6]. If a second IMU is mounted on the second shoe of the pedestrian, the accuracy can be increased by using a Kalman filter with inequality constraints, as demonstrated by [10].
In this article the mathematical model for an IMU/GNSS based system is outlined in section 2. Section 3 describes the sensors used in the indoor/outdoor test in section 4. A method to stabilize the height is presented in section 5 and our
implemented zero-rotation detection is shown in section 6. The system is also tested with a smartphone IMU (section 7) instead of the IMU from section 4.
Figure 1: Calculatedfoot-mounted IMU trajectory in a georeferenced 3D model of a Hochschule Karlsruhe building (building B)
2. Mathematical model
The core of the pedestrian navigation is a 15 state error-state Kalman filter with additional zero-velocity and zero-rotation detection algorithms. The system has the following state space model, as described in [11]:
d dt
/Ap\ Av 0
A\j7 . = 0
Aba \Ab J Is
I 0 0 0 0
0
\fn x] 0 0 0
0
-Rb 0 0 0
0 \ 0
-K 0 0 /
/AP\
Av A\j7
Aba
\Abra/
+ B -w
(1)
B -w =
/ 0 0 0
-Ri 0 0 0
0 -Ri 0 0
\ 0 0 I 0,
0 0 0 I /
n.
CO
nba VW
(2)
In equation (1) is Ap the position error (in the n-frame), Av the velocity error in the n-frame, A\j/ the three component attitude error (from body to n-frame) and the sensor
bias errors Aba,/ Abra.
The current estimated rotation matrix is the transformation
from the body-frame to the navigation-frame (n-frame). [fn x] is the skew symmetric matrix of the acceleration relative to free fall in the navigation frame (specific force or proper acceleration). The accelerometer and gyroscope bias errors are modeled as random-walk processes with equation (2). The accelerometer sensor noise is added with the vector na, the gyroscope sensor noise with the vector nra. The stochastic modeling of the IMU sensor errors is critical if the MEMS sensors have a significant bias, but this requires absolute measurements, such as barometric heights and/or GNSS measurements. Otherwise it can be beneficial for well-calibrated sensors to discard the bias modeling for indoor navigation scenarios, as shown in [8]. The accelerometer is used to stabilize roll and pitch during zero-velocity phases. The yaw angle could be provided by a magnetometer but in buildings the magnetometer is often unusable near the ground, see [2]. A magnetometer (combined with the WMM2010 model) was only used for an initial alignment in the indoor scenarios, otherwise GNSS was used to estimate the heading in the filter routines during acceleration periods with the system model in equation (1). The complete measurement equation model for the Kalman filter is described in [11]. Equation (1) and (2) are integrated over time (depending on the sampling rate) to form a complete Kalman filter.
Quite a few different approaches for the detection of zero-velocity epochs on foot-mounted IMUs can be found in literature such as in [3], [4] and [9]. In the described algorithm below several of these approaches are combined. The functionality reads as follows (1 = true) for every epoch at an epoch k: The norm of the gyroscope-measurement-vector ct)n0rm k is below a threshold tra :
= j^"' ®norm,k ^ t(D (3)
(o, otherwise
The norm of the accelerometer-measurement-vector anQrmk is in between two thresholds ta minand ta
max-
£2k = • ta_min < anormk < ta_max (4)
[0, otherwise
The standard deviation of the norm of the accelerometer measurement vector sa k is below a threshold aa (over n epochs, depending on the sampling rate of the sensor):
C3k = i1' °a (5)
(^0, otherwise
The standard deviation of the norm of the gyroscope measurement vector is below a threshold Co,:
C4k = j1' S®<k affl (6)
(^0, otherwise
For a zero-velocity update at a given epoch k, Clk to C4k must all be 1. If a zero-velocity epoch is detected, a pseudo-measurement is then applied in the filter which leads to corrections of the navigation solution.
3. Custom IMU
A custom INS/IMU was developed at the Hochschule Karlsruhe in cooperation with the company teXXmo GmbH (www.robinette.de). The custom INS can run the filter algorithms directly on-board or it can output the raw sensor data for the processing on e. g. a laptop (IMU mode). The exact hardware specs are listed in table 1. The algorithms run on a normal PC, but the same source code is also used on a microcontroller (MCU). For the following tests, the system was used in pure raw-data mode. The GNSS receiver emits GPS raw data (code-, phase- and Doppler measurements), so a differential GNSS solution can be calculated in post-processing with a GNSS reference station.
Table 1: Custom INS/IMU components.
MCU STM32F407 32-bit 168 MHz ARM Cortex-M4 with FPU
Gyroscope InvenSense ITG-3200 Gyroscope
Accelerometer Bosch BMA280 Accelerometer
Magnetometer Honeywell HMC5883L
GNSS receiver u-blox LEA 6-T
Real-time OS ChibiOS 2.6 stable branch
Barometer MS5611 Barometer
Figure 2 shows the custom INS/IMU mounted on a shoe. This setup was used to capture all the raw-data for the analysis in this article, except for the smartphone IMU section. A L1/L2 helix antenna is attached to the system (Antcom Corp. P/N: 321216P-XS-X).
Figure 2: Custom low-cost IMU with integrated GPS receiver and GPS helix antenna 4. Simulated GNSS Outtake for Seamless Navigation
To test the seamless indoor/outdoor capabilities of the navigation algorithms, an 11-minute walk around the Hochschule Karlsruhe campus was tracked with the custom foot-mounted IMU/GNSS system (see figure 2). The total walking distance was 1 km. During this time a single frequency GPS solution was available the whole time. For the seamless indoor/outdoor test, the GPS signal was removed from the processing after 120 seconds to simulate a GPS outage for 7 minutes. The initial two minutes of GPS information allows the filter to stabilize the accelerometer bias
estimation (see figure 4) and estimate the yaw angle. The advantage of the system is that the GPS antenna has a fixed lever arm relative to the IMU. Other systems mount the GPS antenna on the shoulder of the pedestrian [2]. With the fixed antenna configuration, the L1-GPS and MEMS combination works very well even without zero-velocity updates.
At the end of the outage, the position gets corrected by the L1 GPS position measurement by 8 m; during the outage, the trajectory was provided solely by the IMU and the zero-velocity/zero-rotation detection. The resulting trajectory is depicted in figure 2. The same trajectory is also shown in Google Earth in figure 3. Figure 3 also contains the estimated orientation as an orange arrow. It can also be seen that the yaw-angle gets corrected with the re-enabled GPS position.
Y [m] (East)
Figure 3: Trajectory with a seven-minute simulated GNSS outage
End GNSS outage
Figure 4: Same trajectory (from figure 3) visualized in Google Earth. Arrows indicate walking
direction (Hochschule Karlsruhe Campus)
5. Height stabilization methods
When no initial GNSS position is available, the pedestrian IMU navigation works well, except for the height channel, if the accelerometer has a significant bias. Figure 5 shows a 400 m trajectory where a pedestrian walks the same track eight times and stops exactly at the starting point. The 2D difference from the starting point is 70 cm, which is a relative error of about 0.18 %. But the height difference from the start is almost 10 m. To stabilize the height, barometer measurements are enabled for the same data set. The resulting trajectory can be seen in figure 6. This reduces the resulting height difference to about 1 m, and also improves the 2D error: 25 cm (relative error: 0.06 %).
Another possible way to stabilize the height is to make a very simple assumption about the ground surface: if the surface is flat (with the exception of stairs), the height from the previous step (Atstep seconds ago, i. e. h(t — Atstep)) should equal the height from the current step (height at time t: h(t)). The height difference from the current epoch to the last detected step is dh (dh = h(t) — h(t — Atstep)).
Ufrt _ ~ Atstep), dh< 5 cm
W)corrected - I ^ dh > S cm (3)
If dh is smaller than the defined threshold (here 5 cm), a virtual height measurement (with the height from the previous step) is added to the Kalman filter to
stabilize the height channel. If the difference is larger, no virtual measurement is added (the new height is accepted). This approach only works well if the surface is actually flat (e. g. strict indoor scenarios). Figure 7 shows this algorithm applied to the data-set from figure 5 and 6. No barometer is used in figure 7. The height error is even smaller: 32 cm and the 2D error is reduced to 12 cm (relative error 0.03 %). The tests are summarized in table 2. Another test with staircases was performed to see if the algorithm works in this case too. The resulting height profile is shown in figure 8 and it can be seen that the height fixing works in this case too. The requirement is that the stairs have a height difference of more than 5 cm. With better sensors, this threshold can be reduced. This algorithm can be tuned with the threshold and the standard deviation of the virtual measurement, so this can be seen as a soft-constraint in the Kalman filter. In the example data-set of figure 7, a standard deviation of 10 cm for the soft-constraint was used. In a mixed indoor/outdoor scenario, the standard deviation could be set to higher values like 1-2 m, but in this case a barometer works better.
Table 2: Height error of a 10-minute walk (uncalibrated acc.)
Height error (m) Relative 2D error Trajectory
No height stabilization 9.74 m
With barometer 0.92 m
Height stabilization algorithm 0.12 m
0.18 % 0.06 % 0.03 %
Figure 5 Figure 6 Figure 7
Trajectory O Starting point □ End point
X [m] (North)
Y [m] (East)
15
-15
Figure 5: Height divergence with an uncalibrated accelerometer
Figure 6: Height with an uncalibrated accelerometer, but with enabled barometer
Figure 7: Height divergence with flat surface assumption (without barometer)
Figure 8: Walking upstairs with the flat surface assumption (without
barometer)
6. Zero Rotation Detection
/
\/
-T ajec ory
Starting point End point
10 8 6 4
Î 2
0 z
1 0 * -2
-4
-6
0
Y [m] (East)
Y [m] (East)
Figure 9: Same data-set without ZRU
(left side),with ZRU (right side) - 10-minute walk
An important aspect is the detection of zero rotation epochs. During epochs with no rotation, artificial zero rotation updates (ZRU) can be performed (the Earth rotation can be removed in the calculations, but the Earth rotation is small compared to the capabilities of today's MEMS gyroscopes anyway). It should be noted that ZRU normally doesn't happen during walking, so only when the user/pedestrian has a stable stance position for a longer period of time (several hundred milliseconds) does a true ZRU make sense. False zero rotation updates can harm the gyroscope bias estimation severely. But if applied correctly, occasional ZRUs can improve the navigation solution by reinforcing the gyroscope bias estimation. This can be seen in figure 9, where on the left side a gyroscope with a bias of -0.003 rad/s in the Z-axis is used. Without ZRUs and only zero-velocity updates, the filter fails to correctly estimate the gyro bias along the local plumb-line (which is approximately the Z-axis in this example). The result is the drift in figure 9. If ZRUs are enabled, the gyro bias can be estimated after a few milliseconds along all three gyro axis and the resulting trajectory improves dramatically (see figure 9, right side), as well as the bias standard deviation (see table 3).
Table 3: Gyroscope bias estimation with and without ZRUs
_Bias Gyroscope (rad/s)_
Without ZRU (figure 9, left side) -0.008 -0.013 0.005 (±0.003) With ZRU (figure 9, right side) -0.015 -0.011 -0.003 (± 0.000045)
The mathematical model for the ZRU detection is decoupled from the ZUPT update routines. For the ZRU detection, only gyroscope data is analyzed and the variance is calculated (s£, with the redundancy ) over a time interval tZi?{/. The true variance (o^) of the respective sensor in rest has to be calculated in a controlled environment without external vibrations. The hypothesis can then be tested with the Fisher distribution:
8
6
5
0
Null hypothesis: E{s£} = o^
Alternative hypothesis: E{s£} > o^ Test value:
Critical value (with significance level a):
The larger variance has to be assigned to the index 1 for the critical value calculation. 7. Smartphone IMU
Most of today's smartphones come with a built-in IMU. This leads to the question of whether these low-cost devices can be used in indoor- and outdoor navigation scenarios. For this purpose a smartphone (Samsung Galaxy S3) is put in a case which is strapped with a hook-and-loop fastener to one shoe (see figure 10). In order to obtain the raw data of the IMU an app was written which records the raw sensor measurements for postprocessing. The frequency amounts to approx. 100 Hz.
With the smartphone IMU the same hallway-loop was walked 5 times, with each walk as a timely independent Figure 10: Smartphone mounted on shoe measurement (fixed start and end point). The resulting trajectories of these raw data measurements are shown in figure 11. The length of one such track was approx. 100 m long.
Table 4: Errors of the repeated time-independent walks depicted in figure 11 -average walking time approx. two minutes
run number Length [m] 2D error [m] 3D error[m] Relative 2D error [%]
1 98.33 220 292 224
2 105.22 2.75 3.98 2.62
3 105.20 4.86 4.87 4.62
4 104.90 3.18 3.50 3.04
5 102.90 3.18 3.18 3.09
As illustrated in table 4 the errors of these smartphone-walks are larger compared to dedicated IMUs (section 5), therefore a current uncalibrated internal smartphone IMU could be used for indoor navigation to bridge short distances (< 100 m). With a proper configuration (higher sampling rate, appropriate bandwidth filters) and a misalignment calibration the results would be of course better, but it is informative to see how such an IMU performs without these optimizations. It should be noted that the shape of this trajectory cancels out several systematic errors [8].
Figure 11: Repeated time-independent walks on the same track with a smartphone IMU - walking time approx. two minutes (errors in table 4)
8. SUMMARY
The concept and implementation of a low-cost indoor/outdoor pedestrian navigation system, developed at the Hochschule Karlsruhe has been shown. The current generation of MEMS and L1-GNSS sensors are more than capable of providing an accurate navigation state vector for many different applications (with the right algorithms). For example the live tracking of service personnel in a building (figure 12) is possible with this technology. The demonstrated INS-system can work in an autonomous way negating the need for additional techniques. It has been documented that the frequency bandwidth and noisiness of the IMU-measurements correlate with the time that the navigation solution is solid. A good zero-rotation detection can be essential if the gyroscope is uncalibrated and the bias is high. For
pure indoor scenarios with flat surfaces and stairs, the presented height stabilization method works reliably and increases the performance of the navigation system.
Figure 12: Calculated and visualized trajectory in a georeferenced 3D model of a Hochschule Karlsruhe building (building B)
9. ACKNOWLEDGEMENT
The funding of the joint research project "GNSS/INS and multi-sensor navigation algorithms and platforms for mobile navigation and object geo-referencing" by the Ministry of Economics and the navigation and mobile IT industry of Baden-Württemberg is gratefully acknowledged. The objective of the research team 'NAVKA' (www.navka.de) at the central research center at the Institute of Applied Research (IAF) at University of Applied Sciences Karlsruhe is to develop robust algorithms for multi-sensor GNSS and MEMS-based platforms for the navigation of arbitrary bodies and for mobile indoor and outdoor geo-referencing applications independent from extern infrastructure. The industrial partner IN GmbH has provided a software 3D model of the Hochschule Karlsruhe campus building B that is used for visualization (also in real-time with the Sphinx Open Online software).
REFERENCES
1. Bebek, O. et al. (2010): Personal navigation via high-resolution gait-corrected inertial measurement units. In: IEEE Transactions on Instrumentation and Measurement. Vol. 59/11.
2. Bird, J., Arden, D. (2011): Indoor Navigation with Foot-Mounted Strapdown Intertial
navigation and Magnetic Sensors. In: IEEE Wireless Communications Journal. Volume 18. 2011.
3. Foxlin, E. (2005): Pedestrian tracking with shoe-mounted inertial sensors. In: Computer Graphics and Applications, IEEE. Vol. 25.
4. Godha, S and Lachapelle, G. (2008): Foot mounted inertial system for pedestrian navigation. In: Measurement Science and Technology. Vol. 19/7. IOP Publishing.
5. Klingbeil, L., Romanovas, M. (2012): Rekursive Bayesische Schätzverfahren zur Lokalisierung von Personen innerhalb von Gebäuden. In: zfv 6/2012.
Publisher: DVW e.V. - Gesellschaft für Geodäsie, Geoinformation und Landmanagement.
6. Koyuncu, H., Yang, S. H. (2010): A Survey of Indoor Positioning and Object Locating Systems. In: IJCSNS International Journal of Computer Science and Network Security, Vol. 10/5, May 2010.
7. Nilsson, J., Skog, I., Händel, P. (2010): Evaluation of zero-velocity detectors for foot-mounted inertial navigation systems. Indoor Positioning and Indoor Navigation Conference (IPIN).
8. Nilsson, J., Skog, I., Händel, P. (2012): A note on the limitations of ZUPTs and the implications on sensor error modeling. Proceeding of 2012 International Conference on Indoor Positioning and Indoor Navigation (IPIN), 13-15th November 2012.
9. Park, S. K. and Suh, Y. S. (2010): A Zero Velocity Detection Algorithm Using Inertial Sensors for Pedestrian Navigation Systems. In: Sensors. Vol. 10/10. Publisher: Molecular Diversity Preservation International.
10. Skog, I., Nilsson, J., Zachariah, D. and Handel, P. (2012): Fusing the information from two navigation systems using an upper bound on their maximum spatial separation. Proceeding of 2012 International Conference on Indoor Positioning and Indoor Navigation (IPIN), 13-15th November 2012.
11. Wendel J. (2011): Integrierte Navigationssysteme: Sensordatenfusion, GPS und inertiale Navigation. Oldenbourg Verlag, München.
BIOGRAPHICAL NOTES
Prof. Dr.-Ing. Reiner Jäger
Professor for Satellite and Mathematical Geodesy, Adjustment and Software Development at Karlsruhe University of Applied Sciences (HSKA) Head of the Laboratory of GNSS and Navigation, of the Institute of Geomatics at IAF and of the joint RaD project "GNSS-supported low-cost multi-sensor systems for mobile platform navigation and object geo-referencing" (www.navka.de) Member FIG Commission 5 WG 5.4 Kinematic Measurements
CONTACTS
Jan Zwiener, M.Sc.
University of Applied Sciences - Institute of Applied Research (IAF) Moltkestraße 30 76133 Karlsruhe Germany
Tel. +49 (0)721 925-2586 Fax +49 (0)721 925-2927 Email: Jan.Zwiener@HS-Karl sruhe.de
Website: www.navka.de
© Jan Zwiener, Axel Lorenz, Reiner Jäger, 2014
