基于混合H2／H∞濾波信息融合的室內(nèi)定位算法*

張 呈，張大龍，劉成明，喬寅嵩，孫 頂

（鄭州大學(xué) 網(wǎng)絡(luò)空間安全學(xué)院，河南 鄭州 450002）

0 引 言

隨著室內(nèi)定位服務(wù)（indoor positioning service，IPS）在個(gè)人生活、商業(yè)、工業(yè)管理等領(lǐng)域的需求不斷增長下，室內(nèi)定位技術(shù)也在快速發(fā)展［1］。目前，基于接收信號強(qiáng)度指示（received signal strength indication，RSSI）的WiFi、低功耗藍(lán)牙信標(biāo)（iBeacon）定位技術(shù)應(yīng)用較為廣泛，這些技術(shù)可以達(dá)到一定程度的定位需求。但由于室內(nèi)環(huán)境存在的多徑傳播、衰減、非視距問題以及信標(biāo)精度的原因，導(dǎo)致其定位精度并不高。行人航位推算（pedestrian dead reckoning，PDR）不需要部署信標(biāo)，是一種基于慣性傳感器（加速度計(jì)，陀螺儀，磁力計(jì)）的定位技術(shù)，如今智能手機(jī)也集成了慣性傳感器，使其具有了成本低，使用方便，應(yīng)用場景廣泛的優(yōu)點(diǎn)。但慣性傳感器精度低和累積定位誤差的問題限制了PDR算法不能獨(dú)立使用。由于單種定位技術(shù)自身存在著特性缺陷，基于融合互補(bǔ)的室內(nèi)定位在近年來越來越得到關(guān)注。文獻(xiàn)［2］提出了一種基于WiFi、藍(lán)牙、PDR 多源信息融合的室內(nèi)定位方法，通過隨機(jī)森林（random forest，RF）和K最近鄰（K-nearest neighbor，KNN）相結(jié)合提升了指紋匹配的準(zhǔn)確性，使用粒子濾波及地圖約束的方法來融合多源信息，提高了整體定位精度。文獻(xiàn)［3］采用擴(kuò)展卡爾曼濾波（extended Kalman filtering，EKF）改進(jìn)WiFi／PDR 定位系統(tǒng)，結(jié)合利用實(shí)時(shí)記錄的氣壓計(jì)和地理數(shù)據(jù)計(jì)算目標(biāo)高度，實(shí)現(xiàn)室內(nèi)三維定位。文獻(xiàn)［4］提出了一種PDR 和地磁指紋融合的方法，不需要部署額外信標(biāo)裝置，通過螢火蟲算法（firefly algorithm，F(xiàn)A）優(yōu)化粒子濾波的采樣過程，提高了濾波精度。上述提到的粒子濾波和卡爾曼濾波（Kalman filtering，KF），前者計(jì)算量大導(dǎo)致實(shí)時(shí)性差，而后者要求系統(tǒng)符合高斯分布的假設(shè)前提，室內(nèi)非視距環(huán)境下，系統(tǒng)往往不符合該前提，從而影響了定位系統(tǒng)的精度和魯棒性。

本文提出使用混合H2／H∞濾波作為RSSI 定位結(jié)果和PDR算法的融合器，一方面使用RSSI 定位結(jié)果校正了PDR算法減少了累積誤差，另一方面不對噪聲的分布和模型誤差做出過多要求，更符合實(shí)際環(huán)境需求。

1 改進(jìn)計(jì)算效率的加權(quán)KNN定位方法

指紋定位是一種常用的非測距定位技術(shù)，適合非視距特征明顯的室內(nèi)環(huán)境，定位過程分為離線采樣和在線定位2個(gè)階段。離線采樣階段的目的是為了建立指紋數(shù)據(jù)庫：在定位區(qū)域按一定間隔設(shè)置采樣點(diǎn)RSSI，一組RSSI 序列值采樣結(jié)果對應(yīng)著一個(gè)位置信息，即為一條指紋。對收集到的多條指紋數(shù)據(jù)處理后形成指紋庫。

