基于卡爾曼濾波優(yōu)化航向的PDR算法

朱軍桃,林知宇,李海林,任招財(cái),陳榮生,蘭榮添,代程遠(yuǎn)

(桂林理工大學(xué),廣西 桂林 541004)

隨著智能設(shè)備的發(fā)展,位置信息在現(xiàn)實(shí)生產(chǎn)生活中的應(yīng)用越來(lái)越廣泛,提高了諸多領(lǐng)域的生產(chǎn)生活效率。在室外位置信息的獲取中,GNSS提供了較高精度的位置信息,使得智能設(shè)備廣泛使用的傳感器在定位時(shí)達(dá)到米級(jí)至亞米級(jí)的定位精度,可以應(yīng)對(duì)絕大部分對(duì)于室外位置信息的需求。在室內(nèi)環(huán)境中,由于建筑物本身的遮擋及多路徑效應(yīng)等因素的影響,GNSS信號(hào)存在較大的衰減,同時(shí)多路徑導(dǎo)致定位的誤差較大,造成了在室內(nèi)環(huán)境中GNSS信號(hào)可用性大大降低,無(wú)法提供滿(mǎn)足室內(nèi)使用精度的定位與導(dǎo)航服務(wù)需求。

為了解決上述需求,目前的室內(nèi)定位[1]廣泛采用Wi-Fi[2-3]、地磁場(chǎng)指紋[4-5]、超寬帶技術(shù)(ultra wide band,UWB)[6-7]、計(jì)算機(jī)視覺(jué)[8]、慣性傳感器[9-11]、藍(lán)牙[12]等方法進(jìn)行實(shí)現(xiàn)。Wi-Fi、UWB需要借助信號(hào)發(fā)射裝置并配合接收裝置使用,提供室內(nèi)定位信號(hào),應(yīng)用過(guò)程中需要提前準(zhǔn)備相關(guān)設(shè)備;同時(shí)Wi-Fi、地磁場(chǎng)指紋、計(jì)算機(jī)視覺(jué)等室內(nèi)定位方案需要前期采集數(shù)據(jù),尤其在大范圍應(yīng)用場(chǎng)景下需要準(zhǔn)備的前期工作較多。

行人航位推算(pedestrian dead reckoning, PDR)算法作為一種當(dāng)前相對(duì)成熟、易于實(shí)現(xiàn)的室內(nèi)定位算法,可以?xún)H使用智能設(shè)備的地磁場(chǎng)傳感器、慣性傳感器信息,在不借助外界有源信號(hào)源的情況下,基于初始位置對(duì)設(shè)備位置進(jìn)行位置推算與定位,在室內(nèi)定位應(yīng)用中可以協(xié)助其他傳感器信息對(duì)數(shù)據(jù)進(jìn)行修正進(jìn)而提高定位精度。PDR算法主要包括步數(shù)估計(jì)、步長(zhǎng)估計(jì)及航向估計(jì),其中,航向估計(jì)可以使用慣性傳感器與地磁傳感器實(shí)現(xiàn)。但室內(nèi)定位環(huán)境中磁場(chǎng)環(huán)境復(fù)雜,會(huì)受到電子設(shè)備、金屬制品等的干擾,造成地磁場(chǎng)信息偏差。受傳感器性能的限制,智能設(shè)備傳感器本身存在一定的觀測(cè)誤差,同時(shí)慣性導(dǎo)航誤差會(huì)隨時(shí)間累積,因此在結(jié)合慣性設(shè)備的使用過(guò)程中,位置信息的誤差會(huì)隨時(shí)間而增大,進(jìn)而對(duì)PDR算法中的航向估計(jì)造成影響,即影響位置推算結(jié)果。PDR算法可與Wi-Fi[13]、UWB[14]、計(jì)算機(jī)視覺(jué)等室內(nèi)定位方法相融合,從而顯著提高精度及算法的穩(wěn)定性,減少PDR算法行人軌跡中“穿墻”現(xiàn)象的發(fā)生。

針對(duì)地磁信息在室內(nèi)定位中受到復(fù)雜電磁環(huán)境的影響,以及智能手機(jī)傳感器性能的限制所導(dǎo)致的數(shù)據(jù)精度誤差,PDR算法中航向信息會(huì)產(chǎn)生角度誤差,本文使用卡爾曼濾波算法,結(jié)合陀螺儀與加速度傳感器信息,對(duì)智能設(shè)備在PDR算法中的角度數(shù)據(jù)進(jìn)行優(yōu)化,判斷智能設(shè)備的航向角,并通過(guò)多個(gè)傳感器的數(shù)據(jù)計(jì)算智能設(shè)備轉(zhuǎn)向的角度,結(jié)合優(yōu)化算法,對(duì)航向數(shù)據(jù)的計(jì)算進(jìn)行優(yōu)化,以期能提高PDR算法角度信息的可靠性,進(jìn)而增加PDR算法的定位精度。

