基于抗差EKF的Wi-Fi/MEMS融合室內(nèi)定位算法

張千坤，鐘志剛，陳任翔，張校偉（.中訊郵電咨詢?cè)O(shè)計(jì)院有限公司鄭州分公司，河南鄭州 450007；.中訊郵電咨詢?cè)O(shè)計(jì)院有限公司，北京 00048）

1 概述

隨著基于位置的服務(wù)（LBS）［1-2］被越來越多的移動(dòng)用戶所使用，給無(wú)線網(wǎng)絡(luò)定位技術(shù)帶來了巨大的機(jī)遇。在室外環(huán)境下，全球?qū)Ш叫l(wèi)星系統(tǒng)（GNSS）基本可以滿足移動(dòng)用戶的定位需求，但是在市區(qū)或者室內(nèi)環(huán)境下會(huì)受到建筑物的影響，性能變差無(wú)法達(dá)到定位的要求［3］。

為了彌補(bǔ)GNSS定位方面的缺陷，基于無(wú)線網(wǎng)［4-5］、藍(lán)牙［6］、超寬帶（UWB）［7］等的定位技術(shù)開始普及，其中基于無(wú)線網(wǎng)的定位技術(shù)由于造價(jià)低、分布廣，應(yīng)用最為廣泛，主要包括基于指紋的定位技術(shù)［8］、基于測(cè)距的定位技術(shù)［9］、基于測(cè)角的定位技術(shù)［10］。但是，由于環(huán)境因素、定位精度等問題限制了無(wú)線網(wǎng)單一定位技術(shù)的發(fā)展，其中指紋定位技術(shù)由于受多徑干擾的影響，信號(hào)接收強(qiáng)度（RSSI）會(huì)發(fā)生抖動(dòng)，影響定位的效果，測(cè)角與測(cè)距的定位技術(shù)需要在有直射徑的環(huán)境下才能準(zhǔn)確定位，在非視距的環(huán)境下會(huì)產(chǎn)生很大的定位誤差。近年來，數(shù)據(jù)融合廣泛用于室內(nèi)定位中，基于此，本文提出一種新的定位方法，首先對(duì)線陣AOA 估計(jì)技術(shù)進(jìn)行改進(jìn)，提出一種基于L陣的空間平滑AOA 估計(jì)技術(shù)，可以準(zhǔn)確地估計(jì)出信號(hào)的方位角與俯仰角，提高角度估計(jì)的準(zhǔn)確性；其次，聯(lián)合多個(gè)Wi-Fi 接入點(diǎn)（AP）測(cè)量的角度利用最小二乘擬合得到移動(dòng)終端的位置；最后設(shè)計(jì)抗差卡爾曼濾波器（EKF），將AOA 定位結(jié)果與MEMS 傳感器解算的信息進(jìn)行融合，抑制MEMS 傳感器產(chǎn)生的累計(jì)誤差，并解決AOA 定位在NLOS環(huán)境下無(wú)法定位的問題。

2 定位算法描述

本文提出的基于抗差EKF的Wi-Fi/MEMS 融合室內(nèi)定位系統(tǒng)框圖如圖1 所示。主要包括4 個(gè)部分，分別是基于Wi-Fi 的AOA 定位模塊，速度與航向角計(jì)算模塊，抗差EKF 濾波模塊以及位置結(jié)算模塊。首先利用從Wi-Fi 設(shè)備中提取的CSI 信息通過一種空間平滑技術(shù)虛擬大規(guī)模陣列天線，利用2 維L 陣MUSIC 算法對(duì)俯仰角和方位角進(jìn)行估計(jì)，并利用最小二乘算法結(jié)合多個(gè)AP的角度得到定位結(jié)果；其次根據(jù)MEMS傳感器輸出的參數(shù)對(duì)目標(biāo)進(jìn)行步態(tài)檢測(cè)和四元素計(jì)算，得到目標(biāo)的速度信息；最后將Wi-Fi 的定位結(jié)果作為抗差EKF 濾波器的輸入信息，對(duì)目標(biāo)進(jìn)行二維定位，并結(jié)合氣壓計(jì)的輸出信息，得到目標(biāo)的高度進(jìn)行三維定位。

