机器⼈导航（仿真）（⼆）——amcl定位
导航实现02_amcl定位
参考视频：
所谓定位就是推算机器⼈⾃⾝在全局地图中的位置，当然，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<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>
View Code
<!--测试⽂件-->
<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 也随之⽽改变。
版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。
发表评论