室內(nèi)環(huán)境約束的行人航向粒子濾波修正方法

劉 盼，張 榜，黃 超，楊衛(wèi)軍，徐正蓺

(1.中國(guó)科學(xué)院 上海高等研究院，上海 201210； 2.中國(guó)科學(xué)院大學(xué)，北京 100049； 3.上?？萍即髮W(xué) 信息科學(xué)與技術(shù)學(xué)院，上海 201210)(*通信作者電子郵箱xuzy@sari.ac.cn)

0 引言

隨著互聯(lián)網(wǎng)、物聯(lián)網(wǎng)、智能移動(dòng)設(shè)備等相關(guān)技術(shù)的發(fā)展和應(yīng)用，基于位置的服務(wù)(Location Based Service, LBS)以其強(qiáng)大的實(shí)用性和廣闊的發(fā)展空間逐漸成為了學(xué)術(shù)領(lǐng)域和工程領(lǐng)域共同關(guān)注的熱點(diǎn)。

在室內(nèi)環(huán)境中，LBS技術(shù)已經(jīng)開始在很多場(chǎng)景中發(fā)揮著重要的作用，比如機(jī)場(chǎng)或車站等大型建筑中的實(shí)時(shí)定位與導(dǎo)航、大型商場(chǎng)中基于位置的商品廣告推送、展覽館中參觀路線的引導(dǎo)以及基于位置點(diǎn)的講解等。這些服務(wù)和功能都是對(duì)如今現(xiàn)代化建筑規(guī)模不斷擴(kuò)大、功能不斷全面化的適應(yīng)性產(chǎn)物，也是提升人們生活質(zhì)量和改善生活體驗(yàn)的剛性需求，而它們得以實(shí)現(xiàn)的基礎(chǔ)之一就是室內(nèi)定位技術(shù)[1]。面對(duì)日益增長(zhǎng)的室內(nèi)定位需求，相關(guān)領(lǐng)域的科研人員正在不斷地嘗試?yán)眯碌姆椒ɑ蛘吒倪M(jìn)現(xiàn)有方法來提高室內(nèi)定位的精度。

步行者航位推算(Pedestrian Dead Reckoning, PDR)[2-4]是一種基于慣性傳感器元件的室內(nèi)行人定位算法，該算法通過慣性傳感器采集到的行人運(yùn)動(dòng)數(shù)據(jù)(主要包括三軸加速度計(jì)、三軸陀螺儀數(shù)據(jù)等)、行人運(yùn)動(dòng)時(shí)的環(huán)境數(shù)據(jù)(主要包括三軸磁力計(jì)、激光測(cè)距數(shù)據(jù)等)來計(jì)算行人步伐長(zhǎng)度和行進(jìn)航向，并結(jié)合起始位置的設(shè)定來得到行人當(dāng)前的位置坐標(biāo)，最終可以通過搭建的智能顯示平臺(tái)等呈現(xiàn)出室內(nèi)行人的運(yùn)動(dòng)軌跡。在步行者航位推算算法中，步伐檢測(cè)、步長(zhǎng)估計(jì)、航向估計(jì)是三個(gè)關(guān)鍵問題，通過對(duì)這三個(gè)方面的優(yōu)化，可以有效地提高室內(nèi)定位的精度。在步伐檢測(cè)[5]方面，一般是通過某個(gè)時(shí)間段內(nèi)由加速度計(jì)得到的加速度或者由陀螺儀得到的角速度變化為零來認(rèn)定當(dāng)前腳部處于靜止?fàn)顟B(tài)，這一方法中使用到的慣性數(shù)據(jù)采集元件是佩戴在腳部的；對(duì)于步長(zhǎng)估計(jì)[6]，一般是通過經(jīng)驗(yàn)公式、步長(zhǎng)模型等方法來進(jìn)行，有時(shí)候還會(huì)結(jié)合卡爾曼濾波等算法來消除部分噪聲；而為了減小航向誤差，零速度更新(Zero Velocity Update, ZUPT)[7]或者零角速度更新(Zero Angle Rate Update, ZARU)的方法常被使用。由于應(yīng)用慣性系統(tǒng)的定位方式往往缺乏真實(shí)可信的航向觀測(cè)指標(biāo)，以上方法雖然有一定的效果，但是累積誤差仍然是一個(gè)十分明顯的問題。

