航向約束的行人導航算法研究

鄭佳瑤+蘇中+李擎

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

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

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

本文在啟發(fā)式思想HDE的基礎(chǔ)上，提出一種基于主方向的航向修正算法來抑制航向發(fā)散?；谛腥嗽诮Y(jié)構(gòu)化的室內(nèi)環(huán)境中行走，航跡近似為直線，航向變化保持在常值范圍內(nèi)的事實，當有誤差引起航向發(fā)生變化時，將行人當前的航向與主航向的偏差作為觀測值，利用EKF進行誤差估計從而修正航向角。相對比HDE算法直接修正陀螺儀輸出，積分計算角度并利用反饋變量修正航向的方法，本文提出的算法能有效地避免HDE積分計算角度的過程產(chǎn)生累積誤差的問題。

1 系統(tǒng)模型

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

2 算法實現(xiàn)

2.1 ZUPT算法endprint

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

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

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

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

2.2.1 航向估計

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

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

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

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

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

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

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

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

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

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

2.2.2 航向修正

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

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

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

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

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

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

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

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

3 實驗與結(jié)果

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

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

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

4 結(jié) 語

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

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

參考文獻

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

[2] 謝波，江一夫，嚴恭敏，等.個人導航融合建筑平面信息的粒子濾波方法[J].中國慣性技術(shù)學報，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ù)學報，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ù)學報，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è)計及其姿態(tài)解算和控制算法研究[D].成都：電子科技大學，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