機器人導航(仿真)(二)——amcl定位

導航實現02_amcl定位

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

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

所謂定位就是推算機器人自身在全局地圖中的位置，當然，SLAM中也包含定位算法實現，不過SLAM的定位是用於構建全局地圖的，是屬於導航開始之前的階段，而當前定位是用於導航中，導航中，機器人需要按照設定的路線運動，通過定位可以判斷機器人的實際軌跡是否符合預期。在ROS的導航功能包集navigation中提供了 amcl 功能包，用於實現導航中的機器人定位。

1.amcl簡介

AMCL(adaptive Monte Carlo Localization) 是用於2D移動機器人的概率定位系統，它實現了自適應（或KLD采樣）蒙特卡洛定位方法，可以根據已有地圖使用粒子濾波器推算機器人位置。

amcl已經被集成到了navigation包，navigation安裝前面也有介紹，命令如下:

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

2.amcl節點說明

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

3.amcl使用

3.1編寫amcl節點相關的launch文件

關於launch文件的實現，在amcl功能包下的example目錄已經給出了示例，可以作為參考，具體實現:

roscd amcl
ls examples

該目錄下會列出兩個文件: amcl_diff.launch 和 amcl_omni.launch 文件，前者適用於差分移動機器人，后者適用於全向移動機器人，可以按需選擇。

此處參考前者，新建 launch 文件，復制 amcl_diff.launch 文件內容並修改如下:

<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/><!-- 里程計模式為差分 -->
        <param name="odom_alpha5" value="0.1"/>
        <param name="transform_tolerance" value="0.2" />
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="30"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="5000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.8"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.2"/>
        <param name="update_min_a" value="0.5"/>

        <!--設置坐標系：odom、map 和 機器人基坐標系-->
        <param name="odom_frame_id" value="odom"/><!-- 里程計坐標系 -->
        <param name="base_frame_id" value="base_footprint"/><!-- 添加機器人基坐標系 -->
        <param name="global_frame_id" value="map"/><!-- 添加地圖坐標系 -->

        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="0.1"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
    </node>
</launch>

參數

~odom_model_type(string, default:"diff")

• 里程計模型選擇: "diff","omni","diff-corrected","omni-corrected" (diff 差速、omni 全向輪)

~odom_frame_id(string, default:"odom")

• 里程計坐標系。

~base_frame_id(string, default:"base_link")

• 機器人極坐標系。

~global_frame_id(string, default:"map")

• 地圖坐標系。

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

3.2編寫測試launch文件

amcl節點是不可以單獨運行的，運行 amcl 節點之前，需要先加載全局地圖，然后啟動 rviz 顯示定位結果，上述節點可以集成進launch文件，內容示例如下:

<launch>
    <!-- 設置地圖的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 運行地圖服務器，並且加載設置的地圖-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/>
    <!-- 啟動AMCL節點 -->
    <include file="$(find mycar_nav)/launch/amcl.launch" />
    <!-- 運行rviz -->
    <node pkg="rviz" type="rviz" name="rviz"/>
</launch>

<!--測試文件-->
<launch>
    <!--啟動 rviz-->
    <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"  />

    <!--加載地圖服務-->
    <include file="$(find nav_demo)/launch/nav03_map_server.launch" />
    
    <!--amcl文件-->
    <include file="$(find nav_demo)/launch/nav04_amcl.launch" />

</launch>

當然，launch文件中地圖服務節點和amcl節點中的包名、文件名需要根據自己的設置修改。

3.3執行

1.先啟動 Gazebo 仿真環境；

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

2.啟動鍵盤控制節點：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

3.啟動上一步中集成地圖服務、amcl 與 rviz 的 launch 文件；

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

4.在啟動的 rviz 中，添加RobotModel、Map組件，分別顯示機器人模型與地圖，添加 posearray 插件，設置topic為particlecloud來顯示 amcl 預估的當前機器人的位姿，箭頭越是密集，說明當前機器人處於此位置的概率越高；

 

5.通過鍵盤控制機器人運動，會發現 posearray 也隨之而改變。