李磊磊，王藝翔，桂訓(xùn)雅，陳家斌，黎祖成，韓勇強(qiáng)

（北京理工大學(xué) 自動(dòng)化學(xué)院，北京 100081）

單目相機(jī)與IMU 組合的視覺/慣性定位系統(tǒng)[1]具有低成本、高自主性的特點(diǎn)。相對(duì)于純視覺系統(tǒng)，由于IMU 的加入，可以獲取尺度、橫滾角與俯仰角的觀測(cè)量，并且IMU 對(duì)角速度與線加速度預(yù)積分的值能夠在紋理較少的場(chǎng)景校正視覺特征跟蹤解算值，提升系統(tǒng)的穩(wěn)定性與可靠性。但是環(huán)境場(chǎng)景中光線的變化、前端初始化提供初始值的不同、運(yùn)行過程中特征跟蹤失敗等問題都會(huì)影響系統(tǒng)整體精度，并且系統(tǒng)長(zhǎng)時(shí)間運(yùn)行會(huì)產(chǎn)生累積誤差，使得系統(tǒng)估計(jì)值不再可靠。利用先驗(yàn)信息正是減小系統(tǒng)累積誤差，提高系統(tǒng)穩(wěn)定性的一種有效的方法。

近年來，基于地圖的視覺定位一直是個(gè)活躍的研究領(lǐng)域[2-4]。在過去的幾年中，利用先驗(yàn)激光雷達(dá)地圖進(jìn)行視覺定位的研究比較多。文獻(xiàn)[5]提出了一種針對(duì)城市環(huán)境的單目視覺定位系統(tǒng)，利用人工從激光雷達(dá)地圖中的道路標(biāo)記提取的一組稀疏點(diǎn)，之后利用Chamfer 匹配將圖像中檢測(cè)到的道路標(biāo)記與地圖中的標(biāo)記進(jìn)行匹配。文獻(xiàn)[6]利用從視覺和先驗(yàn)激光雷達(dá)數(shù)據(jù)中提取的平面結(jié)構(gòu)來融合異構(gòu)地圖，將共面性約束引入到光束平差法（Bundle Adjustment,BA）中，改善視覺里程計(jì)性能。文獻(xiàn)[7]提出將激光雷達(dá)和視覺特征的約束結(jié)合起來，通過順序觀測(cè)驗(yàn)證閉環(huán)待匹配幀，提供高質(zhì)量的閉環(huán)檢測(cè)。這些方法都需要使用激光雷達(dá)，成本較高，并且需要從激光雷達(dá)地圖中再次提取視覺特征。

因此，本文提出基于先驗(yàn)視覺特征地圖匹配的方法，能夠更低成本并且高效地提高系統(tǒng)的穩(wěn)定性，減少累積誤差。地圖匹配是以當(dāng)前地圖中儲(chǔ)存的特征點(diǎn)及其對(duì)應(yīng)的相機(jī)幀的位姿作為基準(zhǔn)，通過詞袋模型[8]進(jìn)行特征匹配提供閉環(huán)約束，提高系統(tǒng)精度與可靠性。

1 系統(tǒng)總體框架

視覺/慣性系統(tǒng)整體框架如圖1所示，VIO（視覺/慣性里程計(jì)）[9]通過局部滑動(dòng)窗口解算出窗口內(nèi)關(guān)鍵幀的位姿，并且計(jì)算相鄰幀之間的相對(duì)位姿，用來做局部的位置估計(jì)。地圖中儲(chǔ)存著歷史幀的位姿與其相鄰的相對(duì)位姿以及觀測(cè)到的特征點(diǎn)和描述子[10]。當(dāng)關(guān)鍵幀從滑動(dòng)窗口中滑出時(shí)，額外計(jì)算關(guān)鍵幀的描述子并檢測(cè)更多的特征點(diǎn)以匹配地圖中的回環(huán)幀，當(dāng)相似度計(jì)算超過一定的閾值并通過篩選后依然能夠匹配到地圖中的回環(huán)幀，即開始進(jìn)行閉環(huán)優(yōu)化，解算出當(dāng)前幀與回環(huán)幀之間的相對(duì)位姿，優(yōu)化得到當(dāng)前幀在歷史坐標(biāo)系下的位姿，計(jì)算出累積偏移，進(jìn)行全局一致性優(yōu)化，將偏移添加到每一幀中，消除累積誤差。由于地圖的加入，如果從VIO 滑出的關(guān)鍵幀與地圖中的歷史幀可以成功地匹配，則能夠利用地圖中的位姿對(duì)全局的每一幀進(jìn)行漂移補(bǔ)償，提高系統(tǒng)的可靠性。

