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

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

0 引言

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

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

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

1 算法總體框架

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

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

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

2 基于虛擬地標(biāo)的誤差補(bǔ)償方法

2.1 虛擬地標(biāo)的提取

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

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

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

(1)

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

(2)

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

Lposi=(xk,yk)，k取當(dāng)Δφk最大時(shí)

(3)

如式(3)所示，變化值最大的時(shí)刻所在的位置點(diǎn)識別為虛擬地標(biāo)的位置Lposi。

2.2 虛擬地標(biāo)的匹配

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

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

圖3 虛擬地標(biāo)匹配示意圖Fig.3 Virtual landmark matching schematic diagram

因此，當(dāng)2個(gè)不同的虛擬路標(biāo)點(diǎn)間的直線距離滿足二維高斯分布的方差要求時(shí)，則認(rèn)為2個(gè)虛擬路標(biāo)點(diǎn)對應(yīng)同一真實(shí)路標(biāo)點(diǎn)，此過程稱為虛擬路標(biāo)點(diǎn)的匹配。

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

圖4 虛擬地標(biāo)篩選算法Fig.4 The algorithm of choosing virtual landmark

2.3 基于虛擬地標(biāo)的誤差補(bǔ)償方法

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

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

(4)

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

(5)

當(dāng)虛擬地標(biāo)匹配成功時(shí)，位置誤差的數(shù)學(xué)模型為

(6)

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

e=AM-U

(7)

其中

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

?Mop=(ATA)-1ATU

(8)

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

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

φ′=

(9)

φn=φ′+Δφo

(10)

3 基于虛擬地標(biāo)的慣性SLAM方法

3.1 慣性概率地圖

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

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

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

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

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

(11)

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

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

(12)

(13)

3.2 基于虛擬地標(biāo)的慣性SLAM算法

由3.1節(jié)的假設(shè)可知，地圖的估計(jì)僅與位置的時(shí)間序列相關(guān)，又因?yàn)閷⒌貓D劃分成六邊形柵格，對于地圖的估計(jì)可以表示為地圖中每個(gè)六邊形柵格的估計(jì)的乘積，即

(14)

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

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

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

4 實(shí)驗(yàn)

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

圖7 實(shí)驗(yàn)設(shè)計(jì)路線圖Fig.7 The experiment environment and designed path

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

(a)虛擬地標(biāo)提取

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

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

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

(a)算法效果對比圖

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

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

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

利用本文的虛擬地標(biāo)提取方法，對行人慣性里程計(jì)原始軌跡和兩種算法處理過的軌跡進(jìn)行處理，得到表1所示每條軌跡每圈中兩點(diǎn)的對應(yīng)地標(biāo)點(diǎn)位置坐標(biāo)。

表1 用于誤差評估的虛擬地標(biāo)點(diǎn)位置

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

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

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

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

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

5 結(jié)論

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

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