綜述 | 基于特征的視覺同步定位和建圖（1）

Rana Azzam1 · Tarek Taha2 · Shoudong Huang3 · Yahya Zweiri4 

接收日期：2019 年 10 月 30 日/接受時(shí)間：2020 年 1 月 8 日/在線發(fā)布時(shí)間：2020 年 1 月 16 日 ? Springer Nature Switzerland AG 2020

翻譯：幸運(yùn)的石頭
摘要視覺同步定位和建圖（SLAM）在過去幾年中引起了高度關(guān)注。在本文中，呈現(xiàn)了關(guān)于最先進(jìn)的基于特征的視覺 SLAM 方法的綜述。綜述中根據(jù)在環(huán)境中觀察到的視覺特征進(jìn)行分類。視覺特征可以分為不同的層次：點(diǎn)和邊這樣的低級(jí)特征，平面和斑點(diǎn)這樣的中級(jí)特征，以及語義標(biāo)記的對(duì)象這樣的高級(jí)特征。本研究得出的關(guān)于視覺 SLAM 方法的最關(guān)鍵研究空白之一是缺乏普遍性。雖然一些方法在準(zhǔn)確性和效率方面表現(xiàn)得非常成熟。然而，它們是為非常特定的環(huán)境量身定制的，例如特征豐富的靜態(tài)環(huán)境。在不同環(huán)境中運(yùn)行時(shí)，此類方法的性能會(huì)嚴(yán)重下降。此外，由于軟件和硬件的限制，保證視覺 SLAM 方法的魯棒性極具挑戰(zhàn)性。盡管語義在視覺 SLAM 中被大量使用，但通過結(jié)合特征之間的關(guān)系來理解場(chǎng)景尚未完全探索。整篇論文都對(duì)此類研究進(jìn)行了詳細(xì)討論。關(guān)鍵詞 機(jī)器人·SLAM·定位·傳感器·因子圖·語義1 引言經(jīng)過幾十年的詳盡研究和深入調(diào)查，同步定位和建圖 (SLAM) 持續(xù)在機(jī)器人社區(qū)進(jìn)行的研究中占據(jù)主導(dǎo)地位。SLAM 是一個(gè)問題，即估計(jì)機(jī)器人車輛在以前未探索的環(huán)境中導(dǎo)航的位置，同時(shí)逐步構(gòu)建它的地圖。SLAM估計(jì)是基于通過安裝在車輛上的傳感器收集的測(cè)量值完成的，這些傳感器包括：視覺、距離、光、位置和慣性傳感器等等。SLAM 系統(tǒng)以多種不同的方法使用這些測(cè)量值來定位機(jī)器人并繪制其周圍環(huán)境。然而，任何 SLAM 系統(tǒng)的搭建都包括一組通用組件，例如：地圖/軌跡初始化；數(shù)據(jù)關(guān)聯(lián)；和閉環(huán)。然后可以使用不同的估計(jì)技術(shù)來估計(jì)機(jī)器人的軌跡并生成環(huán)境地圖。每個(gè) SLAM 方法的實(shí)現(xiàn)其細(xì)節(jié)都依賴于所使用的傳感器，也因此依賴于從環(huán)境中收集的數(shù)據(jù)。在本文中，我們徹底回顧了最新的視覺 SLAM 系統(tǒng)，重點(diǎn)關(guān)注基于特征的方法，使用傳統(tǒng)的視覺傳感器（如單目、深度或立體相機(jī)）來觀察環(huán)境。從現(xiàn)在起，如果視覺 SLAM 系統(tǒng)分別使用單目相機(jī)、RGB-D 相機(jī)或立體（雙目或多目）相機(jī)，則它們被稱為單目 SLAM、RGB-D SLAM 或立體 SLAM。非常規(guī)的基于事件的視覺傳感器，例如基于異步時(shí)間的圖像傳感器 (ATIS) [98] 和動(dòng)態(tài)的有源像素視覺傳感器 (DAVIS) [11]，在 [64, 131, 132] 中被提出，也可用于解決 SLAM 問題。它的操作原理是受生物學(xué)啟發(fā)的，它不是以設(shè)定的速率捕獲幀，而是異步捕獲事件，這些事件是獨(dú)立像素亮度的時(shí)間戳變化。由于其從環(huán)境中獲取信息的獨(dú)特方式，需要進(jìn)行范式轉(zhuǎn)換來構(gòu)建適應(yīng)此類信息的算法。基于事件的 SLAM 超出了本文的范圍，感興趣的讀者可以參考 [41] 中的綜合調(diào)查。一些 SLAM 系統(tǒng)僅依賴于視覺測(cè)量，而另一些則通過不同的觀察來增強(qiáng)它們，例如距離或慣性測(cè)量。多種類型的觀察結(jié)果的融合可能會(huì)增加算法的復(fù)雜性，需要更多的計(jì)算資源，并增加平臺(tái)的成本。但是，它使系統(tǒng)更可靠，對(duì)異常值更魯棒，并且對(duì)故障具有彈性。要選擇適合已開發(fā)的視覺 SLAM 系統(tǒng)的視覺傳感器，應(yīng)考慮以下幾點(diǎn)。根據(jù)單個(gè)單目框架的觀察是不可能辨別環(huán)境的尺度的。為了彌補(bǔ)這一點(diǎn)，單目 SLAM 系統(tǒng)采用不同的方法來推斷深度，例如使用一組一個(gè)或多個(gè)其他傳感器來獲得可以推斷深度的測(cè)量值、使用神經(jīng)網(wǎng)絡(luò)假設(shè)觀察到的特征的深度，或者通過利用有關(guān)環(huán)境的先驗(yàn)信息，例如觀察到的特征的大小。RGB-D 相機(jī)可以提供有關(guān)單幀深度的信息，但它們對(duì)光非常敏感，這可能會(huì)限制它們的應(yīng)用場(chǎng)景或它們可以成功運(yùn)行的環(huán)境。立體相機(jī)克服了單目和 RGB-D 相機(jī)的局限性，但它們更昂貴且更耗費(fèi)計(jì)算資源。視覺傳感器的選擇還取決于要使用的機(jī)器人平臺(tái)。例如，地面車輛對(duì)車載傳感器的重量沒有任何限制，這使得所有選項(xiàng)都可以選擇。然而，如果要使用飛行器，單目相機(jī)似乎是最方便的選擇，因?yàn)樗梢詿o縫地安裝在機(jī)上，因?yàn)樗亓枯p、體積小、功率要求低。然而，所采用的算法必須處理獲得的視覺觀察的尺度不確定性。視覺測(cè)量可以在不同的細(xì)節(jié)層次上進(jìn)行處理。直接法 SLAM 系統(tǒng)，例如：[34, 85, 86]，處理圖像中所有像素或像素子集的強(qiáng)度。然后，基于亮度一致性約束[139]，在多個(gè)觀測(cè)值之間建立對(duì)應(yīng)關(guān)系。另一方面，基于特征的 SLAM 以具有獨(dú)特屬性的特征為目標(biāo)，并且可以通過所采用的檢測(cè)算法重復(fù)檢測(cè)。這種系統(tǒng)的例子包括 [65, 91, 97]。特征可以分為不同的等級(jí)；點(diǎn)、角和線等低級(jí)特征，斑點(diǎn)和平面等中級(jí)特征，以及語義標(biāo)記的對(duì)象等高級(jí)特征，如圖 1 所示。視覺 SLAM 系統(tǒng)可能采用單個(gè) [23 , 45, 88] 或不同特征級(jí)別的混合 [10, 54, 138]。在我們的綜述中，我們根據(jù)用于執(zhí)行定位和建圖的特征對(duì)最先進(jìn)的基于特征的視覺 SLAM 解決方案進(jìn)行分類。在每個(gè)類別中，都對(duì)所采用的 SLAM 的實(shí)現(xiàn)進(jìn)行了徹底的討論和比較。強(qiáng)調(diào)每個(gè)類別的優(yōu)缺點(diǎn)，最后強(qiáng)調(diào)開放的研究問題。1.1 SLAM 的現(xiàn)有調(diào)查提出的 SLAM 方法由該領(lǐng)域的幾位研究人員進(jìn)行了調(diào)查，并強(qiáng)調(diào)了迄今為止的開放研究問題。圖 1 從同一視覺框架中提取的不同視覺特征。左：低級(jí)特征（SURF [6]），中：中級(jí)特征（平面），右：高級(jí)特征（語義標(biāo)記的對(duì)象）在 [14] 中，作者認(rèn)為 SLAM 正在進(jìn)入魯棒感知時(shí)代并在可擴(kuò)展性、魯棒性和表示等幾個(gè)性能指標(biāo)方面徹底討論了最先進(jìn)解決方案的主要特征。此外，論文還討論了硬件和算法層面的最新進(jìn)展，并指出了尚待解決的研究問題。[139] 對(duì)基于關(guān)鍵幀的 SLAM 方法進(jìn)行了全面回顧，其中介紹了基于關(guān)鍵幀的單目 SLAM 的一般架構(gòu)和相應(yīng)的實(shí)現(xiàn)方法。在 [109] 中進(jìn)行的調(diào)查針對(duì) SLAM 方法，這些方法忽略了所研究的環(huán)境是靜態(tài)的假設(shè)，并解決了重建動(dòng)態(tài)環(huán)境所采用的基礎(chǔ)技術(shù)。同樣，[96] 中提出的調(diào)查研究了可以在動(dòng)態(tài)環(huán)境中運(yùn)行的 SLAM 方法，以及那些使用可以通過視覺傳感器獲得的異構(gòu)數(shù)據(jù)的方法，例如：顏色、深度和語義信息。依賴于觀察場(chǎng)景中原始特征的視覺 SLAM 方法在 [44] 中進(jìn)行了調(diào)查，并根據(jù)用于這些特征的描述符進(jìn)行分類，強(qiáng)調(diào)了它們的優(yōu)缺點(diǎn)。[141] 中提供了對(duì)視覺里程計(jì)和視覺 SLAM 的結(jié)構(gòu)的概述，以及基礎(chǔ)公式和設(shè)備選擇。類似地，在 [40] 中，基于 SLAM 主要構(gòu)建的實(shí)現(xiàn)分析了視覺 SLAM 的解決方案，并分析了它們?cè)趧?dòng)態(tài)環(huán)境中的失敗。[120] 中回顧的 SLAM 方法分為基于特征的方法、直接方法和基于 RGB-D 的方法。對(duì) 2016 年最先進(jìn)的解決方案進(jìn)行了比較，隨后是與上述類別相關(guān)的一組開放研究問題。最后，可以在 [115] 中找到最近關(guān)于 SLAM 的一項(xiàng)調(diào)查，重點(diǎn)是語義。在本文中，我們對(duì)最新最先進(jìn)的基于特征的視覺 SLAM 系統(tǒng)進(jìn)行了全面調(diào)查，我們根據(jù)元素（即特征）對(duì)回顧過的方法進(jìn)行分類，它們從視覺框架中提取以定位機(jī)器人和重建環(huán)境。這些特征屬于以下類別之一：低級(jí)、中級(jí)或高級(jí)特征。因此，所回顧的方法分類如圖 2 所示。圖 2 基于特征的視覺 SLAM 方法分類我們的綜述為有興趣研究基于特征的視覺 SLAM 的各種實(shí)施選項(xiàng)和進(jìn)展的研究人員提供了全面的參考。屬于同一特征級(jí)別類別的方法會(huì)根據(jù)它們完成的其他目標(biāo)進(jìn)一步分組，例如實(shí)時(shí)性能、處理場(chǎng)景動(dòng)態(tài)以及對(duì)數(shù)據(jù)關(guān)聯(lián)失敗的恢復(fù)能力。列出并分析了使這些目標(biāo)成為可能的技術(shù)。這將幫助讀者準(zhǔn)確地確定這些方法中的每一種是什么，以及他們需要采用和/或改進(jìn)哪些實(shí)施方法來開發(fā)一個(gè)可以實(shí)現(xiàn)一組特定目標(biāo)的系統(tǒng)。本文的其余部分安排如下，通用 SLAM 系統(tǒng)的結(jié)構(gòu)在第 3 節(jié)中介紹。圖 2 中詳細(xì)討論了 SLAM 構(gòu)建塊以及不同的實(shí)現(xiàn)選項(xiàng)。第 3 節(jié)提供了對(duì)基于特征的視覺 SLAM 系統(tǒng)及其設(shè)計(jì)選擇的回顧和分析。第4節(jié)我們強(qiáng)調(diào)了我們的綜述結(jié)果并確定需要進(jìn)一步調(diào)查的未解決問題。2 SLAM 構(gòu)建塊在深入研究當(dāng)前最先進(jìn)解決方案的實(shí)現(xiàn)細(xì)節(jié)之前，簡(jiǎn)要討論視覺 SLAM 的常見組件，包括（1）地圖/軌跡初始化，（2）數(shù)據(jù)關(guān)聯(lián)，（ 3) 閉環(huán)，(4) 重定位，和 (5) 估計(jì)算法，如圖 3 所示。首先提供每個(gè)組件的目的，然后是最流行的實(shí)現(xiàn)方法（如果適用）。圖 3 SLAM 流水線2.1 地圖/軌跡初始化在新環(huán)境中開始機(jī)器人任務(wù)時(shí)，其先驗(yàn)地圖不可用，因此有必要估計(jì)周圍環(huán)境的 3D 結(jié)構(gòu)以及機(jī)器人相對(duì)于它的位置.這是對(duì)地圖的初步評(píng)估，將根據(jù)在整個(gè)任務(wù)中收集的傳感器測(cè)量結(jié)果進(jìn)行迭代更新。此過程僅需要在啟動(dòng)時(shí)引導(dǎo)系統(tǒng)。當(dāng)使用不同的傳感器時(shí)，有幾種方法可以進(jìn)行初始化。例如，一個(gè)深度幀或一個(gè)立體對(duì)足以初始化地圖，如 [97, 118] 中所述，因?yàn)樗鼈兲峁┝藛文繋狈Φ纳疃群捅壤畔ⅰＡ硪环矫?，可以在單目相機(jī)運(yùn)行時(shí)手動(dòng)進(jìn)行初始化，例如[32]，其中系統(tǒng)提供了有關(guān)觀察到的場(chǎng)景的先驗(yàn)信息，包括四個(gè)特征的位置和外觀，解決了尺度不確定性問題。地圖初始化常用的其他算法示例是迭代最近點(diǎn) (ICP) [8, 20]、圖像對(duì)齊 [80, 112]、五點(diǎn)算法 [114] 以及模型擬合算法，例如隨機(jī)樣本抽取 (RANSAC) [38] 或 MLESAC [ 123]，以及相對(duì)于相機(jī)的逆深度參數(shù)化，用于參數(shù)化觀察到的特征[25]。運(yùn)動(dòng)學(xué)模型，例如 [22] 和慣性測(cè)量的積分，如 [29] 中所述，可用于初始化軌跡。2.2 數(shù)據(jù)關(guān)聯(lián)在環(huán)境中進(jìn)行運(yùn)動(dòng)時(shí)，機(jī)器人可能會(huì)多次感知同一區(qū)域。建立每次觀察相同場(chǎng)景時(shí)收集的圖像幀之間的對(duì)應(yīng)關(guān)系，對(duì)于估計(jì)地圖和機(jī)器人的軌跡至關(guān)重要，被稱為數(shù)據(jù)關(guān)聯(lián)。基于特征的方法以特征為目標(biāo)，這些特征是圖像中表現(xiàn)出獨(dú)特屬性的區(qū)域。特征可以有不同的尺度：低級(jí)特征，如幾何基元；中級(jí)特征，如超像素；或高級(jí)特征，如語義標(biāo)記的對(duì)象。特征最關(guān)鍵的特征是可重復(fù)性，這使得特征在出現(xiàn)在從不同視點(diǎn)拍攝的多個(gè)幀中時(shí)可重復(fù)檢測(cè)。為了檢測(cè)圖像中的特征，文獻(xiàn)中針對(duì)不同的特征類型提出了幾種檢測(cè)器。對(duì)于低級(jí)特征，例如點(diǎn)、線、邊和角，表 1 顯示了特征檢測(cè)器和描述符的一些示例。檢測(cè)到特征后，將其與其周圍像素一起從圖像中提取，然后分配一個(gè)定量度量，稱為描述符，以方便與其他特征匹配。表 1 特征檢測(cè)器和描述符為了檢測(cè)圖像中的平面，使用了模型擬合算法，例如 RANSAC。還可以結(jié)合建模和卷積神經(jīng)網(wǎng)絡(luò) (CNN) 來識(shí)別圖像中的平面，例如墻壁 [136]。對(duì)于高級(jí)特征，提出了幾種用于檢測(cè)對(duì)象并在圖像中對(duì)它們進(jìn)行語義標(biāo)記的技術(shù)，包括但不限于條件隨機(jī)場(chǎng) (CRF) [51]、支持向量機(jī) (SVM) [30] 和深度神經(jīng)網(wǎng)絡(luò)（例如：single shot multi-box detector（SSD）[74]，you only look once  (YOLO) [104]）。可以在兩幅圖像中的特征之間（2D-2D 匹配）、3D 地圖中的一個(gè)點(diǎn)與其在圖像幀上的投影之間（3D-2D 匹配）或兩幅圖像之間建立低級(jí)特征之間的對(duì)應(yīng)關(guān)系。重建地圖中的 3D 點(diǎn)（3D-3D 匹配）[140]，如圖 4a 所示。圖 4 數(shù)據(jù)關(guān)聯(lián)和閉環(huán)示例將當(dāng)前圖像中的特征與另一圖像中的特征（2D-2D）匹配是通過在第二張圖像中的窗口內(nèi)搜索來執(zhí)行的，該窗口包含當(dāng)前圖像中特征的位置。如果兩個(gè)圖像之間的轉(zhuǎn)換是已知的，則搜索將減少到一維，因此可以建立對(duì)極幾何[50]。特征描述符之間的相似性可以根據(jù)它們的類型使用不同的量來衡量，例如距離平方和、L1/L2 范數(shù)或漢明距離等等。由于它們的高計(jì)算要求，這些措施可能會(huì)阻礙系統(tǒng)的性能，并且可以被類似于 [89] 的 kd-tree 搜索或諸如 [42] 的二元詞袋方法所取代。當(dāng)需要在給定環(huán)境的 3D 結(jié)構(gòu)的情況下估計(jì)相機(jī)的位姿時(shí)，3D-2D 匹配是必要的。將假設(shè)姿勢(shì)周圍的 3D 點(diǎn)投影到當(dāng)前圖像幀上。然后使用前面提到的技術(shù)將二維投影與圖像中的 2D 特征進(jìn)行匹配。在重新訪問一個(gè)位置時(shí)，即關(guān)閉一個(gè)循環(huán)，相應(yīng)的 3D 地標(biāo)被匹配 (3D-3D)，從而產(chǎn)生一個(gè)校正的、無漂移的路徑。通過比較平面參數(shù)（例如法線（例如：[54]）、重疊以及在當(dāng)前幀中檢測(cè)到的平面與地圖中可用的那些（例如[138]）。如果距離低于特定閾值，則建立對(duì)應(yīng)關(guān)系。否則，會(huì)在地圖中添加一個(gè)新平面。為了建立語義標(biāo)記的地標(biāo)之間的對(duì)應(yīng)關(guān)系，預(yù)測(cè)標(biāo)簽用于將檢測(cè)與地圖中的地標(biāo)相關(guān)聯(lián)。如果環(huán)境中出現(xiàn)同一對(duì)象類別的多個(gè)實(shí)例，則必須超過它們之間的最小距離閾值才能考慮在地圖中插入新的地標(biāo)[10]。否則，檢測(cè)與其最近的地標(biāo)相關(guān)聯(lián)。在最近提出的 SLAM 解決方案 [95] 中，對(duì)象在類別級(jí)別被檢測(cè)和表征，而不僅僅是實(shí)例級(jí)別。這是基于這樣一個(gè)事實(shí)，即一個(gè)類中的所有對(duì)象都具有共同的 3D 點(diǎn)，而與它們的不同類別無關(guān)。下一節(jié)將更詳細(xì)地討論數(shù)據(jù)關(guān)聯(lián)的其他方法。2.3 閉環(huán)隨著機(jī)器人完成其任務(wù)，來自多個(gè)來源的誤差累積導(dǎo)致估計(jì)偏離真實(shí)軌跡（一個(gè)例子如圖4b所示）。這種漂移可能會(huì)嚴(yán)重影響環(huán)境的重建，從而導(dǎo)致正在進(jìn)行的機(jī)器人任務(wù)失敗。為了糾正這種漂移，文獻(xiàn)中提出了幾種技術(shù)來檢測(cè)回環(huán)閉合，即檢測(cè)當(dāng)前觀察到的場(chǎng)景是否被機(jī)器人更早地評(píng)估，從而實(shí)現(xiàn)全局一致性。全局一致性是 SLAM 估計(jì)近似匹配地面實(shí)況并且重建地圖符合觀察區(qū)域的真實(shí)拓?fù)浣Y(jié)構(gòu)的條件。然而，局部一致性是指觀察結(jié)果在局部匹配但可能不是全局匹配的情況[84]。閉環(huán)通常涉及兩個(gè)主要步驟：視覺位置識(shí)別和幾何驗(yàn)證。前者可以使用 kd-tree 搜索 [75]、詞袋方法 [63]、貝葉斯過濾 [2]、深度學(xué)習(xí) [43, 135] 和視覺特征匹配 [53, 79] 來完成，而后者可以通過圖像對(duì)齊和 RANSAC [38] 來實(shí)現(xiàn)。2.4 重定位重定位是 SLAM 系統(tǒng)從機(jī)器人運(yùn)動(dòng)到任意位置的致命定位失敗中恢復(fù)的能力。這種失敗可能是由于多種原因造成的，例如突然運(yùn)動(dòng)、運(yùn)動(dòng)模糊或缺少特征 [139]。此外，機(jī)器人車輛可能會(huì)通過不受機(jī)器人控制的操作重新定位，在這種情況下，機(jī)器人的全局位置將被確定 [12]。這些情況被稱為綁架機(jī)器人問題 [35]，可以使用多種技術(shù)解決，包括但不限于匹配特征描述符 [71]、重新觀察語義標(biāo)記對(duì)象 [48、106]、極線幾何[82]，或二元詞袋方法 [91, 105]，2.5 估計(jì)算法估計(jì)算法需要解決 SLAM 約束，可以分為批量處理和增量算法。批量處理算法，例如全局束調(diào)整 (GBA) [125] 和全圖 SLAM [122]，處理機(jī)器人收集的大量測(cè)量值，需要相對(duì)較長(zhǎng)的時(shí)間，來重建環(huán)境的地圖以及機(jī)器人的軌跡。另一方面，增量算法在新測(cè)量值到達(dá)時(shí)計(jì)算地圖和軌跡的估計(jì)值。一些增量算法，如 [61] 對(duì)整個(gè)機(jī)器人任務(wù)收集的整個(gè)測(cè)量值集進(jìn)行操作，而其他算法，如 [60] 對(duì)在一小段時(shí)間內(nèi)收集的這些測(cè)量值的子集進(jìn)行操作，這有助于在線操作方式。雖然批量處理算法成功地實(shí)現(xiàn)了全局一致性，但它們的計(jì)算成本很高，因此可能會(huì)阻礙實(shí)時(shí)操作。此外，由于內(nèi)存資源有限，它們可能不適用于大規(guī)模環(huán)境或連續(xù)操作系統(tǒng)，這強(qiáng)調(diào)了不受此類限制的增量算法的重要性。當(dāng)運(yùn)用增量算法進(jìn)行估計(jì)時(shí)，由于增量算法不考慮所有的測(cè)量值，故而使用所有舊的數(shù)據(jù)進(jìn)行關(guān)聯(lián)決策是不可能的，與其他算法相比，這可能會(huì)增加累積誤差。在下文中，批量處理算法，例如 GraphSLAM [122] 和 GBA [125]，以及增量算法，例如擴(kuò)展卡爾曼濾波器 (EKF) [122]、增量平滑和映射 [60、61]，以及局部捆綁調(diào)整（LBA）[87]，進(jìn)行簡(jiǎn)要介紹。2.5.1 擴(kuò)展卡爾曼濾波器 (EKF) [122]給定一段時(shí)間內(nèi)記錄的多個(gè)測(cè)量值，可能來自多個(gè)傳感器，EKF 估計(jì)被觀察系統(tǒng)的狀態(tài)。系統(tǒng)的狀態(tài)由環(huán)境和機(jī)器人車輛的狀態(tài)組成。前者描述了在環(huán)境中觀察到的地標(biāo)的姿態(tài)，而后者描述了車輛的運(yùn)動(dòng)學(xué)。估計(jì)過程涉及過濾每個(gè)與測(cè)量相關(guān)的噪聲，以減少估計(jì)狀態(tài)的整體不確定性。然后，EKF根據(jù)從環(huán)境中收集的測(cè)量結(jié)果，通過多次預(yù)測(cè)迭代和更新來估計(jì)系統(tǒng)的狀態(tài)，如圖5所示。圖 5 擴(kuò)展卡爾曼濾波器算法 [108]2.5.2 因子圖 SLAM [122]正如該算法的名稱所表示的那樣，一個(gè)圖用于重建環(huán)境地圖以及機(jī)器人在其中的軌跡。地圖特征和機(jī)器人姿態(tài)表示為頂點(diǎn)，并使用編碼兩種非線性約束的邊連接：運(yùn)動(dòng)和測(cè)量，如圖 6a 所示。所有約束的總和使 SLAM 成為非線性最小二乘問題。為了獲得全局一致的估計(jì)，首先對(duì)所有約束進(jìn)行線性化，產(chǎn)生稀疏信息矩陣和信息向量。由于矩陣的稀疏性和為了更有效的計(jì)算，使用變量消除算法減小了矩陣的大小。然后采用推理方法來找到圖節(jié)點(diǎn)的姿勢(shì)分配，從而最大限度地減少由約束施加的誤差。或者，可以單獨(dú)使用環(huán)境中的連續(xù)機(jī)器人姿勢(shì)來利用姿勢(shì)圖[119]估計(jì)機(jī)器人的位置。該問題中使用的圖包括作為節(jié)點(diǎn)的機(jī)器人姿態(tài)和作為這些節(jié)點(diǎn)之間的邊的運(yùn)動(dòng)約束，如圖 6b 所示。捆綁調(diào)整 (BA) [125] 是因子圖 SLAM 的一個(gè)實(shí)例，可以定義為一個(gè)細(xì)化過程，該過程使用從環(huán)境中收集的一系列圖像同時(shí)優(yōu)化圖 6 因子和位姿圖示例3D 結(jié)構(gòu)、相機(jī)軌跡以及可能的校準(zhǔn)參數(shù)，如圖所示在圖 7 中。評(píng)估系統(tǒng)誤差的成本函數(shù)被最小化，以產(chǎn)生改進(jìn)的重建估計(jì)。如果在估計(jì)中考慮了自機(jī)器人任務(wù)開始以來的所有測(cè)量，則該過程稱為 GBA，并且已知計(jì)算量很大，這會(huì)阻礙在線操作 [36]。在 [87] 中提出了一種計(jì)算效率更高的方法，即增量調(diào)整 3維重建和相機(jī)軌跡，稱為局部束調(diào)整 (LBA)。在接收到新測(cè)量時(shí)，僅調(diào)整 n 個(gè)最近幀的窗口。使用 LBA 可以實(shí)時(shí)執(zhí)行 SLAM。圖 7 束調(diào)整示例ParallaxBA 是 [145] 中提出的 BA 的另一種變體，其中使用視差角而不是其歐幾里得坐標(biāo)或反深度對(duì)特征進(jìn)行參數(shù)化，ParallaxBA 在準(zhǔn)確性和收斂性方面優(yōu)于傳統(tǒng) BA。2.5.3 增量平滑和映射增量平滑和映射是一種 SLAM 方法，它在從環(huán)境中收集測(cè)量值的同時(shí)逐漸計(jì)算地圖和機(jī)器人軌跡的估計(jì)。文獻(xiàn)中提出了幾種方法，其中最流行的是 iSAM [61] 和 iSAM2 [60]。iSAM 使用平方根信息矩陣的 QR 因子分解來執(zhí)行平滑，而 iSAM2 對(duì)稱為貝葉斯樹的新數(shù)據(jù)結(jié)構(gòu)進(jìn)行操作，該數(shù)據(jù)結(jié)構(gòu)是從因子圖中獲得的。