步幅和建筑方向輔助的行人導航算法

于飛, 白紅美, 高偉, 趙博, 葉攀

(哈爾濱工程大學 自動化學院, 黑龍江 哈爾濱 150001)

?

步幅和建筑方向輔助的行人導航算法

于飛, 白紅美, 高偉, 趙博, 葉攀

(哈爾濱工程大學 自動化學院, 黑龍江 哈爾濱 150001)

摘要：針對零速校正過程中航向誤差觀測性差引起的定位精度低的問題，在零速校正的基礎上，采用行人步幅和建筑方向輔助的航向誤差校正算法，對系統原有觀測量擴維。根據行人運動狀態得到步幅航向和建筑方向，建立基于速度誤差和航向誤差的量測模型，利用卡爾曼濾波器實現濾波更新。并且研究了RTS平滑算法，對軌跡進行平滑以解決軌跡突變問題。采用荷蘭Xsens公司的MTi-G型IMU進行場地試驗，通過對比不同算法的定位誤差驗證算法的有效性。結果表明，當行人運動距離為70 m時，其最大定位誤差不超過3.4 m。

關鍵詞：IMU；零速校正；行人步幅；建筑方向；RTS平滑；定位誤差

行人導航系統主要用于行人在已知或未知環境下的自主導航和實時定位。當GPS信號丟失或強度較弱無法完成導航任務時，基于微機電系統(micro electro mechanical systems，MEMS)慣性器件的IMU可以為行人完成自主式導航的功能[1]。然而長時間導航時，IMU的誤差隨時間積累，導致導航精度降低[2]。為減小IMU引起的誤差，Christoph等采用視覺陀螺輔助的IMU系統[3]，Leppakoski等對Wi-Fi與IMU的組合系統進行了研究[4]。Delikostidis等分析了路標輔助的算法[5]。這些方法雖能提高導航精度，但均需提供外界信息，而且增加了系統成本。因此，研究在無外界信號情況下提高導航定位精度的算法顯得尤為重要。故提出基于行人步幅和建筑方向的輔助算法，并利用RTS平滑算法對軌跡進行平滑處理，解決軌跡突變問題。

1零速校正

當IMU固定于行人腳部時[6]，利用檢測到的零速能有效修正系統的導航參數誤差[7]。以速度誤差為觀測量時，航向誤差是通過北向與東向加速度和速度誤差相關聯，而北向和東向加速度理論值應為零，這使得航向誤差的可觀測性非常差[8]。高階IMU能夠提高系統性能，但會增加系統成本和尺寸，且誤差依然會累積。磁力計對應用環境有一定的要求。因此提出基于行人步幅和建筑方向的航向誤差校正算法，以減小行人導航的定位誤差。系統總體算法流程如圖1。

圖1　系統總體算法流程圖Fig. 1　The overall algorithm flowchart of system

首先利用IMU的輸出進行導航解算和零速檢測，當行人步態處于零速狀態時，應用零速校正和航向誤差校正算法，通過卡爾曼濾波器估計狀態向量，得到修正后的導航參數，同時保存濾波參數，利用RTS平滑算法得到平滑后的運動軌跡。

2航向誤差校正算法

航向誤差校正算法建立在行人步幅和建筑方向的基礎上。首先需要利用行人的運動狀態獲得步幅航向和建筑方向，然后對原有觀測量進行擴維，以速度誤差和航向誤差為觀測量推導系統模型，最后采用卡爾曼濾波器對導航參數進行估計，并利用估計結果修正導航參數。

2.1行人步幅和建筑方向分析

在行人運動過程中，每運動一步均會產生步幅航向和步長。步長定義為一步的距離，步幅航向定義為兩個連續步之間的角度[9]，如下

(1)

圖2　步幅航向示意圖Fig. 2　Schematic diagram of stride heading

在引入步幅航向之前，需要判斷行人是否已經走出一步。只有當行人已經邁出一步，才可以根據式(1)得到步幅航向。由于行人運動的靈活性較大，本文利用廣泛似然比檢測算法判斷行人的步態[10]，相對于傳統的閾值檢測算法，此方法充分利用了傳感器信息，并將零速檢測問題模型化為假設檢驗問題，可對快走、跑步等運動狀態進行準確檢測。表1給出了行人在正常、快走和跑步運動狀態的步態檢測結果，默認一步運動中只有一個零速區間，實驗共有34個真實零速區間。

表1　步態檢測結果

由表1可知，傳統算法只適用于正常步速情況，無法準確檢測快走和跑步時的零速情況。GLRT檢測算法則可對正常步速、快走和跑步等運動狀態進行準確的檢測，能夠檢測到較為完整的零速區間。

