邱笑晨，趙晨旭，張 海，許 輝

(1.北京航空航天大學(xué)自動(dòng)化學(xué)院，北京 100083；2.北京航空航天大學(xué)中法工程師學(xué)院，北京 100083；3.北京公共交通控股(集團(tuán))有限公司，北京 100161)

0 引言

同步定位與地圖構(gòu)建[1](Simultaneous Localization and Mapping, SLAM)是指搭載特定傳感器的運(yùn)動(dòng)主體，在沒(méi)有先驗(yàn)環(huán)境信息的條件下，在運(yùn)動(dòng)過(guò)程中建立環(huán)境的模型，并估計(jì)自己的實(shí)時(shí)運(yùn)動(dòng)狀態(tài)的方法[2]。若這里的傳感器為相機(jī)，則稱之為視覺(jué)SLAM。

目前，主流的視覺(jué)SLAM方法按照估計(jì)框架可以分為基于濾波的方法和基于關(guān)鍵幀光束平差法(Bundle Adjustment, BA)[3]。按照?qǐng)D像信息的利用程度可以分為基于特征點(diǎn)法和基于直接法(直接利用像素信息)?；跒V波的視覺(jué)SLAM的主要思想是將每一時(shí)刻t的系統(tǒng)狀態(tài)用一個(gè)高斯概率模型表示，且系統(tǒng)狀態(tài)由一個(gè)濾波器不斷更新，從而得到不同的SLAM系統(tǒng)，其中最具代表性的是MonoSLAM[2]和MSCKF[4]。并行追蹤與制圖(Parallel Tracking and Mapping, PTAM)由Klein等[5]于2007年提出，是最早期基于關(guān)鍵幀光束平差法的單目視覺(jué)SLAM系統(tǒng)，利用傳統(tǒng)的特征點(diǎn)法實(shí)現(xiàn)。ORB-SLAM是Mur-Artal等[6]于2015年提出的基于PTAM算法框架改進(jìn)的視覺(jué)SLAM系統(tǒng)，它利用了ORB(Oriented FAST and Rotated, BRIEF)特征，是目前性能最好的單目視覺(jué)SLAM系統(tǒng)之一，其二代版本加入了雙目和RGB-D模式，進(jìn)一步提高了其對(duì)不同傳感器配置的適應(yīng)能力。DTAM[7]和LSD-SLAM[8-9]是兩種基于直接法的具有代表性的單目視覺(jué)SLAM算法，其主要特點(diǎn)是不依賴特征點(diǎn)的提取與匹配，通常在圖像模糊、特征缺失等場(chǎng)景下魯棒性更好。

從性能上，ORB-SLAM具有良好的泛用性，支持單目、雙目、RGB-D等多種模式[10]；其回環(huán)檢測(cè)算法保證了ORB-SLAM能夠有效地抑制累計(jì)誤差，并且利用相同手段實(shí)現(xiàn)的重定位功能使得系統(tǒng)在失效后能夠迅速重定位；ORB-SLAM使用3個(gè)線程完成系統(tǒng)，實(shí)現(xiàn)了快速跟蹤和建圖，能夠保證軌跡和建圖的一致性[11]。

從功能上，可以將ORB-SLAM分為視覺(jué)里程計(jì)(Visual Odometry,VO)前端部分和回環(huán)(Loop Detection)后端部分。本文主要針對(duì)單目ORB-SLAM的系統(tǒng)架構(gòu)進(jìn)行簡(jiǎn)要介紹，以視覺(jué)導(dǎo)航中關(guān)鍵視覺(jué)里程計(jì)為對(duì)象，對(duì)影響系統(tǒng)精度的不同環(huán)節(jié)進(jìn)行分析，結(jié)論中提出可能的改進(jìn)措施。

1 ORB-SLAM系統(tǒng)概述

