擴(kuò)展卡爾曼濾波的安卓手機(jī)定位算法研究

袁陽(yáng),周汝雁,張?jiān)?/p>

(1. 上海海洋大學(xué) 信息學(xué)院,上海 201306;2. 農(nóng)業(yè)部漁業(yè)信息重點(diǎn)實(shí)驗(yàn)室,上海 201306)

0 引 言

隨著手機(jī)的廣泛使用以及人們對(duì)手機(jī)的依賴程度的提高,手機(jī)定位應(yīng)用的場(chǎng)景也越來(lái)越多,這種需求促使市場(chǎng)上出現(xiàn)了多種手機(jī)定位軟件．現(xiàn)代生活節(jié)奏的加快,日常生活中實(shí)時(shí)獲取高精度地理位置信息也成為人們不斷關(guān)注的熱點(diǎn),例如:危險(xiǎn)品運(yùn)輸車定位追蹤[1],無(wú)人機(jī)配送[2]等．目前智能手機(jī)室內(nèi)外定位的研究才剛起步,科研成果較少,主要原因是智能手機(jī)全球定位系統(tǒng)(GPS)定位模塊在性能上與 GPS 接收機(jī)相差甚遠(yuǎn)[3]．任超等[4]研究了基于 GPSOne 技術(shù)的智能手機(jī)定位精度及可靠性;Zandbergen等[5]比較了輔助GPS、WiFi 和蜂窩定位技術(shù),還研究了高靈敏度GPS手機(jī)的定位精度[6];但以上研究皆是使用輔助設(shè)備來(lái)提高手機(jī)定位精度,本文研究了利用Android手機(jī)自帶的GPS芯片,通過(guò)設(shè)計(jì)擴(kuò)展卡爾曼濾波定位算法來(lái)提高定位精度．

目前求解用戶位置最常用的算法是加權(quán)最小二乘法．加權(quán)最小二乘法的思想就是賦予每個(gè)衛(wèi)星的偽距測(cè)量值不同的權(quán)重,權(quán)重越大的偽距測(cè)量值在解方程的過(guò)程中起到的作用越大[7]． 加權(quán)最小二乘算法定位解算的步驟為: 數(shù)據(jù)準(zhǔn)備與初值設(shè)定、線性化方程組、求解矩陣方程以及迭代性判斷．加權(quán)最小二乘法不需要?jiǎng)討B(tài)知識(shí),僅使用偽距測(cè)量來(lái)估計(jì)位置,但該方法存在缺點(diǎn):估計(jì)的質(zhì)量完全取決于測(cè)量的質(zhì)量,并且其要求最少的測(cè)量次數(shù)(要估計(jì)用戶位置和接收機(jī)時(shí)鐘偏差,通常需要4個(gè)衛(wèi)星)．卡爾曼濾波技術(shù)是20世紀(jì)60年代產(chǎn)生的一種遞推線性最小方差估計(jì)技術(shù),估計(jì)誤差最小的更多狀態(tài)的估計(jì)值[8],具有以下特點(diǎn)[9]: 1)處理對(duì)象是隨機(jī)信號(hào); 2)被處理對(duì)象無(wú)有用和干擾之分,被處理信號(hào)通過(guò)濾波進(jìn)行估算; 3)系統(tǒng)的白噪聲激勵(lì)和量測(cè)噪聲并非要濾除,而是估計(jì)所需要的統(tǒng)計(jì)特征．GPS動(dòng)態(tài)定位自適應(yīng)卡爾曼濾波算法,通過(guò)仿真實(shí)驗(yàn)可知該方法快速有效地提高了GPS定位精度[10],濾波過(guò)程穩(wěn)定[11-12]．并且擴(kuò)展卡爾曼濾波在偽距差分定位[13]、多徑誤差削弱[14]、松組合導(dǎo)航[15]等應(yīng)用中具有較好的效果．本文就是在擴(kuò)展卡爾曼濾波算法的基礎(chǔ)上,具體設(shè)計(jì)并實(shí)現(xiàn)了靜態(tài)卡爾曼濾波和動(dòng)態(tài)卡爾曼濾波算法,兩種算法最主要的差別就是它們?cè)跔顟B(tài)向量和狀態(tài)轉(zhuǎn)移矩陣上的差別,動(dòng)態(tài)卡爾曼濾波在狀態(tài)向量中增加了速度參數(shù),在低速動(dòng)態(tài)實(shí)驗(yàn)中能夠結(jié)合時(shí)間來(lái)獲得更好的定位結(jié)果．