在零速檢測的基礎上，根據兩個相鄰的零速點即可計算行人步幅航向，計算得到的步幅航向將在下一步中用到[11]。零速檢測結果可用二進制表示，如圖3所示，“0”為非零速狀態，“1”為零速狀態[12]，圖中黑點表示每個零速區間的第一個零速點，即用于計算步幅航向的時刻點。

圖3　零速檢測結果Fig. 3　Result of zero velocity detection

建筑方向通過2D地圖獲取，利用MATLAB對圖像進行處理，根據追蹤到的像素點即可計算建筑方向的值[13]。

建筑方向的獲取方法為：首先利用Canny檢測算子檢測圖像上的邊緣；在邊緣檢測的基礎上，采用Hough變換檢測圖像上的直線；根據直線起始點和結束點的像素值，即可獲得建筑方向：

(2)

式中：φbm為獲得的建筑方向，δx、δy分別為起始點和結束點在x和y方向上的像素差。圖4給出了圖像處理得到的建筑方向。

圖4　建筑方向Fig. 4　Building heading

圖4中，黑色直線為檢測到的一條直線，直線的起始點和結束點分別已在圖中標注，將兩點的像素值代入式(2)，可得到該建筑方向為1.589 rad。

為得到更加準確的建筑方向φb，利用零速檢測點處的導航參數對其修正，零速點如圖4中的黑點所示，分析相鄰3個零速點的航向變化值：

(3)

式中：θk為采樣時刻k時的航向值，km-1為前一個零速點的采樣時刻，km-2為第1個零速點的采樣時刻，α和β分別為前2個采樣時刻對應航向的修正權值。當km-1和km-2時刻的濾波精度相同時，權值α和β相等。由式(3)可知，此時應取兩時刻航向的平均值與k時刻的航向值進行比較，即滿足α=β=1，故權值α和β應滿足：

(4)

當兩時刻的濾波精度不同時，其具體數值可根據對應時刻的濾波效果決定，即由km-1和km-2采樣時刻的濾波協方差矩陣Pkm-1和Pkm-2決定。若km-2時刻的航向濾波精度高，則對應權值大；若濾波精度低，則其權值小。km-1時刻亦然。所以權值和對應時刻的航向濾波精度值成反比：

(5)

(6)

由式(3)可得到相鄰3個零速點的航向變化值，利用此航向變化值可對建筑航向進行修正，得到修正后的建筑方向：

(7)

式中：φbm為由地圖得到的建筑方向，δθ為航向變化值，φb為修正后的建筑方向。

2.2系統模型

在分析步幅航向和建筑方向后，即可推導系統模型。首先分析系統的狀態空間模型，選取狀態向量x為

(8)

式中：p3×1為位置向量，v3×1為速度向量，a3×1為姿態向量，δw3×1為陀螺零偏向量，δf3×1為加速度計零偏向量。狀態轉移矩陣F(t)如下

(9)

(10)

式中：W(t)為系統白噪聲。

由融合零速校正和航向誤差校正算法[12]，得到系統的觀測模型：

(11)

(12)

由此得到量測矩陣H4×15：

(13)

式中：xk-1(9)為濾波更新得到的上一時刻的縱搖角，Vk為量測噪聲。各協方差矩陣如下所示：

濾波協方差陣：

(14)

系統噪聲協方差矩陣：

(15)

量測噪聲協方差矩陣：

(16)

其中

3RTS平滑算法

零速校正算法和航向誤差校正算法均以零速檢測為基礎，在修正區間能有效限制誤差增長，在非零速區間，誤差依然持續增長，故會產生軌跡突變現象。為避免急劇修正現象的產生，本文在行人每個單步運動中利用RTS平滑算法對軌跡進行平滑，以得到平滑的運動軌跡。

RTS平滑算法包括前向濾波和后向平滑兩部分。本文中前向濾波采用卡爾曼濾波，在濾波過程中，保存每個時刻的狀態向量Xk、估計均方誤差陣Pk、一步預測均方誤差Pk,k-1以及狀態轉移矩陣Φk,k-1，保存值將在后向平滑過程中用到[14]。

前向濾波的卡爾曼濾波算法為

后向平滑過程按k=N,N-1,…1逆時針方向遞推解算得到平滑估計結果。后向平滑算法為

4試驗驗證

為驗證算法的有效性，利用荷蘭Xsens公司的MTi-G型IMU進行場地試驗。實物如圖5所示，其內部集成了三軸MEMS加速度計、陀螺和磁力計。其中，加速度計量程為±5.23rad/s，陀螺量程為±50m/s2，滿足試驗要求。

圖5　IMU實物圖Fig. 5　Figure of the IMU

將IMU固定于鞋上，盡量與水平地面保持平行。利用自帶數據采集軟件XSENSMTManager對傳感器數據進行實時采集和存儲，并可根據需要導出十進制或二進制的MEMS傳感器數據。系統啟動時，需對系統的采樣頻率、輸出數據格式等參數進行設置。

