基于虛擬地標的行人慣性SLAM算法

曹志國，熊 智，丁一鳴，李婉玲，王鉦淳

(南京航空航天大學自動化學院, 南京 211106)

0 引言

隨著社會的飛速發(fā)展，日常生活以及國防等領(lǐng)域?qū)π腥藢Ш较到y(tǒng)的需求日益增加。在室內(nèi)環(huán)境下，衛(wèi)星信號由于遮擋和多路徑效應不可用，無法滿足室內(nèi)定位的精度需求。因此，迫切需要研究一種不依賴全球衛(wèi)星導航系統(tǒng)(Global Navigation Satellite System, GNSS)的室內(nèi)行人導航技術(shù)。隨著微機電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)技術(shù)的發(fā)展[1]，MEMS慣性測量單元(Inertial Measurement Unit，IMU)憑借體積小、功耗低、質(zhì)量小、便于攜帶等優(yōu)點，使得以MEMS-IMU為基礎(chǔ)的室內(nèi)行人導航方法得到了廣泛的關(guān)注和研究[2-4]。盡管如此，基于低成本IMU的行人慣性導航系統(tǒng)航向角發(fā)散[5-6]這一關(guān)鍵問題尚未完全解決。

為了抑制行人慣性導航系統(tǒng)的航向角發(fā)散，國內(nèi)外學者已經(jīng)提出了許多解決方法，其中具有代表性的算法主要有三種[7]，即啟發(fā)式漂移補償方法、基于預設特征的方法和基于同時定位與建圖(Simultaneous Localization and Mapping，SLAM)的方法。啟發(fā)式漂移補償方法通常假定行人在室內(nèi)沿著平行建筑物內(nèi)墻的方向行走[8-10]，此類方法的缺點在于需要先驗地圖信息，而先驗地圖信息往往難以獲得?；诘貥说姆椒ㄍㄟ^將行人運動過程中采集到的WiFi或磁信號與事先建立的指紋庫進行匹配，輔助行人導航系統(tǒng)定位[11-12]，而這類指紋庫需要提前部署設備，且需要進行維護。SLAM技術(shù)是指裝載單一或多種傳感器的運動主體，在沒有環(huán)境先驗信息的條件下，在運動過程中構(gòu)建環(huán)境地圖的同時估計自身的實時運動[13]。近年來，隨著計算機硬件處理能力的快速提升，SLAM技術(shù)發(fā)展迅速，目前已經(jīng)有多種不同框架的SLAM方法[14-15]。其中，P.Robertson等[16]將二維平面劃分成緊密拼接的六邊形柵格，在估計行人位置的同時對未知環(huán)境構(gòu)建了六邊形網(wǎng)格地圖，實現(xiàn)了僅依靠慣性傳感器探索未知建筑物。該方法雖然可以在一定程度上減弱航向角的漂移，但通常計算量較大，難以滿足實時定位的需求。

針對以上不足，本文提出了一種新的基于虛擬地標的行人慣性SLAM算法，通過從軌跡中提取并匹配虛擬地標點，求解最小誤差位置，并以此為條件補全虛擬地標點間的地圖，即建筑中可行走的通路，從而替換傳統(tǒng)SLAM方法逐步建立的稠密地圖，以降低傳統(tǒng)SLAM算法建圖的迭代次數(shù)和未回環(huán)的無效優(yōu)化，提高了全局位置和地圖估計精度，消除了純慣性行人定位系統(tǒng)的累積誤差，實現(xiàn)了純慣性行人定位系統(tǒng)長時間穩(wěn)定可靠導航。

1 算法總體框架

如圖1所示，本文提出的基于虛擬地標的行人慣性SLAM算法分為兩部分，分別為基于虛擬地標的誤差補償方法和基于慣性概率地圖的SLAM方法。

圖1 基于虛擬地標的慣性SLAM算法框架Fig.1 Inertial SLAM algorithm based on virtual landmark

本文提出的基于虛擬地標的誤差補償方法，以基于慣性概率地圖的SLAM方法得出的位置估計為輸入，通過從估計軌跡中提取并匹配虛擬地標點，求解最小誤差位置，作為建圖的依據(jù)。傳統(tǒng)的基于慣性概率地圖的SLAM方法，在每一步更新位置的同時對地圖進行更新，導致計算量大且收斂速度較慢。因此，本文提出的算法對此進行了簡化，將虛擬地標點視為特征點，只在特征點處更新地圖，而其他位置的地圖則根據(jù)前述的最小誤差位置進行補全，在保證精度的同時降低了計算量。

