基于級聯濾波的行人慣性室內定位方法

張雨帥，郭建新，朱 銳，王祖良，姬 翔 ，王利平

（西京學院 信息工程學院，西安 710123）

0 引言

目前，室內定位導航已經成為各種基于位置的服務和應用的共同研究熱點。針對室內環境中的行人導航問題，國內外學者提出了許多解決方案，如基于慣性導航系統（inertial navigation system,INS）、無線保真（wireless fidelity, WiFi）、藍牙（bluetooth,BT）、超寬帶（ultra-wide band,UWB）等技術的導航系統。其中，由于WiFi在全球的廣泛部署和使用，研究人員對WiFi指紋定位技術做了大量的研究。WiFi指紋定位技術使用WiFi信號強度進行建模和測量。但是WiFi指紋定位技術也存在著許多問題，WiFi指紋定位技術需要多種算法作為支撐，計算量大和算法復雜度相對較高。此外，由于該方法需要大量的先驗信息作為數據支撐，前期工作具有較高的成本。同樣，基于藍牙、超寬帶的導航方法也可以提供高效的室內導航服務。然而，這些方法需要前期建立非常可靠的基礎設施以確保導航系統的性能，這無疑會增加前期工作的成本。

慣性導航系統是一種自主導航技術，它不需要提前在環境中預安裝任何設備，可以有效地避免上述技術難以克服的問題。并且隨著微機電系統（microelectromechanical systems, MEMS）的發展，慣性傳感器作為一種更廉價、更實用的定位解決方案引起了人們的廣泛關注。考慮到人體行走步態的獨特性，將MEMS傳感器安裝在用戶的腳上，可以有效地限制慣性導航的導航誤差。在足部安裝模式下，當周期性地檢測到腳的相位，則會觸發零速更新（zero velocity update, ZUPT），使用卡爾曼濾波（Kalman filter，KF）算法估計和校正慣性導航的解算誤差及慣性傳感器的固有偏差。文獻[12]在零速更新的基礎上，提出了啟發式航向修正和零角速度更新，可以在零速更新階段修正慣性導航的航向誤差。盡管基于卡爾曼濾波零速修正的慣性導航系統有時可以產生相對較好的導航結果，但存在較大的航向誤差，而且MEMS慣性傳感器也存在相對較大的不確定性和隨機性偏差。慣性傳感器的導航的偏差還會隨著時間的推移而不斷增長。為了解決行人慣性導航系統的缺點，研究人員將其與額外的傳感器或輔助信息融合，從而提高導航定位精度。多傳感器融合定位方法都能有效地為用戶提供高精度的室內定位導航。然而，這些方法的基礎設施成本很高，需要大量的勞動力來建立一個完整的室內定位導航系統。文獻[15]提出一種基于粒子濾波地圖與慣性導航融合的室內定位系統，通過依靠地圖信息來修正導航誤差。在沒有額外的輔助定位儀器和設施情況下，室內平面地圖信息是一種經濟簡潔補償定位誤差的方法。

鑒于以上研究內容，本文提出一種級聯結構的室內定位導航系統，將基于擴展卡爾曼濾波的啟發式航向修正，零速更新，零角速度更新與室內平面地圖信息相結合，利用行人運動特征及室內建筑結構信息，將擴展卡爾曼濾波和非遞歸貝葉斯濾波級聯在一起，通過在室內行人導航系統中使用級聯結構算法，從而減小位置更新過程中的誤差，在一定程度上提高導航計算精度。

1 擴展卡爾曼-非遞歸貝葉斯級聯濾波算法

本研究使用慣性傳感器和室內地圖信息，充分利用擴展卡爾曼濾波的優點，同時使用啟發式航向修正、零速更新及零角速度更新三種算法并提出了一個由雙層架構組成的級聯結構的擴展卡爾曼濾波和非遞歸貝葉斯濾波算法，其結構如圖1所示。圖1中：PDR（pedestrian dead reckoning）為行人航跡推算算法。

圖1 級聯結構的行人慣性定位導航系統

下級濾波器使用基于擴展卡爾曼濾波的啟發式航向修正、零速更新、零角速度更新作為初始慣性導航解算誤差修正方案；因此，可以有更好的慣性導航解算結果提供給上級濾波器，同時下級濾波器也可作為單獨的零速修正慣性導航系統。上級濾波器應用行人航位推算方法來計算行人的位置。利用下級濾波器導航結果計算出步長和航向的變化，用以更新上級濾波器的非線性狀態。將先驗地圖信息作為一個獨立的度量來修正低級濾波器的導航結果，兩級濾波器之間的關系如圖1所示。在下級擴展卡爾曼濾波中，選擇行人的姿態信息、角速度、位置、速度以及加速度信息作為狀態向量；對于非遞歸貝葉斯濾波器，狀態向量中只有二維位置信息，從而減小計算負擔。

