基于手機傳感器和互補濾波的行人航向解算

郭 英，孫玉曦,2，姬現(xiàn)磊，馮茗楊，劉清華

(1. 山東科技大學測繪科學與工程學院, 山東 青島 266590； 2. 中國測繪科學研究院，北京 100830)

隨著新型移動設(shè)備(如智能手機、平板電腦、運動手環(huán))及基于位置的服務(location based service，LBS)的迅速發(fā)展，行人航跡推算(pedestrian dead reckoning，PDR)作為室內(nèi)定位的重要技術(shù)，逐漸成為有關(guān)專家和部門研究與應用的熱點。PDR中的姿態(tài)信息主要通過由陀螺儀、加速度計組成的微慣性測量單元(miniature inertial measurement unit，MIMU)測量獲得，但是由于MIMU的漂移及運動噪聲的影響，若不加以處理，這些誤差會隨著時間的增加而積累，導致導航精度降低或完全無法導航[1]。

如何較好地降低各種誤差的影響，專家、學者進行了一系列研究。將陀螺儀和加速度傳感器信號進行融合，從而修正陀螺儀的偏差，該方法能較好地減少水平面傾角的漂移[2]。文獻[3]給出了基于四元數(shù)法的互補濾波器，并通過調(diào)節(jié)互補濾波截止頻率，降低了姿態(tài)解算的運算量，提高了效率和精度。

以上幾種方法雖然都能對行人航向角進行一定的修正，但都沒有考慮外界環(huán)境的影響。由于地磁場復雜多變，且易受外界環(huán)境的干擾，使得導航定位發(fā)生偏差。文獻[4]提出了一種基于準靜止磁場檢測的行人導航算法框架，該方法能在磁干擾環(huán)境中有效地修正航向漂移誤差，但其流程復雜，難以理解與實現(xiàn)。

如何在外界磁干擾環(huán)境及傳感器噪聲較大的情況下較好地推算航向角，已經(jīng)成為研究者們亟待解決的問題?；诖耍疚脑诜治龌パa濾波原理的基礎(chǔ)上，加入PI調(diào)節(jié)器，并以陀螺儀輸出的角速度數(shù)據(jù)為依據(jù)，實時調(diào)節(jié)PI調(diào)節(jié)器的誤差補償系數(shù)，提出一種可以自適應補償系數(shù)的改進型互補濾波算法，以提高濾波效果和航向角融合精度。

1 傳感器姿態(tài)解算

目前，利用陀螺儀進行姿態(tài)解算，用于描述載體坐標系與導航坐標系轉(zhuǎn)換關(guān)系的方法有歐拉角法、方向余弦法和四元數(shù)法等。其中，歐拉角法在求解姿態(tài)時存在奇點，不能用于全姿態(tài)的解算[5]；方向余弦可用于全姿態(tài)的解算但計算量大，不能滿足實時性要求[6]；四元數(shù)法算法簡單，易于操作，只需要求解4個參數(shù)，計算量小，可以在實現(xiàn)過程中修正漂移，應用比較廣泛[7]。

基于四元數(shù)的AHRS姿態(tài)解算算法，根據(jù)采用數(shù)據(jù)的不同，分為三軸陀螺儀數(shù)據(jù)解算、六軸數(shù)據(jù)融合解算及九軸數(shù)據(jù)融合解算。

2 基于改進互補濾波算法的航向角融合

2.1 互補濾波

陀螺儀動態(tài)響應特性良好，但計算姿態(tài)時，長時間工作會產(chǎn)生累積誤差，使得解算出來的姿態(tài)出現(xiàn)漂移。加速度計測量姿態(tài)沒有累積誤差，但動態(tài)響應較差。它們在頻域上具有互補性，可利用互補濾波原理對陀螺儀測得的姿態(tài)角進行高通濾波，對加速度計測得的姿態(tài)角進行低通濾波，得到在高頻段和低頻段都較好的信號，提高測量精度和系統(tǒng)的動態(tài)性能[7]。

2.2 改進型互補濾波