隨著Android 7.0系統(tǒng)的發(fā)布,手機(jī)芯片提供了有關(guān)定位的應(yīng)用程序接口(API),應(yīng)用開發(fā)者可以根據(jù)相關(guān)API獲取定位所需數(shù)據(jù),通過(guò)使用更好的定位算法來(lái)實(shí)現(xiàn)更高的定位精度．本文就是通過(guò)API中提供的location類獲取定位所需參數(shù),同時(shí)使用安卓開發(fā)軟件設(shè)計(jì)了靜態(tài)卡爾曼濾波、動(dòng)態(tài)卡爾曼濾波定位算法．鑒于手機(jī)的應(yīng)用場(chǎng)景,本文完成了靜態(tài)與低速動(dòng)態(tài)實(shí)驗(yàn),并與加權(quán)最小二乘法進(jìn)行了對(duì)比分析,結(jié)果表明兩種擴(kuò)展卡爾曼濾波算法具有更好的定位效果．

1 算法描述

1.1 偽距測(cè)量

對(duì)于衛(wèi)星與接收機(jī)之間偽距的測(cè)量,有如下非線性方程:

ΡRC=r+δtR-δtS+dion+dtrop+ε，

(1)

式中:ΡRC為衛(wèi)星與接收機(jī)之間的偽距;r為衛(wèi)星與接收機(jī)之間的幾何距離;δtR為接收機(jī)鐘差;δtS為衛(wèi)星鐘差;dion為電離層延遲;dtrop為對(duì)流層延遲;ε為接收機(jī)噪聲．

如果知道接收機(jī)的近似位置,就可以將公式(1)線性化,其近似位置可以通過(guò)系統(tǒng)API中得到．所以在近似位置坐標(biāo)處用一階泰勒級(jí)數(shù)展開,得到如下線性方程:

ΡRC-r0+δtS-d0,ion-d0,trop，R

(2)

式中: (XS,YS,ZS)為衛(wèi)星的位置坐標(biāo)；(X0,Y0,Z0)為接收機(jī)的近似位置坐標(biāo)．下標(biāo)0表示通過(guò)使用近似接收機(jī)位置信息來(lái)估計(jì)的參數(shù)．通過(guò)該線性化偽距方程,可以得到觀測(cè)矩陣．

1.2 卡爾曼濾波

1.2.1 擴(kuò)展卡爾曼濾波計(jì)算過(guò)程

卡爾曼濾波算法核心思想在于:預(yù)測(cè)+測(cè)量反饋．算法將通過(guò)系統(tǒng)狀態(tài)方程計(jì)算得到的預(yù)測(cè)值作為先驗(yàn)信息,之后在觀測(cè)部分更新此信息．卡爾曼濾波計(jì)算過(guò)程如下:

1) 該算法在第k個(gè)歷元的狀態(tài)向量xk及其協(xié)方差矩陣Pk的時(shí)間預(yù)測(cè)方程為

(3)

(4)

式中:Fk為狀態(tài)轉(zhuǎn)換矩陣;Qk為過(guò)程噪聲矩陣．

2) 計(jì)算更新向量γk及其協(xié)方差矩陣Sk:

(5)

(6)

式(5)和式(6)中:zk為觀測(cè)向量;Hk為觀測(cè)矩陣;Rk為測(cè)量噪聲協(xié)方差矩陣．

