多傳感器融合的行人航位推算方法研究

曹娟, 崔學(xué)榮， 李娟, 張國(guó)平

(1.中國(guó)石油大學(xué)(華東) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院, 山東 青島 266580;2.中國(guó)石油大學(xué)(華東) 海洋與空間信息學(xué)院, 山東 青島 266580)

0 引言

在移動(dòng)互聯(lián)網(wǎng)技術(shù)和通信行業(yè)快速發(fā)展的推動(dòng)下，定位服務(wù)日益受到人們的關(guān)注與應(yīng)用。相關(guān)研究表明，如今人們?cè)谑覂?nèi)的時(shí)間已經(jīng)超過(guò)70%。但因?yàn)槭覂?nèi)環(huán)境復(fù)雜多變，全球定位系統(tǒng)(Global Positioning System,GPS)受建筑物等的遮擋，無(wú)法為用戶提供較為準(zhǔn)確的定位服務(wù)。因此研究高精度的室內(nèi)定位方法是非常有必要的。常見(jiàn)的室內(nèi)定位技術(shù)主要有WI-FI定位[1]、藍(lán)牙定位[2]、ZigBee定位[3]和地磁定位[4]等。這些室內(nèi)定位技術(shù)存在不同缺點(diǎn)，例如信號(hào)不穩(wěn)定、易受干擾、成本高等缺點(diǎn)，所以難以實(shí)現(xiàn)廣泛推廣與應(yīng)用。

行人航位推算(Pedestrian Dead Reckoning,PDR)[5-6]可在已知初始位置的情況下，推算出行人在任一時(shí)刻的位置[7]。PDR定位基于設(shè)備中集成的傳感器實(shí)現(xiàn)完全自主定位，不易受外界環(huán)境干擾[8]，具有成本低且短時(shí)間定位精度高等優(yōu)點(diǎn)。微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System, MEMS)慣性傳感器逐漸形成樣式小、功耗低的特點(diǎn)[9]，這為使用移動(dòng)終端進(jìn)行PDR定位提供了可能。

文獻(xiàn)[10]中提出了一種借助建筑幾何信息實(shí)現(xiàn)行人航向的實(shí)時(shí)補(bǔ)償方案，通過(guò)提高定向精度來(lái)抑制定位誤差的累積傳遞。但是借助建筑物信息時(shí)可能會(huì)引入額外誤差；文獻(xiàn)[11]利用加速度二次積分及步長(zhǎng)滑動(dòng)窗口法進(jìn)行在線步長(zhǎng)估計(jì)的方法，但是加速度二次積分的誤差很難消除。文獻(xiàn)[12]中，提出了一種基于神經(jīng)網(wǎng)絡(luò)和智能手機(jī)內(nèi)置多傳感器融合的PDR室內(nèi)定位方法，但是計(jì)算復(fù)雜度較高，不適用于手機(jī)客戶端。通過(guò)分析行人運(yùn)動(dòng)模型及步行姿態(tài)，設(shè)計(jì)了一種基于零速檢測(cè)的加速度量測(cè)幅值計(jì)步算法，實(shí)現(xiàn)了載體坐標(biāo)系和行人地理坐標(biāo)系的轉(zhuǎn)換，剔除了偽零速現(xiàn)象[13]。雖然該算法的計(jì)步準(zhǔn)確率和環(huán)境適應(yīng)度較高，但是實(shí)驗(yàn)室需要慣性傳感器設(shè)備綁在腳部，應(yīng)用性較差。朱莊生等[14]人提出了一種多源信息自適應(yīng)步態(tài)檢測(cè)算法。該方法通過(guò)綜合考慮人體運(yùn)動(dòng)過(guò)程中的加速度信號(hào)和角速度信號(hào)，根據(jù)不同的步態(tài)特征通過(guò)設(shè)定不同的自適應(yīng)閾值條件實(shí)現(xiàn)步數(shù)的檢測(cè)。算法可以適用于不同的運(yùn)動(dòng)狀態(tài)，步數(shù)檢測(cè)的準(zhǔn)確率高于峰值檢測(cè)和閾值檢測(cè)方法。文獻(xiàn)[15]采用的傳感器為加速度傳感器和電子羅盤(pán)，把定位結(jié)果與真實(shí)路徑進(jìn)行匹配，來(lái)減小航向估計(jì)的誤差，但是地圖中采集真實(shí)路徑時(shí)容易帶入誤差；Fan Qigao等[16]人提出了一種改進(jìn)的行人航位推算算法，主要包括改進(jìn)的步長(zhǎng)估計(jì)和航向估計(jì)。使用小波去噪算法對(duì)原始信號(hào)進(jìn)行預(yù)處理，然后利用多閾值方法以改善步長(zhǎng)估計(jì)算法，針對(duì)航向估計(jì)誤差和離群值的問(wèn)題，提出了魯棒的自適應(yīng)卡爾曼濾波算法，并結(jié)合互補(bǔ)濾波提高定位精度。

