记录一下一年前跑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 documentation
https://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站，看看实际效果。
扫一扫
3521
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