圖1 系統(tǒng)框圖

2.1 基于Wi-Fi的AOA定位技術(shù)

由文獻(xiàn)［11］可知，傳統(tǒng)的MUSIC 算法只能解算出小于陣列天線數(shù)量的信號(hào)個(gè)數(shù)，利用子載波CSI 信息可以重新構(gòu)造陣列天線的方向矩陣，擴(kuò)展信號(hào)子空間特征向量，從而虛擬大規(guī)模的陣列天線，解決在室內(nèi)環(huán)境下由于多徑信號(hào)過多，造成MUSIC 算法無(wú)法區(qū)分出信號(hào)子空間與噪聲子空間的問題［12］。但是這種算法只能估計(jì)到達(dá)信號(hào)的方位角，如果終端與AP 之間的高度落差較大，則角度估計(jì)會(huì)出現(xiàn)偏差。本文在此基礎(chǔ)上，提出一種L陣空間平滑AOA 估計(jì)算法，可以同時(shí)對(duì)俯仰角和方位角進(jìn)行估計(jì)，克服了高度對(duì)方位角估計(jì)的影響。

由于Wi-Fi 信號(hào)是OFDM 信號(hào)，信號(hào)到達(dá)天線引起的相位差包括2 個(gè)部分，分別是由信號(hào)到達(dá)時(shí)間引起的不同子載波之間的相位差以及信號(hào)到達(dá)不同天線由角度引起的相位差。

信號(hào)到達(dá)時(shí)間引起的相位差可以表示為Ωτk=e-j2πΔfτk，L陣天線之間的相位差包含2 個(gè)部分，分別為，當(dāng) 有2×M-1 根天線（x軸M根，y軸M-1 根），N個(gè)子載波時(shí)，接收到信號(hào)的方向矩陣可以表示為：

式中：

Φx,θk,φk——x軸2根天線之間的相位差

Φy,θk,φk——y軸2根天線之間的相位差

由無(wú)線網(wǎng)卡采集到L陣3個(gè)通道的90個(gè)子載波可以表示為：

其中csim,n(m=1,2,3;n=1,2,…,30)表示的是第m根天線，第n個(gè)子載波上測(cè)量得到的CSI。

圖2 空間平滑示意圖

在每根天線上選擇15 個(gè)子載波為一組進(jìn)行前項(xiàng)空間平滑［13］，平滑后的結(jié)果如圖3所示。

圖3 空間平滑結(jié)果

從圖3中可以看出，經(jīng)過空間平滑以后，可以由原先的三根天線虛擬出現(xiàn)在的45根天線，遠(yuǎn)大于室內(nèi)多徑信號(hào)的數(shù)量，結(jié)合文獻(xiàn)［11］利用MUSIC 算法可以準(zhǔn)確地估計(jì)出信號(hào)的方位角。同時(shí)結(jié)合多臺(tái)AP 的AOA信息，利用最小二乘算法對(duì)目標(biāo)的位置信息進(jìn)行解算。

式中：

R——AP的個(gè)數(shù)

——終端在位置p時(shí)到第i臺(tái)接收機(jī)的角度

θi——第i臺(tái)接收機(jī)估計(jì)出的角度

2.2 速度與航向角估計(jì)

本系統(tǒng)選擇手機(jī)自帶的慣性傳感器進(jìn)行定位，定位框圖如圖4所示。

圖4 MEMS定位框圖

2.2.1 速度估計(jì)

行人在行走的時(shí)候加速度以正弦波的形式變化［14］，因此可以通過三軸加速度計(jì)測(cè)量出的值來檢測(cè)行人的步態(tài)，三軸加速度計(jì)值建立的步態(tài)檢測(cè)模型可以表示為：

式中：

ax，ay，az——三軸加速度數(shù)據(jù)

為了消除傳感器自帶的噪聲干擾，本文選擇低通濾波器對(duì)三軸加速度計(jì)的輸出值進(jìn)行濾波處理。因此公式（4）可以寫成：

