基于慣性遞推原理的行人自主定位方法綜述及展望

張文超，魏東巖，袁 洪, 李欣雨

(1. 中國(guó)科學(xué)院空天信息創(chuàng)新研究院，北京 100094；2. 北京信息科技大學(xué)信息與通信工程學(xué)院，北京 100192)

0 引言

隨著城鎮(zhèn)化進(jìn)程的加快，我國(guó)正在建設(shè)越來(lái)越多的大型建筑，伴隨而來(lái)的是城市人口越來(lái)越多，未來(lái)80%的人將花費(fèi)80%的時(shí)間處于室內(nèi)環(huán)境[1]，室內(nèi)環(huán)境下的位置服務(wù)需求將越來(lái)越大。同時(shí)，火災(zāi)救援、礦井救援、位置監(jiān)控等應(yīng)急救援和公共安全活動(dòng)，也需要在室內(nèi)環(huán)境下提供滿足一定精度且連續(xù)的位置服務(wù)[2-3]，因此，高精度室內(nèi)行人導(dǎo)航定位技術(shù)未來(lái)將會(huì)被廣泛應(yīng)用。

目前，室內(nèi)行人導(dǎo)航中存在著大量的基于不同機(jī)制的定位技術(shù)，按照是否需要基礎(chǔ)設(shè)施進(jìn)行劃分[4]，可分為基于基礎(chǔ)設(shè)施(Infrastructure-based)的定位技術(shù)，如：全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)[5]、偽衛(wèi)星(Pseudolites, PL)[6]、無(wú)線局域網(wǎng)(Wireless Local Area Network，WLAN)[7]、射頻識(shí)別(Radio Frequency Identification，RFID)[8]、藍(lán)牙(Blue-Tooth，BT)、移動(dòng)蜂窩網(wǎng)絡(luò)和超寬帶(Ultra-Wide Band，UWB)[9]等；以及基于自包含(Self-contained)傳感器的定位技術(shù)，主要是以微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)傳感器(加速度計(jì)、陀螺儀、磁強(qiáng)計(jì)、氣壓計(jì)、視覺(jué)傳感器)為主的定位技術(shù)[10-13]。

其中，由于MEMS傳感器技術(shù)的迅速發(fā)展，以微慣性測(cè)量單元(Micro Inertial Measurement Unit, MIMU)為主要設(shè)備的穿戴式室內(nèi)定位方法在實(shí)際應(yīng)用場(chǎng)景中具有顯著優(yōu)勢(shì)[13-14]。MIMU 主要由三軸陀螺儀和三軸加速度計(jì)構(gòu)成，也可附加其他傳感器，如磁強(qiáng)計(jì)(測(cè)量地球磁場(chǎng)信息)和氣壓計(jì)(測(cè)量高程)等，在給定的運(yùn)動(dòng)初始狀態(tài)條件下，通過(guò)測(cè)量載體相對(duì)于慣性空間的運(yùn)動(dòng)參數(shù)，經(jīng)過(guò)轉(zhuǎn)換處理后就可得到載體的姿態(tài)和航向等信息。MIMU傳感器具有低成本、測(cè)量范圍大、完全自主、不受干擾、信息量大、實(shí)時(shí)性高等特點(diǎn)[15]。不同于無(wú)線信號(hào)室內(nèi)定位技術(shù)，基于MIMU的行人室內(nèi)定位不需要提前安置大量的無(wú)線基礎(chǔ)設(shè)施，其精度僅局限于設(shè)備測(cè)量的準(zhǔn)確程度。

因此，基于MIMU的行人室內(nèi)定位方法是本文討論的重點(diǎn)。其主要有兩類解決辦法[16]：一類方法利用慣性傳感器對(duì)步數(shù)進(jìn)行計(jì)數(shù)，并估計(jì)步長(zhǎng)，結(jié)合航向輸出，推算當(dāng)前位置信息，原理如圖1(a)所示；另一類是將慣性傳感器與身體固連(主要置于腳上、腿上等部位)，采用慣性解算的方法，對(duì)行進(jìn)過(guò)程中的每一步進(jìn)行慣性遞推，并計(jì)算位置增量，同時(shí)利用零速修正(Zero Velocity Update，ZUPT)等方法約束慣性累積誤差，從而輸出可靠的位置信息，其原理如圖1(b)所示。

(a)

上述兩類解決辦法各有優(yōu)勢(shì)和劣勢(shì)，但從計(jì)算方法和結(jié)果上來(lái)說(shuō)，純慣性遞推算法的導(dǎo)航機(jī)制基于牛頓運(yùn)動(dòng)定律，理論上計(jì)算結(jié)果更精確可靠，該方法利用三個(gè)正交方向的角速度和加速度信息可實(shí)時(shí)積分出行人的三維速度、位置和姿態(tài)；而步長(zhǎng)+航向遞推的算法模型是一種估算方法，依賴的不確定參數(shù)較多，多適用于二維平面的速度和位置計(jì)算。因此，本文重點(diǎn)圍繞基于純慣性遞推原理的行人自主定位方法進(jìn)行綜述，該方法的定位系統(tǒng)基本結(jié)構(gòu)如圖2所示[17]。

圖2 腳綁式MEMS傳感器定位系統(tǒng)基本結(jié)構(gòu)[17]

1 基于慣性遞推原理的行人定位方法分析

基于慣性遞推原理的行人定位方法的原理是利用行人運(yùn)動(dòng)的周期性規(guī)律，結(jié)合有效的概率約束模型，約束慣性遞推過(guò)程中的漂移誤差，從而實(shí)現(xiàn)行人運(yùn)動(dòng)過(guò)程中的可靠定位?；玖鞒倘鐖D3所示，其中包含幾個(gè)關(guān)鍵的環(huán)節(jié)：行人周期性運(yùn)動(dòng)特性分析與檢測(cè)、零速區(qū)間檢測(cè)、MIMU慣性誤差校準(zhǔn)、慣性航位遞推、誤差約束策略和信息融合算法等。