2 特征地圖管理

2.1 地圖儲(chǔ)存

視覺/慣性系統(tǒng)可以實(shí)現(xiàn)系統(tǒng)對(duì)于當(dāng)前位姿的估計(jì)，實(shí)時(shí)計(jì)算出系統(tǒng)的運(yùn)行軌跡。在相同場(chǎng)景下，系統(tǒng)每次啟動(dòng)都會(huì)重新估計(jì)自身位置，但并不是每一次都能得到全局最優(yōu)解，即每次軌跡都能與真實(shí)軌跡的誤差值較小。因此，本文利用系統(tǒng)運(yùn)行生成的位姿圖作為先驗(yàn)信息，矯正系統(tǒng)誤差。

對(duì)于特征地圖，其通過地圖匹配將當(dāng)前位姿重定位到其中，則地圖中需要儲(chǔ)存檢測(cè)匹配的條件，即關(guān)鍵幀的索引、特征點(diǎn)以及描述子，同時(shí)還需要儲(chǔ)存關(guān)鍵幀的位姿。如果系統(tǒng)運(yùn)行的環(huán)境存在閉環(huán)的情況，地圖中存在回環(huán)幀，則還需要儲(chǔ)存回環(huán)幀的索引以及回環(huán)幀與關(guān)鍵幀的相對(duì)位置和航向角。式(1)為保存一個(gè)關(guān)鍵幀的狀態(tài)，地圖由一個(gè)個(gè)這種形式的關(guān)鍵幀組成。圖2為位姿圖儲(chǔ)存形式。是k幀的平移和旋轉(zhuǎn)，是關(guān)鍵幀與回環(huán)幀

圖2 位姿圖儲(chǔ)存形式Fig.2 Storage form of pose map

其中，k是關(guān)鍵幀的索引，l是回環(huán)幀的索引，、之間的相對(duì)平移和航向角，u、v、brief則為此幀觀測(cè)到的特征點(diǎn)的像素坐標(biāo)及其brief描述子。

2.2 地圖加載

加載地圖和地圖儲(chǔ)存使用同樣的格式，將每一個(gè)關(guān)鍵幀的、作為初始值，通過與多個(gè)相鄰幀的相對(duì)位姿將每幀聯(lián)系起來，同時(shí)添加、的回環(huán)信息與回環(huán)幀建立關(guān)系。

3 地圖匹配算法

3.1 地圖特征匹配

視覺前端通過提取檢測(cè)并匹配特征點(diǎn)來利用圖像信息，在一個(gè)滑動(dòng)窗口中通過非線性優(yōu)化緊耦合的方式處理視覺信息與慣性信息，當(dāng)超過滑窗限定的最大幀數(shù)時(shí)，則會(huì)邊緣化舊的關(guān)鍵幀，被邊緣化的關(guān)鍵幀在加入關(guān)鍵幀數(shù)據(jù)庫（KF 數(shù)據(jù)庫，保存關(guān)鍵幀的狀態(tài)信息，保存的狀態(tài)信息如式(1)所示）之前會(huì)再次提取更多的FAST（Features from accelerated segment test，一種通過分段測(cè)試來加快特征提取速度的特征點(diǎn)提取方法）特征點(diǎn)，并計(jì)算brief描述子以用來與地圖中的關(guān)鍵幀進(jìn)行特征匹配。

