基于先驗特征地圖匹配約束的視覺/慣性定位方法

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

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

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

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

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

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

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

2 特征地圖管理

2.1 地圖儲存

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

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

圖2 位姿圖儲存形式Fig.2 Storage form of pose map

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

2.2 地圖加載

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

3 地圖匹配算法

3.1 地圖特征匹配

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 實驗驗證

4.1 實驗平臺

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

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

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

圖7 飛行實驗圖Fig.7 Flight Experiment

4.2 實驗過程與結(jié)果

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

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

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

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

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

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

利用第二組～第四組數(shù)據(jù)的軌跡圖對兩者進行評估。由圖11和圖12可以看出，使用地圖匹配算法的估計軌跡比未使用地圖匹配算法的結(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ù)計算誤差，表1列出了三組數(shù)據(jù)的誤差的詳細對比情況。

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

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

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

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

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

5 結(jié) 論

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