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

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

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

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

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

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

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

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

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

2 基于改進(jìn)互補(bǔ)濾波算法的航向角融合

2.1 互補(bǔ)濾波

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

2.2 改進(jìn)型互補(bǔ)濾波

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

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

φerr=φdc-φ

(1)

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

(2)

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

(3)

最終，融合后航向角的計(jì)算公式為

(4)

將其與下一時(shí)刻加速度和磁力計(jì)解算的航向角作差值運(yùn)算，以形成負(fù)反饋。改進(jìn)型互補(bǔ)濾波算法如圖1所示。

3 航向角推算流程

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

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

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

3.2 基于改進(jìn)互補(bǔ)濾波算法解算流程

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

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

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

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

4.2 測試試驗(yàn)

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

4.2.1 室外測試試驗(yàn)

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

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

表1 室外試驗(yàn)精度分析

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

由圖3和表1分析可知：

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

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

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

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

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

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

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

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

5 結(jié) 語

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