圖3 基于慣性遞推原理的行人定位基本流程

1.1 行人運(yùn)動(dòng)特性分析與檢測(cè)

1.1.1 行人運(yùn)動(dòng)特性分析

行人正常的行走過(guò)程是周期性的[18]，根據(jù)運(yùn)動(dòng)特性可以將其劃分為若干階段。如圖4所示，以右腳為例，分為右腳跟觸地階段、腳放平的站立中間階段、站立結(jié)束階段、預(yù)擺動(dòng)階段(腳尖離地)、初始擺動(dòng)階段、擺動(dòng)中期和擺動(dòng)末期，然后重新開(kāi)始周期性運(yùn)動(dòng)[19]。

圖4 行人一次行進(jìn)周期內(nèi)的運(yùn)動(dòng)情況[19]

通過(guò)有效劃分行人運(yùn)動(dòng)過(guò)程中的不同階段，可以幫助分析腳步的周期運(yùn)動(dòng)特性，進(jìn)而利用其規(guī)律約束慣性行人航跡推算(Pedestrian Dead Reckon，PDR)定位結(jié)果的發(fā)散與漂移。然而，在行人運(yùn)動(dòng)過(guò)程中,若想要實(shí)現(xiàn)各個(gè)階段的準(zhǔn)確劃分與識(shí)別，方法復(fù)雜且可靠性差。目前，基于慣性遞推原理的穿戴式PDR系統(tǒng)主要利用行人行進(jìn)過(guò)程中的站立靜止階段(零速階段)，該階段行人腳的速度約為零，可以用來(lái)作為觀測(cè)量，有效約束慣性推算定位結(jié)果的發(fā)散[19]；而其余階段對(duì)于行人定位幫助作用較小，因此可將行人腳步周期性運(yùn)動(dòng)簡(jiǎn)化為四個(gè)階段，如圖5所示。

圖5 行人步態(tài)特征示意圖[19]

由圖5可知，一個(gè)步態(tài)周期是指從一只腳的腳后跟離地開(kāi)始，到該腳的腳后跟落地為止，可分為四個(gè)階段：起步階段、跨步階段、落地階段和靜止階段。對(duì)于靜止階段(零速階段)的檢測(cè)，目前通用的方法就是將單個(gè)MIMU設(shè)備綁在人的腳上(如圖2所示)，當(dāng)行人開(kāi)始行走時(shí)，IMU會(huì)不斷測(cè)量腳運(yùn)動(dòng)的加速度和角速度信息，其中當(dāng)腳落地之后會(huì)出現(xiàn)一個(gè)短暫的零速階段，該階段MIMU傳感器的輸出基本恒定，加速度約為重力加速度，角速度約為零(忽略地球自轉(zhuǎn))。因此，可根據(jù)該數(shù)據(jù)輸出特征識(shí)別行人步態(tài)[20]。

1.1.2 零速區(qū)間檢測(cè)方法

根據(jù)文獻(xiàn)[18]和文獻(xiàn)[21]中的研究可知，目前常用的零速區(qū)間檢測(cè)方法都是基于閾值比較原理。利用零速階段MIMU輸出加速度約為重力加速度，角速度約為零(忽略地球自轉(zhuǎn))，構(gòu)造檢測(cè)量T，然后將檢測(cè)量T和經(jīng)驗(yàn)閾值進(jìn)行比較,從而確定零速區(qū)間，如式(1)所示

(1)

常用的構(gòu)造零速檢測(cè)量T的方法如下：

1)加速度方差法(MV)

(2)

2)加速度幅值法(MAG)

(3)

3)角速率幅值法(ARE)

(4)

4)基于廣義似然比的檢測(cè)法(SHOE)

(5)

1.1.3 行人運(yùn)動(dòng)類型識(shí)別方法

在實(shí)際行人運(yùn)動(dòng)過(guò)程中，行人不僅有正常行走，通常還包括各種復(fù)雜運(yùn)動(dòng)[23]，如：靜止?fàn)顟B(tài)、行走、乘電梯、上下樓梯、站立/行走在水平移動(dòng)電梯上、站立/行走在自動(dòng)扶梯上等。每種運(yùn)動(dòng)情況又可進(jìn)行細(xì)分，如行走狀態(tài)可分為：慢走、快走、小跑、奔跑等。不同的運(yùn)動(dòng)類型下可采用的定位約束方法也不盡相同，如果可以有效識(shí)別出行人運(yùn)動(dòng)過(guò)程中的運(yùn)動(dòng)類型，對(duì)于提高慣性PDR系統(tǒng)定位結(jié)果的準(zhǔn)確性具有良好的輔助作用。

1.1.2節(jié)中提出的零速區(qū)間檢測(cè)方法通常是針對(duì)行人在正常行走狀態(tài)下的檢測(cè)方法。因此，當(dāng)行人同時(shí)出現(xiàn)多種運(yùn)動(dòng)切換時(shí)，前述檢測(cè)方法將失效。目前常用的運(yùn)動(dòng)類型識(shí)別方法，主要利用三軸加速度計(jì)、三軸陀螺儀、三軸磁強(qiáng)計(jì)、單軸氣壓計(jì)等傳感器的輸出，構(gòu)造特征量進(jìn)行檢測(cè)。如文獻(xiàn)[24]所述，可構(gòu)造的特征量約51個(gè)，基本方法如表1所示。

表1中，zi表示行人運(yùn)動(dòng)過(guò)程中不同傳感器的測(cè)量值。根據(jù)特征量構(gòu)造的方法整體上被分為四類：1)統(tǒng)計(jì)特征，利用某種度量來(lái)衡量整個(gè)觀測(cè)窗口中數(shù)據(jù)的分布或中心值；2)時(shí)域特征，用于分析信號(hào)如何在觀察窗口內(nèi)變化；3)頻域特征，用于分析信號(hào)的頻率；4)幅值特征，用于分析信號(hào)強(qiáng)度的能量特性。綜合利用上述特征量，基本可以實(shí)現(xiàn)典型運(yùn)動(dòng)類型的識(shí)別，文獻(xiàn)[25-26]中的研究均有相關(guān)論述。同時(shí)，還有學(xué)者利用上述特征量構(gòu)造機(jī)器學(xué)習(xí)和模式識(shí)別算法，并利用這些智能方法實(shí)現(xiàn)不同類型行人運(yùn)動(dòng)的識(shí)別。

