基于激光雷達增強的視覺SLAM 多機器人協作地圖構建方法的研究

李燊陽 鄧三鵬* 權利紅 祁宇明 丁昊然 劉天慧

（1天津職業技術師范大學機器人及智能裝備研究院，天津，300222；2 天津市智能機器人技術及應用企業重點實驗室，天津，300350；3 天津博諾智創機器人技術有限公司，天津，300350）

0 引言

隨著機器人技術的快速發展，多機器人協作系統在工業自動化、無人駕駛等領域得到廣泛應用。地圖構建作為多機器人系統中的重要任務，對于機器人的導航、定位和路徑規劃具有關鍵意義。然而，環境中的不確定性和動態性給地圖構建帶來了挑戰，特別是在復雜、大規模的場景中，這種不確定性和動態性使得多機器人協作地圖構建變得越發艱難。

為應對多機器人協作地圖構建問題，CAMPOS 等[1]提出的ORB-SLAM3 算法，在原有的SLAM 系列算法的基礎上，添加并構建了一個Atlas 地圖集和一個DBoW2關鍵幀數據庫。通過對DBoW2 關鍵幀的跟蹤匹配，實現了多機器人協作地圖構建的重定位、循環閉合和地圖合并。但ORB-SLAM3 算法僅能構建出稀疏點云地圖，地圖包含的信息量少。林沛楊等[2]提出的基于視覺SLAM多機器人協作地圖構建算法，雖構建出了稠密點云融合地圖，但在光線強度等環境因素變化的情況下，視覺SLAM 算法的魯棒性和時效性會受到較大的影響。

針對以上問題，本文提出了一種結合激光雷達和視覺傳感器的多機器人協作SLAM 方法。首先，各機器人以ORB-SLAM3 為基礎，以自身為主機器人開始建圖，利用改進的卡爾曼濾波算法計算出雷達相機的融合位姿，根據該位姿得到局部稠密點云地圖。然后，利用關鍵幀跟蹤模型和ICP 算法得到機器人之間的坐標轉換關系，進而得到各機器人的世界坐標系。最后，在世界坐標系上將局部點云拼接與濾波，得到稠密點云融合地圖。同時，本文將多個機器人進行有效的通信和協調，實現地圖數據共享和信息交換，提高了整個系統的定位精度和地圖構建精度。

1 單機器人局部地圖的創建

1.1 ORB-SLAM3 簡介

ORB-SLAM3 是在基于ORB 特征的三維定位與地圖構建算法ORB-SLAM2 和ORB-SLAM-VI 的基礎上擴展改進得到的開源的、支持多種相機的實時視覺SLAM 系統。它能夠借助針孔和魚眼模型，使用單目、立體或紅綠藍—深度（RGB-D）傳感器在純視覺或者視覺慣導模式下工作。相關實驗表明，ORB-SLAM3 在EuRoC 數據集上的單目慣性單會話操作比VINS-Mono（一個基于視覺慣性里程計的閉環和多重地圖功能的單目視覺系統）準確2.6 倍，且ORB-SLAM3 的地圖集基于DBoW2 關鍵幀數據庫構建，采用了具有高召回率的地點識別技術[3]。ORB-SLAM3 的整體框圖如圖 1 所示，圖中，IMU、BA、Sim3、SE3 分別是慣性測量單元（Inertial Measurement Unit）、束調整（Bundle Adjustment）、相似變換（Similarity Transformation in 3D）、 特殊歐幾里德群（Special Euclidean Group in 3D）的英文首字母縮寫。

圖1 ORB-SLAM3 框圖

1.2 激光雷達與視覺傳感器的數據融合

