航向約束的行人導(dǎo)航算法研究

鄭佳瑤+蘇中+李擎

摘 要： 針對(duì)MEMS慣性器件隨時(shí)間累積零漂誤差大及航向誤差可觀測(cè)性差，導(dǎo)致行人導(dǎo)航航向發(fā)散的問題，提出一種基于主方向的航向修正算法?；谛腥搜刂本€行走航向角保持在常值范圍內(nèi)的事實(shí)，當(dāng)航向發(fā)生變化時(shí)，將行人當(dāng)前的航向與主航向的偏差作為觀測(cè)值，利用 EKF進(jìn)行誤差估計(jì)從而修正航向，最后將導(dǎo)航模塊固定在鞋面上進(jìn)行約200 m的行走實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，該方法能有效地抑制航向發(fā)散，定位誤差約為總行走路程的1%。

關(guān)鍵詞： 行人導(dǎo)航； 航向發(fā)散； 航向修正； EKF

中圖分類號(hào)： TN961?34； TP391.9 文獻(xiàn)標(biāo)識(shí)碼： A 文章編號(hào)： 1004?373X（2017）24?0001?04

Abstract： The MEMS inertial device has big zero?drift error with time accumulation and has poor observability for its heading error， which may cause the heading divergency of the pedestrian navigation. Therefore， a heading correction algorithm based on the main direction is proposed. Based on the fact that the heading angle while the pedestrian travels along the straight line maintains within the constant range， taking the deviation between the pedestrian′s current heading and master heading as the observation value when the heading changes， the extended Kalman filter （EKF） is used to estimate the error， so as to correct the heading. The navigation module is fixed on the upper surface of the shoes for about 200 m walking experiment. The experimental results show that the method can effectively suppress the heading divergency， and its positioning error is about 1% of the total walking distance.

Keywords： pedestrian navigation； heading divergency； heading correction； extended Kalman filter

0 引 言

目前，GPS室外定位技術(shù)已相當(dāng)成熟，但GPS在建筑物遮擋時(shí)信號(hào)容易丟失，易受環(huán)境的影響且民用精度不高，在室內(nèi)定位存在明顯的缺陷，因此，無法滿足人們對(duì)室內(nèi)終端定位的需求。而低成本的基于MEMS?IMU的個(gè)人導(dǎo)航[1]系統(tǒng)不需要任何外部設(shè)施，非常適合在室內(nèi)以及GPS信號(hào)較弱的地方使用。然而該系統(tǒng)的慣性器件存在隨時(shí)間累積的漂移誤差，是造成行人導(dǎo)航航向發(fā)散的主要誤差源。為了解決這一問題，國內(nèi)外研究人員[2]借助一些附加信息提出了建筑物信息融合算法、磁航向輔助算法等方法，但這些附加信息增加了系統(tǒng)的復(fù)雜度，易受客觀條件的干擾，影響了導(dǎo)航系統(tǒng)的應(yīng)用效果?；趩l(fā)式方法利用建筑物主方向校正航向的思想，Ref提出了一種啟發(fā)式漂移減少的HDR算法來修正垂直方向的陀螺儀角速率[3]；但該算法需要設(shè)計(jì)特殊的濾波器對(duì)陀螺儀角速率進(jìn)行濾波，增加了系統(tǒng)的復(fù)雜度。之后Ref等學(xué)者在HDR的基礎(chǔ)上提出一種啟發(fā)式漂移消除算法（HDE）[4]，通過修正陀螺儀的輸出并利用反饋?zhàn)兞啃Ｕ较?；但陀螺儀積分計(jì)算角度的過程會(huì)產(chǎn)生一定的累積誤差，仍存在航向發(fā)散的現(xiàn)象。

本文在啟發(fā)式思想HDE的基礎(chǔ)上，提出一種基于主方向的航向修正算法來抑制航向發(fā)散。基于行人在結(jié)構(gòu)化的室內(nèi)環(huán)境中行走，航跡近似為直線，航向變化保持在常值范圍內(nèi)的事實(shí)，當(dāng)有誤差引起航向發(fā)生變化時(shí)，將行人當(dāng)前的航向與主航向的偏差作為觀測(cè)值，利用EKF進(jìn)行誤差估計(jì)從而修正航向角。相對(duì)比HDE算法直接修正陀螺儀輸出，積分計(jì)算角度并利用反饋?zhàn)兞啃拚较虻姆椒ǎ疚奶岢龅乃惴苡行У乇苊釮DE積分計(jì)算角度的過程產(chǎn)生累積誤差的問題。

