基于手機陀螺儀航向修正算法

郭 英，姬現磊,2，劉清華，李冠澤,2，徐 瑩

（1. 山東科技大學 測繪科學與工程學院，青島 266590；2. 中國測繪科學研究院，北京100830）

隨著智能手機技術和微機電系統（Micro Electro Mechanical System, MEMS）技術的高速發展，MEMS傳感器已成為手機的必備器件，并且智能手機已普及千家萬戶，這些都為基于手機慣性傳感器定位技術的應用提供了巨大的優勢[1]。基于步態檢測和航行估計（Step and Heading, SHS）的航跡推算系統（PedestrianDead Reckoning, PDR），通過分析、處理加速度數據和陀螺儀數據，獲取每一步的步長和航向信息，由起始位置逐步推算出實時定位信息[4,7,11]。

消除由陀螺儀的漂移誤差導致的航向誤差一直是阻礙室內定位精度提升的難點[9,12]，為了減少航向誤差，學者們提出了幾種航向修正的方法：Borenstein等人[3]提出了基于腳踏式慣性測試單元（Inertial Measure Unit, IMU）的啟發式偏移減少算法（Heuristic Drift Elimination Reduction, HDR）和啟發式漂移消除算法（Heuristic Drift Elimination, HDE），該算法建立一個反饋機制，通過反饋信息對陀螺儀數據進行修正，進而達到對航向角修正的目的；樓喜中等人[6]在HDE的基礎上，將航向角作為直接修改的對象，消除了部分陀螺儀積分誤差；趙輝等人[8]基于HDE算法提出了基于卡爾曼濾波和經驗參數公式的算法；Ju等人[2]提出了高級啟發式漂移消除算法（Advanced Heuristic Drift Elimination, AHDE），該算法把行走分為了三種狀態，在非直線行走時AHDE不啟用，在主導方向行走和直線但非主導方向行走時分別建立擴展卡爾曼濾波（Extended Kalman Filter, EKF）進行航向誤差修正；Jimenez等人[5]提出了改進的啟發式漂移消除算法（improved Heuristic Drift Elimination, iHDE），直接對航向角誤差進行估計，將方位角與主導方向的偏差作為觀測量，利用EKF對航向進行修正。