1 PDR算法

PDR算法是一種利用設(shè)備本身所集成的傳感器,自主測(cè)量與計(jì)算定位信息的慣性定位方法。通過(guò)設(shè)備集成的慣性傳感器與磁力傳感器,得到距離、角度信息,并結(jié)合上一時(shí)刻的坐標(biāo)數(shù)據(jù),推算當(dāng)前時(shí)刻坐標(biāo)信息。PDR算法的原理如圖1所示,公式如下

圖1 PDR推算位置原理

(1)

式中,(xi,yi)為設(shè)備推算出的在當(dāng)前坐標(biāo)系下的位置坐標(biāo);(x0,y0)為初始狀態(tài)下設(shè)備在當(dāng)前坐標(biāo)系下的位置坐標(biāo),可以通過(guò)上一時(shí)刻PDR算法推算的位置信息得到,也可以通過(guò)Wi-Fi定位、NFC標(biāo)簽等其他方式得到;n為上一時(shí)刻到當(dāng)前時(shí)刻所走過(guò)的步數(shù);l為上一時(shí)刻到當(dāng)前時(shí)刻走過(guò)的n步中每一步對(duì)應(yīng)的步長(zhǎng)。通過(guò)對(duì)應(yīng)時(shí)刻走過(guò)的步數(shù)與對(duì)應(yīng)的步長(zhǎng)信息得到航向角信息。

智能手機(jī)各軸的信息如圖2所示。X軸正向?yàn)槲粘质謾C(jī)情況下的正右方向;Y軸正向?yàn)槲粘质謾C(jī)情況下屏幕的上方方向,即機(jī)身指向正前方;Z軸正向垂直于屏幕,屏幕正面方向?yàn)檎颉?/p>

圖2 智能手機(jī)各軸示意

1.1 步數(shù)檢測(cè)

根據(jù)運(yùn)動(dòng)狀態(tài)的不同,步數(shù)檢測(cè)可以依據(jù)智能手機(jī)手持前進(jìn)狀態(tài)、位于口袋狀態(tài)、導(dǎo)航狀態(tài)等,以及慣性傳感器數(shù)據(jù),采用Y軸加速度的波峰、波谷軸檢測(cè)及合成加速度檢測(cè)的方法進(jìn)行。其中,Y軸加速度波峰、波谷檢測(cè)法適用于手機(jī)手持前進(jìn)狀態(tài)、位于口袋狀態(tài)的步數(shù)檢測(cè);合成加速度適用于手持手機(jī)導(dǎo)航狀態(tài)。

如圖3所示,行人手持手機(jī)前進(jìn)時(shí),手機(jī)隨著手臂擺動(dòng),可以近似為繞著肩膀、以手臂為半徑的圓周運(yùn)動(dòng)。在行人前進(jìn)方向上,每向前行走一步,手機(jī)的運(yùn)動(dòng)軌跡可以近似為前進(jìn)-后退-前進(jìn)或后退-前進(jìn)-后退。此時(shí)Y軸加速度輸出的加速度信息類(lèi)似正弦波,正弦波的波峰-波峰或者波谷-波谷可近似對(duì)應(yīng)實(shí)際行走的一步。因此可以利用波峰或波谷檢測(cè)步數(shù),利用相鄰的波峰或波谷信息,得到每一步對(duì)應(yīng)的加速度數(shù)據(jù),并以此計(jì)算步長(zhǎng)信息。本文將采用波峰檢測(cè)法對(duì)數(shù)據(jù)進(jìn)行檢測(cè)。

圖3 手持手機(jī)前進(jìn)Y軸數(shù)據(jù)

1.2 步長(zhǎng)計(jì)算

當(dāng)前常用的步長(zhǎng)模型包括固定步長(zhǎng)、線性及非線性模型。線性步長(zhǎng)的步頻模型[15]公式為

S=af+b

(2)

式中,f為步頻;a、b為模型系數(shù),可通過(guò)試驗(yàn)擬合得到。