本文主要從步頻檢測(cè)和航向判斷兩方面對(duì)PDR算法進(jìn)行改進(jìn)。提出了一種相鄰波峰(波谷)二次判斷及多重閾值聯(lián)合的步頻推算方法以及卡爾曼濾波融合多傳感器的航向估計(jì)方法。

1 行人航位推算

1.1 行人航位推算

行人航位推算是一個(gè)不斷累積迭代的過(guò)程，通過(guò)采集慣性傳感器的數(shù)據(jù)，估計(jì)出行人步數(shù)、步長(zhǎng)和航向信息，用于估算下一步位置。因此，只要知道前一時(shí)刻的位置信息，就可以計(jì)算出當(dāng)前位置。令當(dāng)前時(shí)刻的位置為L(zhǎng)N，如式(1)。

(1)

2 多傳感器融合的行人航位推算方法

2.1 相鄰波峰(波谷)二次判斷及多重閾值的聯(lián)合步頻推算方法

本文針對(duì)步頻檢測(cè)提出了一種相鄰波峰(波谷)二次判斷及多重閾值聯(lián)合的步頻推算方法。具體算法如下。

(1) 運(yùn)動(dòng)狀態(tài)判斷：采用滑動(dòng)窗口方法求原始加速度的均值和方差，若方差大于某一閾值則認(rèn)為處于運(yùn)動(dòng)狀態(tài)，否則為靜止?fàn)顟B(tài)；

(2) 首次判斷波峰/波谷：設(shè)置校驗(yàn)點(diǎn)，若校驗(yàn)點(diǎn)為波峰/波谷，則對(duì)該采樣頻率內(nèi)的數(shù)據(jù)去均值化，尋找波峰和波谷，并設(shè)置一次閾值，過(guò)濾掉一部分不屬于正常行走的加速度值。設(shè)置校驗(yàn)點(diǎn)的目的是為了減少計(jì)算量；

(3) 尋找相鄰波峰/波谷中的最大和最小值：經(jīng)過(guò)上述判斷后，從相鄰波峰/波谷中尋找最大和最小值，去掉偽波峰和偽波谷；

(4) 二次判斷波峰/波谷：根據(jù)上一步確定好的波峰波谷的狀態(tài)，再次判斷校驗(yàn)點(diǎn)是否為波峰/波谷，避免偽波峰(波谷)的干擾；

(5) 多重閾值校驗(yàn)：若校驗(yàn)點(diǎn)滿足波峰/波谷，則采用滑動(dòng)窗口法，計(jì)算最近三步波峰的均值和方差，若均值和方差以及兩次波峰之間的時(shí)間差小于某一閾值，經(jīng)過(guò)多次試驗(yàn)最終確定的峰值方差的閾值為σ<5，當(dāng)前峰值P與均值μ滿足P-μ<2，時(shí)間差T<300即可認(rèn)為是走了一步，步數(shù)加一。