1 系統(tǒng)模型

本文將基于MEMS?IMU的導(dǎo)航模塊固定在鞋面上，根據(jù)人行走時(shí)腳接觸地面的周期性零速特征觸發(fā)零速修正技術(shù)（ZUPT）[5]，由于航向誤差的可觀測(cè)性差，ZUPT無法修正航向誤差，引入了一種基于主方向的航向修正算法，實(shí)時(shí)修正導(dǎo)航系統(tǒng)的累積誤差。導(dǎo)航系統(tǒng)的整體框圖如圖1所示，其中虛線框部分為基于主方向的航向修正算法的實(shí)現(xiàn)過程。首先采集IMU數(shù)據(jù)，根據(jù)IMU中陀螺儀和加速度計(jì)的輸出進(jìn)行步態(tài)檢測(cè)，當(dāng)檢測(cè)到行人處于腳接觸地面的零速時(shí)刻，觸發(fā)ZUPT算法，從而修正導(dǎo)航系統(tǒng)的速度、位置和水平姿態(tài)誤差?；谛腥嗽诮Y(jié)構(gòu)化的室內(nèi)環(huán)境沿直線行走，航向變化保持在常值范圍內(nèi)的事實(shí)，當(dāng)有誤差引起該航向發(fā)生變化時(shí)，觸發(fā)本文所提的航向修正算法，利用行人當(dāng)前的航向與主航向的偏差估計(jì)航向角誤差，從而抑制航向發(fā)散，最后將ZUPT算法和所提出的航向修正方法集成的誤差作為系統(tǒng)的觀測(cè)值，利用EKF進(jìn)行誤差估計(jì)，進(jìn)一步提高導(dǎo)航系統(tǒng)的定位精度。

2 算法實(shí)現(xiàn)

2.1 ZUPT算法endprint

基于步態(tài)檢測(cè)的零速修正技術(shù)已廣泛應(yīng)用在鞋式行人導(dǎo)航中，本文采用三條件（C1，C2，C3）判斷方法[6]并通過中值濾波技術(shù)來實(shí)現(xiàn)步態(tài)檢測(cè)。當(dāng)檢測(cè)到腳在地面完全靜止或處于行走時(shí)的支撐階段，如圖2所示，利用該時(shí)刻的靜止步態(tài)作為觸發(fā)條件來開啟零速修正ZUPT方法，并將零速度更新作為“偽測(cè)量”應(yīng)用于擴(kuò)展卡爾曼濾波器進(jìn)行誤差估計(jì)，從而修正IMU系統(tǒng)的長(zhǎng)時(shí)間累積誤差，提高定位精度。

ZUPT算法的實(shí)現(xiàn)是根據(jù)人在行走的過程中，腳與地面接觸瞬間，速度為零的事實(shí)，實(shí)時(shí)修正由于慣性傳感器的測(cè)量誤差、噪聲以及其他因素引起的速度誤差。速度誤差的方程如下：

然后將該速度誤差作為觀測(cè)值，利用EKF進(jìn)行誤差估計(jì)，從而修正導(dǎo)航系統(tǒng)的速度、位置和水平姿態(tài)誤差。由于慣性器件隨時(shí)間累積的零漂誤差較大且航向誤差可觀測(cè)性差[7]，導(dǎo)致ZUPT無法修正航向角，是造成行人導(dǎo)航航向發(fā)散的主要誤差源。本文在ZUPT修正導(dǎo)航系統(tǒng)誤差的同時(shí)，引入了一種基于主方向的航向修正算法來抑制航向發(fā)散，進(jìn)一步提高導(dǎo)航系統(tǒng)的精度。

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

2.2.1 航向估計(jì)

本文通過采集IMU數(shù)據(jù)進(jìn)行捷聯(lián)解算和四元數(shù)更新的方法[8]來推算航向角，具體過程如下：

四元數(shù)是指由四個(gè)元構(gòu)成的數(shù)，表達(dá)式如下：

利用四元數(shù)初始姿態(tài)角可表示如下：

