激光雷达建图后基于amcl实现自动定位

雷达型号：robosens helios（32线）
系统平台：Linux18.04+ros
新手思路仅供参考。

由于手头只有一个激光雷达，前期在选择建图算法的时候使用的是hector-slam，见连接：学习笔记， 对办公室进行了建图和保存，下面使用amcl进行定位。

amcl(adaptiveMonteCarloLocalization)自适应的蒙特卡洛定位,是用于2D移动机器人的概率定位系统。它实现了自适应（或KLD采样）蒙特卡洛定位方法，该方法使用粒子过滤器根据已知地图跟踪机器人的姿态。
调用只需要通过ros安装navigation包即可（如果没记错的话navigation里面包含amcl，只需要安装navigation就行）。

sudo apt-get install ros-melodic-navigation

安装后在自己的工作空间创建启动的launch文件。

<launch>
<arg name="map_file" default="/home/test/map_files/mymap_office0.yaml"/>

  <!--加载地图-->
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!--amcl start-->
<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="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>

launch文件主要干两件事，一个是加载地图，一个是启动amcl节点（同时也给以amcl中的部分参数进行定义）。
这里有个坑就是因为进行定位需要激光雷达和惯导同时进行的，由于手头只有激光雷达，在启动激光雷达、启动转换二维点（amcl使用的是二维匹配）、启动amcl节点后，会发现激光雷达和地图尺寸不匹配、相对位置也不对，并且无法给定初始点。

此处需要增加一个插件laser_scan_matcher，它是利用激光消息/scan来估计位姿，从而发布/odom，可以理解为模拟了惯导。
首先安装：

sudo apt-get install ros-melodic-laser-scan-matcher

或者安装scan-tools也可以，这个里面包含了laser_scan_matcher。

sudo apt-get install ros-melodic-scan-tools

然后写launch文件（文章中的launch可以尝试结合起来，这样不用一个一个启动，但是需要搞清楚先后顺序）。

<launch>

<!--  set up data playback from bag or not-->
  <param name="/use_sim_time" value="false"/>

 <!--publish an example base_link -> laser transform -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /rslidar 10" />

<!--start the laser scan_matcher -->
  <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" 
    name="laser_scan_matcher_node" output="screen">
    <param name="use_imu" value="false"/>
    <param name="use_odom" value="false"/>
    <param name="use_cloud_input" value="false"/>
    <param name="fixed_frame" value = "odom"/>
    <param name="max_iterations" value="10"/>
  </node>
</launch>

然后需要通过激光雷达、三维转二维点云、laser_scan_matcher、amcl节点即可将激光雷达和地图匹配显示在rviz中，可以通过在rviz中通过2D-Pose-Estimate可实现手动发布初始位姿，PoseArray可显示粒子情况。需要注意的是，需要在fixed frame :map下进行初始位姿的发布。此时貌似就能够通过前后左右自主定位，但是由于初始粒子聚在一起定位过程十分缓慢（不知道我的理解对不对）。

后续想要相对便捷的自主定位，让初始的粒子分布整个地图，以便计算位姿，可以通过前后左右移动激光雷达进行自主定位，我这里是加在了amcl的launch包里，在amcl节点启动后面加上如下命令。

  <param name="initial_cov_xx" value="4"/>
  <param name="initial_cov_yy" value="4"/>
  <param name="initial_cov_aa" value="4"/>

激光雷达自主定位

不过这种方法比较慢，尤其是对于有重复相似场景的地方容易定位出现问题，可以手动给一个大概的初始位置然后再让其自主定位，这样会快很多。
如果出现定位出错的时候，可以在终端在终端可以调用ros里面的global_localization服务：

rosservice call /global_localization "{}"

重新调用后粒子会重新铺满整个地图。