ORB-SLAM系統(tǒng)的算法部分主要由3個(gè)并行的線程組成，分別是：跟蹤(Tracking)、局部建圖(Local Mapping)和回環(huán)檢測(cè)(Loop Closing)。此外，在初始化構(gòu)建地圖時(shí)會(huì)臨時(shí)發(fā)起2個(gè)線程分別求解單應(yīng)矩陣和本質(zhì)矩陣，執(zhí)行完畢后自行停止；在每次執(zhí)行完回環(huán)位姿圖優(yōu)化后，會(huì)在回環(huán)檢測(cè)線程中臨時(shí)發(fā)起1個(gè)線程，執(zhí)行全局BA優(yōu)化，該線程在執(zhí)行完畢后自行停止，或者被另一次全局BA優(yōu)化打斷。本節(jié)主要介紹單目ORB-SLAM的具體架構(gòu)及優(yōu)化框架。

1.1 系統(tǒng)架構(gòu)分析

ORB-SLAM系統(tǒng)架構(gòu)如圖 1[6]所示，結(jié)合圖示分析3個(gè)主要線程的基本功能如下所示。

圖1 ORB-SLAM系統(tǒng)架構(gòu)Fig.1 Overview of ORB-SLAM system structure

1)跟蹤：線程輸入為每一幀圖像，在未初始化時(shí)，嘗試?yán)脙蓭瑘D像進(jìn)行初始化。初始化完成后，對(duì)獲得的每一幀圖像，通過(guò)特征描述子實(shí)現(xiàn)圖像特征點(diǎn)與局部地圖點(diǎn)的有效匹配，運(yùn)用光束平差法來(lái)最小化重投影誤差，從而優(yōu)化當(dāng)前幀相機(jī)位姿，實(shí)現(xiàn)每幀圖像采集時(shí)刻相機(jī)的跟蹤和定位。滿足特定條件時(shí)，將當(dāng)前幀確定為關(guān)鍵幀。

2)局部建圖：線程的輸入為跟蹤線程插入的關(guān)鍵幀。基于新添加的關(guān)鍵幀，維護(hù)和拓展新的局部地圖點(diǎn)，并運(yùn)用光束平差法優(yōu)化局部地圖中所有關(guān)鍵幀的位姿及局部地圖中的所有地圖點(diǎn)。同時(shí)對(duì)關(guān)鍵幀進(jìn)行篩選，剔除冗余關(guān)鍵幀。

3)回環(huán)檢測(cè)：線程的輸入為經(jīng)過(guò)局部建圖篩選過(guò)的關(guān)鍵幀。將當(dāng)前關(guān)鍵幀的詞袋(Bag of Words，BoW)向量存入全局詞袋數(shù)據(jù)庫(kù)中，從而加速后續(xù)幀的匹配。同時(shí)檢測(cè)是否存在回環(huán)，若存在則通過(guò)執(zhí)行位姿圖(Pose Graph)優(yōu)化來(lái)優(yōu)化全體關(guān)鍵幀位姿，抑制累計(jì)漂移誤差。在位姿圖優(yōu)化完成之后，會(huì)臨時(shí)發(fā)起一個(gè)獨(dú)立線程執(zhí)行全局光束平差法，來(lái)得到整個(gè)系統(tǒng)最優(yōu)結(jié)構(gòu)(地圖點(diǎn))和運(yùn)動(dòng)(關(guān)鍵幀位姿)的結(jié)果[12]。

1.2 單目ORB-SLAM中的光束平差法

ORB-SLAM系統(tǒng)在若干個(gè)部分采用光束平差法[13]進(jìn)行優(yōu)化，包括在跟蹤過(guò)程中優(yōu)化當(dāng)前幀的位姿(純運(yùn)動(dòng)BA)，在局部建圖線程中優(yōu)化局部地圖中的關(guān)鍵幀位姿和地圖點(diǎn)位置(局部BA)，以及在回環(huán)檢測(cè)后優(yōu)化所有的關(guān)鍵幀位姿和地圖點(diǎn)位置(全局BA)。這三部分的主要內(nèi)容分別如下：

(1)

式中，ρ是Huber代價(jià)函數(shù)；Σ是協(xié)方差矩陣，與特征點(diǎn)的尺度相關(guān)；πm是針孔相機(jī)投影函數(shù)，定義如下

(2)

其中，(fx,fy)是焦距，(cx,cy)是主點(diǎn)坐標(biāo)。