式中，分別表示偏航角、俯仰角和橫滾角，且在初始時(shí)刻的值均為0。

利用畢卡三階算法更新四元數(shù)的方法如下：

式中：為時(shí)刻的四元數(shù)；為3個(gè)軸角度平方和；為單位矩陣；為對(duì)機(jī)體坐標(biāo)系相對(duì)地理坐標(biāo)系的旋轉(zhuǎn)角速度的斜對(duì)稱矩陣的積分。

四元數(shù)更新后，利用四元數(shù)表示姿態(tài)轉(zhuǎn)換矩陣，兩者的關(guān)系如下：

得到姿態(tài)轉(zhuǎn)換矩陣后，推算航向角如下：

式中，是姿態(tài)矩陣的第行、第列元素。

通過以上方法，可根據(jù)陀螺儀的輸出計(jì)算得到航向角。由于陀螺儀輸出存在隨時(shí)間累積的漂移誤差，從而導(dǎo)致航向發(fā)散的問題，需采用相關(guān)的方法來消除航向漂移誤差。

2.2.2 航向修正

基于啟發(fā)式方法（HDE）中主方向的思想，本文設(shè)定行人在室內(nèi)行走有8個(gè)主航向，如圖3所示，假設(shè)行人初始航向?yàn)?，左邊為負(fù)，右邊為正。

通常，行人在結(jié)構(gòu)化的室內(nèi)環(huán)境中沿直線行走時(shí)，航向變化不大且始終保持在常值范圍內(nèi)；當(dāng)行人轉(zhuǎn)向時(shí)，航向會(huì)發(fā)生較大的變化，基于這個(gè)事實(shí)，可以通過航向的變化判斷行人是否轉(zhuǎn)向。為了判斷準(zhǔn)確，本文采用相鄰3步的航向進(jìn)行轉(zhuǎn)向判斷，公式如下：

當(dāng)行人沿直線行走，有誤差引起航向發(fā)生變化時(shí)，利用行人當(dāng)前的航向和主航向的偏差來估計(jì)航向角誤差，如下：

最后將該偏差作為系統(tǒng)的觀測(cè)值，利用EKF進(jìn)行誤差估計(jì)從而修正航向。

本文建立的12維狀態(tài)方程如下：

本文通過集成傳統(tǒng)的ZUPT算法和基于主方向的航向修正算法消除系統(tǒng)累積誤差的過程，引入了速度誤差和航向誤差兩個(gè)觀測(cè)值。當(dāng)行人處于靜止步態(tài)觸發(fā)ZUPT修正方法時(shí)，速度誤差可直接被觀測(cè)到，其觀測(cè)方程如下：

當(dāng)行人沿直線行走，有誤差引起航向發(fā)生變化，觸發(fā)文中所提的航向修正算法時(shí)，航向誤差的觀測(cè)方程如下：

由于HDE算法只適用于行人在規(guī)則的室內(nèi)沿直線行走時(shí)的航向修正，當(dāng)行人轉(zhuǎn)彎時(shí)，HDE算法將停止航向修正并帶來新的誤差。為了使本文所提出的算法適用于行人轉(zhuǎn)彎時(shí)繼續(xù)航向修正，本文定義了一個(gè)航向偏差標(biāo)準(zhǔn)差表達(dá)式[9?11]，即式（18）中的，如下：

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

為了驗(yàn)證所提算法的有效性，本文將實(shí)驗(yàn)室自研制的基于MEMS?IMU的導(dǎo)航模塊固定在鞋面上，在室內(nèi)進(jìn)行行走實(shí)驗(yàn)。

實(shí)驗(yàn)環(huán)境為教學(xué)樓6樓的走廊，行走的路線為如圖4所示的一個(gè)閉環(huán)矩形，起止位置均為A，通過仿真實(shí)驗(yàn)得到的航跡推算結(jié)果如圖5所示。

從圖5中可以看出，由于航向誤差的不可觀測(cè)性，使得ZUPT修正的軌跡與實(shí)際軌跡存在偏差，定位誤差為總行走路程的2.1%；而在ZUPT的基礎(chǔ)上引入本文所提的修正方法后，修正的軌跡與真實(shí)軌跡基本吻合，定位誤差為總行走路程的0.7%。該方法與傳統(tǒng)的ZUPT算法相比，有效地解決了行人導(dǎo)航航向發(fā)散的問題，提高了導(dǎo)航系統(tǒng)的定位精度。

