1. Cartographer 是什么Cartographer 是 Google 开源的实时 SLAM 系统支持 2D 和 3D同一套系统可以适配不同传感器配置例如 2D 激光、3D 点云、IMU、轮速里程计等。它的核心思想是前端构建局部一致的子地图后端通过位姿图优化把这些子地图和轨迹节点约束到全局一致的位置上。官方文档也把 Cartographer 抽象成两个相关子系统Local SLAM 负责生成一系列局部一致的 submapGlobal SLAM 在后台寻找约束和回环并把这些 submap 更一致地连接起来。Cartographer 的基本数据流可以理解为传感器数据 ↓ 数据同步与预处理 ↓ PoseExtrapolator 位姿预测 ↓ 点云滤波 ↓ Scan Matching 当前 scan 匹配 active submap ↓ 插入 submap ↓ 生成 trajectory node ↓ PoseGraph 添加 node-submap 约束 ↓ 后台回环检测 ↓ 全局优化 ↓ 输出全局一致地图和轨迹这里面有两个关键词最重要submap局部子地图是 Cartographer 地图构建的基本单元。 pose graph位姿图是 Cartographer 后端优化的核心结构。Cartographer 不是直接维护一张从头到尾不断更新的巨大地图而是不断生成很多小的局部地图。每个小地图局部比较准确但是不同小地图之间可能存在累计误差所以需要后端把它们整体优化。2. Cartographer 的整体框架Cartographer 可以拆成两个主模块Cartographer ├── Local SLAM / LocalTrajectoryBuilder │ ├── 接收 range data │ ├── 传感器时间同步 │ ├── PoseExtrapolator 位姿预测 │ ├── 重力对齐 │ ├── 点云滤波 │ ├── scan-to-submap 匹配 │ ├── active submap 插入 │ └── 生成 trajectory node │ └── Global SLAM / PoseGraph ├── 保存 trajectory node ├── 保存 submap node ├── 建立 node-submap 约束 ├── 后台回环检测 ├── 添加 loop closure constraint └── 全局 sparse pose adjustment 优化官方调参文档中明确说Local SLAM 的任务是生成好的 submaps而 Global SLAM 的任务是把这些 submaps 更一致地连接起来Global SLAM 本质上是 pose graph optimization通过节点和子地图之间的 constraints 进行优化。3. Cartographer 输入输出Cartographer 的输入通常包括LaserScan / PointCloud2激光或点云数据是主要建图数据。 IMU提供角速度、线加速度、重力方向等信息。 Odometry提供轮速里程计或外部里程计预测。 TF提供传感器坐标系到机器人坐标系之间的外参关系。在 ROS 里Cartographer 算法库本身不直接等同于 ROS 节点cartographer_ros负责把 ROS 的 topic、tf、参数、服务封装到 Cartographer 算法接口里。Cartographer ROS 官方仓库说明它是 Cartographer 的 ROS 集成工程。常见输出包括/map全局栅格地图。 /submap_list当前所有子地图的信息。 /trajectory_node_list轨迹节点信息。 /scan_matched_points2匹配后的点云。 TF通常发布 map、odom、base_link 等坐标系之间的关系。4. Local SLAM前端到底做什么Local SLAM 是 Cartographer 前端。它的目标不是一次性得到全局最优地图而是稳定地产生局部一致的子地图和轨迹节点。核心过程是1. 收到一帧或多帧 range data 2. 通过 PoseExtrapolator 预测当前 scan 的初始位姿 3. 对点云做距离过滤、重力对齐、体素滤波 4. 将当前 scan 和当前 active submap 做 scan matching 5. 得到 scan 在 local map frame 下的位姿 6. 如果运动量足够大就把 scan 插入 active submap 7. 生成 trajectory node交给后端 PoseGraph。官方文档说明Local SLAM 会把组装并滤波后的 scan 插入当前 submapscan matching 使用 PoseExtrapolator 给出的初始猜测。这个初始猜测会利用除 range finder 以外的传感器数据来预测当前 scan 应该插入到 submap 的位置。5. AddRangeData()前端主入口Cartographer 2D 前端的一个核心入口是LocalTrajectoryBuilder2D::AddRangeData()。这个函数接收一批还没有完全同步的 range data然后进行同步、时间处理、位姿外推、距离过滤、点云累积等操作。源码中可以看到它先通过range_data_collator_做数据整理然后使用extrapolator_对每个点时间做位姿外推接着根据min_range、max_range把点分成 returns 和 misses。下面是接近源码逻辑的简化版每一行都加了注释std::unique_ptrMatchingResult AddRangeData( // 定义前端接收 range data 的函数返回匹配结果 const std::string sensor_id, // sensor_id 表示当前数据来自哪个传感器 const sensor::TimedPointCloudData unsynchronized_data) { // unsynchronized_data 是还未完全同步整理的带时间点云数据 auto synchronized_data // 创建同步后的数据变量 range_data_collator_.AddRangeData(sensor_id, // 把当前传感器 ID 传给数据整理器 unsynchronized_data); // 把未同步的点云数据传给数据整理器 if (synchronized_data.ranges.empty()) { // 如果同步后没有有效 range 点 return nullptr; // 直接返回空表示这一帧暂时不处理 } // 结束空数据判断 const common::Time time synchronized_data.time; // 取出当前同步数据的时间戳 if (!options_.use_imu_data()) { // 如果配置中不使用 IMU 数据 InitializeExtrapolator(time); // 就用当前时间初始化位姿外推器 } // 结束 IMU 配置判断 if (extrapolator_ nullptr) { // 如果位姿外推器还没有初始化 return nullptr; // 当前帧无法预测位姿直接返回空 } // 结束外推器判断 std::vectortransform::Rigid3f range_data_poses; // 创建数组保存每个点对应时刻的预测位姿 range_data_poses.reserve(synchronized_data.ranges.size()); // 提前分配空间避免循环中频繁扩容 for (const auto range : synchronized_data.ranges) { // 遍历同步数据中的每个 range 点 common::Time time_point // 计算当前点的真实时间 time common::FromSeconds(range.point_time.time); // 当前帧时间加上点自身的相对时间 range_data_poses.push_back( // 把当前点对应的预测位姿加入数组 extrapolator_-ExtrapolatePose(time_point).castfloat()); // 使用 PoseExtrapolator 外推当前点时刻的位姿 } // 结束逐点位姿外推 for (size_t i 0; i synchronized_data.ranges.size(); i) { // 再次遍历每个 range 点 const auto hit synchronized_data.ranges[i].point_time; // 取出当前点的测量信息 Eigen::Vector3f origin_in_local // 计算当前点所属激光原点在 local 坐标系下的位置 range_data_poses[i] * // 使用该点时刻的位姿 synchronized_data.origins.at( // 取出该点对应的传感器原点 synchronized_data.ranges[i].origin_index); // origin_index 表示当前点对应哪个原点 sensor::RangefinderPoint hit_in_local // 创建当前击中点在 local 坐标系下的位置 range_data_poses[i] * sensor::ToRangefinderPoint(hit); // 把点从传感器坐标系变换到 local 坐标系 Eigen::Vector3f delta hit_in_local.position - origin_in_local; // 计算从激光原点到击中点的向量 float range delta.norm(); // 计算当前点的距离长度 if (range options_.min_range()) { // 如果距离大于最小有效距离 if (range options_.max_range()) { // 如果距离小于最大有效距离 accumulated_range_data_.returns.push_back(hit_in_local); // 把该点作为有效 hit 点保存 } else { // 如果距离超过最大有效距离 hit_in_local.position // 修改该点位置 origin_in_local // 从激光原点出发 options_.missing_data_ray_length() / range * delta; // 截断到 missing_data_ray_length 长度处 accumulated_range_data_.misses.push_back(hit_in_local); // 把该点作为 miss 点保存 } // 结束最大距离判断 } // 结束最小距离判断 } // 结束点遍历 num_accumulated_; // 累积的 range data 数量加一 if (num_accumulated_ options_.num_accumulated_range_data()) { // 如果累积帧数达到配置要求 return AddAccumulatedRangeData(time, // 调用下一阶段处理累积后的 range data accumulated_range_data_, // 传入累积后的 hit/miss 数据 gravity_alignment, // 传入重力对齐姿态 sensor_duration); // 传入传感器时间间隔 } // 结束累积数量判断 return nullptr; // 如果没有达到累积数量就暂时不输出匹配结果 } // 函数结束这里有几个关键变量synchronized_data同步整理后的 range data。 range_data_poses每个点对应时刻的预测位姿。 accumulated_range_data_.returns有效击中点后面用于地图占据更新。 accumulated_range_data_.misses超过最大距离或未击中的点后面用于空闲区域更新。 num_accumulated_当前已经累积了多少帧 range data。 num_accumulated_range_data配置参数控制多少帧 range data 合成一个 scan。6. 位姿外推 PoseExtrapolatorCartographer 在 scan matching 前需要一个初始位姿。这个初始位姿由PoseExtrapolator生成。它会结合历史 scan matching 位姿、IMU 数据、里程计数据等信息预测当前 scan 或当前点在 local 坐标系下的大致位姿。源码中典型调用是extrapolator_-ExtrapolatePose(time_point)每一行解释版如下common::Time time_point // 定义当前点的时间 time common::FromSeconds(range.point_time.time); // 当前 scan 时间加上点的相对时间 transform::Rigid3d pose_at_point_time // 定义当前点时刻的预测位姿 extrapolator_-ExtrapolatePose(time_point); // 使用位姿外推器预测当前时间的机器人位姿 range_data_poses.push_back( // 把预测位姿保存起来 pose_at_point_time.castfloat()); // 转成 float 类型供后续点云变换使用这里要注意一个点Cartographer 不是只对整帧 scan 用一个时间它会考虑 range data 中点的时间信息。官方文档也说明距离测量是在一段时间内完成的而 ROS 消息通常是批量发送的Cartographer 会根据每条消息时间处理机器人运动造成的扫描形变更多的 range data 有助于组装更一致的 scan。7. 点云距离过滤returns 和 missesCartographer 把 range data 分成两类returns有效击中点表示激光打到了障碍物或结构表面。 misses未击中或超过最大距离的点表示射线方向上一定范围内可以认为是空闲。这一点对概率栅格地图非常重要因为地图更新时不只更新障碍物也更新空闲空间。核心逻辑是Eigen::Vector3f delta hit_in_local.position - origin_in_local; // 计算激光原点到当前点的向量 float range delta.norm(); // 计算当前激光点距离 if (range options_.min_range()) { // 小于最小距离的点通常视为无效噪声 if (range options_.max_range()) { // 在最大距离内说明是可靠击中点 accumulated_range_data_.returns.push_back(hit_in_local); // 放入 returns用于提高占据概率 } else { // 超过最大距离说明不作为真实障碍物 hit_in_local.position // 修改点的位置 origin_in_local // 从激光原点出发 options_.missing_data_ray_length() / range * delta; // 按比例截断到指定长度 accumulated_range_data_.misses.push_back(hit_in_local); // 放入 misses用于更新空闲区域 } // 最大距离判断结束 } // 最小距离判断结束变量解释origin_in_local激光雷达原点在 local 坐标系下的位置。 hit_in_local激光点在 local 坐标系下的位置。 delta从雷达原点指向击中点的向量。 range当前点的距离。 min_range最小有效距离小于它的点通常被认为太近或不可靠。 max_range最大有效距离大于它的点通常不作为真实 hit。 missing_data_ray_length超过 max_range 时用这个长度表示空闲射线。8. AddAccumulatedRangeData()累积点云进入匹配流程当累积的 range data 数量达到num_accumulated_range_data后就会进入AddAccumulatedRangeData()。源码中可以看到它会先计算重力对齐后的位姿预测然后对点云做自适应体素滤波再调用ScanMatch()成功后把点云变换到 local 坐标系并插入 submap。下面是每一行注释版std::unique_ptrMatchingResult AddAccumulatedRangeData( // 定义处理累积 range data 的函数 const common::Time time, // 当前 scan 的时间 const sensor::RangeData gravity_aligned_range_data, // 已经重力对齐后的点云数据 const transform::Rigid3d gravity_alignment, // 重力对齐旋转 const absl::optionalcommon::Duration sensor_duration) { // 当前传感器数据的时间间隔 if (gravity_aligned_range_data.returns.empty()) { // 如果没有有效 hit 点 return nullptr; // 直接返回空表示不生成匹配结果 } // 空点云判断结束 const transform::Rigid3d non_gravity_aligned_pose_prediction // 定义未重力对齐的预测位姿 extrapolator_-ExtrapolatePose(time); // 使用位姿外推器预测当前 scan 位姿 const transform::Rigid2d pose_prediction // 定义 2D 平面上的预测位姿 transform::Project2D( // 将 3D 位姿投影成 2D 位姿 non_gravity_aligned_pose_prediction * // 当前预测位姿 gravity_alignment.inverse()); // 乘以重力对齐的逆变换得到水平平面内的位姿 const sensor::PointCloud filtered_gravity_aligned_point_cloud // 定义滤波后的重力对齐点云 sensor::AdaptiveVoxelFilter( // 使用自适应体素滤波 gravity_aligned_range_data.returns, // 输入有效 hit 点 options_.adaptive_voxel_filter_options()); // 输入自适应体素滤波参数 if (filtered_gravity_aligned_point_cloud.empty()) { // 如果滤波后点云为空 return nullptr; // 直接返回空 } // 滤波后空点云判断结束 std::unique_ptrtransform::Rigid2d pose_estimate_2d // 定义 scan matching 后的 2D 位姿 ScanMatch(time, // 传入当前时间 pose_prediction, // 传入预测初值 filtered_gravity_aligned_point_cloud); // 传入滤波后的点云 if (pose_estimate_2d nullptr) { // 如果 scan matching 失败 return nullptr; // 直接返回空 } // 匹配失败判断结束 const transform::Rigid3d pose_estimate // 定义最终 3D 位姿估计 transform::Embed3D(*pose_estimate_2d) * gravity_alignment; // 将 2D 位姿嵌入 3D并恢复重力对齐旋转 extrapolator_-AddPose(time, pose_estimate); // 把当前估计位姿加入外推器更新运动状态 sensor::RangeData range_data_in_local // 定义 local 坐标系下的 range data TransformRangeData( // 对 range data 做坐标变换 gravity_aligned_range_data, // 输入重力对齐点云 transform::Embed3D(pose_estimate_2d-castfloat())); // 使用匹配后的 2D 位姿转换到 local 坐标系 std::unique_ptrInsertionResult insertion_result // 定义插入 submap 的结果 InsertIntoSubmap(time, // 传入当前时间 range_data_in_local, // 传入 local 坐标系下的 range data filtered_gravity_aligned_point_cloud, // 传入滤波点云 pose_estimate, // 传入当前估计位姿 gravity_alignment.rotation()); // 传入重力对齐旋转 return absl::make_uniqueMatchingResult( // 创建并返回前端匹配结果 MatchingResult{time, // 保存时间 pose_estimate, // 保存估计位姿 std::move(range_data_in_local), // 保存 local 坐标系下 range data std::move(insertion_result)}); // 保存插入 submap 的结果 } // 函数结束这里最核心的是三步1. pose_prediction先预测当前 scan 大概在哪里。 2. ScanMatch()再用 scan matching 把位姿优化到更准确的位置。 3. InsertIntoSubmap()最后把当前 scan 插入 active submap。9. 点云滤波VoxelFilter 和 AdaptiveVoxelFilterCartographer 会对点云做滤波主要目的有两个1. 降低点数减少 scan matching 计算量 2. 保持点云空间分布更加均匀避免近处密集点过度主导优化。官方文档说明Cartographer 先应用固定大小体素滤波再使用自适应体素滤波自适应体素滤波会在最大体素边长限制下尽量达到目标点数。关键代码const sensor::PointCloud filtered_gravity_aligned_point_cloud // 定义滤波后的点云变量 sensor::AdaptiveVoxelFilter( // 调用自适应体素滤波函数 gravity_aligned_range_data.returns, // 输入有效击中点 options_.adaptive_voxel_filter_options()); // 输入滤波参数包括 max_length、min_num_points 等变量解释gravity_aligned_range_data.returns重力对齐后的有效点云。 adaptive_voxel_filter_options自适应体素滤波参数。 max_length最大体素边长。 min_num_points希望滤波后至少保留的点数。 filtered_gravity_aligned_point_cloud最终用于 scan matching 的点云。10. ScanMatch()当前 scan 和 active submap 匹配ScanMatch()是 Local SLAM 的核心。它输入预测位姿和滤波点云输出 scan matching 后的位姿估计。源码中可以看到它优先取 active submaps 中的第一个 submap 作为匹配对象如果启用在线相关匹配会先做一次粗匹配然后再调用 CeresScanMatcher 做精匹配。每一行注释版std::unique_ptrtransform::Rigid2d ScanMatch( // 定义 scan matching 函数返回 2D 位姿 const common::Time time, // 当前 scan 的时间 const transform::Rigid2d pose_prediction, // PoseExtrapolator 给出的预测位姿 const sensor::PointCloud filtered_point_cloud) { // 滤波后的点云 if (active_submaps_.submaps().empty()) { // 如果当前还没有 active submap return absl::make_uniquetransform::Rigid2d(pose_prediction); // 直接返回预测位姿作为估计结果 } // active submap 判断结束 std::shared_ptrconst Submap2D matching_submap // 定义用于匹配的子地图 active_submaps_.submaps().front(); // 取 active submaps 中的第一个 submap transform::Rigid2d initial_ceres_pose pose_prediction; // 初始化 Ceres 优化初值为预测位姿 if (options_.use_online_correlative_scan_matching()) { // 如果开启在线相关匹配 const double score // 定义粗匹配得分 real_time_correlative_scan_matcher_.Match( // 调用实时相关匹配器 pose_prediction, // 输入预测位姿 filtered_point_cloud, // 输入当前 scan 点云 *matching_submap-grid(), // 输入当前 submap 的概率栅格 initial_ceres_pose); // 输出更好的 Ceres 初值 kRealTimeCorrelativeScanMatcherScoreMetric-Observe(score); // 记录相关匹配得分指标 } // 在线相关匹配结束 auto pose_observation // 定义 Ceres 优化后的位姿观测 absl::make_uniquetransform::Rigid2d(); // 创建一个空的 2D 位姿对象 ceres::Solver::Summary summary; // 定义 Ceres 求解摘要用于记录优化信息 ceres_scan_matcher_.Match( // 调用 CeresScanMatcher 做精匹配 pose_prediction.translation(), // 输入预测平移用作平移先验 initial_ceres_pose, // 输入 Ceres 初始位姿 filtered_point_cloud, // 输入当前点云 *matching_submap-grid(), // 输入 submap 概率栅格 pose_observation.get(), // 输出优化后的位姿 summary); // 输出优化摘要 return pose_observation; // 返回 scan matching 后的位姿 } // 函数结束这里的关键变量pose_prediction外推器预测的初值。 matching_submap当前 scan 要匹配的 active submap。 initial_ceres_poseCeres 优化初值可能经过相关匹配修正。 pose_observation最终 scan matching 得到的位姿。 filtered_point_cloud用于匹配的滤波点云。 matching_submap-grid()当前 submap 的概率栅格地图。11. CeresScanMatcher非线性最小二乘精匹配CeresScanMatcher 的目标是调整当前 scan 的位姿让点云落到 submap 中占据概率更高的位置同时不要偏离预测初值太远。源码中CeresScanMatcher2D::Match()会添加三类残差1. occupied space residual点云和概率栅格的匹配残差 2. translation residual平移不要偏离预测值太多 3. rotation residual旋转不要偏离初始值太多。源码中还可以看到occupied_space_weight、translation_weight、rotation_weight都从配置中读取Ceres 求解器使用DENSE_QR。每一行注释版void CeresScanMatcher2D::Match( // 定义 Ceres scan matching 函数 const Eigen::Vector2d target_translation, // 预测位姿的平移部分用作平移先验 const transform::Rigid2d initial_pose_estimate, // 输入初始位姿估计 const sensor::PointCloud point_cloud, // 输入当前 scan 点云 const Grid2D grid, // 输入 submap 的概率栅格 transform::Rigid2d* const pose_estimate, // 输出优化后的位姿 ceres::Solver::Summary* const summary) const { // 输出 Ceres 求解摘要 double ceres_pose_estimate[3] { // 定义 Ceres 优化变量数组 initial_pose_estimate.translation().x(), // 第 0 维是 x 平移初值 initial_pose_estimate.translation().y(), // 第 1 维是 y 平移初值 initial_pose_estimate.rotation().angle()}; // 第 2 维是旋转角初值 ceres::Problem problem; // 创建 Ceres 优化问题 problem.AddResidualBlock( // 添加占据空间残差块 CreateOccupiedSpaceCostFunction2D( // 创建概率栅格匹配代价函数 options_.occupied_space_weight() / // 使用 occupied_space_weight 作为权重 std::sqrt(static_castdouble(point_cloud.size())), // 按点数开方归一化避免点数影响权重尺度 point_cloud, // 输入当前 scan 点云 grid), // 输入 submap 概率栅格 nullptr, // 不使用鲁棒核函数 ceres_pose_estimate); // 优化变量是 x、y、theta problem.AddResidualBlock( // 添加平移先验残差块 TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction( // 创建平移差代价函数 options_.translation_weight(), // 输入平移权重 target_translation), // 输入预测平移 nullptr, // 不使用鲁棒核函数 ceres_pose_estimate); // 优化变量仍然是 x、y、theta problem.AddResidualBlock( // 添加旋转先验残差块 RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction( // 创建旋转差代价函数 options_.rotation_weight(), // 输入旋转权重 ceres_pose_estimate[2]), // 输入旋转初值 nullptr, // 不使用鲁棒核函数 ceres_pose_estimate); // 优化变量仍然是 x、y、theta ceres::Solve(ceres_solver_options_, problem, summary); // 调用 Ceres 求解最小二乘问题 *pose_estimate transform::Rigid2d( // 将优化后的数组重新封装成 Rigid2d {ceres_pose_estimate[0], ceres_pose_estimate[1]}, // 设置优化后的 x、y 平移 ceres_pose_estimate[2]); // 设置优化后的旋转角 theta } // 函数结束12. Scan Matching 的核心公式Cartographer 2D 中一个 scan 的位姿可以表示为ξ (ξx, ξy, ξθ)其中ξxscan 在 submap 坐标系下的 x 方向平移。 ξyscan 在 submap 坐标系下的 y 方向平移。 ξθscan 在 submap 坐标系下的旋转角。scan 点从自身坐标系变换到 submap 坐标系的公式是Tξ p Rξ p tξ展开就是Rξ [ cosξθ -sinξθ sinξθ cosξθ ] tξ [ ξx ξy ]变量解释p当前 scan 中的一个点。 Rξ由 ξθ 构成的二维旋转矩阵。 tξ二维平移向量。 Tξp点 p 经过位姿 ξ 变换后在 submap 坐标系下的位置。论文中给出的 Ceres scan matching 目标函数是让变换后的 scan 点尽量落在 submap 高占据概率位置上可以写成argmin_ξ Σ_k (1 - M_smooth(Tξ h_k))²变量解释h_k当前 scan 中第 k 个激光点。 Tξ h_k第 k 个点通过位姿 ξ 变换到 submap 坐标系后的坐标。 M_smoothsubmap 概率栅格的平滑插值函数。 M_smooth(Tξ h_k)变换后的点落在 submap 上对应位置的占据概率。 1 - M_smooth(Tξ h_k)点没有落在高占据概率区域时产生的残差。直观理解就是如果位姿估计正确激光点应该落在地图里“更像障碍物”的地方也就是占据概率高的位置如果落在空闲区域残差就会变大。论文中明确把 scan matching 表述为非线性最小二乘问题并通过 Ceres 优化 scan pose。13. SubmapCartographer 的局部地图单元Submap 是 Cartographer 的核心地图单元。论文中将 submap 表示为概率栅格M : rZ × rZ → [pmin, pmax]变量解释M子地图概率栅格。 r地图分辨率例如 0.05m。 Z整数网格坐标。 rZ × rZ以分辨率 r 离散化后的二维栅格坐标空间。 [pmin, pmax]每个栅格占据概率的取值范围。简单说submap 里面每个格子都存一个概率概率高这个格子更可能是障碍物。 概率低这个格子更可能是空闲区域。 未知这个格子还没有被可靠观测。论文中说明若干连续 scan 会构建一个 submapscan 插入 submap 时会计算 hit set 和 miss sethit 对应激光击中的格子miss 对应激光射线路径穿过但没有击中的格子。14. 概率栅格更新公式Cartographer 使用 odds 形式更新概率。odds 定义为odds(p) p / (1 - p)如果某个格子被激光击中即 hit那么更新形式可以写成M_new(x) clamp( odds⁻¹( odds(M_old(x)) × odds(p_hit) ) )miss 的更新类似只是把p_hit换成p_miss。变量解释x地图中的某个栅格。 M_old(x)该栅格更新前的占据概率。 M_new(x)该栅格更新后的占据概率。 p_hit激光击中时使用的概率更新参数。 p_miss激光穿过但未击中时使用的概率更新参数。 odds(p)把概率转成 odds 形式。 odds⁻¹把 odds 转回概率。 clamp把概率限制在 [pmin, pmax] 范围内。直观理解激光终点所在格子占据概率上升。 激光路径经过的格子占据概率下降。 没有观测到的格子保持原状。论文中给出了 odds 更新公式并说明 hit 和 miss 使用类似的更新方式。15. InsertIntoSubmap()把当前 scan 插入 active submap当前 scan 匹配成功后还不一定会插入 submap。Cartographer 会先用motion_filter_判断当前位姿和上一次插入节点是否太接近。如果变化太小就跳过避免图优化节点过多。源码中InsertIntoSubmap()先调用motion_filter_.IsSimilar()如果不相似才调用active_submaps_.InsertRangeData()插入数据并生成TrajectoryNode::Data。每一行注释版std::unique_ptrInsertionResult InsertIntoSubmap( // 定义插入 submap 的函数 const common::Time time, // 当前 scan 时间 const sensor::RangeData range_data_in_local, // 当前 scan 在 local 坐标系下的 range data const sensor::PointCloud filtered_point_cloud, // 滤波后的点云 const transform::Rigid3d pose_estimate, // scan matching 后的位姿估计 const Eigen::Quaterniond gravity_alignment) { // 重力对齐旋转 if (motion_filter_.IsSimilar(time, pose_estimate)) { // 如果当前帧和上一关键帧时间/距离/角度都很接近 return nullptr; // 不插入 submap也不生成新的 trajectory node } // 运动滤波判断结束 std::vectorstd::shared_ptrconst Submap2D insertion_submaps // 定义当前 scan 插入的 submap 列表 active_submaps_.InsertRangeData(range_data_in_local); // 将当前 range data 插入 active submaps return absl::make_uniqueInsertionResult( // 创建插入结果对象 InsertionResult{ // 构造 InsertionResult std::make_sharedconst TrajectoryNode::Data( // 创建 trajectory node 数据 TrajectoryNode::Data{ // 构造节点数据结构 time, // 保存节点时间 gravity_alignment, // 保存重力对齐旋转 filtered_point_cloud, // 保存滤波后的点云 {}, // 2D 中不使用 high_resolution_point_cloud {}, // 2D 中不使用 low_resolution_point_cloud {}, // 2D 中不使用 rotational_scan_matcher_histogram pose_estimate}), // 保存当前 scan matching 得到的位姿 std::move(insertion_submaps)}); // 保存当前 scan 插入的 submaps } // 函数结束变量解释motion_filter_运动滤波器用于减少冗余节点。 active_submaps_当前正在构建的子地图管理器。 range_data_in_local已经根据匹配位姿变换到 local 坐标系的点云数据。 insertion_submaps当前 scan 被插入的 submap 列表。 TrajectoryNode::Data后端 PoseGraph 中一个轨迹节点的数据。16. Motion Filter为什么不是每一帧都插入Motion Filter 的作用是控制节点数量。它一般根据三个量判断当前帧是否和上一插入帧过于接近时间差是否足够大。 平移差是否足够大。 旋转差是否足够大。如果机器人几乎没动但是激光还在高频输入Cartographer 没必要把每一帧都加入图优化。否则后端会产生大量重复节点优化变慢内存也会增加。可以把它理解为变化很小的 scan只参与短期前端处理不生成图优化节点。 变化足够大的 scan插入 submap并生成 trajectory node。这样能控制 PoseGraph 规模让系统保持实时性。17. Trajectory Node后端图优化的轨迹节点当 scan 被成功插入 submap 后Cartographer 会生成一个 trajectory node。这个节点保存time节点时间戳。 gravity_alignment重力对齐姿态。 filtered_point_cloud滤波后的点云。 pose_estimate当前 scan matching 后的位姿。 insertion_submaps当前节点插入过哪些 submap。这些信息会进入 PoseGraph。后端会根据 node 和 submap 之间的相对关系建立约束。可以这样理解Local SLAM 输出 当前 scan 的局部位姿 当前 scan 的滤波点云 当前 scan 插入的 submaps PoseGraph 接收 trajectory node submap node node-submap constraint18. Global SLAM后端 PoseGraph 做什么Global SLAM 在后台运行它的任务是全局一致性优化。Local SLAM 会不断产生 submap 和 trajectory node但由于每次 scan matching 只依赖当前局部 submap时间久了会出现累计误差。因此后端需要把所有节点和子地图统一放进一个图里优化。官方文档说明Global SLAM 是一种 GraphSLAM本质是通过 nodes 和 submaps 之间的 constraints 构建 pose graph然后优化整个约束图。文档还把 constraints 形象地描述成连接节点的“绳子”局部约束保持局部结构一致全局约束把回环位置拉回一致。PoseGraph 里面主要有Submap node子地图节点。 Trajectory node轨迹节点。 Intra-submap constraint当前 scan 插入当前 submap 产生的局部约束。 Loop closure constraint历史 scan 和历史 submap 匹配成功后产生的回环约束。 Optimization problem根据所有约束优化节点和子地图的全局位姿。19. PoseGraph 优化公式论文中的全局优化可以写成argmin_{Ξm, Ξs} 1/2 Σ_ij ρ( E²(ξ_i^m, ξ_j^s; Σ_ij, ξ_ij) )变量解释Ξm所有 submap 位姿的集合。 Ξs所有 scan 节点位姿的集合。 ξ_i^m第 i 个 submap 的全局位姿。 ξ_j^s第 j 个 scan 节点的全局位姿。 ξ_ijscan j 在 submap i 中匹配得到的相对位姿约束。 Σ_ij该约束的不确定性或协方差信息。 E当前全局位姿下该约束产生的误差。 ρ鲁棒核函数用于减小异常约束的影响。这个优化问题的意义是调整所有 submap 和 scan 节点的全局位姿 让所有局部约束和回环约束尽可能同时满足。论文中说明回环优化同样被建模为非线性最小二乘问题优化变量包括 scan 位姿和 submap 位姿约束来自 scan 和 submap 的相对匹配结果。20. 回环检测scan-to-submap 约束搜索Cartographer 的回环检测不是只看当前帧和当前 submap而是在后台把历史节点和已经完成的 submap 做匹配。如果匹配得分足够高就添加一个新的约束到 PoseGraph。流程可以理解为某个 submap 完成 ↓ 不再继续插入新 scan ↓ 后台线程拿历史 trajectory node 与该 submap 尝试匹配 ↓ 如果匹配得分足够高 ↓ 生成 loop closure constraint ↓ 加入 PoseGraph ↓ 下一次全局优化时修正轨迹和地图论文中提到当 submap 完成后它会参与 loop closure 的 scan matching如果在搜索窗口中找到足够好的匹配就把这个匹配作为 loop closing constraint 加入优化问题。为了实时完成回环匹配Cartographer 使用 branch-and-bound 方法和每个完成 submap 的多层预计算栅格。21. Branch-and-Bound为什么能实时回环回环检测需要在比较大的搜索窗口里找 scan 和 submap 的匹配位姿。如果暴力枚举所有x、y、θ候选计算量会很大。Branch-and-Bound 的思想是Branch把搜索空间逐层拆分成更小区域。 Bound估计某个区域理论上能达到的最高匹配得分。 Prune如果这个最高得分都不可能超过当前最优解就直接剪掉这个区域。论文中 branch-and-bound scan matching 的目标可以写成ξ* argmax_{ξ ∈ W} Σ_k M_nearest(Tξ h_k)变量解释ξ*搜索得到的最佳匹配位姿。 W搜索窗口。 h_k当前 scan 中第 k 个点。 Tξ h_k点 h_k 经过候选位姿 ξ 变换后的坐标。 M_nearest把坐标映射到最近栅格后的概率值。 Σ_k M_nearest(Tξ h_k)当前候选位姿的匹配得分。这个目标和前面的 Ceres 精匹配不同。这里更像是粗搜索通过遍历候选位姿计算匹配得分找到一个足够好的候选再进一步精化。论文明确给出了该匹配目标并说明可以通过后续 Ceres scan matching 改善匹配质量。22. 参数理解下面这些参数对 Cartographer 效果影响很大。TRAJECTORY_BUILDER_2D.min_range 0.3 -- 小于 0.3m 的激光点被过滤避免近距离噪声或机器人自身结构影响 TRAJECTORY_BUILDER_2D.max_range 30.0 -- 大于 30m 的激光点不作为有效 hit 点用于控制可靠测距范围 TRAJECTORY_BUILDER_2D.missing_data_ray_length 5.0 -- 超过 max_range 的射线按 5m 长度处理用于更新空闲区域 TRAJECTORY_BUILDER_2D.num_accumulated_range_data 1 -- 累积多少个 range data 后组成一个 scan 进入前端匹配 TRAJECTORY_BUILDER_2D.voxel_filter_size 0.025 -- 固定体素滤波尺寸尺寸越小点云越密计算越慢 TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching false -- 是否启用前端实时相关匹配开启后更鲁棒但更耗时 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight 1.0 -- 栅格占据匹配残差权重越大越相信地图匹配 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight 10.0 -- 平移先验权重越大越不允许匹配结果偏离预测平移 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight 40.0 -- 旋转先验权重越大越不允许匹配结果偏离预测角度 POSE_GRAPH.optimize_every_n_nodes 90 -- 每插入 90 个节点执行一次全局优化 POSE_GRAPH.constraint_builder.min_score 0.55 -- 回环约束最低匹配得分低于该分数不接受约束 POSE_GRAPH.constraint_builder.sampling_ratio 0.3 -- 后端约束搜索的采样比例用于控制计算量官方文档中也指出POSE_GRAPH.optimize_every_n_nodes控制全局优化批次大小将其设为 0 可以禁用 Global SLAM从而专注调试 Local SLAM 行为。文档还说明后端 constraint builder 会使用 sampling ratio 控制约束构建数量采样太少可能漏掉约束采样太多会降低实时性。23. Cartographer 完整工作流程1. ROS 数据进入系统 LaserScan / PointCloud2 / IMU / Odometry / TF ↓ 2. SensorBridge 转换数据 ROS 消息转换为 Cartographer 内部数据结构 ↓ 3. RangeDataCollator 同步数据 整理多传感器 range data形成时间有序点云 ↓ 4. PoseExtrapolator 预测位姿 根据历史位姿、IMU、里程计等预测当前 scan 位姿 ↓ 5. 点云预处理 min_range / max_range 过滤 returns / misses 分类 重力对齐 固定体素滤波 自适应体素滤波 ↓ 6. ScanMatch 使用预测位姿作为初值 可选实时相关匹配粗配准 CeresScanMatcher 精配准 ↓ 7. 得到 pose_estimate 当前 scan 在 local 坐标系下的估计位姿 ↓ 8. MotionFilter 判断 如果运动太小则跳过插入 如果运动足够大则继续 ↓ 9. InsertIntoSubmap 将当前 range data 插入 active submaps 更新 hit / miss 栅格概率 ↓ 10. 生成 TrajectoryNode 保存时间、点云、位姿、重力对齐信息 ↓ 11. PoseGraph 添加节点和约束 建立 node-submap 局部约束 ↓ 12. 后台回环检测 finished submap 与历史 node 做匹配 ↓ 13. 添加 loop closure constraint 匹配成功则添加回环约束 ↓ 14. 全局优化 优化所有 submap 和 trajectory node 的全局位姿 ↓ 15. 输出地图和轨迹 发布 map、submap、trajectory、tf24. 总结Cartographer 是一个以submap 和 pose graph为核心的实时 SLAM 系统。它的整体设计不是把所有激光数据直接塞进一张全局地图里也不是只依赖单帧之间的短期匹配而是把建图过程拆成两个层次前端 Local SLAM 负责局部准确后端 Global SLAM 负责全局一致。前端不断把当前 scan 匹配到当前 active submap 上并把匹配后的 scan 插入 submap后端则把 scan 节点、submap 节点以及它们之间的相对位姿约束组织成 pose graph周期性地做全局优化。这样设计的好处是前端可以保持实时后端可以在后台慢慢修正累计漂移最终输出一张全局更一致的地图。Cartographer 前端最核心的入口是LocalTrajectoryBuilder2D::AddRangeData()。它接收带时间信息的 range data先通过RangeDataCollator做数据同步与排序然后使用PoseExtrapolator对每个点的时间进行位姿外推。这个细节非常重要因为激光扫描不是严格瞬时完成的机器人在扫描过程中会运动。如果直接把一整帧点云当成同一时刻的数据点云会出现运动形变。Cartographer 通过每个点或每批 range data 的时间信息结合外推器预测该时刻机器人位姿再把点转换到统一坐标系下。经过这一步后系统再根据min_range和max_range对点做距离过滤把可靠击中点放入returns把超过最大距离的射线按missing_data_ray_length截断后放入misses。这两个集合后面会分别用于更新占据区域和空闲区域。点云进入 scan matching 前还要经过滤波。滤波的目的不是随便减少点数而是控制计算复杂度并改善点云空间分布。Cartographer 中常见的是固定体素滤波和自适应体素滤波。固定体素滤波用固定的 voxel size 减少点数自适应体素滤波则在最大体素尺寸限制下尽量保留目标数量的点。这样可以避免点云过密导致 Ceres 优化计算量过大也可以避免某些近距离高密度区域对匹配产生过强影响。滤波后的点云会进入ScanMatch()也就是 Local SLAM 中最核心的位姿估计阶段。ScanMatch()的主要任务是把当前 scan 对齐到当前 active submap。它先使用 PoseExtrapolator 给出的pose_prediction作为初始位姿。如果开启在线相关匹配系统会先在一定搜索窗口内寻找一个更好的初始位姿得到initial_ceres_pose随后调用CeresScanMatcher2D::Match()做精匹配。CeresScanMatcher 的优化变量是二维位姿[x, y, θ]它添加了三类残差占据空间残差、平移先验残差、旋转先验残差。占据空间残差希望当前 scan 点经过位姿变换后落在 submap 中占据概率高的位置平移和旋转先验则限制优化结果不要过度偏离预测初值。这样做能兼顾地图匹配和运动连续性避免匹配结果因为局部噪声而发生过大跳变。Submap 是 Cartographer 地图表达的核心。每个 submap 是一张概率栅格地图栅格内保存占据概率。激光终点对应 hit会提高对应栅格的占据概率激光射线路径经过但没有击中的区域对应 miss会降低这些栅格的占据概率。概率更新使用 odds 形式即odds(p)p/(1-p)这样多次观测可以通过 odds 的乘法形式融合再通过odds^-1转回概率并用clamp限制概率范围。这个机制让 Cartographer 能够稳定地融合多帧激光观测被多次击中的位置逐渐变成障碍物区域被多次穿过的位置逐渐变成空闲区域没有观测到的位置保持未知。当 scan matching 得到pose_estimate后Cartographer 并不会无条件把每一帧都插入 pose graph。它会通过MotionFilter判断当前帧和上一关键帧之间的时间、平移、旋转变化是否足够大。如果变化很小当前帧可以跳过不生成新的 trajectory node。这样可以减少冗余节点降低后端优化压力。如果变化足够大系统会调用InsertIntoSubmap()把当前 scan 插入 active submaps并生成TrajectoryNode::Data。这个节点保存当前时间、重力对齐信息、滤波点云和当前估计位姿。后端 PoseGraph 会基于这些节点和 submap 建立约束。Cartographer 后端的核心是 PoseGraph。前端每生成一个 trajectory node并把它插入某些 submap后端就可以记录 node 和 submap 之间的相对位姿关系。这种局部约束保持了局部地图结构的连续性。但是只靠局部约束长期运行仍然会产生累计误差。因此 Cartographer 还会在后台进行回环检测。当某个 submap 完成、不再插入新 scan 后它会参与历史节点匹配。如果某个历史 trajectory node 能够和这个 finished submap 在搜索窗口中匹配成功并且得分足够高系统就会添加一个 loop closure constraint。这个回环约束会告诉后端某个历史节点和某个子地图其实对应同一个空间区域只是当前估计位姿存在偏差。全局优化时PoseGraph 会同时调整所有 submap 和 trajectory node 的全局位姿使局部约束和回环约束整体误差最小。为了让回环检测实时运行Cartographer 使用 branch-and-bound 思想做 scan-to-submap 匹配。它不会暴力搜索所有候选位姿而是把搜索空间分层用预计算栅格快速估计某个区域可能达到的最高匹配得分。如果某个区域的得分上界已经低于当前最优解就直接剪枝不再继续细分搜索。这样可以在较大的搜索窗口中快速找到高质量候选位姿。找到候选后还可以继续用 CeresScanMatcher 做精匹配提高最终约束质量。从工程角度看Cartographer 的关键不是某一个单独函数而是整套链路的配合AddRangeData()负责接收和整理数据PoseExtrapolator负责预测位姿AdaptiveVoxelFilter负责控制点云规模ScanMatch()负责当前 scan 和 active submap 对齐InsertIntoSubmap()负责更新局部子地图并生成轨迹节点PoseGraph负责后台约束搜索和全局优化。理解 Cartographer 时最重要的是抓住三条主线第一前端通过 scan-to-submap 构建局部一致 submap第二submap 使用概率栅格表达 hit 和 miss 的占据更新第三后端通过 node-submap constraints 和 loop closure constraints 优化全局一致性。一句话概括Cartographer 的本质是“局部子地图构建 扫描匹配 位姿图优化”。前端保证每个 submap 内部尽量准确后端通过约束和回环把所有 submap 与轨迹节点拉到全局一致的位置上最终得到实时更新、可回环修正的地图和轨迹。版权声明 辛苦码字不易转载请注明原文出处和作者信息谢谢理解欢迎分享与交流但拒绝任何形式的商业转载或洗稿。