【10天速通Navigation2】(三) ：Cartographer建图算法配置：从仿真到实车，从原理到实现

前言

往期内容：
- 第一期：【10天速通Navigation2】(一) 框架总览和概念解释
- 第二期：【10天速通Navigation2】(二) ：ROS2gazebo阿克曼小车模型搭建-gazebo_ackermann_drive等插件的配置和说明
本教材将贯穿nav2的全部内容，使用ROS2和C++实现一些仿真乃至实车中常见的建图和路径规划算法，例如cartographer,ORB-SLAM,RRT,hybrid-astar。我们将注重与原理讲解和代码实现，去详细讲解每一步的配置过程和代码复现细节。
同理本教材默认大家有一些基础的ROS2和C++的编程基础，故不对一些基础部分进行详细说明。
本教程使用的环境：
- ROS2 humble
- ubuntu 22.04 LTS
本期我们将从建图算法cartographer原理入手，再从仿真到实车进行配置，达到以下的效果：
- 仿真建图
- 实车建图

1 Cartographer原理解读

1-1 介绍

Cartographer是由Google开发的一套基于图优化的激光SLAM（Simultaneous Localization and Mapping，即时定位与地图构建）算法。该算法同时支持2D和3D激光SLAM，能够跨平台使用，并支持多种传感器配置，如Lidar（激光雷达）、IMU（惯性测量单元）、Odometry（里程计）、GPS和Landmark等。
为啥不使用基于滤波方法的Gmapping？
- 当地图很大的时候，粒子更新的计算量和实时性有问题（粒子估计计算量会成倍增加）
- Gmapping没有闭环检测。
下述大部分内容节选自《机器人SLAM导航核心技术与实战》,书本关于SLAM的内容讲得很好，这里结合了本人的理解一起呈现给大家。

1-2 基于优化方法方法的SLAM系统

基于优化方法方法的SLAM系统通常采用前端建图，闭环检测，后端全局优化这样的经典框架。

这样基于优化方法，就可以实现大规模地图的构建。
- 只要有新的传感器数据输入，前端的局部建图就会实时的进行，这样就可以保证实时性
- 一旦检测到回环，后端建图就开始了，保证计算量。（后端运行的时候即使计算量很大，计算速度很慢，但是不影响前端，不影响实时性）

1-3 局部建图

在进一步了解Cartographer局部地图构建的原理之前，我们首先需要知道Cartographer的地图结构，Cartographer采用局部地图采用局部子图(submap)的方式来组织整个地图,而每个子图都是由一些系列雷达帧组成的，通过拼接局部地图，我们就获得了一张张局部地图submap。全部的局部地图拼接在一起，我们就得到了全局地图。

局部建图就是利用传感器扫描数据构建局部地图的过程。

1-3-1 局部优化

那么在构建局部地图的过程中，我们往往需要对一系列雷达扫描到的轮廓进行匹配和合并，进而构建一张完整的局部地图，这时候就不得不介绍两种匹配合并的算法：
- Scan-to-scan matching:相邻两帧之间去匹配，去更新代价地图。
- Scan-to-map matching:采用当前雷达帧和已构建出的地图进行匹配。
很明显Scan-to-scan matching使用相邻的两帧容易造成误差累积，偏差会越来越大，因此在Cartographer中选取了Scan-to-map matching，这样可以减小误差，特征也更加明显。

1-3-2 新雷达数据加入子图–概率栅格地图更新

和Gmapping类似，Cartographer的子图也采用概率栅格地图,即在栅格所划分的地图中，概率值越大，越表示该区域所存在障碍物的可能性越高。
之所以采用如此原理进行更新，主要是应对不同雷达帧在扫描到同一个栅格时候出现不同的结果。什么意思呢，也就是假如我连续两帧雷达在同一个位置扫描出的结果分别是有障碍物和无障碍物，那我要怎么办？
这里就需要进行两种情况的讨论了：
我们以上述栅格地图为例子：当前雷达 $\zeta$ 所处发射出的射线，其中的一帧雷达数据扫描结果显示 $h_3$ 位置有障碍物，这时候我们要求 $h_3$ 这个栅格以及这条射线扫描到的所有栅格进行分类讨论：

