WiFi與行人航跡推算自適應(yīng)無跡卡爾曼濾波融合定位算法

余成波， 成科宏

(重慶理工大學(xué)遠(yuǎn)程測試與控制研究所, 重慶 400054)

近年來，人們對位置服務(wù)的需求極速增長，傳統(tǒng)GPS和北斗衛(wèi)星定位系統(tǒng)在商場、地下停車場等結(jié)構(gòu)復(fù)雜的建筑區(qū)，性能急劇下降而無法滿足人們位置需求，因此室內(nèi)定位與導(dǎo)航成為研究熱點(diǎn)。目前的室內(nèi)定位技術(shù)主要包括射頻識(shí)別、ZigBee、WiFi、低功耗藍(lán)牙、行人航跡推算、超寬帶(ultra wideband，UWB)等方法[1]。單一技術(shù)定位受其固有劣勢影響，定位精度和魯棒性都較低，所以通過研究融合技術(shù)獲取更多數(shù)據(jù)信息降低不確定性，提高定位精度和魯棒性具有重要意義。文獻(xiàn)[2]提出了行人航跡推算(pedestrian dead reckoning，PDR)+地磁的組合定位方法，將PDR定位結(jié)果的2個(gè)步長作為地磁搜索區(qū)，再利用相關(guān)地磁匹配算法得到最佳估計(jì)點(diǎn)，但地磁受室內(nèi)其他設(shè)備產(chǎn)生磁場影響較大，該算法僅僅使用PDR的粗略位置縮小地磁匹配空間，未考慮噪聲因素影響，系統(tǒng)魯棒性不強(qiáng)。文獻(xiàn)[3]提出了使用容積卡爾曼濾波(CKF)模型及地圖匹配進(jìn)行PDR和WiFi數(shù)據(jù)的融合，能夠大幅度提高定位精度，雖然CKF利用容積點(diǎn)進(jìn)行非線性函數(shù)傳遞更加精確，但在系統(tǒng)狀態(tài)緯度小于4維時(shí)，二者估計(jì)精度和穩(wěn)定性相當(dāng)，且CKF計(jì)算量更大。文獻(xiàn)[4]討論了粒子濾波方法對PDR和室內(nèi)平面地圖進(jìn)行了融合，并在融合過程中結(jié)合地圖匹配信息抑制了粒子退化問題，但需要對室內(nèi)地圖環(huán)境進(jìn)行分析并設(shè)定標(biāo)準(zhǔn)物，前期工作量較大。文獻(xiàn)[5]提出了使用全球定位系統(tǒng)(global positioning system，GPS)和PDR的室內(nèi)外定位模式，該方法通過分析戶外行人運(yùn)動(dòng)狀態(tài)建立數(shù)據(jù)集，將數(shù)據(jù)集分類后應(yīng)用于GPS異常的室內(nèi)定位，該方法結(jié)構(gòu)簡單，但室內(nèi)定位精度有待提高。文獻(xiàn)[6]采用相關(guān)的冗余融合和互補(bǔ)融合對傳感器數(shù)據(jù)和WiFi數(shù)據(jù)進(jìn)行了融合，其方法有效抑制了WiFi信號(hào)的振蕩性，但忽略了傳感器數(shù)據(jù)的不確定性。文獻(xiàn)[7]重點(diǎn)研究了行人狀態(tài)估計(jì)算法的精確度，并考慮室內(nèi)環(huán)境的干擾進(jìn)行了算法增強(qiáng)，有效量化了姿態(tài)估計(jì)誤差，但單一PDR累積概率誤差仍然對系統(tǒng)影響較大。

針對WiFi信號(hào)易受室內(nèi)復(fù)雜環(huán)境干擾造成定位誤差較大，魯棒性不強(qiáng)，而單一PDR存在累積誤差，傳統(tǒng)融合方法精度與魯棒性難以良好兼容。本文設(shè)計(jì)了一種加入自適應(yīng)調(diào)整因子的UKF算法融合Wifi定位信息與PDR定位信息，推算用戶的最優(yōu)估計(jì)位置。

1 行人航跡推算(PDR)

航跡推算基于手機(jī)慣性傳感器單元。當(dāng)已知行人在某一時(shí)刻的初始位置，根據(jù)加速度計(jì)，方向羅盤傳感器數(shù)據(jù)獲取步伐、步長和航向信息推算行人下一時(shí)刻的位置[8]。PDR原理圖如圖1所示。

圖1 PDR原理圖Fig.1 PDR schematic diagram

