基于角度的HDE算法在室內(nèi)行人航跡定位中的研究

樓喜中，周樂宇，葉敏展，賈一展，金 寧

（1.中國計(jì)量學(xué)院信息工程學(xué)院，杭州 310018；2.中國電信股份有限公司浙江分公司，杭州 310040）

基于角度的HDE算法在室內(nèi)行人航跡定位中的研究

樓喜中1*，周樂宇1，葉敏展2，賈一展1，金 寧1

（1.中國計(jì)量學(xué)院信息工程學(xué)院，杭州 310018；2.中國電信股份有限公司浙江分公司，杭州 310040）

針對現(xiàn)有啟發(fā)式偏移消除算法HDE（Heuristic Drift Elimination）中航向角推算不準(zhǔn)確、反饋系數(shù)魯棒性較差的問題，提出了對經(jīng)過擴(kuò)展卡爾曼濾波的航向角進(jìn)行啟發(fā)式漂移消除的算法AHDE（Angle Heuristic Drift Elimination）。首先利用擴(kuò)展卡爾曼濾波EKF（Extended Kalman Filter）融合陀螺儀、加速度信息，通過四元數(shù)的更新來估計(jì)航向角，再用啟發(fā)式漂移消除算法對航向角進(jìn)行修正，最后結(jié)合步數(shù)及步長信息推算出行人的行走軌跡。實(shí)驗(yàn)結(jié)果顯示，在行走方向較為固定的典型室內(nèi)環(huán)境中，行走距離在250 m時(shí)，該算法平均誤差不超過2 m，而HDE算法誤差會(huì)達(dá)到4 m左右。并且該系統(tǒng)具有比對陀螺儀數(shù)據(jù)進(jìn)行啟發(fā)式漂移消除系統(tǒng)更強(qiáng)的魯棒性。當(dāng)以100 Hz頻率讀取數(shù)據(jù)時(shí)，AHDE算法反饋系數(shù)的選擇范圍由HDE算法的［0.001，0.028］擴(kuò)展為［0.005，0.23］，幾乎擴(kuò)大了一個(gè)數(shù)量級(jí)。

軌跡推算；啟發(fā)式漂移消除；擴(kuò)展卡爾曼濾波；計(jì)步器

在現(xiàn)代生活中，人們對定位與導(dǎo)航的信息需求日益膨脹，在GPS等衛(wèi)星定位技術(shù)不能使用的室內(nèi)環(huán)境中，基于慣性測試單元IMU（Inertial Measure U-nit）的室內(nèi)導(dǎo)航技術(shù)應(yīng)用越來越廣泛。Levi和Judd 在1996年首次提出了行人航跡推算 PDR （Pedestrian Dead Reckoning）的概念［1］，主要是根據(jù)陀螺儀數(shù)據(jù)計(jì)算航向信息，再基于行人步態(tài)的運(yùn)動(dòng)特征，利用加速度計(jì)信息估計(jì)步數(shù)和步長，結(jié)合這3個(gè)信息來推算行人的位置和姿態(tài)等信息。Johann等人則在PDR模塊的基礎(chǔ)上提出了更為精確的室內(nèi)行人軌跡的啟發(fā)式漂移消除推算算法 HDE （Heuristic Drift Elimination）［2］。該算法主要指出約99%的建筑都有4個(gè)或8個(gè)主要行進(jìn)方向，在這種室內(nèi)環(huán)境中，前一時(shí)刻的航向角和主要行進(jìn)方向間的偏差可以用來構(gòu)成一個(gè)反饋系統(tǒng)，再利用反饋參數(shù)對陀螺儀數(shù)據(jù)進(jìn)行修正，以達(dá)到修正航向角的目的。文獻(xiàn)［3］繼續(xù)根據(jù)室內(nèi)環(huán)境行進(jìn)方向較為固定這一特點(diǎn)，提出了改良式的啟發(fā)式漂移消除算法（iHDE），主要是通過對姿態(tài)角和主方向進(jìn)行卡爾曼濾波以更新最后人體的姿態(tài)信息。