2 擴展卡爾曼濾波的零速修正算法

在下級濾波器中，慣性導航系統解算算法的連續模型如下：

1）采用卡爾曼濾波方法估計偏差項對原始加速度和陀螺傳感器數據進行偏置補償，即

2）利用偏置補償陀螺儀數據進行方向更新，即

4）將重力加速度移除后的加速度值進行積分，獲得導航坐標系下的速度，再對速度積分獲得導航坐標系的位置為

式中：=[,,]為時間時擴展卡爾曼濾波校正之前的三軸速度；=[,,r]為時間時擴展卡爾曼濾波校正之前的三軸位移。

行人慣性導航系統輸入數據是以設備幀表示時間序列的加速度和角速度。利用加速度的二重積分計算行人二維地圖上的位移。然而，由于傳感器干擾引起的誤差累積，位移的偏差隨時間增加而變大。使用基于擴展卡爾曼濾波的零速修正算法，可以修正導航的累計誤差，而在零速修正算法中，關鍵的步驟是對零速狀態進行檢測。文獻[18]提出了一種最優零速檢測器，該算法的性能優于基于加速度幅值零速檢測算法和加速度方差零速檢測算法，因此采用最優零速檢測器作為下級濾波器的零速檢測算法。

最優零速檢測器的模型為

當測量序列的值小于閾值時，行人處于零速狀態，反之則為運動狀態。當行人處于靜止狀態時，依據以下 6個步驟使用擴展卡爾曼濾波算法對行人的慣性導航解算結果進行修正：

1）時間處的15個誤差狀態向量為

2）線性化狀態轉換模型為

式中：為單位矩陣；為零矩陣；為斜對稱矩陣。

的計算方法為

其計算方法為

3）測量模型為

4）時刻的誤差狀態是在獲取時刻的測量結果后得到的。在獲取時刻的測量結果后，卡爾曼濾波更新方程為

式中：K為卡爾曼濾波增益；m為實際的測量誤差；δx為預測的錯誤狀態。

在本文的擴展卡爾曼濾波算法中對啟發式航向修正（HDR）、零角速率更新（ZARU）、零速更新(ZUPT)三種算法進行了融合應用。實際誤差測量m為擴展卡爾曼濾波提供信息，實際誤差測量m計算為

測量矩陣的為

式中，=[0 0 1]為航向誤差的觀測矩陣。

5）卡爾曼濾波增益用通常的公式計算為

式中：P為估計誤差協方差矩陣，該矩陣是在時刻的基礎上，根據時刻?1收到的測量結果計算得到的。在時刻使用約瑟夫公式中的卡爾曼增益計算出協方差矩陣P，即

6）使用無偏誤差項濾波后的狀態向量δx來優化當前的速度和位置，即

3 非遞歸貝葉斯的地圖匹配算法

通過下級擴展卡爾曼濾波修正后的導航結果計算步長和航向，結合其步長和航向信息，實現了PDR作為本文上級非遞歸貝葉斯濾波器使用的狀態模型。

當行人靜止時，每兩個坐標之間的距離被定義為步長，即

式中：x、y為根據下級擴展卡爾曼濾波估計的位置。每一步的航向變化是指當行人靜止時，擴展卡爾曼濾波提供的每兩個航向之間的差值，即

因此，非遞歸貝葉斯濾波的測量模型可以表示為

式中：v、v為獨立于步長和航向變化的噪聲。

與基于粒子濾波的地圖匹配導航研究不同，非遞歸貝葉斯濾波地圖匹配算法利用室內平面圖建立一個保持靜態的柵格化概率分布函數(|)，為地圖先驗概率。如圖2所示，密度水平與行人在相應位置的行走可能性成正比，當墻邊界開始時，藍色部分的分數降至零，于是將行人行走路徑約束至室內走廊中，因此非遞歸貝葉斯濾波不需要大量的蒙特卡羅模擬，也不需要對每個粒子進行明確的穿墻檢查。

圖2 靜態的平面地圖概率分布函數

