基于級聯(lián)濾波的行人慣性室內(nèi)定位方法

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

（西京學(xué)院 信息工程學(xué)院，西安 710123）

0 引言

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

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

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

1 擴(kuò)展卡爾曼-非遞歸貝葉斯級聯(lián)濾波算法

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

圖1 級聯(lián)結(jié)構(gòu)的行人慣性定位導(dǎo)航系統(tǒng)

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

2 擴(kuò)展卡爾曼濾波的零速修正算法

在下級濾波器中，慣性導(dǎo)航系統(tǒng)解算算法的連續(xù)模型如下：

1）采用卡爾曼濾波方法估計偏差項(xiàng)對原始加速度和陀螺傳感器數(shù)據(jù)進(jìn)行偏置補(bǔ)償，即

2）利用偏置補(bǔ)償陀螺儀數(shù)據(jù)進(jìn)行方向更新，即

4）將重力加速度移除后的加速度值進(jìn)行積分，獲得導(dǎo)航坐標(biāo)系下的速度，再對速度積分獲得導(dǎo)航坐標(biāo)系的位置為

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

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

最優(yōu)零速檢測器的模型為

當(dāng)測量序列的值小于閾值時，行人處于零速狀態(tài)，反之則為運(yùn)動狀態(tài)。當(dāng)行人處于靜止?fàn)顟B(tài)時，依據(jù)以下 6個步驟使用擴(kuò)展卡爾曼濾波算法對行人的慣性導(dǎo)航解算結(jié)果進(jìn)行修正：

1）時間處的15個誤差狀態(tài)向量為

2）線性化狀態(tài)轉(zhuǎn)換模型為

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

的計算方法為

其計算方法為

3）測量模型為

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

式中：K為卡爾曼濾波增益；m為實(shí)際的測量誤差；δx為預(yù)測的錯誤狀態(tài)。

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

測量矩陣的為

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

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

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

6）使用無偏誤差項(xiàng)濾波后的狀態(tài)向量δx來優(yōu)化當(dāng)前的速度和位置，即

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

通過下級擴(kuò)展卡爾曼濾波修正后的導(dǎo)航結(jié)果計算步長和航向，結(jié)合其步長和航向信息，實(shí)現(xiàn)了PDR作為本文上級非遞歸貝葉斯濾波器使用的狀態(tài)模型。

當(dāng)行人靜止時，每兩個坐標(biāo)之間的距離被定義為步長，即

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

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

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

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

圖2 靜態(tài)的平面地圖概率分布函數(shù)

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

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

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

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

表1 NGIMU 傳感器參數(shù)

圖3 安裝在足部的NGIMU

在實(shí)驗(yàn)樓內(nèi)開展行人室內(nèi)導(dǎo)航實(shí)驗(yàn)（樓層平面圖如圖4所示），走廊呈回字形，南側(cè)走廊長約80.8 m，南北向走廊長約27.17 m，走廊寬度約2 m。實(shí)驗(yàn)從地圖的西南角出發(fā)行走一圈回到起點(diǎn)，行走規(guī)劃的軌跡如圖4所示，行走一圈的距離大約為215.94 m。

圖4 室內(nèi)平面圖

使用采集的數(shù)據(jù)開展擴(kuò)展卡爾曼濾波零速更新慣性行人導(dǎo)航算法以及基于級聯(lián)結(jié)構(gòu)的擴(kuò)展卡爾曼濾波、非遞歸貝葉斯濾波導(dǎo)航算法實(shí)驗(yàn)。215.94 m測試實(shí)驗(yàn)零速修正校正方案下的二維平面位置估計如圖5所示。

圖5 215.94 m測試實(shí)驗(yàn)二維平面的位置估計

圖6至圖9分別為 52.17、215.94、431.88、647.82 m測試實(shí)驗(yàn)的級聯(lián)慣性導(dǎo)航算法與零速修正慣性導(dǎo)航算法導(dǎo)航估計軌跡對比。因?yàn)椴杉瘮?shù)據(jù)時，行人行走的狀態(tài)以及傳感器固有的誤差，相同的運(yùn)動軌跡導(dǎo)航解算出的軌跡會略微有所不同，但這并不影響慣性導(dǎo)航系統(tǒng)的整體性能。

圖6 52.17 m測試實(shí)驗(yàn)二維平面的位置估計

圖7 215.94 m測試實(shí)驗(yàn)導(dǎo)航結(jié)果對比

圖8 431.88 m測試實(shí)驗(yàn)導(dǎo)航結(jié)果對比

圖9 647.82 m測試實(shí)驗(yàn)導(dǎo)航結(jié)果對比

為驗(yàn)證本系統(tǒng)的有效性和穩(wěn)定性，本文進(jìn)行了多組不同距離室內(nèi)實(shí)驗(yàn)，測試人員的行走速度在0.93~1.2 m/s間，屬于慢速行走。本文算法與零速更新算法和零速修正算法在不同實(shí)驗(yàn)條件下解算的距離如表2所示，并以解算距離與實(shí)際距離之間差值作為導(dǎo)航算法的評價標(biāo)準(zhǔn)，實(shí)驗(yàn)結(jié)果如圖10所示。

表2 不同算法解算距離結(jié)果 單位：m

圖10 實(shí)驗(yàn)導(dǎo)航距離誤差對比

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

4 結(jié)束語

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