表1 行人運(yùn)動(dòng)特征量構(gòu)造方法

1.2 MIMU慣性誤差分析及校準(zhǔn)

MIMU慣性誤差是影響慣性PDR定位結(jié)果的主要因素，因此需要進(jìn)行有效地分析與校準(zhǔn)。

1.2.1 IMU誤差分析

IMU的誤差主要來(lái)源于三部分，包括噪聲、刻度系數(shù)和安裝誤差[27]。加速度計(jì)和陀螺儀的測(cè)量模型如式(6)所示

(6)

其中，a表示加速度計(jì)測(cè)量值；g表示陀螺儀測(cè)量值；T表示安裝誤差的變換矩陣；K表示刻度系數(shù)；B表示正交的參考坐標(biāo)系；S表示非正交的設(shè)備坐標(biāo)系；aS和wS分別表示加速度和角速度測(cè)量值；b和ν分別表示零偏和白噪聲。

1.2.2 IMU誤差校準(zhǔn)

IMU誤差的經(jīng)典校準(zhǔn)方法是通過(guò)轉(zhuǎn)臺(tái)等基準(zhǔn)設(shè)備進(jìn)行誤差校準(zhǔn)。這里主要介紹不借助基準(zhǔn)設(shè)備的校準(zhǔn)方法，校準(zhǔn)流程如圖6所示,該方法主要通過(guò)IMU的靜止和旋轉(zhuǎn)過(guò)程實(shí)現(xiàn)。根據(jù)校準(zhǔn)方式的不同，可分為離線校準(zhǔn)和在線校準(zhǔn)。離線校準(zhǔn)通常在正式定位前進(jìn)行，通過(guò)將傳感器靜止一段時(shí)間進(jìn)行初始化，再人為旋轉(zhuǎn)若干次，以達(dá)到校準(zhǔn)效果，初始化時(shí)間T一般在分鐘量級(jí)，旋轉(zhuǎn)后保持靜態(tài)時(shí)間t一般為幾秒左右，旋轉(zhuǎn)次數(shù)大約為幾十次，一般來(lái)說(shuō)，靜態(tài)時(shí)間越長(zhǎng)、旋轉(zhuǎn)次數(shù)越多越好，至少要大于要求解的參數(shù)個(gè)數(shù)，這樣才能避免奇異性。雖然離線校準(zhǔn)可以達(dá)到較好的精度，但大多數(shù)應(yīng)用并不具備離線校準(zhǔn)的條件，因此需要在定位過(guò)程中進(jìn)行在線校準(zhǔn)。在線校準(zhǔn)主要是通過(guò)檢測(cè)運(yùn)動(dòng)過(guò)程中的IMU靜態(tài)時(shí)段和動(dòng)態(tài)時(shí)段來(lái)實(shí)現(xiàn)IMU誤差校準(zhǔn)。

圖6 慣性誤差校準(zhǔn)方法基本流程圖

從具體的校準(zhǔn)方法上看，加速度計(jì)的校準(zhǔn)通常包含六面法校準(zhǔn)和優(yōu)化模型校準(zhǔn)法等，其中優(yōu)化模型校準(zhǔn)法通常用于在線校準(zhǔn)；陀螺儀的離線校準(zhǔn)常采用Allan方差法。

1.2.2.1 靜態(tài)檢測(cè)

在加速度計(jì)和陀螺儀的校準(zhǔn)過(guò)程中，常常需要傳感器處于靜止?fàn)顟B(tài)，因此在校準(zhǔn)過(guò)程中需要判斷傳感器是否處于靜止?fàn)顟B(tài)。判斷依據(jù)如下

ζ(tw)=

(7)

其中，Vartw(at)表示加速度at在時(shí)間段tw內(nèi)的方差。判斷時(shí)，只需要將ζ(tw)和閾值ζ(Tinit)進(jìn)行比較即可，離線校準(zhǔn)和在線校準(zhǔn)過(guò)程均需靜態(tài)檢測(cè)。

1.2.2.2 加速度計(jì)校準(zhǔn)

加速度計(jì)的校準(zhǔn)一般都是將加速度計(jì)靜止，然后根據(jù)測(cè)量值的二范數(shù)等于當(dāng)?shù)刂亓铀俣冗@一規(guī)則進(jìn)行校準(zhǔn)。一般會(huì)用1.2.2.1節(jié)中提到的靜態(tài)檢測(cè)算法在采樣序列上挑出靜態(tài)測(cè)量時(shí)段，按照下述方法實(shí)現(xiàn)。

1)六面法校準(zhǔn)加速度計(jì)

建立加速度計(jì)的校準(zhǔn)模型

(8)

其中，At表示最終的真實(shí)值;Amx、Amy、Amz表示加速度計(jì)三軸的測(cè)量值；R3×3表示旋轉(zhuǎn)矩陣；Scalex、Scaley、Scalez表示加速度計(jì)三軸的刻度系數(shù)；offsetx、offsety、offsetz表示零偏。將式(8)變換后可得

(9)

這時(shí)候就可以用最小二乘法求解上面的方程了。寫(xiě)作β=(XTX)-1XTY，其中β為式(9)中4×3矩陣，X是坐標(biāo)的測(cè)量值，Y是右邊的真值。關(guān)于真值可以把加速度計(jì)按三個(gè)軸向六個(gè)位置放置(每個(gè)軸向包含正反向)，這樣就對(duì)加速度計(jì)做了校準(zhǔn)，該方法常用于加速度計(jì)的離線校準(zhǔn)過(guò)程。

2)優(yōu)化模型校準(zhǔn)方法