4 結(jié) 語

針對(duì)MEMS慣性器件隨時(shí)間累積的零漂誤差大及航向誤差可觀測(cè)性差，ZUPT無法修正航向從而導(dǎo)致航向發(fā)散的問題，因此，在啟發(fā)式思想HDE的基礎(chǔ)上提出了一種基于主方向的航向修正算法。首先設(shè)定行人在室內(nèi)行走有8個(gè)主航向，然后基于行人在結(jié)構(gòu)化的室內(nèi)環(huán)境中行走，航跡近似為直線，航向變化保持在常值范圍的事實(shí)。當(dāng)有誤差引起航向發(fā)生變化時(shí)，將行人當(dāng)前的航向與主航向的偏差作為觀測(cè)值，利用 EKF進(jìn)行誤差估計(jì)從而修正航向角。最后通過仿真實(shí)驗(yàn)驗(yàn)證了本文所提算法能有效地抑制航向發(fā)散，證明所提算法的有效性和準(zhǔn)確性。

為了更好地驗(yàn)證本文所提方法的有效性，今后將在更復(fù)雜的室內(nèi)環(huán)境中進(jìn)行行走實(shí)驗(yàn)，并在該方法的基礎(chǔ)上融入地圖或超寬帶UWB等定位信息，進(jìn)一步提高導(dǎo)航定位精度。

參考文獻(xiàn)

[1] 張金亮，秦永元，梅春波.基于MEMS慣性技術(shù)的鞋式個(gè)人導(dǎo)航系統(tǒng)[J].中國慣性技術(shù)學(xué)報(bào)，2011，19（3）：253?256.

[2] 謝波，江一夫，嚴(yán)恭敏，等.個(gè)人導(dǎo)航融合建筑平面信息的粒子濾波方法[J].中國慣性技術(shù)學(xué)報(bào)，2013，21（1）：1?6.

[3] LI Y， SONG Q， MA M， et al. Near real time heading drift correction for indoor pedestrian tracking based on sequence detection [C]// Proceedings of 2016 International Conference on Indoor Positioning and Indoor Navigation. Alcala de Henares： IEEE， 2016： 1?4.endprint

[4] 樓喜中，周樂宇，葉敏展，等.基于角度的HDE算法在室內(nèi)行人航跡定位中的研究[J].傳感技術(shù)學(xué)報(bào)，2015，15（4）：598?602.

[5] JIM?NEZ A R， SECO F， PRIETO J C， et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot?mounted IMU [C]// Proceedings of 2010 IEEE the 7th Workshop on Positioning Navigation and Communication. Dresden： IEEE， 2010： 135?143.

[6] 李超，蘇中，朱嘉林.可穿戴式自主定位技術(shù)的零速觸發(fā)算法研究[J].傳感技術(shù)學(xué)報(bào)，2014，14（5）：627?632.

[7] ZHANG X， RONG Z， GUO M F， et al. Yaw error self?observation algorithm for pedestrian navigation via foot?mounted inertial navigation system [J]. Journal of Chinese inertial technology， 2015， 23（4）： 457?466.

[8] 何瑜.四軸飛行器控制系統(tǒng)設(shè)計(jì)及其姿態(tài)解算和控制算法研究[D].成都：電子科技大學(xué)，2015：65?77.

[9] JIM?NEZ A R， SECO F， ZAMPELLA F， et al. Improved heuristic drift elimination （iHDE） for pedestrian navigation in complex buildings [C]// Proceedings of 2011 International Conference on Indoor Positioning and Indoor Navigation. Guimaraes： IEEE， 2011： 1?8.

[10] D?EZ L E， BAHILLO A， BATAINEH S， et al. Enhancing improved heuristic drift elimination for step?and?heading based pedestrian dead?reckoning systems [C]// Proceedings of 2016 IEEE the 38th Annual International Conference of Engineering in Medicine and Biology Society. Orlando： IEEE， 2016： 4415?4418.

[11] JU H J， LEE M S， PARK C G， et al. Advanced heuristic drift elimination for indoor pedestrian navigation [C]// Proceedings of 2014 International Conference on Indoor Positioning and Indoor Navigation. Busan： IEEE， 2014： 729?732.endprint