0 引言

    行人導航系統(PNS)主要用于跟蹤人員的實時位置信息?；谖C電慣性測量單元(MEMS IMU)的行人導航定位系統由于其不受環境約束、使用靈活和魯棒性好等優點，在應急救災、反恐安全以及日常生活等方面均具有較高的應用價值^[1]。

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

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

1 計步檢測與步長估計

1.1 計步檢測

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

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

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

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

    峰值探測效果圖如圖1所示，圖中圓圈表示峰值點，一個圓圈代表一步。

從圖1可以看出，共有35個圓圈，表明行走了35步，與實際行走步數吻合，說明該計步探測算法具有較高準確率。

1.2 步長估計

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

    線性步長估計模型如下所示：

    本文認為步長估計應采用非線性模型進行估計更為準確，并且非線性模型只有一個訓練參數，更易于進行實時的步長估計，所以在進行單步距離計算時采用非線性模型。

2 航向估計與修正

2.1 航向估計

    基于PDR的行人導航系統的導航精度主要由步長估計精度和航向估計精度決定?？紤]到算法精度，本文采用四元數法來解算航向角。

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

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

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

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

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

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

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

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

3 實驗與結果

    為驗證提出的航向修正算法的有效性，本文利用自研的MIMU導航模塊進行室內行人定位實驗，將模塊固定在腰部，設定行走軌跡為一個矩形，實際的效果圖如圖3所示。

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

    為了進一步驗證本文算法的修正效果，另外進行了兩圈矩形行走實驗，實際效果如圖4所示。

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

4 結論

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

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

參考文獻

[1] 申崇江，馮成濤，崔瑩，等.穿戴式室內行人航位推算系統研究[C].第五屆中國衛星導航學術年會論文集-S9 組合導航與導航新方法，2014.

[2] 曾慶化，萬駿煒，劉建業，等.基于蜂窩網格粒子濾波的行人導航航向估計方法[J].中國慣性技術學報，2014(5)：576-579.

[3] 公續平，魏東巖，李祥紅，等.一種面向智能終端的視覺陀螺儀/PDR/GNSS組合導航方法[C].第六屆中國衛星導航學術年會，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] 胡安冬，王堅，高井祥.一種基于地圖匹配輔助行人航位推算的室內定位方法[J].測繪科學技術學報，2014(5)：529-532.