基于輪式機器人的實時3D柵格地圖構建

劉安睿劼，王耀力

太原理工大學 信息與計算機學院，太原 030024

1 引言

SLAM（Simultaneous Localization and Mapping）同時定位與地圖構建是機器人自主導航中的一個非常重要的領域，它是機器人在未知環境中通過傳感器信息確定自身的空間位置，并建立所處空間的環境模型。隨著機器人技術的不斷發展，作為機器人的一個重要分支，SLAM技術也越來越重要。無論是室內機器人導航還是旋翼無人機自主導航又或是現在火熱的自動駕駛，SLAM 技術都作為高效的建圖和定位工具。傳統的SLAM 技術大多采用激光雷達或者聲吶系統進行二維地圖的創建[1-3]。雖然現在激光雷達在室內和室外應用不懼任何挑戰，但是由于激光雷達成本過高，所以它不能夠在低成本系統中應用。因此SLAM 領域出現了以視覺傳感器為基礎的VSLAM（Visual Simultaneous Localization and Mapping）。VSLAM 能夠對環境中的復雜結構進行完整的表示。現在比較流行的系統有MSCKF[4]、OKVIS[5]、LSD-SLAM[6]、ORB_SLAM2[7-8]、VINS-Mono[9]等。但是這些系統只是簡單地輸出了位姿軌跡和環境三維點云，并沒有具體構建能用于機器人導航的地圖。

Santana 等人[10]提出了一種基于單目相機使用單應矩陣構建2D 地圖的方法，該方法在分割的時候判斷圖像的特征點是否在同一平面來快速判斷是否有障礙物，但是由于單目視覺沒有尺度信息，所以建立的2D 柵格地圖并不適用于現實中的機器人定位和導航。Dia 等人[11]提出了一種新型的用于占有網格地圖的逆傳感器模型（ISM），但是并沒有SLAM 方面的實際應用。Hull[12]提出了一種基于LSD-SLAM 的實時占據網格建圖，與文獻[10]的方法類似，使用平面信息構建單目SLAM 地圖，利用點云地圖投影的方式生成柵格地圖。Gregorio 等人[13]提出了一個高效的機器人導航建圖框架，一種3D 柵格地圖構建算法，但是其計算量太大，對于大型機器人系統來說，并不希望部分功能占用太多系統資源。

ORB-SLAM2[7-8]是一個比較完善的VSLAM 系統，包含跟蹤、局部建圖、回環檢測和全局BA，并且提供RGB-D相機的使用接口，但是它只能輸出關鍵幀、稀疏點云和位姿信息。稀疏點云并不能用于輪式機器人的導航，因此針對ORB_SLAM2 的這些缺點，利用它提供的關鍵幀、位姿信息以及RGB-D 相機接口，通過3D 柵格地圖構建算法生成可用于輪式機器人導航的3D柵格地圖。

2 逆傳感器模型

機器人在沒有先驗的未知環境中，通過自身的傳感器建立考慮噪聲和不確定因素的概率分布模型來估計外部環境，這個模型稱為傳感器模型（Sensor Model，SM）。在傳感器觀測到外部環境數據之后，利用這些數據建立環境地圖的模型稱為逆傳感器模型（Inverse Sensor Model，ISM）

逆傳感器模型最初由Elfes[14]在1989 年提出，后來由Thrun[15]進行了詳細推導。ISM模型主要是利用傳感器的測量數據來表示占用網格地圖的單元格是否被占用的概率。從傳感器角度來說，它與測量的障礙物之間的所有單元格都需要一個概率值，該概率值表示空閑或被占據的二元狀態的可能性。超出障礙物的任何單元格或者并沒有被傳感器觀測到的單元格都可以被視為未知。傳感器測量通常會受到某種形式的噪聲和不確定性的影響，所以單元格的概率狀態分配有助于將噪聲和不確定性結合到地圖中[16]。

