基于EKF的初始狀態自適應室內融合定位算法*

胡文強， 胡建鵬， 吳 飛， 陸雯霞

(上海工程技術大學 電子電氣工程學院,上海 201620)

0 引 言

隨著5G的商用落地，室內定位技術再次引起人們的廣泛關注[1]。日益增長的生活與商業需求的位置服務刺激了室內定位系統和相關技術的快速發展。典型的室內定位技術包括Wi-Fi、藍牙、射頻識別(radio frequency identification,RFID)、超寬帶等。Wi-Fi定位的網絡覆蓋廣，可滿足大多數室內定位需求，但其信號易受環境干擾，導致定位結果波動較大。行人航位推算(pedestrian dead reckoning,PDR)定位方法利用慣性傳感單元(inertial measurement unit，IMU)實現航位推算定位，目前移動終端設備支持良好，但存在較大的慣性累積誤差[2]。

為提高上述單一定位系統的性能，研究人員對有關Wi-Fi與PDR的融合定位系統與技術進行了探討與分析。文獻[3]中Wi-Fi定位采用了一種改進的WKNN算法，PDR定位算法使用了多重約束條件的步態檢測和在線步長估計方法，最后將擴展卡爾曼濾波(EKF)作為Wi-Fi和PDR定位的融合濾波器。文獻[4]提出了一種魯棒約束性的卡爾曼濾波(KF)來增強基于位置服務的性能并用于Wi-Fi/PDR組合導航算法。文獻[5]采用的是一種基于最小二乘法的實時測距誤差補償模型和基于加權最小二乘法的自適應Wi-Fi精細時間測量定位算法，并利用緊耦合融合定位算法進行融合。文獻[6]提出了一種基于自適應粒子濾波的室內行人Wi-Fi與PDR 組合定位算法，通過自適應因子自動調節觀測量對粒子分布的影響。文獻[7]在Wi-Fi定位上將空間搜索限制與均值濾波相結合來提高計算效率和定位性能，PDR部分提出了一種帶有相關處理和測量噪聲姿態確定的EKF算法以獲取長期航向，并將一種自修正KF融合定位算法運用到Wi-Fi/MEMS融合階段。

上述研究通過融合方法仍存在如下問題：1)忽略了包括始發位置與初始航向角在內的初始狀態確定過程，這樣可能會導致定位系統在有限時間內無法快速收斂；2)在使用EKF算法進行融合時，Wi-Fi數據的誤差衡量往往使用了自定義的固定值，沒有很好的將真實環境下Wi-Fi數據波動所帶來的影響反饋給EKF系統。為此，本文在基于EKF的室內Wi-Fi/PDR融合定位算法基礎上，提出了一種初始狀態自適應的解算方法和一種基于鄰近狀態RSSI歐氏距離的改進EKF算法，降低了因初始值設定不當而導致的誤差，同時能夠根據定位環境與迭代所反饋的信息進行動態調整，從而提高了最終的定位精度。

1 單定位系統方法

1.1 基于WKNN的Wi-Fi指紋定位

Wi-Fi指紋定位包括兩個階段：采集指紋庫的離線階段和匹配定位的在線階段[8]。

離線階段通過移動終端收集特定Wi-Fi的信號強度數據，首先將待測區域劃分為幾個網格，用每個網格的頂點作為參考點(reference point，RP)；將每個RP經過優化后的特征信息(包括坐標信息、Wi-Fi信號強度值等)作為一組位置指紋數據[9]。

在線階段利用接收到的信號強度數據通過匹配算法得到當前移動終端的位置。在線階段使用WKNN算法，WKNN通過不同指紋節點的貢獻比來給每個指紋分配權重，提高了KNN的算法精度[8]。未知點的估算坐標為

(1)

式中x,y為RP的坐標，wi為第i個樣本點的權值。

1.2 PDR定位