為了減小慣性定位的累積誤差，研究人員提出了借助額外的信號(hào)發(fā)生設(shè)備來進(jìn)行定位校準(zhǔn)的方法。這里采用的額外設(shè)備信號(hào)經(jīng)常被作為建立指紋庫(kù)的根據(jù)之一，或者直接通過信號(hào)強(qiáng)度得到距離進(jìn)而融合到其他算法中。英國(guó)的Ubisense系統(tǒng)[8]采用的超寬帶脈沖技術(shù)，其定位精度達(dá)到了6～10 cm。劉小康等[9]利用ZigBee 無線傳感器網(wǎng)絡(luò)的信號(hào)強(qiáng)弱建立了指紋數(shù)據(jù)庫(kù)，將定位平均誤差限制在了1.5 m以內(nèi)。還有利用無線網(wǎng)絡(luò)Wi-Fi技術(shù)[10]結(jié)合區(qū)域聚類算法來實(shí)現(xiàn)更加精確的室內(nèi)定位的。此外，諸如藍(lán)牙[11]、射頻識(shí)別等需要額外架設(shè)信號(hào)設(shè)備的室內(nèi)定位技術(shù)手段都得到了一定程度的研究和利用。不過這種憑借額外設(shè)備輔助的定位方法，無論是在設(shè)備自身成本還是在設(shè)備鋪設(shè)維護(hù)等方面的成本都帶來了更重的負(fù)擔(dān)，同時(shí)，不確定的室內(nèi)環(huán)境信號(hào)干擾往往對(duì)這些方法的應(yīng)用和準(zhǔn)確度的提高造成較大困難。

此外，研究人員嘗試了基于地磁場(chǎng)匹配的航向校準(zhǔn)方法來提高定位精度[12]，這種方法根據(jù)室內(nèi)地磁場(chǎng)的分布情況建立數(shù)據(jù)庫(kù)，能夠在定位過程中于一定程度上校準(zhǔn)航向，同時(shí)容易得到行人行走的真北方向，不過仍舊難以避免在復(fù)雜室內(nèi)環(huán)境中信號(hào)干擾的問題。近年，利用粒子濾波融合支持向量機(jī)和卡爾曼濾波的方式[13]也開始被嘗試，這種方式通過對(duì)傳統(tǒng)卡爾曼濾波PDR算法的改進(jìn)和對(duì)數(shù)據(jù)進(jìn)行預(yù)處理，在一定程度上提高了室內(nèi)定位精度；但仍然明顯地存在著行人估計(jì)位置穿墻等不理想的情況。還有研究人員利用地圖匹配結(jié)合區(qū)域生長(zhǎng)算法[14]，根據(jù)輸入的地圖圖片信息，對(duì)每一步的落點(diǎn)位置進(jìn)行修正，這種方法也在一定程度上提高了定位精度，并且有效地避免了穿墻情景，最終將整體誤差控制在1%左右；但是該算法只將地圖信息應(yīng)用在了去除穿墻粒子的層面，并且最終誤差仍舊偏大。

綜上所述，本文針對(duì)傳統(tǒng)慣性定位算法中航向累積誤差的問題，提出了一種室內(nèi)環(huán)境約束的粒子濾波算法來進(jìn)行行人航向修正。在算法整體設(shè)計(jì)上，首先利用PDR框架下的傳統(tǒng)卡爾曼濾波方法得到室內(nèi)行人步長(zhǎng)和原始航向，并將后者作為粒子濾波航向修正的濾波對(duì)象，同時(shí)融合室內(nèi)環(huán)境約束以完成航向校準(zhǔn)。區(qū)別于以往地圖匹配中單純剔除穿墻粒子或僅在拐彎處建立信標(biāo)的方法，改進(jìn)的算法通過粒子動(dòng)態(tài)選擇地圖數(shù)據(jù)得到的航向與墻體夾角來構(gòu)建粒子自修正機(jī)制，通過粒子動(dòng)態(tài)選擇地圖數(shù)據(jù)得到的粒子與墻體距離構(gòu)建粒子賦權(quán)機(jī)制，以更加充分地利用室內(nèi)環(huán)境信息。此外，該算法將航向作為唯一狀態(tài)量，相比于傳統(tǒng)粒子濾波過程中多狀態(tài)量同時(shí)求解的特點(diǎn)，單一狀態(tài)更便于數(shù)據(jù)分析和處理，降低了計(jì)算復(fù)雜度。

實(shí)驗(yàn)部分通過佩戴多慣性測(cè)量單元(Multiple Inertial Measurement Unit, MIMU)[15]傳感器在辦公樓內(nèi)分多組多人次進(jìn)行了不同行走距離的實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明,所提算法可以減小航向誤差，提高定位精度，并且在行人行走距離增加時(shí)具有更好的穩(wěn)定性。

1 基于卡爾曼濾波的PDR室內(nèi)定位

航位推算算法是主流的室內(nèi)定位方法之一，但由于傳感器誤差的累積，往往會(huì)形成較大的航向和定位累積誤差。通過引入卡爾曼濾波算法可以在一定程度上使此種誤差減小，以提高航向和定位的精度。而由于缺乏真實(shí)可信的行人方向觀測(cè)數(shù)據(jù)，基于卡爾曼濾波的PDR室內(nèi)定位算法得到的航向仍需校準(zhǔn)。下面將簡(jiǎn)要給出PDR原理和卡爾曼濾波流程，并將其解算結(jié)果作為粒子濾波對(duì)象，以針對(duì)航向進(jìn)一步修正。