由公式(5)可以計(jì)算卡爾曼增益Kk:

(7)

3) 由式(3)、(4)、(5)、(7)可以得到最后的測(cè)量更新方程:

(8)

(9)

1.2.2 靜態(tài)卡爾曼濾波實(shí)現(xiàn)

對(duì)于靜態(tài)用戶,本文設(shè)計(jì)的在歷元k處的狀態(tài)向量xk為

(10)

靜態(tài)用戶不會(huì)改變其移動(dòng)位置,這意味著隨著時(shí)間的推移(X,Y,Z)坐標(biāo)保持不變．因此只需要考慮如何對(duì)接收器時(shí)鐘的動(dòng)態(tài)行為進(jìn)行建模,其近似為:

(11)

(12)

式中,ΔT是測(cè)量時(shí)間間隔．

所以就有狀態(tài)轉(zhuǎn)換矩陣Fk為

(13)

在靜態(tài)情況下,可以假設(shè)用戶沒有移動(dòng)并且接收器時(shí)鐘具有一些頻率和相位誤差．因此,過(guò)程噪聲矩陣Q就可以寫成如下形式:

(14)

式中接收器時(shí)鐘參數(shù)Sg,Sf分別表示為

Sg≈2π2h-2,

(15)

(16)

式中,h-2和h0分別為隨機(jī)頻率噪聲和白噪聲的功率譜密度(PSD)．

1.2.3 動(dòng)態(tài)卡爾曼濾波實(shí)現(xiàn)

對(duì)于動(dòng)態(tài)用戶,必須考慮用戶速度的問(wèn)題．所以需要重新定義新的狀態(tài)向量如下:

(17)

式中:U,V,W分別表示X、Y、Z方向上的速度,初始值為0．

動(dòng)態(tài)卡爾曼濾波的狀態(tài)向量發(fā)生改變,因此也需要重新定義一個(gè)新的狀態(tài)轉(zhuǎn)換矩陣Fk和一個(gè)新的過(guò)程噪聲矩陣Qk,其定義如下:

(18)

式中，SX、SY和SZ分別為位置隨機(jī)過(guò)程的光譜幅度,根據(jù)經(jīng)驗(yàn),本文中取值分別為0.5,0.5,0.1．

2 實(shí)驗(yàn)及設(shè)備說(shuō)明

2.1 設(shè)備說(shuō)明

本文實(shí)驗(yàn)使用的手機(jī)設(shè)備是小米6X,手機(jī)參數(shù)如表1所示.

表1 小米6X手機(jī)參數(shù)

該手機(jī)中Android版本是Android 8.1,可以通過(guò)API中提供的location類獲取定位所需參數(shù)．運(yùn)行內(nèi)存和處理器能夠保證有較好的數(shù)據(jù)收集和數(shù)據(jù)處理能力,并且支持GPS導(dǎo)航和北斗衛(wèi)星導(dǎo)航系統(tǒng)(BDS),為實(shí)驗(yàn)提供了設(shè)備基礎(chǔ)．本文算法的實(shí)現(xiàn)是使用GPS衛(wèi)星數(shù)據(jù)．

2.2 實(shí)驗(yàn)說(shuō)明

本文實(shí)驗(yàn)分為2種:靜態(tài)實(shí)驗(yàn)和低速動(dòng)態(tài)實(shí)驗(yàn)．

靜態(tài)實(shí)驗(yàn)是指將手機(jī)置于某地靜止不動(dòng)獲取數(shù)據(jù)．靜態(tài)實(shí)驗(yàn)要求四周沒有反射環(huán)境,能夠很好地接收衛(wèi)星信號(hào)．通過(guò)分析靜態(tài)實(shí)驗(yàn)定位結(jié)果圖像以及各個(gè)定位結(jié)果到平均值點(diǎn)的距離差的大小,判斷每種方法在靜態(tài)實(shí)驗(yàn)中的定位效果．