匹配的方法則是基于詞袋模型的相似度計(jì)算，通過詞袋模型計(jì)算出一個(gè)幀的詞袋向量（由n維的0 或者1 組成的），兩幀詞袋向量1v、v2的相似度由式(2)計(jì)算：

計(jì)算當(dāng)前幀的詞袋向量，和KF 數(shù)據(jù)庫中的回環(huán)幀詞袋向量之間的相似度。如果相似度最高且是最早的回環(huán)幀（索引最?。徒?dāng)前幀與回環(huán)幀之間的聯(lián)系，獲得回環(huán)待匹配幀。直接使用描述符之間的匹配來尋找相同特征點(diǎn)存在離群點(diǎn)的情況。為了減少誤匹配，獲得更準(zhǔn)確的匹配結(jié)果，還需要對(duì)待匹配幀的特征點(diǎn)進(jìn)行篩選。首先利用RANSAC 對(duì)已匹配的2D像素點(diǎn)求基礎(chǔ)矩陣剔除異常點(diǎn)[11]，再使用RANSAC對(duì)特征點(diǎn)的3D 位置坐標(biāo)與2D 像素點(diǎn)進(jìn)行PnP 計(jì)算剔除外點(diǎn)[12]。如果剔除后回環(huán)待匹配幀與當(dāng)前幀之間仍然有足夠的正確匹配的特征點(diǎn)，那么就可以視為一個(gè)成功的匹配。由于后續(xù)回環(huán)幀要放在滑動(dòng)窗口當(dāng)中構(gòu)建重投影誤差約束一起進(jìn)行優(yōu)化，所以需要有足夠的匹配特征點(diǎn)來保證優(yōu)化的準(zhǔn)確性。因此，當(dāng)兩幀之間匹配到特征點(diǎn)對(duì)數(shù)量大于某一數(shù)量，即閾值時(shí)，即認(rèn)為得到了一個(gè)成功的匹配。匹配特征點(diǎn)對(duì)閾值過高會(huì)導(dǎo)致正確的回環(huán)幀匹配失敗，閾值過低會(huì)導(dǎo)致匹配錯(cuò)誤的回環(huán)幀或者匹配正確的回環(huán)幀但是優(yōu)化的結(jié)果不理想，甚至導(dǎo)致系統(tǒng)發(fā)散。通過實(shí)驗(yàn)將閾值選擇為25 對(duì)，可以在正確匹配的情況下得到比較好的減少累積誤差的效果。

相對(duì)于運(yùn)動(dòng)狀態(tài)平穩(wěn)的情況下，快速旋轉(zhuǎn)或者快速移動(dòng)等情況導(dǎo)致相機(jī)模糊會(huì)使匹配到的特征點(diǎn)變少，在保證閾值能夠篩選掉誤匹配幀的情況下，可以減少閾值，但是還要保證匹配到的特征點(diǎn)足夠多，使得系統(tǒng)能夠減少累積誤差。

對(duì)視覺環(huán)境，可以分為特征較多，特征一般和特征較少三種情況，其中特征較多和特征一般的情況會(huì)使用極大值抑制算法，讓固定數(shù)量的特征點(diǎn)較為均勻的分布在圖像里，所以25 對(duì)正確匹配的特征點(diǎn)的策略仍然可以使用；對(duì)于特征較少的情況，不能提取到最大數(shù)量的特征點(diǎn)，可能會(huì)因?yàn)樘卣鼽c(diǎn)的數(shù)量不足而導(dǎo)致正確的回環(huán)未被識(shí)別，這時(shí)可以在保證回環(huán)的正確匹配和匹配后能夠減少累積誤差的前提下減少閾值，以達(dá)到回環(huán)檢測(cè)減少累積誤差的目的。

總之，閾值的選取應(yīng)該在以100%的回環(huán)匹配正確率，并且匹配到的特征點(diǎn)足夠多以使得系統(tǒng)向誤差減少的方向進(jìn)行優(yōu)化這兩個(gè)標(biāo)準(zhǔn)來進(jìn)行選取。

