松靈機器人scout mini小車 自主導航(3)——建圖導航模擬

遥感摆烂人發表於2024-07-11
機器人

松靈機器人Scout mini小車建圖導航模擬

在之前的文章中，我們已經介紹瞭如何在gazebo和rviz對scout mini小車進行模擬，並且測試了新增自定義的感測器，在本文章中將進一步介紹如何利用scout mini小車 在模擬環境中建圖和導航。
倉庫連結: https://gitee.com/agent-explorer/robotics/tree/master/scout_mini_navigation

1.模擬環境準備

直接複製倉庫連結:https://gitee.com/agent-explorer/robotics/tree/master/scout_mini_navigation 。
進入到scout_mini_navigation中，配置好ROS環境後直接執行setup.bash指令碼，自動安裝相關環境。

sudo bash setup.bash
catkin_make
source devel.setup.bash

這裡需要注意：在setup.bash過程中可能出現gazebo有關包錯誤，可以自行官網下載

2. 模擬建圖

2.1 執行gmapping

在確保安裝好gazebo模型之後(後續地圖會用到gazebo模型，需要提前下載)，直接使用gmapping建圖。

roslaunch scout_gazebo_sim scout_mini_gmapping.launch

開啟以下視窗則證明gmapping正常工作。

img
img

2.2 環境探索建圖

之後開啟一個新的終端，執行鍵盤控制節點，控制小車探索環境:

rosrun tele_twist_keyboard teleop_twist_keyboard.py

控制車輛在地圖中行走，直到將整幅地圖構建完畢

img

2.3 地圖儲存

執行下面命令儲存構建好的地圖

cd src/scout_gazebo_sim/maps/
rosrun map_server map_saver -f testmap

即可在工作空間看到儲存的地圖:

img

這裡需要注意testmap.yaml中image屬性是相對路徑，如果是絕對路徑則會出現問題

2.4 自主導航

啟動導航launch

roslaunch scout_gazebo_sim scout_mini_navigation.launch

看到如下介面代表成功啟動：
img
img

點選2D Nav Goal 選擇目標點，小車可以完成自主導航。

img

3.感測器配置

在之前的文章中介紹瞭如何自定義新增相機感測器，本章將進一步介紹如何新增鐳射雷達和imu兩個感測器，相關配置均在 scout_description/urdf/empty.urdf中。

3.1 新增鐳射雷達

Gazebo提供了ray標籤用於自定義鐳射雷達，ray包含了scan和range兩個屬性。scan描述鐳射雷達水平和垂直方向上的取樣線數，range定義每束鐳射的屬性。

Code 
<ray>
    <scan>
        <horizontal>
            <!--- 水平方向上生成 720 條取樣線 --->
            <samples>720</samples>
            <!--- 返回的範圍資料點的個數 = 解析度 * 樣本數 --->
            <resolution>1</resolution>
            <!--- 最大和最小角度，弧度制。決定鐳射雷達扇形的範圍 --->
            <min_angle>-2.09439504</min_angle>
            <max_angle>2.09439504</max_angle>
        </horizontal>
    </scan>
    <range>
        <!--- 每個取樣線的 最小距離 和 最大距離 以及 解析度  --->
        <min>0.05</min>
        <max>8.0</max>
        <resolution>0.01</resolution>
    </range>
    <noise>
       <!--- 新增高斯噪聲 --->
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.01</stddev>
    </noise>
</ray>

ray 標籤支援的所有屬性可以在 http://sdformat.org/spec?ver=1.6&elem=sensor#sensor_ray 找到

之後我們還需要新增 Gazebo 雷達外掛廣播我們雷達的資訊：

Code 
<plugin name="sensor_ray_controller" filename="libgazebo_ros_laser.so">
    <topicName>/scan</topicName>  <!--- 雷達資訊廣播話題名 --->
    <frameName>sensor_ray</frameName>  <!--- 和雷達 link 名稱一致 --->
</plugin>

可用外掛可以在 https://classic.gazebosim.org/tutorials
tut=ros_gzplugins#Tutorial:UsingGazebopluginswithROS 找到

3.2新增IMU

方法和鐳射雷達類似，標籤屬性 http://sdformat.org/spec?ver=1.6&elem=sensor#sensor_imu 。
外掛資訊 https://classic.gazebosim.org/tutorials?tut=ros_gzplugins#IMUsensor(GazeboRosImuSensor) 。

3.3 Odom釋出

在模擬中，我們使用libgazebo_ros_p3d.so外掛廣播odom。這個外掛用於在模擬中獲取任意 link 的位姿真值，並透過 odom 格式的訊息進行廣播。

其具體配置筆者加加在了src/scout_description/urdf/scout_mini.gazebo 中的第 30~39 行：

Code 
<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>50.0</updateRate>
    <bodyName>base_link</bodyName> <!--- 廣播 base_link 的位姿真值--->
    <topicName>odom</topicName> <!--- topic 名稱，這裡作為 odom 進行廣播 --->
    <gaussianNoise>0.01</gaussianNoise>
    <frameName>world</frameName> <!--- 參考座標系，根據 odom 定義，這裡選擇世界原點 --->
    <xyzOffsets>0 0 0</xyzOffsets>
    <rpyOffsets>0 0 0</rpyOffsets>
</plugin>

4. ROS相關配置

在小車自主建圖過程中，最重要的幾個ROS包以及功能對應如下：

├── move_base ：導航包，根據參照的訊息進行路徑規劃;
├── gmapping ：根據鐳射資料建立地圖;
├── amcl : 用於在已知地圖中定位;
└── mapserver ：用於儲存地圖為YAML格式
此外還有：
|——robot_pose_ekf ：是 ROS 中實現多感測器融合輸出機器人位姿的程式包。

