基于rplidar || hokuyo+imu的cartographer建图

基于rplidar || hokuyo+imu的cartographer建图

本文介绍加入imu优化odom以及定位的建图修改方法。

首先需要做的是搞清楚 需要做什么，看起来像句废话。那么废话不多说，我们想加入imu去优化odom和location，在雷达运动过程中使定位更准，建图效果更好。既然要加入imu，那么tf树肯定需要调整，需要加入imu_link。那么具体怎么做呢，又需要修改那些配置文件呢？
本人受原仿真命令的启发

roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/bags/cartographer.bag

不懂的需要先运行bag包进行仿真。其tf树包含imu话题，也有base_link。那么需要修改的文件有3个launch文件，lua文件以及urdf文件。

1. launch文件修改

选择在backpack_2d.launch的基础上进行修改。
首先，需要修改的是仿真时钟

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

这里必须为false，否则静态tf变换不能发布。
然后加载机器人模型，此处需要注意的是urdf配置文件路径和名字，不再赘述。
然后发布机器人模型以及tf变换

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /> 
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

接下来启动cartographer_node，加载lua文件，但是此处的话题映射需加入imu话题

<remap from="scan" to="/scan" />
<remap from="imu" to="/dji_sdk/imu" />

接下来与上篇相同，启动cartographer_occupancy_grid_node节点和rviz，不再赘述。

2. lua文件修改

lua文件需要与上述launch文件配套。
lua文件的参数也不要随意删减和添加，只需要修改部分参数即可。

tracking_frame = "body_FLU",      --instead imu_link if use dji
published_frame = "base_link",

tracking_frame修改为imu的frame_id，运行imu后可以输出查看imu的数据，就可以看到frame_id.

rostopic echo /imu

published_frame写为base_link.
下面给出我的配置参数：

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "body_FLU",      --instead imu_link if use dji
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1

TRAJECTORY_BUILDER_2D.min_range = 0.15
TRAJECTORY_BUILDER_2D.max_range = 12.
TRAJECTORY_BUILDER_2D.submaps.num_range_data=35
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.imu_gravity_time_constant=10.0
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight=10
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight=40
POSE_GRAPH.constraint_builder.max_constraint_distance=4.

return options

3. urdf文件修改

urdf文件也要与launch文件配套。
此处的urdf文件主要描述机器人模型以及传感器的相对位置。也就是静态tf变换，由launch发布tf变换。

<robot name="cartographer_backpack_2d">
  <material name="orange">
    <color rgba="1.0 0.5 0.2 1" />
  </material>
  <material name="gray">
    <color rgba="0.2 0.2 0.2 1" />
  </material>

  <link name="body_FLU">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>

  <link name="laser">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <cylinder length="0.05" radius="0.03" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <link name="base_link" />

  <joint name="body_FLU_joint" type="continuous">
    <parent link="base_link" />
    <child link="body_FLU" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>

  <joint name="laser_joint" type="continuous">
    <parent link="base_link" />
    <child link="laser" />
    <origin xyz="0.1007 0 0.0558" rpy="0. 0. 0." />
  </joint>
</robot>

这里的laser_joint和body_FLU_joint可以根据实际情况进行修改。只要能完成静态tf树就行。
至此，所有的配置文件修改完毕，终于可以进行试验了。

4. 试验结果

查看节点图：

rqt_graph

每次修改需要运行：

catkin_make_isolated --install –use-ninja
source install_isolated/setup.bash

然后启动雷达和cartographer建图。

roslaunch cartographer_ros  ***.launch

实际场景建图结果：
这是在相同的场景移动速度更快的情况下的建图，效果明显更好，上图突出的是玻璃透射出去的部分，仅供参考。