考慮人的行走步長(zhǎng)與身高、體重等因素有一定關(guān)聯(lián)性,固定步長(zhǎng)算法在使用中存在一定的局限性,對(duì)于不同的人可能產(chǎn)生的誤差大小并不相同;同時(shí)由于每一步的步長(zhǎng)并不完全一致,且在轉(zhuǎn)彎等情況下,步長(zhǎng)相比直行有大幅改變,存在一定的隨機(jī)性。因此,基于人的身高、體重等信息計(jì)算得到的線性步長(zhǎng)會(huì)因步長(zhǎng)的隨機(jī)性產(chǎn)生一定的誤差。

本文使用基于行人加速度信息計(jì)算步長(zhǎng)的非線性步長(zhǎng),即利用智能手機(jī)的加速度計(jì)得到的步長(zhǎng)信息。常用的非線性步長(zhǎng)為Weinberg方法[16],公式為

(3)

式中,S為數(shù)據(jù)計(jì)算得到的單步步長(zhǎng)信息;k為非線性步長(zhǎng)模型系數(shù),利用多組試驗(yàn)結(jié)果采用最小二乘法得到;a為智能設(shè)備在Y軸上對(duì)應(yīng)該單步范圍內(nèi)的加速度信息,amax為最大加速度,amin為最小加速度。經(jīng)測(cè)試,Weinberg方法在不同場(chǎng)景下定位精度更高,因此本文采用Weinberg方法。

1.3 航向信息

PDR算法中的航向信息,是利用地磁場(chǎng)信息并結(jié)合加速度計(jì)信息計(jì)算當(dāng)前設(shè)備的姿態(tài),通過(guò)計(jì)算俯仰、偏航角修正磁場(chǎng)數(shù)據(jù)的信息,得到智能設(shè)備在當(dāng)前姿態(tài)下Y軸正向?qū)?yīng)平面指向的航向信息。

通過(guò)加速度計(jì)數(shù)據(jù)計(jì)算得到設(shè)備的俯仰角θ與偏航角γ,公式如下

θ=arcsin(Gy/g)

(4)

γ=arctan(-Gx/Gz)

(5)

式中,G為設(shè)備加速度計(jì)測(cè)得各軸上的加速度信息,與加速度各軸正向相同的為正,相反的為負(fù),單位為m/s;g為當(dāng)?shù)刂亓铀俣取Ｍㄟ^(guò)俯仰角與偏航角計(jì)算得到設(shè)備當(dāng)前的航向角φm為

(6)

式中,M為設(shè)備磁力傳感器測(cè)得各軸上的磁場(chǎng)信息。通過(guò)航向角φm,并結(jié)合磁偏角修正量信息md,計(jì)算得到當(dāng)前設(shè)備Y軸正向指向的方位角信息φn為

φn=φm+md

(7)

式中,磁偏角修正量西偏為負(fù),東偏為正,可通過(guò)公開(kāi)信息查詢(xún)得到當(dāng)?shù)卮牌恰Ｍ勇輧x是智能設(shè)備中檢測(cè)角速度的傳感器。檢測(cè)特定時(shí)間段的角速度,并利用角速度信息與時(shí)間進(jìn)行積分,得到設(shè)備在一個(gè)時(shí)間段內(nèi)繞各軸的旋轉(zhuǎn)角度。陀螺儀受到外界電磁信號(hào)的干擾較小,可以短時(shí)間內(nèi)獲得相對(duì)高精度的角度信息。但陀螺儀在沒(méi)有外界數(shù)據(jù)對(duì)其角度信息進(jìn)行修正的情況下,會(huì)產(chǎn)生誤差累積。具體表現(xiàn)為,在長(zhǎng)時(shí)間的觀測(cè)中,陀螺儀會(huì)出現(xiàn)特定方向上的誤差,即零偏誤差,需要使用前對(duì)陀螺儀進(jìn)行測(cè)量得到誤差信息并加以消除。根據(jù)Android設(shè)備的參考文檔,陀螺儀獲得設(shè)備圍繞各個(gè)角度旋轉(zhuǎn)的角度公式為

r=dt·ω

(8)

式中,dt為陀螺儀記錄的當(dāng)前時(shí)刻與上一時(shí)刻的時(shí)間差;ω為當(dāng)前時(shí)刻與上一時(shí)刻之間的角速度;r為積分得到的dt時(shí)間內(nèi)設(shè)備繞對(duì)應(yīng)軸的旋轉(zhuǎn)角。

利用地磁傳感器的航線信息與陀螺儀的角度信息合成的模型公式為

φk+1=φk±ω·dt+wk·dt

(9)

式中,φk+1為合成后角度;φk為上一時(shí)刻角度,由地磁場(chǎng)數(shù)據(jù)計(jì)算得到;ω為上一時(shí)刻到當(dāng)前時(shí)刻的角速度,由陀螺儀計(jì)算數(shù)據(jù)計(jì)算得到;wk為角速度積分的增量。