HDE和HDR算法適合簡單的行走路線，在非主導直線行走30~60 s后，HDE算法的效用使室內定位算法惡化，定位精度下降嚴重[2,10]。雖然AHDE和iHDE算法能適應一般的復雜路線，但都是基于 INS (Inertial Navigation System)+EKF+ZUPT (Zero velocity UPdate）框架（IEZ）下使用該算法，尤其是iHDE算法被建議使用在IEZ框架下[2]。以上四種算法的提出都是基于腳踏式IMU進行研究的，對于用戶來說，測量裝置置于腳下會對用戶日常行走帶來不舒適感，基于手機MEMS傳感器定位技術有著巨大的優勢條件。本文對iHDE算法進行了改進，將其應用在手機MEMS陀螺儀和基于SHS的PDR算法系統中，使算法更加簡單實用。

1 iHDE算法框架改進

在人造建筑中大多數走廊都是直的，并且彼此平行或正交，把建筑物墻壁和走廊的方向稱為建筑物的“主導方向”，超過99%的人造建筑有四個或八個主導方向[3]。啟發式消除算法就是依托在這種環境下的航向修正算法，如果計算出的航向角與最接近的主導航向相匹配，則實施算法進行航向矯正。

iHDE是由 Jimenez等人[5]提出的改進的 HDE算法。相對比HDE算法，iHDE不僅在室內簡易環境下效果突出，在一定的復雜環境下也表現了較好的效果，Jimenez建議iHDE算法用于IEZ算法框架中。IEZ算法是指基于腳踏式IMU的PDR算法，這種算法根據IMU置于鞋底的數據特性，加入了零速更新算法（ZUPT）來抑制誤差的積累。另外，該框架融入了零角速度更新（Zero Angular Rate Update, ZARU），當腳部運動時，對角速度進行零角速度矯正。最終應用包含15個元素誤差狀態向量的EKF對行人的位置和姿態做出估計，該向量中包含加速度誤差和陀螺儀誤差（分別為、、姿態誤差()、位置誤差()以及速度誤差()。

本文研究對象為智能手機，手機姿態多樣化（手持、放口袋、打電話等狀態），ZUPT算法和ZARU算法已不再適用，即IEZ框架在基于手機的行人定位研究中已不再適用。本文iHDE算法應用在SHS-PDR算法框架下，應用以航向與主航向的偏差為狀態量的卡爾曼濾波器（Kalman Filter，KF）對航向角做出估計，簡化了iHDE算法。改進后的iHDE算法框圖如圖1所示。該算法基于 SHS-PDR算法系統，通過三軸加速度數據和三軸陀螺儀數據獲取基本的步長信息和航向信息，再對航向信息進行分析及修正，最終由步長和修正后的航向信息獲得精確位置信息。

圖1 系統框架Fig.1 System architecture

2 改進算法實現

2.1 航向角解算

第1步：選用四元數法來解算航向信息、初始化四元數。

常用的姿態描述方法有歐拉角法、方向余弦法和四元數法。由于相對比其他兩個方法，四元數方法具有計算簡單，易于操作，且能避免出現歐拉角所產生的“奇點”問題的優點，故本文選擇四元數法來解算姿態信息。

初始姿態信息設置：航向角為90°，俯仰角和橫滾角設置都為0°。初始化四元數(,,,)公式如下：

式中：h為數據采樣間隔；q(t)、q(t+h)分別是t和t+h時刻的四元數；為

根據上述公式即可獲得實時的四元數更新。

第3步：四元數歸一化。

雖然選用簡便且精度高的航向角解算方法能獲取具有一定精度的航向信息，但陀螺儀的輸出隨著時間而產生隨機漂移誤差，這嚴重制約著航向角精度的提升[12]。

2.2 對航向角進行修正

本文對航向角信息設置了兩次判別分析，旨在準確地對行走狀態分類，避免出現不良校正情況。另外，應用 KF算法，狀態量只有航向與主導航向的差值，極大地簡化了iHDE算法。每當行人沿主導方向行走時，iHDE算法就會生效，對航向信息進行修正，從而抑制了漂移誤差，進而提高了定位的精度。具體算法步驟如下。

第1步：設定主導航向。

主方向可以設定為4個或8個主導方向，如圖2所示，本文研究以8個主導方向為研究基礎，主導方向的間隔date值為45°。

圖2 主導方向Fig.2 Dominant direction

第2步：辨別行走狀態。

選擇最接近的主導方向，計算行走航向與主導方向的差值：

即得行走航向與主導方向的差值。

第3步：航向角標準差求解。

第4步：卡爾曼濾波處理。

將行走航向與最近的主導方向的差值 δ θ（i）作為狀態量輸入卡爾曼濾波算法中，其預測方程為

計算誤差方差陣：

觀測方程為：

計算卡爾曼增益：

計算i時刻最優估值：

更新i時刻下X(i|i)的方差陣：

2.3 航跡推算

經iHDE算法修正過的航向角信息與加速度計分析所得的步長信息相結合，獲得精確的定位信息。所謂航跡推算，即由上一位置的坐標結合當前一步的航向角信息與步長信息求解當前位置的坐標，公式如下：

式中，x、y為位置坐標，S為步長。

3 實驗分析

3.1 數據采集

為驗證航向修正算法的有效性，共設計了5組實驗，實驗以魅藍NOTE3手機為研究對象。魅藍NOTE3是2016年4月份生產的手機，剛發售時的價格因機身內存不同設有799元和999元兩個價位，本實驗采用的是價位為799元的手機，該手機中有陀螺儀、加速度計、磁力計等基本傳感器。本文以陳春陽[13]設計的安卓軟件“導航項目”為工具來獲取傳感器數據，該軟件簡單實用，滿足實驗的需求。實驗數據采集頻率為60 Hz，在數據采集實驗中實驗者手持手機，并平放于身前（如圖3所示），以正常步速按設計的路線行走。

根據行走路線的復雜程度，可分為常規路線實驗和挑戰性路線實驗。

常規路線實驗是行走路線以主導方向行走為主，實驗場地為山東科技大學測繪學院辦公樓3樓的回型走廊，行走的矩形路線長62 m、寬42 m。該實驗包括矩形一圈行走實驗和矩形兩圈行走實驗。

圖3 數據采集Fig.3 Data acquisition

為進一步驗證iHDE算法的實用性和穩定性，另外又進行了3組挑戰性實驗（非主導方向行走占大比例的路線）：1）折線行走實驗：該路線先沿主導方向行走10 m，然后轉動30°行走8 m的距離，最后沿主導方向行走 10 m；2）閉合三角形行走實驗：該路線為先沿主導方向行走6.93 m，然后轉動30°行走8 m的距離，最后沿主導方向行走4 m回到起點位置；3）帶環形行走實驗：該路線為先沿主導方向行走30 m，然后沿半徑為25 m的花壇行走一圈，最后沿主導方向行走30 m回到起點位置。

本文將5組實驗數據分別運用原始SHS-PDR算法、基于SHS-PDR算法系統的HDE算法以及iHDE算法處理，不同方法在同一圖中繪出路線圖可得到直觀的對比，并進行了精度分析。

3.2 常規路線實驗分析

圖4為矩形一圈行走中航向角變化圖，圖5為實驗效果圖，表1是對常規路線實驗的精度分析。由圖4~5和表1分析可知：

圖4 航向角變化圖Fig.4 Change of heading angle

1）陀螺儀的隨機漂移誤差導致未修正的航向角變化存在波動，這將對定位精度有一定的影響；經iHDE算法修正后的航向角變化更平滑，體現了 iHDE算法可以通過對航向角修正來實現消除陀螺儀漂移誤差的影響（見圖4）。

圖5 常規實驗路線圖Fig.5 Regular route experiment roadmap

表1 常規實驗精度分析Tab.1 Accuracy analysis of regular route experiment

2）未經航向修正的SHSPDR算法行走路線有非常嚴重的航向偏移現象，雖然HDE算法和iHDE算法最終的總行進距離（Total Traveled Distance，TTD）誤差都很低且相差不大，但行走過程中HDE算法存在明顯的航向偏移，出現了行走路線從建筑物中穿過的現象。iHDE算法平均TTD誤差為0.2%，與未經航向修正的算法相比定位精度提升了 88.9%，證明了iHDE算法在常規路線中具有高精度、高穩定性的優點。

3.3 挑戰性實驗分析

由表2和圖6分析可知：

1）HDE算法由于在非主導直線行走和曲線行走情況下錯誤地校正，使行走路線出現嚴重的偏移，定位精度急劇下降。在復雜的行走路線中，HDE算法穩定性低，航向修正功能失效。

圖6 挑戰性實驗路線圖Fig.6 Challenging route experiment roadmap

表2 挑戰性實驗精度分析Tab.2 Accuracy analysis of challenging route experiment

2）本文設計的iHDE算法進行了更準確的行走狀態分析，避免了錯誤的校正，平均TTD誤差為0.9%，與未經航向修正的算法相比定位精度提升了 43.8%。故iHDE算法仍可以達到對路線整體定位精度提升的效果，說明iHDE算法穩定性更高，可以適用于具有一定復雜環境路線中。

4 結 論

本文以低價位手機為實驗對象，選用四元數法解算航向信息、二階龍格庫塔法進行四元數更新，然后把經過四元數解算的航向信息與HDE算法、iHDE算法得到的航向信息分別與步長信息相結合，實現位置解算。通過5組實驗和3種不同方法的定位精度分析可得以下結論：

1）iHDE算法消除了由于漂移引起的誤差，大幅度提升了定位精度。綜合5組實驗，iHDE算法在相對復雜化的情況下，TTD誤差范圍為0.2%~0.9%，與未經航向修正的算法相比定位精度提升范圍為 43.8%~88.9%，與 HDE相比具有更高的精度和穩定性，且適合更復雜的情況。

2）本文實驗表明，iHDE算法可用于 SHS-PDR系統中，并且該算法在低價位的智能手機上得到了較好的應用，有利于為更廣大的手機用戶提供更可靠的行人定位服務。

本文已初步把iHDE算法應用到基于手機傳感器的 SHS-PDR算法框架中，在日后的研究中，可以考慮加入磁力計來對航向進一步修正，將iHDE算法和磁力計修正算法相融合，從而得到更精確的航向信息。

（

）：

[1] 劉宇, 周帆, 李云梅, 等. 基于人體多方位運動的三維自主導航定位算法[J]. 中國慣性技術學報, 2016, 24(4):449-453.Liu Y, Zhou F, Li Y M, et al. Three-dimensional autonomous navigation algorithm based on multi-azimuth movement pattern[J]. Journal of Chinese Inertial Technology, 2016, 24(4): 449-453.

[2] Ju H J, Min S L, Chan G P, et al. Advanced heuristic drift elimination for indoor pedestrian navigation[C]//International Conference on Indoor Positioning and Indoor Navigation. IEEE, 2015: 729-732.

[3] Borenstein J, Ojeda L. Heuristic drift elimination for personnel tracking systems[J]. Journal of Navigation,2010, 63(4): 591-606.

[4] Hsu Y L, Chen Y J, Shih S W. A particle filter approach for pedestrian dead reckoning using wearable sensors[C]//International Conference on Innovative Mobile and Internet Services in Ubiquitous Computing. IEEE Computer Society, 2016: 26-32.

[5] Jimenez A R, Seco F, Zampella F, et al. Improved heuristic drift elimination (iHDE) for pedestrian navigation in complex buildings[C]//International Conference on Indoor Positioning and Indoor Navigation. IEEE, 2011: 1-8.

[6] 樓喜中, 周樂宇, 葉敏展, 等. 基于角度的 HDE算法在室內行人航跡定位中的研究[J]. 傳感技術學報,2015, 28(4): 598-602.Lou X, Zhou L, Ye M, et al. Researching on indoor pedestrian trajectory based on angle HDE algorithm[J].Chinese Journal of Sensors and Actuators, 2015, 28(4):598-602.

[7] Su L M, Ju H, Song J W, et al. Kinematic model-based pedestrian dead reckoning for heading correction and lower body motion tracking[J]. Sensors, 2015, 15(11):28129-53.

[8] 趙輝, 李擎, 李超. 基于主方向的行人自主定位航向修正算法[J]. 電子技術應用, 2016, 42(11): 108-111.Zhao H, Li Q, Li C. A heading correction algorithm based on the main direction for pedestrian navigation[J]. Application of Electronic Technique, 2016, 42(11): 108-111.

[9] 曾慶軍, 劉慧婷, 張明. 基于 HDR的陀螺儀隨機漂移分析與處理[J]. 江蘇大學學報(自然科學版), 2016,37(3): 332-336.Zeng Q J, Liu H T, Zhang M. Analysis and processing on random drift of gyroscope based on HDR[J]. Journal of Jiangsu University (Natural Science Edition), 2016, 37(3):332-336.

[10] Diez L E, Bahillo A, Bataineh S, et al. Enhancing improved heuristic drift elimination for step-and-heading based pedestrian dead-reckoning systems[C]//International Conference of the IEEE Engineering in Medicine &Biology Society. 2016: 4415.

[11] Harle R. A survey of indoor inertial positioning systems for pedestrians[J]. IEEE Communications Surveys &Tutorials, 2013, 15(3): 1281-1293.

[12] 田曉春, 陳家斌, 韓勇強, 等. 一種優化的小波閾值去噪方法在行人導航系統中的應用[J]. 中國慣性技術學報, 2015, 23(4): 442-445.Tian X C, Chen J B, Han Y Q, et al. Application of optimized wavelet threshold de-nosing method in pedestrian navigation system[J]. Journal of Chinese Inertial Technology, 2015, 23(4): 442-445.

[13] 陳春陽. 基于運動傳感器的手機室內定位方法研究[D].青島: 山東科技大學, 2015.Chen C Y. Study on mobile phone sensor based indoor positioning method[D]. Qingdao: Shandong University of Science and Technology, 2015.