由于上述過(guò)程不對(duì)地圖點(diǎn)位置進(jìn)行優(yōu)化，相當(dāng)于進(jìn)行PnP，但ORB-SLAM作者稱之為純運(yùn)動(dòng)BA。

2)局部BA：同樣通過(guò)最小化重投影誤差，同時(shí)優(yōu)化當(dāng)前關(guān)鍵幀位姿、與其存在共視關(guān)系的局部關(guān)鍵幀的位姿TL以及局部關(guān)鍵幀所能觀測(cè)到的所有地圖點(diǎn)的三維坐標(biāo)PL，并將與局部關(guān)鍵幀形成共視但不與當(dāng)前關(guān)鍵幀構(gòu)成共視的關(guān)鍵幀的位姿TF作為約束項(xiàng)參與優(yōu)化。優(yōu)化的過(guò)程中不改變TF，從而使得優(yōu)化結(jié)果關(guān)于全局地圖存在一致性。上述過(guò)程由公式描述如下

(3)

其中，χj為第j個(gè)局部關(guān)鍵幀匹配到地圖點(diǎn)的特征點(diǎn)集合。

3)全局BA：具體過(guò)程與局部BA類似，其優(yōu)化對(duì)象包括除了第一幀的其余所有關(guān)鍵幀位姿及全局地圖中所有的地圖點(diǎn)。初始幀位置被固定是為了保持全局一致性，消除隨機(jī)化[12]。

2 系統(tǒng)精度分析

ORB-SLAM作為基于關(guān)鍵幀BA的視覺(jué)SLAM系統(tǒng)，在其運(yùn)行過(guò)程中光束平差法的效果對(duì)于系統(tǒng)精度的影響至關(guān)重要。在單目ORB-SLAM視覺(jué)里程計(jì)部分(不包括回環(huán)和全局BA后端)，針對(duì)關(guān)鍵幀位姿的優(yōu)化共有三種情況：第一種是當(dāng)前幀成功匹配足夠多的已有地圖點(diǎn)后進(jìn)行的第一次純運(yùn)動(dòng)BA，優(yōu)化得到當(dāng)前幀的初始位姿；第二種是成功更新當(dāng)前幀共視的局部關(guān)鍵幀和局部地圖點(diǎn)后進(jìn)行的第二次純運(yùn)動(dòng)BA，進(jìn)一步優(yōu)化當(dāng)前幀的位姿；第三種是當(dāng)前幀被確定為關(guān)鍵幀后，針對(duì)當(dāng)前關(guān)鍵幀及其所對(duì)應(yīng)的局部關(guān)鍵幀集合和局部地圖點(diǎn)集合的局部BA優(yōu)化，實(shí)現(xiàn)局部地圖的整體優(yōu)化，三種BA的優(yōu)化作用分析如下。

2.1 首次純運(yùn)動(dòng)BA優(yōu)化

系統(tǒng)在成功初始化后，會(huì)對(duì)新來(lái)的圖像幀進(jìn)行ORB特征點(diǎn)的提取和對(duì)應(yīng)描述子的計(jì)算，然后通過(guò)基于速度的模型或基于參考關(guān)鍵幀的模型進(jìn)行圖像特征點(diǎn)與已有地圖點(diǎn)的匹配，并獲得當(dāng)前幀的初始位姿。其中基于速度的模型主要適用于載體勻速運(yùn)動(dòng)的情況，而基于參考關(guān)鍵幀的模型則適用于其他情況。在成功獲得足夠多的匹配點(diǎn)對(duì)后，通過(guò)純運(yùn)動(dòng)BA優(yōu)化(相當(dāng)于進(jìn)行PnP)當(dāng)前幀位姿。此次純運(yùn)動(dòng)BA的輸入量是當(dāng)前幀已得到的特征點(diǎn)與地圖點(diǎn)的匹配關(guān)系，輸出量為優(yōu)化后的當(dāng)前幀位姿。