1.1 航位推算算法

航位推算算法一般是利用慣性傳感器采集三軸加速度數(shù)據(jù)、三軸角速度數(shù)據(jù)，對(duì)其進(jìn)行一次積分、二次積分、零速度檢測(cè)、步伐判定等一系列的處理后得到步長(zhǎng)估計(jì)和航向估計(jì)，再結(jié)合設(shè)定的初始位置坐標(biāo)點(diǎn)，逐步或者逐點(diǎn)繪制出室內(nèi)行人的運(yùn)動(dòng)軌跡。文中涉及到的濾波算法均是在航位推算框架下完成的。

在二維坐標(biāo)系中航位推算算法如圖1所示。其中：p0、pN代表室內(nèi)行人起始位置以及終點(diǎn)位置的坐標(biāo)點(diǎn)，pk、pk+1代表行走過程中在k落點(diǎn)時(shí)刻和k+1落點(diǎn)時(shí)刻的坐標(biāo)，所謂落點(diǎn)時(shí)刻即是當(dāng)步伐檢測(cè)算法判定行人腳部貼合地面的穩(wěn)定時(shí)刻。另外，lk、θk代表k落點(diǎn)時(shí)刻行人的步長(zhǎng)估計(jì)和航向估計(jì)。

圖1 航位推算算法示意圖Fig. 1 Schematic diagram of dead reckoning algorithm

在得到k落點(diǎn)時(shí)刻的步長(zhǎng)估計(jì)和航向估計(jì)之后，結(jié)合k落點(diǎn)時(shí)刻的位置坐標(biāo)，就可以進(jìn)一步地通過式(1)得到k+1落點(diǎn)時(shí)刻的位置坐標(biāo)：

(1)

其中：(Xk,Yk)和(Xk+1,Yk+1)分別表示k落點(diǎn)時(shí)刻和k+1落點(diǎn)時(shí)刻行人在二維坐標(biāo)系中的位置坐標(biāo)；θk+1和Δθ分別表示k+1落點(diǎn)時(shí)刻航向以及k落點(diǎn)時(shí)刻所產(chǎn)生的航向變化量。

值得注意的是，由于PDR算法本身對(duì)于步長(zhǎng)和航向的估計(jì)是通過單純積分的方式得到的，這并未對(duì)其中慣性元件產(chǎn)生的噪聲加以抑制，因此會(huì)存在較大的累積誤差。為了減小累積誤差，就需要結(jié)合適當(dāng)?shù)臑V波算法。

1.2 基于卡爾曼濾波的航位推算

卡爾曼濾波算法是在室內(nèi)定位研究中主流的噪聲處理方法之一，該算法通過構(gòu)建狀態(tài)模型并結(jié)合觀測(cè)數(shù)據(jù)對(duì)每一時(shí)刻的狀態(tài)量進(jìn)行噪聲濾除，能夠在一定程度上減小PDR算法中存在的累積誤差。本文提出的算法中用到的步長(zhǎng)以及原始航向就是通過傳統(tǒng)的卡爾曼濾波算法結(jié)合PDR得到的。

狀態(tài)模型為：

Tk|k-1=ATk-1|k-1+uk

(2)

其中Tk=[θk,ωk,vk,pk]為狀態(tài)量，θk、ωk、vk、pk分別為航向、姿態(tài)角、速度和位置；A為狀態(tài)轉(zhuǎn)移矩陣；uk為系統(tǒng)噪聲。

測(cè)量方程為：

Gk=HTk|k-1+rk

(3)

其中：Gk=[vk,ωk]為觀測(cè)向量；H為測(cè)量矩陣；rk為測(cè)量噪聲。在檢測(cè)到腳部靜止[16]時(shí)，進(jìn)行濾波更新，具體的卡爾曼算法實(shí)現(xiàn)流程如圖2所示。 通過前后兩次零速度時(shí)刻的位置坐標(biāo)p1、p2可以計(jì)算當(dāng)前步長(zhǎng)為lk=‖pk-pk-1‖，此步長(zhǎng)將作為后續(xù)粒子濾波算法的輸入。

圖2 卡爾曼濾波算法示意圖Fig. 2 Schematic diagram of Kalman filtering algorithm

通過實(shí)驗(yàn)驗(yàn)證，基于卡爾曼濾波算法得到的行人步長(zhǎng)估計(jì)比較準(zhǔn)確。同時(shí)從圖2中可以直觀地看出，該算法缺乏真實(shí)可信的航向觀測(cè)數(shù)據(jù)，這導(dǎo)致該算法航向估計(jì)仍存在較大的累積誤差。針對(duì)這個(gè)問題，本文提出室內(nèi)環(huán)境約束的粒子濾波算法，將航向作為狀態(tài)量進(jìn)一步濾波修正，以達(dá)到更為理想的定位效果。

2 室內(nèi)環(huán)境約束的粒子濾波航向修正方法