在線定位階段的任務(wù)是通過當(dāng)前待定位結(jié)點(diǎn)收集到的RSSI序列，在指紋數(shù)據(jù)庫執(zhí)行匹配算法，得到估計(jì)位置。加權(quán)KNN（weighed KNN，WKNN）算法是常用的指紋匹配算法，它在KNN算法基礎(chǔ)上采用加權(quán)系數(shù)對匹配到的k個(gè)指紋位置坐標(biāo)進(jìn)行加權(quán)位置估計(jì)，常用的加權(quán)系數(shù)為與指紋的歐氏距離倒數(shù)。

為了保證定位的實(shí)時(shí)性，指紋定位的頻率一般設(shè)置比較高，由于移動終端的計(jì)算資源有限，可能會出現(xiàn)計(jì)算結(jié)果延遲的現(xiàn)象。針對此問題，本文提出一種結(jié)合哈希表和Kd-Tree的存儲結(jié)構(gòu)，用于改進(jìn)WKNN 算法指紋匹配的檢索效率。Kd-Tree是一種軸對齊的BSP 樹，常用于KNN 算法在大規(guī)模的高維數(shù)據(jù)空間進(jìn)行快速檢索，具有場景自適應(yīng)劃分、低存儲消耗和快速遍歷等優(yōu)勢［5］?；贙d-Tree樹的KNN算法有建樹和搜索2 個(gè)步驟，文獻(xiàn)［6］介紹了該算法具體過程。在一次匹配中，將有效信標(biāo)的標(biāo)識連接作為鍵，把相關(guān)指紋庫數(shù)據(jù)構(gòu)建成Kd-Tree作為值，將生成的鍵值對存在哈希表中?？紤]行人的移動特征，一定時(shí)間內(nèi)有效信標(biāo)選擇大概率是相同的，即后續(xù)相同鍵的待定位點(diǎn)，可直接檢索，這樣就達(dá)到了一次建樹、多次搜索的目的。該結(jié)構(gòu)在行人小范圍移動、長時(shí)間停留的場景中，有效地提高了計(jì)算效率。改進(jìn)后的指紋在線定位過程如圖1所示。

圖1 改進(jìn)計(jì)算效率的WKNN定位過程

具體描述如下：

1）初始階段：定位前在移動終端初始化哈希表，鍵類型為信標(biāo)序列標(biāo)識的連接字符串（形式如“AP1-AP2-AP3-AP4”），值類型為Kd-Tree結(jié)構(gòu)。

2）收集階段：假設(shè)指紋定位在線階段一次收到的RSSI信號序列按值排序，取最大的m個(gè)作為有效信標(biāo)。將這幾個(gè)有效信標(biāo)標(biāo)識連接作為鍵。

3）哈希查找階段：在哈希表中查找第二步得到的鍵，命中跳轉(zhuǎn)第5步，不命中則執(zhí)行步驟4的建樹操作。

4）建樹階段：在原始指紋庫取鍵對應(yīng)的信標(biāo)列，過濾無效行后建立Kd-Tree結(jié)構(gòu)作為值。將鍵值對添加到哈希表中。

5）WKNN（Kd-Tree）階段：用哈希檢索到的Kd-Tree 作KNN運(yùn)算得到最近鄰的k條指紋，已待測點(diǎn)RSSI序列與指紋點(diǎn)RSSI序列歐氏距離的倒數(shù)為權(quán)值得到最終估計(jì)位置

式中 d為指紋與待定位點(diǎn)之間的歐氏距離。

2 PDR算法原理

PDR是一種由已知位置結(jié)合慣性傳感器得到的估計(jì)步頻、估計(jì)航向和估計(jì)步長，計(jì)算出下一步位置的相對定位算法［7］。