圖 2展示了2組來(lái)自KITTI數(shù)據(jù)集[14]在不同場(chǎng)景下的數(shù)據(jù)，在首次純運(yùn)動(dòng)BA優(yōu)化前后的關(guān)鍵幀位姿與真值進(jìn)行尺度與坐標(biāo)系對(duì)齊[15]后，得到的運(yùn)動(dòng)軌跡示意圖。

通過(guò)對(duì)圖 2(b)中的軌跡曲線分析可知：若SLAM系統(tǒng)在場(chǎng)景中的運(yùn)動(dòng)較為平滑，則在純運(yùn)動(dòng)BA優(yōu)化前基于速度模型或者參考關(guān)鍵幀模型得到的位姿初值相比于真值的誤差已經(jīng)不是很大，此時(shí)此次BA優(yōu)化的效果并不顯著。通過(guò)軌跡曲線圖也可以看出，優(yōu)化前后的關(guān)鍵幀位置幾乎重合。

(a)基于KITTI數(shù)據(jù)集004序列得到的軌跡曲線圖

(b)基于010序列得到的軌跡曲線圖圖2 首次純運(yùn)動(dòng)BA優(yōu)化后的KITTI數(shù)據(jù)集軌跡曲線Fig.2 Trajectory curves of sequences of KITTIdataset after first motion-only BA optimization

而從圖 2(a)中的曲線圖可以看出，由于實(shí)際場(chǎng)景中存在載體或傳感器抖動(dòng)，即便是在直路行駛的車(chē)輛，其位置在微觀上也會(huì)存在抖動(dòng)(注意橫軸坐標(biāo)間距相對(duì)縱軸而言很小)；又由于傳感器測(cè)量誤差和算法等因素影響，其估計(jì)結(jié)果存在更加明顯的抖動(dòng)。但從圖中可以看出，相比較而言純運(yùn)動(dòng)BA優(yōu)化前的抖動(dòng)更加劇烈。純運(yùn)動(dòng)BA能夠有效地平滑估計(jì)結(jié)果，利用已有匹配地圖點(diǎn)的位置約束修正當(dāng)前幀的位姿，從而在一定程度上抑制積累誤差，避免將過(guò)大的誤差帶入到后續(xù)的優(yōu)化過(guò)程中。

2.2 第二次純運(yùn)動(dòng)BA優(yōu)化

若當(dāng)前幀的初始位姿被純運(yùn)動(dòng)BA修正并剔除錯(cuò)誤的匹配點(diǎn)對(duì)后，仍有足夠多的正確匹配，系統(tǒng)則判定跟蹤成功。此時(shí)系統(tǒng)會(huì)基于修正后的當(dāng)前幀位姿嘗試匹配更多的地圖點(diǎn)，并更新當(dāng)前幀的共視局部關(guān)鍵幀集合和局部地圖點(diǎn)集合，然后基于這些局部信息進(jìn)行第二次純運(yùn)動(dòng)BA優(yōu)化，進(jìn)一步修正當(dāng)前位姿。此次純運(yùn)動(dòng)BA的輸入量為更新后的當(dāng)前幀所有特征點(diǎn)與地圖點(diǎn)的匹配關(guān)系，輸出量為進(jìn)一步修正的當(dāng)前幀位姿。

圖 3所示為基于與2.1節(jié)相同的數(shù)據(jù)集得到的第二次純運(yùn)動(dòng)BA優(yōu)化前后當(dāng)前幀位姿與真值的比較結(jié)果。

由圖 3(b)中的軌跡曲線可知，從宏觀角度看，若當(dāng)前幀的位姿誤差已經(jīng)小于系統(tǒng)修正誤差的最小范圍，即當(dāng)前幀的位姿已經(jīng)足夠準(zhǔn)確，則此次純運(yùn)動(dòng)BA不再起到很明顯的作用。

(a)基于KITTI數(shù)據(jù)集004序列得到的軌跡曲線圖

(b)基于010序列得到的軌跡曲線圖圖3 第二次純運(yùn)動(dòng)BA優(yōu)化后的KITTI數(shù)據(jù)集軌跡曲線Fig.3 Trajectory curves of sequences of KITTIdataset after second motion-only BA optimization