低速動(dòng)態(tài)實(shí)驗(yàn)又分為電動(dòng)車實(shí)驗(yàn)和步行實(shí)驗(yàn)．電動(dòng)車實(shí)驗(yàn)是將手機(jī)置于電動(dòng)車上行駛,步行實(shí)驗(yàn)是手持手機(jī)行走,兩個(gè)實(shí)驗(yàn)都盡量保持勻速,且要求良好的實(shí)驗(yàn)環(huán)境．通過(guò)分析實(shí)驗(yàn)路線定位結(jié)果圖像的變化幅度與實(shí)際路線的相似情況以及各個(gè)定位結(jié)果到實(shí)際路線的距離差,判斷哪種方法更加適合動(dòng)態(tài)定位．

靜態(tài)實(shí)驗(yàn)與低速動(dòng)態(tài)實(shí)驗(yàn)數(shù)據(jù)收集時(shí)間與平均速度如表2所示,從表中可知兩個(gè)低速動(dòng)態(tài)實(shí)驗(yàn)速度不高,是屬于低速動(dòng)態(tài)實(shí)驗(yàn)．

表2 靜態(tài)實(shí)驗(yàn)與低速動(dòng)態(tài)實(shí)驗(yàn)數(shù)據(jù)收集時(shí)間與平均速度

3 實(shí)驗(yàn)結(jié)果分析

3.1 靜態(tài)實(shí)驗(yàn)

本文中靜態(tài)實(shí)驗(yàn)地點(diǎn)是上海海洋大學(xué)信息學(xué)院旁邊校友之家的樓頂,該實(shí)驗(yàn)環(huán)境良好,四周空曠無(wú)反射環(huán)境．實(shí)驗(yàn)時(shí)間為2019年5月6日中午12點(diǎn)20分,持續(xù)獲取數(shù)據(jù)時(shí)間為半個(gè)小時(shí)．靜態(tài)實(shí)驗(yàn)定位結(jié)果如圖1所示(文中所有圖片均使用Google Earth軟件畫出).

(藍(lán)色:加權(quán)最小二乘法結(jié)果;紅色:靜態(tài)卡爾曼濾波結(jié)果;白色:動(dòng)態(tài)卡爾曼濾波結(jié)果)圖1 3種算法的定位結(jié)果圖

由圖1的定位結(jié)果可知,對(duì)于靜態(tài)用戶,靜態(tài)卡爾曼濾波算法定位結(jié)果最集中,變化幅度最小,說(shuō)明每一時(shí)刻的定位結(jié)果幾乎沒有發(fā)生太大變化,其定位結(jié)果最好．動(dòng)態(tài)卡爾曼濾波算法定位結(jié)果相對(duì)集中,變化幅度相對(duì)較小,僅有幾個(gè)數(shù)據(jù)結(jié)果較差,定位結(jié)果在旁邊水面．加權(quán)最小二乘法算法定位結(jié)果變化幅度最大結(jié)果最差,有較多的數(shù)據(jù)結(jié)果定位在旁邊的水面上以及草地上;其定位結(jié)果在各個(gè)方向上都有結(jié)果較差的數(shù)據(jù)．

根據(jù)計(jì)算出來(lái)的定位結(jié)果坐標(biāo),取3種方法的經(jīng)緯度坐標(biāo)的平均值(作為有效點(diǎn)),并根據(jù)坐標(biāo)轉(zhuǎn)換公式算出3種方法每個(gè)定位結(jié)果坐標(biāo)點(diǎn)到有效點(diǎn)的距離差(單位:m),最后算出距離差的平均值,均方根值和均方根誤差．其結(jié)果如表3所示:

表3 3種方法距離差的平均值,均方根值和均方根誤差 m