采集到的原始加速度,如圖1所示。

圖1 原始加速度信號(hào)圖

采用該方法處理過(guò)的加速度數(shù)據(jù),如圖2所示。

圖2 處理后的加速度信號(hào)圖

兩圖對(duì)比發(fā)現(xiàn)該算法可以很好地過(guò)濾掉偽波峰和偽波谷。

2.2 基于卡爾曼濾波的航向判斷

方向傳感器獲取的是絕對(duì)方向，不存在誤差累積，但磁力計(jì)的精度容易受磁干擾的影響，而使航向估計(jì)出現(xiàn)誤差；陀螺儀短時(shí)間內(nèi)精度高，受環(huán)境干擾極小，但存在誤差累積。結(jié)合兩者的優(yōu)缺點(diǎn)，本文用加速度傳感器和磁力計(jì)模擬方向傳感器，其值作為狀態(tài)值，用卡爾曼濾波算法與陀螺儀進(jìn)行融合，減小航向估計(jì)中的漂移現(xiàn)象。

卡爾曼濾波常用于線性模型，是一種自回歸濾波器，濾波效率很高。濾波過(guò)程可以用式(2)和式(3)表示，分為兩個(gè)過(guò)程：時(shí)間更新和測(cè)量更新。前者實(shí)際是一個(gè)預(yù)測(cè)過(guò)程，根據(jù)先驗(yàn)知識(shí)預(yù)測(cè)當(dāng)前狀態(tài)并計(jì)算先驗(yàn)估計(jì)的協(xié)方差，后者是一個(gè)更新校正過(guò)程。

(1) 預(yù)測(cè)過(guò)程，如式(2)。

Pk-=APk-1AT+Q

(2)

(2) 更新過(guò)程,如式(3)。

K=Pk-HT(HPk-HT+R)-

Pk=(E-KH)Pk-

(3)

式中,X表示狀態(tài)值;Z表示測(cè)量值;A和B表示系統(tǒng)狀態(tài);K表示卡爾曼增益;P表示預(yù)測(cè)估計(jì)協(xié)方差矩陣;Q和R分別表示過(guò)程噪聲與測(cè)量噪聲，實(shí)驗(yàn)中其初取分別取為0.01和0.05，H為觀測(cè)矩陣。

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

實(shí)驗(yàn)時(shí)采用不同品牌不同型號(hào)的手機(jī)進(jìn)行驗(yàn)證，例如vivo X27、小米6、紅米note5、oppo R17等。本文的實(shí)驗(yàn)結(jié)果以華為榮耀8手機(jī)為例，手機(jī)系統(tǒng)為Android7.0。實(shí)驗(yàn)地點(diǎn)選取中國(guó)石油大學(xué)(華東)工科樓E座1樓大廳，平面圖，如圖3所示。

圖3 室內(nèi)平面圖

其中實(shí)驗(yàn)區(qū)域?yàn)橹虚g走廊，長(zhǎng)為38.5米，寬為12.8米。

對(duì)于步頻驗(yàn)證實(shí)驗(yàn)，在25-200步范圍內(nèi)共采集了8個(gè)不同區(qū)間的步數(shù)。每個(gè)步數(shù)區(qū)間各進(jìn)行5次試驗(yàn)并取平均值作為最終的步頻計(jì)數(shù)。實(shí)驗(yàn)時(shí)采集不同身高、不同體重行人的不同行走速度下的步頻情況，實(shí)驗(yàn)結(jié)果,如圖4所示。

圖4 步頻估計(jì)結(jié)果對(duì)比圖