在本文中，我們提出了對經(jīng)過擴(kuò)展卡爾曼濾波的航向角進(jìn)行啟發(fā)式漂移消除的算法——AHDE。該算法在計(jì)算航向角時(shí)引入了加速度信息以提高準(zhǔn)確性，同時(shí)，該算法也提高了系統(tǒng)的魯棒性。不過，和HDE算法相同，AHDE算法也是根據(jù)室內(nèi)環(huán)境中行進(jìn)的幾個(gè)主方向來對航向角進(jìn)行修正，因此，在一些歌劇院、室內(nèi)運(yùn)動(dòng)場等空曠環(huán)境中，軌跡追蹤效果也會(huì)相應(yīng)減弱。本文主要結(jié)構(gòu)安排如下：第二章在HDE算法框圖的基礎(chǔ)上，提出了存在的一些缺點(diǎn)，并介紹了AHDE算法的模型框圖；AHDE算法的具體實(shí)現(xiàn)則在第3章中進(jìn)行敘述；第4章中，我們通過實(shí)驗(yàn)來驗(yàn)證算法的有效性；最終在第5章給出結(jié)論。

1 算法模型

圖1 角速度啟發(fā)式漂移消除系統(tǒng)框圖

圖2 系統(tǒng)總體框

本文提出的算法AHDE的主要框圖如圖2所示，為了解決上述算法的問題，系統(tǒng)首先利用陀螺儀信息更新四元數(shù)，以加速度信息為觀測值利用擴(kuò)展卡爾曼濾波對四元數(shù)進(jìn)行修正，由修正后的四元數(shù)可以解算出行人當(dāng)前的航向角。接著系統(tǒng)通過啟發(fā)式漂移消除算法中的反饋環(huán)對航向角進(jìn)行修正。同時(shí)，根據(jù)行人行進(jìn)過程中的加速度信息，我們可以解算出人體的計(jì)步信息和步長信息。最后，修正后的航向角信息結(jié)合計(jì)步信息、步長信息就可以確認(rèn)人體當(dāng)前的位置信息，從而進(jìn)行室內(nèi)行人軌跡追蹤。

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

2.1 步數(shù)檢測和步長估計(jì)

隨著人體的走動(dòng)，加速度計(jì)各軸數(shù)據(jù)會(huì)呈現(xiàn)出具有波峰波谷的震蕩波形，通過對這些波形的分析，我們可以判斷出人體是否在行進(jìn)過程中。目前，基于這種傳感器的計(jì)步檢測方法有很多種，本文主要采用基于中間閥值穿越的計(jì)步算法［4］。算法的主要包含兩個(gè)步驟：首先檢測到一個(gè)從峰值到谷值穿越的一個(gè)過程，則被暫定為完整的一步；再對約束條件進(jìn)行判斷，如果在這一步中加速度穿越了中間閾值，并且加速度峰峰值大于一定的閥值，那么就可以確認(rèn)為完整的一步。

對于步長估計(jì)，本文采用了文獻(xiàn)［5］中提及的步長估計(jì)算法。這種算法主要是針對不同個(gè)體選取不同的標(biāo)定系數(shù)，利用實(shí)時(shí)步頻信息對步長進(jìn)行估計(jì)。

式中：L為步長；fstep為實(shí)時(shí)步頻；K1，K2為針對不同個(gè)體而標(biāo)定的系數(shù)。

2.2 航向角推算

文獻(xiàn)［6］中討論了姿態(tài)解算過程中誤差的主要來源，而陀螺儀的隨機(jī)漂移誤差就是其中來源之一。因此，只根據(jù)陀螺儀數(shù)據(jù)來計(jì)算航向角會(huì)造成較大誤差。

目前，用卡爾曼濾波算法結(jié)合四元數(shù)來更新姿態(tài)角的算法已經(jīng)日益成熟［7-10］。本文將誤差四元數(shù)作為狀態(tài)量進(jìn)行卡爾曼濾波，引入加速度信息作為觀測值，可以有效地修正陀螺儀輸出量中的隨機(jī)漂移分量，算法具體步驟如下：

第一步，在初始時(shí)刻，我們定義3個(gè)姿態(tài)角均為0°，并利用如下公式對四元數(shù)進(jìn)行初始化：

式中：q0，q1，q2，q3為四元數(shù)；ψ，θ，γ分別為航向角，俯仰角，橫滾角。

