基于智能手機(jī)的PDR 定位航向估計算法研究

周 剛 李善文

(江西核工業(yè)測繪院集團(tuán)有限公司 江西南昌330038)

1 引言

室內(nèi)的導(dǎo)航定位[1]是當(dāng)前全球的研究熱點，目前全球定位導(dǎo)航系統(tǒng)[2]如GPS、北斗[3]等發(fā)展成熟，能滿足人們生產(chǎn)、生活的需求。但室外定位無法延伸至室內(nèi)，近年來國內(nèi)對室內(nèi)定位技術(shù)研究不斷深入，近年來出現(xiàn)許多室內(nèi)定位技術(shù)[4]，如WiFi、藍(lán)牙、UWB、ZigBee、慣導(dǎo)等。隨著智能手機(jī)的普及和MEMS 技術(shù)的快速發(fā)展，慣性傳感器已成為現(xiàn)在主流智能手機(jī)的標(biāo)準(zhǔn)配置?；贛EMS 傳感器技術(shù)[5]的行人航跡推算[6]（Pedestrian Dead Reckoning，PDR）算法在智能手機(jī)端定位研究越來越廣泛。相比于慣導(dǎo)定位，手機(jī)端的MEMS 慣性傳感器性能低，無法采用傳統(tǒng)的慣性導(dǎo)航原理即采用加速度積分計算距離，陀螺儀角加速度積分計算姿態(tài)角。所以在PDR 定位中是利用慣性傳感器數(shù)據(jù)進(jìn)行步頻檢測-步長估計和航向估計算法來實現(xiàn)位置迭代更新，航向估計是基于智能手機(jī)PDR 定位的難點，當(dāng)航向估計無誤差時，定位軌跡將不再發(fā)散，很好的擬合在既定路徑上。一般采用陀螺儀和磁力計來估計行人的行走時的方向，但磁力計受地球以及周邊磁場的影響較大，陀螺儀雖不受外部環(huán)境影響，但誤差會隨時間而累積。隨著多源融合技術(shù)的廣泛應(yīng)用，目前航向估計算法都是融合MEMS 傳感器的加速度計、陀螺儀、磁力計等九軸傳感器數(shù)據(jù)來對航向進(jìn)行估計，本文展開基于智能手機(jī)的PDR 定位方式研究，著重對航向估計算法進(jìn)行實驗分析與論證。

2 基于智能手機(jī)的PDR 定位

基于智能手機(jī)的PDR 定位是利用手機(jī)內(nèi)置的MEMS 慣性傳感器來對行人的軌跡進(jìn)行推算，其主要步驟包括步頻檢測、步長估計和航向估計。

2.1 步頻檢測

目前針對PDR 定位的步頻檢測方法[7]主要有峰值檢測算法和零速更新算法，零速更新算法針對足綁式MEMS 傳感器的步頻檢測更為有效。由于智能手機(jī)在使用者的使用過程中會有不同的攜帶方式，加速度計三軸方向會實時調(diào)整，通過檢測合加速度的變化，利用峰值檢測算法更為精確。

a合(t)為加速度計在t 時刻三軸的總加速度，ax、ay、az分別表示加速度計在t 時刻X 軸、Y 軸、Z 軸的加速度分量。為濾除重力和其他因素干擾，采用滑動均值濾波器對數(shù)據(jù)進(jìn)行平滑處理，平滑結(jié)果如圖1所示，設(shè)定峰值檢測的各項閾值條件對步數(shù)進(jìn)行檢測：

1）設(shè)定波峰閾值amax，即在一個檢測周期內(nèi)a合(t)≥amax才會一個有效波峰。

2）設(shè)定波峰之間的時間閾值Δ(t)，即從第二個有效波峰起，連續(xù)檢測相鄰波峰的時刻差是否滿足時間閾值要求。

3）設(shè)定峰谷差閾值Δa，由于手機(jī)攜帶一般置于上半身，在數(shù)據(jù)波形上來看，一個波峰會伴隨著小波峰，設(shè)置峰谷差使檢測更為精準(zhǔn)。

圖1 滑動均值濾波器對合加速度值平滑結(jié)果

2.2 步長估計