2.1 理想的逆傳感器模型

進行ISM 模型建立之前，先需要做出一些假設，使得傳感器模型易于構建。

假設1 獨立性假設。每個單元格的占用概率獨立于地圖中的每個其他單元。

假設2 使用高斯分布來表示觀測數據的噪聲。

噪聲測量的高斯概率分布函數，用于模擬傳感器測量的噪聲。方差σz表示測量z的不確定性，主要由正態分布曲線的寬度表示。方差越大表明測量的不確定性越大，導致曲線越寬。相反，較小的方差表明測量的不確定性較小，曲線較薄。

理想的逆傳感器模型是在無噪聲、完美的傳感器的前提下建模的。如圖1 所示。在網格地圖中，mi代表相應的網格單元是否被占用的隨機變量。P(mi)代表i單元格的占用概率。P(mi)=0 代表絕對空閑，P(mi)=1代表絕對占據，P(mi)=0.5 代表未知。在這種理想情況下，在障礙物或距離測量之前的每個單元被設置為空閑，并且在障礙物之后的每個單元被設置為未知。在障礙物的實際范圍測量中，設置緩沖范圍L以考慮測量不以網格內的單元為中心的可能性。使用進行測量的單元的中心計算到傳感器的距離。理想情況下，在此單元格內的任何位置進行的測量都會為此單元格指定1。

理想的逆傳感器模型可表示為：

其中，r表示傳感器的觀測范圍，L為緩沖范圍；區間內g(r)=0 表示傳感器沒有觀測到障礙物。z-區間內g(r)=1 表示障礙物已經被傳感器觀測到。otherwiser>0 區間內g(r)=0.5 代表未知，傳感器無法對該區域進行觀測。r必須為正的，因為傳感器永遠不應該返回它后面的測量值。緩沖范圍L用來考慮測量不以網格內的單元為中心的情況。通常L的大小選擇為單元格對角之間的距離。

范圍測量z包含高斯噪聲，因此可以寫成：

標準的噪聲測量的高斯概率分布函數如圖2所示，其中r為沿測量線到測量z的范圍，所以：

圖2 具有正態分布不確定性的高斯概率分布函數（方差σ=0.5）

2.2 實際逆傳感器模型

實際中的ISM 模型是將高斯噪聲融入理想傳感器模型得到的，所以對它們進行卷積即可得到實際ISM模型：

由公式（1）能很明顯地看出來g(r)是分段函數，因此必須以分段的方式進行模型的卷積，這意味著g(r)必須分割積分，因此完整的卷積寫成：

其中r的范圍為(a,b)，并定義分段代表的單元格占用概率，所以可以得到：

取誤差函數：

所以公式（7）可以寫成：

這時候考慮g(r)的分段特性，由公式（1）給出，所以可以得到完整的分段卷積：

公式（11）顯示了最終推導出來的逆傳感器模型公式。

高斯逆傳感器模型如圖3所示。

2.3 逆傳感器模型參數選擇

2.3.1L值的選取

在評估L值對ISM模型的影響時，采用固定變量的方法。首先固定z=2 m 值和高斯噪聲模型σ=0.15，通過改變L值的大小，評估ISM 的變化。不同L值對ISM模型的影響，如圖4所示。

圖4 顯示了不同的L值如何縮小或者加寬曲線的峰值。隨著L值的增大，波峰高度在不斷上升，說明ISM 模型的網格單元被占用的概率在不斷增大。因此如果L值過大，那么將會有更多的單元格被判斷為有障礙物，現實中沒有障礙物的地方可能會被判斷成有障礙物，造成比較大的偏差。如果L值過小，就會釋放比較多的單元格變成未占用，可能會把有障礙物的單元格釋放，造成誤差。通過實驗，當L的取值為單元格的對角線長度的時候，概率偏差最小[17]。

2.3.2σ值對ISM的影響