優(yōu)化模型校準(zhǔn)法可用于在線校準(zhǔn)和離線校準(zhǔn)，基本可以校準(zhǔn)上述IMU誤差分析中加速度的所有誤差。在加速度校準(zhǔn)時(shí)，為了進(jìn)一步將變換矩陣T簡(jiǎn)化，一般假設(shè)正交坐標(biāo)系BF和加速度計(jì)坐標(biāo)系A(chǔ)F的x軸重合，且BF的y軸在AF的x軸和y軸的平面上。所以可進(jìn)一步寫(xiě)為

(10)

那么待求解參數(shù)變?yōu)?/p>

(11)

定義狀態(tài)方程為

aO=h(aS,θacc)=TaKa(aS+ba)

(12)

因?yàn)樵诓杉铀俣扔?jì)讀數(shù)的時(shí)候取的是一個(gè)小窗口內(nèi)的平均值，所以忽略了高斯白噪聲，進(jìn)而有了優(yōu)化的代價(jià)函數(shù)

(13)

一般會(huì)有多組較為明顯的、穩(wěn)定的旋轉(zhuǎn)量放入代價(jià)函數(shù)中求解待求解參數(shù)，然后選取殘差最小的一組所對(duì)應(yīng)的參數(shù)即可。

1.2.2.3 陀螺儀校準(zhǔn)

陀螺儀的校準(zhǔn)分為兩個(gè)部分：Allan方差校準(zhǔn)零偏和優(yōu)化方式求解刻度系數(shù)及安裝誤差。在校準(zhǔn)陀螺儀的時(shí)候，要使用到加速度計(jì)的校準(zhǔn)信息，所以加速度計(jì)校準(zhǔn)的好壞關(guān)系到整個(gè)IMU的校準(zhǔn)效果。

(1)Allan方差校準(zhǔn)陀螺儀零偏

在Allan方差分析中，共有5個(gè)噪聲參數(shù)：量化噪聲、角度隨機(jī)游走、零偏不穩(wěn)定性、速度隨機(jī)游走和速度爬升。Allan方差的計(jì)算方法[28]如下：

1)陀螺儀靜止放置時(shí)間為T(mén)，單個(gè)采樣周期為τ0，共有N組采樣值;

2)計(jì)算單次采樣輸出角度θ和平均因子m，m要盡量取得均勻

(14)

3)計(jì)算Allan方差，不同m值情況下會(huì)有不同的Allan方差值

(15)

其中，τ=mτ0；

4)一般在繪制Allan方差曲線時(shí)使用的是Allan方差的平方根，所以將式(15)中的計(jì)算結(jié)果取平方根即可。

通過(guò)Allan方差分析得到陀螺儀誤差的過(guò)程，一般要采集好幾個(gè)小時(shí)的數(shù)據(jù)。如果僅需要零偏參數(shù)，則在初始放置50s左右的時(shí)間就足夠了。

(2)優(yōu)化方式求解尺度因子及軸偏差

在校準(zhǔn)陀螺儀的剩余參數(shù)的時(shí)候，挑選的是加速度計(jì)旋轉(zhuǎn)校準(zhǔn)過(guò)程中的動(dòng)態(tài)時(shí)段。校準(zhǔn)過(guò)的靜態(tài)時(shí)段加速度計(jì)測(cè)量值的平均值可作為初始的重力向量和旋轉(zhuǎn)完成后的實(shí)際重力向量。該方法可用于陀螺儀的在線校準(zhǔn)。

在已知陀螺儀的零偏后，還需要校準(zhǔn)的參數(shù)為

(16)

這些參數(shù)的校準(zhǔn)方式為：陀螺儀任意轉(zhuǎn)動(dòng)，對(duì)積分得到的角度和加速度測(cè)量值求得的角度進(jìn)行比較即可。這里把根據(jù)加速度計(jì)算得到的測(cè)量值當(dāng)作參考值，所以前面對(duì)加速度的校準(zhǔn)是非常重要的。具體校準(zhǔn)過(guò)程如下所述。

設(shè)由加速度計(jì)測(cè)量值得到了一個(gè)初始的加速度向量ua,k-1，陀螺儀n個(gè)測(cè)量值wi，則可以得到旋轉(zhuǎn)之后新的重力向量ug,k為

(17)

進(jìn)一步得到代價(jià)函數(shù)

(18)

其中，ua,k為由加速度的測(cè)量值得到的實(shí)際重力向量。在式(17)中會(huì)涉及離散時(shí)間的陀螺儀積分問(wèn)題，可選擇使用四階龍格庫(kù)塔法。

1.3 慣性遞推PDR方法分析

1.1節(jié)描述了行人運(yùn)動(dòng)的基本規(guī)律，以及有效檢測(cè)行人運(yùn)動(dòng)過(guò)程中零速區(qū)間和識(shí)別行人運(yùn)動(dòng)類型的方法。本節(jié)重點(diǎn)分析慣性PDR中的慣性遞推算法和零速約束算法。

1.3.1 慣性PDR算法

穿戴式慣性PDR主要通過(guò)將MIMU傳感器綁定在行人腳上，跟蹤腳部運(yùn)動(dòng)的三軸加速度和角速度信息，基于航位推算原理，利用慣性積分方法連續(xù)計(jì)算行人行走的相對(duì)距離和航向，如圖1(b)所示。然后，利用初始位置和航向計(jì)算當(dāng)前行人的絕對(duì)位置及方位[29]。核心計(jì)算公式如下

(19)

1.3.2 零速約束算法

1.4 運(yùn)動(dòng)約束方法

由于慣性誤差的漂移，慣性遞推定位結(jié)果會(huì)隨時(shí)間發(fā)散。同時(shí)，由于MIMU 器件的精度比傳統(tǒng)慣性器件精度低很多，定位累積誤差很大，因此必須采取必要的輔助手段對(duì)定位誤差進(jìn)行修正。

