关于速腾激光雷达在实际移动平台上配置cartographer实现室内建图定位

记录一下一年前跑cartographer室内定位建图的流程。硬件配置上，我使用的是速腾16线激光雷达，控制器用的英伟达的Jetson Xavier。首先，我是有一套自动驾驶的代码(实验室祖传下来*^_^*)，接下来就是考虑将cartographer的定位嵌入进去。

0、Cartographer一些元素的介绍

        cartographer迭代了很多版本，最开始使用的是Kalman滤波器，后面改成了Ceres非线性优化，我用的是非线性优化的某个版本。cartographer的Code确实是写的很专业（刚开始看很懵逼），核心的算法和ros是解耦的，在学习cartographer的过程中，自己理了一下大概的框架，后面再写写对大框架的认识，先简单介绍一下：

CSM：想象给定一个栅格地图（或者submap子图），拿到当前帧的点云之后，怎样才能知道激光雷达所在的位置呢？最简单的办法，把激光雷达放在地图的每个格子上，计算在这个位置时，点云是否与地图重合，重合程度最高的位置就是激光雷达的位姿；这个过程，就是一个暴力搜索的过程，但是由于前端维护的是一个个子图计算也还好。回环检测需要遍历大面积地图，采用了分枝定界破这个暴力搜索！

回环：FastCSM，分枝定界，先在低分辨率地图上搜索，逐渐在高分辨率地图上搜索，满足条件之后直接剪枝不再浪费cpu去搜索；

前端：scan2map的方式，采用CeresScanMatch(初值不错，效果就不错)，外部传感器提供的初值不佳时（无imu等），采用RealTimeCSM去求得不错的初值；

后端：CeresScanMatch构建约束优化submap位姿；

imu作用：2d时，用加速度矫正激光；3d时提供重力方向（必须！！）。为激光帧间匹配提供位置初值；（cartographer只使用了线加速度和角速度信息）

体素滤波器/自适应：下采样激光，减少激光数据量，一定程度也解决激光近密远疏；

位姿外推器：在前端匹配之前，融合多传感器数据（odom，imu，lidar），提供前端匹配的初值；

        安装：可以考虑将底层cartographer和ceres安装到系统(sudo make install)，只将cartographer_ros拷贝到自己代码工程中编译即可，这样比较方便。如果是安装到系统，安装之前，需要先修改一下以下两个配置文件：

-- trajectory_build.lua

include "trajectory_builder_2d.lua"
include "trajectory_builder_3d.lua"

TRAJECTORY_BUILDER = {
  trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
  trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
--  pure_localization_trimmer = {    -- 建图配置里关闭，定位配置里reset这个参数就行了
--    max_submaps_to_keep = 3,
--  },
  collate_fixed_frame = true,
  collate_landmarks = false,
}

  --  pose_graph.lua

  --  trimmer操作: 对子图的删除，可能导致子图不连续，影响建图
  --  overlapping_submaps_trimmer_2d = {
  --    fresh_submaps_count = 1,
  --    min_covered_area = 2,
  --    min_added_submaps_count = 5,
  --  },

一、激光与imu接入

        由于我是测试室内定位，需要cartographer建立栅格地图，2d可以不接入imu数据，如果跑室外3d那么必须接入imu数据，并且在lua文件配置中，追踪的坐标系也必须设置为imu的坐标系（3d激光需要使用z轴的数据去估计重力方向）。我这里测试为了方便，就跑的纯激光，定位效果也还算不错。

二、修改urdf

urdf这块主要是静态坐标系之间的标定关系，ros一般使用urdf去描述，很方便。这块的坐标名字需要与lua文件的坐标系对应；

三、修改建图的配置文件和启动文件

launch启动文件：

<launch>
  <param name="/use_sim_time" value="false" />

    #修改为自己的车辆描述文件
  <param name="robot_description" 
    textfile="$(find vehicle_description)/urdf/my_vehicle.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" /> 
    
    #配置节点
    #激光和imu话题映射，速腾的话题为lslidar_point_cloud
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename my_robot.lua"
      output="screen">
    <remap from="points2" to="lslidar_point_cloud" /> 
    <!-- <remap from="imu" to="raw_imu" /> -->
  </node> 
    #显示
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" /> 

    #启动建图节点
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros" 
      type="cartographer_occupancy_grid_node" args="-resolution 0.05"   />
</launch>

下面是lua文件的修改：主要是坐标系对应、外推器关闭、和激光类型，如果要使用gps、landmark、odom的数据，将对应配置true一下，remap映射一下话题就行了。

