0 引言

    行人導(dǎo)航系統(tǒng)(PNS)主要用于跟蹤人員的實(shí)時(shí)位置信息。基于微機(jī)電慣性測量單元(MEMS IMU)的行人導(dǎo)航定位系統(tǒng)由于其不受環(huán)境約束、使用靈活和魯棒性好等優(yōu)點(diǎn)，在應(yīng)急救災(zāi)、反恐安全以及日常生活等方面均具有較高的應(yīng)用價(jià)值^[1]。

    行人慣性導(dǎo)航系統(tǒng)通常采用陀螺儀來估計(jì)航向，由于陀螺儀存在漂移誤差，航向修正問題一直是國內(nèi)外研究的熱點(diǎn)。如南京航空航天大學(xué)曾慶化等人提出了一種蜂窩網(wǎng)格粒子濾波算法^[2]；中國科學(xué)院光電研究院公續(xù)平等人提出采用視覺陀螺儀的方法^[3]；英國諾丁漢大學(xué)ABDULRAHIM K等人提出利用建筑物的結(jié)構(gòu)信息來修正行人航向信息的方法^[4]；美國密歇根大學(xué)BORESTEIN J等人提出啟發(fā)式漂移消除算法，利用行人走直線時(shí)航向角的變化量對陀螺儀輸出進(jìn)行校正^[5]。但以上方法的修正效果不夠理想，算法復(fù)雜且實(shí)時(shí)性較差。

    為了解決行人慣性導(dǎo)航中航向角發(fā)散的問題，在不引入外部信息的情況下，本文提出了一種基于主方向的航向修正算法，相比于啟發(fā)式漂移消除算法中利用二進(jìn)制積分控制器對陀螺儀的角速率進(jìn)行修正的方法，本文直接對航向角誤差進(jìn)行估計(jì)，并將差值作為觀測量，利用卡爾曼濾波器對航向進(jìn)行修正，算法更加簡練實(shí)用。

1 計(jì)步檢測與步長估計(jì)

1.1 計(jì)步檢測

    采用行人航位推算(PDR)的方法進(jìn)行位置解算，首先要進(jìn)行準(zhǔn)確的計(jì)步檢測，即判斷行人是否跨步，然后再進(jìn)行步長估計(jì)。計(jì)步檢測主要利用加速度計(jì)信號，為了提高檢測的魯棒性，將三軸加速度信號作如下處理：

    常用的計(jì)步檢測方法有：峰值檢測、平區(qū)檢測法和過零檢測法^[6]。簡便起見，本文采用峰值檢測法來進(jìn)行計(jì)步檢測。為提高檢測的準(zhǔn)確率，特別加入以下兩個(gè)約束條件：

    (1)加速度峰值必須大于閾值，避免因身體抖動帶來的虛檢測；

    (2)兩連續(xù)峰值之間的時(shí)間間隔必須大于設(shè)定閾值，去除一步中多峰值的情況。

    峰值探測效果圖如圖1所示，圖中圓圈表示峰值點(diǎn)，一個(gè)圓圈代表一步。

從圖1可以看出，共有35個(gè)圓圈，表明行走了35步，與實(shí)際行走步數(shù)吻合，說明該計(jì)步探測算法具有較高準(zhǔn)確率。

1.2 步長估計(jì)

    常用的步長估計(jì)模型主要分為：線性估計(jì)模型和非線性估計(jì)模型。

    線性步長估計(jì)模型如下所示：

    本文認(rèn)為步長估計(jì)應(yīng)采用非線性模型進(jìn)行估計(jì)更為準(zhǔn)確，并且非線性模型只有一個(gè)訓(xùn)練參數(shù)，更易于進(jìn)行實(shí)時(shí)的步長估計(jì)，所以在進(jìn)行單步距離計(jì)算時(shí)采用非線性模型。

2 航向估計(jì)與修正

2.1 航向估計(jì)

    基于PDR的行人導(dǎo)航系統(tǒng)的導(dǎo)航精度主要由步長估計(jì)精度和航向估計(jì)精度決定。考慮到算法精度，本文采用四元數(shù)法來解算航向角。

    在初始時(shí)刻，定義3個(gè)姿態(tài)角為0，利用式（4）初始化四元數(shù)，其中Ψ₀、θ₀、γ₀分別為初始的航向角、俯仰角和橫滾角，公式左邊為初始四元數(shù)。

   當(dāng)陀螺儀數(shù)據(jù)更新后，利用二階龍格庫塔算法求解四元數(shù)微分方程，進(jìn)行四元數(shù)的更新。四元數(shù)微分方程如式(5)所示，其中ω_x、ω_y、ω_z為3個(gè)軸的角速率，Q為t時(shí)刻的四元數(shù)。

    通過以上步驟的解算，便可以利用陀螺儀的角速率輸出來得到行人的航向角，但陀螺儀的輸出會隨著時(shí)間而產(chǎn)生漂移，從而影響航向角的解算精度。為了抑制由于陀螺漂移而產(chǎn)生的航向角發(fā)散現(xiàn)象，需要采用相關(guān)手段對航向角進(jìn)行修正。

