第九章 SLAM

本章簡介

機器人研究的問題包含許許多多的領域，我們常見的幾個研究的問題包括：建圖(Mapping)、定位(Localization)和路徑規劃（Path Planning），如果機器人帶有機械臂，那麼運動規劃（Motion Planning）也是重要的一個環節。而同步定位與建圖（SLAM）問題位於定位和建圖的交集部分。

SLAM需要機器人在未知的環境中逐步建立起地圖，然後根據地區確定自身位置，從而進一步定位。

這一章我們來看ROS中SLAM的一些功能包，也就是一些常用的SLAM演算法，例如Gmapping、Karto、Hector、Cartographer等演算法。這一章我們不會去關注演算法背後的數學原理，而是更注重工程實現上的方法，告訴你SLAM演算法包是如何工作的，怎樣快速的搭建起SLAM演算法。

9.1 地圖

9.1.1 直觀印象

ROS中的地圖很好理解，就是一張普通的灰度影象，通常為pgm格式。這張影象上的黑色畫素表示障礙物，白色畫素表示可行區域，灰色是未探索的區域。如下圖所示， 

在SLAM建圖的過程中，你可以在RViz裡看到一張地圖被逐漸建立起來的過程，類似於一塊塊拼圖被拼接成一張完整的地圖。這張地圖對於我們定位、路徑規劃都是不可缺少的資訊。事實上，地圖在ROS中是以Topic的形式維護和呈現的，這個Topic名稱就叫做/map，它的訊息型別是nav_msgs/OccupancyGrid。

9.1.2 鎖存

由於/map中實際上儲存的是一張圖片，為了減少不必要的開銷，這個Topic往往採用鎖存（latched）的方式來發布。什麼是鎖存？其實就是：地圖如果沒有更新，就維持著上次釋出的內容不變，此時如果有新的訂閱者訂閱訊息，這時只會收到一個/map

的訊息，也就是上次釋出的訊息；只有地圖更新了（比如SLAM又建出來新的地圖），這時/map才會釋出新的內容。 鎖存器的作用就是，將釋出者最後一次釋出的訊息儲存下來，然後把它自動傳送給後來的訂閱者。這種方式非常適合變動較慢、相對固定的資料（例如地圖），然後只發布一次，相比於同樣的訊息不定的釋出，鎖存的方式既可以減少通訊中對頻寬的佔用，也可以減少訊息資源維護的開銷。

9.1.3 nav_msgs/OccupancyGrid

然後我們來看一下地圖的OccupancyGrid型別是如何定義的，你可以通過rosmsg show nav_msgs/OccupancyGrid來檢視訊息，或者直接rosed nav_msgs OccupancyGrid.msg

來檢視srv檔案。

std_msgs/Header header #訊息的報頭
    uint32 seq
    time stamp
    string frame_id #地圖訊息繫結在TF的哪個frame上，一般為map
nav_msgs/MapMetaData info #地圖相關資訊
    time map_load_time #載入時間
    float32 resolution #解析度 單位：m/pixel
    uint32 width #寬 單位：pixel
    uint32 height #高 單位：pixel
    geometry_msgs/Pose origin #原點
        geometry_msgs/Point position
            float64 x
            float64 y
            float64 z
        geometry_msgs/Quaternion orientation
            float64 x
            float64 y
            float64 z
            float64 w
int8[] data #地圖具體資訊

這個srv檔案定義了/map話題的資料結構，包含了三個主要的部分:header, info和data。header是訊息的報頭，儲存了序號、時間戳、frame等通用資訊，info是地圖的配置資訊，它反映了地圖的屬性，data是真正儲存這張地圖資料的部分，它是一個可變長陣列，int8後面加了[]，你可以理解為一個類似於vector的容器，它儲存的內容有width*height個int8型的資料,也就是這張地圖上每個畫素。

9.2 Gmapping

9.2.1 Gmapping SLAM軟體包

