基于主方向的行人自主定位航向修正算法*

趙 輝，李 擎，李 超

（北京信息科技大學 高動態導航技術北京市重點實驗室，北京 100101）

基于主方向的行人自主定位航向修正算法*

趙 輝，李 擎，李 超

（北京信息科技大學 高動態導航技術北京市重點實驗室，北京 100101）

無信標環境下的行人導航問題是目前導航領域的難題和研究熱點，考慮到系統的便攜性和實用性，行人自主導航系統多采用慣性器件進行定位解算。針對當前的行人慣性導航系統航向角發散問題，在啟發式漂移消除算法（HDE）的基礎上，提出一種基于主方向的航向修正算法，根據室內的行走方向大多分為8個主方向的事實，當檢測到行人軌跡為直線時，將當前的航向角與主方向角的差值作為觀測量進行卡爾曼濾波，對航向角進行修正，并利用腰部PDR的方案進行了單圈和兩圈矩形軌跡實驗。實驗結果表明，該算法在航向修正方面具有一定有效性，且重復性好，定位誤差為總路程的1%～2%。

室內行人導航；陀螺漂移；航向修正；主方向；卡爾曼濾波器

0 引言

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

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

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

1 計步檢測與步長估計

1.1 計步檢測

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

其中，a3D為三軸合成加速度，ax、ay、az分別為三軸加速度信號。采用滑動平均濾波對合成加速度進行處理，以消除其中的噪聲信息。

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

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

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

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

圖1 峰值探測

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

1.2 步長估計

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

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

式中，SL為估計步長，SF為步頻，SV為每一步的加速度方差，a、b、c為回歸參數。

非線性步長估計模型如下：

式中，Amax、Amin分別為一步中加速度的最大值和最小值，k為經驗常數，通過實際測試確定。

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

2 航向估計與修正

2.1 航向估計

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

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

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

姿態轉換矩陣得到后，利用式(7)求出航向角。其中Ψ為航向角，Tij(n)表示姿態矩陣的第 i行、第 j列的元素。

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

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

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

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

圖2 主方向劃分

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

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

式中，m表示行走路線狀態，1代表直線，0代表轉彎；Ψk表示當前步的航向角，Ψk-1和 Ψk-2分別表示前一步和前兩步的航向角，Ψth表示偏差閾值。通常情況下，設定角度值10°作為閾值。

當判斷行人走直線時，計算當前航向角與當前主方向差值，將差值作為觀測量進行卡爾曼濾波。

設置9維誤差狀態向量，如下所示：

式中δφn=[δθδγδΨ]，為3個姿態角誤差向量；δvn=[δvxδvyδvz]為三軸速度誤差向量；εn=[εxεyεz]為三軸角速度誤差向量。

建立的誤差狀態方程如下所示：

式中 Φk，k-1是狀態轉移矩陣，Γk-1和 Wk-1分別是噪聲強度矩陣和噪聲矩陣。

針對航向角誤差的觀測方程如下所示：

其中，Vyaw為航向角觀測噪聲矩陣。

3 實驗與結果

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

圖3 單矩形軌跡行走效果圖

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

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

圖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.

A heading correction algorithm based on the main direction for pedestrian navigation

Zhao Hui，Li Qing，Li Chao
(Beijing Key Laboratory of High Dynamic Navigation Technology，Beijing Information Science and Technology University，Beijing 100101，China)

Pedestrian navigation in a non beacon environment is a difficult problem and a hot research topic in the field of navigation.Considering the portability and practicality of the system,the pedestrian self-navigation system uses the inertial device for positioning.Aiming at the heading divergence problem of the current pedestrian inertial navigation system,a heading correction algorithm based on the main direction under the foundation of Heuristic Drift Elimination is proposed.It is a fact that most of the corridors and the paths in the man-made buildings are perpendicular or parallel to each other.The heading direction is divided into eight directions.The difference between the current heading and the main direction is as the observation value when the pedestrian is walking along a straight line.Then the filter is used to correct the heading angels.Single rectangle and double rectangle tests are done.The results show that the algorithm can correct the heading direction efficiently and the location error is about 1%to 2%of total travel distance.

indoor pedestrian navigation；gyroscope drift；heading correction；main direction；Kalman filter

TP391.9

A

10.16157/j.issn.0258-7998.2016.11.029

趙輝，李擎，李超.基于主方向的行人自主定位航向修正算法[J].電子技術應用，2016，42(11)：108-111.

英文引用格式：Zhao Hui，Li Qing，Li Chao.A heading correction algorithm based on the main direction for pedestrian navigation[J]. Application of Electronic Technique，2016，42(11)：108-111.

2016-03-29)

趙輝（1992-），男，碩士研究生，主要研究方向：行人自主導航與定位。

李擎（1964-），通信作者，女，博士，教授，主要研究方向：高動態導航與控制，E-mail：liqing@bistu.edu.cn。

李超（1990-），男，博士，主要研究方向：室內定位技術、組合導航算法。

國家自然科學基金(61471046)；北京市教委市屬高校創新能力提升計劃項目(TJSHG201510772017）