办公问答网

 找回密码
 立即注册
搜索
热搜: 活动 交友 discuz
查看: 249|回复: 9

Cartographer 3D实时建图教程(超详细)

[复制链接]

2

主题

5

帖子

9

积分

新手上路

Rank: 1

积分
9
发表于 2022-9-22 17:36:38 | 显示全部楼层 |阅读模式
简述

​       此文将详细的介绍一下如何配置cartographer的3D建图。cartographer主要用于2D建图,但是也是进行3D建图的。cartographer的3D建图必须要IMU数据,这就要求我们在lua配置文件中配置好IMU数据和雷达数据的坐标系关系,cartographer需要借助这个坐标系关系修正雷达的数据。这个坐标系关系的配置,对于小白而言(比如我),是进行3D建图的主要难点,当时折腾了好久。而此文的重点其实也是相对详细的介绍如何根据实际的传感器数据,设置好这个坐标转换关系。网上关于3D建图的配置教程确实不少,但是都差点意思,看完大概率还是不知道怎么设置... 希望此文可以给大伙们提供一个完整的思路及配置方法。如果觉得对你有帮助一定多多点赞哇~ 用爱发电的博主需要鼓励!
​       如果只使用雷达数据进行cartographer的2D建图,可以不用管这个坐标系关系的配置。可以参考之前的几篇文章,包含cartographer环境的极速配置与测试、手持2D建图的简单配置。
环境极速配置:
手持2D建图:
大家中秋快乐 ~    今天爬了山,吃了自助,发了此文,看了动漫,非常不错!
零、流程简述

需知:我进行3D建图时只使用了雷达和IMU的数据,并没有使用移动平台的里程计等相关内容,雷达和IMU都是固定在小车底盘上面的,遥控器控制小车底盘进行运动。后文涉及的配置过程也都将基于该情况。然后这里的配置流程是简述,用于梳理配置流程,大概看看就行,后文将会详细介绍
cartographer的3D建图配置其实也就三步走(已设置超链接,可查看参考的原文),具体如下:

  • 1)确定传感器之间的位姿
​       因为IMU的数据将用于修正雷达数据,所以需要进行数据的坐标转换,这里就需要确定好安装的雷达和IMU之间的空间位姿关系。同时,还需要知道IMU数据、雷达数据的frame_id,即这些数据都分别发布在哪一个坐标系上。此外,因为雷达和imu其实都是安装在底盘上的,所以就需要配置底盘、雷达、IMU三者的坐标关系。我们一般使用URDF文件描述机器人的主要组件及其之间的关系,这个URDF文件编写后需要在launch文件中引用。前面查到的frame_id需要在URDF文件中使用。不知道URDF是啥?可以参考下面的文章:

  • 2)配置参数文件(lua)
​       上一步的URDF配置传感器之间的位姿关系。这一步则需要配置一下cartographer的TF树。这里的修改主要涉及到cartographer_ros/cartographer_ros/configuration_files下的lua文件中的几个参数:
map_frame = "map",  -- 默认即可
-- 如果没有imu,下面两个frame可以都修改为雷达的坐标系,通常为laser
-- 如果有imu需要将 tracking_frame 更改为imu的frame_id  
  tracking_frame = "base_link",  -- 一般为发布频率最高的传感器的 frame_id
  published_frame = "base_link",  -- 底盘的 frame_id
  odom_frame = "odom",    -- cartographer提供的里程计坐标系 frame_id 【与实际里程计无关】
  provide_odom_frame = false,  -- 是否使用cartographer提供的里程计坐标系

  • 3)配置启动文件(launch)
​       前面两个步骤把主要的内容都完成了,这一步还需设置两个内容:1)调用对应的配置文件 2)配置发布imu数据和雷达数据的话题。launch文件的具体内容见后文。
一、传感器位姿

1 测相对位姿

​       这里我们需要确定雷达和IMU的具体安装位姿关系。以我们的小车为例,雷达中心和IMU中心基本重合,只是存在Z轴方向的高度偏移,由此可以得到雷达和IMU的相对位姿关系(后续编写的URDF文件中需要用)。细心的小伙伴可能会发现雷达上面明明固定的是一个pixhawk飞控。确实如此,我这里直接采用从pixhawk飞控中读取到的IMU数据用于cartographer建图。