在第1章得到原始航向和步長(zhǎng)數(shù)據(jù)的基礎(chǔ)上，本章應(yīng)用室內(nèi)環(huán)境約束的粒子濾波算法對(duì)行人航向進(jìn)行校準(zhǔn)，其中仍然將單步作為航位推算的基本單元。在粒子濾波過程中，利用地圖信息融合進(jìn)行了兩點(diǎn)改進(jìn)：一是在粒子初始化和粒子補(bǔ)償階段，利用原始航向與自主匹配的參照墻體進(jìn)行粒子補(bǔ)償判斷并進(jìn)行補(bǔ)償，以此構(gòu)建粒子自修正機(jī)制；二是在粒子權(quán)重分配階段，依照粒子形成的步伐落點(diǎn)與自主選擇的墻體距離進(jìn)行權(quán)重計(jì)算，以此構(gòu)建依距離賦權(quán)機(jī)制。最終使位置估計(jì)和航向估計(jì)更加貼合實(shí)際情況。

2.1 粒子初始化

傳統(tǒng)的粒子濾波算法應(yīng)用于室內(nèi)定位時(shí)，在粒子初始化階段往往需要構(gòu)建多維度狀態(tài)量，除航向以外，還包括姿態(tài)、二維位置、加速度等，而本算法在粒子初始化階段僅對(duì)卡爾曼濾波得到的原始航向進(jìn)行粒子采樣，采樣后的粒子由式(4)表示：

F=(f1,f2，…，fN)

(4)

其中：F表示粒子集合;N表示本次采樣產(chǎn)生的粒子總數(shù)。

對(duì)于完成采樣的集合F，其航向粒子服從式(5)表示的正態(tài)分布：

(5)

需要注意的是，標(biāo)準(zhǔn)差σ會(huì)根據(jù)粒子位置情況進(jìn)行動(dòng)態(tài)調(diào)整，即當(dāng)全部粒子位置表現(xiàn)為穿墻時(shí)，適當(dāng)增大σ利于提供更多符合修正條件的粒子。該操作也可以視為一種簡(jiǎn)化的重采樣過程。

根據(jù)算法的采樣方式，粒子分為兩種情況，即大于或者小于原始航向。然而，當(dāng)粒子過大、過小或粒子分布過于均衡時(shí)，可能會(huì)出現(xiàn)過擬合或者欠擬合的情況，即有過多粒子出現(xiàn)過度補(bǔ)償或不被補(bǔ)償?shù)默F(xiàn)象。因此，根據(jù)實(shí)驗(yàn)情況，在采樣階段確定一個(gè)恰當(dāng)?shù)臉?biāo)準(zhǔn)差是必要的。

圖3 航向采樣示意圖Fig. 3 Schematic diagram of sampling heading

2.2 粒子航向修正

對(duì)航向完成合理的采樣后就要進(jìn)行粒子修正，在該階段，首先將地圖信息抽象成用線段表示的結(jié)構(gòu)體，具體做法是根據(jù)室內(nèi)環(huán)境以及行人室內(nèi)行走特點(diǎn)，對(duì)通路尺寸和相對(duì)位置進(jìn)行實(shí)地測(cè)量和標(biāo)注，以兩點(diǎn)一線方式結(jié)合角度在平面內(nèi)繪制出墻體輪廓，并以二維坐標(biāo)數(shù)據(jù)的形式加以保存以便于后續(xù)計(jì)算。

結(jié)構(gòu)體如式(6)和式(7)所示：

map=(s1,s2,s3,s4...sn)

(6)

si=(p1i,p2i,λi)

(7)

其中：n表示墻體數(shù)量；si表示第i個(gè)墻體，且由起點(diǎn)p1i、終點(diǎn)p2i、墻體方向λi構(gòu)成。

基于已有的室內(nèi)環(huán)境結(jié)構(gòu)體，提出通過動(dòng)態(tài)地關(guān)聯(lián)室內(nèi)環(huán)境數(shù)據(jù)與待校準(zhǔn)樣本，即依據(jù)航向采樣產(chǎn)生的粒子與其相鄰墻體形成夾角的情況，自適應(yīng)地對(duì)粒子航向進(jìn)行補(bǔ)償。

其中，在每一個(gè)粒子選擇參照墻體時(shí)，會(huì)根據(jù)結(jié)合步長(zhǎng)推算的當(dāng)前落點(diǎn)位置時(shí)刻坐標(biāo)對(duì)平面所有墻體與該坐標(biāo)的相對(duì)距離進(jìn)行排序。完成距離排序后會(huì)出現(xiàn)以下三種情況：

1)有唯一的最小距離值。

2)有不唯一的最小距離值，且多個(gè)相等的最小距離來自平行的墻體。

3)有不唯一的最小距離值，且多個(gè)相等的最小距離來自不平行的墻體。