2 卡爾曼濾波

卡爾曼濾波是一種在最小均方根誤差準(zhǔn)則下得出的作為一種狀態(tài)最優(yōu)估計(jì)的方法,在無(wú)人機(jī)、機(jī)器人等姿態(tài)解算中被廣泛應(yīng)用,在室內(nèi)定位中也有很大的應(yīng)用潛力[17]。卡爾曼濾波可以根據(jù)上一個(gè)時(shí)刻的系統(tǒng)狀態(tài),推測(cè)下一時(shí)刻的系統(tǒng)真實(shí)狀態(tài)。本文將卡爾曼濾波應(yīng)用于融合磁場(chǎng)、加速度、陀螺儀傳感器的角度信息計(jì)算,以提高航向角精度。

先根據(jù)上一時(shí)刻的數(shù)據(jù)進(jìn)行預(yù)測(cè)狀態(tài)估計(jì),公式為

(10)

利用上一時(shí)刻的協(xié)方差求先驗(yàn)估計(jì)協(xié)方差矩陣,公式為

(11)

計(jì)算卡爾曼增益,公式為

(12)

用更新后的參數(shù)進(jìn)行后驗(yàn)估計(jì),公式為

(13)

更新后驗(yàn)估計(jì)協(xié)方差矩陣,公式為

(14)

利用卡爾曼濾波優(yōu)化航向的PDR算法具體步驟如圖4所示。

圖4 卡爾曼濾波優(yōu)化航向PDR算法步驟

由智能設(shè)備的加速度傳感器、地磁傳感器、陀螺儀分別得到加速度、地磁、陀螺儀的角度數(shù)據(jù),利用加速度數(shù)據(jù)得到步長(zhǎng)、步數(shù)信息,并計(jì)算設(shè)備當(dāng)前的俯仰角、橫滾角,結(jié)合地磁傳感器得到地磁數(shù)據(jù),計(jì)算得到當(dāng)前設(shè)備的航向角信息。利用陀螺儀計(jì)算得到對(duì)應(yīng)時(shí)間內(nèi)繞設(shè)備各軸旋轉(zhuǎn)的角度信息,利用卡爾曼濾波將其與地磁場(chǎng)得到的方位角信息進(jìn)行融合,得到融合后航向角信息。利用融合后的航向角信息與步長(zhǎng)、步數(shù)信息,并將上一時(shí)刻的坐標(biāo)作為計(jì)算的初始坐標(biāo),對(duì)當(dāng)前坐標(biāo)進(jìn)行推算,計(jì)算得到當(dāng)前時(shí)刻的坐標(biāo)。

3 試驗(yàn)結(jié)果分析

3.1 試驗(yàn)設(shè)置

使用小米10手機(jī)作為試驗(yàn)器材,手機(jī)的慣性傳感器、地磁傳感器與陀螺儀均設(shè)置為50 Hz的采樣頻率對(duì)數(shù)據(jù)進(jìn)行采樣。試驗(yàn)開(kāi)始前,智能設(shè)備進(jìn)行“8”字形運(yùn)動(dòng),使磁力傳感器獲取各個(gè)角度的磁場(chǎng)信息,并靜置一段時(shí)間同時(shí)記錄陀螺儀數(shù)據(jù),得到陀螺儀的零偏誤差,在數(shù)據(jù)處理中消除零偏誤差。

在確保試驗(yàn)路線無(wú)行人與物品遮擋與干擾的情況下,采用單手持智能手機(jī)方式前進(jìn),手機(jī)隨手臂自然擺動(dòng),在同一參考行進(jìn)路線上,進(jìn)行20組試驗(yàn)。

3.2 試驗(yàn)分析

將利用加速度計(jì)修正的地磁信息作為航向參考依據(jù)的PDR算法,與利用卡爾曼濾波對(duì)陀螺儀與加速度計(jì)進(jìn)行修正的地磁信息作為航向參考依據(jù)的PDR算法作對(duì)比。兩種方法使用相同試驗(yàn)采集的加速度、地磁、陀螺儀傳感器得到的數(shù)據(jù)進(jìn)行對(duì)比。試驗(yàn)路線圖及其中一組數(shù)據(jù)如圖5所示。

圖5 試驗(yàn)路線及其結(jié)果

