路永樂，劉洪志，郭俊啟，陳自然，向高軍，劉 宇

（重慶郵電大學(xué) 光電信息感測與傳輸技術(shù)重慶市重點實驗室，重慶 400065）

行人導(dǎo)航系統(tǒng)（Pedestrian Navigation System，PNS）是一種實現(xiàn)對行人實時位置信息跟蹤的導(dǎo)航裝置[1]。根據(jù)導(dǎo)航環(huán)境，可將行人導(dǎo)航系統(tǒng)分為室外導(dǎo)航和室內(nèi)導(dǎo)航。室內(nèi)導(dǎo)航又可分為基于射頻信號的定位技術(shù)和包含傳感器的定位技術(shù)[2]。慣性導(dǎo)航屬于后者，其特點是無需依靠外界信號，自主性強，但誤差隨時間累積。隨著MEMS技術(shù)的快速發(fā)展，慣性導(dǎo)航朝著低成本，低功耗，輕質(zhì)量的方向發(fā)展[3]，并得到了快速而廣泛的應(yīng)用。

由于MEMS陀螺儀自身漂移使航向角誤差隨時間累積[4-6]，為了減少純慣性室內(nèi)導(dǎo)航航向角誤差，德國學(xué)者采用粒子濾波和后期地圖匹配處理的方法得到最優(yōu)導(dǎo)航結(jié)果[7]。此方法需要用到后期的地圖匹配，不能滿足實時定位的需求。美國Borestein J.等人提出啟發(fā)式漂移消除算法（Heuristic Drift Elimination）[8]，該方法根據(jù)室內(nèi)環(huán)境將其分為四個或者八個主方向，利用行人航向與主方向之間的差值作為反饋系統(tǒng)，將反饋參數(shù)對角速度信息進行修正，從而修正航向。A. R. Jiménez等人在此基礎(chǔ)上進一步做了改進，提出了改進型啟發(fā)式漂移算法（Improved Heuristic Drift Elimination）[9]，將姿態(tài)角和主方向進行擴展卡爾曼濾波來獲取姿態(tài)信息。這兩種算法都是基于足部導(dǎo)航方式實現(xiàn)，Luis E. Diez等人提出了基于腕部的PDR （Pedestrian Dead Reckoning）導(dǎo)航方式的加強型的啟發(fā)式漂移算法（Enhancing Improved Heuristic Drift Elimination）[10]。

本文在啟發(fā)式漂移算法的基礎(chǔ)上，提出了基于腰部佩戴方式的行人導(dǎo)航裝置的航向反饋修正算法，通過對行人直行的角速度和航向信息進行分析，設(shè)計了一種行人直行的判別方法，并利用此條件作為反饋修正的基礎(chǔ)，將樓道和走廊等主方向和非主方向上直行的航向均值信息作為反饋修正的先驗條件，利用行人航向和主方向差值最為AUKF的狀態(tài)量，從而實現(xiàn)航向修正，提高了室內(nèi)行人導(dǎo)航精度。

1 系統(tǒng)介紹

本系統(tǒng)采用嵌入式系統(tǒng)作為行人自主導(dǎo)航硬件測試平臺。該嵌入式系統(tǒng)集成了多個傳感器件，如三軸MEMS加速度、三軸MEMS陀螺儀和MEMS氣壓計等。

導(dǎo)航系統(tǒng)所采用的行人定位導(dǎo)航算法框架如圖 1所示。算法流程為首先對加速度計等傳感器件的原始數(shù)據(jù)進行濾波預(yù)處理，然后分別對加速度計、陀螺儀進行安裝誤差校準(zhǔn)和刻度因子校正，對處理后的傳感器數(shù)據(jù)利用四元數(shù)和歐拉角之間的轉(zhuǎn)換算法以及擴展卡爾曼濾波（EKF）算法獲得航姿角信息，以及步長、步態(tài)和步數(shù)等信息，帶入到PDR算法獲得行人二維位置信息和氣壓計解算的高度信息，實現(xiàn)行人的三維實時定位。

圖1 系統(tǒng)框架Fig.1 system framework

2 行人室內(nèi)導(dǎo)航算法

