《電子技術(shù)應(yīng)用》
您所在的位置：首頁 > 嵌入式技術(shù) > 設(shè)計應(yīng)用 > 基于主方向的行人自主定位航向修正算法
基于主方向的行人自主定位航向修正算法
2016年電子技術(shù)應(yīng)用第11期
趙 輝,，李 擎,，李 超
北京信息科技大學(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個主方向的事實(shí)，當(dāng)檢測到行人軌跡為直線時,，將當(dāng)前的航向角與主方向角的差值作為觀測量進(jìn)行卡爾曼濾波,，對航向角進(jìn)行修正，并利用腰部PDR的方案進(jìn)行了單圈和兩圈矩形軌跡實(shí)驗(yàn),。實(shí)驗(yàn)結(jié)果表明,，該算法在航向修正方面具有一定有效性，且重復(fù)性好,，定位誤差為總路程的1%～2%,。
中圖分類號： TP391.9
文獻(xiàn)標(biāo)識碼： A
DOI：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.
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
Abstract： 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.
Key words : indoor pedestrian navigation；gyroscope drift,；heading correction,；main direction；Kalman filter

0 引言

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

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

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

1 計步檢測與步長估計

1.1 計步檢測

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

jsj1-gs1.gif

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

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

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

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

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

jsj1-t1.gif

1.2 步長估計

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

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

jsj1-gs2-3.gif

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

2 航向估計與修正

2.1 航向估計

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

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

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

jsj1-gs4-7.gif

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

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

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

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

jsj1-t2.gif

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

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

jsj1-gs8-13.gif

jsj1-gs14.gif

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

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

jsj1-t3.gif

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

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

jsj1-t4.gif

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

4 結(jié)論

    航向發(fā)散問題是行人導(dǎo)航定位的主要誤差源之一，為了抑制由于陀螺漂移而帶來的航向誤差問題,，本文在啟發(fā)式漂移消除算法（HDE）的基礎(chǔ)上,，提出了基于主方向的航向修正算法，將行人航向劃分為8個主方向,，當(dāng)檢測到行人在沿類直線軌跡行走時,，將行人當(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].中國慣性技術(shù)學(xué)報,，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] 胡安冬,，王堅，高井祥.一種基于地圖匹配輔助行人航位推算的室內(nèi)定位方法[J].測繪科學(xué)技術(shù)學(xué)報,，2014(5)：529-532.

此內(nèi)容為AET網(wǎng)站原創(chuàng),，未經(jīng)授權(quán)禁止轉(zhuǎn)載。