Gmapping演算法是目前基於鐳射雷達和里程計方案裡面比較可靠和成熟的一個演算法，它基於粒子濾波，採用RBPF的方法效果穩定，許多基於ROS的機器人都跑的是gmapping_slam。這個軟體包位於ros-perception組織中的slam_gmapping倉庫中。 其中的slam_gmapping是一個metapackage，它依賴了gmapping，而演算法具體實現都在gmapping軟體包中，該軟體包中的slam_gmapping程式就是我們在ROS中執行的SLAM節點。如果你感興趣，可以閱讀一下gmapping的原始碼。

如果你的ROS安裝的是desktop-full版本，應該預設會帶gmapping。你可以用以下命令來檢測gmapping是否安裝

apt-cache search ros-$ROS_DISTRO-gmapping

如果提示沒有，可以直接用apt安裝

sudo apt-get install ros-$ROS_DISTRO-gmapping

gmapping在ROS上執行的方法很簡單

rosrun gmapping slam_gmapping

但由於gmapping演算法中需要設定的引數很多，這種啟動單個節點的效率很低。所以往往我們會把gmapping的啟動寫到launch檔案中，同時把gmapping需要的一些引數也提前設定好，寫進launch檔案或yaml檔案。 具體可參考教學軟包中的slam_sim_demo中的gmapping_demo.launch和robot_gmapping.launch.xml檔案。

9.2.2 Gmapping SLAM計算圖

gmapping的作用是根據鐳射雷達和里程計（Odometry）的資訊，對環境地圖進行構建，並且對自身狀態進行估計。因此它得輸入應當包括鐳射雷達和里程計的資料，而輸出應當有自身位置和地圖。 下面我們從計算圖（訊息的流向）的角度來看看gmapping演算法的實際執行中的結構： 位於中心的是我們執行的slam_gmapping節點，這個節點負責整個gmapping SLAM的工作。它的輸入需要有兩個：

輸入

• /tf以及/tf_static： 座標變換，型別為第一代的tf/tfMessage或第二代的tf2_msgs/TFMessage其中一定得提供的有兩個tf，一個是base_frame與laser_frame之間的tf，即機器人底盤和鐳射雷達之間的變換；一個是base_frame與odom_frame之間的tf，即底盤和里程計原點之間的座標變換。odom_frame可以理解為里程計原點所在的座標系。

• /scan :鐳射雷達資料，型別為sensor_msgs/LaserScan

/scan很好理解，Gmapping SLAM所必須的鐳射雷達資料，而/tf是一個比較容易忽視的細節。儘管/tf這個Topic聽起來很簡單，但它維護了整個ROS三維世界裡的轉換關係，而slam_gmapping要從中讀取的資料是base_frame與laser_frame之間的tf,只有這樣才能夠把周圍障礙物變換到機器人座標系下，更重要的是base_frame與odom_frame之間的tf，這個tf反映了里程計（電機的光電碼盤、視覺里程計、IMU）的監測資料，也就是機器人里程計測得走了多少距離，它會把這段變換髮布到odom_frame和laser_frame之間。

因此slam_gmapping會從/tf中獲得機器人里程計的資料。

輸出

• /tf： 主要是輸出map_frame和odom_frame之間的變換
• /slam_gmapping/entropy： std_msgs/Float64型別，反映了機器人位姿估計的分散程度
• /map： slam_gmapping建立的地圖
• /map_metadata： 地圖的相關資訊

輸出的/tf裡又一個很重要的資訊，就是map_frame和odom_frame之間的變換，這其實就是對機器人的定位。通過連通map_frame和odom_frame，這樣map_frame與base_frame甚至與laser_frame都連通了。這樣便實現了機器人在地圖上的定位。

同時，輸出的Topic裡還有/map，在上一節我們介紹了地圖的型別，在SLAM場景中，地圖是作為SLAM的結果被不斷地更新和釋出。

9.2.3 里程計誤差及修正

目前ROS中常用的里程計廣義上包括車輪上的光電碼盤、慣性導航元件（IMU）、視覺里程計，你可以只用其中的一個作為odom，也可以選擇多個進行資料融合，融合結果作為odom。通常來說，實際ROS專案中的里程計會發布兩個Topic：

• /odom: 型別為nav_msgs/Odometry，反映里程計估測的機器人位置、方向、線速度、角速度資訊。
• /tf: 主要是輸出odom_frame和base_frame之間的tf。這段tf反映了機器人的位置和方向變換，數值與/odom中的相同。