如果之前的雷达帧构建的子图中 $h_3$ 和这一帧雷达经过的栅格从来没有被扫描到，那么我们就使用雷达概率观测模型的 $P_{hit}$ 和 $P_{miss}$ 分别对这些栅格进行概率初始化：
$M_{\text{new}}(x) = \begin{cases} P_{\text{hit}}, & \text{if } \text{state}(x) = \text{hit} \\ P_{\text{miss}}, & \text{if } \text{state}(x) = \text{miss} \end{cases}$
如果之前的雷达帧构建的子图中栅格已经被初始化为 $M_{old}(x)$ ，那么这时候我们就需要使用到两次不同的数据通过下属公式对该栅格进行迭代处理：
$M_{new}(x)= \left\{ \begin{array}{ll} clamp(odd_s^{-1}(odd_s(M_{old}(x)) \cdot odds(P_{hit})), & 当state(x)=hit时 \\ clamp(odd_s^{-1}(odd_s(M_{old}(x)) \cdot odds(P_{miss})), & 当state(x)=miss时 \end{array} \right.$
其中, $odds(prob)=\frac{prob}{1-prob}$

通过上述更新机制，我们就能处理同一个位置不同雷达帧出现不同结果的问题，这常常被应用在处理环境中动态障碍物的干扰上。

1-3-3 雷达源局部优化

上述更新子图的一个重要前提是雷达的源 $\zeta$ 误差极小的情况下，而往往在真实过程中我们同样需要对这个雷达的源 $\zeta$ 进行局部优化
这其实是一个非线性最小二乘问题，在新的雷达数据加入子图之前通过运动预测出雷达位姿附近窗口内进行搜索匹配。而对于这个问题，Cartographer使用到了Ceres非线性优化工具来解决这个问题。
$\arg \min_{\zeta} \sum_{k=1}^{K} \left( 1 - M_{\text{smooth}}(T_{\zeta} h_k) \right)^2$

1-4 闭环检测

请添加图片描述

尽管我们已经通过上述优化方法对于观测源雷达的位姿进行局部优化，但是随着距离增加，误差累积，当机器人运动一圈后回到原点会发现地图无法完全闭环，出现了重影。
通过引入闭环检测，我们可以对误差的地方进行修正,最直观的原理就是判断机器人何时回到了之前走过的地方，而这通常有几种方法：
- 暴力搜索匹配：把雷达帧和地图上的所有点进行匹配（耗时，计算量大）
- 分支界定匹配：
分支界定匹配，简单说就是把地图的分辨率降低，然后依次提高分辨率进行匹配，类似于一种二分的感觉进行从大到小的匹配。分支界定匹配属于广度优先策略,暴力搜索匹配可以看成分支界定匹配处于最高分辨率情况下的特例

1-5 全局建图

闭环检测成功以后，会触发后端全局优化，并更新全局代价地图.

2 Cartographer配置和参数说明

2-1 安装

那事不宜迟,我们选择最快的方法进行cartographer安装

sudo apt install ros-humble-cartographer-ros

注意的是这里我们安装的是cartographer-ros而不是cartographer本身
可以输入下属指令判断是否安装成功

ros2 pkg list | grep cartographer

请添加图片描述

2-2 lua配置文件

Lua是一种轻量级的编程语言，被广泛应用于游戏开发、脚本编写和网络应用中。Lua文件通常以.lua为扩展名，包含了Lua语言的代码。
在cartographer建图算法中,我们将使用lua对其中的参数进行配置:
- 老规矩先上完整代码我们再来细看参数

 
include "map_builder.lua"
include "trajectory_builder.lua"
 
options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_footprint",
  published_frame = "odom",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  use_odometry = true,
  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.5,
  submap_publish_period_sec = 0.2,
  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
MAP_BUILDER.num_background_threads =2
 
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1 --积累几帧激光数据作为一个标准单位scan
TRAJECTORY_BUILDER_2D.min_range = 0.1  --激光的最近有效距离
TRAJECTORY_BUILDER_2D.max_range = 12.   --激光最远的有效距离
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5. --无效激光数据设置距离为该数值
TRAJECTORY_BUILDER_2D.use_imu_data = false  --是否使用imu数据
 
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 
--线距离搜索框，在这个框的大小内，搜索最佳scan匹配  减小该参数可以增强实时的建图效果，降低闭环优化的效果，形成闭环时，产生的重影较多
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher. angular_search_window = math.rad(10.) --角度搜索框的大小
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 20.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 2e-1  
--影响的是过程中的效果，间接会影响最后的优化时间长
 
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 30.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 30.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 20
 
 
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 40
--num_range_data设置的值与CPU有这样一种关系，值小(10)，CPU使用率比较稳定，整体偏高，值大时，CPU短暂爆发使用(插入子图的时候)，平时使用率低，呈现极大的波动状态。
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.55
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.49
 
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.05   --//尽量小点  // 如果移动距离过小, 或者时间过短, 不进行地图的更新
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.3)
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.5
 
POSE_GRAPH.optimization_problem.huber_scale = 1e2  --鲁棒核函数，去噪
 
POSE_GRAPH.optimize_every_n_nodes = 35   --后端优化节点
POSE_GRAPH.global_constraint_search_after_n_seconds = 10 
 
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 15  --优化迭代步数
POSE_GRAPH.optimization_problem.ceres_solver_options.num_threads = 1
 
POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.constraint_builder.min_score = 0.50
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.6
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 3.
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth = 5. --搜索方法，界定分支法，求解问题构成一个搜索树，depth是构造树的深度
POSE_GRAPH.global_sampling_ratio = 0.001
 


return options

2-2-1 Option

options: 这是一个表（table），包含了Cartographer算法的各种配置选项。

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

map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,

map_builder: 指定用于构建地图的类。

trajectory_builder: 指定用于构建轨迹的类。

  map_frame = "map",
  tracking_frame = "base_footprint",
  published_frame = "odom",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  use_odometry = true,

map_frame: 地图的坐标系。
tracking_frame: 跟踪的坐标系，通常是机器人底盘的坐标系。
published_frame: 发布的坐标系，通常是用于导航的坐标系。注意这里用于设置cartographer是否发布从tracking_frame到odom_frame之间的tf树

为了满足建图时候的tf坐标数结构完整

因此如果这里设置为false,你就需要额外在开一个新的tf静态坐标转换节点用于广播odom到base_footprint之间的变换

 ros2 run tf2_ros static_transform_publisher 0.0 0.0 0.0 0.0 0.0 0.0 odom base_footprint

odom_frame: 里程计坐标系。
provide_odom_frame: 是否提供里程计坐标系。
publish_frame_projected_to_2d: 是否将3D数据投影到2D进行发布。
use_pose_extrapolator: 是否使用姿态外推器。
==use_odometry: 是否使用里程计数据。==通常我们需要引入odom或者IMU用于辅助定位,否则会出现偏移和无法闭环的问题(后面我们会讲到)
use_nav_sat: 是否使用导航卫星数据。
use_landmarks: 是否使用地标数据。
num_laser_scans: 使用的激光扫描仪数量。
num_multi_echo_laser_scans: 使用多回声激光扫描仪的数量。
num_subdivisions_per_laser_scan: 每个激光扫描数据的细分数量。
num_point_clouds: 使用的点云数据数量。
lookup_transform_timeout_sec: 查找变换的超时时间。
submap_publish_period_sec: 发布子地图的周期。
pose_publish_period_sec: 发布姿态的周期。
trajectory_publish_period_sec: 发布轨迹的周期。
rangefinder_sampling_ratio: 激光采样比率。
odometry_sampling_ratio: 里程计采样比率。
fixed_frame_pose_sampling_ratio: 固定帧姿态采样比率。
imu_sampling_ratio: IMU采样比率。
landmarks_sampling_ratio: 地标采样比率。

以下是其他配置参数的说明：

MAP_BUILDER.use_trajectory_builder_2d = true
MAP_BUILDER.num_background_threads =2
 
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1 --积累几帧激光数据作为一个标准单位scan
TRAJECTORY_BUILDER_2D.min_range = 0.1  --激光的最近有效距离
TRAJECTORY_BUILDER_2D.max_range = 12.   --激光最远的有效距离
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5. --无效激光数据设置距离为该数值
TRAJECTORY_BUILDER_2D.use_imu_data = false  --是否使用imu数据
 
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 
--线距离搜索框，在这个框的大小内，搜索最佳scan匹配  减小该参数可以增强实时的建图效果，降低闭环优化的效果，形成闭环时，产生的重影较多
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher. angular_search_window = math.rad(10.) --角度搜索框的大小
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 20.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 2e-1  
--影响的是过程中的效果，间接会影响最后的优化时间长
 
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 30.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 30.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 20
 
 
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 40
--num_range_data设置的值与CPU有这样一种关系，值小(10)，CPU使用率比较稳定，整体偏高，值大时，CPU短暂爆发使用(插入子图的时候)，平时使用率低，呈现极大的波动状态。
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.55
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.49
 
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.05   --//尽量小点  // 如果移动距离过小, 或者时间过短, 不进行地图的更新
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.3)
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.5
 
POSE_GRAPH.optimization_problem.huber_scale = 1e2  --鲁棒核函数，去噪
 
POSE_GRAPH.optimize_every_n_nodes = 35   --后端优化节点
POSE_GRAPH.global_constraint_search_after_n_seconds = 10 
 
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 15  --优化迭代步数
POSE_GRAPH.optimization_problem.ceres_solver_options.num_threads = 1
 
POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.constraint_builder.min_score = 0.50
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.6
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 3.
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.branch_and_bound_depth = 5. --搜索方法，界定分支法，求解问题构成一个搜索树，depth是构造树的深度
POSE_GRAPH.global_sampling_ratio = 0.001

MAP_BUILDER.use_trajectory_builder_2d: 是否使用2D轨迹构建器。
MAP_BUILDER.num_background_threads: 地图构建器使用的后台线程数。
TRAJECTORY_BUILDER_2D 相关参数：
- num_accumulated_range_data: 累积多少帧激光数据作为一个扫描单位。
- min_range 和 max_range: 激光数据的有效距离范围。
- missing_data_ray_length: 无效激光数据的设置距离。
- use_imu_data: 是否使用IMU数据。
- use_online_correlative_scan_matching: 是否使用在线相关扫描匹配。
- real_time_correlative_scan_matcher 相关参数: 实时相关扫描匹配器的配置。
- ceres_scan_matcher 相关参数: Ceres扫描匹配器的配置。
- submaps 相关参数: 子地图的配置。
- motion_filter 相关参数: 运动过滤器的配置。
- TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window 参数定义了在实时相关扫描匹配器中，进行角度搜索的范围。这个参数的值是以弧度为单位的角度，用于确定在匹配当前激光扫描数据与之前扫描数据时，可以接受的最大角度差异。,后续当地图出现飘逸的时候需要i修改这个参数.
POSE_GRAPH 相关参数：
- optimization_problem 相关参数: 优化问题的配置。
- optimize_every_n_nodes: 每隔多少个节点进行一次优化。
- global_constraint_search_after_n_seconds: 多少秒后进行全局约束搜索。
- constraint_builder 相关参数: 约束构建器的配置

2-3 启动文件

这里直接参考小鱼的节点
cartographer_node:这是Cartographer的核心节点，负责处理传感器数据（如激光扫描和IMU数据），执行SLAM（Simultaneous Localization and Mapping，同时定位与建图）算法，并构建环境的地图。它还负责轨迹的优化和闭环检测。
cartographer_occupancy_grid_node:这个节点将Cartographer生成的概率网格转换为ROS中的nav_msgs/OccupancyGrid消息格式，这样就可以被其他导航和路径规划节点使用。这个网格通常用于机器人的路径规划和避障。

import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    # 定位到功能包的地址
    pkg_share = FindPackageShare(package='qingzhou_sim').find('qingzhou_cartographer')
    
    #=====================运行节点需要的配置=======================================================================
    # 是否使用仿真时间，我们用gazebo，这里设置成true
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    # 地图的分辨率
    resolution = LaunchConfiguration('resolution', default='0.05')
    # 地图的发布周期
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
    # 配置文件夹路径
    configuration_directory = LaunchConfiguration('configuration_directory',default= os.path.join(pkg_share, 'config') )
    # 配置文件
    configuration_basename = LaunchConfiguration('configuration_basename', default='qingzhou_2d.lua')

    
    #=====================声明三个节点，cartographer/occupancy_grid_node/rviz_node=================================
    cartographer_node = Node(
        package='cartographer_ros',
        executable='cartographer_node',
        name='cartographer_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
        arguments=['-configuration_directory', configuration_directory,
                   '-configuration_basename', configuration_basename])

    occupancy_grid_node = Node(
        package='cartographer_ros',
        executable='cartographer_occupancy_grid_node',
        name='cartographer_occupancy_grid_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
        arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec])

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        # arguments=['-d', rviz_config_dir],
        parameters=[{'use_sim_time': use_sim_time}],
        output='screen')

    #===============================================定义启动文件========================================================
    ld = LaunchDescription()
    ld.add_action(cartographer_node)
    ld.add_action(occupancy_grid_node)
    ld.add_action(rviz_node)

    return ld