雖然互補濾波能同時消除低頻和高頻的干擾，實現(xiàn)姿態(tài)數(shù)據(jù)的融合，但是由于互補濾波的低通阻帶衰減較慢，且噪聲較大時誤差大，濾波效果不理想[8]。因此，本文在此基礎(chǔ)上，增加PI調(diào)節(jié)器，并以陀螺儀輸出的角速度數(shù)據(jù)為依據(jù)，計算適當?shù)拈撝?，實時調(diào)整補償系數(shù)kp，構(gòu)成改進型互補濾波器，提高姿態(tài)數(shù)據(jù)融合精度，從而解算出準確的航向角。

以航向角φ為例，首先，將通過磁力計和加速度計解算的航向角φdc和上一時刻的最終航向角φ作差

φerr=φdc-φ

(1)

然后，將φerr作為PI調(diào)節(jié)器的輸入，進行修正誤差

(2)

補償系數(shù)kp可以通過式(3)得到，其中，ωmax為手機陀螺儀的最大量程，ωres為手機陀螺儀的分辨率，ki取0.000 6。

(3)

最終，融合后航向角的計算公式為

(4)

將其與下一時刻加速度和磁力計解算的航向角作差值運算，以形成負反饋。改進型互補濾波算法如圖1所示。

3 航向角推算流程

3.1 九軸數(shù)據(jù)融合解算流程

融合九軸數(shù)據(jù)的姿態(tài)解算算法是在數(shù)據(jù)層進行融合的，其將加速度計數(shù)據(jù)向量叉乘誤差與磁力計數(shù)據(jù)向量叉乘誤差相加，作為PI調(diào)節(jié)的輸入，得到補償值來修正陀螺儀的角速度輸出；然后通過解算四元數(shù)微分方程，求得更新后的四元數(shù)，進而根據(jù)姿態(tài)矩陣的四元數(shù)表達式解算實時姿態(tài)角，將解算的航向角作為行人航向角進行PDR軌跡跟蹤。

由于該算法把加速度計、陀螺儀和磁力計原始數(shù)據(jù)融合在一起進行四元數(shù)解算，當磁力計突然受到強磁干擾時，其自身的誤差會擴散到橫滾角和俯仰角，從而影響整個姿態(tài)角的解算精度，使得解算出的航向角出現(xiàn)較大的誤差。

3.2 基于改進互補濾波算法解算流程

將加速度計和磁力計數(shù)據(jù)解算的航向角和陀螺儀輸出的角速度數(shù)據(jù)進行互補濾波融合，得到融合后的航向角，又將其與手機電子羅盤解算的航向角作差值運算，作為PI調(diào)節(jié)的輸入，形成負反饋。由于本文算法是將電子羅盤解算的航向角和陀螺儀積分后航向角在外部融合，即使有強磁干擾，也不會造成全方位的失控。將融合的φ作為行人的航向角，對行人運動過程進行軌跡跟蹤。本文算法流程如圖2所示。

4 試驗驗證和結(jié)果分析

4.1 數(shù)據(jù)采集平臺

本文采用mi6智能手機作為行人導航數(shù)據(jù)采集與存儲模塊進行測試試驗。在mi6智能手機上安裝AndroSensor，用來采集行人運動過程中的三軸加速度、三軸陀螺儀及三軸磁力計數(shù)據(jù)。為了達到較高的測量精度，在采集數(shù)據(jù)之前要對手機的內(nèi)置傳感器進行校準。加速度計和陀螺儀要進行零偏校準[18]，磁力計要進行橢球校正。

4.2 測試試驗

為了驗證本文算法對陀螺儀累計誤差有較好的修正效果，在室外環(huán)境下，設(shè)計了多種行進路線銜接的獨特路線，并以實際行人行走路線為參考，進行了有、無PI調(diào)節(jié)互補濾波融合的對比試驗；考慮外界磁場環(huán)境對試驗的影響，又在外界磁干擾強、弱環(huán)境下，分別與九軸數(shù)據(jù)融合算法進行對比，驗證了本文算法有較強的抗磁干擾性能。

4.2.1 室外測試試驗