另外，前端如果在没有使用imu情况下无法提供初值，需要开启use_online_correlative_scan_matching，为ceres优化提供初值，建图精度比较高。

在使用imu数据的情况下，无法完成建图，如图所示，确定urdf关系正确的情况下，可能因为imu数据频率不够（正常imu得有上百hz的速度），勉强能建图之后，旋转时效果不好那么应该是ceres的平移和旋转权重没有调参。

TRAJECTORY_BUILDER_nD.ceres_scan_matcher.translation_weight
TRAJECTORY_BUILDER_nD.ceres_scan_matcher.rotation_weight

以上配置完之后，建图基本上就可以正常运行了。至于lua文件中的后端优化的参数如何设置，可以去官网看看，自己亲自调调才能理解。Cartographer — Cartographer documentationhttps://google-cartographer.readthedocs.io/en/latest/

--我的my_robot.lua一些配置（建议复制一份2d配置文件，然后再进行修改）
include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",   --if using imu, must set tracking_frame to be imu
  published_frame = "base_link",    

  odom_frame = "odom",        --cartographer是否提供odom坐标系，影响不大，关闭的话，那么slam发布的定位直接就是，map->base_link，而不经过odom了
  provide_odom_frame = false,

  publish_frame_projected_to_2d = false,     --是否发布为纯2d位姿(x,y,theta)
  publish_tracked_pose = true,    --激光定位的输出，查看话题/tracked_pose
  use_pose_extrapolator = false,     -- 纯激光定位，建议关闭，没有更多信息给位姿外推器，会导致初始时刻车子定位各种古怪，关掉就好了

  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_point_clouds = 1,            --激光类型，速腾的机械雷达选这个，我只用一个雷达，为1
  num_subdivisions_per_laser_scan = 1,    --一帧激光拆多少包发送，主要是畸变处理，室内定位移动速度不是很快，设置1没啥问题
 
  use_odometry = false,    -- 都不使用
  use_nav_sat = false,
  use_landmarks = false,

  ......
}
  MAP_BUILDER.use_trajectory_builder_2d = true    --2d建图
    -- 不使用imu
  TRAJECTORY_BUILDER_2D.use_imu_data = false
  TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1    -- 多少次激光为一帧，进行运动补偿，我认为应该和num_subdivisions_per_laser_scan一致
    -- 无imu，就要使用相关性匹配为ceres提供初值，提升精度
  TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
    -- 激光深度限制（range>max_range, 深度设置为missing_data_ray_length）
  TRAJECTORY_BUILDER_2D.min_range = 0.15
  TRAJECTORY_BUILDER_2D.max_range = 16
  TRAJECTORY_BUILDER_2D.missing_data_ray_length = 16
    -- 激光高度限制
  TRAJECTORY_BUILDER_2D.min_z = -0.1
  TRAJECTORY_BUILDER_2D.max_z = 0.3
    -- 运动滤波器（相当于关键帧策略）
  TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(3.)	--只有当scan的平移、旋转超过阈值时才会被加入到submap
  TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1   --单位m
  TRAJECTORY_BUILDER_2D.submaps.num_range_data = 90  --一个子图多少帧数据
  ......
    -- 全局slam多少个节点进行优化
  POSE_GRAPH.optimize_every_n_nodes = 50    --=0，则关闭后端

建图效果如下：（激光频率也就10hz，无imu，建出来也算还好）

四、修改定位的配置文件和启动文件

        建完图，接下来自然就是定位了。我理解的SLAM定位分为两类，直接帧间递推输出频率较高的定位数据，这个当然依赖高鲁棒性的前端里程计，这种定位可能会漂，不会突变；还有一种是建完图，再进行匹配定位。因此slam侧重也有两类，侧重建图 or 侧重定位。

        扯远了。。。书接上文，我们已经建好了栅格地图（哦，建图完成调用cartographer的服务就能保存），接下来就是配置定位文件backpack_2d_localization.lua：

include "my_robot.lua"        -- 修改为自己的建图配置文件

TRAJECTORY_BUILDER.pure_localization_trimmer = {     -- 内存中用于匹配的子图数量
  max_submaps_to_keep = 3,
}

    --你需要reset的my_robot.lua里面的参数
POSE_GRAPH.optimize_every_n_nodes = 20

return options

        传说中cartographer的定位是优于基于粒子滤波的amcl定位。但是我不知道，哈哈哈，我没有实际跑过amcl定位，只是用虚拟雷达数据跑过（真实数据和虚拟数据差别还是蛮大的，后面可能会做一下），在此不下定论。。。后面找找以前做的时候录制的视频，后面放个视频在b站，看看实际效果。