3 Cartographer仿真和实车演示

那么现在要做的很简单,启动以下节点:
- gazebo仿真节点,上一期提到的
- 键盘控制节点
- cartographer建图节点

3-1 仿真实操与问题解决

那很显然,我们遇到了第一个问题
如上图所示,仿真小车在前半段的地图运行一定距离时候,出现了odom和map坐标系严重偏离的情况,这是因为雷达扫描不到边缘，周围的环境没有出现特征点,SLAM出现了估计错误.
很简单，这里是仿真，把雷达扫描的范围扩大
由于扩大的雷达的扫描距离,就不会出现估计的偏差,如下我们完成了一圈的建图,这时候就可以开始进行地图保存

3-2 map_server

map_server 是一个用于在ROS（Robot Operating System）中提供地图数据的节点。它主要用于读取、保存和发布地图数据，这些地图数据通常是由SLAM（Simultaneous Localization and Mapping）算法生成的，或者是由其他方式创建的静态地图。
我们写一个bash脚本用于地图保存

#!/bin/bash

# 获取当前的日期和时间，格式为 YYYY-MM-DD_HH-MM-SS
current_datetime=$(date '+%Y-%m-%d_%H-%M-%S')

# 构建地图文件名
map_filename="map_$current_datetime"

# 切换到相应的目录并运行map_saver_cli命令
cd src/qingzhou_cartographer/maps
ros2 run nav2_map_server map_saver_cli -t map -f "$map_filename"