圖3 地圖匹配流程圖Fig.3 Map Matching Flow Chart

3.2 地圖約束的局部定位算法

在VIO 中，滑動(dòng)窗口優(yōu)化視覺重投影誤差以及IMU 預(yù)積分誤差，在局部BA 中調(diào)整優(yōu)化關(guān)鍵幀的位姿、特征點(diǎn)深度，得到一個(gè)窗口內(nèi)的最優(yōu)變量。窗口中僅優(yōu)化了一小部分最新的幀，無法做到全局最優(yōu)，因此長(zhǎng)時(shí)間一定會(huì)存在累積誤差。

當(dāng)系統(tǒng)成功匹配地圖中的關(guān)鍵幀，可利用地圖關(guān)鍵幀位姿矯正滑窗中關(guān)鍵幀的累積誤差，達(dá)到無漂移的位置，如圖4所示。設(shè)l表示閉合幀，在地圖約束的局部定位算法中，固定l幀的初始位姿(p?l,)，即窗口不優(yōu)化l幀的位姿。當(dāng)回環(huán)幀被成功檢索時(shí)，將閉環(huán)幀與關(guān)鍵幀的相對(duì)位姿和初始位姿添加到窗口中，建立閉環(huán)幀l觀測(cè)到特征點(diǎn)的重投影誤差模型作為地圖約束，與視覺殘差和IMU 殘差共同優(yōu)化。式(3)為整個(gè)非線性優(yōu)化滑動(dòng)窗口中代價(jià)函數(shù)表示的誤差項(xiàng)。

圖4 回環(huán)約束加入滑動(dòng)窗口中Fig.4 Adding loop closure constraints to the sliding window

其中rp、Hp為邊緣化的先驗(yàn)信息，x為待優(yōu)化的狀態(tài)量，包括滑動(dòng)窗口內(nèi)每一幀在世界坐標(biāo)系下的位置、速度、姿態(tài)，在IMU 本體坐標(biāo)系中陀螺儀和加速度計(jì)的偏置，IMU 坐標(biāo)系與相機(jī)坐標(biāo)系之間的平移和旋轉(zhuǎn)，以及觀察到特征點(diǎn)的逆深度。為IMU測(cè)量的殘差，α為IMU 測(cè)量的集合，為滑動(dòng)窗口中連續(xù)兩個(gè)幀bk和bk+1之間的IMU 測(cè)量，為對(duì)應(yīng)測(cè)量的協(xié)方差矩陣；為視覺測(cè)量的殘差，β是觀察到特征點(diǎn)的集合，表示第i個(gè)特征點(diǎn)在滑動(dòng)窗口內(nèi)幀j中觀察到的視覺測(cè)量，為對(duì)應(yīng)測(cè)量的協(xié)方差矩陣；為回環(huán)重投影殘差，χ是觀察到閉環(huán)特征點(diǎn)的集合，為第i個(gè)特征點(diǎn)在閉環(huán)幀l中觀察到的視覺測(cè)量，為閉環(huán)幀l在世界坐標(biāo)系w下的姿態(tài)和位置，作為常量加入到優(yōu)化中，為對(duì)應(yīng)測(cè)量的協(xié)方差矩陣。ρ（·）是Huber 核函數(shù)。

3.3 全局位姿圖優(yōu)化

