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

何世瓊，陳雨，周青華

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

0 引言

基于視覺的同時定位與地圖構建技術作為機器人感知未知環境的核心技術，也是實現機器人自動化的關鍵技術［1］。視覺傳感器作為機器人的“眼睛”，在沒有先驗的環境中為機器人的自主感知提供了可行性。傳統雷達、聲吶等測距儀器無法獲得精確豐富的信息，視覺傳感器克服了傳統傳感器體積大、精度低等缺點［2］，可以為機器人獲取更全面的環境信息。今天，SLAM技術已經成為機器人自主導航、自動駕駛、無人機導航等領域最關鍵的技術之一。

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

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

綜上，本文提出一種基于地圖融合的Mutimap-SLAM算法，該算法在Atlas多地圖系統的基礎上，使用CSAO地圖融合方法，CSAO先計算出關鍵幀位姿之間的轉換關系，然后Atlas中的多張子地圖依據該轉換關系實現地圖融合，以此來提高算法在困難場景中的精度和性能。最后，通過實驗驗證了該方法的可行性和算法的精度。

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

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

Atlas多地圖系統是一個由多張子地圖組成的地圖集，如圖1所示，將整體地圖部分劃分為活躍地圖（active map）和非活躍地圖（inactive map），活躍地圖是指新創建的關鍵幀所更新的那個地圖。Atlas中的每張子地圖都有自己的關鍵幀、地圖點、共視圖和生成樹，并且所有子地圖信息都被存儲在一個DBoW數據庫中。

圖1 多地圖系統示意圖

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

Atlas多地圖系統讓SLAM算法即使在特征點缺失的情況下，也可以通過不斷地生成關鍵幀來實現重定位找回。增加了Atlas多地圖系統的ORB-SLAM3算法結構如圖2所示，該算法可以在保證算法的魯棒性的同時，提高建圖精度。

圖2 ORB-SLAM3算法結構

1.2 CSAO地圖融合

為了獲得更精確的全局地圖，需要將非活躍地圖的信息補充到當前活躍地圖中，馬樹軍等［10］提出了一種在后端利用卡爾曼濾波融合子地圖的方法，但是因為基于卡爾曼濾波框架，導致算法的復雜度高并且對數據的關聯錯誤十分敏感。本文提出單位四元數求絕對旋轉的封閉解（CSAO）的方式，先計算出關鍵幀之間的映射關系，再根據映射實現地圖融合。假設當前Active Map中的關鍵幀表示為Ka，與之對應的Inactive Map待吞并關鍵幀表示為Ks，當前活躍地圖要融合非活躍地圖，需要通過計算出兩幀之間的變換關系，后者即可通過該變化關系映射到前者，從而實現地圖融合。

將求解變換關系拆分為求解平移、旋轉、和尺度因子三個部分。假設當前有n對匹配的三維點，記為和，將兩個位置的三位點構建新的三維坐標記為和，則誤差項可以表示為公式（1），

其中，

則代價函數可以寫為公式（3），

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

此時，求解姿態的轉換關系已經簡化為求解旋轉量，再根據旋轉量即可求得平移和尺度因子。旋轉用四元數Rrl表示，因為兩個四元數的向量積結果不一定時純虛數，所以用來表示，于是新的代價函數可以表示為公式（7）。此時，即可構造M矩陣公式（8）求出矩陣公式N（9），N的最大特征向量即為所求旋轉四元數。

1.3 絕對軌跡誤差

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

其中，

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

2 實驗結果與分析

2.1 實驗環境與數據集

該算法在Intel Core i7（4核@3.6 Hz）臺式計算機上測試，程序在ubuntu18.04上運行，涉及的軟件有Pangolin、Opencv、PCL、Eigen 3和g2o等開源庫，還使用了evo工具來評估系統的性能。實驗使用的是由蘇黎世聯邦理工學院計算機視覺組采集制作的雙目+IMU數據集［11］，包含了普通房間和machine hall兩個場景，實驗所用數據集詳細信息見表1。

表1 實驗采用的數集

2.2 全局地圖精度

由于1—3號簡單數據集中的場景紋理豐富，ORB-SLAM和MultiMap-SLAM得到的RMSE ATE相似，無法突出MultiMap-SLAM的優越性。選擇4—7號困難數據集對比兩種算法的建圖精度，實驗結果如表2所示。

表2 ORB-SLAM和MultiMap-SLAM在Euroc數據集中的RMSE ATE比較

在4號數據集中，MultiMap-SLAM的ATE略高于ORB-SLAM，但是基本持平；在5號數據集中，MultiMap-SLAM的ATE最大值為0.113，中值為0.071，最小值為0.009，相較ORB-SLAM的0.133，0.060，0.007分 別 下 降 了15.00%，15.49%，22.22%；在6號數據集中，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號數據集中，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展示了兩種算法構建的全局地圖，結果表明，基于CSAO地圖融合的MultiMap-SLAM能有效降低全局地圖的絕對軌跡誤差，獲得更精確的全局軌跡地圖。

圖3 估計軌跡與真實軌跡的絕對軌跡誤差

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

2.3 Mut i map-SLAM算法性能

本文將算法的性能定義為多個會話窗口同時運行該算法時，算法所表現出的精度。先單獨用1—5號數據集運行Mutimap-SLAM得到RMSE ATE的結果，再同時用1—5號數據集運行Mutimap-SLAM得到RMSE ATE結果，直接引用VINSstereo［12］和VINSMonocular Inertial［13］原 文 中的性能測試數據進行對比。如表3所示，Mutimap-SLAM的平均RMSE ATE為0.074，VINS Monocular Inertial的平均RMSE ATE為0.210，相比之下Mutimap-SLAM將多會話RMSE ATE降低了64.7%。

表3 Mutimap-SLAM與VINS-SLAM的多會話性能比較

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

表4 Mutimap-SLAM與CCM-SLAM的多會話性能比較

上述實驗結果均表明，即使與以性能著稱的SLAM算法對比，Mutimap-SLAM算法也表現出良好的性能。

3 結語

本文在基于ORB-SLAM算法基礎上，基于CSAO地圖融合方法，提出了一種使用多地圖系統的視覺SLAM算法Mutimap-SLAM。通過相同數據集上的對比實驗，得到以下結論。

（1）CSAO地圖融合方法應用在ORB-SLAM中具有可行性，可以實現同時定位與建圖。

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

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