人行走時的步長具有隨機(jī)性，每個人由于身高、體重、年齡等不同，步長呈現(xiàn)多樣性。在實際應(yīng)用中查表法并不適用，根據(jù)人行走時加速度計與步長的非線性關(guān)系，可以得出步長估計模型：

根據(jù)一個檢測周期內(nèi)合加速度的最大值A(chǔ)max、最小值A(chǔ)min與步長參數(shù)ε 的關(guān)系，可以估算出其行走的步長L，其中步長參數(shù)ε 可以通過數(shù)據(jù)訓(xùn)練而實時調(diào)整。

2.3 航向估計

姿態(tài)解算方法一般分為兩類，一是從頻域濾波、消除噪聲，不考慮信號噪聲的統(tǒng)計特性，如PI 互補濾波[8]；另一種是使用狀態(tài)空間法在時域內(nèi)設(shè)計濾波器，如卡爾曼濾波[9]，梯度下降法[10]，都實現(xiàn)了加速度計、陀螺儀和磁力計輸出信息利用的最大化，下面簡要分析三種算法的實現(xiàn)原理。

2.3.1 PI 互補濾波法

導(dǎo)航坐標(biāo)系下加速度計數(shù)據(jù)ab=[axayaz]T和mb=[mxmymz]T磁力計數(shù)據(jù)通過旋轉(zhuǎn)矩陣到地理坐標(biāo)系時分別與地理坐標(biāo)系下的an=[0 0 1]T和mn=[mxmymz]T不相等，即：

將加速度計數(shù)據(jù)和磁力計數(shù)據(jù)誤差分別進(jìn)行向量積得到誤差，再進(jìn)行累加后得到總誤差[exeyez]T。

對陀螺儀輸出的數(shù)據(jù)[gxgygz]T進(jìn)行校正，設(shè)定好PI 參數(shù)：

通過修正后的角速度與四元數(shù)對采樣時間T 進(jìn)行積分得到新的四元數(shù)，再反解歐拉角得到航向角。

2.3.2 梯度下降法

加速度計與磁力計融合能解算出機(jī)體實時運動下的姿態(tài)，但都存在誤差，先將加速度計和磁力計的誤差進(jìn)行整合，構(gòu)建誤差函數(shù)，得到梯度下降法中的雅克比矩陣：

再進(jìn)行三種傳感器數(shù)據(jù)融合：

2.3.3 卡爾曼濾波法

將加速度計與磁力計共同解算的姿態(tài)角作為卡爾曼濾波器中觀測值，陀螺儀解算的姿態(tài)角作為預(yù)測值，測定好三種傳感器測量中誤差等參數(shù)，構(gòu)建卡爾曼濾波器。假設(shè)K 時刻的航向角為φk，狀態(tài)方程和預(yù)測方程為：

其中狀態(tài)估計方程為：

卡爾曼增益：

更新后的濾波值為：

3 實驗設(shè)計

（1）為驗證三種航向解算算法對航向角解算的精度，設(shè)置轉(zhuǎn)臺實驗如下圖2 所示。將智能手機(jī)放置在旋轉(zhuǎn)平臺上，用水準(zhǔn)尺對平臺進(jìn)行水平校正，設(shè)置好旋轉(zhuǎn)起點。靜置一段時間后，將旋轉(zhuǎn)平臺逆時針勻速旋轉(zhuǎn)5 圈重回到起點位置后靜置一段時間。從理論上來說，由于平臺置于水平，所以智能手機(jī)旋轉(zhuǎn)一圈后，姿態(tài)角中俯仰角和橫滾角不變，航向角有360°的變化。通過對比三種算法對姿態(tài)角解算的精度，再對各算法的優(yōu)缺性進(jìn)行分析。

圖2 轉(zhuǎn)臺實驗示意圖

（2）為對比三種航向估計算法對PDR 定位軌跡的影響，設(shè)置PDR 定位實驗，實驗示意圖如下圖3所示。在室內(nèi)合適空間采用測距儀精確測定四個坐標(biāo)點，分別設(shè)置為A（1,1）、B（1,7）、C（8,7）、D（8,1）。將四點圍成的圖形作為真實軌跡。利用黑色膠帶進(jìn)行標(biāo)識，膠帶寬為5cm，設(shè)定起始點為A。從A 點出發(fā)沿著軌跡ABCD 進(jìn)行PDR 定位再回到起點A 完成閉合。為控制單一變量，其中步頻檢測和步長估計分別采用本文中2.1 節(jié)、2.2 節(jié)方法。以上兩個實驗都采用開源的Android 手機(jī)慣性傳感器數(shù)據(jù)采集軟件進(jìn)行數(shù)據(jù)采集。