算法原理如圖2所示，式（2）為一步航位推算的公式

圖2 航位推算原理

式中 （Ek，Nk）為位置坐標(biāo)，Sk為步長，θk為航向。PDR可分為步態(tài)檢測算法、步長估計(jì)算法、航向估計(jì)算法3部分內(nèi)容。

2.1 PDR步態(tài)檢測算法

步態(tài)檢測算法的原理：行人行走一步過程中加速度計(jì)三軸會呈現(xiàn)明顯的波峰，其中垂直地面方向的波形更為明顯，同時(shí)隨著人不斷行走呈現(xiàn)出周期性特征。針對不同人體部位的加速度波形，常用步頻估計(jì)算法有峰值探測法、零點(diǎn)交叉法、自相關(guān)法、快速傅里葉變換（fast Fourier transform，F(xiàn)FT）法，Stance Phase 探測法等［8］。為了消除重力分量和儀器噪聲的影響需要先對加速度計(jì)測量信號進(jìn)行預(yù)處理，常用預(yù)處理方法有：多點(diǎn)平滑、低通濾波、差分處理、小波去噪等。

2.2 PDR步長估計(jì)模型

步長估計(jì)模型有常數(shù)模型、線性模型、非線性模型等。式（3）為常數(shù)模型，其中Sk為步長，ws為高斯噪聲，它假設(shè)人的步長宏觀上是穩(wěn)定，變化不大的。實(shí)際上人的行走姿勢多變，步長差異大，所以該模型的誤差較大

文獻(xiàn)［9］提出一種基于步頻和行人身高的步長線性模型

式中 h為身高；fstep為步頻；a，b，c為模型系數(shù)。實(shí)驗(yàn)表明該模型錯(cuò)誤率在5%，但是要給步行者設(shè)置特定參數(shù)，實(shí)用性低。

Weinberg模型［10］是非線性模型的典型代表，研究發(fā)現(xiàn)，人的步長和垂直加速度的變化存在相關(guān)關(guān)系，據(jù)此提出了式（5）中基于步態(tài)垂直加速度最大值和最小值的非線性模型

式中 Amax，Amin分別為行人一步內(nèi)垂直地面方向的最大加速度和最小加速度；K 為模型系數(shù)。該模型的精度較高且易實(shí)現(xiàn)。

2.3 PDR航向估計(jì)

剛體的姿態(tài)一般可以用旋轉(zhuǎn)矢量、旋轉(zhuǎn)矩陣、四元數(shù)、和歐拉角這4種方式表示［11］。Android手機(jī)的旋轉(zhuǎn)向量傳感器（Sensor.TYPE_ROTATION_VECTOR）是一種基于陀螺儀、磁力計(jì)和加速度計(jì)融合得到的軟件傳感器，通過它可以得到四元數(shù)，再由式（6）轉(zhuǎn)換為歐拉角，最后再將偏航角和真實(shí)坐標(biāo)系配準(zhǔn)可得到航向

式中 q0，q1，q2，q3為四元數(shù)；yaw，pitch，roll 分別為偏航角、俯仰角、橫滾角。偏航角表示設(shè)備的y軸與磁北極之間的夾角，俯仰角表示平行于設(shè)備屏幕的平面與平行于地面的平面之間的夾角，翻滾角表示垂直于設(shè)備屏幕的平面和垂直于地面的平面之間的角度。使用atan2 函數(shù)代替arctan，因?yàn)楹笳呷≈捣秶鸀椋?π／2，π／2］，而旋轉(zhuǎn)范圍為360°。此外，Android傳感器庫中的getOrientation（）方法提供了一種快捷的方式，它返回一個(gè)偏航角、俯仰角、橫滾角的弧度數(shù)組，該方法思路是通過旋轉(zhuǎn)矩陣轉(zhuǎn)換得到歐拉角。

3 濾波融合算法

3.1 H∞濾波算法

離散H∞濾波的算法流程如下