如文獻(xiàn)[30]所述，現(xiàn)有的腳綁式PDR系統(tǒng)的主要問(wèn)題是行人行進(jìn)過(guò)程中PDR系統(tǒng)的航向漂移。 ZUPT輔助的EKF算法對(duì)航向的約束較小，隨著時(shí)間的推移，估計(jì)的軌跡會(huì)明顯偏離實(shí)際軌跡。因此，許多學(xué)者針對(duì)航向約束的方法進(jìn)行了深入的研究[31-32]。零角速率更新(Zero Angular Rate Update，ZARU)是一種可以估計(jì)每個(gè)零速階段中陀螺儀偏差的方法。但是在實(shí)際的行人行走過(guò)程中的零速階段，往往會(huì)存在系統(tǒng)性的運(yùn)動(dòng)和殘余角速率[32]，所以滿足ZARU的條件通常不會(huì)真正發(fā)生，除非行人長(zhǎng)時(shí)間保持穩(wěn)定站立姿勢(shì)[33]。

另外，由于大多數(shù)建筑物具有規(guī)則的結(jié)構(gòu)，因此有些學(xué)者建議使用建筑物中走廊的方向作為約束，這已被證明是解決基于MIMU的PDR系統(tǒng)航向漂移的有效方法。例如，Abdulrahim等學(xué)者[34]強(qiáng)調(diào)指出，大多數(shù)室內(nèi)走廊沿4個(gè)主要方向(稱為主導(dǎo)方向)相互平行或正交，并與建筑物的外圍墻壁平行。他們提出了啟發(fā)式漂移消除(Heuristic Drift Elimination，HDE)算法，該算法通過(guò)與最接近的主導(dǎo)方向匹配以校正慣性遞推航向。然后，利用主導(dǎo)方向與步幅航向之間的航向差(航向誤差)作為測(cè)量結(jié)果輸入到Kalman濾波器中。該方法適用于行人沿主導(dǎo)方向行走。但是，如果長(zhǎng)時(shí)間沿非主導(dǎo)方向行走，則PDR系統(tǒng)仍會(huì)出現(xiàn)航向漂移。Jimenez對(duì)此方法進(jìn)行了改進(jìn)，調(diào)整了該算法在非主導(dǎo)方向的判別方法，使其可以容納行人行走非主導(dǎo)方向路徑，這種改進(jìn)的方法稱為iHDE[35]。張文超等[36]指出,HDE和iHDE方法都使用相鄰腳步之間的位置(步幅)方向來(lái)確定行人的行走方向是否靠近室內(nèi)走廊方向(主方向)，然后使用位置方向和最近的參考走廊的方向差糾正行人慣性遞推航向。然而，從根本上講，參考走廊航向接近的是當(dāng)前軌跡的位置航向，而非當(dāng)前慣性遞推航向。因此，他提出了利用參考走廊航向計(jì)算當(dāng)前腳步處的估計(jì)位置，然后使用估計(jì)位置與慣性遞推位置差約束航向偏差，并通過(guò)實(shí)驗(yàn)證明了該方法對(duì)系統(tǒng)航向的修正效果良好。

磁場(chǎng)也是常用的航向校準(zhǔn)方法。但是，建筑物內(nèi)部的磁場(chǎng)會(huì)隨著空間和時(shí)間的變化而迅速變化，并且對(duì)某些金屬物體或電氣設(shè)備也很敏感[37-38]。Yang等[39]提出了采用地磁校正航向算法，將磁航向和慣性航向的航向差作為測(cè)量值來(lái)校正PDR系統(tǒng)航向。Afzal[40]提出了通過(guò)捕獲行人零速區(qū)間內(nèi)的磁場(chǎng)變化來(lái)估計(jì)航向誤差，但是由于磁力計(jì)在室內(nèi)環(huán)境中會(huì)更容易受到干擾，因此不能保證其計(jì)算航向角的可靠性；Skog[41]提出了行人行進(jìn)過(guò)程中的定位誤差是關(guān)于左右腳對(duì)稱的,通過(guò)在兩只腳上都安裝基于IMU的PDR定位系統(tǒng)，然后利用兩只腳間的最大距離約束PDR系統(tǒng)的定位結(jié)果，可以有效消除PDR系統(tǒng)航向誤差。

另外，高程發(fā)散是多樓層定位中PDR系統(tǒng)面臨的主要問(wèn)題。常用的方法是利用氣壓高度計(jì)輸出作為高度約束，但在一些特殊情況如消防救火、士兵作戰(zhàn)等條件下，現(xiàn)場(chǎng)的大火或爆炸可能瞬間改變氣壓，從而影響氣壓與高度的關(guān)系。此時(shí)，可以利用上下樓梯的運(yùn)動(dòng)規(guī)律進(jìn)行高度和樓層估計(jì)[42]。目前，文獻(xiàn)中使用的方法主要是利用俯仰角的變化[43]或相鄰腳步間的高程變化[44]來(lái)確定行人是在平面還是樓梯上行走。國(guó)防科技大學(xué)的谷陽(yáng)等[42]基于建筑物室內(nèi)臺(tái)階高度為定值，提出了采用基于隱馬爾可夫模型(Hidden Markov Model, HMM)算法的動(dòng)作識(shí)別和臺(tái)階數(shù)量統(tǒng)計(jì)的方法，以估算高度變化，效果良好。張文超等[36]基于建筑物內(nèi)樓梯坡度通常為20°～45°，提出了使用相鄰一個(gè)/多個(gè)腳步之間的坡度(高程差除以步幅長(zhǎng)度)來(lái)確定行人的狀態(tài)。如果行人在平面上行走，則保持高程和前序腳步處高程一致；如果行人在樓梯上行走，則使用樓梯的坡度(通常為20°～45°)和HDE模型約束當(dāng)前腳步處的高程，效果良好。

1.5 信息融合算法

