基于卡爾曼濾波優化航向的PDR算法

朱軍桃,林知宇,李海林,任招財,陳榮生,蘭榮添,代程遠

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

隨著智能設備的發展,位置信息在現實生產生活中的應用越來越廣泛,提高了諸多領域的生產生活效率。在室外位置信息的獲取中,GNSS提供了較高精度的位置信息,使得智能設備廣泛使用的傳感器在定位時達到米級至亞米級的定位精度,可以應對絕大部分對于室外位置信息的需求。在室內環境中,由于建筑物本身的遮擋及多路徑效應等因素的影響,GNSS信號存在較大的衰減,同時多路徑導致定位的誤差較大,造成了在室內環境中GNSS信號可用性大大降低,無法提供滿足室內使用精度的定位與導航服務需求。

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

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

針對地磁信息在室內定位中受到復雜電磁環境的影響,以及智能手機傳感器性能的限制所導致的數據精度誤差,PDR算法中航向信息會產生角度誤差,本文使用卡爾曼濾波算法,結合陀螺儀與加速度傳感器信息,對智能設備在PDR算法中的角度數據進行優化,判斷智能設備的航向角,并通過多個傳感器的數據計算智能設備轉向的角度,結合優化算法,對航向數據的計算進行優化,以期能提高PDR算法角度信息的可靠性,進而增加PDR算法的定位精度。

1 PDR算法

PDR算法是一種利用設備本身所集成的傳感器,自主測量與計算定位信息的慣性定位方法。通過設備集成的慣性傳感器與磁力傳感器,得到距離、角度信息,并結合上一時刻的坐標數據,推算當前時刻坐標信息。PDR算法的原理如圖1所示,公式如下

圖1 PDR推算位置原理

(1)

式中,(xi,yi)為設備推算出的在當前坐標系下的位置坐標;(x0,y0)為初始狀態下設備在當前坐標系下的位置坐標,可以通過上一時刻PDR算法推算的位置信息得到,也可以通過Wi-Fi定位、NFC標簽等其他方式得到;n為上一時刻到當前時刻所走過的步數;l為上一時刻到當前時刻走過的n步中每一步對應的步長。通過對應時刻走過的步數與對應的步長信息得到航向角信息。

智能手機各軸的信息如圖2所示。X軸正向為握持手機情況下的正右方向;Y軸正向為握持手機情況下屏幕的上方方向,即機身指向正前方;Z軸正向垂直于屏幕,屏幕正面方向為正向。

圖2 智能手機各軸示意

1.1 步數檢測

根據運動狀態的不同,步數檢測可以依據智能手機手持前進狀態、位于口袋狀態、導航狀態等,以及慣性傳感器數據,采用Y軸加速度的波峰、波谷軸檢測及合成加速度檢測的方法進行。其中,Y軸加速度波峰、波谷檢測法適用于手機手持前進狀態、位于口袋狀態的步數檢測;合成加速度適用于手持手機導航狀態。

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

圖3 手持手機前進Y軸數據

1.2 步長計算

當前常用的步長模型包括固定步長、線性及非線性模型。線性步長的步頻模型[15]公式為

S=af+b

(2)

式中,f為步頻;a、b為模型系數,可通過試驗擬合得到。

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

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

(3)

式中,S為數據計算得到的單步步長信息;k為非線性步長模型系數,利用多組試驗結果采用最小二乘法得到;a為智能設備在Y軸上對應該單步范圍內的加速度信息,amax為最大加速度,amin為最小加速度。經測試,Weinberg方法在不同場景下定位精度更高,因此本文采用Weinberg方法。

1.3 航向信息

PDR算法中的航向信息,是利用地磁場信息并結合加速度計信息計算當前設備的姿態,通過計算俯仰、偏航角修正磁場數據的信息,得到智能設備在當前姿態下Y軸正向對應平面指向的航向信息。

通過加速度計數據計算得到設備的俯仰角θ與偏航角γ,公式如下

θ=arcsin(Gy/g)

(4)

γ=arctan(-Gx/Gz)

(5)

式中,G為設備加速度計測得各軸上的加速度信息,與加速度各軸正向相同的為正,相反的為負,單位為m/s;g為當地重力加速度。通過俯仰角與偏航角計算得到設備當前的航向角φm為

(6)

式中,M為設備磁力傳感器測得各軸上的磁場信息。通過航向角φm,并結合磁偏角修正量信息md,計算得到當前設備Y軸正向指向的方位角信息φn為

φn=φm+md

(7)

式中,磁偏角修正量西偏為負,東偏為正,可通過公開信息查詢得到當地磁偏角。陀螺儀是智能設備中檢測角速度的傳感器。檢測特定時間段的角速度,并利用角速度信息與時間進行積分,得到設備在一個時間段內繞各軸的旋轉角度。陀螺儀受到外界電磁信號的干擾較小,可以短時間內獲得相對高精度的角度信息。但陀螺儀在沒有外界數據對其角度信息進行修正的情況下,會產生誤差累積。具體表現為,在長時間的觀測中,陀螺儀會出現特定方向上的誤差,即零偏誤差,需要使用前對陀螺儀進行測量得到誤差信息并加以消除。根據Android設備的參考文檔,陀螺儀獲得設備圍繞各個角度旋轉的角度公式為