式中：

axfilter，ayfilter，azfilter——濾波后的三軸加速度計(jì)輸出值

本文通過檢測(cè)Atotal是否為局部最大值以及是否大于給定的閾值A(chǔ)0來判定此時(shí)目標(biāo)是否跨步。假設(shè)MEMS傳感器的輸出頻率為fs，利用步態(tài)檢測(cè)可以檢測(cè)出每秒由于目標(biāo)走動(dòng)而出現(xiàn)的峰值點(diǎn)的個(gè)數(shù)，從而得到相鄰峰值點(diǎn)之間的采樣個(gè)數(shù)ΔN，則可以得到目標(biāo)走到第k步需要的時(shí)間為，進(jìn)而得到速度為vk=，其中vk表示的是第k步時(shí)的速度，Pk表示第k步時(shí)的步長(zhǎng)。

2.2.2 航向角估計(jì)

本文采用四元素法對(duì)終端的航向角進(jìn)行計(jì)算，四元素法的表示形式為q=w+xi+yj+zk，

載體坐標(biāo)系與參考坐標(biāo)系之間的轉(zhuǎn)換為：

其中(q)為姿態(tài)旋轉(zhuǎn)矩陣，根據(jù)姿態(tài)旋轉(zhuǎn)矩陣與四元素的關(guān)系［15］可以得到：

在已知四元素后，通過哥氏定理變換［16］可以得出四元素的微分方程：

式中：

ωx，ωy，ωz——傳感器在載體坐標(biāo)系下的三軸角速度的數(shù)據(jù)

本文設(shè)置四元素為濾波器的狀態(tài)變量，對(duì)式（8）進(jìn)行離散化處理，得到：

式中：

Ts——采樣時(shí)間間隔

I——單位矩陣

——初始四元素

k——采樣時(shí)刻

對(duì)四元素進(jìn)行歸一化結(jié)合公式（9）即可更新姿態(tài)旋轉(zhuǎn)矩陣。

由于陀螺儀的不理想容易造成四元素計(jì)算時(shí)出現(xiàn)誤差，本文使用磁力計(jì)和加速度計(jì)來建立新的狀態(tài)方程和觀測(cè)方程：

式中：

Wk——過程激勵(lì)噪聲

Vk+1——觀測(cè)噪聲

Yk+1——觀測(cè)變量，計(jì)算公式如下：

通過擴(kuò)展卡爾曼模型更新四元素，利用四元素與歐拉角的關(guān)系［15］得到航向角為：

3 Wi-Fi/MEMS融合定位

本文采用EKF 算法將Wi-Fi 的定位結(jié)果與MEMS傳感器解算的速度與航向進(jìn)行組合，建立如下狀態(tài)方程：

式中：

Et-1和Nt-1——t-1時(shí)刻的地理東位置和北位置

νt-1和φt-1——t-1時(shí)刻終端的速度和航向角

Wt-1——t-1時(shí)刻的狀態(tài)噪聲

p(Wt-1)～N(0,Qt)，Qt——狀態(tài)噪聲矩陣的協(xié)方差矩陣

則觀測(cè)方程可以表示為：

式中：

Vt——t時(shí)刻的觀測(cè)噪聲

p(vt)～N(0,Rt)，Rt——觀測(cè)噪聲協(xié)方差矩陣

抗差M估計(jì)［17］又稱M-LS 抗差估計(jì)，是一種基于等價(jià)權(quán)模型的抗差估計(jì)，通過構(gòu)造等價(jià)權(quán)，使未知參數(shù)估計(jì)盡可能避免受到粗誤差的影響，為了解決實(shí)際環(huán)境中的粗誤差以及模型誤差，本文將抗差M估計(jì)與EKF 算法相結(jié)合，得到抗差EKF 濾波器，此時(shí)卡爾曼增益可以寫為：

式中：

(t)——卡爾曼增益

P(t,t-1)——t時(shí)刻通過估計(jì)得到的誤差協(xié)方差矩陣