如何實(shí)現(xiàn)MIMU測(cè)量信息與運(yùn)動(dòng)約束方法(零速更新等)的有效融合，是穿戴式PDR定位過(guò)程中的重要環(huán)節(jié)。目前，穿戴式PDR中通用的信息融合算法為Kalman濾波算法和貝葉斯濾波算法等。Kalman濾波算法是目前文獻(xiàn)研究中通用的算法模型，該算法可有效實(shí)現(xiàn)與MIMU慣性遞推相關(guān)的約束策略的信息融合。另外，貝葉斯濾波算法常用于實(shí)現(xiàn)MIMU測(cè)量信息與非同源信息的融合，例如MIMU測(cè)量信息與地圖信息的融合等。

1.5.1 Kalman濾波算法

在前述零速檢測(cè)過(guò)程中，可以有效區(qū)分行人行進(jìn)過(guò)程中的零速階段和動(dòng)態(tài)階段。在零速期間，采用基于Kalman濾波模型的約束算法可有效約束行人運(yùn)動(dòng)狀態(tài)的發(fā)散。按照Kalman濾波模型[45-47]，首先建立狀態(tài)方程

(20)

(21)

(22)

1.5.2 貝葉斯濾波算法

在基于慣性遞推原理的穿戴式室內(nèi)行人定位中，貝葉斯濾波算法常常用于將慣性遞推定位結(jié)果和地圖或者室內(nèi)樓層信息結(jié)合，利用地圖(或者樓層)等確定性信息輔助約束慣性PDR遞推定位結(jié)果。基于貝葉斯模型，目前文獻(xiàn)中常采用的方法是利用室內(nèi)地圖的拓?fù)浣Y(jié)構(gòu)信息(一系列的關(guān)鍵節(jié)點(diǎn))結(jié)合粒子濾波算法，將慣性PDR遞推位置約束到拓?fù)涔?jié)點(diǎn)的連接線范圍內(nèi)[48-49]。這些方法魯棒性較強(qiáng)，可以提供較可靠的定位結(jié)果；但是在大型開(kāi)放廳中，將導(dǎo)航路徑限制到節(jié)點(diǎn)連接邊緣通常是不切實(shí)際的，且計(jì)算過(guò)程中需要進(jìn)行大量的粒子迭代計(jì)算，復(fù)雜度較高，該算法在地圖輔助的PDR定位文獻(xiàn)中均有論述。這里主要介紹了一種非回溯貝葉斯濾波算法[50]，該算法兼具粒子濾波算法的優(yōu)勢(shì)，同時(shí)可以有效降低概率計(jì)算的復(fù)雜度。該算法不同于通用性的地圖約束算法(采用地圖的拓?fù)涔?jié)點(diǎn)信息結(jié)構(gòu))，而是采用柵格地圖建立概率密度函數(shù)圖，結(jié)合貝葉斯濾波算法實(shí)現(xiàn)慣性PDR軌跡的有效約束。

同式(21)類似，先建立估計(jì)模型

xk=f(xk-1)+wk-1

(23)

zk=h(xk)+vk

(24)

這里，下標(biāo)k表示時(shí)標(biāo)；xk∈Rn表示待估計(jì)的狀態(tài)向量；zk∈Rn表示含噪聲的測(cè)量向量；w∈Rn表示過(guò)程噪聲；v∈Rm表示測(cè)量噪聲；函數(shù)f和h是非線性函數(shù)。式(23)為預(yù)測(cè)模型(時(shí)間更新)，用于表示行人的運(yùn)動(dòng)；式(24)為獨(dú)立的測(cè)量模型(測(cè)量更新)，用于校準(zhǔn)更新。式(23)和式(24)相互配合，利用獨(dú)立的觀測(cè)量zk來(lái)估計(jì)狀態(tài)量xk。然后采用地圖的柵格圖建立行人活動(dòng)區(qū)域的概率密度函數(shù)p(xk|FP)，密度水平與行人在相應(yīng)位置的行走可能性成正比，如圖7所示。

(a)室內(nèi)柵格地圖 (b) 提取行人室內(nèi)活動(dòng)區(qū)域 (c) 概率密度灰度圖偽彩色圖像

根據(jù)圖7中給出的行人活動(dòng)區(qū)域概率密度圖，可以有效約束基于慣性遞推原理的室內(nèi)行人行走方向。圖8所示為根據(jù)圖7中的概率密度圖和基于慣性遞推原理的行人定位結(jié)果得到室內(nèi)行人似然位置的過(guò)程。圖8(a)中的拱形二維高斯分布表示概率密度圖和慣性遞推定位結(jié)果概率疊加的過(guò)程;圖8(b)中紅色圓區(qū)域?yàn)楦鶕?jù)慣性遞推似然值和概率密度圖得到的當(dāng)前行人出現(xiàn)的最大概率區(qū)域，顏色最深的地方為當(dāng)前時(shí)刻行人最有可能出現(xiàn)的位置;如圖8(c)所示，白色軌跡為純慣性PDR遞推軌跡，紅色區(qū)域?yàn)楦鶕?jù)地圖信息提取的行人可能活動(dòng)區(qū)域，綠色軌跡為采用上述非回溯貝葉斯濾波算法得到的行人定位軌跡。可以看到,軌跡被很好地約束到了行人應(yīng)該出現(xiàn)的區(qū)域內(nèi)。

圖9所示為作者參加2019年意大利比薩IPIN2019國(guó)際室內(nèi)定位與導(dǎo)航大賽時(shí)，自測(cè)試階段采用上述非回溯貝葉斯濾波算法處理穿戴式PDR比賽數(shù)據(jù)得到的室內(nèi)行人定位軌跡，測(cè)試路線長(zhǎng)約1.9～2km，橫跨多個(gè)室內(nèi)大樓，并穿越了三個(gè)樓層，且樓層之間有自動(dòng)扶梯和樓梯切換。處理之后得到的軌跡75% 累積分布函數(shù)(Cumulative Distribution Function，CDF)誤差在2m以內(nèi)，誤差/軌跡長(zhǎng)約為1‰。因此，采用基于地圖輔助的非回溯貝葉斯濾波算法得到的慣性PDR行人定位軌跡的精度和可靠性較高。

