|
|
简述
此文将详细的介绍一下如何配置cartographer的3D建图。cartographer主要用于2D建图，但是也是进行3D建图的。cartographer的3D建图必须要IMU数据，这就要求我们在lua配置文件中配置好IMU数据和雷达数据的坐标系关系，cartographer需要借助这个坐标系关系修正雷达的数据。这个坐标系关系的配置，对于小白而言（比如我），是进行3D建图的主要难点，当时折腾了好久。而此文的重点其实也是相对详细的介绍如何根据实际的传感器数据，设置好这个坐标转换关系。网上关于3D建图的配置教程确实不少，但是都差点意思，看完大概率还是不知道怎么设置... 希望此文可以给大伙们提供一个完整的思路及配置方法。如果觉得对你有帮助一定多多点赞哇~ 用爱发电的博主需要鼓励！
如果只使用雷达数据进行cartographer的2D建图，可以不用管这个坐标系关系的配置。可以参考之前的几篇文章，包含cartographer环境的极速配置与测试、手持2D建图的简单配置。
环境极速配置：
手持2D建图：
大家中秋快乐 ~ 今天爬了山，吃了自助，发了此文，看了动漫，非常不错！
零、流程简述
需知：我进行3D建图时只使用了雷达和IMU的数据，并没有使用移动平台的里程计等相关内容，雷达和IMU都是固定在小车底盘上面的，遥控器控制小车底盘进行运动。后文涉及的配置过程也都将基于该情况。然后这里的配置流程是简述，用于梳理配置流程，大概看看就行，后文将会详细介绍。
cartographer的3D建图配置其实也就三步走(已设置超链接，可查看参考的原文)，具体如下：
因为IMU的数据将用于修正雷达数据，所以需要进行数据的坐标转换，这里就需要确定好安装的雷达和IMU之间的空间位姿关系。同时，还需要知道IMU数据、雷达数据的frame_id，即这些数据都分别发布在哪一个坐标系上。此外，因为雷达和imu其实都是安装在底盘上的，所以就需要配置底盘、雷达、IMU三者的坐标关系。我们一般使用URDF文件描述机器人的主要组件及其之间的关系，这个URDF文件编写后需要在launch文件中引用。前面查到的frame_id需要在URDF文件中使用。不知道URDF是啥？可以参考下面的文章：
上一步的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提供的里程计坐标系
前面两个步骤把主要的内容都完成了，这一步还需设置两个内容：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=&#34;cartographer_backpack_3d&#34;>
<material name=&#34;orange&#34;>
<color rgba=&#34;1.0 0.5 0.2 1&#34; />
</material>
<material name=&#34;gray&#34;>
<color rgba=&#34;0.2 0.2 0.2 1&#34; />
</material>
<!-- 描述雷达的尺寸大小、颜色 -->
<link name=&#34;laser&#34;>
<visual>
<origin xyz=&#34;0.0 0.0 0.0&#34; />
<geometry>
<cylinder length=&#34;0.07&#34; radius=&#34;0.05&#34; />
</geometry>
<material name=&#34;gray&#34; />
</visual>
</link>
<!-- IMU or 底盘 （可以描述尺寸大小也可以不描述） -->
<link name=&#34;base_link&#34; />
<!-- 描述 雷达和IMU/底盘 的相对位姿 -->
<joint name=&#34;laser_link_joint&#34; type=&#34;fixed&#34;>
<parent link=&#34;base_link&#34; />
<child link=&#34;laser&#34; />
<origin xyz=&#34;0.00 0. -0.07&#34; rpy=&#34;0. 0 3.1416&#34; />
</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 = &#34;map&#34;, -- 默认即可
-- 如果没有imu，下面两个frame可以都修改为雷达的坐标系，通常为laser
-- 如果有imu需要将 tracking_frame 更改为imu的frame_id
tracking_frame = &#34;base_link&#34;, -- 一般为发布频率最高的传感器的 frame_id
published_frame = &#34;base_link&#34;, -- 底盘的 frame_id
odom_frame = &#34;odom&#34;, -- 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树，能够从&#34;map_frame -> published_frame&#34;。如下图便是我根据自身环境设置后，能够成功建图时的一个完整的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 &#34;map_builder.lua&#34;
include &#34;trajectory_builder.lua&#34;
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = &#34;map&#34;,
-- 下面两个frame需要修改为雷达的 坐标系，通常为laser
-- 如果有imu 需要将 tracking_frame 更改为 imu的那个link
tracking_frame = &#34;base_link&#34;,
published_frame = &#34;base_link&#34;,
odom_frame = &#34;odom&#34;,
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=&#34;/use_sim_time&#34; value=&#34;false&#34; />
<!-- 需要包含自己设置的URDF描述文件 -->
<param name=&#34;robot_description&#34;
textfile=&#34;$(find cartographer_ros)/urdf/handon_demo.urdf&#34; />
<node name=&#34;robot_state_publisher&#34; pkg=&#34;robot_state_publisher&#34;
type=&#34;robot_state_publisher&#34; />
<node name=&#34;joint_state_publisher&#34; pkg=&#34;joint_state_publisher&#34;
type=&#34;joint_state_publisher&#34; />
<node name=&#34;cartographer_node&#34; pkg=&#34;cartographer_ros&#34;
type=&#34;cartographer_node&#34; args=&#34;
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename hand_test_3d.lua&#34;
output=&#34;screen&#34;>
<!-- 上面的 configuration_basename 需要修改为自己对应的lua文件名-->
<!-- 下面需要修改为雷达数据的话题 -->
<remap from=&#34;points2&#34; to=&#34;/point_cloud_raw&#34; />
<!-- 发布的imu话题 /mavros/imu/data -->
<remap from=&#34;imu&#34; to=&#34;mavros/imu/data&#34; />
</node>
<node name=&#34;cartographer_occupancy_grid_node&#34; pkg=&#34;cartographer_ros&#34;
type=&#34;cartographer_occupancy_grid_node&#34; args=&#34;-resolution 0.05&#34; />
<!-- lx_3d.rviz 中添加了3d点云的显示 (他编写了对应的代码实现的) -->
<!-- 如果下载的是官方的cartographer代码，需要修改为 demo_3d.rviz -->
<node name=&#34;rviz&#34; pkg=&#34;rviz&#34; type=&#34;rviz&#34; required=&#34;true&#34;
args=&#34;-d $(find cartographer_ros)/configuration_files/lx_3d.rviz&#34; />
</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建图内容还有【地图保存】、【参数配置】这两部分的内容，将会在后续的文章中介绍，敬请期待 ~ |
|
发表于 2022-9-22 17:36:38