2 基于虛擬地標的誤差補償方法

2.1 虛擬地標的提取

不同于傳統(tǒng)意義上需要提前布置設備的地標，虛擬地標從行人慣性里程計輸出的航跡中提取。本文研究只針對單層建筑中的行人慣性導航，在單層建筑中，直角轉(zhuǎn)角點是最顯著的特征，因此利用慣性傳感器的信號辨識直角轉(zhuǎn)角點，即本文所述的虛擬地標點。如圖2所示，藍色線即為行人航向角的估計值，紅色線圈出的即為直角轉(zhuǎn)角過程，可以看出，轉(zhuǎn)彎動作開始前的穩(wěn)定航向角值與完成轉(zhuǎn)彎動作后的穩(wěn)定航向角值的差值在90°左右，但轉(zhuǎn)角的動作通常不在一步之內(nèi)完成，且由于傳感器的測量誤差，導致轉(zhuǎn)角的航向變化值會有波動。

圖2 航向角變化曲線圖Fig.2 The yaw angle date during walking

因此，本文提出了一種自適應滑動窗口方法，以辨識行人航跡中的直角轉(zhuǎn)角點。根據(jù)行人在轉(zhuǎn)彎過程中航向角變化值持續(xù)較大的特點，本文先通過設置轉(zhuǎn)彎狀態(tài)閾值判斷行人是否處于轉(zhuǎn)彎狀態(tài)，具體計算方法如式(1)所示

(1)

其中，Sk為表征行人k時刻是否處于轉(zhuǎn)彎狀態(tài)的狀態(tài)量，為1時表示行人處于轉(zhuǎn)彎狀態(tài)，反之則不處于；Δφk表示k時刻航向角的變化量；φ1表示轉(zhuǎn)彎狀態(tài)閾值，根據(jù)多次實驗發(fā)現(xiàn)，選取φ1=10°較為合適。滑動窗口的長度為轉(zhuǎn)彎狀態(tài)量連續(xù)為1的區(qū)間長度，由行人進入轉(zhuǎn)彎狀態(tài)到轉(zhuǎn)彎狀態(tài)結(jié)束的時間決定。對滑動窗口內(nèi)的所有航向角變化量進行累加，即可得到完整轉(zhuǎn)彎的航向角變化，如式(2)所示

(2)

其中，φsum即為航向角的完整變化量，若φsum落在[80°,100°]的閾值區(qū)間，則認為滑動窗口內(nèi)為直角轉(zhuǎn)角過程，且將被判定為直角轉(zhuǎn)角過程中的滑動窗口內(nèi)的航向角變化序列記錄下來

Lposi=(xk,yk)，k取當Δφk最大時

(3)

如式(3)所示，變化值最大的時刻所在的位置點識別為虛擬地標的位置Lposi。

2.2 虛擬地標的匹配

由于行人慣性里程計輸出的位置存在累積誤差，基于行人慣性里程計輸出的位置提取出的虛擬地標位置同樣存在累積誤差，即同一真實轉(zhuǎn)角點對應的虛擬地標點位置不同。因此,需要對上述的虛擬地標點進行匹配和篩選。

為了進行虛擬地標的匹配，本文認為通過行人慣性里程計識別出的虛擬路標點的位置是對建筑物中真實轉(zhuǎn)角點位置的觀測，服從以真實轉(zhuǎn)角點位置為期望、行人慣性里程計位置誤差為方差的二維高斯分布[7]，如圖3所示，其中紅色點為真值點，黑色圓圈表示二維高斯分布的范圍。

圖3 虛擬地標匹配示意圖Fig.3 Virtual landmark matching schematic diagram

因此，當2個不同的虛擬路標點間的直線距離滿足二維高斯分布的方差要求時，則認為2個虛擬路標點對應同一真實路標點，此過程稱為虛擬路標點的匹配。