試驗時波特率設置為115 200，數據采樣頻率為100Hz，傳感器輸出為標定后的數據，可直接用于導航解算。步行試驗持續8min左右，全程大約70m，試驗開始和結束的地點在同一位置，以確保得到返回時的位置誤差。場地試驗在哈爾濱工程大學軍工操場內進行，如圖6所示，其中箭頭代表開始點，圓點繪制的線路為試驗時行人的運動軌跡。

圖6　試驗場地圖Fig. 6　Map of the test field

數據采集完成后，進行離線分析。單獨采用INS算法得到的航向如圖7所示。由圖7可知，單獨采用INS算法得到的航向波動較大，且呈發散趨勢，需進行進一步修正。采用ZUPT+航向誤差校正算法與單獨采用ZUPT的航向對比圖如圖8所示。

圖7　INS航向Fig. 7　The heading of INS algorithm

圖8　航向對比圖Fig. 8　The comparison chart of heading

由圖8可以看出，采用航向誤差校正算法能夠有效減小航向誤差。在前200s內，零速校正算法能使航向保持在一定精度范圍內。但在200s之后，隨著時間的推移，航向誤差逐漸增大。表2給出了航向誤差的具體數值，表中數據為各時間段內的最大航向誤差值。

表2　200 s后的航向誤差對比結果

由表2可以看出，單獨采用零速校正算法時的航向誤差最大可達到0.262 rad，而且在各時間段內的誤差均超過0.174 rad；采用航向誤差校正算法后的最大誤差減小為0.163 rad，各時間段內的誤差最小為0.060 rad。由此可見，航向誤差校正算法能有效提高航向的精度。

采用零速校正和航向誤差校正算法得到的行人運動軌跡如圖9所示。

圖9　行人運動軌跡Fig. 9　The trajectory for pedestrian

由圖9和圖6對比可以看出，此算法解算得到的行人運動軌跡與真實運動軌跡一致，故此算法能完成對行人的自主導航和定位功能。為更好的分析定位結果，以IMU/GPS組合系統得到定位結果為參考，進一步量化分析行人的定位誤差，行人運動過程中的定位誤差如圖10所示，誤差的具體數值見表3。

圖10　行人定位誤差Fig. 10　The positioning error for pedestrian

誤差類型誤差/m占總距離百分比/%最大誤差3.384.82平均誤差1.211.72返回誤差1.051.51

由圖10和表3可以看出，在300～400 s，行人的位置誤差達到最大值，為3.38 m。在整個運動過程中，其位置誤差不超過運動總距離的5%，并且其位置誤差不隨時間累積，能夠限制在一定范圍內。由此可見，采用航向誤差校正算法能有效減小行人的定位誤差。利用RTS平滑算法后的行人運動軌跡對比圖如圖11所示。

圖11　平滑運動軌跡對比圖Fig. 11　The comparison chart of smoothed trajectory

由圖11可以看出，和平滑前的軌跡相比，平滑后的軌跡較為平滑，不存在軌跡突變現象。在保證定位精度的同時，采用RTS平滑算法能有效解決軌跡突變問題。

5結論

本文在零速校正的基礎上，對步幅和建筑方向輔助的航向誤差校正算法進行了研究，通過數據的離線分析可得到以下結論：

1)利用修正建筑方向和步幅航向之差作為觀測量對導航參數實現濾波更新，由航向誤差和定位誤差的結果對比可以看出，該方法的定位精度較傳統的零速校正算法有所提高；

2)在航向誤差校正算法的基礎上，采用RTS平滑算法對定位軌跡進行后向平滑，在保證了定位精度的同時，解決了只在零速區間內進行校正導致的軌跡突變問題；

3)此算法不需額外的信號源，這意味著不論試驗場地大小，系統的成本不會增加，但其不足在于校正算法依賴于零速點的準確檢測，當行人處于非靜止狀態時，誤差仍然是累積的，未來的工作主要在于解決運動狀態下誤差累積的問題。

參考文獻：

[1]周啟帆, 張海, 常艷紅, 等. 基于MIMU的行人導航算法研究[J]. 系統仿真技術, 2010, 6(4): 281-286.

ZHOU Qifan, ZHANG Hai, CHANG Yanhong, et al. Research of pedestrian navigation algorithm based on MIMU[J]. System simulation technology, 2010, 6(4): 281-286.

[2]陳述奇, 劉青峰, 冉龍明, 等. 低成本MIMU/編碼器組合的高精度航姿系統[J]. 壓電與聲光, 2012, 34(3): 368-371.