本系統(tǒng)采用的導(dǎo)航坐標(biāo)系為當(dāng)?shù)氐仄阶鴺?biāo)系，簡稱n系，x、y、z方向分別指向0°方向、90°方向和重力反方向。載體坐標(biāo)系為b系，x、y、z方向分別為載體的前、左、上方向。本文中分別代表第i個采樣時刻n系和b系的三軸加速度值。采樣頻率均為50 Hz。

本算法主要利用三軸加速度進行步數(shù)檢測和步長估計，與航向信息一起帶入到PDR算法中得到行人二維坐標(biāo)，再與高度信息結(jié)合得到三維位置信息。因高度信息不影響航向精度，故本文不對高度解算進行展開。

2.1 步數(shù)檢測

圖2 目標(biāo)加速度波峰檢測圖Fig.2 Target acceleration peak detection

2.2 步長估計

Harvey Weinderg[11]提出步長與加速度的最大最小幅度之差呈一定相關(guān)性，故采用如下公式計算步長：

2.3 航姿角解算

2.3.1 姿態(tài)角的初始解算

2.3.2 歐拉角與四元數(shù)的相互轉(zhuǎn)換

2.4 航跡推算算法

3 航向反饋修正算法

HDE算法是在 99%的室內(nèi)環(huán)境下走廊和過道是矩形結(jié)構(gòu)的基礎(chǔ)上提出的，并將矩形結(jié)構(gòu)的四個方向命名為主方向。本文在該算法的基礎(chǔ)上建立了基于腰部佩戴方式的航向反饋修正算法。本論文中嵌入式定位模塊的z軸與脊柱平行置于腰間，佩戴方式如圖3所示。

置于腰部的行人運動模式判別不同于足部。本文提出了針對腰間采集數(shù)據(jù)方式的行人行走模式的判別方法，為了簡化模型，我們將行人在室內(nèi)的行走模式分為直行、非直行。而根據(jù)行人是否在主方向上直行，又可將行人的行走模式分為在主方向的直行和在非主方向的直行，而其他情況統(tǒng)一為曲線行走。

圖3 消防單兵定位裝置佩戴示意圖Fig.3 Wearing mode for fire-fighting position device

3.1 直行判定

在本算法中，行人直行是作為反饋修正的基礎(chǔ)條件，所以對行人的直行判定方式成為了本論文的重點，根據(jù)行人在直行、轉(zhuǎn)向和彎型走廊情況下z軸角速度的采集，分別給出圖4~6所示的變化曲線。

考慮行人在短時內(nèi)航向角不會出現(xiàn)較大偏差，因而將航向角融入到直行判定中。

1）加速度滑動方差檢測法（Accelebration Moving Variance Detector, AMVD）檢測步態(tài)：行人在有步態(tài)和無步態(tài)過程中加速度波動幅度差別較大，可設(shè)置一定的滑動窗口求取方差和適當(dāng)?shù)姆讲铋撝祦韺π腥耸欠裉幱谛凶郀顟B(tài)進行區(qū)分，公式如下：

圖4 直行情況下的z軸角速度和航向角變化圖Fig.4 Variations of z-axis angular velocity and heading angle in the case of going straight

圖5 轉(zhuǎn)向情況下的z軸角速度和航向角變化圖Fig.5 Variations of z-axis angular velocity and heading angle in the case of steering

圖6 彎道行走情況下的z軸角速度和航向角變化圖Fig.6 Variations of z-axis angular velocity and heading angle in the case of curve walking

2）三步航向差值判定直行：行人在直行過程中，航向角變化較小，故利用三步間航向差值進行直行判定。

3）角速度判定直行：為了與三步的航向差值判直行相對應(yīng)，在角速度判直行中也采集連續(xù)運動的三步間隔內(nèi)所記錄的角速度值。從圖4~6中可以看出，在轉(zhuǎn)向中角速度值明顯大于彎道和直行，故記錄在這段時間內(nèi)角速度絕對值小于閾值的個數(shù)占總個數(shù)的比例，記作 R1，并對該比例設(shè)定閾值 r1。當(dāng)小于 r1時，認(rèn)定是轉(zhuǎn)向，A3記為0，大于r1時則認(rèn)定為彎道行走或者直行，A3記為1，其公式如下所示：