在研究σ值對ISM 的影響時，先固定變量z=2 m和緩沖范圍L=0.5 m，通過改變σ的不同值，評估ISM模型。對于ISM 模型來說標準偏差σ表示占用預測的不確定性。如圖5所示，較小的σ值對應較高較窄的峰值，較大的σ值對應較低較寬的峰值。隨著σ值的不斷增大，不確定因素和噪聲的影響越來越大，實際測量周圍的更多單元可能被認為是占用的。經過實驗測試，σ=0.15 時，構建出來的地圖效果最好[17]。

2.3.3觀測范圍的不確定性

對于不同的觀測范圍z，也會影響ISM模型，如圖6所示。從圖中可以看出，隨著觀測范圍的不斷擴大，相同模型的網格單元被占用的概率越來越小。這可能導致實際包含障礙物的單元被分配的概率低于總測量范圍的峰值概率，ISM模型的判斷會越來越不準確。經過實驗測試，觀測范圍z=0.3～5 m 時，ISM 模型判斷準確率最高[17]。

圖3 卷積得到高斯逆傳感器模型(z=2 m,L=1 m,σ=0.5)

圖4 參數L 的不同取值對ISM模型的影響（z=2 m,σ=0.15）

圖5 σ 值對逆傳感器模型的影響（z=2 m,L=0.5 m）

圖6 測量距離的變化對ISM模型的影響（L=0.4 m）

3 柵格地圖模型

柵格圖旨在將環境劃分為均勻大小的單元網格。網格單元表示環境的一部分，并且可以存儲是被占用、空閑還是未知相對應的信息。在占用的情況下，它表示在單元格內存在障礙物，并且對于路徑規劃算法或機器人，該空間不可穿越；在空閑的情況下，環境的這一部分是可穿越的，沒有障礙；在未知的情況下，沒有關于該區域的環境狀況的信息，表示需要探索該區域。根據環境所需的細節不同，可以將網格劃分為各種分辨率，例如1 cm 至5 m 的范圍內。網格單元可以各自包含被占用概率的值，其中概率值的范圍從0（空閑）到1（占用），而值0.5 意味著單元的狀態不明，這允許將具有噪聲或不確定性的傳感器測量建模到地圖中。

理想情況下，從開始到時間t的給定傳感器測量值z和位姿估計x已知的情況下，系統將計算地圖m上的后驗分布：

為了計算方便，把所有相對于機器人位姿x的傳感器測量值z轉換為世界坐標系中的坐標，因此可以將位姿并入測量值z。消去x得到如下公式：

柵格地圖m可以被劃分成i個大小相等的網格單元，其中單個單元格可以表示為。每個單元格mi被占用的概率（簡寫成（簡寫成）表示單元格空閑的概率。對于擁有1 000 個單元格的網格地圖來說，它擁有21000種可能，這個計算量過大，因此將單元格獨立統計是一個比較好的選擇。公式（12）可以寫成：

把公式（16）代入公式（15）可得：

根據相同原理互補概率為：

為了方便計算，可以用公式（17）除以公式（18），可得：

為了避免概率在接近0 或1 時數值不穩定，對公式（19）取對數：

4 ORB_SLAM2柵格地圖

ORB_SLAM2 是 2017 年 Raul Mur-Artal 等人提出的一個完整的SLAM 系統，它在ORB_SLAM 的基礎上增加了雙目和RGB-D 相機的使用算法，并且增加了重定位模塊。該系統包含地圖復用、閉環檢測以及重定位功能，并且可以在CPU中實時工作。

ORB_SLAM2 系統有三個主要的并行線程：（1）跟蹤線程。跟蹤線程通過尋找與本地地圖匹配的特征，應用BA（Bundle Adjustment）最小化重投影誤差，實現了相機每幀的定位。（2）局部地圖線程。局部地圖線程映射管理局部地圖并對其進行優化，執行局部BA 優化。（3）循環關閉線程。循環關閉線程檢測大循環并通過執行姿勢圖優化來校正累積漂移。該線程啟動第四個線程以在姿勢圖優化之后執行完整BA，以計算最佳結構和運動解決方案。RGB-D相機預處理如圖7所示；ORB_ SLAM2系統框架如圖8所示。

