基于CSAO地圖融合的同步定位與建圖

何世瓊，陳雨，周青華

（1.四川大學(xué)電子信息學(xué)院，成都 610065；2.四川大學(xué)空天科學(xué)與工程學(xué)院，成都 610065）

0 引言

基于視覺的同時(shí)定位與地圖構(gòu)建技術(shù)作為機(jī)器人感知未知環(huán)境的核心技術(shù)，也是實(shí)現(xiàn)機(jī)器人自動(dòng)化的關(guān)鍵技術(shù)［1］。視覺傳感器作為機(jī)器人的“眼睛”，在沒有先驗(yàn)的環(huán)境中為機(jī)器人的自主感知提供了可行性。傳統(tǒng)雷達(dá)、聲吶等測(cè)距儀器無(wú)法獲得精確豐富的信息，視覺傳感器克服了傳統(tǒng)傳感器體積大、精度低等缺點(diǎn)［2］，可以為機(jī)器人獲取更全面的環(huán)境信息。今天，SLAM技術(shù)已經(jīng)成為機(jī)器人自主導(dǎo)航、自動(dòng)駕駛、無(wú)人機(jī)導(dǎo)航等領(lǐng)域最關(guān)鍵的技術(shù)之一。

2007年，Davison提出了第一個(gè)單目視覺SLAM系統(tǒng)MonoSLAM［3］。同年，Klein等人提出了PTAM（parallel tracking and mapping）［4］，該算法首次提出視覺SLAM中區(qū)分前后端的概念，實(shí)現(xiàn)了跟蹤與建圖的并行化。PTAM第一次使用了非線性優(yōu)化，在視覺SLAM的研究歷程中具有重要意義，此后，視覺SLAM的后端以非線性優(yōu)化為主要手段。2014年Forster等人提出了基于稀疏直接法的SVO（semi-direct visual odoemtry）［5］，SVO跟蹤一些關(guān)鍵點(diǎn)并根據(jù)其周圍的信息估計(jì)相機(jī)運(yùn)動(dòng)和位置。為了實(shí)現(xiàn)算法的效率和輕量化，SVO不具備回環(huán)檢測(cè)和后端優(yōu)化功能，因此建圖效果差并且存在累計(jì)誤差。

ORB-SLAM系列是基于特征的視覺SLAM的優(yōu)秀代表，2015年ORB-SLAM［6］被Mur-Artal等人提出。它不僅支持單目、雙目和RGBD三種模式，還創(chuàng)新地提出了使用三線程來(lái)完成SLAM。在ORB-SLAM的基礎(chǔ)上，該團(tuán)隊(duì)在2017年提出了ORB-SLAM2［7］，它能實(shí)現(xiàn)地圖重用和回環(huán)檢測(cè)，在后端采用基于單目和雙目的非線性光束平差（bundle adjustment，BA），ORB-SLAM2中還提出了重定位，其將地圖中特征點(diǎn)的數(shù)量小于某個(gè)閾值時(shí)視為丟失跟蹤，該方法在相機(jī)平穩(wěn)運(yùn)行并且紋理豐富的地圖中表現(xiàn)出良好的精度。但是，由于該算法的地圖管理只針對(duì)單張地圖，所以重定位失敗的情況常有發(fā)生，從而造成構(gòu)建的全局地圖精度下降。2018年該團(tuán)隊(duì)在其基礎(chǔ)上增加了一個(gè)魯棒的多地圖處理系統(tǒng)Atlas［8］，第二年，整合了ORB-SLAM系列功能和最大后驗(yàn)概率的ORB-SLAM3［9］被公開發(fā)表，該算法串聯(lián)了前兩代的工作并且增加了針孔和魚眼相機(jī)模型，創(chuàng)新地在IMU初始化階段引入了MAP，大大提高了算法的魯棒性，但是由于其依賴ORB特征，在實(shí)時(shí)性上依然存在不足。

