SLAM建圖

一、SLAM

1.1 介紹

SLAM是機器人自身位置不確定的情況下，在完全未知的環境中創建地圖，同時利用地圖進行自主定位。

• 定位：機器人根據周圍環境估計自身在地圖上的位置坐標。
• 地圖構建：機器人根據傳感器的結果來構建一張地圖。

1.2 原理

圖1. SLAM原理

二、數據結構

2.1 激光雷達

一般在SLAM中使用可以直接獲得深度信息的傳感器。以激光雷達為例，在終端上輸入rosmsg show sensor_msgs/LaserScan可以獲得其數據結構

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min //可檢測范圍的起始角度
float32 angle_max //可檢測范圍的終止角度，與angle_min組成激光雷達的可檢測范圍
float32 angle_increment //相鄰數據幀之間的角度步長
float32 time_increment //采集到相鄰數據幀之間的時間步長，當傳感器處於相對運動狀態時進行補償使用
float32 scan_time //采集一幀數據所需要的時間
float32 range_min //最近可檢測深度的閾值
float32 range_max //最遠可檢測深度的閾值
float32[] ranges //一幀深度數據的存儲數組
float32[] intensities

而RGB-D攝像頭，也可以通過紅外攝像頭獲取周圍環境的深度信息，可以通過depthimage_to_laserscan功能包轉化為激光雷達的數據。

2.2 里程計

機器人在定位時要用到其里程計，在終端上輸入rosmsg show nav_msgs/Odometry 可以獲得其數據結構

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose //機器人當前位置坐標，包括機器人的XYZ三軸位置和方向參數，以及用於校正誤差的協方差矩陣
  geometry_msgs/Pose pose 
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist //機器人當前的運動狀態，包括XYZ三軸的線速度與角速度，以及用於校正誤差的協方差矩陣
  geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
  float64[36] covariance

三、SLAM功能包

3.1 介紹

借助SLAM功能包，我們只需要輸入特定格式的傳感器信息，就可以自動建圖和定位。在ROS中這種功能的包有很多，比如用於平面地圖的gmapping，hector，cartographer等功能包，也有用於三維地圖的rtabmap功能包。

3.2 gmapping

以gmapping功能包為例，gmapping是基於二維激光雷達的 SLAM建圖定位的功能包，采用的是Rao-Blackwellized粒子濾波算法，gmapping的總體框架如圖2所示。深度信息，里程計信息，IMU信息是輸入信息，二維柵格地圖是輸出地圖。其中深度信息和里程計信息是必須輸入的，IMU可不輸入。

圖2. gmapping功能包的總體框架

3.3 應用

我們借助gazebo，創建一個如圖3所示的虛擬迷宮，用於小車的測試。圖4是gmapping功能包的應用效果，可以看到小車的運動和地圖構建都是同步的。

圖3. 虛擬迷宮

圖4. gmapping應用效果