在彎道行走中由于行人朝著某一個方向轉(zhuǎn)動，故采集到的角速度值會更多地偏向正或者負(fù)的方向。如圖6中，角速度分布在正值的較多。而行人在直行過程中角速度在正負(fù)的分布情況較為均勻，如圖4所示。故在記錄A3的同時，分別記錄大于零和小于零的角速度的個數(shù)占總個數(shù)的比例，記做R2和R3。選取R2和R3中較大的比例參與直行的判定，并對此比例設(shè)定閾值r2，當(dāng)大于r2時，A4記為0，當(dāng)小于r2時，A4記為1，其公式如下所示：

通過對不同行人進行行走測試，當(dāng)ωth為 60，r1為97%，r2為65.5%時能準(zhǔn)確地判斷行人直行狀態(tài)。

當(dāng)A1、A2、A3、A4同時為1時檢測到行人直行，用邏輯“與”來表示：

Flag_straight為直行標(biāo)志位，當(dāng)為1時代表直行，當(dāng)為0時代表非直行。

3.2 主方向上直行

1）主方向范圍設(shè)置：根據(jù)行人的設(shè)定直行晃動角度范圍range和主方向航向，當(dāng)前航向角與初始時刻航向角（即 0°）的差值為-range（即 360-range）到range，90-range到90+range，180-range到180+range，270-range到270+range的角度范圍內(nèi)時，即認(rèn)為處于主方向范圍內(nèi)，如圖7中虛線所示。

圖7 主方向和主方向航向范圍Fig.7 Main direction and its heading range

2）主方向直行：行人在主方向上且直行，用下式表示：

該輸出值參與位置解算，并作為下一時刻航向初始值參與四元數(shù)和歐拉角解算，從而實現(xiàn)反饋修正的目的。

3.3 非主方向上直行

當(dāng)在非主方向上且行人直行時，滿足非主方向直行條件，用如下公式表示：

圖8 航向反饋流程圖Fig.8 Flow chart of heading feedback

4 實驗驗證

本實驗所用的傳感器部分參數(shù)如表1所示。

表1 傳感器部分參數(shù)Tab.1 Some parameters of sensors

4.1 矩形室內(nèi)環(huán)境

本次實驗地點選取重慶郵電大學(xué)一教學(xué)樓，結(jié)構(gòu)如圖9所示，該樓共有四層。規(guī)定統(tǒng)一的路線，起點→2(ABC)→3(DEF)→4(GHI)→3(DEF)→2(ABC)→3(DEF)→4(GHI)→3(DEF)→2(ABC)→3(DEF)→4(GHI)→3(DEF)→2(ABC)→3(DEF)→2(ABC)→3(DEF)→2(ABC)→3(DEF)→A→終點，其中2、3、4分別代表第二、三、四層，括號中內(nèi)容為行走順序點。出發(fā)時的航向為0°，三維坐標(biāo)為(0, 0, 0)，并規(guī)定行人結(jié)束時方向與出發(fā)時一致，全長1000 m。

圖9 結(jié)構(gòu)示意圖Fig.9 Structure sketch map

圖10 是行人在行走30 min過程中真實航向角、無反饋修正航向角和反饋修正航向角的對比圖（為了防止在圖中0°附近有突跳的航向值，故將0~20°的值進行上移，整個航向角范圍為20°~380°）。

圖10 航向?qū)Ρ葓DFig.10 Heading comparison chart

通過圖 10實驗對比分析，在無航向角反饋修正時，航向角逐漸偏離真實航向角，且差值隨時間的增加而增加。當(dāng)行人到達終點時，無反饋修正航向角為318.3°，與出發(fā)點差值已達到41.7°，超出了行人行走中正常航向偏差。而通過航向反饋修正算法后，達到終點時反饋修正航向角為 6.3°，保持在行人正常的身體擺動角度范圍內(nèi)，是行人行走中正常的航向偏差。

圖11和圖12分別為無反饋修正和有反饋修正的行人軌跡圖。無修正的航向角在15 min后軌跡嚴(yán)重偏離真實路線。航向反饋修正軌跡在30 min行走過程中與實際路線保持一致。故該算法將精準(zhǔn)的航向解算時間提升了兩倍以上。表2為不同行人在相同路線下的測試結(jié)果，導(dǎo)航精度均在1%以內(nèi)。