(a) (b) (c)

圖9 2019年意大利比薩IPIN國(guó)際室內(nèi)定位比賽數(shù)據(jù)分析結(jié)果(紅線為解算后的測(cè)試軌跡)

2 基于慣性遞推原理的行人定位方法存在問(wèn)題及發(fā)展方向

從目前基于慣性遞推原理的行人定位的發(fā)展情況來(lái)看，面臨的主要問(wèn)題和發(fā)展方向如下。

2.1 提高M(jìn)EMS慣性器件的精度及可靠性

從MEMS慣性器件角度來(lái)看，隨著目前MEMS技術(shù)的長(zhǎng)足發(fā)展，基于MIMU傳感器的行人自主定位方法已取得了很大的進(jìn)展，但由于受到行人復(fù)雜運(yùn)動(dòng)特性及各種應(yīng)用環(huán)境的影響，目前MIMU傳感器的精度和可靠性并不能完全滿足行人自主定位的需求，基于MIMU傳感器的行人定位方法的普適性和可靠性仍需加強(qiáng)。

因此，從根本上大幅度提高M(jìn)EMS慣性器件本身的精度和可靠性，有效降低慣性遞推過(guò)程中累積誤差的影響，同時(shí)結(jié)合行人運(yùn)動(dòng)過(guò)程中必要的運(yùn)動(dòng)約束信息和環(huán)境特征信息，有效提高行人自主定位的普適性和可靠性，這將是未來(lái)基于慣性遞推原理的行人自主定位技術(shù)的主要發(fā)展方向。

2.2 改進(jìn)MEMS器件測(cè)量誤差和算法模型誤差估計(jì)方法

基于MIMU傳感器的行人定位方法中，誤差來(lái)源主要有兩類：1)低成本MEMS傳感器測(cè)量誤差；2)算法模型誤差。低成本MIMU 器件測(cè)量誤差并不穩(wěn)定，尤其在行人行進(jìn)過(guò)程中，受振動(dòng)、腳落地后微小旋轉(zhuǎn)等因素的影響，MIMU測(cè)量誤差會(huì)發(fā)生變化；同時(shí)算法模型誤差(通常被設(shè)定為零均值白噪聲)并未真實(shí)反映實(shí)際行人運(yùn)動(dòng)過(guò)程中的真實(shí)誤差。因此，正確估計(jì)MIMU傳感器測(cè)量誤差以及設(shè)定正確的算法模型誤差，對(duì)于行人定位至關(guān)重要。

文獻(xiàn)[51-52]均對(duì)MEMS傳感器的器件測(cè)量誤差進(jìn)行了有效建模估計(jì)，主要采用的方法是測(cè)量前靜止分析和利用Kalman濾波估計(jì)測(cè)量過(guò)程中MEMS傳感器的測(cè)量誤差；同時(shí)，文獻(xiàn)[22]指出，由于ZUPT方法自身算法模型的限制性，特別是在行人多種運(yùn)動(dòng)情況下，該算法自身也會(huì)引入額外的模型誤差，導(dǎo)致解算結(jié)果出現(xiàn)漂移。目前，一些學(xué)者認(rèn)為應(yīng)該采用更加復(fù)雜準(zhǔn)確的方法估計(jì)行人行進(jìn)過(guò)程中的系統(tǒng)模型誤差，從而降低誤差影響，提高定位結(jié)果長(zhǎng)時(shí)間的準(zhǔn)確性和穩(wěn)定性；另外一些學(xué)者則認(rèn)為不應(yīng)該估計(jì)傳感器誤差和算法模型誤差，因?yàn)檫@些誤差會(huì)隨著環(huán)境和行走狀態(tài)發(fā)生變化，如果去估計(jì)這些誤差可能會(huì)再次引入額外誤差。

所以，在行人行進(jìn)過(guò)程中，如何有效估計(jì)和約束傳感器誤差是目前穿戴式PDR面臨的主要問(wèn)題。

2.3 提高零速階段檢測(cè)自適應(yīng)性和準(zhǔn)確性

受行人復(fù)雜運(yùn)動(dòng)特性的影響，零速階段往往檢測(cè)不準(zhǔn)確，導(dǎo)致在非靜止階段也會(huì)修正IMU慣性解算，從而引入誤差，使得最終行人位置解算結(jié)果發(fā)散；目前常用的檢測(cè)方法主要是1.1.2節(jié)中所述四種方法的變形與結(jié)合。這些方法的共同點(diǎn)在于均需要設(shè)定零速檢測(cè)的判定閾值，然而隨著行人運(yùn)動(dòng)狀態(tài)(正常行走、跑步、上下樓等)的變化，這些閾值也會(huì)隨之發(fā)生顯著變化，如果設(shè)定的閾值過(guò)小或者過(guò)大，均會(huì)導(dǎo)致零速階段的誤檢測(cè)。

另如文獻(xiàn)[53-54]中，提出了一些自適應(yīng)閾值零速區(qū)間檢測(cè)算法。主要區(qū)別在于，這些算法可以根據(jù)行人運(yùn)動(dòng)特性和閾值之間的近似關(guān)系，自適應(yīng)地改變檢測(cè)閾值。然而在實(shí)際應(yīng)用過(guò)程中，對(duì)于行人產(chǎn)生的各種隨機(jī)運(yùn)動(dòng)并不明顯。同時(shí)，自適應(yīng)模型算法并不普遍適用于不同的人。近年來(lái)，提出了基于人工智能(Artificial Intelligence，AI)的零速檢測(cè)方法來(lái)滿足變動(dòng)行人運(yùn)動(dòng)狀態(tài)下零速區(qū)間的檢測(cè)[55]。 這些方法采用運(yùn)動(dòng)分類和深度學(xué)習(xí)方法，無(wú)需調(diào)整零速檢測(cè)閾值即可實(shí)現(xiàn)零速區(qū)間的有效檢測(cè)[56]；但需要大量的訓(xùn)練數(shù)據(jù)，從而增加了計(jì)算成本、降低了系統(tǒng)的實(shí)時(shí)性。