由圖5可以看出,在經(jīng)過(guò)第2次轉(zhuǎn)向后,未經(jīng)優(yōu)化的PDR算法得到的角度數(shù)據(jù),與設(shè)計(jì)路線產(chǎn)生了一定的夾角,導(dǎo)致最終的定位誤差,且誤差會(huì)隨著距離與時(shí)間的增加而增大;而優(yōu)化后的PDR算法角度數(shù)據(jù)更接近于設(shè)計(jì)路線,誤差隨距離與時(shí)間而增大的趨勢(shì)較小,即優(yōu)化后的角度信息更接近于真實(shí)情況,定位信息更準(zhǔn)確。20次試驗(yàn)的誤差分布如圖6所示。

圖6 20次試驗(yàn)誤差分布

20次試驗(yàn)PDR算法得到的坐標(biāo)與設(shè)計(jì)線路的誤差分布區(qū)間見(jiàn)表1。

表1 不同方法誤差分布 m

由表1可知,20次試驗(yàn)中,經(jīng)過(guò)卡爾曼濾波融合的航向信息相比未經(jīng)卡爾曼濾波處理的數(shù)據(jù),主要分布在(1,2]m誤差區(qū)間內(nèi),占55%;[0,0.5]m及(0.5,1]m誤差區(qū)間的結(jié)果占35%;>2 m的誤差僅占10%。未經(jīng)濾波的數(shù)據(jù),僅有15%的數(shù)據(jù)在[0,2]m的誤差區(qū)間內(nèi),75%的數(shù)據(jù)誤差大于2 m。同一次試驗(yàn)中,使用卡爾曼濾波對(duì)航向數(shù)據(jù)融合的PDR算法得到的結(jié)果誤差,均小于僅使用地磁得到航向信息的PDR算法得到的結(jié)果,即使用卡爾曼濾波修正航向后的數(shù)據(jù)精度更高。

使用不同方法獲得航向信息的試驗(yàn)誤差結(jié)果見(jiàn)表2。

表2 不同方法的誤差 m

由表2可知,使用加速度對(duì)俯仰、偏航角進(jìn)行修正的地磁信息為航向的PDR算法,其平均誤差為2.93 m,最大誤差為4.21 m,中誤差為3.07 m;而利用卡爾曼濾波使用陀螺儀與加速度對(duì)俯仰、偏航角進(jìn)行修正的地磁信息進(jìn)行融合得到的航向信息的PDR算法,精度有明顯提升,平均誤差為1.36 m,最大誤差為2.22 m,中誤差為1.36 m。利用卡爾曼濾波優(yōu)化航向的PDR算法相對(duì)于僅使用地磁的PDR算法,平均誤差減小了1.63 m,最大誤差減小了1.99 m,中誤差減小了1.71 m。

同時(shí),在20組試驗(yàn)數(shù)據(jù)計(jì)算得到的路線中,由于部分?jǐn)?shù)據(jù)的行進(jìn)路線靠近附近行人與電子設(shè)備,造成磁場(chǎng)傳感器采集數(shù)據(jù)產(chǎn)生一定波動(dòng),對(duì)航向角指示產(chǎn)生了偏差,在未經(jīng)過(guò)卡爾曼濾波處理的情況下,行進(jìn)方向會(huì)出現(xiàn)偏差,影響最終的定位精度;在使用卡爾曼濾波對(duì)航向進(jìn)行優(yōu)化的情況下,上述干擾仍然對(duì)最終定位精度產(chǎn)生了一定的影響。

4 結(jié) 語(yǔ)

智能手機(jī)地磁傳感器測(cè)量精度相對(duì)有限,且室內(nèi)定位中的室內(nèi)電磁環(huán)境復(fù)雜,影響地磁場(chǎng)的測(cè)量精度。PDR算法使用慣性與地磁場(chǎng)信息,在慣性導(dǎo)航中誤差會(huì)不斷累積。本文使用卡爾曼濾波,結(jié)合慣性傳感器加速度信息,對(duì)地磁場(chǎng)傳感器數(shù)據(jù)的精度進(jìn)行改進(jìn)。經(jīng)試驗(yàn)驗(yàn)證,優(yōu)化后最終平均誤差由2.93 m提升至1.36 m,大部分結(jié)果誤差在2 m范圍內(nèi)。

但該算法仍有較大的改進(jìn)空間,可以考慮使用深度學(xué)習(xí)等方法進(jìn)行優(yōu)化;同時(shí)可利用Wi-Fi、UWB、計(jì)算機(jī)視覺(jué)、NFC標(biāo)簽等有源數(shù)據(jù)獲取的位置信息與PDR算法的定位信息進(jìn)行比較與融合,提升定位精度。