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

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

?

步幅和建筑方向輔助的行人導(dǎo)航算法

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

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

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

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

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

1零速校正

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

圖1　系統(tǒng)總體算法流程圖Fig. 1　The overall algorithm flowchart of system

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

2航向誤差校正算法

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

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

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

(1)

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

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

表1　步態(tài)檢測結(jié)果

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

在零速檢測的基礎(chǔ)上，根據(jù)兩個相鄰的零速點即可計算行人步幅航向，計算得到的步幅航向?qū)⒃谙乱徊街杏玫絒11]。零速檢測結(jié)果可用二進制表示，如圖3所示，“0”為非零速狀態(tài)，“1”為零速狀態(tài)[12]，圖中黑點表示每個零速區(qū)間的第一個零速點，即用于計算步幅航向的時刻點。

圖3　零速檢測結(jié)果Fig. 3　Result of zero velocity detection

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

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

(2)

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

圖4　建筑方向Fig. 4　Building heading

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

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

(3)

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

(4)

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

(5)

(6)

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

(7)

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

2.2系統(tǒng)模型

在分析步幅航向和建筑方向后，即可推導(dǎo)系統(tǒng)模型。首先分析系統(tǒng)的狀態(tài)空間模型，選取狀態(tài)向量x為

(8)

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

(9)

(10)

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

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

(11)

(12)

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

(13)

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

濾波協(xié)方差陣：

(14)

系統(tǒng)噪聲協(xié)方差矩陣：

(15)

量測噪聲協(xié)方差矩陣：

(16)

其中

3RTS平滑算法

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

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

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

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

4試驗驗證

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

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

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

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

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

數(shù)據(jù)采集完成后，進行離線分析。單獨采用INS算法得到的航向如圖7所示。由圖7可知，單獨采用INS算法得到的航向波動較大，且呈發(fā)散趨勢，需進行進一步修正。采用ZUPT+航向誤差校正算法與單獨采用ZUPT的航向?qū)Ρ葓D如圖8所示。

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

圖8　航向?qū)Ρ葓DFig. 8　The comparison chart of heading

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

表2　200 s后的航向誤差對比結(jié)果

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

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

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

由圖9和圖6對比可以看出，此算法解算得到的行人運動軌跡與真實運動軌跡一致，故此算法能完成對行人的自主導(dǎo)航和定位功能。為更好的分析定位結(jié)果，以IMU/GPS組合系統(tǒng)得到定位結(jié)果為參考，進一步量化分析行人的定位誤差，行人運動過程中的定位誤差如圖10所示，誤差的具體數(shù)值見表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%，并且其位置誤差不隨時間累積，能夠限制在一定范圍內(nèi)。由此可見，采用航向誤差校正算法能有效減小行人的定位誤差。利用RTS平滑算法后的行人運動軌跡對比圖如圖11所示。

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

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

5結(jié)論

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

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

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

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

參考文獻：

[1]周啟帆, 張海, 常艷紅, 等. 基于MIMU的行人導(dǎo)航算法研究[J]. 系統(tǒng)仿真技術(shù), 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/編碼器組合的高精度航姿系統(tǒng)[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慣性技術(shù)的鞋式個人導(dǎo)航系統(tǒng)[J]. 中國慣性技術(shù)學(xué)報, 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]宮曉琳, 張蓉, 房建成. 固定區(qū)間平滑算法及其在組合導(dǎo)航系統(tǒng)中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 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

文獻標(biāo)志碼：A

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

doi:10.11990/jheu.201412076

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

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

收稿日期：2014-12-29.

網(wǎng)絡(luò)出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20151218.1049.002.html

網(wǎng)絡(luò)出版日期：2015-12-18.

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