圖3 PDR 定位實驗示意圖

4 實驗數(shù)據(jù)處理與分析

如將采集的傳感器數(shù)據(jù)，分別用三種航向估計算法進(jìn)行處理后，三種算法的航向角如圖4 所示，三種算法的俯仰角如圖5 所示，三種算法的橫滾角如圖6 所示。

圖4 三種算法的航向角解算曲線圖

圖5 三種算法的俯仰角解算曲線圖

圖6 三種算法的橫滾角解算曲線圖

由圖4 可知，梯度下降法和卡爾曼濾波法對航向角的解算曲線基本重合，兩種算法的解算精度要高于PI 互補濾波法。由圖5 可知，卡爾曼濾波法和PI 互補濾波法對俯仰角的解算曲線的誤差比梯度下降法要小，由實驗設(shè)置環(huán)境可知，表明卡爾曼濾波法和PI 互補濾波法的抗差能力更強。且三種算法的誤差不超過0.04rad，說明三種算法的解算精度高。由圖6 可知，卡爾曼濾波法對橫滾角的解算曲線誤差比PI 互補濾波法和梯度下降法要小，誤差約為0.02rad。由實驗設(shè)置環(huán)境可知，表明卡爾曼濾波法比PI 互補濾波法、梯度下降法的抗差能力更強。再在相同條件下，比對三種航向估計算法對PDR 定位軌跡的影響，將定位實驗采集的數(shù)據(jù)處理得到圖a、b、c、d四個定位軌跡圖：

圖7 定位軌跡圖

由圖a 知，藍(lán)色框線表示參考的真實路徑，橙色曲線表示PI 互補濾波法的定位軌跡圖，綠色曲線表示梯度下降法的定位軌跡圖，紅色曲線表示卡爾曼濾波法的定位軌跡圖。由對比可知，三種算法在定位初期定位航向誤差小，與參考路線的擬合程度高。隨著定位時間的累積，航向估計誤差逐漸累積，在第一個轉(zhuǎn)向點后，三種算法的PDR 定位軌跡逐漸發(fā)散，但卡爾曼濾波法的發(fā)散程度最小，與第二段參考曲線的擬合程度最高。表明基于卡爾曼濾波的航向估計算法精度高，且抗差能力強。圖b、圖c、圖d 分別表示三種算法的PDR 定位軌跡，說明了PDR 定位中航向估計誤差對定位軌跡的影響大，純PDR 定位的誤差在短時間內(nèi)小，長時間定位誤差會逐漸加大，必須對PDR 定位誤差進(jìn)行抑制和校正。

5 結(jié)束語

本文通過對基于智能手機(jī)的PDR 定位進(jìn)行研究，介紹了步頻檢測和步長估計算法原理和模型，著重介紹了目前九軸傳感器數(shù)據(jù)融合的航向估計算法，并設(shè)置對比實驗對三種算法對姿態(tài)角解算精度進(jìn)行比較，通過數(shù)據(jù)處理并進(jìn)行分析，結(jié)果表明基于卡爾曼濾波法對歐拉角中航向角解算精度高，對俯仰角和橫滾角的解算誤差小，抗差能力強。為驗證三種航向估計算法對PDR 定位軌跡的影響，設(shè)置了三種算法的PDR 定位實驗，對數(shù)據(jù)進(jìn)行處理后分析，結(jié)果表明了基于卡爾曼濾波的航向估計算法的PDR定位軌跡對參考軌跡的擬合更好，誤差小，抗差能力強。通過本文可知，航向估計對PDR 定位的影響最大，在短距離內(nèi)累積誤差較小，定位精度較高；長時間、長距離定位時，PDR 定位精度急劇下降，必須對PDR 的航向誤差進(jìn)行校正，為后續(xù)航向校正算法與多定位方式融合算法研究打下堅實基礎(chǔ)。