由于計(jì)算能力的限制，為確保系統(tǒng)的實(shí)時(shí)性，只能使用局部的滑動(dòng)窗口優(yōu)化狀態(tài)量，隨著系統(tǒng)運(yùn)行時(shí)間的延長(zhǎng)，估計(jì)誤差也會(huì)慢慢累積，使得估計(jì)結(jié)果不可靠。如圖5所示，由于IMU 的加入，重力矢量可以被測(cè)量，因此相對(duì)重力矢量平面的運(yùn)動(dòng)可以感知，即橫滾角及俯仰角可觀測(cè)，所得到的橫滾角與俯仰角是世界坐標(biāo)系下的絕對(duì)角度。隨著載體的移動(dòng)，3D 位置坐標(biāo)及航向角相對(duì)參考坐標(biāo)系發(fā)生改變，即x，y，z方向和航向角yaw 是相對(duì)于參考坐標(biāo)系的相對(duì)估計(jì)。因此系統(tǒng)存在累積漂移的狀態(tài)量為平移和航向角，為校正全局的累積誤差，減小計(jì)算量，僅在四自由度（x，y，z 與航向角）優(yōu)化所有的位姿圖。

圖5 四個(gè)漂移方向示意圖Fig.5 Four accumulated drift direction

如果從VIO 的滑動(dòng)窗口邊緣化的關(guān)鍵幀沒有檢測(cè)到閉環(huán)，則會(huì)被存入關(guān)鍵幀數(shù)據(jù)庫中，構(gòu)成位姿圖。這些位姿圖與先驗(yàn)特征地圖一樣，關(guān)鍵幀作為位姿圖的一個(gè)頂點(diǎn)，每個(gè)頂點(diǎn)存在順序邊與回環(huán)邊與其他頂點(diǎn)聯(lián)系起來，如圖2所示。

順序邊：由VIO計(jì)算出來的兩幀之間的相對(duì)位姿，順序邊中包含兩幀之間的相對(duì)平移與航向角。順序邊殘差定義為：

回環(huán)邊：如果該幀檢測(cè)到了閉環(huán)，則地圖約束的局部定位能獲得回環(huán)邊的值，可連接該幀與閉環(huán)幀?；丨h(huán)邊也包含四自由度的位姿，回環(huán)邊的殘差定義為：

當(dāng)從局部滑動(dòng)窗口中滑出的關(guān)鍵幀與地圖中的幀檢測(cè)為閉環(huán)時(shí)，則對(duì)數(shù)據(jù)庫的所有幀進(jìn)行四自由度的位姿優(yōu)化。將所有的順序邊以及回環(huán)邊殘差構(gòu)成以下的最小二乘問題：

其中，δ是所有順序邊的集合，ρ·（）是Huber 核函數(shù)，目的是減少誤匹配導(dǎo)致錯(cuò)誤回環(huán)對(duì)系統(tǒng)造成的影響，而順序邊是由VIO 使用IMU 殘差和視覺殘差優(yōu)化得到的相對(duì)位姿，當(dāng)VIO 位姿估計(jì)準(zhǔn)確時(shí)，不需要使用核函數(shù)減少順序邊殘差對(duì)系統(tǒng)的影響；而當(dāng)VIO 位姿估計(jì)不準(zhǔn)確時(shí)，此時(shí)加入核函數(shù)也無法得到一個(gè)更好的結(jié)果，因此對(duì)順序邊的殘差不使用核函數(shù)。在進(jìn)行四自由度優(yōu)化時(shí)，只優(yōu)化piw、ψi，地圖中的位姿被固定，不做優(yōu)化，這樣才能使用地圖中的位姿減少系統(tǒng)的累積誤差。

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

4.1 實(shí)驗(yàn)平臺(tái)

本實(shí)驗(yàn)平臺(tái)由傳感器等硬件和視覺/慣性定位算法軟件兩部分組成。軟件平臺(tái)基于機(jī)器人操作系統(tǒng)（Robot Operating System,ROS），利用ROS 分布式框架實(shí)現(xiàn)傳感器與處理器的通訊，并使用C++將理論公式編寫為相應(yīng)的函數(shù)與類來實(shí)現(xiàn)。對(duì)于上文提及的優(yōu)化問題，在程序中調(diào)用Google 的Ceres 非線性優(yōu)化庫對(duì)設(shè)定變量進(jìn)行優(yōu)化。同時(shí)，通過文件讀寫方式將系統(tǒng)關(guān)鍵幀數(shù)據(jù)庫保存在本地文件中或者將本地關(guān)鍵幀數(shù)據(jù)添加到系統(tǒng)的數(shù)據(jù)庫中，從而實(shí)現(xiàn)地圖的存儲(chǔ)或加載。