CHEN Shuqi, LIU Qingfeng, RAN Longming, et al. Design and test of low-cost MIMU/encoder integrated precision attitude heading reference system[J]. Piezoelectrics & acoustooptics, 2012, 34(3): 368-371.

[3]KESSLER C, ASCHER C, FRIETSCH N, et al. Vision-based attitude estimation for indoor navigation using vanishing points and lines[C]//Position Location and Navigation Symposium (PLANS). Indian Wells, USA, 2010: 310-318.

[5]DELIKOSTIDIS I, ENGEL J, RETSIOS B, et al. Increasing the usability of pedestrian navigation interfaces by means of landmark visibility analysis[J]. Journal of navigation, 2013, 66(4): 523-537.

[6]張金亮, 秦永元, 梅春波. 基于MEMS慣性技術的鞋式個人導航系統[J]. 中國慣性技術學報, 2011, 19(3): 253-256.

ZHAGN Jinliang, QIN Yongyuan, MEI Chunbo. Shoe-mounted personal navigation system based on MEMS inertial technology[J]. Journal of Chinese inertial technology, 2011, 19(3): 253-256.

[7]PARK S K, SUH Y S. A zero velocity detection algorithm using inertial sensors for pedestrian navigation systems[J]. Sensors, 2010, 10(10): 9163-9178.

[8]GODHA S, LACHAPELLE G. Foot mounted inertial system for pedestrian navigation[J]. Measurement science and technology, 2008, 19(7): 075202.

[9]FENG Wenquan, ZHAO Hongbo, ZHAO Qi, et al. Integration of GPS and low cost INS for pedestrian navigation aided by building layout[J]. Chinese journal of aeronautics, 2013, 26(5): 1283-1289.

[10]SKOG I, HANDEL P, NILSSON J O, et al. Zero-velocity detection-An algorithm evaluation[J]. IEEE Transactions on biomedical engineering, 2010, 57(11): 2657-2666.

[11]JIMéNEZ A R, SECO F, ZAMPELLA F, et al. Improved heuristic drift elimination with magnetically-aided dominant directions (MiHDE) for pedestrian navigation in complex buildings[J]. Journal of location based services, 2012, 6(3): 186-210.

[12]JIMENEZ A R, SECO F, PRIETO J C, et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU[C]//Proceedings of the 7th Workshop on Positioning Navigation and Communication. Dresden, Germany, 2010: 135-143.

[13]ABDULRAHIM K, HIDE C, MOORE T, et al. Aiding MEMS IMU with building heading for indoor pedestrian navigation[C]//Proceedings of Ubiquitous Positioning Indoor Navigation and Location Based Service(UPINLBS). Kirkkonummi, Finland, 2010: 1-6.

[14]宮曉琳, 張蓉, 房建成. 固定區間平滑算法及其在組合導航系統中的應用[J]. 中國慣性技術學報, 2012, 20(6): 687-693.

GONG Xiaolin, ZHANG Rong, FANG Jiancheng. Fixed-interval smoother and its applications in integrated navigation system[J]. Journal of Chinese inertial technology, 2012, 20(6): 687-693.

Stride and building heading aided algorithm for pedestrian navigation

YU Fei, BAI Hongmei, GAO Wei, ZHAO Bo, YE Pan

(College of Automation, Harbin Engineering University, Harbin 150001, China)

Abstract：In view of the low positioning accuracy achieved owing to poor observability of the cause of heading error during zero velocity updating (ZUPT), on the basis of ZUPT, we propose a heading aided algorithm based on pedestrian stride and the building direction to expand the dimensions of the observation. From the movement of a pedestrian, we obtained the stride direction and building heading. Then, we established a measurement model based on the velocity and heading errors, using a Kalman filter for the update. Furthermore, we investigated the Rauch Rung Streibel (RTS) smoothing algorithm, and smoothed the trajectory using the RTS smoother to solve the mutation problem. Finally, we conducted experiments to prove the algorithm's validity by using the MTi-G inertial measurement unit (IMU) of Xsens in the Netherlands, and verified the effectiveness of the algorithm by comparing its positioning error with those of different algorithms. The results show that the maximum positioning error does not exceed 3.4 m when the walking distance is 70 m.

Keywords：inertial measurement unit (IMU); zero velocity updating (ZUPT); pedestrian stride; building direction; RTS smoothing; positioning error

中圖分類號：V241.5

文獻標志碼：A

文章編號：1006-7043(2016)03-408-06

doi:10.11990/jheu.201412076

作者簡介：于飛(1974-), 男, 教授,博士生導師;通信作者：白紅美, E-mail:baihongmei_kit@163.com.

基金項目：國家自然科學基金資助項目(51379047).

收稿日期：2014-12-29.

網絡出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20151218.1049.002.html

網絡出版日期：2015-12-18.

白紅美(1989-), 女, 碩士研究生.