由于步長和航向都會間接影響行人的位置似然，通過靜態平面圖概率密度函數和擴展運動模型來解決似然項的不確定性。擴展運動模型(|,… ,)預測行人在直線段運動時在直線上的位置，以及在靠近走廊拐角時在曲線上的位置。傳感器似然和先驗概率密度函數的均值位于一個行人環繞概率密度函數(|,)，該概率密度函數由步長表示位置密度。先驗、似然、基于步長的似然和靜態平面圖組合形成后驗概率密度函數。因此，非遞歸貝葉斯濾波器的最終濾波方程為

式中：(|)為傳感器似然概率密度函數；(|)為先驗概率密度函數。

4 實驗結果與分析

為對基于級聯濾波的行人慣性室內定位方法的性能進行評估，在實際的室內進行了實驗測試，使用克斯-艾奧科技有限公司（x-io Technologies Limited）的NGIMU作為慣性導航傳感器設備，其參數如表1所示。本系統使用了帶有外殼的NGIMU，尺寸為56 mm×39 mm×18 mm，重量為46 g。如圖3所示，用一條拉伸帶將NGIMU固定在腳上。

表1 NGIMU 傳感器參數

圖3 安裝在足部的NGIMU

在實驗樓內開展行人室內導航實驗（樓層平面圖如圖4所示），走廊呈回字形，南側走廊長約80.8 m，南北向走廊長約27.17 m，走廊寬度約2 m。實驗從地圖的西南角出發行走一圈回到起點，行走規劃的軌跡如圖4所示，行走一圈的距離大約為215.94 m。

圖4 室內平面圖

使用采集的數據開展擴展卡爾曼濾波零速更新慣性行人導航算法以及基于級聯結構的擴展卡爾曼濾波、非遞歸貝葉斯濾波導航算法實驗。215.94 m測試實驗零速修正校正方案下的二維平面位置估計如圖5所示。

圖5 215.94 m測試實驗二維平面的位置估計

圖6至圖9分別為 52.17、215.94、431.88、647.82 m測試實驗的級聯慣性導航算法與零速修正慣性導航算法導航估計軌跡對比。因為采集數據時，行人行走的狀態以及傳感器固有的誤差，相同的運動軌跡導航解算出的軌跡會略微有所不同，但這并不影響慣性導航系統的整體性能。

圖6 52.17 m測試實驗二維平面的位置估計

圖7 215.94 m測試實驗導航結果對比

圖8 431.88 m測試實驗導航結果對比

圖9 647.82 m測試實驗導航結果對比

為驗證本系統的有效性和穩定性，本文進行了多組不同距離室內實驗，測試人員的行走速度在0.93~1.2 m/s間，屬于慢速行走。本文算法與零速更新算法和零速修正算法在不同實驗條件下解算的距離如表2所示，并以解算距離與實際距離之間差值作為導航算法的評價標準，實驗結果如圖10所示。

表2 不同算法解算距離結果 單位：m

圖10 實驗導航距離誤差對比

從圖6至圖9可以看出，級聯結構算法估計軌跡接近實際測試軌跡，與零速修正算法對比可以進一步消除慣性導航的航向漂移。圖10為零速更新算法、零速修正算法、級聯結構算法三種估計行走的距離誤差，僅使用零速更新算法的慣性導航系統不同距離實驗中平均誤差為 30.35 m,基于零速修正的導航系統不同距離實驗的平均距離誤差為12.40 m，而級聯結構的慣性導航系統不同距離實驗的平均距離誤差為 0.45 m。隨著距離的增加，零速修正算法的距離誤差最大，其次是零速修正算法，這是因為隨著運動距離和時間的增加，慣性導航的誤差累計會越來越難以克服，而本文提出的級聯結構慣性導航算法可以有效地將距離誤差控制在1 m以內。綜上所述，本文提出的系統可以很好地校正航向漂移的問題，使行人行走的軌跡更加接近實際規劃軌跡，并且本系統可以進一步糾正距離誤差。

4 結束語

本文提出了一種完全非基礎設施的、低成本的室內導航系統，它比現有的方法更便宜、更快。該算法只使用室內地圖信息和NGIMU傳感器。由于該方法僅使用室內地圖信息和慣性導航傳感器，因此不需要預先測量、預先安裝或額外的傳感器。該系統采用兩層結構算法，融合室內地圖信息和MEMS傳感器，來實現室內導航。實驗結果表明：所提出的方法能夠達到所需的導航計算精度；地圖匹配的非遞歸貝葉斯濾波方法可以大幅度提高零速修正慣性導航的精度。