本實(shí)驗(yàn)通過自搭建傳感器構(gòu)建硬件平臺(tái)。硬件部分使用小型PC（Intel Core i7-5850EQ，四核八線程，16GB RAM）處理視覺與慣性信息，并處理后端非線性優(yōu)化的計(jì)算。數(shù)據(jù)采集利用傳感器組與PC 通訊獲取。實(shí)驗(yàn)使用灰點(diǎn)單目相機(jī)感知圖像信息，通過USB3.0 連接PC，并以20 Hz 的頻率發(fā)送分辨率為960x600 的圖像；使用MTI-G710 感知慣性信息，以200 Hz 的頻率輸出角速度與加速度。圖6為各傳感器與PC 之間的組裝圖，相機(jī)與IMU 之間通過10 分頻電路連接，使用IMU 的觸發(fā)信號(hào)觸發(fā)相機(jī)快門，實(shí)現(xiàn)相機(jī)與IMU 之間的時(shí)間戳同步。圖7為使用無人機(jī)作為載體采集數(shù)據(jù)時(shí)的場(chǎng)景。

圖6 實(shí)驗(yàn)設(shè)備組裝圖Fig.6 Experiment Equipment Assembly Diagram

圖7 飛行實(shí)驗(yàn)圖Fig.7 Flight Experiment

4.2 實(shí)驗(yàn)過程與結(jié)果

在場(chǎng)景和軌跡相對(duì)單一的條件下，驗(yàn)證先驗(yàn)地圖對(duì)系統(tǒng)穩(wěn)定性提升和對(duì)累積誤差的抑制情況。利用無人機(jī)搭載硬件同步的相機(jī)和IMU 在室外采集數(shù)據(jù)集，每次無人機(jī)預(yù)設(shè)軌跡都相同，運(yùn)行時(shí)長(zhǎng)大約15 分鐘，運(yùn)行軌跡總里程約為6.4 公里，為兩圈方形軌跡。預(yù)設(shè)軌跡如圖8所示。以高精度差分GPS 作為基準(zhǔn)，測(cè)試系統(tǒng)使用實(shí)驗(yàn)采集的數(shù)據(jù)集計(jì)算無人機(jī)航線，計(jì)算軌跡水平圓概率誤差，對(duì)比加載地圖與無預(yù)設(shè)地圖系統(tǒng)計(jì)算軌跡的誤差。

圖8 飛行預(yù)設(shè)軌跡Fig.8 Flight preset trajectory

圖9為加載地圖時(shí)，系統(tǒng)與地圖匹配優(yōu)化估計(jì)得到的軌跡，圖10為未加載地圖時(shí)，使用同樣的數(shù)據(jù)集，系統(tǒng)計(jì)算的軌跡。其中紅色軌跡為GPS 真實(shí)軌跡，黃色為先驗(yàn)地圖軌跡，藍(lán)色為系統(tǒng)估計(jì)軌跡?？梢钥闯觯褂眉虞d地圖時(shí)，系統(tǒng)計(jì)算的軌跡基本上能夠與先驗(yàn)地圖重合，即通過匹配先驗(yàn)地圖能夠很好地約束軌跡誤差。在無先驗(yàn)地圖的條件下，由于實(shí)驗(yàn)過程中一些不確定的因素，導(dǎo)致估計(jì)軌跡與真實(shí)軌跡相比誤差較大。

圖9 使用地圖的軌跡估計(jì)圖Fig.9 Trajectory Estimation with Map Matching

圖10 未使用地圖的軌跡估計(jì)圖Fig.10 Trajectory Estimation without Map Matching