PDR定位算法根據行人狀態(位置、航向角等)的變化，實時累積計算行人的位置[10]。PDR的定位過程為：二維平面內，行人的運動軌跡可以通過步長和方向角計算得到，若已知tk-1時刻的位置坐標為Ptk-1=(Etk-1,Ntk-1)，則tk時刻位置坐標為

(2)

式中dtk-1為tk-1時刻到tk時刻之間的行走步長，θtk-1為tk-1時刻的航向角。通過式(2)可以推算出每一步的狀態信息。

在步頻檢測上，PDR可采用峰值檢測法，通過檢測加速度計輸出信號的波峰來識別行人步數，對兩個連續波峰的搜索即確定為一步[11]。而在步長推算上，本文通過Weinberg步長估計模型來推算行人步長

(3)

式中k為步長比例因子，amax和amin分別為行人行走一步時所檢測到的最大加速度和最小加速度。

2 基于EKF的Wi-Fi/PDR融合定位算法

在EKF融合定位系統中，本文在初值確定上采用了初始多點協同卡爾曼濾波的方法，融合系統在EKF的迭代過程中對相鄰兩個狀態的RSSI歐氏距離進行度量，并反饋給EKF系統的下一狀態，從而降低了Wi-Fi數據波動對系統所造成的影響，獲得了精確而又穩定的融合定位結果，本文的系統流程如圖1所示。

圖1 Wi-Fi/PDR融合定位算法流程

2.1 初始狀態的解算

KF初值偏差可能會導致濾波計算的前若干步結果產生較大偏差，即使進入收斂狀態后，也可能因為初值設置不當，導致結果不能收斂到狀態的真實值。因此，本文采用了基于KF的初始位置和多點KF的航向角解算方法。

通過對KF公式的迭代推導[12]，可得第k次的迭代結果為

(4)

假設可以獲取初始m個定位點，其中,第i個定位點的Wi-Fi掃描次數為ki，則可以得到濾波后的定位點Li為

(5)

其中,初始位置為L0，而初始航向角為θ0

(6)

2.2 基于相鄰狀態RSSI距離反饋的EKF算法

由于EKF算法的初始協方差矩陣為自定義常量，但復雜環境下的動態系統，其協方差矩陣不會為固定值，這里采用了以相鄰狀態下的RSSI歐氏距離為基礎的噪聲反饋機制[13]。

行人行走過程中，對相鄰兩步的RSSI信號指紋進行歐氏距離的求解，在每個系統中，它會保持在穩定的取值范圍里面，記為[Dmin,Dmax]，然后將其線性放縮到了一個定位精度的閾值范圍，即[Smin,Smax]，最后將其動態作為EKF的觀測協方差矩陣的參數，以此來動態衡量Wi-Fi的噪聲。

通過上一步的距離差決定下一步的觀測協方差矩陣的協方差值，如式(7)所示

(7)

式中Dk為未知點與第k個狀態的RSSI與上一個狀態的歐氏距離。

通過多組實驗測量可以得到在一個固定環境下Dk的取值范圍為[Dmin,Dmax]，而該系統的定位精度范圍為[Smin,Smax]，對其進行尺度縮放，則有

(8)

EKF的具體過程如下所示:

系統狀態方程、觀測方程分別為

(9)

(10)

由Wi-Fi指紋定位給出融合系統的初始位置(x1,y1)，初始協方差矩陣為P1，系統過程噪聲協方差矩陣Q由行人航位推算方法各要素的平均誤差組成，觀測噪聲協方差矩陣由Wi-Fi指紋定位的平均誤差組成。融合定位系統狀態值如下

(11)

(12)

EKF線性化與初始化完成后，濾波主要過程如下所示

X′k=AXk-1

(13)

P′k=APk-1AT+Q

(14)

Kk=P′kHT(HP′kHT+R)-1

(15)

Xk=X′k+Kk(Zk-HX′k)

(16)

Pk=(1-KkH)P′k

(17)

3 算法驗證與分析

