郭 英，姬現(xiàn)磊,2，劉清華，李冠澤,2，徐 瑩

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

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

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

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

1 iHDE算法框架改進(jìn)

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

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

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

圖1 系統(tǒng)框架Fig.1 System architecture

2 改進(jìn)算法實(shí)現(xiàn)

2.1 航向角解算

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

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

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

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

根據(jù)上述公式即可獲得實(shí)時的四元數(shù)更新。

第3步：四元數(shù)歸一化。

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

2.2 對航向角進(jìn)行修正

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

第1步：設(shè)定主導(dǎo)航向。

主方向可以設(shè)定為4個或8個主導(dǎo)方向，如圖2所示，本文研究以8個主導(dǎo)方向為研究基礎(chǔ)，主導(dǎo)方向的間隔date值為45°。

圖2 主導(dǎo)方向Fig.2 Dominant direction

第2步：辨別行走狀態(tài)。

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

即得行走航向與主導(dǎo)方向的差值。

第3步：航向角標(biāo)準(zhǔn)差求解。

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

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

計算誤差方差陣：

觀測方程為：

計算卡爾曼增益：

計算i時刻最優(yōu)估值：

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

2.3 航跡推算

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

式中，x、y為位置坐標(biāo)，S為步長。

3 實(shí)驗分析

3.1 數(shù)據(jù)采集

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

根據(jù)行走路線的復(fù)雜程度，可分為常規(guī)路線實(shí)驗和挑戰(zhàn)性路線實(shí)驗。

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

圖3 數(shù)據(jù)采集Fig.3 Data acquisition

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

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

3.2 常規(guī)路線實(shí)驗分析

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

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

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

圖5 常規(guī)實(shí)驗路線圖Fig.5 Regular route experiment roadmap

表1 常規(guī)實(shí)驗精度分析Tab.1 Accuracy analysis of regular route experiment

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

3.3 挑戰(zhàn)性實(shí)驗分析

由表2和圖6分析可知：

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

圖6 挑戰(zhàn)性實(shí)驗路線圖Fig.6 Challenging route experiment roadmap

表2 挑戰(zhàn)性實(shí)驗精度分析Tab.2 Accuracy analysis of challenging route experiment

2）本文設(shè)計的iHDE算法進(jìn)行了更準(zhǔn)確的行走狀態(tài)分析，避免了錯誤的校正，平均TTD誤差為0.9%，與未經(jīng)航向修正的算法相比定位精度提升了 43.8%。故iHDE算法仍可以達(dá)到對路線整體定位精度提升的效果，說明iHDE算法穩(wěn)定性更高，可以適用于具有一定復(fù)雜環(huán)境路線中。

4 結(jié) 論

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

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

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

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

（

）：

[1] 劉宇, 周帆, 李云梅, 等. 基于人體多方位運(yùn)動的三維自主導(dǎo)航定位算法[J]. 中國慣性技術(shù)學(xué)報, 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算法在室內(nèi)行人航跡定位中的研究[J]. 傳感技術(shù)學(xué)報,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]. 電子技術(shù)應(yīng)用, 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ī)漂移分析與處理[J]. 江蘇大學(xué)學(xué)報(自然科學(xué)版), 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] 田曉春, 陳家斌, 韓勇強(qiáng), 等. 一種優(yōu)化的小波閾值去噪方法在行人導(dǎo)航系統(tǒng)中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 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] 陳春陽. 基于運(yùn)動傳感器的手機(jī)室內(nèi)定位方法研究[D].青島: 山東科技大學(xué), 2015.Chen C Y. Study on mobile phone sensor based indoor positioning method[D]. Qingdao: Shandong University of Science and Technology, 2015.