由於以上三種里程計都是對機器人的位姿進行估計，存在著累計誤差，因此當運動時間較長時，odom_frame和base_frame之間變換的真實值與估計值的誤差會越來越大。你可能會想，能否用鐳射雷達資料來修正odom_frame和base_frame的tf。事實上gmapping不是這麼做的，里程計估計的是多少，odom_frame和base_frame的tf就顯示多少，永遠不會去修正這段tf。gmapping的做法是把里程計誤差的修正釋出到map_frame和odom_frame之間的tf上，也就是把誤差補償在了地圖座標系和里程計原點座標系之間。通過這種方式來修正定位。

這樣map_frame和base_frame，甚至和laser_frame之間就連通了，實現了機器人在地圖上的定位。

• /odom

9.2.4 服務

slam_gmapping也提供了一個服務：

• /dynamic_map： 其srv型別為nav_msgs/GetMap，用於獲取當前的地圖。

該srv定義如下： nav_msgs/GetMap.srv

# Get the map as a nav_msgs/OccupancyGrid
---
nav_msgs/OccupancyGrid map

可見該服務的請求為空，即不需要傳入引數，它會直接反饋當前地圖。

9.2.5 引數

slam_gmapping需要的引數很多，這裡以slam_sim_demo教學包中的gmapping_demo的引數為例，註釋了一些比較重要的引數，具體請檢視ROS-Academy-for-Beginners/slam_sim_demo/launch/include/robot_gmapping.launch.xml

<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="base_frame" value="$(arg base_frame)"/> <!--底盤座標系-->
<param name="odom_frame" value="$(arg odom_frame)"/> <!--里程計座標系-->
<param name="map_update_interval" value="1.0"/> <!--更新時間(s)，每多久更新一次地圖，不是頻率-->
<param name="maxUrange" value="20.0"/> <!--鐳射雷達最大可用距離，在此之外的資料截斷不用-->
<param name="maxRange" value="25.0"/> <!--鐳射雷達最大距離-->
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="minimumScore" value="200"/>
<param name="srr" value="0.01"/>
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/>
<param name="linearUpdate" value="0.5"/>
<param name="angularUpdate" value="0.436"/>
<param name="temporalUpdate" value="-1.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="80"/>
<param name="xmin" value="-25.0"/>
<param name="ymin" value="-25.0"/>
<param name="xmax" value="25.0"/>
<param name="ymax" value="25.0"/>
<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>

演示截圖

gmapping演算法演示效果圖如下：

 

9.3 Karto

9.3.1 Karto SLAM計算圖

Karto SLAM和Gmapping SLAM在工作方式上非常類似，如下圖所示

輸入的Topic同樣是/tf和/scan，其中/tf裡要連通odom_frame與base_frame，還有laser_frame。這裡和Gmapping完全一樣。

唯一不同的地方是輸出，slam_karto的輸出少相比slam_gmapping了一個位姿估計的分散程度.

9.3.2 服務

與Gmapping相同，提供/dynamic_map服務

9.3.3 引數

這裡以ROS-Academy-for-Beginners中的karto_slam為例，選取了它的引數檔案slam_sim_demo/param/karto_params.yaml，關鍵位置做了註釋：

# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.2 
minimum_travel_heading: 0.174                  #in radians
scan_buffer_size: 70
scan_buffer_maximum_scan_distance: 20.0
link_match_minimum_response_fine: 0.8
link_scan_maximum_distance: 10.0
loop_search_maximum_distance: 4.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 0.4     # gets squared later
loop_match_minimum_response_coarse: 0.8
loop_match_minimum_response_fine: 0.8

# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.3
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.03

# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03

# Scan Matcher Parameters
distance_variance_penalty: 0.3              # gets squared later
angle_variance_penalty: 0.349                # in degrees (gets converted to radians then squared)
fine_search_angle_offset: 0.00349               # in degrees (gets converted to radians)
coarse_search_angle_offset: 0.349            # in degrees (gets converted to radians)
coarse_angle_resolution: 0.0349                # in degrees (gets converted to radians)
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: false

演示截圖