H(t)——t時(shí)刻的系統(tǒng)測(cè)量矩陣

(t)——觀測(cè)噪聲協(xié)方差矩陣

根據(jù)文獻(xiàn)［18］，IGG3函數(shù)表示為

為滿足在室內(nèi)環(huán)境下跨樓層定位需求，本文通過使用氣壓計(jì)來解算終端的高度，根據(jù)壓高公式：

式中：

Ps——?dú)鈮褐怠?/p>

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

本文采用Intel 5300 網(wǎng)卡來采集CSI 信息，所使用的智能手機(jī)終端包含有三軸加速度計(jì)、磁力計(jì)、陀螺儀以及氣壓計(jì)等，滿足Wi-Fi與MEMS融合的條件。

為了測(cè)試高度對(duì)線陣角度估計(jì)的影響，本文首先選擇室外環(huán)境，分別將AP和發(fā)射機(jī)之間的相對(duì)高度設(shè)置為0 cm、100 cm、200 cm，得到如表1所示結(jié)果。

從表1 中可以看出高度對(duì)線陣的角度估計(jì)有影響，隨著高度的增加誤差逐漸增大，繼續(xù)用L陣進(jìn)行測(cè)試得到的結(jié)果如表2所示。

表1 不同高度線陣角度估計(jì)誤差統(tǒng)計(jì)

從表2 中可以看出，增加高度對(duì)L陣的角度估計(jì)影響不大，在有相對(duì)高度時(shí)，角度估計(jì)的結(jié)果要優(yōu)于線陣。

表2 不同高度L陣角度估計(jì)誤差統(tǒng)計(jì)

隨后，本文選擇典型的室內(nèi)環(huán)境進(jìn)行測(cè)試，長(zhǎng)和寬分別為12.5 和7.5 m，接收機(jī)放置高度為2.4 m，角度測(cè)試結(jié)果如圖5所示。

從圖5 中可以看出，在相同高度下基于L 陣的角度估計(jì)優(yōu)于線陣，說明高度對(duì)線陣方位角估計(jì)有影響，本文提出的基于L 陣的超分辨角度估計(jì)算法可以很好地解決這個(gè)問題。

圖5 角度估計(jì)結(jié)果

本文選擇4臺(tái)AP 在室內(nèi)環(huán)境下進(jìn)行定位測(cè)試，具體的行走軌跡圖如圖6 所示，定位環(huán)境包含LOS 環(huán)境以及NLOS 環(huán)境，圖6 中S1-S2 為L(zhǎng)OS 環(huán)境（AP 與終端之間存在直射徑），S2-S3為NLOS環(huán)境，得到的定位結(jié)果如圖7所示。

圖6 室內(nèi)行走軌跡圖

圖7 室內(nèi)定位結(jié)果

從圖7中定位結(jié)果可以看出，在NLOS環(huán)境下由于接收機(jī)接收不到直射徑，造成角度估計(jì)精度降低，使得定位結(jié)果比LOS 環(huán)境下定位結(jié)果差，但是當(dāng)AOA 與MEMS傳感器融合以后，克服了NLOS環(huán)境下定位精度較低的問題，且較單AOA 定位系統(tǒng)，定位精度明顯提升，中值誤差在1 m 以內(nèi)，可以滿足室內(nèi)場(chǎng)景的定位需求。

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

本文提出一種基于抗差EKF的Wi-Fi/MEMS 融合室內(nèi)算法，研究了包括改善AOA 估計(jì)技術(shù)，提出了基于Wi-Fi 的超分辨L 陣AOA 估計(jì)算法，提高了角度估計(jì)的精度，并聯(lián)合多個(gè)AP 確定移動(dòng)終端的位置，在此基礎(chǔ)上，將Wi-Fi 定位結(jié)果與MEMS 信息通過抗差EKF 濾波器進(jìn)行融合，解決NLOS 環(huán)境下無(wú)法定位的問題，提高了定位精度。該算法利用CSI 信息進(jìn)行定位，很容易移植到4G LTE系統(tǒng)以及5G NR系統(tǒng)中。