第二步，當(dāng)陀螺儀數(shù)據(jù)更新后，用畢卡三階算法［11］進(jìn)行四元數(shù)的更新：

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

第4步，對四元數(shù)進(jìn)行擴(kuò)展卡爾曼濾波.本文不對擴(kuò)展卡爾曼濾波展開詳細(xì)敘述，詳細(xì)過程可以參考文獻(xiàn)［12-13］。在擴(kuò)展卡爾曼濾波的時(shí)間更新方程中，先驗(yàn)估計(jì)狀態(tài)量和先驗(yàn)估計(jì)協(xié)方差分別為：

式中：X（k|k-1）為先驗(yàn)估計(jì)狀態(tài)量，在這里初始化為0，P為協(xié)方差矩陣，Q為過程噪聲矩陣。接下去是狀態(tài)更新方程，包括卡爾曼增益：

式中：H，R分別為測量雅克比矩陣和觀測噪聲協(xié)方差矩陣。

狀態(tài)向量更新：

式中：X（k|k）為更新后的狀態(tài)量，在這里是誤差四元數(shù)ΔQ，而Z（k）為加速度計(jì)的測量值與實(shí)際加速度的差值。

協(xié)方差矩陣更新：

式中：P（k|k）即為更新后的協(xié)方差。

2.3 對角度的啟發(fā)式漂移消除

從系統(tǒng)總體框圖中，我們可以看到，啟發(fā)式漂移消除算法的核心思想是采用了一個(gè)反饋系統(tǒng)，利用前一時(shí)刻的航向角對當(dāng)前時(shí)刻的航向角進(jìn)行修正。主要步驟如下：

第1步，我們需要建立坐標(biāo)系，也就是設(shè)定主方向。一般來說，主方向可以設(shè)定為4個(gè)或8個(gè)，在本文中，我們設(shè)定了4個(gè)主方向，因此，主方向間的角度間隔Δ為90°。而每次初始時(shí)刻前進(jìn)的方向即為我們所定義的0°方向，其余3個(gè)方向則分別為90°，180°，270°。

第2步，在反饋回路中，前一時(shí)刻航向角ψi-1相對于當(dāng)前主方向的偏移角以及偏移方向可以用MOD函數(shù)來求取。根據(jù)MOD函數(shù)的定義：

MOD（n，d）=n-dINT（n/d）

其中，INT（k）為取整函數(shù)，結(jié)果為不大于k值的最大整數(shù)。所以MOD函數(shù)的結(jié)果在區(qū)間［0，d］中。將ψi-1，Δ代入MOD函數(shù)，即：

不同的偏移方向，所以接下去的二進(jìn)制控制器就可以通過判斷Ei的正來決定修正的方向。當(dāng)Ei大于0的時(shí)候，則表明航向角方向往左偏移，我們就要對反饋?zhàn)兞縄i減去修正常數(shù)ic，使得航向角方向修正回當(dāng)前主方向；如果Ei小于0，則表明航向角方向往右偏移，我們就要對反饋?zhàn)兞縄i加上修正常數(shù)ic，使得航向角方向修正回當(dāng)前主方向。Ei經(jīng)過二進(jìn)制控制器后，我們可以確定當(dāng)前時(shí)刻的反饋系數(shù)Ii，即：

其中ic為常數(shù)。

第4步，在得到Ii后，對當(dāng)前的航向角進(jìn)行修正，即：ψi=ψraw，i+Ii。最后結(jié)合步態(tài)信息和航向角信息確定人體當(dāng)前的位置信息：

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

為了驗(yàn)證所提算法的有效性，本文以包含MPU6050傳感器的模塊進(jìn)行試驗(yàn)。將模塊固定在鞋子上，并以100 Hz的頻率讀取陀螺儀，加速度計(jì)的數(shù)據(jù)。

首先，我們繞著一個(gè)正方形建筑轉(zhuǎn)一圈，測試結(jié)果如圖3所示。

圖4 樓層內(nèi)部行走測試圖