綜上，本文提出一種基于地圖融合的Mutimap-SLAM算法，該算法在Atlas多地圖系統(tǒng)的基礎(chǔ)上，使用CSAO地圖融合方法，CSAO先計(jì)算出關(guān)鍵幀位姿之間的轉(zhuǎn)換關(guān)系，然后Atlas中的多張子地圖依據(jù)該轉(zhuǎn)換關(guān)系實(shí)現(xiàn)地圖融合，以此來(lái)提高算法在困難場(chǎng)景中的精度和性能。最后，通過(guò)實(shí)驗(yàn)驗(yàn)證了該方法的可行性和算法的精度。

1 基于CSAO地圖融合的SLAM算法

1.1 基于At l as的ORB-SLAM算法

Atlas多地圖系統(tǒng)是一個(gè)由多張子地圖組成的地圖集，如圖1所示，將整體地圖部分劃分為活躍地圖（active map）和非活躍地圖（inactive map），活躍地圖是指新創(chuàng)建的關(guān)鍵幀所更新的那個(gè)地圖。Atlas中的每張子地圖都有自己的關(guān)鍵幀、地圖點(diǎn)、共視圖和生成樹，并且所有子地圖信息都被存儲(chǔ)在一個(gè)DBoW數(shù)據(jù)庫(kù)中。

圖1 多地圖系統(tǒng)示意圖

在Atlas多地圖系統(tǒng)中，一方面Active Map部分作為當(dāng)前工作模塊，用于和關(guān)鍵幀進(jìn)行匹配和融合。另一方面，為了降低特征匹配的計(jì)算量，InActive Map部分不需要匹配。在ORB-SLAM特征跟蹤過(guò)程中，每次關(guān)鍵幀的插入都需要與全局地圖中的詞袋數(shù)據(jù)庫(kù)進(jìn)行對(duì)照。如果與當(dāng)前幀匹配上的關(guān)鍵幀位于非活躍地圖中，則當(dāng)前活躍的地圖吞并對(duì)應(yīng)的非活躍地圖。

Atlas多地圖系統(tǒng)讓SLAM算法即使在特征點(diǎn)缺失的情況下，也可以通過(guò)不斷地生成關(guān)鍵幀來(lái)實(shí)現(xiàn)重定位找回。增加了Atlas多地圖系統(tǒng)的ORB-SLAM3算法結(jié)構(gòu)如圖2所示，該算法可以在保證算法的魯棒性的同時(shí)，提高建圖精度。

圖2 ORB-SLAM3算法結(jié)構(gòu)

1.2 CSAO地圖融合

為了獲得更精確的全局地圖，需要將非活躍地圖的信息補(bǔ)充到當(dāng)前活躍地圖中，馬樹軍等［10］提出了一種在后端利用卡爾曼濾波融合子地圖的方法，但是因?yàn)榛诳柭鼮V波框架，導(dǎo)致算法的復(fù)雜度高并且對(duì)數(shù)據(jù)的關(guān)聯(lián)錯(cuò)誤十分敏感。本文提出單位四元數(shù)求絕對(duì)旋轉(zhuǎn)的封閉解（CSAO）的方式，先計(jì)算出關(guān)鍵幀之間的映射關(guān)系，再根據(jù)映射實(shí)現(xiàn)地圖融合。假設(shè)當(dāng)前Active Map中的關(guān)鍵幀表示為Ka，與之對(duì)應(yīng)的Inactive Map待吞并關(guān)鍵幀表示為Ks，當(dāng)前活躍地圖要融合非活躍地圖，需要通過(guò)計(jì)算出兩幀之間的變換關(guān)系，后者即可通過(guò)該變化關(guān)系映射到前者，從而實(shí)現(xiàn)地圖融合。

將求解變換關(guān)系拆分為求解平移、旋轉(zhuǎn)、和尺度因子三個(gè)部分。假設(shè)當(dāng)前有n對(duì)匹配的三維點(diǎn)，記為和，將兩個(gè)位置的三位點(diǎn)構(gòu)建新的三維坐標(biāo)記為和，則誤差項(xiàng)可以表示為公式（1），