2 查frame_id

​       cartographer的3D建图将会使用到IMU的线速度和角速度数据,并将其用于校准雷达的点云数据。为此,我们需要在对应的配置文件中定义好imu和雷达的相对坐标关系。在进行3D建图的配置文件修改之前,我们需要确定IMU数据的话题和他的frame_id,并在rviz中进行一个可视化的验证(雷达消息确认的步骤相同,之前文章已经叙述过,此处略)。我这里是使用了PX4飞控发布的IMU数据作为cartographer的3D建图所需的IMU数据源,对应的话题为/mavros/imu/data。我们可以使用下面的命令查看正在活跃的话题的frame_id。这里的frame_id很重要,他代表着发布这个imu数据的坐标系的名称是什么。
# rostopic echo [topic]  | grep frame_id
# 查看我的imu话题的frame_id
rostopic echo /mavros/imu/data  | grep frame_id

# 查看雷达点云数据的frame_id
rostopic echo /point_cloud_raw  | grep frame_id​       由此可以获取到我的imu数据的frame_id为base_link,而雷达数据的frame_id为laser。此外,我们还可以借助rviz,看看base_link坐标系下有没有对应的imu数据。如下图中,需要将Fixed frame修改为frame_id,同时需要添加一个Imu组件并选择对应的话题。如果一切正确,就可以看见如图一个箭头,我们此时移动imu模块(我这里是pixhawk飞控),箭头也会对应偏移(主要是体现在姿态角的变化)。


3 编写URDF

​       这里查到的frame_id即为URDF文件中对应物体的link name,务必一致,不然无法建立一个Cartographer需要的tf树,也就无法正常进行3D建图。因为我这里查得雷达数据frame_id为laser,而imu则为base_link。用于cartographer3D建图的URDF文件中一般得描述三个物体:底盘、雷达、IMU。因为我这里imu的frame_id为base_link,而base_link一般是底盘的frame_id,所以我这里直接默认底盘和IMU是完全重合的,即只在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="laser">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <!-- IMU or 底盘 (可以描述尺寸大小也可以不描述) -->
  <link name="base_link" />

  <!-- 描述 雷达和IMU/底盘 的相对位姿 -->
  <joint name="laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="laser" />
    <origin xyz="0.00 0. -0.07" rpy="0. 0 3.1416" />
  </joint>

</robot>​       上述的urdf文件需要根据自己的安装位姿以及话题的frame_id来设置。urdf文件可以描述发布雷达和imu的坐标系的相对位姿,如果没有这个相对位姿,cartographer是无法进行正常的3D建图的。如果准备加入移动机器人的里程计数据,也需要查一下对应话题的frame_id,然后描述一下雷达和imu相对里程计的frame_id的相对位姿即可(三个物体只需描述两个关系,即2个joint;两个物体只需描述一个joint)。
二、配置参数文件

​       如前面所介绍的,我们主要需关注下面这几个 frame 分别代表什么,有什么用,然后结合自己的情况进行修改设置就行了。map_frame一般就设置为map,无需修改;我们主要需要关注tracking_frame和published_frame;而odom_frame是可选择是否使用的,通过设置provide_odom_frame的参数实现。
map_frame = "map",  -- 默认即可
-- 如果没有imu,下面两个frame可以都修改为雷达的坐标系,通常为laser
-- 如果有imu需要将 tracking_frame 更改为imu的frame_id  
  tracking_frame = "base_link",  -- 一般为发布频率最高的传感器的 frame_id
  published_frame = "base_link",  -- 底盘的 frame_id
  odom_frame = "odom",    -- cartographer提供的里程计坐标系 frame_id 【与实际里程计无关】
  provide_odom_frame = false,  -- 是否使用cartographer提供的里程计坐标系1 tracking_frame