本文選擇學校的足球場作為室外試驗場地，是由于足球場較為空曠，沒有高大建筑物，可視為外界磁場干擾較小的環(huán)境。在足球場的罰球區(qū)設(shè)計如圖3所示的閉合路線(路線設(shè)計包含直線行走，弧線行走，直角轉(zhuǎn)彎及任意角度轉(zhuǎn)彎，涵蓋了行人正常行走的各種情況)，測試人員手持智能手機進行校準后，圍繞操場行走一個如圖3所示的設(shè)計路線，然后通過設(shè)計的航向解算算法對手機傳感器數(shù)據(jù)進行處理，得到其在室外環(huán)境下的行人行走軌跡。

室外測試試驗數(shù)據(jù)統(tǒng)計結(jié)果見表1。

表1 室外試驗精度分析

注：DTO為路線閉合差與其總路線的比值[11]；AHE為平均航向誤差。

由圖3和表1分析可知：

(1) 本文算法適用于多種行進路線(直線行走，弧線行走，直角轉(zhuǎn)彎及任意角度轉(zhuǎn)彎)(如圖3所示)。

(2) 由于室外環(huán)境下外界磁干擾較弱，加入PI調(diào)節(jié)，行人軌跡閉合差與無PI算法相比，相差不大；而AHE則大幅度減小，減小了1.9°，驗證了本文算法能夠?qū)π腥撕较蚪沁M行較好的修正(詳見表1)。

(3) 本文算法與九軸數(shù)據(jù)融合算法在室外環(huán)境下，行人軌跡閉合差分別為1.2和0.8 m，AHE分別為-1.2°和-1.7°(見表1)，相差不大，驗證了其在室外弱磁干擾環(huán)境中的適用性。

4.2.2 室內(nèi)測試試驗

本文選擇學校J6的室內(nèi)矩形過道作為室內(nèi)試驗場地，路線長62 m，寬42 m。由于室內(nèi)環(huán)境下，現(xiàn)代建筑物的鋼筋混凝土結(jié)構(gòu)及辦公室門把手等都會對磁力計數(shù)據(jù)產(chǎn)生干擾，視為外界磁場干擾較大的環(huán)境。測試人員手持智能手機，進行校準以后，圍繞J6矩形過道進行測試試驗，然后通過設(shè)計的航向解算算法對手機傳感器數(shù)據(jù)進行處理，得到其在室內(nèi)環(huán)境下行人行走的軌跡圖，如圖4所示。

室內(nèi)測試試驗數(shù)據(jù)統(tǒng)計結(jié)果見表2。

表2 室內(nèi)試驗精度分析

由圖4和表2分析可知：

(1) 在室內(nèi)環(huán)境下，有PI調(diào)節(jié)算法，其DTO誤差為3.0%，與無PI調(diào)節(jié)算法相比定位精度提升了68.4%，與九軸融合算法相比定位精度提升了65.9%。

(2) 由于室內(nèi)環(huán)境下外界磁干擾增強，九軸數(shù)據(jù)融合算法解算的行人軌跡出現(xiàn)明顯的偏差，本文算法解算行人軌跡則與參考路線較為吻合(如圖4(b)所示)，其AHE減小了1.8°(見表2)，驗證了本文算法有較強的抗磁干擾性能，即使在磁干擾的環(huán)境下也有較強的適用性。

5 結(jié) 語

基于低成本的智能手機，針對傳統(tǒng)行人航向解算算法中存在陀螺儀累積誤差的問題，本文提出了一種改進的航向角融合算法。該算法根據(jù)智能手機傳感器自身的特點，對互補濾波算法進行了一定的改進，并根據(jù)陀螺儀輸出角速度數(shù)據(jù)，實時調(diào)節(jié)誤差補償系數(shù)進行PI調(diào)節(jié)，將調(diào)節(jié)后的手機電子羅盤解算的航向角與陀螺儀數(shù)據(jù)進行融合，得到融合后更可靠的航向角。試驗證明，該算法對于磁干擾較強的環(huán)境也有很好的適用性，能較好地過濾掉加速度計的噪聲干擾、收斂陀螺儀的累積積分誤差，滿足使用者的需求，對于低成本的室內(nèi)行人導航有一定的應用價值。