實(shí)驗(yàn)步數(shù)與真實(shí)步數(shù)較為接近，步數(shù)差別在3步左右，準(zhǔn)確率為98%。圖4中調(diào)用系統(tǒng)計(jì)步傳感器方法是指調(diào)用華為榮耀8手機(jī)中的TYPE_STEP_COUNTER傳感器。該方法采用的是系統(tǒng)回調(diào)而非查詢的方式，定位初始階段無(wú)法實(shí)時(shí)獲取步數(shù)，因此并不適用于PDR定位；波峰(波谷)及時(shí)間聯(lián)合推算方法指波峰(波谷)檢測(cè)與時(shí)間閾值結(jié)合。

實(shí)驗(yàn)結(jié)果表明，改進(jìn)的相鄰波峰(波谷)二次判斷及多重閾值聯(lián)合的步頻推算方法的步頻最接近真實(shí)，使步頻估計(jì)的準(zhǔn)確率得以提高。

采用提出的卡爾曼濾波融合加速度傳感器、磁力計(jì)和陀螺儀的方法，在多次轉(zhuǎn)彎的情況下平均航向誤差為13.6°，比用互補(bǔ)濾波方法的航向估計(jì)誤差38°有較大提高，與真實(shí)值更接近,如圖5所示。

圖5 航向判斷結(jié)果圖

為驗(yàn)證改進(jìn)的相鄰波峰(波谷)二次判斷及多重閾值聯(lián)合的步頻推算方法以及卡爾曼濾波融合多傳感器的航向估計(jì)方法對(duì)于PDR定位的準(zhǔn)確性，選取圖6中的A點(diǎn)為起始點(diǎn)，沿圖中藍(lán)色(深色直線)線條軌跡進(jìn)行實(shí)驗(yàn)。先后經(jīng)過(guò)B、C、D、E和F之后，回到A，如圖6所示。

圖6 實(shí)驗(yàn)結(jié)果圖

圖6中紅色(深色虛線)軌跡的航向計(jì)算采用本文提出的卡爾曼濾波融合多傳感器的方法，步頻計(jì)算為本文提出的波峰(波谷)二次判斷及多重閾值聯(lián)合的步頻推算方法，實(shí)驗(yàn)結(jié)果為圖中的步數(shù)1和距離1所示；黃色(淺色虛線)軌跡的航向計(jì)算采用互補(bǔ)濾波，步頻計(jì)算采用回調(diào)系統(tǒng)計(jì)步傳感器的方法，實(shí)驗(yàn)結(jié)果為圖中的步數(shù)2和距離2所示。實(shí)驗(yàn)中整個(gè)軌跡的真實(shí)長(zhǎng)度為92.5 m，用本文提出的步頻推算方法結(jié)合多傳感器融合的航向判斷，單次試驗(yàn)得到的結(jié)果為91 m，誤差為1.5 m。經(jīng)過(guò)多次試驗(yàn)統(tǒng)計(jì)后，平均誤差為1.8 m。

4 總結(jié)

對(duì)于PDR定位，本文從步頻檢測(cè)、航向判斷兩方面進(jìn)行了改進(jìn)，提出了一種相鄰波峰(波谷)二次判斷及多重閾值聯(lián)合的步頻推算方法，可以較好地過(guò)濾掉抖動(dòng)手機(jī)產(chǎn)生的“假步”，步頻檢測(cè)的準(zhǔn)確率可達(dá)到98%；采用提出的卡爾曼融合多傳感器的航向判斷方法平均航向誤差為13.6°。在多次轉(zhuǎn)彎后，PDR定位誤差可以達(dá)到1.8 m。

利用文中的方法并不能完全消除PDR定位中的累積誤差。文中采用的步長(zhǎng)為固定步長(zhǎng)，不能根據(jù)不同行人自適應(yīng)步長(zhǎng)，會(huì)使定位誤差較大。因此未來(lái)可以研究自適應(yīng)步長(zhǎng)計(jì)算方法，根據(jù)不同行人計(jì)算不同步長(zhǎng)，以此來(lái)降低誤差。