r=dt·ω

(8)

式中,dt為陀螺儀記錄的當前時刻與上一時刻的時間差;ω為當前時刻與上一時刻之間的角速度;r為積分得到的dt時間內設備繞對應軸的旋轉角。

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

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

(9)

式中,φk+1為合成后角度;φk為上一時刻角度,由地磁場數據計算得到;ω為上一時刻到當前時刻的角速度,由陀螺儀計算數據計算得到;wk為角速度積分的增量。

2 卡爾曼濾波

卡爾曼濾波是一種在最小均方根誤差準則下得出的作為一種狀態最優估計的方法,在無人機、機器人等姿態解算中被廣泛應用,在室內定位中也有很大的應用潛力[17]。卡爾曼濾波可以根據上一個時刻的系統狀態,推測下一時刻的系統真實狀態。本文將卡爾曼濾波應用于融合磁場、加速度、陀螺儀傳感器的角度信息計算,以提高航向角精度。

先根據上一時刻的數據進行預測狀態估計,公式為

(10)

利用上一時刻的協方差求先驗估計協方差矩陣,公式為

(11)

計算卡爾曼增益,公式為

(12)

用更新后的參數進行后驗估計,公式為

(13)

更新后驗估計協方差矩陣,公式為

(14)

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

圖4 卡爾曼濾波優化航向PDR算法步驟

由智能設備的加速度傳感器、地磁傳感器、陀螺儀分別得到加速度、地磁、陀螺儀的角度數據,利用加速度數據得到步長、步數信息,并計算設備當前的俯仰角、橫滾角,結合地磁傳感器得到地磁數據,計算得到當前設備的航向角信息。利用陀螺儀計算得到對應時間內繞設備各軸旋轉的角度信息,利用卡爾曼濾波將其與地磁場得到的方位角信息進行融合,得到融合后航向角信息。利用融合后的航向角信息與步長、步數信息,并將上一時刻的坐標作為計算的初始坐標,對當前坐標進行推算,計算得到當前時刻的坐標。

3 試驗結果分析

3.1 試驗設置

使用小米10手機作為試驗器材,手機的慣性傳感器、地磁傳感器與陀螺儀均設置為50 Hz的采樣頻率對數據進行采樣。試驗開始前,智能設備進行“8”字形運動,使磁力傳感器獲取各個角度的磁場信息,并靜置一段時間同時記錄陀螺儀數據,得到陀螺儀的零偏誤差,在數據處理中消除零偏誤差。

在確保試驗路線無行人與物品遮擋與干擾的情況下,采用單手持智能手機方式前進,手機隨手臂自然擺動,在同一參考行進路線上,進行20組試驗。

3.2 試驗分析

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

圖5 試驗路線及其結果

由圖5可以看出,在經過第2次轉向后,未經優化的PDR算法得到的角度數據,與設計路線產生了一定的夾角,導致最終的定位誤差,且誤差會隨著距離與時間的增加而增大;而優化后的PDR算法角度數據更接近于設計路線,誤差隨距離與時間而增大的趨勢較小,即優化后的角度信息更接近于真實情況,定位信息更準確。20次試驗的誤差分布如圖6所示。

圖6 20次試驗誤差分布

20次試驗PDR算法得到的坐標與設計線路的誤差分布區間見表1。

表1 不同方法誤差分布 m

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

使用不同方法獲得航向信息的試驗誤差結果見表2。

表2 不同方法的誤差 m

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

同時,在20組試驗數據計算得到的路線中,由于部分數據的行進路線靠近附近行人與電子設備,造成磁場傳感器采集數據產生一定波動,對航向角指示產生了偏差,在未經過卡爾曼濾波處理的情況下,行進方向會出現偏差,影響最終的定位精度;在使用卡爾曼濾波對航向進行優化的情況下,上述干擾仍然對最終定位精度產生了一定的影響。

4 結 語

智能手機地磁傳感器測量精度相對有限,且室內定位中的室內電磁環境復雜,影響地磁場的測量精度。PDR算法使用慣性與地磁場信息,在慣性導航中誤差會不斷累積。本文使用卡爾曼濾波,結合慣性傳感器加速度信息,對地磁場傳感器數據的精度進行改進。經試驗驗證,優化后最終平均誤差由2.93 m提升至1.36 m,大部分結果誤差在2 m范圍內。

但該算法仍有較大的改進空間,可以考慮使用深度學習等方法進行優化;同時可利用Wi-Fi、UWB、計算機視覺、NFC標簽等有源數據獲取的位置信息與PDR算法的定位信息進行比較與融合,提升定位精度。