趙 輝，李 擎，李 超

（北京信息科技大學(xué) 高動態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京 100101）

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

趙 輝，李 擎，李 超

（北京信息科技大學(xué) 高動態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京 100101）

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

室內(nèi)行人導(dǎo)航；陀螺漂移；航向修正；主方向；卡爾曼濾波器

0 引言

行人導(dǎo)航系統(tǒng)(PNS)主要用于跟蹤人員的實(shí)時(shí)位置信息?；谖C(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ì)信號，為了提高檢測的魯棒性，將三軸加速度信號作如下處理：

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

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

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

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

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

圖1 峰值探測

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

1.2 步長估計(jì)

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

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

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

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

式中，Amax、Amin分別為一步中加速度的最大值和最小值，k為經(jīng)驗(yàn)常數(shù)，通過實(shí)際測試確定。

本文認(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ù)，其中 Ψ0、θ0、γ0分別為初始的航向角、俯仰角和橫滾角，公式左邊為初始四元數(shù)。

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

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

通過以上步驟的解算，便可以利用陀螺儀的角速率輸出來得到行人的航向角，但陀螺儀的輸出會隨著時(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所示。

圖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)動，公式如下所示：

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

當(dāng)判斷行人走直線時(shí)，計(jì)算當(dāng)前航向角與當(dāng)前主方向差值，將差值作為觀測量進(jìn)行卡爾曼濾波。

設(shè)置9維誤差狀態(tài)向量，如下所示：

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

建立的誤差狀態(tài)方程如下所示：

式中 Φk，k-1是狀態(tài)轉(zhuǎn)移矩陣，Γk-1和 Wk-1分別是噪聲強(qiáng)度矩陣和噪聲矩陣。

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

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

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

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

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

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

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

圖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)航定位精度。

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

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].電子技術(shù)應(yīng)用，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-），男，碩士研究生，主要研究方向：行人自主導(dǎo)航與定位。

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

李超（1990-），男，博士，主要研究方向：室內(nèi)定位技術(shù)、組合導(dǎo)航算法。

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