运行上述命令后map_server将把我们的地图保存为如下形式:
**map.yaml文件描述了地图的属性和相关的PGM文件

image: map_2024-10-23_23-10-11.pgm
mode: trinary
resolution: 0.05
origin: [-1.97, -3.77, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

image: 地图的PGM文件名，这个文件包含了地图的像素数据。
mode: 地图的模式，trinary表示地图使用三种值：0（空闲），100（占用）和-1（未知）。
resolution: 地图中每个单元格代表的实际距离，这里是0.05米。
origin: 地图原点在真实世界坐标系中的位置，这里是[-1.97, -3.77, 0]，表示地图左下角的位置。
negate: 一个标志，用来指示是否反转地图的占用值，0表示不反转。
occupied_thresh: 占用阈值，用于确定一个单元格是否被占用，这里是0.65，意味着如果PGM文件中单元格的值大于或等于65，则该单元格被认为是占用的。
free_thresh: 空闲阈值，用于确定一个单元格是否空闲，这里是0.25，意味着如果PGM文件中单元格的值小于或等于25，则该单元格被认为是空闲的。
同时我们有的得到一张保存的pgm图片
这时候我们仿真就完成了cartographer的配置.

3-2 实车建图问题与解决

这里我拿另一个项目实车所遇到的问题和解决方案一同和大家分享

3-2-1 实车建图飘逸问题

如下图实车的雷达更新频率只有10hz,单纯使用雷达进行匹配容易造成地图出现飘逸
而往往实车中是有轮式里程计的存在的,这时候我们只需要打开lua配置中的使用里程计
这时候驱动小车移动一圈回到原点后发现地图没有出现大幅度漂移但是仍有模糊现象,这时候我们需要
把最小角度改小!!!
这样地图基本上没问题了.

3-2-2 实车建图里程计报错问题

Carto建图报错找不到Odom
可能原因一：Odom话题非发布一查话题确实掉了
可能原因二：tf树断开
本地下载tf树

sudo apt-get install ros-humble-rqt-tf-tree

运行

 ros2 run rqt_tf_tree rqt_tf_tree --force-discover

发现tf树确实断开,缺少

请添加图片描述

完整的tf树应该这样,这里有两种方法发布base_footprint–>odom

如果provide_odom_frame这个参数打开，会自动帮你进行odom到base_footprint的转换，

把他关掉的话就需要自己额外进行静态坐标变换推荐关掉自己发布

ros2 run tf2_ros static_transform_publisher 0.0 0.0 0.0 0.0 0.0 0.0 odom base_footprint

4 小节

本节我们介绍了cartographer的原理,并分别从仿真和实车进行配置和问题分析.
如有错误,欢迎指出!!!
下一期我们讲讲如何在仿真中配置libgazebo_ros_camera深度相机插件,并使用ORB-SLAM进行建图
感谢大家的支持!!!

【10天速通Navigation2】(三) ：Cartographer建图算法配置：从仿真到实车，从原理到实现

前言

1 Cartographer原理解读

1-1 介绍

1-2 基于优化方法方法的SLAM系统

1-3 局部建图

1-3-1 局部优化

1-3-2 新雷达数据加入子图–概率栅格地图更新

1-3-3 雷达源局部优化

1-4 闭环检测

1-5 全局建图

2 Cartographer配置和参数说明

2-1 安装

2-2 lua配置文件

2-2-1 Option

2-3 启动文件

3 Cartographer仿真和实车演示

3-1 仿真实操与问题解决

3-2 map_server

3-2 实车建图问题与解决

3-2-1 实车建图飘逸问题

3-2-2 实车建图里程计报错问题

4 小节

相关文章