由圖1所示，S0為行人初始時(shí)刻的位置坐標(biāo)S0(N0,E0)，Ln為k時(shí)刻的步長，θk為k時(shí)刻航向角，則下一時(shí)刻位置Sk(Nk,Ek)為

(1)

1.1 步數(shù)及步長估計(jì)

行人水平運(yùn)動(dòng)時(shí)，可以從手機(jī)加速度傳感器數(shù)據(jù)中檢測到加速度值的周期性變化，其近似正弦曲線，利用文獻(xiàn)[9]中閾值檢測模型實(shí)現(xiàn)步數(shù)估計(jì)。步長估計(jì)常見有兩種方法：一種是根據(jù)行人身體的特征在步行過程中設(shè)定固定的步長; 另一種是基于人類行走特征采用線性或非線性動(dòng)態(tài)步長[10]。其計(jì)算公式如下：

L=A+BSf+CSv+ws

(2)

(3)

式中：L為步長；Sf為第k步步頻；Sv為該步加速度方差；ws為高斯白噪聲；amax、amin為三軸加速度的最大值和最小值；A、B、C、K為常數(shù)。由于采用變長會(huì)增加算法時(shí)間復(fù)雜度，只需要在短時(shí)間內(nèi)利用傳感器數(shù)據(jù)進(jìn)行融合，定值步長的誤差在短時(shí)間內(nèi)對系統(tǒng)整體精度的影響極為有限，所以步長采用定值，L取60 cm[11]。

1.2 航向角估計(jì)

行人方向的識(shí)別一般由手機(jī)中陀螺儀測量的角速度積分獲得，但是陀螺儀隨著時(shí)間的增加產(chǎn)生累積誤差并且隨著傳感器溫度升高偏置漂移誤差也會(huì)增加[12]。方向羅盤能夠直接獲得角度信息，所以使用方向羅盤估計(jì)行人航向角。

2 WiFi室內(nèi)定位模型

基于WiFi信號(hào)強(qiáng)度的位置指紋識(shí)別是目前WiFi室內(nèi)定位最常用的方法之一，其分為兩步：一是位置指紋庫的離線建立；二是指紋庫的在線匹配。

2.1 WiFi信號(hào)強(qiáng)度指紋庫建立

離線階段：假設(shè)實(shí)驗(yàn)區(qū)域有M個(gè)AP，表示為AP1,AP2,…,APM。在實(shí)驗(yàn)環(huán)境中每隔一定距離設(shè)定N個(gè)采樣點(diǎn)(RP)，表示為RP1,RP2,…,RPN。對于每個(gè)RP，接收來自不同AP的信號(hào)強(qiáng)度(rssi)值矢量S=(rssi1,rssi2,…,rssiM)與其對應(yīng)位置坐標(biāo)(xi,yi)，i=1,2,…,N存儲(chǔ)在指紋庫中用于在線匹配。

2.2 在線匹配算法

在線階段：用戶在待測點(diǎn)接收來自不同AP的信號(hào)強(qiáng)度值矢量R=(Rrssi1,Rrssi2,…,RrssiM)，通過計(jì)算實(shí)時(shí)測量的R向量與指紋庫向量S的差異來進(jìn)行用戶位置估計(jì)，通常比較數(shù)據(jù)點(diǎn)之間的歐幾里德距離(ED)[13]。因此，使用傳統(tǒng)加權(quán)k-近鄰(WKNN)的算法考慮其最接近的k個(gè)數(shù)據(jù)點(diǎn)的加權(quán)值來估計(jì)待測點(diǎn)位置。待測點(diǎn)位置計(jì)算為

(4)

(5)

(6)

式(6)中：wi表示k個(gè)最接近待測點(diǎn)位置RPi的權(quán)值；(xi,yi)表示RPi的位置坐標(biāo)；(x,y)表示待測點(diǎn)估計(jì)位置坐標(biāo)。

3 WiFi與PDR融合定位算法

單一的WiFi室內(nèi)定位會(huì)因?yàn)樵肼?、同頻干擾、多徑效應(yīng)和室內(nèi)環(huán)境等因素的影響，造成RSSI的嚴(yán)重波動(dòng)而導(dǎo)致定位結(jié)果不準(zhǔn)確[1]?；诙鄠鞲衅鞯腜DR雖然在短時(shí)間內(nèi)精度高，但隨著時(shí)間推移會(huì)產(chǎn)生累積誤差和傳感器漂移誤差，同時(shí)單一的PDR導(dǎo)航也需要初始位置對準(zhǔn)。卡爾曼濾波在導(dǎo)航、制導(dǎo)和控制等領(lǐng)域應(yīng)用廣泛，傳統(tǒng)的卡爾曼濾波(Kalman filtering)利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)[14]。然而現(xiàn)實(shí)中大多存在非線性系統(tǒng)，UKF則常用于非線性系統(tǒng)的目標(biāo)狀態(tài)的最優(yōu)估計(jì)[15]。

