基于Cartographer的3D SLAM(Lidar+IMU)

1 系统要求

虽然 Cartographer 可以在其他系统上运行,但已确认可以在满足以下要求的系统上运行:

  • 64-bit, modern CPU (e.g. 3rd generation i7)
  • 16 GB RAM
  • Ubuntu 14.04 (Trusty) and 16.04 (Xenial)
  • gcc version 4.8.4 and 5.4.0

目前支持以下ROS发行版:

  • Indigo
  • Kinetic
  • Lunar
  • Melodic

2 编译与安装

为了编译 Cartographer ROS,我们建议使用wstool和rosdep。为了更快的构编译建,我们还建议使用Ninja。

sudo apt-get update
sudo apt-get install -y python-wstool python-rosdep ninja-build

catkin_ws中创建一个新的cartographer_ros工作空间。

mkdir catkin_ws
cd catkin_ws
wstool init src
wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src

安装cartographer_ros的依赖项(proto3和deb软件包)。如果您在安装ROS后已经执行了命令sudo rosdep init,将会输出错误,可以忽略此错误。

src/cartographer/scripts/install_proto3.sh
sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

编译和安装:

catkin_make_isolated --install --use-ninja

注意:由于某种原因,无法下载ceres-solver,解决办法是在Github上下载ceres-slover并放到指定的路径下。

3 使用自己的bag进行测试

3.1 录制bag

对于3D Cartographer,必须依赖IMU,具体原因参见:https://google-cartographer-ros.readthedocs.io/en/latest/faq.html#why-is-imu-data-required-for-3d-slam-but-not-for-2d 。

因此,要录制包含Velodyne VLP-16和Xsens MTi-300的数据的bag。

另外,还需要做传感器间的标定,在urdf文件中会用到。

3.2 分析bag

使用cartographer_rosbag_validate工具来自动分析bag中的数据。

cartographer_rosbag_validate -bag_filename path_your_bag.bag

以下是我的输出:

I0816 11:15:15.414490 22913 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/imu_raw" (frame_id: "mti/data"):
Count: 23827  Min: 0.004766  Max: 0.015202  Mean: 0.010000
[0.004766, 0.005810)	                    	Count: 48 (0.201452%)	Total: 48 (0.201452%)
[0.005810, 0.006853)	                    	Count: 1 (0.004197%)	Total: 49 (0.205649%)
[0.006853, 0.007897)	                    	Count: 0 (0.000000%)	Total: 49 (0.205649%)
[0.007897, 0.008940)	                    	Count: 2 (0.008394%)	Total: 51 (0.214043%)
[0.008940, 0.009984)	             #######	Count: 8915 (37.415539%)	Total: 8966 (37.629581%)
[0.009984, 0.011028)	        ############	Count: 14812 (62.164772%)	Total: 23778 (99.794350%)
[0.011028, 0.012071)	                    	Count: 0 (0.000000%)	Total: 23778 (99.794350%)
[0.012071, 0.013115)	                    	Count: 0 (0.000000%)	Total: 23778 (99.794350%)
[0.013115, 0.014158)	                    	Count: 26 (0.109120%)	Total: 23804 (99.903473%)
[0.014158, 0.015202]	                    	Count: 23 (0.096529%)	Total: 23827 (100.000000%)
E0816 11:15:15.414909 22913 rosbag_validate_main.cc:382] Point data (frame_id: "velodyne") has a large gap, largest is 0.101079 s, recommended is [0.0005, 0.05] s with no jitter.
I0816 11:15:15.415345 22913 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/points_raw" (frame_id: "velodyne"):
Count: 2361  Min: 0.100648  Max: 0.101079  Mean: 0.100864
[0.100648, 0.100691)	                    	Count: 3 (0.127065%)	Total: 3 (0.127065%)
[0.100691, 0.100734)	                    	Count: 9 (0.381194%)	Total: 12 (0.508259%)
[0.100734, 0.100777)	                   #	Count: 67 (2.837781%)	Total: 79 (3.346040%)
[0.100777, 0.100820)	                  ##	Count: 274 (11.605252%)	Total: 353 (14.951292%)
[0.100820, 0.100864)	             #######	Count: 860 (36.425243%)	Total: 1213 (51.376534%)
[0.100864, 0.100907)	             #######	Count: 798 (33.799236%)	Total: 2011 (85.175774%)
[0.100907, 0.100950)	                  ##	Count: 275 (11.647607%)	Total: 2286 (96.823380%)
[0.100950, 0.100993)	                    	Count: 56 (2.371876%)	Total: 2342 (99.195259%)
[0.100993, 0.101036)	                    	Count: 17 (0.720034%)	Total: 2359 (99.915291%)
[0.101036, 0.101079]	                    	Count: 2 (0.084710%)	Total: 2361 (100.000000%)

提取关键信息:

  • IMU topic:/imu_raw
  • IMU frame_id:mti/data
  • Velodyne topic:/points_raw
  • Velodyne frame_id:velodyne

3.3 编写.lua文件

my_demo.lua:

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "mti/data",
  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 = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,
  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.,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66

return options

