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

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

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

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

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

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

1 PDR算法

PDR算法是一種利用設(shè)備本身所集成的傳感器,自主測量與計算定位信息的慣性定位方法。通過設(shè)備集成的慣性傳感器與磁力傳感器,得到距離、角度信息,并結(jié)合上一時刻的坐標(biāo)數(shù)據(jù),推算當(dāng)前時刻坐標(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),可以通過上一時刻PDR算法推算的位置信息得到,也可以通過Wi-Fi定位、NFC標(biāo)簽等其他方式得到;n為上一時刻到當(dāng)前時刻所走過的步數(shù);l為上一時刻到當(dāng)前時刻走過的n步中每一步對應(yīng)的步長。通過對應(yīng)時刻走過的步數(shù)與對應(yīng)的步長信息得到航向角信息。

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

圖2 智能手機各軸示意

1.1 步數(shù)檢測

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

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

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

1.2 步長計算

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

S=af+b

(2)

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

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

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

(3)

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

1.3 航向信息

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

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

θ=arcsin(Gy/g)

(4)

γ=arctan(-Gx/Gz)

(5)

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

(6)

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

φn=φm+md

(7)

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

r=dt·ω

(8)

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

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

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

(9)

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

2 卡爾曼濾波

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

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

(10)

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

(11)

計算卡爾曼增益,公式為

(12)

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

(13)

更新后驗估計協(xié)方差矩陣,公式為

(14)

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

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

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

3 試驗結(jié)果分析

3.1 試驗設(shè)置

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

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

3.2 試驗分析

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

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

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

圖6 20次試驗誤差分布

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

表1 不同方法誤差分布 m

由表1可知,20次試驗中,經(jīng)過卡爾曼濾波融合的航向信息相比未經(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。同一次試驗中,使用卡爾曼濾波對航向數(shù)據(jù)融合的PDR算法得到的結(jié)果誤差,均小于僅使用地磁得到航向信息的PDR算法得到的結(jié)果,即使用卡爾曼濾波修正航向后的數(shù)據(jù)精度更高。

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

表2 不同方法的誤差 m

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

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

4 結(jié) 語

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

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