其中，

則代價(jià)函數(shù)可以寫為公式（3），

將公式（3）展開可得公式（4），

此時(shí)，求解姿態(tài)的轉(zhuǎn)換關(guān)系已經(jīng)簡(jiǎn)化為求解旋轉(zhuǎn)量，再根據(jù)旋轉(zhuǎn)量即可求得平移和尺度因子。旋轉(zhuǎn)用四元數(shù)Rrl表示，因?yàn)閮蓚€(gè)四元數(shù)的向量積結(jié)果不一定時(shí)純虛數(shù)，所以用來(lái)表示，于是新的代價(jià)函數(shù)可以表示為公式（7）。此時(shí)，即可構(gòu)造M矩陣公式（8）求出矩陣公式N（9），N的最大特征向量即為所求旋轉(zhuǎn)四元數(shù)。

1.3 絕對(duì)軌跡誤差

本文使用軌跡地圖的絕對(duì)軌跡誤差（absolute trajectory error，ATE）來(lái)評(píng)價(jià)算法精度，直接計(jì)算估計(jì)位姿與真實(shí)位姿之間的差值，避免了里程計(jì)的平移誤差和旋轉(zhuǎn)誤差的計(jì)算，減小計(jì)算量的同時(shí)能直觀地反映算法精度和軌跡全局一致性。第i幀的ATE定義如公式（10）。

其中，

公式（11）中Qi∈SE(3)表示算法的真實(shí)位姿，Pi∈SE(3)表示算法的估計(jì)位姿。將兩幀之間的時(shí)間間隔用Δ表示，使用均方根誤差RMSE統(tǒng)計(jì)公式（11）得到公式（10），其中trans(Fi)表示每?jī)蓭g的平移誤差。

2 實(shí)驗(yàn)結(jié)果與分析

2.1 實(shí)驗(yàn)環(huán)境與數(shù)據(jù)集

該算法在Intel Core i7（4核@3.6 Hz）臺(tái)式計(jì)算機(jī)上測(cè)試，程序在ubuntu18.04上運(yùn)行，涉及的軟件有Pangolin、Opencv、PCL、Eigen 3和g2o等開源庫(kù)，還使用了evo工具來(lái)評(píng)估系統(tǒng)的性能。實(shí)驗(yàn)使用的是由蘇黎世聯(lián)邦理工學(xué)院計(jì)算機(jī)視覺組采集制作的雙目+IMU數(shù)據(jù)集［11］，包含了普通房間和machine hall兩個(gè)場(chǎng)景，實(shí)驗(yàn)所用數(shù)據(jù)集詳細(xì)信息見表1。

表1 實(shí)驗(yàn)采用的數(shù)集

2.2 全局地圖精度

由于1—3號(hào)簡(jiǎn)單數(shù)據(jù)集中的場(chǎng)景紋理豐富，ORB-SLAM和MultiMap-SLAM得到的RMSE ATE相似，無(wú)法突出MultiMap-SLAM的優(yōu)越性。選擇4—7號(hào)困難數(shù)據(jù)集對(duì)比兩種算法的建圖精度，實(shí)驗(yàn)結(jié)果如表2所示。

表2 ORB-SLAM和MultiMap-SLAM在Euroc數(shù)據(jù)集中的RMSE ATE比較

在4號(hào)數(shù)據(jù)集中，MultiMap-SLAM的ATE略高于ORB-SLAM，但是基本持平；在5號(hào)數(shù)據(jù)集中，MultiMap-SLAM的ATE最大值為0.113，中值為0.071，最小值為0.009，相較ORB-SLAM的0.133，0.060，0.007分 別 下 降 了15.00%，15.49%，22.22%；在6號(hào)數(shù)據(jù)集中，MultiMap-SLAM的ATE最大值為0.027，中值為0.015，最小值為0.003，相比ORB-SLAM的0.401，0.212，0.007分別下降了93.26.00%，92.93%，57.14%；在7號(hào)數(shù)據(jù)集中，MultiMap-SLAM的ATE最大值為0.051，中值為0.027，最小值為0.003，相較ORB-SLAM的0.401，0.212，0.007分別下降了84.21.00%，83.92%，0.00%。此外，圖3展示了兩種算法構(gòu)建的全局地圖，結(jié)果表明，基于CSAO地圖融合的MultiMap-SLAM能有效降低全局地圖的絕對(duì)軌跡誤差，獲得更精確的全局軌跡地圖。