對(duì)于第1)種和第2)種情況，可以依據(jù)兩直線夾角的性質(zhì)任選一條線段作為參照墻體。對(duì)于第3)種情況，可以認(rèn)為此時(shí)行人處于轉(zhuǎn)彎處或者某些粒子散落到墻體內(nèi)：對(duì)于前者，此時(shí)不予補(bǔ)償，待行人轉(zhuǎn)彎過后再重復(fù)以上墻體選擇過程；對(duì)于后者，直接將墻體內(nèi)粒子舍棄。

(8)

所有的粒子完成航向補(bǔ)償后具有這樣的特點(diǎn)，即不論粒子偏向行人直線行走整體方向的哪一側(cè)，補(bǔ)償行為總是通過增大或者減小航向粒子數(shù)值使其向行人行走方向收縮，以此來達(dá)到校準(zhǔn)原始航向的目的。但是，對(duì)于每一個(gè)粒子應(yīng)在校準(zhǔn)過程中占有多大的比重，還需要通過權(quán)重分配算法進(jìn)一步確定。

2.3 粒子權(quán)重分配

在傳統(tǒng)的粒子濾波中引入室內(nèi)環(huán)境約束的一大優(yōu)勢(shì)就是可以根據(jù)環(huán)境條件設(shè)置合理的偽觀測(cè)數(shù)據(jù)，而該數(shù)據(jù)又是粒子濾波過程中確定權(quán)重分配的必要前提。在本文算法中，依據(jù)行人室內(nèi)行走實(shí)際情況對(duì)通路中線進(jìn)行坐標(biāo)數(shù)據(jù)轉(zhuǎn)換，標(biāo)定每一個(gè)粒子位置到中線的距離，以此作為偽觀測(cè)數(shù)據(jù)。

因?yàn)樵诹Ｗ雍较蛐拚A段已經(jīng)將落入墻體的粒子進(jìn)行剔除，所以本階段權(quán)重分配只針對(duì)落在通路內(nèi)的粒子。在此，提出通過關(guān)聯(lián)室內(nèi)環(huán)境數(shù)據(jù)與粒子落點(diǎn)構(gòu)建依距離賦權(quán)機(jī)制，即將某一落點(diǎn)時(shí)刻所有粒子到中線的距離進(jìn)行排序，按照距離大小確定粒子權(quán)重。

在上述過程中的墻體選取方式與上一階段粒子補(bǔ)償時(shí)的墻體選取方式相同，因此不再贅述。但需要注意的是，在粒子賦權(quán)階段會(huì)將形成一條通路的兩個(gè)墻體都選取出來，以便后續(xù)計(jì)算。

至此，粒子開始進(jìn)行具體的權(quán)重計(jì)算，如圖4所示，粒子權(quán)重由通路中線向選取的兩側(cè)墻體逐漸遞減，其關(guān)系滿足式(9):

wi∝1/di

(9)

其中:wi為第i個(gè)航向粒子的絕對(duì)權(quán)重;對(duì)于第i個(gè)航向粒子,di為估計(jì)位置到通路中線的距離。

式(10)確定了粒子歸一化權(quán)重值:

(10)

其中：Wi為歸一化權(quán)重；N為參與權(quán)重分配的所有粒子總數(shù)。

值得注意的是，為了提高計(jì)算效率，實(shí)際權(quán)重分配是由每個(gè)粒子到其相鄰墻體的距離決定的，如式(11)所示：

(11)

其中ci表示通路中線到墻體的距離。

圖4 權(quán)重分配示意圖Fig. 4 Schematic diagram of weight allocation

這樣設(shè)計(jì)是因?yàn)?，一方面?dāng)粒子距離墻面越遠(yuǎn)，距離通路中線就越近，其權(quán)重也就越大；另一方面，式(11)可以直接從已有的室內(nèi)環(huán)境結(jié)構(gòu)體中獲取數(shù)據(jù)，從而避免增加不必要的計(jì)算代價(jià)。

2.4 確定行人位置

完成粒子采樣、粒子航向修正、粒子權(quán)重分配的過程后，進(jìn)一步地，可以利用式(12)得到校準(zhǔn)后的航向：

(12)

然后結(jié)合步長(zhǎng)估計(jì)就可以利用式(1)中(Xk+1，Yk+1)的計(jì)算方法得到行人當(dāng)前位置坐標(biāo)。

綜上所述，以下給出本文算法的偽代碼。

輸入 室內(nèi)環(huán)境測(cè)量數(shù)據(jù)、通過卡爾曼濾波得到的步長(zhǎng)估計(jì)lk以及原始航向θk(k=1,2,…,M)；

輸出 校準(zhǔn)后的航向θcor以及位置坐標(biāo)(X，Y)。

1)

對(duì)于θk∈[θ1,θM]執(zhí)行以下命令

//零速度檢測(cè)算法得到行人前行過程中M個(gè)航向角

2)

//粒子采樣過程

3)

F=normrnd(θk,σ,1,N)

//采樣粒子數(shù)為N，正態(tài)分布期望為θk，標(biāo)準(zhǔn)差為σ