圖3中，黑色實(shí)線表示實(shí)際行走路線，并且起點(diǎn)和終點(diǎn)重合；虛線表示利用擴(kuò)展卡爾曼濾波后的航向角進(jìn)行軌跡推算的結(jié)果；帶倒三角實(shí)線表示HDE算法的軌跡推算結(jié)果；帶加號(hào)標(biāo)記實(shí)線表示AHDE算法的軌跡推算結(jié)果。我們可以看到，由于陀螺儀誤差，最后兩個(gè)轉(zhuǎn)彎時(shí)，航向角轉(zhuǎn)動(dòng)幅度無法達(dá)到實(shí)際的90°，從而偏離實(shí)際軌跡，而利用航向角直接進(jìn)行軌跡推算存在較大的誤差，再經(jīng)過啟發(fā)式漂移消除算法后，最終誤差大大減小，最終測試位置與實(shí)際位置誤差為1.9 m。

為了檢驗(yàn)該算法在復(fù)雜行走軌跡中的效果，我們在一幢建筑物4樓內(nèi)部進(jìn)行了更為復(fù)雜的行走。測試結(jié)果如圖4所示。圖4中，黑色虛線表示實(shí)際行走路線，起點(diǎn)和終點(diǎn)重合；實(shí)線表示根據(jù)AHDE算法得出的軌跡推算結(jié)果。最終測試位置與實(shí)際位置誤差為1.6 m。

為了檢驗(yàn)系統(tǒng)的魯棒性，主要是看反饋系數(shù)對系統(tǒng)造成多大的影響，我們對同一組數(shù)據(jù)采用不同的反饋系數(shù)進(jìn)行仿真，結(jié)果如圖5所示。

圖5 反饋系數(shù)測試

圖5中，起點(diǎn)為坐標(biāo)原點(diǎn)，帶六邊形實(shí)線和帶加號(hào)標(biāo)記實(shí)線分別是系數(shù)為 0.001、0.028時(shí)，利用HDE算法進(jìn)行修正后的結(jié)果。在0.001和0.028這兩個(gè)極值處，行進(jìn)過程中和真實(shí)軌跡最大誤差分別為3.7 m、4.9 m；當(dāng)系數(shù)小于0.001或大于0.028，誤差會(huì)大幅增大。帶正方形標(biāo)記實(shí)線和帶倒三角標(biāo)記虛線分別是系數(shù)為0.005、0.23時(shí)，利用AHDE算法進(jìn)行修正后的結(jié)果。在0.005和0.23這兩個(gè)極值處的最大誤差分別為2 m、2.5 m；當(dāng)系數(shù)小于0.005或大于0.23，誤差也會(huì)大幅增大。黑色實(shí)線為真實(shí)軌跡?？梢钥吹?，在可接受精度范圍內(nèi)AHDE算法中反饋系數(shù)的可變區(qū)間比HDE算法幾乎提高了一個(gè)數(shù)量級(jí)。因此，該算法對反饋系數(shù)的選擇有更寬的范圍，有效地提高了系統(tǒng)的魯棒性。

4 結(jié)論

本文利用陀螺儀數(shù)據(jù)更新四元數(shù)，并利用加速度信息作為觀測量，對四元數(shù)進(jìn)行擴(kuò)展卡爾曼濾波，最終用四元數(shù)推算出航向角，減少了原來利用陀螺儀數(shù)據(jù)直接求取航向角的誤差。

在對陀螺儀數(shù)據(jù)進(jìn)行啟發(fā)式漂移消除過程中，由于陀螺儀的數(shù)據(jù)首先要經(jīng)過雙重低通濾波，因此，反饋系數(shù)的輕微改變就會(huì)對修正結(jié)果造成很大的影響，降低了系統(tǒng)的魯棒性。而本文提出的系統(tǒng)對反饋系數(shù)的選取要求并沒有十分嚴(yán)格，提高了系統(tǒng)的魯棒性。并且經(jīng)過多次測試，最終的推算位置與實(shí)際位置誤差不超過2 m。

在接下去的工作中，我們將在AHDE的基礎(chǔ)上融合地圖信息，這樣可以避免穿墻等錯(cuò)誤情況出現(xiàn)；除此之外，我們還將改進(jìn)該算法使其更好地適應(yīng)圓形等不規(guī)則軌跡，并且還可以導(dǎo)入磁力計(jì)信息來提高航向角的準(zhǔn)確度。

［1］ Levi Robert W，Judd Thomas.Dead Reckoning Navigational System Using Accelerometer to Measure Foot Impacts［P］.US，5583776. 1996-12-10.