1）對于系統(tǒng)方程

式中 Xk，Yk分別為狀態(tài)向量、量測向量；Fk，Hk分別為狀態(tài)矩陣、量測矩陣；wk，vk為未知分布的噪聲。Zk為估計(jì)狀態(tài)的線性組合，Lk為自定義矩陣，當(dāng)Lk＝I時(shí)直接估計(jì)狀態(tài)Xk。

2）定義代價(jià)函數(shù)

式中 P0，Qk，Rk分別為初始估計(jì)誤差、系統(tǒng)噪聲、觀測噪聲矩陣；Sk為狀態(tài)變量線性組合估計(jì)誤差協(xié)方差陣。上述4個(gè)矩陣均為對稱正定矩陣。

3）在濾波過程時(shí)刻保持

4）下述更新過程可使得代價(jià)函數(shù)J ＜1／θ

3.2 H∞濾波與KF的聯(lián)系

文獻(xiàn)［12］基于矩陣逆定理推導(dǎo)了KF的等價(jià)形式

對比發(fā)現(xiàn)H∞濾波和此形式KF的區(qū)別在于，濾波增益和估計(jì)誤差協(xié)方差更新方程中H∞濾波多了一個(gè)項(xiàng)。當(dāng)θ ＝0時(shí)，H∞濾波將轉(zhuǎn)換為基于H2范數(shù)的KF。

3.3 混合H2／H∞濾波

文獻(xiàn)［13］提出了一種基于KF增益K（2）和H∞濾波增益K（∞）凸組合的混合濾波增益，表示如下

式中 d∈［0，1］。該方法在KF 最小均方差估計(jì)結(jié)果和H∞的悲觀估計(jì)提供了一種均衡。該混合濾波算法的關(guān)鍵設(shè)計(jì)在于權(quán)值K的設(shè)置。主要考慮2 個(gè)方面：1）保證凸組合混合濾波增益的穩(wěn)定性；2）需要考慮KF相對與H∞濾波性能的相對權(quán)重。文獻(xiàn)［14］基于上述思想推導(dǎo)出了在認(rèn)為2種濾波不相關(guān)條件下，混合估計(jì)誤差方差P最小的d，具體過程如下：混合H2／H∞濾波的估計(jì)誤差方差陣表示為

式中 P（2），P（2∞），P（∞）分別為KF估計(jì)誤差方差陣、KF估計(jì)誤差與H∞濾波估計(jì)誤差協(xié)方差陣、H∞濾波估計(jì)誤差方差陣。若使混合估計(jì)誤差方差P 最小，則等價(jià)于使P 的秩tr（P）最小，并且可以認(rèn)為2 種濾波估計(jì)不相關(guān)，即P（2∞）＝0。根據(jù)上述條件最后由?［tr（P）］／?d ＝0推導(dǎo)得出

3.4 混合H2／H∞濾波融合RSSI和PDR算法

指紋匹配得到的定位點(diǎn)是離散絕對的，而PDR推算出的定位點(diǎn)是連續(xù)相對的，將指紋的定位結(jié)果和PDR信息融合，用得到的融合位置再去校正PDR 算法，可以降低PDR的累計(jì)誤差，得到比單種定位技術(shù)更好的定位精度。選擇混合H2／H∞濾波作為融合器，一方面，系統(tǒng)擁有和KF 相近的精度，另一方面，系統(tǒng)也具備了一定魯棒性。融合定位系統(tǒng)總體流程如圖3所示。

圖3 融合定位過程

建立如下系統(tǒng)

式中 狀態(tài)向量Xk＝［PN，k，PE，k，θk］T，PN，k，PE，k，θk分別為北向坐標(biāo)、東向坐標(biāo)、方向角；Fk為狀態(tài)矩陣。由狀態(tài)方程

得到雅克比矩陣