平均值越小表明通過(guò)某種算法得到的定位結(jié)果坐標(biāo)越集中．均方根誤差值越小,說(shuō)明定位結(jié)果越好．從表3中可以看出,靜態(tài)卡爾曼濾波算法得到的距離差的平均值、均方根值和均方根誤差分別為0.28、0.32、0.15,其值均為其中最小,也直接說(shuō)明靜態(tài)卡爾曼濾波的定位結(jié)果最好．動(dòng)態(tài)卡爾曼濾波算法的定位結(jié)果其次,加權(quán)最小二乘法的定位結(jié)果最差．

綜上可知,靜態(tài)卡爾曼濾波算法更加適合靜態(tài)用戶定位．

3.2 低速動(dòng)態(tài)實(shí)驗(yàn)

3.2.1 電動(dòng)車實(shí)驗(yàn)

電動(dòng)車實(shí)驗(yàn)地點(diǎn)為：上海海洋大學(xué)體育館旁大路,道路中間無(wú)遮蔽物,道路兩旁有樹木,距離道路中間有一段距離,不會(huì)有反射等情況．行走路線為直線,距離約為320 m,收集數(shù)據(jù)時(shí)間為1分30秒,電動(dòng)車平均速度約為:13 km/h．3種方法的定位結(jié)果路線與實(shí)際路線如圖2所示.

(藍(lán)色:加權(quán)最小二乘法結(jié)果;紅色:靜態(tài)卡爾曼濾波結(jié)果;白色:動(dòng)態(tài)卡爾曼濾波結(jié)果;黑色:實(shí)際路線)圖2 3種方法的定位結(jié)果路線與實(shí)際路線圖

圖2中紅色、白色、藍(lán)色和黑色分別代表了靜態(tài)卡爾曼濾波、動(dòng)態(tài)卡爾曼濾波、加權(quán)最小二乘法的定位結(jié)果和實(shí)際路線．從圖中可以看出,動(dòng)態(tài)卡爾曼濾波和加權(quán)最小二乘法的定位結(jié)果與實(shí)際路線接近,其中動(dòng)態(tài)卡爾曼濾波定位結(jié)果更加平滑,更接近實(shí)際路線,定位結(jié)果更好．靜態(tài)卡爾曼濾波因?yàn)槠涠ㄎ唤Y(jié)果變化幅度小,其路線明顯偏離了實(shí)際路線,定位結(jié)果最差．

同步行實(shí)驗(yàn)數(shù)據(jù)處理相同,由于靜態(tài)卡爾曼濾波方法明顯偏離了實(shí)際路線,在這里不做計(jì)算．計(jì)算加權(quán)最小二乘法和動(dòng)態(tài)卡爾曼濾波2種算法每個(gè)定位結(jié)果到實(shí)際行走路線的誤差．它們的平均值,均方根值和均方根誤差如表4所示.

表4 2種算法誤差的平均值,均方根值和均方根誤差 m

從表4中可以看出,動(dòng)態(tài)擴(kuò)展卡爾曼濾波算法誤差的平均值,均方根值和均方根誤差均比加權(quán)最小二乘法的值小,表明使用該算法能夠得到更好的定位結(jié)果．從該數(shù)據(jù)可知,對(duì)于電動(dòng)車實(shí)驗(yàn),動(dòng)態(tài)擴(kuò)展卡爾曼濾波定位結(jié)果最好,比加權(quán)最小二乘法更加適合低速動(dòng)態(tài)實(shí)驗(yàn)．

3.2.2 步行實(shí)驗(yàn)

步行實(shí)驗(yàn)地點(diǎn)是上海海洋大學(xué)操場(chǎng),該地點(diǎn)是一個(gè)空曠環(huán)境,周圍沒有反射物體．手持手機(jī)收集數(shù)據(jù),行走路線為外圈橢圓形,速度約為5.4 km/h,收集數(shù)據(jù)時(shí)間為5分40秒． 其定位結(jié)果如圖3所示.