現有的激光雷達融合視覺傳感器的做法都是在SLAM 前端的集成部分進行了傳感器的數據融合[4]。ORB-SLAM3 在ORB-SLAM2 的基礎上進行了擴展，現有ORB-SLAM3 的前端數據融合被稱為RGB-D 模式，該模式是在ORB-SLAM3 系統框架外部進行相機與雷達數據的實時處理，將處理完成的數據放入ORB-SLAM3 系統模型中，進而估計出相機的位姿和地圖點位置。這種模式下，由于傳感器數據在系統模型外部處理，導致系統整體過于松散，時效性較差。

RGB-D 模式可以將之前生成和保存的深度圖進行前端的數據集成，即將雷達傳感器的點云數據（pcd）通過對應節點轉換成深度圖[5]，通過SLAM 接口集成進去，RGB-D 模式的數據融合流程圖如圖 2 所示。

圖2 RGB-D 數據融合流程圖

本文所設計的方法使用已開源的Depth 模塊，在RGB-L（L 為雷達（LiDAR）的英文首字母）模式下使用傳統的計算機視覺（Computer Vision，CV）在線對激光雷達數據進行上采樣，將采樣后的結果提供給SLAM接口，交由直接集成到ORB-SLAM3 的Depth 模塊進行處理[6]。RGB-L 數據融合流程圖如圖3 所示。

圖3 RGB-L 數據融合流程圖

圖4 基本原理圖

圖5 多機器人協作融合框圖

圖6 Gazebo 仿真環境

圖7 局部地圖和融合地圖

Depth 模塊通過擴展卡爾曼濾波器（EKF）進行實現。在機器人的實際運動中，相機和雷達的傳感器位姿估計可能出現其中一個失效的情況。當實際環境不夠明亮時，單獨以雷達點云信息完成位姿估計，而當環境明亮時，將同時使用兩種傳感器，由相機的視覺數據提供地圖點的視覺觀測，激光雷達數據提供關于地圖點的距離和角度觀測，通過不斷的觀測和預測，對系統的狀態進行估計和更新，進而結合ORB-SLAM3 系統模型，估計相機的位姿和地圖點位置?；驹砣鐖D 4 所示。

默認兩個傳感器都符合高斯分布，系統的方程為非線性可微分函數。觀測模型采用雷達和相機的觀測模型，而相機的位姿需要轉換到雷達的極坐標系下，因此系統采用雷達、相機聯合標定的轉換矩陣進行轉換，通過傳感器模型實現觀測信息的更新[7]。

2）預測步驟：根據上一個時刻的狀態估計和控制輸入，使用傳感器模型對相機載體的狀態進行預測。將時間段內的位姿增量作為輸入，和分別代表相鄰的時間戳：

3）更新步驟：將相機和激光雷達的測量數據與預測的狀態進行對比，利用卡爾曼濾波的思想來融合測量和預測信息。EKF 使用線性化的近似方法來處理非線性的測量和狀態模型。通過計算卡爾曼增益，將測量數據的可靠性與預測數據進行權衡，更新目標的位姿狀態估計。

更新后的觀測方程為：

通過RGB-L 模式下得到的機器人精準的位姿和深度圖像，ORB-SLAM3 可以將稀疏特征點轉換為稠密的3D 點云。通過調整ORB-SLAM3 的參數來進行稠密點云地圖的構建，即設置參數“DENSE_RECONSTRUCTION=true”，使用半全局匹配（Semi-Global Matching， SGM）算法來估計連續幀之間的稀疏特征點的深度值。最后，通過積分這些深度值構建出每個機器人的局部稠密點云地圖。

2 多機器人協作地圖的構建

本文所提出的基于激光雷達增強的視覺SLAM 多機器人地圖構建方法，分別在各自的機器人上，通過建立世界坐標系將局部地圖進行完整地圖的融合和構建。

2.1 世界坐標系的建立

在上述提到的ORB-SLAM3 主體框架里，存在著存放不同機器人關鍵幀的數據庫DBoW2。數據庫中相鄰關鍵幀的位姿估計可以由關鍵幀跟蹤模型和ICP 算法求得[7-8]。利用其估計的位姿可以將構建世界坐標系的過程分為如下步驟：

