从零搭建一台基于ROS的自动驾驶车—–4.定位

前言
在之前的几篇文章中，已经实现了ROS和小车底盘的运动控制，通过Cartgrapher算法来建立一张全局静态地图。接下来需要做的是定位。
所谓定位就是推算机器人自身在全局地图中的位置，当然，SLAM中也包含定位算法实现，不过SLAM的定位是用于构建全局地图的，是属于导航开始之前的阶段，而当前定位是用于导航中，导航中，机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期。在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。

一、amcl介绍
AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。
ACML也就是采集激光雷达的点云数据与静态地图的点云数据进行匹配来估计机器人目前所处于地图中的位置。
amcl已经被集成到了navigation包，navigation安装前面也有介绍，命令如下:

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

二、amcl的使用
1.amcl订阅的节点
scan(sensor_msgs/LaserScan)
获取激光雷达的点云数据。

tf(tf/tfMessage)
坐标变换消息，当你的功能包里面没有TF变化的时候，可以使用laser_scan_matcher这个功能包来提供odom到base_link之间的一个TF变换
navigation laser_scan_matcher参数配置

initialpose(geometry_msgs/PoseWithCovarianceStamped)
用来初始化粒子滤波器的均值和协方差。这个是用来设置机器人的初始位姿
实现RVIZ 2D POSE ESTIMATE 功能设置机器人初始坐标

map(nav_msgs/OccupancyGrid)
获取地图数据。
2.amcl发布的节点
amcl_pose(geometry_msgs/PoseWithCovarianceStamped)
机器人在地图中的位姿估计。

particlecloud(geometry_msgs/PoseArray)
位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。

tf(tf/tfMessage)
发布从 odom 到 map 的转换。

节点图如下所示：

3.amcl启动的launch文件

启动amcl的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"/>

  <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>

amcl节点是不可以单独运行的，运行 amcl 节点之前，需要先加载全局地图，然后启动 rviz 显示定位结果，上述节点可以集成进launch文件，内容示例如下:

<launch>

   <include file="$(find nav)/launch/read_map.launch" />
   <include file="$(find rplidar_ros)/launch/rplidar.launch" />
   <include file="$(find nav)/launch/laser_scan_matcher.launch" />
   <include file="$(find nav)/launch/amcl.launch" />
   
</launch>

总结