而由圖 3(a)的比較結(jié)果可以看出，每次優(yōu)化前的當(dāng)前幀位姿相比于真值已經(jīng)沒(méi)有特別大的偏差，但是相對(duì)優(yōu)化后的結(jié)果而言，部分關(guān)鍵幀位置仍然存在著一些不合理的跳躍現(xiàn)象，主要是因?yàn)槭状渭冞\(yùn)動(dòng)BA優(yōu)化當(dāng)前幀位姿時(shí)并未充分地利用所有潛在的匹配地圖點(diǎn)的約束信息。因此，第二次純運(yùn)動(dòng)BA優(yōu)化當(dāng)前幀位姿后，不僅進(jìn)一步修正了誤差較大的幀位姿，同時(shí)在有限的地圖點(diǎn)位置約束下使不同時(shí)刻的當(dāng)前幀的位姿的變化盡可能的光滑。此時(shí)較大的位姿誤差已經(jīng)得到盡可能的修正，接著會(huì)通過(guò)局部BA的優(yōu)化調(diào)整所有局部關(guān)鍵幀的位姿及局部地圖點(diǎn)的世界坐標(biāo)系坐標(biāo)，從而使得位姿變化更加光滑。

此處要求位姿變化是光滑的，是因?yàn)槊看蔚漠?dāng)前關(guān)鍵幀位姿都是基于已被修正過(guò)的局部關(guān)鍵幀位姿得到的，在當(dāng)前關(guān)鍵幀引入到系統(tǒng)的位姿誤差被約束到合理范圍的前提下，不同時(shí)刻的當(dāng)前關(guān)鍵幀的位姿變化應(yīng)當(dāng)是平滑的。

2.3 局部BA優(yōu)化

若第二次純運(yùn)動(dòng)BA修正當(dāng)前幀位姿后仍能得到足夠多的有效匹配地圖點(diǎn)對(duì)，且當(dāng)前幀滿足被設(shè)置成關(guān)鍵幀的條件，即當(dāng)前幀與其局部關(guān)鍵幀的相同的地圖匹配點(diǎn)數(shù)量不超過(guò)90%，且滿足局部建圖線程空閑或距離上一次插入關(guān)鍵幀的時(shí)間超過(guò)閾值，當(dāng)前幀便被設(shè)置成關(guān)鍵幀。接著基于當(dāng)前關(guān)鍵幀三角化新的局部地圖點(diǎn)并剔除不合理的局部地圖點(diǎn)，然后針對(duì)局部地圖中所有的關(guān)鍵幀及地圖點(diǎn)進(jìn)行一次局部BA優(yōu)化。此次局部BA優(yōu)化的輸入量是當(dāng)前關(guān)鍵幀所在的局部地圖中所有的局部關(guān)鍵幀位姿和所有局部關(guān)鍵幀中特征點(diǎn)與地圖點(diǎn)的匹配關(guān)系，同時(shí)利用所有與局部關(guān)鍵幀形成共視關(guān)系，但不與當(dāng)前關(guān)鍵幀形成共視關(guān)系的關(guān)鍵幀位姿來(lái)約束優(yōu)化結(jié)果，這些位姿在優(yōu)化過(guò)程中保持不變，輸出量是所有局部關(guān)鍵幀優(yōu)化后的位姿和局部地圖點(diǎn)優(yōu)化后的三維坐標(biāo)。

圖 4的三幅曲線圖是基于同一數(shù)據(jù)集局部BA優(yōu)化得到的軌跡曲線圖的不同表現(xiàn)形式。其中(a)和(b)圖的含義是：在一次共有83個(gè)關(guān)鍵幀生成的系統(tǒng)運(yùn)行過(guò)程中，第74個(gè)和第75個(gè)關(guān)鍵幀分別插入時(shí)，當(dāng)前所有的局部關(guān)鍵幀在局部BA優(yōu)化前后的軌跡曲線圖以及全部軌跡真值，而(c)圖表示所有不同時(shí)刻的當(dāng)前關(guān)鍵幀在局部BA優(yōu)化前后的軌跡曲線以及真值。第一種方式側(cè)重于橫向展示多次局部BA對(duì)局部關(guān)鍵幀位姿的連續(xù)性影響，而第二種方式則側(cè)重于縱向展示每次BA對(duì)當(dāng)前新插入關(guān)鍵幀的直接性影響。