(藍(lán)色:加權(quán)最小二乘法結(jié)果；紅色：靜態(tài)卡爾曼濾波結(jié)果;白色:動(dòng)態(tài)卡爾曼濾波結(jié)果)圖3 步行實(shí)驗(yàn)結(jié)果

圖3中,紅色、白色和藍(lán)色分別代表了靜態(tài)卡爾曼濾波、動(dòng)態(tài)卡爾曼濾波和加權(quán)最小二乘法的定位結(jié)果．行走路線為操場(chǎng)最外圈、接近紅白相間處的橢圓形,由于實(shí)際路線沒有具體坐標(biāo)數(shù)據(jù),所以并未在圖中畫出,也無(wú)法計(jì)算兩種方法距離行走路線誤差的平均值,均方根值和均方根誤差．從圖中可知,加權(quán)最小二乘法和動(dòng)態(tài)卡爾曼濾波最接近實(shí)際行走路線,適合動(dòng)態(tài)用戶定位．但其中動(dòng)態(tài)卡爾曼濾波路線波動(dòng)幅度較小,可以看出其誤差更?。o態(tài)卡爾曼濾波結(jié)果同電動(dòng)車實(shí)驗(yàn)相同,定位結(jié)果變化幅度小,與實(shí)際路線明顯偏差較大,說(shuō)明靜態(tài)卡爾曼濾波不適合動(dòng)態(tài)用戶．從靜態(tài)卡爾曼濾波的推導(dǎo)公式中也可以看出,在它的狀態(tài)向量、轉(zhuǎn)換矩陣中沒有速度的概念,不能令其定位結(jié)果隨時(shí)間變化而快速改變．

從電動(dòng)車實(shí)驗(yàn)和步行實(shí)驗(yàn)結(jié)果可知:動(dòng)態(tài)卡爾曼濾波算法更加適合低速動(dòng)態(tài)實(shí)驗(yàn)．

4 結(jié)束語(yǔ)

針對(duì)現(xiàn)階段直接利用手機(jī)的GPS芯片原始數(shù)據(jù)的定位研究比較少的背景,本文主要對(duì)基于手機(jī)GPS原始數(shù)據(jù)定位可行性以及定位算法進(jìn)行了研究．利用Android 7.0系統(tǒng)提供的API獲取定位所需GPS原始數(shù)據(jù)參數(shù),并將這些參數(shù)應(yīng)用到設(shè)計(jì)的靜態(tài)卡爾曼濾波和動(dòng)態(tài)卡爾曼濾波算法中．為測(cè)試算法的性能,進(jìn)行了靜態(tài)實(shí)驗(yàn)和滿足日常手機(jī)定位需要的低速動(dòng)態(tài)實(shí)驗(yàn),并與傳統(tǒng)的加權(quán)最小二乘法作對(duì)比分析．在分別進(jìn)行的靜態(tài)實(shí)驗(yàn)以及電動(dòng)車實(shí)驗(yàn)和步行實(shí)驗(yàn)的實(shí)驗(yàn)結(jié)果可以看出:在靜態(tài)實(shí)驗(yàn)中,靜態(tài)卡爾曼濾波誤差的平均值和均方根誤差值都是最小,其定位結(jié)果最好．在低速動(dòng)態(tài)實(shí)驗(yàn)中,動(dòng)態(tài)卡爾曼濾波誤差的平均值和均方根誤差值都是最小,定位結(jié)果最好,更加接近實(shí)際路線．

實(shí)驗(yàn)結(jié)果證明了利用手機(jī)GPS原始數(shù)據(jù)實(shí)現(xiàn)定位的可行性,同時(shí)也證明了本文設(shè)計(jì)的卡爾曼濾波算法可以提高相應(yīng)場(chǎng)景的定位精度,論文的研究結(jié)果為將來(lái)實(shí)現(xiàn)靜態(tài)與動(dòng)態(tài)的高精度手機(jī)定位算法(如RTK算法)提供了必要的理論依據(jù)．