4)

//粒子航向補(bǔ)償過程

5)

構(gòu)建室內(nèi)環(huán)境結(jié)構(gòu)體map=(s1,s2,…,sn)

6)

選擇參照墻體si用其完成第8)～9)行代碼計(jì)算

7)

fori= 1:N

8)

9)

10)

11)

end

//在補(bǔ)償過程之后粒子數(shù)量可能低于初始的采樣數(shù)，

//但為了方便敘述，仍用N表示

12)

//粒子權(quán)重分配

13)

選擇通路墻體si、sj用其完成第14)～15)行代碼計(jì)算

14)

di=第i個(gè)粒子距離墻面的距離

15)

//計(jì)算歸一化的權(quán)重

16)

end

//粒子濾波對(duì)航向校準(zhǔn)的過程完畢

17)

//得到優(yōu)化后的第k步的航向

18)

//計(jì)算位置坐標(biāo)

19)

以上偽代碼對(duì)提出算法的整體邏輯進(jìn)行了概括。進(jìn)一步地，可以通過分析其中一些關(guān)鍵步驟，討論算法在計(jì)算代價(jià)上的改進(jìn)。

在代碼第3)行粒子采樣時(shí)，僅涉及到原始航向θk一個(gè)維度，相比于傳統(tǒng)粒子濾波算法中構(gòu)建多維度狀態(tài)量的過程，將矩陣運(yùn)算變?yōu)閿?shù)值運(yùn)算，其對(duì)于運(yùn)算代價(jià)的減少是顯而易見的。此外，在代碼第6)、13)、15)行的計(jì)算過程中實(shí)現(xiàn)了對(duì)室內(nèi)環(huán)境結(jié)構(gòu)體的復(fù)用，尤其是在第15)行中通過轉(zhuǎn)換偽觀測(cè)數(shù)據(jù)在權(quán)重分配中的應(yīng)用方式，可以有效地提升計(jì)算效率。

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

為了驗(yàn)證本文提出的室內(nèi)環(huán)境約束的行人航向粒子濾波修正方法的有效性，測(cè)試人員在研究院1號(hào)辦公樓三層進(jìn)行了多組不同距離的重復(fù)實(shí)驗(yàn)。實(shí)驗(yàn)過程如圖5所示，圖中測(cè)試人員右腳佩戴MIMU集成的慣性傳感器模塊，佩戴位置位于腳尖。該模塊采樣頻率為125 Hz，可以同時(shí)完成4組時(shí)鐘對(duì)齊的數(shù)據(jù)采集。圖5中測(cè)試人員右手為手持式激光測(cè)距儀，用來測(cè)試實(shí)際行走距離以及進(jìn)行定位精度標(biāo)定，其測(cè)量精度為0.01 m。

圖5 實(shí)驗(yàn)過程示意圖Fig. 5 Schematic diagram of experimental process

在數(shù)據(jù)處理過程中，使用的重力加速度為9.788 m/s2，使用的加速度數(shù)據(jù)和陀螺儀數(shù)據(jù)為MIMU輸出數(shù)據(jù)的均值，采樣粒子數(shù)為3 000。本文算法最終軌跡效果以及數(shù)據(jù)對(duì)比結(jié)果通過Matlab軟件仿真來實(shí)現(xiàn)。

通過實(shí)地測(cè)試分析，本文中對(duì)兩種室內(nèi)定位算法的結(jié)果進(jìn)行了比較：一種是本文提出的室內(nèi)環(huán)境約束的行人航向粒子濾波修正算法，另一種是傳統(tǒng)的卡爾曼濾波室內(nèi)定位算法。兩種算法均可在行人行走估計(jì)軌跡與實(shí)際軌跡的對(duì)比中直觀地看到解算效果。如圖6所示，實(shí)線軌跡為實(shí)際行人室內(nèi)行走路線，行走距離為246 m，虛線軌跡為傳統(tǒng)卡爾曼濾波算法得到的行人行走路線效果，雙劃線軌跡為本文提出算法得到的行人行走路線效果。

由圖6可知，對(duì)比實(shí)際行走軌跡，傳統(tǒng)卡爾曼濾波算法的解算會(huì)出現(xiàn)較大的位置和航向偏差，并發(fā)生明顯的穿墻行走現(xiàn)象；經(jīng)過計(jì)算，該算法終點(diǎn)位置絕對(duì)誤差為0.57 m，相對(duì)誤差為0.23%，在終點(diǎn)處航向累計(jì)誤差為7.6°。而本文提出算法所得到的估計(jì)軌跡更加貼合實(shí)際軌跡，并且有效地避免了穿墻行走現(xiàn)象；經(jīng)過計(jì)算，本文算法終點(diǎn)位置的絕對(duì)誤差為0.25 m，相對(duì)誤差為0.10%，在終點(diǎn)處航向累計(jì)誤差為0.39°。因此，在該次實(shí)驗(yàn)中，本文算法相比傳統(tǒng)卡爾曼濾波定位算法降低了0.32 m的絕對(duì)位置累積誤差，降低了7.21°的航向累積誤差。本次實(shí)驗(yàn)結(jié)果可以在一定程度上說明本文算法的有效性。