本文采用的匹配方法依賴閾值的選取，因此為了盡可能避免匹配失敗和誤匹配的情況出現(xiàn)，除了多次實驗選取合適的閾值外，本文還針對短距離內(nèi)出現(xiàn)多個轉(zhuǎn)角的復雜情況，設置了虛擬路標點的步數(shù)區(qū)間閾值Lsth1。因為行人慣性里程計的誤差具有隨時間緩慢發(fā)散的特點，短距離內(nèi)的匹配對定位精度的提升有限，所以當2個虛擬路標點對應的步數(shù)編號小于步數(shù)區(qū)間閾值時，則只存儲先識別出的虛擬路標點，如圖4所示，對虛擬地標的篩選可以在降低匹配難度的同時減小誤差補償?shù)挠嬎懔俊?/p>

圖4 虛擬地標篩選算法Fig.4 The algorithm of choosing virtual landmark

2.3 基于虛擬地標的誤差補償方法

光束平差法是現(xiàn)有視覺SLAM系統(tǒng)求解三維空間坐標和相機位姿普遍采用的方法。雖然本文使用的傳感器為慣性傳感器，但同樣需要對位置誤差進行處理，因此，本文將光束平差法的思想引入基于虛擬地標的誤差補償中。

本文首先對誤差進行了數(shù)學建模。本文提出的誤差補償方法以基于行人航位推算算法的行人慣性里程計的輸出為輸入，因此采用步長航向模型，同時為了簡化計算，將位置誤差近似成高斯分布，可以得到如下的運動學模型

(4)

其中，k表示步數(shù)的編號;Lk表示第k步的步長;φk表示第k步的航向;ex,k和ey,k為步長和航向估計不準確引起的位置誤差。為了進一步簡化位置誤差的數(shù)學模型，令ux,k=Lkcosφk，uy,k=Lksinφk，則位置誤差數(shù)學模型簡化為

(5)

當虛擬地標匹配成功時，位置誤差的數(shù)學模型為

(6)

根據(jù)上述數(shù)學模型，可對行人慣性里程計輸出的步長和航向信息建立線性方程組

e=AM-U

(7)

其中

其中，假設(x0,y0)和(x3,y3)為匹配的2個虛擬地標點，則第7、8行為虛擬地標匹配時所增加的行，其余行以此類推，為了使得誤差最小，即

?Mop=(ATA)-1ATU

(8)

其中，Mop為使得全局位置誤差最小的位置矢量，也即是誤差補償后的位置矢量。

由上述算法的特性可知，在虛擬地標匹配計算后，虛擬地標位置點之前的路徑都會得到修正，即可用修正后的位置求得修正后的航向角，如式(9)所示

φ′=

(9)

φn=φ′+Δφo

(10)

3 基于虛擬地標的慣性SLAM方法

3.1 慣性概率地圖

由于虛擬地標點的稀疏性，前述方法只在虛擬地標這些離散點有效，而無法作用于行人行走的全過程。為了解決該問題，本文引進了基于慣性概率地圖的方法。

基于慣性概率地圖的方法最早由德國宇航中心的P.Robertson等[16]提出，該方法首先將行人慣性導航問題建模成動態(tài)貝葉斯網(wǎng)絡，如圖5所示。

圖5 行人慣性導航動態(tài)貝葉斯網(wǎng)絡圖Fig.5 Pedestrian inertial navigation dynamic Bayesian network

其中，P為位置變量；U為步長矢量；Z為IMU的量測；Map為地圖變量。

根據(jù)動態(tài)貝葉斯網(wǎng)絡模型，行人慣性導航問題可以建模成SLAM問題，即將慣性傳感器測量到的數(shù)據(jù)視為觀測，估計以該觀測為條件的其他狀態(tài)量的聯(lián)合概率分布。針對SLAM問題已有多種較為成熟的框架，作為其中一種基于粒子濾波器的SLAM框架，F(xiàn)astSLAM將地圖看成由許多不同地標組成，并假設在已知機器人運動路徑的前提下，2個不同地標的估計條件獨立。因此，行人慣性導航的概率估計可分解為

(11)

根據(jù)式(11)，位置和地圖的聯(lián)合條件分布可以分解成構(gòu)圖問題和定位問題。針對構(gòu)圖問題，可將二維地圖柵格化，可以使用三角形、矩形或六邊形作為柵格形狀，而在60°的離散角度分割下可認為行人各個方向行走的概率是獨立的，所以將柵格形狀選為六邊形。針對定位問題，利用動態(tài)貝葉斯網(wǎng)絡的特點，可分解為

p({PU}k|{PU}0:k-1)·

(12)

(13)