圖7 ORB_SLAM2 RGB-D相機預處理

圖8 ORB_SLAM2算法框架

本文算法以ORB_SLAM2 為前端，ROS 為中間層，ISM和柵格地圖構建算法為后端，實時構建能用于機器人導航的3D柵格地圖。前端ORB_SLAM2利用RGBD相機完成關鍵幀選取、相機位姿估計以及稀疏點云的計算，并在ROS 上發布這些話題。ISM 算法從ROS 訂閱這些數據，作為傳感器觀測數據zt和機器人姿態xt，進而通過柵格地圖構建算法生成所需的地圖，地圖采用八叉樹進行儲存以減少存儲空間和內存使用[18]。具體流程圖如圖9所示。

圖9 ORB_SLAM2實時柵格地圖構建算法流程圖

5 實驗

5.1 實驗說明

本文采用ROS 系統作為中間平臺，采用Rviz（Ros Visualization）作為實時顯示平臺。本文實驗Rviz 中地圖網格大小選取為1 m2。3D 柵格地圖精度（單元格大小）決定了地圖的精細程度，體現更多的細節單元格需要小一點（不小于0.01 m），如果為了體現地圖的大輪廓，那么單元格就要選擇的大一點。本文所有實驗中地圖單元格都選取為0.05 m，因此根據本文2.3.1小節可得，σ取值為經驗值0.15。

5.2 TUM數據集實驗

慕尼黑工業大學（TUM）的計算機視覺實驗室提供了RGB-D 大型數據集。數據集采用Microsoft Kinect傳感器以全幀速率（30 Hz）和傳感器分辨率（640×480）記錄彩色圖像和深度圖像數據，并且提供了地面實況軌跡。本文采用輪式機器人所采集的數據集rgbd_dataset_freiburg2_pioneer_slam作為本次實驗圖像輸入。RGB-D相機運動軌跡及點云圖如圖10所示。圖10顯示了輪式機器人的運動軌跡以及稀疏點云，但是由于點云太過稀疏無法進行導航以及路徑規劃。實時生成的3D 柵格地圖如圖11所示。該圖能夠清楚地表示出障礙物所在的位置，以及房間的墻面信息，但是障礙物和墻面略有不規則是因為在計算特征點的時候，有的地方缺少紋理，有的地方超出深度傳感器的探測范圍，所以無法計算出足夠的特征點，因此輸入到ISM 模型中的數據有部分偏差，使得生成的墻面和障礙物有些不規則。圖11 下方沒有墻面是因為數據集中并沒有拍攝到該處的圖像。

圖10 rgbd_dataset_freiburg2_pioneer_slam運動軌跡及點云圖

圖11 rgbd_dataset_freiburg2_pioneer_slam 3D柵格地圖

圖12 機器人仿真環境

5.3 仿真輪式機器人實驗

本文采用中科院軟件所-重德智能機器人聯合研究中心提供的機器人仿真環境，該環境包含一個完整的機器人系統。仿真環境由Gazebo構建，提供對外接口，能夠通過ROS獲取RGB圖像數據和深度圖像數據。本次實驗利用該仿真環境，并與ORB_SLAM2 進行融合，通過鍵盤控制輪式機器人的運動。仿真環境如圖12 所示，運動軌跡及點云圖如圖13 所示，實時3D 柵格地圖如圖14 所示。圖14 能夠非常完美地顯示該仿真環境，因為該仿真環境有大量紋理豐富的墻面以及物品，并且沒有特別多的雜物。在圖中左下角缺少一塊是因為該處墻面是光滑的。沒有任何紋理存在，因此ISM模型沒有接收到任何數據，也就無法生成地圖。