3.1 UKF算法原理

非線性系統(tǒng)模型：

(7)

式(7)中：X(k)為k時(shí)刻的系統(tǒng)過程狀態(tài)；Z(k)為k時(shí)刻的系統(tǒng)測量狀態(tài)；f(·)為系統(tǒng)方程；h(·)為測量方程；W(k)為系統(tǒng)過程噪聲；V(k)為測量噪聲；W(k)、V(k)均為高斯白噪聲。系統(tǒng)噪聲協(xié)方差矩陣為Q，W(k)～N(0,Q)。測量噪聲協(xié)方差矩陣為R，V(k)～N(0,R)。UKF算法詳細(xì)步驟如下。

第1步狀態(tài)向量均值、方差初始化。

(8)

(9)

第2步通過UT變換得到2N+1個(gè)Sigma點(diǎn)。

(10)

第3步計(jì)算Sigma點(diǎn)相應(yīng)的權(quán)值。

(11)

式(11)中：i表示第i個(gè)Sigma點(diǎn)；下標(biāo)m表示均值；下標(biāo)c表示協(xié)方差；λ為縮放比例參數(shù)；α為參數(shù)控制采樣點(diǎn)的分布狀態(tài)；β為一個(gè)非負(fù)權(quán)系數(shù)。

第4步時(shí)間更新：

Xi(k+1|k)=f[Xi(k|k)]+W(k)

(12)

(13)

(14)

Zi(k+1|k)=h[Xi(k+1|k)]

(15)

(16)

式中：Xi(k+1|k)為第i個(gè)點(diǎn)k+1時(shí)刻的過程狀態(tài)；Zi(k+1|k)為第i個(gè)點(diǎn)k+1時(shí)刻的測量狀態(tài)。

第5步測量更新：

(17)

(18)

Kk(k+1)=PxzPzz-1

(19)

(20)

P(k+1|k+1)=P(k+1|k)-Kk(k+1)Pzz×

Kk(k+1)T

(21)

式中：Pzz為Sigma點(diǎn)測量狀態(tài)與測量均值的協(xié)方差；Pxz為Sigma點(diǎn)過程狀態(tài)與測量均值的協(xié)方差；Kk為卡爾曼增益系數(shù)；P(k+1|k+1)為k+1時(shí)刻系統(tǒng)協(xié)方差更新。

3.2 自適應(yīng)調(diào)整因子的改進(jìn)UKF算法

融合系統(tǒng)中觀測值的異常會(huì)造成UKF融合系統(tǒng)功能退化，導(dǎo)致最優(yōu)估計(jì)的精度和穩(wěn)定性下降。殘差表示實(shí)際測量值與基于濾波器內(nèi)部模型的最佳測量預(yù)測值之間的差異，可用于調(diào)節(jié)濾波器[16]。殘差的理論協(xié)方差與實(shí)際協(xié)方差差異可用于確定濾波器發(fā)散程度。使用殘差rk的理論協(xié)方差與實(shí)際協(xié)方差作為條件引入自適應(yīng)調(diào)整因子ρ，調(diào)整狀態(tài)向量和觀測向量的協(xié)方差從而調(diào)整卡爾曼增益參數(shù)，增強(qiáng)系統(tǒng)魯棒性和提高定位精度。系統(tǒng)殘差表示為

(22)

殘差rk的理論協(xié)方差為

(23)

殘差rk的實(shí)際協(xié)方差為

(24)

式(24)中：n表示估計(jì)窗口；i0=k-n+1。引入自適應(yīng)調(diào)整因子ρ，如式(25)所示：

(25)

diff=|tr(Ck+1)-tr(C′k+1)|

(26)

式中：ddiff為殘差的理論協(xié)方差與實(shí)際協(xié)方差之間的差異；tr(·)為協(xié)方差矩陣的跡，引入自適應(yīng)調(diào)整因子ρ后協(xié)方差及卡爾曼增益表示為

(27)

Kk(k+1)=ρPxzPzz-1

(28)

3.3 融合系統(tǒng)模型建立

根據(jù)3.2節(jié)所述融合算法原理，融合系統(tǒng)狀態(tài)方程及測量方程建立如下：

(29)

式(29)中：xk+1、yk+1為k+1時(shí)刻行人航跡估算二維坐標(biāo)；L為行人步長；θk+1為k+1時(shí)刻航向角。

(30)