從圖 4(c)的比較結(jié)果可以看出，當(dāng)前關(guān)鍵幀的位姿在局部BA中得到了進(jìn)一步的修正，因此軌跡曲線變得更加平滑?？紤]到局部BA優(yōu)化是將所有局部關(guān)鍵幀中局部地圖點(diǎn)的重投影誤差作為代價(jià)函數(shù)，即在有效修正位姿誤差較大的關(guān)鍵幀的同時(shí)，不斷將系統(tǒng)模型帶來(lái)的誤差平均到所有局部關(guān)鍵幀位姿上。而從圖 4(a)和(b)的軌跡曲線可以看出，當(dāng)前關(guān)鍵幀會(huì)在之后有限個(gè)關(guān)鍵幀插入時(shí)再次參與其局部BA的優(yōu)化，從而不斷平均系統(tǒng)位姿解算誤差，使得最終系統(tǒng)輸出的關(guān)鍵幀位姿曲線變得十分平滑，且每個(gè)關(guān)鍵幀位姿的誤差均在一定的范圍內(nèi)，如圖 5所示。

(a)

(b)

(c)圖4 (a)和(b)表示基于KITTI數(shù)據(jù)集004序列得到的，第74幀關(guān)鍵幀和第75幀關(guān)鍵幀插入時(shí)所有局部關(guān)鍵幀優(yōu)化前后的軌跡曲線示意圖;(c)表示不同時(shí)刻的當(dāng)前關(guān)鍵幀在局部BA優(yōu)化前后的軌跡曲線圖Fig.4 (a) and (b) respectively show the trajectory curves of alllocal keyframes before and after optimization when the 74thkeyfra-me and the 75thkeyframe insert based on the sequence 004 ofKITTI dataset; (c) indicates the trajectory curve of the currentkeyframe before and after local BA optimization at different times

圖5 基于KITTI數(shù)據(jù)集004序列，Visual Odometry部分得到的最終優(yōu)化的全局關(guān)鍵幀軌跡示意圖Fig.5 Final optimized keyframe trajectory graph obtained by thevisual odometry part based on the 004 sequence of KITTI dataset

3 結(jié)論

作為基于關(guān)鍵幀光束平差法的代表方案，ORB-SLAM系統(tǒng)在有效修正誤差較大的全局關(guān)鍵幀位姿的同時(shí)，通過(guò)不斷調(diào)整局部關(guān)鍵幀位姿和局部地圖點(diǎn)在世界坐標(biāo)系中的三維坐標(biāo)，在系統(tǒng)誤差范圍內(nèi)保證位姿變化的連貫性和平滑性。此外，ORB-SLAM還通過(guò)各種約束條件和加速手段保證了系統(tǒng)的實(shí)時(shí)性和魯棒性，并通過(guò)回環(huán)檢測(cè)的方法進(jìn)一步精細(xì)化修正全局地圖和關(guān)鍵幀位姿，有效地提高了系統(tǒng)的性能和精度。

然而，由于ORB-SLAM嚴(yán)重依賴特征點(diǎn)的提取與匹配效果，因此在如紋理缺失的環(huán)境下運(yùn)行會(huì)無(wú)法得到足夠多的穩(wěn)定匹配點(diǎn)對(duì)，此時(shí)光束平差法缺乏足夠的輸入信息，無(wú)法有效地修正位姿偏差。這種情況下，可以引入外界的觀測(cè)如慣性測(cè)量單元(Inertial Measurement Unit, IMU)等，來(lái)約束和修正特征缺失帶來(lái)的偏差，針對(duì)單目情況，還能得到絕對(duì)尺度。近年來(lái)也有越來(lái)越多的研究偏向于視覺(jué)/慣性視覺(jué)里程計(jì)(Visual/Inertial Odometry)方向[16-18]，以期望解決純視覺(jué)SLAM存在的問(wèn)題。