因此，如何實(shí)現(xiàn)對(duì)行人運(yùn)動(dòng)過(guò)程中零速階段的準(zhǔn)確檢測(cè)和判斷，仍是目前穿戴式PDR面臨的主要問(wèn)題。

2.4 優(yōu)化慣性遞推PDR多源信息融合方法

目前，基于MIMU傳感器的穿戴式PDR定位方法，主要采用以MIMU慣性遞推為主，輔以行人運(yùn)動(dòng)規(guī)律中多種信息約束的方式，實(shí)現(xiàn)行人長(zhǎng)時(shí)間自主定位；或者采用環(huán)境中的信息源(地磁、地圖等)約束MIMU慣性解算，從而提高行人長(zhǎng)時(shí)間定位結(jié)果的可靠性。根據(jù)目前的發(fā)展情況，從信息融合結(jié)構(gòu)和信息源方面分析結(jié)果如下：

信息融合結(jié)構(gòu)方面：目前主流的方法主要在IMU解算后的位置域，采用松組合結(jié)構(gòu)進(jìn)行ZUPT和Kalman濾波。文獻(xiàn)[57]中介紹了經(jīng)典的ZUPT輔助的行人定位算法信息融合結(jié)構(gòu)，即：利用零速觀測(cè)量+Kalman濾波松組合結(jié)構(gòu)，在解算結(jié)果層修正IMU慣性遞推位置結(jié)果；文獻(xiàn)[58-59]提出了利用緊組合模型在IMU原始數(shù)據(jù)層進(jìn)行融合，將多個(gè)IMU原始?xì)w算成一個(gè)IMU，利用信息冗余優(yōu)勢(shì)構(gòu)成多傳感器間的協(xié)同，從而提高定位結(jié)果的準(zhǔn)確性和穩(wěn)定性。

信息源方面：文獻(xiàn)[40]通過(guò)采集行人運(yùn)動(dòng)過(guò)程中腳部的三軸加速度、三軸陀螺儀和三軸磁場(chǎng)信息，進(jìn)行有效融合后實(shí)現(xiàn)行人行進(jìn)過(guò)程中的可靠定位；文獻(xiàn)[60]將零角速度變化模型(ZARU)應(yīng)用于航向誤差漂移修正；文獻(xiàn)[61]將建筑物航向模型(HDR)和磁航向應(yīng)用于航向誤差漂移修正；文獻(xiàn)[62]同時(shí)利用兩個(gè)傳感器，將MEMS傳感器同時(shí)固定在口袋和腳尖部位，采集行人行進(jìn)過(guò)程中兩個(gè)部位的運(yùn)動(dòng)信息，通過(guò)傳感器之間的協(xié)同，實(shí)現(xiàn)系統(tǒng)航向的有效約束；文獻(xiàn)[63]同時(shí)將五個(gè)MEMS固定在同一只腳上，構(gòu)成多傳感器信息冗余，提高了系統(tǒng)定位結(jié)果的可靠性；文獻(xiàn)[64]同時(shí)將兩個(gè)IMU分別固定在兩只腳上利用雙腳信息構(gòu)成協(xié)同，以約束系統(tǒng)航向。

目前穿戴式PDR定位方法中，約束信息源眾多，但約束效果并不穩(wěn)定。因此，如何對(duì)目前的信息融合方法進(jìn)行有效的優(yōu)化與整合，對(duì)于實(shí)現(xiàn)室內(nèi)行人長(zhǎng)時(shí)間可靠定位具有重要作用。

2.5 基于應(yīng)用場(chǎng)景特點(diǎn)設(shè)計(jì)不同性價(jià)比終端和算法

基于慣性遞推原理的穿戴式行人定位終端應(yīng)用的場(chǎng)景包括：1)特殊人員的定位，如：消防員應(yīng)急救援定位、單兵叢林/城市協(xié)同作戰(zhàn)定位等，這些場(chǎng)景下對(duì)定位終端的性能要求較高，因此需要采用成本較高、性能較好的慣性終端；同時(shí)，由于應(yīng)用環(huán)境的復(fù)雜性，所設(shè)計(jì)的算法既要保證定位精度，也應(yīng)具備較高的魯棒性。2)大眾用戶的定位，如：大型商場(chǎng)內(nèi)的人員定位、老人或者盲人的監(jiān)督定位等，這些場(chǎng)景下對(duì)定位終端的性能要求不高，應(yīng)采用較低成本的慣性終端，且應(yīng)用場(chǎng)景較單一， 應(yīng)降低設(shè)計(jì)算法的復(fù)雜度，控制定位終端的整體成本，滿足大眾用戶的普及應(yīng)用。

因此，需要根據(jù)不同的應(yīng)用場(chǎng)景，設(shè)計(jì)不同性價(jià)比的終端和算法，以滿足行業(yè)用戶和大眾用戶的需求。

3 總結(jié)

本文對(duì)基于慣性遞推原理的行人自主定位方法進(jìn)行了綜述分析：1)重點(diǎn)分析了行人運(yùn)動(dòng)特性分析與檢測(cè)、MIMU慣性誤差分析和校準(zhǔn)、慣性PDR解算方法、運(yùn)動(dòng)約束方法、信息融合算法；2)分析了目前該方法存在的問(wèn)題及發(fā)展方向；3)總體來(lái)看，目前從器件成本和穩(wěn)定性、定位算法的精度和魯棒性、不同應(yīng)用場(chǎng)景的適用性，都有提升的空間，單純基于慣性遞推原理的行人定位方法并不能滿足各種復(fù)雜應(yīng)用場(chǎng)景的需要；4)未來(lái)，針對(duì)不同行業(yè)用戶和大眾用戶的特點(diǎn)，設(shè)計(jì)不同性價(jià)比的終端和融合約束算法將是該領(lǐng)域的應(yīng)用趨勢(shì)。