3.2 基于虛擬地標的慣性SLAM算法

由3.1節(jié)的假設可知，地圖的估計僅與位置的時間序列相關(guān)，又因為將地圖劃分成六邊形柵格，對于地圖的估計可以表示為地圖中每個六邊形柵格的估計的乘積，即

(14)

其中，h(h=0,1,...,NH-1)表示六邊形柵格的編號。傳統(tǒng)的FootSLAM算法中,地圖與位置序列實時相關(guān)，而本文使用基于虛擬地標的增量稀疏地圖代替實時稠密地圖，僅在轉(zhuǎn)角點處更新地圖，兩轉(zhuǎn)角點間的地圖可根據(jù)2.3節(jié)中求得的最小誤差矢量進行補齊。而3.1節(jié)定位問題的推導只用了六邊形柵格各條邊概率滿足狄利克雷分布的假設，本文依然采用六邊形柵格，故3.1節(jié)的推導仍然成立。算法流程如圖6所示。

圖6 基于虛擬地標的慣性SLAM算法流程圖Fig.6 Inertial SLAM algorithm based on virtual landmark

傳統(tǒng)的FastSLAM算法通過粒子濾波器實現(xiàn)，由于本文算法是基于FastSLAM算法框架的改進，因此本文算法也使用了粒子濾波器。本文算法的輸入為行人慣性里程計每一步輸出的步長和航向變化量，首先進行建議分布采樣，通過對航向變化量加入高斯白噪聲實現(xiàn)，由此生成一定數(shù)量的粒子；然后進行粒子權(quán)重更新，根據(jù)當前粒子的步長矢量與柵格地圖的穿邊關(guān)系更新粒子的權(quán)重；更新權(quán)重后進行位置估計，即對所有粒子按照概率分布求得期望；求得期望之后，要判斷粒子方差是否超出重采樣閾值，若超出則認為權(quán)重小的粒子偏離估計值，因此舍棄權(quán)重較小的粒子，復制權(quán)重大的粒子進行補充，以保持粒子的總數(shù)目不變。同時，將前述求得的期望位置作為已知軌跡輸入到基于虛擬地標的誤差補償中，若此時檢測出虛擬地標，且與之前行走過程中存儲的虛擬地標匹配，則進行全局誤差估計，并更新慣性概率地圖，否則不對慣性概率地圖進行更新。這樣在不損失地圖對可通行區(qū)域的表達的前提下，簡化了地圖的構(gòu)建過程，同時也減少了非必要的粒子更新計算。

4 實驗

為了驗證基于虛擬地標的慣性SLAM算法的有效性，在南京航空航天大學自動化學院的5樓進行了室內(nèi)實驗。如圖7所示，該路線全長約1214m，包括3個房間的行人路徑，起始位置和結(jié)束位置是同一點。

圖7 實驗設計路線圖Fig.7 The experiment environment and designed path

本文使用的慣性里程計[17]經(jīng)過行人航位推算算法處理后輸出的每一步的航向和步長數(shù)據(jù)為本文算法的輸入，其位置誤差約為5%，航向角漂移約為5.4(°)/min，由于行人慣性里程計初始處理過程不是本文要討論的內(nèi)容，故不再進行詳細闡述。

(a)虛擬地標提取

(b)虛擬地標篩選圖8 基于行人慣性里程計原始軌跡的虛擬地標提取Fig.8 Virtual landmark extraction based on original trajectory of pedestrian odometer

如圖8所示，藍色線為在上述實驗路線下重復5圈的行人慣性里程計原始軌跡，紅色圓圈所在位置為本文算法識別出的虛擬地標點。從圖8中可以看出，本文的虛擬地標識別方法準確識別出了5圈軌跡中的共計69個直角轉(zhuǎn)角點，本文算法對識別出的虛擬地標點進行了進一步的篩選，結(jié)果如圖8(b)所示。

從圖8中可以看出，通過2.2節(jié)所述對較小范圍內(nèi)出現(xiàn)的多個轉(zhuǎn)角點進行篩選，圖8(a)紅色圈中的4個地標點只保留圖8(b)紅色圈中的1個。

(a)算法效果對比圖

(b)慣性柵格概率地圖圖9 兩種算法實驗結(jié)果Fig.9 Experiment results of two algorithms