说明:

  • num_subdivisions_per_laser_scan必须大于等于1,否则会报错:Check failed: options.num_subdivisions_per_laser_scan >=1
  • 不清楚TRAJECTORY_BUILDER_3D.num_accumulated_range_data的意义,这里设置成1
  • 传感器为Velodyne VLP-16和Xsens MTi-300,没有用到里程计,GPS和路标,因此use_odometryuse_nav_satuse_landmarks都设置为falsenum_laser_scansnum_multi_echo_laser_scans都为0,num_point_clouds为1
  • tracking_frame必须为IMU,因此这里为mti/data
  • 其他参数保持默认

3.4 编写urdf文件

URDF是ROS中机器人模型的描述格式,包含对机器人刚体外观、物理属性、关节类型等方面的描述。

my_demo.urdf:

<robot name="cartographer_backpack_3d">
  <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="mti/data">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>

  <link name="velodyne">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <link name="base_link" />

  <joint name="imu_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="mti/data" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>

  <joint name="vlp16_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="velodyne" />
    <origin xyz="0. 0.1 0." rpy="0. 0. 0." />
  </joint>
  
</robot>

说明:

  • 根标签设置模型,并定义模型名称
  • 标签用来定义外观属性,标签设置颜色,设置RGBA值
  • joint为关节,用于连接两个组件,在这里,有两个joint,分别为IMU与base_link和Velodyne与base_link;joint的类型为fixed,表明join是固定的,不允许关节发生运动;标签定义了joint的起点,这里为传感器之间的标定参数

3.5 编写launch文件

编写launch文件,实现多节点的配置和启动。

(1)my_demo.launch

<launch>
  <param name="/use_sim_time" value="true" />
  
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/my_demo.urdf" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />

  <node name="cartographer_offline_node" pkg="cartographer_ros"
      type="cartographer_offline_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basenames my_demo.lua
          -urdf_filenames $(find cartographer_ros)/urdf/my_demo.urdf
          -bag_filenames $(arg bag_filenames)"
      output="screen">
    <remap from="points2" to="/points_raw" />
    <remap from="imu" to="/imu_raw" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

说明:

  • 用于话题的重映射,Cartographer需要的是pointsimu,我们提供的是/points_raw/imu_raw
  • cartographer_offline_node速度较快。

执行以下命令开始建图:

roslaunch cartographer_ros my_demo.launch bag_filenames:=/path/to/your_bag.bag

(2)my_demo.launch2

cartographer_offline_node速度较快,看不清建图过程的细节,这时可以使用cartographer_node

两个节点除了速度上的区别,其他不同之处我还不清楚。

<launch>
  <param name="/use_sim_time" value="true" />
  
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/my_demo.urdf" />

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

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename my_demo.lua"
      output="screen">
    <remap from="points2" to="/points_raw" />
    <remap from="imu" to="/imu_raw" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
  
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />
  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" />
</launch>

执行以下命令开始建图:

roslaunch cartographer_ros my_demo2.launch bag_filename:=/path/to/your_bag.bag

bag1:

基于Cartographer的3D SLAM(Lidar+IMU)_第1张图片

bag2:

基于Cartographer的3D SLAM(Lidar+IMU)_第2张图片

完成后会在bag所在目录下生成.pbstream文件。

使用以下命令查看tf树:

rosrun rqt_tf_tree rqt_tf_tree

基于Cartographer的3D SLAM(Lidar+IMU)_第3张图片

3.6 保存地图

保存地图需要两个文件,分别为assert_write_my_demo.luaassets_writer_my_demo.launch

(1)assert_write_my_demo.lua

VOXEL_SIZE = 5e-2

include "transform.lua"

options = {
  tracking_frame = "base_link",
  pipeline = {
    {
      action = "min_max_range_filter",
      min_range = 1.,
      max_range = 60.,
    },
    {
      action = "dump_num_points",
    },

    {
      action = "fixed_ratio_sampler",
      sampling_ratio = 0.01,
    },

    {
      action = "write_pcd",
      filename = "map.pcd"
    },

    {
      action = "color_points",
      frame_id = "velodyne",
      color = { 255., 0., 0. },
    },
  }
}

return options

(2)assets_writer_my_demo.launch

<launch>
  <node name="cartographer_assets_writer" pkg="cartographer_ros" required="true"
      type="cartographer_assets_writer" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename assets_writer_my_demo.lua
          -urdf_filename $(find cartographer_ros)/urdf/my_demo.urdf
          -bag_filenames $(arg bag_filenames)
          -pose_graph_filename $(arg pose_graph_filename)"
      output="screen">
  </node>
</launch>

执行以下命令,生成pcd格式的点云地图。

roslaunch cartographer_ros assets_writer_my_demo.launch bag_filenames:=your_bag.bag pose_graph_filename:=your_bag.bag.pbstream

使用以下命令可视化pcd文件:

pcl_viewer *.pcd

基于Cartographer的3D SLAM(Lidar+IMU)_第4张图片

很明显,地图质量很差。

4 总结

本文录制了Lidar+IMU数据,然后使用Cartographer实现3D SLAM。

可以看出,定位效果比较差,可能的原因是需要:

  • 继续调参
  • 严格标定IMU-Lidar

你可能感兴趣的:(SLAM)