圖11 無航向反饋修正行走效果圖Fig.11 Walking effect before heading feedback correction

圖12 航向反饋修正行走效果圖Fig.12 Walking effect after heading feedback correction

表2 不同行人測試結(jié)果Tab.2 Different pedestrian test results

4.2 非矩形結(jié)構(gòu)室內(nèi)環(huán)境

本實驗選擇重慶郵電大學(xué)信科二樓作為實驗環(huán)境。圖 13(a)中含圓弧形室內(nèi)環(huán)境，當(dāng)再次走到矩形環(huán)境下，航向得到修正。圖13(b)中含非四個主方向的直線環(huán)境，當(dāng)在折線方向時軌跡較為平直，在主方向時也有修正作用，說明該算法適應(yīng)能力較強。

圖13 非矩形結(jié)構(gòu)室內(nèi)環(huán)境Fig.13 Non-rectangular structure indoor environment

5 結(jié) 論

本文針對基于 MEMS-IMU的行人室內(nèi)慣性導(dǎo)航下，航向角誤差隨時間累積的問題，提出了航向反饋修正算法。行人在主方向和非主方向上直行時，將航向角進行反饋，經(jīng)自適應(yīng)無跡卡爾曼濾波后得到最優(yōu)航向角參與PDR算法和作為下一時刻的初始航向角，減少了航向的累積誤差，并提升了導(dǎo)航精度和導(dǎo)航時間，具有很強的工程應(yīng)用價值。

（

）：

[1] Zhang H, Yuan W, Shen Q, et al. A handheld inertial pedestrian navigation system with accurate step modes and device poses recognition[J]. IEEE Sensors Journal,2015, 15(3): 1421-1429.

[2] 陳偉. 基于GPS和自包含傳感器的行人室內(nèi)外無縫定位算法研究[D]. 合肥: 中國科學(xué)技術(shù)大學(xué), 2010.Chen W. Research on GPS/Self-Contained sensors based seamless outdoor/indoor pedestrian positioning algorithm[D]. Hefei: University of Science and Technology of China, 2010.

[3] Perlmutter M, Breit S. The future of the MEMS inertial sensor performance, design and manufacturing[C]//2016 DGON Inertial Sensors and Systems (ISS). 2016: 1-12.

[4] Kang L, Ye L, Song K, et al. Attitude heading reference system using MEMS inertial sensors with dual-axis rotation[J]. Sensors, 2014, 14(10): 18075-18095.

[5] Collin J. MEMS IMU carouseling for ground vehicles[J].IEEE Transactions on Vehicular Technology, 2015, 64(6):2242-2251.

[6] Zhang Y S, Yang X, Xing X M, et al. The standing calibration method of MEMS gyro bias for autonomous pedestrian navigation system[J]. The Journal of Navigation, 2017, 70(3): 607-617.

[7] Robertson P, Angermann M, Krach B, et al. Slam dance:inertial-based joint mapping and positioning for pedestrian navigation[J]. Inside GNSS, 2010, 5: 48-59.

[8] Borenstein J, Kwanmuang L O S. Heuristic reduction of gyro drift for personnel tracking systems[J]. Journal of Navigation, 2009, 62(1): 41-58.

[9] 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.

[10] Diez L E, Bahillo A, Bataineh S, et al. Enhancing improved heuristic drift elimination for wrist-worn PDR systems in buildings[C]//Vehicular Technology Conference (VTCFall). IEEE, 2016: 1-5.

[11] Weinberg H. Using the ADXL202 in pedometer and personal navigation applications[J]. Analog Devices AN-602 application note, 2002, 2(2): 1-6.

[12] 胡高歌, 高社生, 趙巖. 一種新的自適應(yīng) UKF算法及其在組合導(dǎo)航中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 2014,22(3): 357-361, 367.Hu G G, Gao S S, Zhao Y. Novel adaptive UKF and its application in integrated navigation[J]. Journal of Chinese Inertial Technology, 2014, 22(3): 357-361, 367.