4.1 robot_pose_ekf

robot_pose_ekf 是 ROS 中實現多感測器融合輸出機器人位姿的程式包。其基本原理是利用擴充套件卡爾曼濾波，利用多種感測器實現機器人位姿的最優估計。官方百科：http://wiki.ros.org/robot_pose_ekf
在本例中其配置如下：

Code 
<node pkg="robot_pose_ekf" name="robot_pose_ekf" type="robot_pose_ekf">
    <param name="output_frame" value="odom" /> <!--- 釋出 tf 名為 odom --->
    <param name="base_footprint_frame" value="base_link"/> <!--- 建立 output_frame->base_link 的 tf 變換--->
    <param name="freq" value="50.0"/>
    <param name="sensor_timeout" value="1.0"/>

    <param name="odom_used" value="true"/> <!--- 使用原始 odom 資訊--->
    <param name="odom_data" value="odom"/>

    <param name="vo_used" value="false"/> <!--- 不使用視覺里程計 --->
</node>

4.2 gmapping

在建圖環節，我們使用的是gmapping，它可以只用 鐳射雷達和 里程計 資訊進行建圖，其啟動檔案位於 src/scout_gazebo_sim/launch/scout_mini_gmapping.launch 對於本例中使用 2D 鐳射雷達 在 小型場景 中建圖，gmapping 可以達到相當不錯的效果。官方文件：http://wiki.ros.org/gmapping

gmapping 本身需要訂閱 tf 和 scan 兩個話題以獲取 tf 變換資料 和 鐳射雷達資料。

其次，gmapping 需要 鐳射雷達座標 到 base_link 的 tf 變換（本例中由 robot_state_publisher 包自動釋出）：
img

以及 odom 和 base_link 之間的變換（由 robot_pose_ekf 釋出融合資料）：
img

在正確配置 tf 樹以及 topic 後即可讓我們的車輛開始運動，gmapping 會實時更新地圖資料。

4.3 map_server

官方文件地址：http://wiki.ros.org/map_server

用於提供map_server 和 map_saver 功能。其中 map_saver 是一個命令列工具，用於儲存地圖資料為 YAML 格式的檔案：

YAML 
image: map1.pgm # 地圖對應圖片的路徑名；可以是絕對路徑，也可以相對於 YAML 檔案位置的相對路徑. 用不同的畫素顏色描述世界中每個cell的佔用狀態。白色畫素表示free，黑色畫素表示occupied, 其它顏色畫素表示unknown
resolution: 0.050000 # 解析度 米/畫素
origin: [-50.000000, -50.000000, 0.000000] # 地圖左下畫素的2-D姿勢（x，y，yaw），yaw 為逆時針旋轉角度（YAW = 0表示無旋轉）。系統的許多部分目前忽略了 yaw
negate: 0 # 是否翻轉黑白畫素代表的含義
occupied_thresh: 0.65 # 佔用機率大於此閾值的畫素被認為是完全佔用的
free_thresh: 0.196 # 用機率小於此閾值的畫素被認為是自由的

當作為 map_server 使用時建立一個 map_server 節點，利用從磁碟讀取的地圖資料提供 ROS 服務。map_server 的當前實現將 MAP 影像資料中的顏色值轉換為三元佔用值：Free（0），佔用（100）和未知（-1）。

一個簡單的 launch 例子如下:

<node pkg="map_server" type="map_server" name="map_server" args="$(find scout_gazebo_sim)/maps/$(arg map_name).yaml" output="screen">
    <param name="frame_id" value="map"/>
</node>

4.4 amcl

amcl 是一個用於 2D 移動機器人的定位系統，其實現了自適應（或 KLD 取樣）蒙特卡洛定位方法，該方法使用粒子群演算法來跟蹤機器人在已知地圖上的位姿，僅使用已知地圖和鐳射雷達資訊即可執行。官方文件：http://wiki.ros.org/amcl

其配置資訊位於 src/scout_gazebo_sim/param/amcl_params_diff.yaml 一般情況下保持預設即可。

4.5 move_base

官方文件地址：http://wiki.ros.org/move_bas

move_base 就是我們導航系統的核心元件了，給定一個地圖上的目標，它會嘗試計算全域性路徑和區域性路徑用於導航。使用兩個規劃器（全域性和區域性）的意義在於，全域性路徑可以很理想，但是在移動過程中難免會存在遇到障礙物等問題，這時候就需要區域性規劃指導機器人移動。其介面資訊如圖所示（來源：http://wiki.ros.org/move_base）

img

其配置資訊如下：

Code 
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find scout_gazebo_sim)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find scout_gazebo_sim)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find scout_gazebo_sim)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find scout_gazebo_sim)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find scout_gazebo_sim)/param/planner.yaml" command="load" />
    <param name="base_global_planner" value="global_planner/GlobalPlanner" />
    <param name="planner_frequency" value="1.0" />
    <param name="planner_patience" value="5.0" />
    <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
    <param name="controller_frequency" value="5.0" />
    <param name="controller_patience" value="15.0" />
    <param name="clearing_rotation_allowed" value="true" />
</node>

相關的引數內容較多，除 tf 廣播資訊 和 節點名稱 外大部分引數保持預設資訊即可，如有需要可對照官方文件進行修改

5.結語

對於scout_mini 模擬導航的相關介紹到這裡就結束了，作為初學者或者使用者，大部分時候我們並不關心軟體包或者外掛背後的演算法實現。筆者同樣建議在深入學習其背後原理之前先對其效果有直觀感受，在會用的基礎上進行深入，如此才能更快理解演算法為什麼要這樣設計以及如何編寫屬於自己的程式。

參考連結：https://anthonysun256.github.io/scout_mini_navi/

相關文章