［2］ Johann Borenstein，Lauro Ojeda.Heuristic Drift Elimination for Personnel Tracking Systems［J］.The Journal of Navigation，2010，63（4）：591-606.

［3］ Jimenez A R，Seco F，Zampella F，et al.Improved Heuristic Drift Elimination（iHDE）for Pedestrian Navigation in Complex Buildings ［C］//Indoor Positioning and Indoor Navigation（IPIN）.Guimaraes：IPIN，2011：1-8.

［4］ Zhao N.Full-Featured Pedometer Design Realized with 3-Axis Digital Accelerometer［J］.Analog Dialogue，2010：44（6）：17-21.

［5］ 申崇江，馮成濤，崔瑩，等.穿戴式室內(nèi)行人航位推算系統(tǒng)研究［C］//第五屆中國衛(wèi)星導(dǎo)航學(xué)術(shù)年會(huì)，2014.

［6］ Qin Li，Zhang Huixin，Xu Weixing.Optimizing Algorithms of the Attitude of the Flying Objects［J］.Sensors，Proceeding of IEEE，2004（2）：927-930.

［7］ 張榮輝，賈宏光，陳濤，等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算［J］.光學(xué)精密工程，2008，16（10）：1963-1970.

［8］ 葉锃鋒，馮恩信.基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法［J］.傳感技術(shù)學(xué)報(bào)，2012，25（4）：524-528.

［9］ 鄒波，張華，姜軍.多傳感信息融合的改進(jìn)擴(kuò)展卡爾曼濾波定姿［J］.計(jì)算機(jī)應(yīng)用研究，2014，，31（4）：1035-1038.

［10］秦永元.慣性導(dǎo)航［M］.北京：科學(xué)出版社，2006：14-164.

［11］鄧正隆.慣性技術(shù)［M］.哈爾濱：哈爾濱工業(yè)大學(xué)出版社，2006：140-180.

［12］殷紅.基于foot-mounted的IMU室內(nèi)行人航跡推算研究［D］.南昌：南昌大學(xué)理學(xué)院，2013.

［13］賈瑞才，基于四元數(shù)EKF的低成本MEMS姿態(tài)估計(jì)算法［J］.傳感技術(shù)學(xué)報(bào)，2014，27（1）：90-95.

樓喜中（1976-），男，中國計(jì)量學(xué)院副教授，2005年獲得哈爾濱工業(yè)大學(xué)微電子學(xué)與固體電子學(xué)博士學(xué)位，主要研究方向?yàn)闊o線定位、MEMS傳感器導(dǎo)航定位、多天線技術(shù)、信道編碼技術(shù)等。

Researching on Indoor Pedestrian Trajectory Based on Angle HDE Algorithm

LOU Xizhong1*，ZHOU Leyu1，YE Minzhan2，JIA Yizhan1，JIN Ning1
（1.College of Information Engineering，China Jiliang University，Hangzhou 310018，China；2.China Telecom Co Zhejiang branch，Hangzhou 310040，China）

The Heuristic Drift Elimination（HDE）algorithm has disadvantages such as the inaccurate heading angle and poor robustness，the AHDE algorithm was proposed which implementing Heuristic Drift Elimination based on heading angle with Extended Kalman Filter.The heading angle is renewed by Quaternion which is achieved from the integration of gyroscope data and accelerometer data.After updating the heading angle with the algorithm of Heuristic Drift Elimination，the angle can be used to reckon the pedestrian trajectory combined with step number and stride length.The experimental results show that，when walking in typical structured indoor environments，the error of the proposed algorithm is less than 2 m in 250 m distance，and the error of the HDE algorithm is 4 m roughly.On the other hand，when the sampling data rate of the sensors is 100Hz，the convergence range of feedback coefficient of the AHDE system can be extended to［0.005，0.23］，however the range of the HDE system is［0.001，0.028］.That is to say the AHDE system is more robust than the HDE system.

reckon trajectory；heuristic drift elimination；extended Kalman filter；pedometer EEACC：2575

TN96

A

1004-1699（2015）04-0598-05

10.3969/j.issn.1004-1699.2015.04.024

2014-11-19 修改日期：2014-12-30