式中 Sk為第k 步步長。量測向量Z ＝［PN，rssiPE，rssiθ］T，量測矩陣H ＝I3×3。wk，vk分別為狀態(tài)噪聲向量、量測噪聲向量，二者都是分布不明確的噪聲，分別表示為Q ＝

算法步驟如下：1）初始化：基于起點(diǎn)的指紋定位點(diǎn)和航向角初始化初始為單位陣I3×3。2）對于檢測到的步伐循環(huán)k ＝1，2，…，完成以下步驟。3）計(jì)算第k 次的狀態(tài)矩陣Fk。4）根據(jù)第k-1 次，，由式（19）計(jì)算加權(quán)系數(shù)d。5）由式（11）、式（14）分別更新第k次的H∞濾波增益，KF增益。6）結(jié)合步驟4、步驟5的結(jié)果，利用式（17）得到第k次的混合濾波增益Kk。7）將第k-1 次的狀態(tài)后驗(yàn)估計(jì)值代入式（21）得到第k次的狀態(tài)估計(jì)先驗(yàn)值，結(jié)合第k次的觀測值（指紋定位坐標(biāo)、航向角、步長）以及步驟3得到的Fk和步驟6得到的Kk，由＝＋Kk（Zk-）計(jì)算第k次的狀態(tài)后驗(yàn)估計(jì)值。中包含了融合后的定位點(diǎn)信息，用于定位輸出和校正下一步。8）根據(jù)式（13）、式（16）分別更新第k次H∞濾波和KF的估計(jì)誤差方差。

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

在鄭州大學(xué)北校區(qū)1號樓112教室及門前走廊進(jìn)行實(shí)驗(yàn)，112教室長11 m、寬12 m，走廊長11 m、寬4 m。以112教室西南角為零點(diǎn)，正東方向?yàn)閄軸，正北方向?yàn)閅軸，確定坐標(biāo)系（11 m×16 m）。

4.1 指紋和PDR定位結(jié)果分析

首先需要在實(shí)驗(yàn)場景部署藍(lán)牙信標(biāo)。當(dāng)接入點(diǎn)采用的AP數(shù)目超過4 個(gè)后，指紋定位誤差將趨于穩(wěn)定［15］。根據(jù)實(shí)驗(yàn)得知，目標(biāo)點(diǎn)距離信標(biāo)10 m 以上收到的信號強(qiáng)度低、波動大。因此，在部署信標(biāo)時(shí)，要保證每個(gè)接收點(diǎn)的10 m范圍內(nèi)能接收到不小于4個(gè)的有效信標(biāo)信號。根據(jù)上述原則，實(shí)驗(yàn)藍(lán)牙信標(biāo)部署情況如圖4所示。

圖4 指紋定位和PDR定位軌跡

基于安卓平臺開發(fā)了圖5 左的指紋收集工具，指紋采樣距離間隔設(shè)為1.6 m，每個(gè)采樣點(diǎn)收集15 s信號作平均以提高指紋準(zhǔn)確性，最后把收集到的指紋生成離線指紋庫。

圖5 指紋收集器（左）、傳感器數(shù)據(jù)采集器（右）

按照圖4的真實(shí)路徑步行，使用圖5 右的傳感器數(shù)據(jù)采集器，以20 Hz 的頻率采集數(shù)據(jù)?；诜逯堤綔y法對數(shù)據(jù)處理，計(jì)算步數(shù)為57步，取峰頂時(shí)刻RSSI 序列值中的最大4個(gè)有效信號作為特征，使用WKNN算法來進(jìn)行指紋匹配。圖4展示了實(shí)驗(yàn)的真實(shí)步行路徑、指紋定位、PDR定位結(jié)果，定位誤差、誤差累計(jì)分布函數(shù)見圖4，具體誤差數(shù)值如表1?？梢钥闯?，通過藍(lán)牙指紋匹配得出的定位點(diǎn)分布比較離散，定位誤差均方根（root mean square，RMS）為1.62 m，最大誤差為4.54 m。從PDR 路徑可以看出，其定位軌跡連續(xù)且和真實(shí)軌跡相似，行人直線步行時(shí)，航向誤差較為穩(wěn)定，定位結(jié)果較好，但隨著大幅度轉(zhuǎn)角（走廊進(jìn)入112教室）容易發(fā)生較大的航向偏差，隨著步長誤差和航向誤差的累計(jì)，PDR算法的定位結(jié)果的偏差將越來越大。