1）將協作地圖構建的機器人放置在一起，并將機器人雷達相機朝向同一方向，使得機器人相機存在共識關系，以捕捉到足夠多的特征點；

2）啟動機器人各自相機的launch 文件，將觀測到的關鍵幀通過ORB-SLAM3 系統處理后存放到DBoW2 的數據庫中；

3）將各機器人DBoW2 關鍵幀數據庫中存放的關鍵幀作為自身的基礎幀，各機器人使用對應的基礎幀進行世界坐標系的構建；

4）在構建完各自的世界坐標系后，機器人使用上文所設計的RGB-L 模塊進行稠密點云地圖的創建。

2.2 多機器人協作地圖的融合

根據上文所述，各個機器人得到各自協作地圖構建系統的世界坐標系后，利用融合RGB-L 的ORB-SLAM3算法進行整體稠密點云地圖的創建。多機器人協作融合框圖如圖 5 所示。

地圖融合創建的步驟如下：

1）在關鍵幀數據庫中讀取雷達點云和相機圖片數據；

2）在相機圖片關鍵幀具有共識關系的情況下，激活第二代機器人操作系統（Robot Operating System2,ROS2）相關的數據傳輸話題，根據關鍵幀追蹤模型和ICP 算法計算出它們的坐標轉換關系[9]，再根據坐標轉換關系進行機器人世界坐標系的創建；

3）利用改進的卡爾曼濾波進行雷達、相機位姿的融合，獲得機器人的位姿狀態，通過積分深度圖，獲得局部稠密點云地圖；

4）以步驟2）中的世界坐標系為基礎，利用位姿轉換關系進行局部地圖的映射轉換，將其映射到世界坐標系中，融合擴展為全局稠密點云地圖。

3 實驗結果及分析

本次實驗在ROS2 通信框架和Gazebo 仿真平臺上進行，ROS2 利用了分布式通信架構和域ID（ROS_DOMAIN_D）技術，不同機器人的節點只要在同一局域網內，就能實現節點間的相互通信[10]。Gazebo 是一款機器人動力學仿真軟件，其內嵌于ROS2 中，提供高保真度的物理環境模擬與一套完整的傳感器模型，并具備友好的用戶交互界面，用戶可以根據需求進行準確的動力學仿真和數據模擬。

實驗利用亞馬遜物流機器人的書店場景模型模擬地圖構建的場景，如圖 6 所示。利用機器人模型讀取雷達和相機的數據，將雷達和相機的數據輸入到ORB-SLAM3框架中進行處理，然后將處理完的數據進行融合。

根據設計的協作地圖構建方法進行地圖融合。在地圖構建過程中，各模塊運行時間如表1 所示。由表1 可知，本文的融合方法處理機器人的每一幀關鍵數據幀所需的時間不長，各個機器人的數據處理并行運行，使得算法運行效率較高，時效性好。

表1 各模塊運行時間

機器人各自構建的局部地圖和融合地圖如圖 7所示。本方法在燈光較暗的情況下依舊可以保持局部的創建，但地圖邊緣較為模糊不清，具體原因不明，有待進一步實驗研究。在整體地圖的融合上，本方法可以較好地實現融合且誤差較小，總體上實現了魯棒性較好的激光雷達增強視覺SLAM 建圖。

4 結束語

本文提出了一種基于激光雷達增強的視覺SLAM 多機器人協作地圖構建方法，將激光雷達和視覺傳感器的數據進行了融合，避免環境光照等因素的影響，得到機器人的精確位姿數據。本文的地圖構建方法將存在共識關系的機器人利用關鍵幀跟蹤和ICP 算法進行機器人的位姿變換，得到機器人全局世界坐標系。最后在全局世界坐標系上進行了稠密點云地圖的融合擴展。在Gazebo仿真平臺上進行了實驗驗證，結果表明，所提出的方法具有較好的時效性和魯棒性。