為了進(jìn)一步驗(yàn)證本文提出算法的有效性和穩(wěn)定性，測(cè)試人員設(shè)置了5段不同的行走距離，分別為61 m、108 m、215 m、323 m、435 m，每段行走距離進(jìn)行獨(dú)立的三組重復(fù)實(shí)驗(yàn)。實(shí)驗(yàn)計(jì)算兩種算法得到終點(diǎn)位置絕對(duì)誤差以及航向累計(jì)誤差。其中，將三組重復(fù)實(shí)驗(yàn)得到的終點(diǎn)位置絕對(duì)誤差的均值作為對(duì)應(yīng)行走距離的位置絕對(duì)誤差，將三組重復(fù)實(shí)驗(yàn)得到的航向累計(jì)誤差的均值作為對(duì)應(yīng)行走距離的航向累計(jì)誤差，以此減小實(shí)驗(yàn)結(jié)果的偶然性。

圖6 不同算法下行人軌跡對(duì)比Fig. 6 Pedestrian trajectory comparison of different algorithms

5段不同行走距離重復(fù)實(shí)驗(yàn)的位置絕對(duì)誤差和累計(jì)航向誤差統(tǒng)計(jì)如表1所示。

從表1中可以得知，對(duì)于傳統(tǒng)卡爾曼濾波算法來說，不論是位置誤差還是航向誤差，在整體上都是呈現(xiàn)隨著行人行走距離增大而不斷增大的趨勢(shì)。但在一些個(gè)別實(shí)驗(yàn)中，這兩種誤差會(huì)出現(xiàn)小于較短距離條件下誤差的情況，這是由于傳統(tǒng)卡爾曼濾波算法本身在航向漂移方面具有不穩(wěn)定的性質(zhì)造成的。所以在實(shí)際實(shí)驗(yàn)中應(yīng)用多組數(shù)據(jù)統(tǒng)計(jì)求均值的方法是很具有必要的，能夠提高實(shí)驗(yàn)結(jié)果的可信度。

而對(duì)于本文算法來說，從數(shù)據(jù)觀察的角度并沒有發(fā)現(xiàn)明顯的發(fā)散趨勢(shì)；同時(shí)，可以通過數(shù)據(jù)對(duì)比發(fā)現(xiàn)不同距離下的絕對(duì)位置誤差和航向誤差并沒有過大的差距。這就說明了本文算法能夠有效地減小航向漂移的不穩(wěn)定性，進(jìn)一步地，與傳統(tǒng)卡爾曼濾波算法得到的誤差數(shù)值進(jìn)行逐個(gè)對(duì)比也可以說明本文算法能夠在一定程度上減小兩種誤差。不過，為了滿足對(duì)比實(shí)驗(yàn)對(duì)嚴(yán)謹(jǐn)性的要求，解算結(jié)果仍然對(duì)三組重復(fù)實(shí)驗(yàn)取均值。

為了更加直觀地說明本文算法相比于傳統(tǒng)卡爾曼濾波算法在減小終點(diǎn)位置絕對(duì)誤差和累計(jì)航向誤差方面的有效性和穩(wěn)定性，還需要結(jié)合多組重復(fù)實(shí)驗(yàn)的結(jié)果完成進(jìn)一步的數(shù)據(jù)分析。

表1 不同距離重復(fù)實(shí)驗(yàn)誤差統(tǒng)計(jì)Tab. 1 Error statistics of repeated experiments at different distances

傳統(tǒng)卡爾曼濾波定位算法和本文所提算法在不同距離情況下終點(diǎn)位置絕對(duì)誤差的對(duì)比如圖7所示，該圖數(shù)據(jù)是通過對(duì)所有重復(fù)實(shí)驗(yàn)完成均值處理所得。由圖7可知，傳統(tǒng)卡爾曼濾波定位算法在不同行走距離情況下終點(diǎn)位置絕對(duì)誤差始終大于1.50 m，并且，隨著行走距離的增加，絕對(duì)誤差呈現(xiàn)出一種發(fā)散的趨勢(shì)，不斷增大；在行走距離超過400 m時(shí)，該算法絕對(duì)誤差達(dá)到了5.50 m，此時(shí)的相對(duì)誤差為1.3%左右。而本文所提算法在不同行走距離情況下的終點(diǎn)位置絕對(duì)誤差相對(duì)穩(wěn)定，并且始終不超過1.50 m；在行走距離超過400 m時(shí)，該算法絕對(duì)誤差為0.87 m，此時(shí)的相對(duì)誤差僅為0.2%左右。通過兩種算法對(duì)比可知，無論是在特定行走距離方面，還是在隨著行走距離增大時(shí)的誤差趨勢(shì)方面，本文所提算法始終優(yōu)于傳統(tǒng)的卡爾曼濾波算法，并且在行走距離大于400 m時(shí)，基于本文算法的位置精度提高了一個(gè)量級(jí)，表明了本文算法的有效性和穩(wěn)定性。