表1 各種方案的位置誤差數(shù)據(jù)比較 m

4.2 濾波融合定位結(jié)果分析

將藍(lán)牙指紋定位數(shù)據(jù)和PDR數(shù)據(jù)分別通過EKF、H∞、混合H2／H∞三種濾波做融合仿真分析，實(shí)驗(yàn)在2 種條件下進(jìn)行：情況1：根據(jù)統(tǒng)計(jì)經(jīng)驗(yàn)，對于噪聲矩陣Q，R中的參數(shù)σPE，σPN，σθ和σPE，rssi，σPN，rssi分別設(shè)值為0.1 m，0.1 m，0.042 rad，2 m和2 m；情況2：在情況1 基礎(chǔ)上，算法每次迭代都給σPE，σPN加一個(gè)0 ～0.5 m的隨機(jī)擾動用來驗(yàn)證濾波的抗干擾能力。圖6為3種濾波方案在情況1下的定位誤差圖和定位誤差的累計(jì)分布函數(shù)。圖7、表1 分別給出了2種情況具體的定位效果軌跡圖和誤差數(shù)值。

圖6 5 種定位方案的定位誤差圖與定位誤差CDF圖

圖7 3 種濾波預(yù)測定位軌跡（情況1 上、情況2 下）

首先分析情況1下的實(shí)驗(yàn)結(jié)果，可以看出，EKF位置誤 差RMS為0.86 m，H∞濾波位置誤差RMS為0.94 m，略低于EKF，混合H2／H∞濾波位置誤差RMS為0.87 m，性能與EKF相近，從3 種濾波的綜合定位效果來看EKF 和混合H2／H∞濾波稍優(yōu)于H∞濾波，但差距并不明顯。與單種定位技術(shù)相比，3 種濾波融合方案的定位性能都要比藍(lán)牙信標(biāo)指紋定位（位置誤差RMS 為1.62 m）以及PDR（位置誤差RMS為2.24 m）單獨(dú)的定位效果好的多，驗(yàn)證了融合定位技術(shù)的有效性和可行性。

3種濾波在情況2 下的抗干擾能力表現(xiàn)可見，在每步都有隨機(jī)擾動的情況2 中，3 種濾波的穩(wěn)定性迭代都有較大跳躍性。H∞濾波明顯要穩(wěn)定很多，混合H2／H∞濾波的穩(wěn)定性介于二者之間。從定位誤差數(shù)據(jù)來看，H∞表現(xiàn)最優(yōu)，EKF效果最差，混合H2／H∞濾波仍介于二者之間。情況2的實(shí)驗(yàn)驗(yàn)證了H∞濾波擁有良好的抗干擾能力，混合H2／H∞濾波的魯棒性雖然不如H∞，但比EKF好一些。

5 結(jié) 論

本文提出了一種基于混合H2／H∞濾波融合PDR和藍(lán)牙信標(biāo)指紋定位的室內(nèi)定位方法，同時(shí)改進(jìn)了指紋定位的計(jì)算效率。通過藍(lán)牙信標(biāo)指紋定位結(jié)果來修正PDR 的累計(jì)誤差，融合定位效果明顯優(yōu)于二者獨(dú)立的效果。通過對比實(shí)驗(yàn)表明：混合H2／H∞濾波的擁有EKF近似的性能，同時(shí)具備了一定魯棒性，可以在復(fù)雜環(huán)境下提供連續(xù)可靠穩(wěn)定的定位結(jié)果。