2.2 基于主方向的航向修正算法

    陀螺儀的輸出誤差會隨著時(shí)間而不斷增大，在不使用外部輔助信息（如GNSS、地圖）的情況下，通常采用零角速度修正（ZARU）、啟發(fā)式隨機(jī)漂移消除法（HDE）^[5]和磁力計(jì)進(jìn)行偏航角的校正。

    本文在HDE算法的基礎(chǔ)上，提出了基于主方向的航向修正算法。首先假設(shè)行人在室內(nèi)環(huán)境行走時(shí)遵循8個(gè)主方向的原則，即可選移動方向呈一個(gè)“米”字型，如圖2所示。

    HDE算法在計(jì)算航向角過程中，只利用了陀螺儀數(shù)據(jù)，但角速率直接積分會產(chǎn)生累積誤差，另外系統(tǒng)對用來修正角速率的反饋系數(shù)比較敏感，導(dǎo)致系統(tǒng)的魯棒性較差。而本文直接對航向角進(jìn)行處理，將解算得到的航向角與當(dāng)前主方向的差作為觀測量進(jìn)行卡爾曼濾波，從而對航向角進(jìn)行修正。

    當(dāng)行人按直線或接近直線的線路行走時(shí)，相鄰兩步的航向角變化是非常小的。行人進(jìn)行轉(zhuǎn)向時(shí)，當(dāng)前步的航向角相對于前一步的航向角會發(fā)生巨大變化，因此可以利用相鄰步之間航向角的突變來探測行人的轉(zhuǎn)向活動。為了保證轉(zhuǎn)向探測的正確率，本文采用3個(gè)相鄰步間的航向變化情況來探測轉(zhuǎn)向運(yùn)動，公式如下所示：

3 實(shí)驗(yàn)與結(jié)果

    為驗(yàn)證提出的航向修正算法的有效性，本文利用自研的MIMU導(dǎo)航模塊進(jìn)行室內(nèi)行人定位實(shí)驗(yàn)，將模塊固定在腰部，設(shè)定行走軌跡為一個(gè)矩形，實(shí)際的效果圖如圖3所示。

    圖3可以看出，未修正的虛線軌跡的航向角一直在偏移，導(dǎo)致最后形成的是平行四邊形的軌跡，而修正后的軌跡雖然與真實(shí)軌跡仍存在一定誤差，但已較好地接近實(shí)際軌跡，并且誤差為總行進(jìn)路程的1%，說明本文的航向修正算法具有較好的修正效果。

    為了進(jìn)一步驗(yàn)證本文算法的修正效果，另外進(jìn)行了兩圈矩形行走實(shí)驗(yàn)，實(shí)際效果如圖4所示。

    圖4中可以看出，未修正的虛線軌跡嚴(yán)重偏離，兩圈軌跡的重復(fù)性也較差；采用本文算法修正的實(shí)線軌跡較好地接近實(shí)際行走的矩形軌跡，并且兩圈的重復(fù)性也較好，最終誤差為總行程的2.3%，進(jìn)一步驗(yàn)證了本文算法在航向上起到了一定的修正效果，提高了行人導(dǎo)航定位的精度。

4 結(jié)論

    航向發(fā)散問題是行人導(dǎo)航定位的主要誤差源之一，為了抑制由于陀螺漂移而帶來的航向誤差問題，本文在啟發(fā)式漂移消除算法（HDE）的基礎(chǔ)上，提出了基于主方向的航向修正算法，將行人航向劃分為8個(gè)主方向，當(dāng)檢測到行人在沿類直線軌跡行走時(shí)，將行人當(dāng)前航向與當(dāng)前主方向的差值作為觀測量進(jìn)行卡爾曼濾波，從而實(shí)現(xiàn)對航向的修正。

    實(shí)驗(yàn)結(jié)果表明，本文算法能夠較好地修正行人航向角以及運(yùn)動軌跡，從而提高行人導(dǎo)航的定位精度。但本文算法不適合在空曠環(huán)境中行人隨意行走的情況下使用，在以后的工作中，考慮引入絕對位置信息，如地圖信息[7]，或結(jié)合其他室內(nèi)定位方法，如WiFi、UWB等，進(jìn)一步改善行人慣性導(dǎo)航的航向問題，提高導(dǎo)航定位精度。

參考文獻(xiàn)

[1] 申崇江，馮成濤，崔瑩，等.穿戴式室內(nèi)行人航位推算系統(tǒng)研究[C].第五屆中國衛(wèi)星導(dǎo)航學(xué)術(shù)年會論文集-S9 組合導(dǎo)航與導(dǎo)航新方法，2014.

[2] 曾慶化，萬駿煒，劉建業(yè)，等.基于蜂窩網(wǎng)格粒子濾波的行人導(dǎo)航航向估計(jì)方法[J].中國慣性技術(shù)學(xué)報(bào)，2014(5)：576-579.

[3] 公續(xù)平，魏東巖，李祥紅，等.一種面向智能終端的視覺陀螺儀/PDR/GNSS組合導(dǎo)航方法[C].第六屆中國衛(wèi)星導(dǎo)航學(xué)術(shù)年會，2015.

[4] ABDULRAHIM K，HIDE C，MOORE T，et al.Aiding low cost inertial navigation with building heading for pedestrian navigation[J].Journal of Navigation，2011，64(2)：219-233.

[5] BORENSTEIN J，OJEDA L，KWANMUANG S.Heuristic reduction of gyro drift for personnel tracking systems[J].Journal of Navigation，2009，62(1)：41-58.

[6] PARK J，KIM Y，LEE J.Waist mounted Pedestrian Dead-Reckoning system[C].Ubiquitous Robots and Ambient Intelligence(URAI)，2012 9th International Conference on.IEEE，2012：335-336.

[7] 胡安冬，王堅(jiān)，高井祥.一種基于地圖匹配輔助行人航位推算的室內(nèi)定位方法[J].測繪科學(xué)技術(shù)學(xué)報(bào)，2014(5)：529-532.