式(30)中：x′k+1、y′k+1為WiFi指紋匹配結(jié)果的坐標(biāo)。

改進(jìn)UKF融合算法系統(tǒng)流程圖如圖2所示。

圖2 UKF融合系統(tǒng)流程Fig.2 UKF fusion system process

4 實(shí)驗(yàn)設(shè)計(jì)及數(shù)據(jù)分析

4.1 實(shí)驗(yàn)環(huán)境

紅色線路代表路徑1；綠色線路代表路徑2圖3 實(shí)驗(yàn)環(huán)境平面圖Fig.3 Experimental environment plan

4.2 仿真數(shù)據(jù)分析

從圖4～圖7中路徑1、2的實(shí)驗(yàn)結(jié)果對比可以看出，WiFi信號(hào)易受環(huán)境影響，信號(hào)波動(dòng)大，造成定位點(diǎn)跳點(diǎn)和堆積現(xiàn)象；基于多傳感器的PDR算法由于累積誤差的存在，隨著運(yùn)動(dòng)步數(shù)的增加導(dǎo)致漂移逐漸增大。標(biāo)準(zhǔn)UKF會(huì)因?yàn)閃iFi定位結(jié)果(觀測值)的異常值出現(xiàn)而導(dǎo)致系統(tǒng)融合效果出現(xiàn)不佳狀況，而加入自適應(yīng)調(diào)整因子后會(huì)有效的平衡融合系統(tǒng)中狀態(tài)信息和觀測信息的正確比例達(dá)到更優(yōu)的估計(jì)。

圖8可以看出標(biāo)準(zhǔn)UKF算法在1.5 m以內(nèi)的概率約為22%，本文算法位置精度在1.5 m以內(nèi)的概率達(dá)到了約80%，提高了約58%，其算法魯棒性獲得大幅度提高。

圖4 路徑1不同算法實(shí)驗(yàn)結(jié)果對比Fig.4 Comparison of experimental results of different algorithms for path 1

圖5 路徑1本文算法與UKF實(shí)驗(yàn)結(jié)果對比Fig.5 Comparison between the proposed algorithm and the results of the UKF experiment for path 1

圖6 路徑2不同算法實(shí)驗(yàn)結(jié)果對比Fig.6 Comparison of experimental results of different algorithms for path 2

圖7 路徑2本文算法與UKF實(shí)驗(yàn)結(jié)果對比Fig.7 Comparison between the proposed algorithm and the results of the UKF experiment for path 2

圖8 不同方法位置誤差累積誤差概率Fig.8 Accumulated error probability of position error in different methods

表1 幾種方法平均誤差、最大誤差對比

由表1可知，本文算法在平均精度上相比標(biāo)準(zhǔn)UKF有一定的提高，最大誤差也相應(yīng)有所減小，驗(yàn)證了調(diào)整因子的加入有效地提高了精度和增強(qiáng)了系統(tǒng)的魯棒性。

4.3 系統(tǒng)APP效果圖

APP使用Android Studio 3.0.1 搭建，數(shù)據(jù)庫使用自帶SQLite數(shù)據(jù)庫，調(diào)試運(yùn)行平臺(tái)為華為P10VTR-AL00(Android 9.0)。圖9為系統(tǒng)APP數(shù)據(jù)庫采集界面，圖10為系統(tǒng)行人航跡推算界面，圖11為室內(nèi)融合定位界面。

圖9 數(shù)據(jù)庫采集界面Fig.9 APP database collection interface

圖10 PDR航跡推算界面Fig.10 PDR track estimation interface

黑色軌跡為實(shí)際路徑；紅色軌跡為定位軌跡圖11 室內(nèi)融合定位界面Fig.11 Indoor fusion positioning interface

5 結(jié)論

針對單一WiFi室內(nèi)定位的不穩(wěn)定性和PDR的累積誤差問題，提出了一種自適應(yīng)UKF融合算法，該方法通過PDR運(yùn)動(dòng)狀態(tài)過程和WiFi定位信息加入調(diào)整因子ρ，調(diào)整狀態(tài)向量和觀測狀態(tài)的協(xié)方差和卡爾曼增益以增強(qiáng)融合系統(tǒng)的魯棒性，實(shí)驗(yàn)結(jié)果表明如下結(jié)論。

(1)本文算法平均定位精度達(dá)到1.35 m，有效降低了單一方式定位的誤差，與標(biāo)準(zhǔn)UKF濾波算法相比在定位精度上也有所提高。

(2)本文算法位置誤差在1.5 m內(nèi)的概率由原來約22%提高到約80%，提高了約58%，增強(qiáng)了融合定位系統(tǒng)的魯棒性。