第一組數(shù)據(jù)，無人機(jī)在起飛時(shí)受到大風(fēng)的作用導(dǎo)致了一段橫向位移，使得無人機(jī)移動(dòng)過快，對(duì)視覺產(chǎn)生了影響，進(jìn)而導(dǎo)致初始化得到的初始狀態(tài)不夠準(zhǔn)確，使得未使用地圖的系統(tǒng)無法收斂，導(dǎo)致估計(jì)軌跡誤差較大，如圖10，但是在使用地圖之后，地圖給出的先驗(yàn)信息使得系統(tǒng)狀態(tài)能夠向著正確的方向收斂，獲得了比較好的效果，如圖9所示。

利用第二組～第四組數(shù)據(jù)的軌跡圖對(duì)兩者進(jìn)行評(píng)估。由圖11和圖12可以看出，使用地圖匹配算法的估計(jì)軌跡比未使用地圖匹配算法的結(jié)果更加精確。其中圖11為未使用地圖匹配算法的軌跡誤差投影圖。圖12為使用地圖匹配算法的軌跡誤差投影圖。

圖11 未使用地圖的誤差投影圖（第四組數(shù)據(jù)）Fig.11 Error Projection Chart without Map Matching

圖12 使用地圖的誤差投影圖（第四組數(shù)據(jù)）Fig.12 Error Projection Chart witht Map Matching

使用三組數(shù)據(jù)計(jì)算誤差，表1列出了三組數(shù)據(jù)的誤差的詳細(xì)對(duì)比情況。

表1 使用地圖與未使用地圖的誤差比較Tab.1 Error result comparison between using the map and not using the map

算法共分為三個(gè)并行線程，處理前端視覺信息線程發(fā)布跟蹤到的特征點(diǎn)信息；VIO 線程運(yùn)行局部定位算法，對(duì)滑動(dòng)窗口內(nèi)的關(guān)鍵幀和當(dāng)前幀進(jìn)行優(yōu)化，獲得局部定位信息，滑動(dòng)窗口內(nèi)的關(guān)鍵幀第一幀與最后一幀的時(shí)間跨度在運(yùn)動(dòng)穩(wěn)定時(shí)在26 s 左右，在快速旋轉(zhuǎn)時(shí)視覺測(cè)量變化較快，時(shí)間跨度在1 s 左右；全局位姿圖優(yōu)化線程處理4Dof 位姿圖優(yōu)化，共使用了三個(gè)線程，使用的是四核八線程的CPU，因此可以保證并行運(yùn)行，保證實(shí)時(shí)性。

為確定地圖約束進(jìn)行導(dǎo)航的實(shí)時(shí)性，對(duì)第三組數(shù)據(jù)局部定位算法和全局位姿圖優(yōu)化所消耗的時(shí)間進(jìn)行統(tǒng)計(jì)。由表1、表2、表3可以看出，算法在耗時(shí)增加不到 20%的情況下，提升了 91%的導(dǎo)航精度（RMSE）。兼顧了算法的實(shí)時(shí)性和準(zhǔn)確性。

表2 使用地圖與未使用地圖的局部定位算法耗時(shí)比較Tab.2 Time cost comparison of local localization algorithm between using the map and not using the map

表3 使用地圖與未使用地圖的全局位姿圖優(yōu)化耗時(shí)比較Tab.3 Time cost comparison of global pose optimization between using the map and not using the map

5 結(jié) 論

為確保視覺/慣性定位系統(tǒng)長(zhǎng)時(shí)間穩(wěn)定運(yùn)行，提出了基于特征地圖匹配的軌跡誤差約束的方法。該方法利用從滑窗內(nèi)滑出關(guān)鍵幀與地圖中的歷史幀進(jìn)行特征匹配，將匹配成功的回環(huán)幀加入系統(tǒng)的殘差優(yōu)化，先進(jìn)行局部的重新定位，再執(zhí)行四自由度的全局一致性優(yōu)化。機(jī)載實(shí)驗(yàn)證明，該方法達(dá)到利用先驗(yàn)地圖校正視覺/慣性系統(tǒng)累積誤差的目的，同時(shí)也提高了系統(tǒng)的穩(wěn)定性。