​       tracking_frame一般设置为发布频率最高的传感器的frame_id,cartographer将会把其他数据都转移到该坐标系下进行计算。如果只使用雷达数据进行2D建图,那就只需要将其设置为雷达数据话题的frame_id,一般为laser。如果使用雷达数据+IMU进行2D或者3D建图,因为IMU的发布频率明显高于雷达,所以需要设置为imu数据话题的frame_id,一般imu_link。我使用的Pixhawk飞控的imu的frame_id为base_link,故设置为base_link
2 published_frame

​       cartographer发布的tf树最后将指向published_frame,即published_frame不是cartographer提供的,这里如果没设置正确,tf树就不能连接成功,建图也就不能正常进行。这个一般设置为底盘的frame_id,也就是URDF文件中的底盘的link name,一般为base_link、base_footprint之类的名字。因为我的imu的frame_id比较特殊,名为base_link,所以我干脆直接把imu和底盘当做一个物体在URDF中进行描述。由此,我这里将published_frame设置为base_link。我们前面编写的URDF中描述的多个物体,都定义了他们与底盘之间的关系。由此,我们只需要将cartographer发布的tf树指向底盘坐标系,也就可以借助URDF文件找到雷达和IMU的坐标转换关系了。
3 odom_frame

​       odom_frame与实际的里程计话题及消息没有什么关系,他只是cartographer提供的一个中间话题。将provide_odom_frame设置为false,cartographer将会提供tf树map指向published_frame;如果设置为true,cartographer将会提供tf树map->odom_frame指向published_frame。也就是是否需要在map与published_frame之间添加一个中间坐标系。他具体有啥用我暂时也似懂非懂的:在工作环境下有很多复杂的TF转换关系时,这个odom_frame可以发挥一些作用。我这里设置provide_odom_frame为false,即不启用。
​       这一步其实就是在构建一个完整的TF树,能够从"map_frame -> published_frame"。如下图便是我根据自身环境设置后,能够成功建图时的一个完整的TF树。其中很多内容都是pixhwak飞控发布的tf转换内容,我们忽略即可。如果你发现自己无法正常进行3D建图,那不妨查看一下自己的TF树是否完整,cartographer的3D建图的一大难点其实就在于这个TF树的配置上。


再啰嗦几句:如上图,map—>base_link是我们通过修改lua文件中的参数设置的;base_link—>laser则是我们通过配置URDF文件实现的。自己的tf树哪里不对改哪里即可。
4 lua文件

​       前面的参数设置内容不管是2D建图还是3D建图都是适用的。而下面的lua文件则是我3D建图的完整配置文件,2D不适用。
include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
-- 下面两个frame需要修改为雷达的 坐标系,通常为laser
-- 如果有imu 需要将 tracking_frame 更改为 imu的那个link
  tracking_frame = "base_link",  
  published_frame = "base_link",  
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  -- use_pose_extrapolator = true,

  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.,
}

MAP_BUILDER.use_trajectory_builder_3d = true
   TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1
   TRAJECTORY_BUILDER_3D.min_range = 0.3
  --  defualt 60
   TRAJECTORY_BUILDER_3D.max_range = 30.
   TRAJECTORY_BUILDER_2D.min_z = 0.1
   TRAJECTORY_BUILDER_2D.max_z = 1.0
   TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = false
MAP_BUILDER.num_background_threads = 5  -- 后端的线条数,越大实时性越好
   POSE_GRAPH.optimization_problem.huber_scale = 5e2

  -- 设置为0,关闭global slam (取消后端优化,此时再出现建图问题就是前端的锅了)
  -- default:60
   POSE_GRAPH.optimize_every_n_nodes =60   -- 多少个点优化一次  越小优化频率越高

   POSE_GRAPH.constraint_builder.sampling_ratio = 0.03  
   POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 16
   POSE_GRAPH.constraint_builder.min_score = 0.6
   POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60

   POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e3
   POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e3

return options三、配置启动文件

​       前面提及的《一、传感器位姿》和《二、配置参数文件》其实已经把cartographer 3D建图的主要配置内容都详细讲解了。当然,如果想要使用雷达+IMU进行2D建图,也是可以参考前面的内容的。
1 launch文件