圖3 估計(jì)軌跡與真實(shí)軌跡的絕對(duì)軌跡誤差

綜上可知，在特征缺失的困難數(shù)據(jù)集中，本文提出的基于地圖融合的算法具有有效性，能將建圖精度提高至少30%，并得到更準(zhǔn)確的軌跡地圖。

2.3 Mut i map-SLAM算法性能

本文將算法的性能定義為多個(gè)會(huì)話窗口同時(shí)運(yùn)行該算法時(shí)，算法所表現(xiàn)出的精度。先單獨(dú)用1—5號(hào)數(shù)據(jù)集運(yùn)行Mutimap-SLAM得到RMSE ATE的結(jié)果，再同時(shí)用1—5號(hào)數(shù)據(jù)集運(yùn)行Mutimap-SLAM得到RMSE ATE結(jié)果，直接引用VINSstereo［12］和VINSMonocular Inertial［13］原 文 中的性能測(cè)試數(shù)據(jù)進(jìn)行對(duì)比。如表3所示，Mutimap-SLAM的平均RMSE ATE為0.074，VINS Monocular Inertial的平均RMSE ATE為0.210，相比之下Mutimap-SLAM將多會(huì)話RMSE ATE降低了64.7%。

表3 Mutimap-SLAM與VINS-SLAM的多會(huì)話性能比較

此外，將Mutimap-SLAM與CCM-SLAM進(jìn)行比較驗(yàn)證其多會(huì)話性能，CCM-SLAM［14］是一個(gè)基于ORB-SLAM的分布式多無(wú)人機(jī)地圖的系統(tǒng)，整個(gè)系統(tǒng)包括一個(gè)中央服務(wù)終端和多個(gè)運(yùn)行在各個(gè)無(wú)人機(jī)上的子端，每個(gè)子端都可以獨(dú)立自主地運(yùn)行SLAM前端視覺里程計(jì)，建立局部的子地圖，服務(wù)終端則用來(lái)處理子端的地圖數(shù)據(jù)。實(shí)驗(yàn)先在將1、2、3號(hào)數(shù)據(jù)集在本算法中同時(shí)運(yùn)行，再與CCM-SLAM服務(wù)器端同時(shí)處理三個(gè)數(shù)據(jù)集的地圖信息對(duì)比，得到全局地圖的RMSE ATE如表4所示，Mutimap-SLAM（Mono）的單目和雙目全局地圖RMSE ATE分別為0.026和0.037，相比CCMSLAM的0.77提高了2～3倍。

表4 Mutimap-SLAM與CCM-SLAM的多會(huì)話性能比較

上述實(shí)驗(yàn)結(jié)果均表明，即使與以性能著稱的SLAM算法對(duì)比，Mutimap-SLAM算法也表現(xiàn)出良好的性能。

3 結(jié)語(yǔ)

本文在基于ORB-SLAM算法基礎(chǔ)上，基于CSAO地圖融合方法，提出了一種使用多地圖系統(tǒng)的視覺SLAM算法Mutimap-SLAM。通過(guò)相同數(shù)據(jù)集上的對(duì)比實(shí)驗(yàn)，得到以下結(jié)論。

（1）CSAO地圖融合方法應(yīng)用在ORB-SLAM中具有可行性，可以實(shí)現(xiàn)同時(shí)定位與建圖。

（2）在特征稀少的困難場(chǎng)景中，基于CSAO地圖融合方法的Mutimap-SLAM可以提高全局建圖精度。

（3）相比性能良好的視覺SLAM算法，Mutimap-SLAM依然可以保持較高的性能。