機器人導航(仿真)(一)——SLAM建圖

導航實現01_SLAM建圖

參考視頻：【奧特學園】ROS機器人入門課程《ROS理論與實踐》零基礎教程_嗶哩嗶哩_bilibili

參考文檔：http://www.autolabor.com.cn/book/ROSTutorials/

SLAM算法有多種，當前我們選用gmapping，后續會再介紹其他幾種常用的SLAM實現。

1.gmapping簡介

gmapping 是ROS開源社區中較為常用且比較成熟的SLAM算法之一，gmapping可以根據移動機器人里程計數據和激光雷達數據來繪制二維的柵格地圖，對應的，gmapping對硬件也有一定的要求:

• 該移動機器人可以發布里程計消息
• 機器人需要發布雷達消息(該消息可以通過水平固定安裝的雷達發布，或者也可以將深度相機消息轉換成雷達消息)

關於里程計與雷達數據，仿真環境中可以正常獲取的，不再贅述，柵格地圖如案例所示。

gmapping 安裝前面也有介紹，命令如下:

sudo apt install ros-<ROS版本>-gmapping

2.gmapping節點說明

gmapping 功能包中的核心節點是:slam_gmapping。為了方便調用，需要先了解該節點訂閱的話題、發布的話題、服務以及相關參數。

2.1訂閱的Topic

tf (tf/tfMessage)

• 用於雷達、底盤與里程計之間的坐標變換消息。

scan(sensor_msgs/LaserScan)

• SLAM所需的雷達信息。

2.2發布的Topic

map_metadata(nav_msgs/MapMetaData)

• 地圖元數據，包括地圖的寬度、高度、分辨率等，該消息會固定更新。

map(nav_msgs/OccupancyGrid)

• 地圖柵格數據，一般會在rviz中以圖形化的方式顯示。

~entropy(std_msgs/Float64)

• 機器人姿態分布熵估計(值越大，不確定性越大)。

2.3服務

dynamic_map(nav_msgs/GetMap)

• 用於獲取地圖數據。

2.4參數

~base_frame(string, default:"base_link")

• 機器人基坐標系。

~map_frame(string, default:"map")

• 地圖坐標系。

~odom_frame(string, default:"odom")

• 里程計坐標系。

~map_update_interval(float, default: 5.0)

• 地圖更新頻率，根據指定的值設計更新間隔。

~maxUrange(float, default: 80.0)

• 激光探測的最大可用范圍(超出此閾值，被截斷)。

~maxRange(float)

• 激光探測的最大范圍。

.... 參數較多，上述是幾個較為常用的參數，其他參數介紹可參考官網。

2.5所需的坐標變換

雷達坐標系→基坐標系

• 一般由 robot_state_publisher 或 static_transform_publisher 發布。

基坐標系→里程計坐標系

• 一般由里程計節點發布。

2.6發布的坐標變換

地圖坐標系→里程計坐標系

• 地圖到里程計坐標系之間的變換。

3.gmapping使用

3.1編寫gmapping節點相關launch文件

新建功能包

名字為 nav_demo ，輸入依賴包如下：

gmapping  map_server  amcl  move_base

新建 launch 文件夾，再新建 nav01_slam.launch 文件

launch文件編寫可以參考 github 的演示 launch文件：https://github.com/ros-perception/slam_gmapping/blob/melodic-devel/gmapping/launch/slam_gmapping_pr2.launch

復制並修改如下:

<launch>
    <!--仿真環境下，將該參數設置為 true-->
    <param name="use_sim_time" value="true"/>
    <!--gmapping 節點-->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <!--設置雷達話題-->
      <remap from="scan" to="scan"/>
      <!-- 關鍵參數：坐標系-->
      <param name="base_frame" value ="base_footprint" />
      <param name="map_frame" value ="map" />
      <param name="odom_frame" value ="odom" />


      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.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="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.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"/>
    </node>
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <node pkg="rviz" type="rviz" name="rviz"  />
</launch>

關鍵代碼解釋：

<remap from="scan" to="scan"/><!-- 雷達話題 -->
<param name="base_frame" value="base_footprint"/><!--底盤坐標系-->
<param name="odom_frame" value="odom"/> <!--里程計坐標系-->

執行

1.先啟動 Gazebo 仿真環境(此過程略)

2.然后再啟動地圖繪制的 launch 文件:

roslaunch 包名 launch文件名

3.啟動鍵盤鍵盤控制節點，用於控制機器人運動建圖

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

4.在 rviz 中添加組件，顯示柵格地圖

具體如下：

新建兩個窗口，啟動 gazebo

cys@ubuntu:~/demo05_ws$ source ./devel/setup.bash
cys@ubuntu:~/demo05_ws$ roslaunch urdf02_gazebo demo03_env.launch

再輸入如下命令啟動rviz

cys@ubuntu:~/demo05_ws$ source ./devel/setup.bash
cys@ubuntu:~/demo05_ws$ roslaunch nav_demo nav01_slam.launch

新建命令行，輸入命令，讓機器人運動

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

最后，就可以通過鍵盤控制gazebo中的機器人運動，同時，在rviz中可以顯示gmapping發布的柵格地圖數據了，下一步，還需要將地圖單獨保存。

 4.地圖保存

在 launch 里新建 nav02_map_save.launch

<launch>
    <arg name="filename" value="$(find nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

在功能包 nav_demo下新建 文件夾 map，運行 

cys@ubuntu:~/demo05_ws$ source ./devel/setup.bash
cys@ubuntu:~/demo05_ws$ roslaunch nav_demo nav02_map_save.launch 

 

 

 在指定路徑下會生成兩個文件，xxx.pgm 與 xxx.yaml，打開 nav.pgm 查看圖

  

 

 

 

 4.地圖讀取

 

在 launch 里新建 nav03_map_server.launch

<launch>

    <arg name="map" default="nav.yaml" />
    <node pkg="map_server" type="map_server" name="map_server" args="$(find nav_demo)/map/$(arg map)" />

 </launch>

運行，就將地圖信息發不出去了

cys@ubuntu:~/demo05_ws$ source ./devel/setup.bash
cys@ubuntu:~/demo05_ws$ roslaunch nav_demo nav03_map_server.launch 

再運行 rviz ，在命令行輸入 rviz ，增加 Map， Topic 為 /map

 

 

 地圖 yaml 參數詳解

#1.聲明地圖圖片資源的路徑
image: /home/cys/demo05_ws/src/nav_demo/map/nav.pgm
#2.地圖刻度尺，單位是 米/像素
resolution: 0.050000
#3.地圖的相對位姿(按照右手坐標系，地圖右下角相對於rviz中的原點的位姿)
#值1：x方向上的偏移量
#值2：y方向上的偏移量
#值3：地圖的偏航角度(單位是弧度)
origin: [-50.000000, -50.000000, 0.000000]

# 地圖中的障礙物判斷：
# 最終地圖結果：白色是可通行區域，黑色是障礙物，藍灰是未知區域
# 判斷規則：
# 1.地圖中的每個像素都有取值[0,255] 白色：255  黑色：0
# 2.根據像素值計算一個比例：p = (255-x)/255 白色：0  黑色1
# 3.判斷是否是障礙物，p>occupied_thresh 就是障礙物，p<free_thresh 就是無物，可以自由通行

#4.去反
negate: 0
#5.占用閾值
occupied_thresh: 0.65
#6.空閑閾值
free_thresh: 0.196