完整的launch文件如下,注释已经非常的清楚,不再赘述:
<launch>
  <!-- 要跑自己的实际数据,这里务必设置为false -->
  <param name="/use_sim_time" value="false" />
  <!-- 需要包含自己设置的URDF描述文件 -->
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/handon_demo.urdf" />

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

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename hand_test_3d.lua"
      output="screen">
      <!-- 上面的 configuration_basename 需要修改为自己对应的lua文件名-->
    <!-- 下面需要修改为雷达数据的话题 -->
    <remap from="points2" to="/point_cloud_raw" />
    <!-- 发布的imu话题 /mavros/imu/data -->
    <remap from="imu" to="mavros/imu/data" />
  </node>

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

  <!-- lx_3d.rviz 中添加了3d点云的显示  (他编写了对应的代码实现的) -->
  <!-- 如果下载的是官方的cartographer代码,需要修改为 demo_3d.rviz -->
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/lx_3d.rviz" />

</launch>2 启动建图程序

​       可以使用Vscode打开工程文件并Ctrl+Shift+B进行编译,然后就可以运行建图程序咯。下面只是我使用自己录制的bag数据包进行建图时使用的命令,需要自行修改。因为数据丢失,我目前也无法提供之前录制的bag数据包给大家测试,等后续再进行cartographer的建图工作时,再提供到时候录制的数据包吧。
# 1)启动cartographer建图
cd carto_ws/cartographer_detailed_comments_ws/
source ./devel_isolated/setup.bash
roslaunch cartographer_ros hand_test_3d.launch

# 2)启动bag包
cd ~/Documents/Bagfiles/Carto-record/
rosbag play -r 4 2022-08-21-all-3.bagCartographer的3D建图内容还有【地图保存】、【参数配置】这两部分的内容,将会在后续的文章中介绍,敬请期待 ~
回复

使用道具 举报

0

主题

4

帖子

0

积分

新手上路

Rank: 1

积分
0
发表于 2022-9-22 17:37:15 | 显示全部楼层
太卷了[机智][机智][机智]
回复

使用道具 举报

1

主题

6

帖子

5

积分

新手上路

Rank: 1

积分
5
发表于 2022-9-22 17:38:12 | 显示全部楼层
[捂脸] 早就写好了,今天发一下。
回复

使用道具 举报

3

主题

13

帖子

14

积分

新手上路

Rank: 1

积分
14
发表于 2022-9-22 17:39:10 | 显示全部楼层
楼主哪个院校的 有点东西[发呆]
回复

使用道具 举报

1

主题

6

帖子

7

积分

新手上路

Rank: 1

积分
7
发表于 2022-9-22 17:40:03 | 显示全部楼层
强强强[酷]
回复

使用道具 举报

2

主题

9

帖子

12

积分

新手上路

Rank: 1

积分
12
发表于 2022-9-22 17:40:50 | 显示全部楼层
一流985的[爱]
回复

使用道具 举报

0

主题

9

帖子

10

积分

新手上路

Rank: 1

积分
10
发表于 2022-9-22 17:41:41 | 显示全部楼层
一流211,三流958[doge]
回复

使用道具 举报

0

主题

5

帖子

0

积分

新手上路

Rank: 1

积分
0
发表于 2022-9-22 17:41:50 | 显示全部楼层
俺也一样!
回复

使用道具 举报

0

主题

4

帖子

0

积分

新手上路

Rank: 1

积分
0
发表于 2022-9-22 17:42:32 | 显示全部楼层
大佬 更啊 比看小说给力多了[发呆]
回复

使用道具 举报

2

主题

5

帖子

9

积分

新手上路

Rank: 1

积分
9
发表于 2022-9-22 17:43:13 | 显示全部楼层
很强
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

Archiver|手机版|小黑屋|办公问答网

GMT+8, 2025-3-16 13:34 , Processed in 0.101011 second(s), 22 queries .

Powered by Discuz! X3.4

© 2001-2013 Comsenz Inc. Templated By 【未来科技 www.veikei.com】设计

快速回复 返回顶部 返回列表