圖7 不同算法終點(diǎn)位置絕對(duì)誤差對(duì)比Fig. 7 Comparison of absolute error at end position of different algorithms

在航向校準(zhǔn)方面，傳統(tǒng)的卡爾曼濾波算法只將航向作為狀態(tài)量之一，實(shí)際解算時(shí)還要完成包括位置、加速度、姿態(tài)角等多個(gè)參量共同的狀態(tài)轉(zhuǎn)移，該過程相對(duì)繁瑣且航向在長(zhǎng)距離行走情景下依然會(huì)存在較大的偏移。而本文算法只對(duì)單一的航向參量進(jìn)行粒子采樣和航向補(bǔ)償，并通過權(quán)重分配使得每一步落點(diǎn)都更靠近真實(shí)位置，計(jì)算過程相對(duì)簡(jiǎn)潔。同時(shí)我們注意到，行人在行走時(shí)的單步真實(shí)航向只通過慣性傳感器本身很難進(jìn)行數(shù)值標(biāo)定，但在行走一段直線距離時(shí)，根據(jù)起始點(diǎn)位置計(jì)算的航向是相對(duì)準(zhǔn)確且具有實(shí)際意義的。出于以上考慮，本文提出的航向算法在直線行走階段的解算是更加貼近該直線軌跡下的真實(shí)航向的。兩種算法航向誤差對(duì)比如圖8所示，圖中的航向解算就是以直走廊為參考得出的。

圖8 不同算法航向誤差對(duì)比Fig. 8 Comparison of heading error of different algorithms

由圖8可知，傳統(tǒng)卡爾曼濾波算法得到的航向誤差在行走距離為61 m時(shí)就超過了3°，并且隨著行走距離的增加，航向誤差不斷累積，在行走距離超過400 m時(shí)達(dá)到了15°以上。整體上，傳統(tǒng)卡爾曼濾波算法得到的航向誤差呈現(xiàn)發(fā)散趨勢(shì)，同時(shí)短距離時(shí)的較大誤差使得該算法難以在實(shí)際情境下直接應(yīng)用。而本文提出的算法在實(shí)際測(cè)試過程中始終將航向誤差穩(wěn)定在了0.9°以內(nèi)，相比于傳統(tǒng)卡爾曼濾波算法，航向精度有了明顯的提高，驗(yàn)證了本文算法的有效性和穩(wěn)定性。

4 結(jié)語(yǔ)

針對(duì)在傳統(tǒng)卡爾曼濾波室內(nèi)定位算法中航向累積誤差的問題，本文提出了室內(nèi)環(huán)境約束的行人航向粒子濾波修正方法，在卡爾曼濾波的基礎(chǔ)上，結(jié)合室內(nèi)環(huán)境信息，利用粒子濾波算法對(duì)航向進(jìn)行修正，得到了更加精確的行人位置。本文主要貢獻(xiàn)如下:1)在粒子動(dòng)態(tài)選取參照墻體的基礎(chǔ)上，構(gòu)建粒子航向自修正機(jī)制，使補(bǔ)償后的粒子更趨近于行人行走方向。2)構(gòu)建關(guān)聯(lián)通路偽測(cè)量數(shù)據(jù)的依距離賦權(quán)機(jī)制。通過確立合理的粒子賦權(quán)規(guī)則，在有效避免過擬合的基礎(chǔ)上提升了整體航向的精度。3)精簡(jiǎn)了粒子濾波模型，將航向作為唯一的狀態(tài)量，在提高精度的同時(shí)方便了數(shù)據(jù)分析和處理。經(jīng)過實(shí)際場(chǎng)景分距離多組重復(fù)實(shí)驗(yàn)驗(yàn)證，本文算法在減小行人定位絕對(duì)誤差和航向累積誤差方面的有效性及穩(wěn)定性都明顯優(yōu)于傳統(tǒng)卡爾曼濾波算法。但本文算法在底層數(shù)據(jù)降噪處理以及場(chǎng)景泛化方面存在著一定的局限性。未來工作中，一方面會(huì)基于已有成果改進(jìn)粒子狀態(tài)轉(zhuǎn)移過程以進(jìn)一步提高算法場(chǎng)景適用度；另一方面會(huì)融合多部位、多特征底層數(shù)據(jù)進(jìn)行關(guān)聯(lián)研究，并將多數(shù)據(jù)關(guān)聯(lián)應(yīng)用于已有算法預(yù)處理、狀態(tài)轉(zhuǎn)移和權(quán)重更新階段，以此進(jìn)一步提高基于多數(shù)據(jù)融合的行人航向精度。