根據(jù)篩選出的30個虛擬地標點，采用基于虛擬地標的誤差補償方法計算得到圖9(a)中的綠色曲線，即為修正后的5圈軌跡。在實驗中發(fā)現(xiàn)，基于虛擬地標的誤差補償方法只在虛擬地標點處進行全局修正，導致行人行走于離散虛擬地標點間時誤差仍隨時間發(fā)散，而基于虛擬地標的SLAM算法則可以解決這一問題。圖9(a)中的紅色曲線為基于虛擬地標的SLAM算法計算得到的軌跡，可以看出，基于虛擬地標的SLAM算法得到的曲線更加平滑，且誤差更小。圖9(b)為基于虛擬地標的慣性SLAM算法得到的六邊形地圖，六邊形的邊顏色表示該邊的穿邊概率，可以看出，六邊形柵格概率高的邊組成的通路與真實環(huán)境一致。

為了定量比較兩種算法的效果，本文需要選擇相應的位置點計算軌跡誤差。由于圖9(a)中紫色圈中的兩點(0,0)和(61.8,32.8)是實驗路徑中距離最長的兩點，且這兩點沒有用于補償算法的計算，因此本文選擇這兩點作為計算的基準點。

利用本文的虛擬地標提取方法，對行人慣性里程計原始軌跡和兩種算法處理過的軌跡進行處理，得到表1所示每條軌跡每圈中兩點的對應地標點位置坐標。

表1 用于誤差評估的虛擬地標點位置

計算每圈的位置誤差，如圖10所示。從圖10中可以看出，行人慣性里程計原始軌跡的位置誤差隨時間發(fā)散，而兩種算法修正后的軌跡位置誤差穩(wěn)定在一定范圍內(nèi)，其中，基于虛擬地標的SLAM算法的位置誤差要優(yōu)于僅基于虛擬地標的算法。

圖10 位置誤差變化曲線Fig.10 The curve of position error with the number of rounds

圖10所示為所有誤差評估點的平均誤差變化，而在行人定位算法中也常用終點位置誤差來評估精度，若僅考慮終點位置誤差，三者分別為55.20m、4.08m和1.61m，相對誤差分別為4.6%、0.3%和0.1%，可以看出，基于虛擬地標的SLAM算法仍然是精度最高的。

基于虛擬地標的慣性SLAM算法中采用了基于蒙特卡羅方法的粒子濾波器，具有一定的隨機性，故本文對上述行人慣性里程計的數(shù)據(jù)進行了100次的重復處理，并計算了這100次處理的5圈位置誤差的平均值，如圖11所示。其中，基于虛擬地標的慣性SLAM算法平均位置誤差始終小于10m。而由圖10可知，僅基于虛擬地標的誤差補償算法的平均位置誤差為11.1m，因此基于虛擬地標的慣性SLAM算法的平均位置誤差始終優(yōu)于僅基于虛擬地標的誤差補償算法，且其小于9m的概率為97%。

圖11 基于虛擬地標的慣性SLAM算法位置誤差分布Fig.11 Position error distribution of inertial SLAM algorithm based on virtual landmark

5 結(jié)論

針對當前僅使用MIMU的行人導航系統(tǒng)航向角存在較大累積誤差的問題，提出了基于虛擬地標的慣性SLAM算法。本文算法可以在行人行走過程中，利用行人慣性里程計輸出的航向和步長信息識別出虛擬地標點，即直角轉(zhuǎn)角點，當行人重復經(jīng)過同一直角轉(zhuǎn)角點時，即可對軌跡進行修正，同時對航向角的漂移誤差進行補償。在此基礎(chǔ)上，引入SLAM的思想，在估計位置的同時建立六邊形柵格地圖，進一步提高了算法的估計精度和實用性。最后，通過實驗驗證了本文算法在2027.04m2的單層建筑物中空間位置誤差小于10m。

本文提出的基于虛擬地標點的慣性SLAM算法尚具有以下局限性：1)虛擬地標點僅限于直角轉(zhuǎn)角點，算法只適用于處理二維平面的運動；2)基于虛擬地標的誤差補償方法構(gòu)造的最小二乘方程計算復雜度隨時間遞增。在后續(xù)的研究中，將進一步豐富虛擬地標點的種類，將算法推廣到三維空間，且對最小二乘方程的計算進行優(yōu)化，降低其計算復雜度，同時將基于虛擬地標點的慣性SLAM算法應用于行人實時導航中。