本文選取某校電子電氣工程學院大樓7樓作為實驗場所，在地面標定1 m×1 m方格，并以其頂點作為實驗樣本采集點，如圖2所示。實驗選取了8個TP-LINK型號為TL—WR886N的路由器作為定位信標，采用華為榮耀8作為PDR和Wi-Fi數據的采集工具，分別采用Wi-Fi定位、PDR定位和基于EKF的Wi-Fi/PDR融合定位算法進行定位實驗。實驗的行走軌跡為L型路線，起始坐標為(2,1)，終點坐標為(14,13)，總共行走步伐為30步，總計31個位置狀態，實驗采用了固定步長的行走方式，每一步的距離為0.8 m。

圖2 實驗場景

3.1 初始狀態的分析實驗

對于初始位置，在初始航向角已知的情況下(設為0°)，以1 m為間隔，將初始位置偏差值設置為1～10 m，對于其中1，5，10 m軌跡的偏移情況如圖3(a)所示。由輸出的軌跡圖可見，雖然在開始時會有較大的偏離，但最后都能收斂到正常值附近。往往位置偏差的距離越大，收斂的迭代次數也會越大，但是偏差超過一定界限以后，收斂次數會相對穩定下來，如表1所示。

圖3 位置與航向角偏差對結果的影響

表1 位置偏差與其對應的收斂迭代次數

對于初始航向角，在初始位置已知的情況下(設為(2,1))，以5°為間隔，將初始角度偏差值設置為10°～60°，對于其中10°，30°，60°軌跡偏移情況如圖3(b)所示。通過輸出的軌跡圖可見，初始航向角的偏差不會影響輸出結果的收斂速度，但結果會產生持續的偏差，偏差結果會以較慢的速度向正常值附近逼近，所以可以通過平均定位誤差來衡量航向角偏差對結果造成的影響，如表2所示。通過數據可以發現，航向角偏差越大，平均誤差也會越大，當前實驗數據的Wi-Fi定位精度為1.92 m，而在航向角偏差達到45°時，30步迭代后的誤差值為1.99 m，此時融合定位的系統誤差大于單個系統的誤差。

表2 航向角偏差及其對應的平均誤差

通過對以上數據的分析，可以得出：通過EKF對Wi-Fi與PDR進行融合定位時，需要滿足以下條件可以視為一組可接受的初始狀態

Δs≤3m and Δθ≤40°

(18)

如果EKF融合定位系統比單一系統至少要提高30 %的精確度，并以此視為一組良好的初始狀態，則需要滿足以下條件

Δs≤3m and Δθ≤30°

(19)

3.2 初始狀態的解算實驗

實驗分析了式(6)中m取值不同時初始狀態的偏差情況，對5組實驗數據進行求解得到表3的結果，初始位置的平均偏差為Δs=0.11 m，滿足式(19)。航向角的偏差情況與其算法復雜度如圖4所示，隨著m增加，航向角初始狀態越好，盡管隨之復雜度也會增加，但m=5時算法耗時也在20 ms內，不會影響最終的用戶體驗。

表3 初始狀態解算實驗結果

圖4 不同m取值的算法解算結果

通過本文提出的初始狀態解算方法，定位結果能夠快速收斂，4步內即可達到收斂狀態，輸出圖像逼近真實軌跡，如圖5所示。

圖5 多點協同KF的初始狀態解算方法

3.3 改進的EKF算法

圖6 定位軌跡對比

圖7 定位誤差對比

結果表明Wi-Fi定位雖然能夠輸出一個絕對定位值，但數據波動較大，其平均定位精度為1.92 m ，PDR定位在短時間內可以保持良好的精確度，但隨著迭代次數的增加，累積誤差會越來越大，在30步內其平均誤差為1.19 m，但到達終點時誤差為2.21 m。

4 結束語

本文提出的改進EKF算法能很好地融合了Wi-Fi與PDR的優勢，其平均精度為0.97 m，相較于其他EKF算法，其精度有較為明顯的改善，其輸出結果比其他EKF算法平均精度的1.14 m改善了17.2 %，表明本文所提方法能夠有效抑制誤差，提高定位精度。