5.4 室內輪式機器人實驗

圖13 仿真機器人運動軌跡及點云圖

圖14 仿真機器人實時3D柵格地圖

對于室內的輪式機器人實驗，實驗設備如圖15 所示，采用三輪全向輪底盤，最大移動速度0.5 m/s，最大角速度180（°）/s。采用DELLG3（i5-8300H 3.3 GHz 8 GB內存）筆記本電腦作為輪式機器人的控制器。實驗攝像頭為Microsoft KinectV2 RGB-D 相機，彩色圖像分辨率為960×540，深度圖像為320×240，攝像頭采集頻率為30 Hz。本實驗在Ubuntu16 中利用ROS 系統作為平臺進行 RGB-D 相機、ORB_SLAM2 以及 ISM 模型之間的通信。實時3D柵格地圖由ROS系統內的Rviz顯示。

圖15 室內實驗設備

室內實驗場景如圖16 所示，室內運動軌跡及點云地圖如圖17 所示，室內實時3D 柵格地圖如圖18 所示。室內實驗生成的柵格地圖能夠清晰地看到障礙物和兩側的板子，能夠很好地反映室內環境。通過室內實驗可以看出，對于有豐富紋理的室內環境能夠建立準確的3D 柵格地圖，能夠反映障礙物所在的空間位置。但是對于紋理不豐富的障礙物側面以及光滑反光的鋼板，就沒辦法獲得充足的信息，因此本文的算法對于這種情況并沒有辦法建立柵格地圖。

圖16 室內實驗場景

圖17 室內機器人運動軌跡及點云圖

5.5 算法對比分析

本文采用2017 年Gregorio[13]等人提出的Skimap 算法作為對比方法。Skimap是一種高效的機器人導航算法地圖結構，能夠快速獲取3D地圖、2.5D高度地圖以及2D 占據地圖。Skimap 為導航映射框架，依賴于前端提供的位姿、RGB 圖像以及深度圖像。因此對比實驗采用ORB_SLAM2 作為前端，Skimap 作為后端地圖生成算法。對比圖如圖19所示。

圖18 室內機器人實時3D柵格地圖

圖19 室內機器人實時Skimap 3D柵格地圖

為了評估地圖的精度，本文采用長度誤差lerror與角度誤差θerror作為評價指標。分別從本文算法建圖（圖18）以及Skimap 算法建圖（圖19）中取出五段線段以及三個夾角（如圖18），對比實際場景得到誤差值，如表1所示。

表1 地圖誤差值

從建圖的精度上看，本文算法精度和Skimap 算法精度都處于2%之內，本文算法精度略高于Skimap算法精度。ORB_SLAM2 算法有閉環檢測及全局優化模塊[8]，并且本文在進行實驗的時候有較好的閉環條件，能夠很好地減小累計誤差，進一步提高了建圖精度。

實時性方面，本文實時性略差于Skimap，由文獻[13]可知Skimap算法樹搜索時要快于八叉樹和KD樹，但在實驗過程中Skimap算法速度為10 Hz，本文算法處理速度為10 Hz，實時性基本滿足要求。

6 結束語

針對以往的視覺SLAM 系統只能生成位姿以及稀疏點云圖的缺陷，本文在ORB_SLAM2 的基礎上，提出了一種3D 柵格地圖的實時構建算法。該算法建立了適用于視覺SLAM的逆傳感器模型（ISM），詳細推導了柵格地圖的生成機制，并利用算法流程圖清晰地表示ORB_SLAM2 與ISM 結合生成3D 柵格地圖的具體流程。通過數據集實驗、仿真實驗以及室內輪式機器人實驗，表明ISM 模型與ORB_SLAM2 的結合具有較強的實用性和較高的建圖精度，并能夠實時構建3D 柵格地圖。