cartographer官方指导文件说明
第3章 cartographer前端算法流程介绍
3.1 Scan Match扫描匹配
扫描匹配（Scan Matching）是 Cartographer 中实现局部SLAM的核心技术，它通过优化算法将当前激光扫描数据对齐到子图地图中。下面从计算过程、数学模型、参数配置等多个维度进行全面解析：
3.1.1 扫描匹配工作流程
完整处理流程
关键步骤详解
1. 数据预处理
-
运动畸变校正：
range_data = motion_filter_.CorrectDistortion(range_data, pose_estimator);使用IMU/里程计数据补偿扫描期间机器人运动
-
体素滤波：
range_data = voxel_filter_.Filter(range_data);典型参数：
voxel_size = 0.05m
2. 初始位姿估计
- 来源：
- 运动外推器预测值（90%情况）
- 上一帧优化结果
- 里程计读数
3. 匹配方法选择逻辑
if (initial_pose_estimate_quality < threshold) {use_correlative_scan_matcher = true;
} else {use_correlative_scan_matcher = false;
}
3.1.2 两种核心匹配算法
1. 实时相关扫描匹配 (Real-Time Correlative Scan Matcher)
算法原理
搜索空间定义
| 参数 | 典型值 | 计算方式 |
|---|---|---|
| 线性搜索范围 | ±0.5m | search_window = 3*σ_pose |
| 角度搜索范围 | ±0.5rad (≈30°) | angular_window = 3*σ_θ |
| 分辨率 | 0.05m, 0.002rad | 平衡精度与计算量 |
得分计算
score ( T ) = 1 N ∑ i = 1 N M smooth ( T ⊕ h i ) \text{score}(T) = \frac{1}{N} \sum_{i=1}^{N} M_{\text{smooth}}(T \oplus h_i) score(T)=N1i=1∑NMsmooth(T⊕hi)
其中：
- T T T：候选位姿 ( x , y , θ ) (x, y, θ) (x,y,θ)
- h i h_i hi：扫描点 i i i 在扫描坐标系中的坐标
- M smooth M_{\text{smooth}} Msmooth：双三次插值后的子图概率值
特点
- 优点：全局最优性保证，对初始位姿不敏感
- 缺点：计算复杂度 O ( N x × N y × N θ ) O(N_x \times N_y \times N_θ) O(Nx×Ny×Nθ)
2. Ceres优化扫描匹配
优化问题建模
\min_{\xi} \left( \sum_{k=1}^{K} \rho \left( \left[1 - M_{\text{smooth}} \left( T_{\xi} h_k \right) \right]^2 \right) + \lambda_t \| \xi_t - \xi_{t,0} \|^2 + \lambda_r \| \xi_r - \xi_{r,0} \|^2 \right)
$$#### 优化变量
$$
\xi = [x, y, \theta] \in \mathbb{SE}(2)
$$#### 残差项组成
| 残差类型 | 数学形式 | 作用 |
|-------------------|----------------------------------|--------------------------|
| 占据概率残差 | $1 - M_{\text{smooth}}(T_{\xi} h_k)$ | 对齐扫描点与地图 |
| 平移正则项 | $\| \xi_t - \xi_{t,0} \|^2$ | 约束优化不偏离初始估计 |
| 旋转正则项 | $\| \xi_r - \xi_{r,0} \|^2$ | 防止角度跳变 |#### Ceres求解器配置
```cpp
ceres::Solver::Options options;
options.use_nonmonotonic_steps = true;
options.max_num_iterations = 20;
options.num_threads = 4;
options.linear_solver_type = ceres::DENSE_QR;
3.1.3 核心数学模型
1. 概率栅格模型
P ( m x , y = 1 ∣ z 1 : t ) = 1 1 + e − l x , y P(m_{x,y} = 1 | z_{1:t}) = \frac{1}{1 + e^{-l_{x,y}}} P(mx,y=1∣z1:t)=1+e−lx,y1
其中 l x , y l_{x,y} lx,y 是栅格的 log-odds 值，更新规则：
l x , y new = l x , y old + obs ( z t ) l_{x,y}^{\text{new}} = l_{x,y}^{\text{old}} + \text{obs}(z_t) lx,ynew=lx,yold+obs(zt)
2. 双三次插值
M_{\text{smooth}}(x,y) = \sum_{i=0}^{3}\sum_{j=0}^{3} a_{ij} x^i y^j
系数 a i j a_{ij} aij 由周围16个栅格值计算得到
3. 位姿变换
T ξ h k = [ cos θ − sin θ x sin θ cos θ y 0 0 1 ] [ h k , x h k , y 1 ] T_{\xi} h_k = \begin{bmatrix} \cos\theta & -\sin\theta & x \\ \sin\theta & \cos\theta & y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} h_{k,x} \\ h_{k,y} \\ 1 \end{bmatrix} Tξhk= cosθsinθ0−sinθcosθ0xy1 hk,xhk,y1
3.1.4 参数配置详解
Lua配置文件结构
TRAJECTORY_BUILDER_2D = {use_online_correlative_scan_matching = true, -- 是否启用相关匹配ceres_scan_matcher = {occupied_space_weight = 1.0, -- 占据空间残差权重translation_weight = 10.0, -- 平移正则权重rotation_weight = 40.0, -- 旋转正则权重ceres_solver_options = {use_nonmonotonic_steps = false,max_num_iterations = 20, -- 最大迭代次数num_threads = 1, -- 求解线程数}},real_time_correlative_scan_matcher = {linear_search_window = 0.1, -- 线性搜索窗(m)angular_search_window = math.rad(1.), -- 角度搜索窗(rad)translation_delta_cost_weight = 1e-1, -- 平移代价权重rotation_delta_cost_weight = 1e-1, -- 旋转代价权重}
}
参数调优指南
| 场景 | 调整参数 | 推荐值 |
|---|---|---|
| 结构化环境（室内） | 提高occupied_space_weight | 1.5 → 2.0 |
| 动态环境 | 降低translation_weight | 10.0 → 5.0 |
| 高速旋转 | 提高rotation_weight | 40.0 → 60.0 |
| 初始位姿不确定 | 增大linear_search_window | 0.1 → 0.3 |
| 计算资源有限 | 减少max_num_iterations | 20 → 10 |
| 多核系统 | 增加num_threads | 1 → 4 |
3.1.5 高级优化技术
1. 自适应分辨率
if (scan_density > threshold) {options.resolution = 0.05; // 高分辨率
} else {options.resolution = 0.10; // 低分辨率
}
2. 多策略回退机制
graph TD
A[Ceres优化失败] --> B[增大搜索窗口重试]
B --> C{成功？}
C -->|是| D[输出结果]
C -->|否| E[切换相关匹配]
E --> F{成功？}
F -->|是| D
F -->|否| G[使用运动外推]
3. 动态权重调整
// 根据匹配质量动态调整权重
if (match_quality < 0.6) {options.translation_weight *= 2.0;options.rotation_weight *= 1.5;
}
3.1.6 性能优化策略
1. 计算加速技术
| 技术 | 加速比 | 实现方式 |
|---|---|---|
| 栅格预积分 | 3-5x | 预先计算插值系数表 |
| SIMD并行化 | 4-8x | 使用AVX2指令处理扫描点 |
| 分支预测优化 | 1.2-1.5x | 重构条件判断逻辑 |
| 搜索空间剪枝 | 2-10x | 多分辨率分层搜索 |
2. 内存访问优化
// 优化后的内存布局
struct AlignedProbabilityGrid {float* __restrict cells; // 64字节对齐int stride; // 缓存行大小倍数
};
3.1.7 典型问题解决方案
1. 长廊环境匹配失败
解决方案：
-- 增加旋转约束权重
rotation_weight = 60.0
-- 启用全角度搜索
angular_search_window = math.rad(30.)
2. 动态物体干扰
解决方案：
// 动态点过滤
range_data = RemoveDynamicPoints(range_data, grid);
过滤逻辑：
\text{keep} =
\begin{cases}
\text{true} & \text{if } |M(T_{\xi}h_k) - 0.5| > 0.3 \\
\text{false} & \text{otherwise}
\end{cases}
3. 对称环境误匹配
解决方案：
-- 增加平移约束
translation_weight = 20.0
-- 降低优化步长
ceres_solver_options.linear_solver_step = 0.8
3.1.8 与其他模块的交互
1. 与运动外推器的协同
2. 与子图系统的交互
| 操作 | 数据流向 | 触发条件 |
|---|---|---|
| 栅格数据读取 | 子图 → 扫描匹配 | 匹配开始 |
| 匹配质量反馈 | 扫描匹配 → 子图 | 插入扫描前 |
| 多分辨率请求 | 扫描匹配 → 子图 | 相关匹配启用时 |
3.1.9 总结：扫描匹配技术矩阵
| 维度 | Real-Time Correlative | Ceres Optimization |
|---|---|---|
| 精度 | 中等（依赖分辨率） | 高（连续优化） |
| 鲁棒性 | 高（全局搜索） | 中（依赖初始值） |
| 计算耗时 | 10-50ms | 5-20ms |
| 适用场景 | 初始位姿不确定/低特征环境 | 高精度要求/特征丰富环境 |
| 可并行性 | 高（候选位姿独立） | 中（线程级并行） |
| 内存消耗 | 高（需缓存栅格） | 低（仅存储梯度） |
最佳实践：
- 默认启用双模式：相关匹配为Ceres提供优质初值
- 结构化环境：提高占据空间权重（
occupied_space_weight=1.5） - 动态环境：降低正则权重（
translation_weight=5.0） - 资源受限设备：关闭相关匹配，减少Ceres迭代次数
Cartographer通过这种分层匹配策略，在保证鲁棒性的同时实现厘米级定位精度，成为其在大规模环境中稳定运行的关键技术支柱。
3.2 Motion Filter运动滤波器
在 Cartographer 中，Motion Filter（运动滤波器） 是一个关键的决策模块，它决定当前帧激光数据是否需要插入子图。以下是其输入数据、判断逻辑和计算原理的详细解析：
3.2.1 Motion Filter 的输入数据
| 输入项 | 数据来源 | 物理意义 |
|---|---|---|
| 当前帧位姿 | Scan Match 输出的 T_scan_submap（当前帧在子图坐标系中的位姿） | 当前帧的精确位置和朝向 |
| 上一插入帧位姿 | 上一次成功插入子图的扫描帧位姿（存储在 pose_queue_ 中） | 上一次地图更新的参考点 |
| 当前帧时间戳 | 激光雷达数据的时间戳 (sensor::RangeData::time) | 当前帧的采集时刻 |
| 上一插入帧时间戳 | 上一次插入操作的激光帧时间戳 | 上一次地图更新的时刻 |
3.2.2 Motion Filter 的判断逻辑
Motion Filter 通过计算 当前帧 与 上一次插入帧 之间的运动变化量，判断是否达到插入阈值。计算包含三个独立维度：
graph TD
A[当前帧位姿 P_current] --> B[计算与 P_last 的相对运动]
C[上一插入帧位姿 P_last] --> B
D[当前时间 t_current] --> E[计算时间差 Δt]
F[上一插入时间 t_last] --> E
B --> G{是否满足任一阈值？}
E --> G
G -- 是 --> H[允许插入子图]
G -- 否 --> I[丢弃当前帧]
运动变化量计算：
- 平移距离（
distance）\Delta d = \sqrt{(x_c - x_l)^2 + (y_c - y_l)^2} - 旋转角度（
angle）\Delta \theta = |\text{normalize_angle}(\theta_c - \theta_l)| - 时间间隔（
time）\Delta t = t_c - t_l
3.2.3 阈值判断规则
当满足以下 任一条件 时，当前帧将被插入子图：
if (Δd > linear_distance_threshold || Δθ > angular_distance_threshold || Δt > max_time_threshold) {InsertScanIntoSubmap(); // 执行插入
}
参数详解（典型值）：
| 参数名 | Lua配置项 | 默认值 | 作用 |
|---|---|---|---|
| 线性位移阈值 (Δd) | TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters | 0.1 m | 移动超过此距离则触发插入（避免静止时冗余数据） |
| 角度旋转阈值 (Δθ) | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians | 0.5° (≈0.0087 rad) | 旋转超过此角度则触发插入（防止旋转建图模糊） |
| 最大时间阈值 (Δt) | TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds | 5.0 s | 超时强制插入（应对长时间静止后首次运动） |
3.2.4 源码实现解析
关键代码位于 motion_filter.cc：
bool MotionFilter::IsSimilar(const common::Time time, const transform::Rigid3d& pose) {const double delta_time = (time - last_time_).seconds();const transform::Rigid3d delta_pose = last_pose_.inverse() * pose;const double delta_translation = delta_pose.translation().norm();const double delta_rotation = transform::GetAngle(delta_pose);return delta_time < max_time_seconds_ &&delta_translation < max_distance_meters_ &&delta_rotation < max_angle_radians_;
}// 调用逻辑（local_trajectory_builder_2d.cc）
if (!motion_filter_.IsSimilar(time, pose_estimate)) {InsertIntoSubmap(..., pose_estimate); // 执行插入last_pose_ = pose_estimate; // 更新上一插入位姿last_time_ = time; // 更新上一插入时间
}
3.2.5 设计意义与场景分析
1. 核心作用：
- 抑制冗余数据：避免机器人静止或微小运动时重复插入相似扫描
- 降低计算负载：减少子图更新频率（栅格地图插入是计算密集型操作）
- 保护地图质量：防止高频插入导致概率栅格过度饱和（如墙面变厚）
2. 典型场景：
| 场景 | Motion Filter 行为 |
|---|---|
| 机器人静止 | 丢弃所有后续帧（Δt < 5s, Δd=0, Δθ=0） |
| 缓慢直线移动 | 每移动 10cm 插入一帧（Δd > 0.1m 触发） |
| 原地旋转 | 每旋转 0.5° 插入一帧（Δθ > 0.0087 rad 触发） |
| 突发高速运动 | 首次超过阈值即插入，后续帧根据新位姿重新判断 |
| 长时间静止后运动 | 超过 5s 后首帧强制插入（即使 Δd/Δθ 未超阈值） |
3.2.6 参数调优建议
| 问题现象 | 调整策略 |
|---|---|
| 地图细节不足（特征丢失） | 减小 max_distance_meters (→0.05m) 和 max_angle_radians (→0.004 rad) |
| CPU占用过高 | 增大 max_distance_meters (→0.2m) 或 max_angle_radians (→0.02 rad) |
| 旋转建图模糊 | 单独减小 max_angle_radians (→0.005 rad) |
| 长时间静止后首帧插入延迟 | 减小 max_time_seconds (→3.0s) |
注：在动态环境中（如人流密集场景），建议配合降低
hit_probability（命中概率）使用：TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.53
3.2.7 总结
Motion Filter 的输入是 Scan Match 输出的当前位姿 与 上一次插入操作的位姿/时间戳，通过计算三者变化量（位移/角度/时间）并与阈值比较，决定是否插入子图。这种机制在保证地图质量的同时，显著提升了系统效率，是 Cartographer 实时性的关键保障之一。
3.3 Submap子图
Cartographer 中的子图(Submap)是其 SLAM 系统的核心创新，下面我将全面解析子图的创建、更新、维护、删除等全生命周期机制，以及状态管理、存储结构和空间关系等关键设计：
3.3.1 子图生命周期管理
1. 创建机制
graph TD
A[新激光帧] --> B{运动滤波通过？}
B -->|是| C[扫描匹配]
C --> D[插入当前活跃子图]
D --> E{子图达到阈值？}
E -->|是| F[冻结当前子图]
F --> G[创建新子图]
G --> H[保持双活跃状态]
-
创建条件：
- 轨迹开始时自动创建第一个子图
- 当前子图达到以下任一阈值：
- 扫描帧数 ≥
num_range_data（默认60） - 覆盖半径 ≥
max_submap_range_meters（默认20m） - 持续时间 ≥
max_submap_time_seconds（默认∞）
- 扫描帧数 ≥
-
创建过程：
// 在 ActiveSubmaps2D::AddRangeData() if (submaps_.front()->num_range_data() >= options_.num_range_data()) {FinishSubmap(); // 冻结当前子图AddSubmap(pose); // 创建新子图 }
2. 更新机制
-
更新触发：
- 运动滤波器通过的激光帧
- 仅更新活跃子图（ACTIVE状态）
-
更新方式：
void Submap2D::InsertRangeData(const sensor::RangeData& range_data,const RangeDataInserterInterface* range_data_inserter) {// 概率栅格更新range_data_inserter->Insert(range_data, grid_.get());++num_range_data_; }使用命中-未命中模型更新概率栅格：
- 命中点：增加占据概率（0.55 → 0.85）
- 未命中区域：降低概率（0.55 → 0.45）
3. 维护机制
-
双活跃子图维护：
std::vector<std::shared_ptr<Submap2D>> submaps_; // 始终包含1-2个活跃子图- 新子图创建后，旧子图继续接收数据直到：
- 新子图达到
num_range_data/2帧 - 确保50%重叠区域
- 新子图达到
- 新子图创建后，旧子图继续接收数据直到：
-
多分辨率维护：
// 为回环检测生成多分辨率地图 void CreateLowResolutionMap(const ProbabilityGrid& original) {low_resolution_grid_ = std::make_unique<ProbabilityGrid>(original.limits(), original.probability_cells());// 降采样处理... }
4. 删除机制
-
删除条件：
graph LR A[子图冻结] --> B{是否被引用？} B -->|无约束引用| C[可删除] B -->|有回环约束| D[保留] C --> E[裁剪队列] -
删除过程：
void PoseGraph2D::TrimSubmap(const SubmapId& id) {if (!pose_graph_->HasConstraints(id)) { // 检查约束引用submap_data_.erase(id); // 删除子图数据optimized_submaps_.erase(id); // 删除优化记录} }
5. 构建完成（冻结）
-
冻结条件：
- 扫描帧数 ≥
num_range_data - 新子图已创建并接收足够数据
- 扫描帧数 ≥
-
冻结操作：
void Submap2D::Finish() {set_state(SubmapState::kFinished); // 状态切换CreateLowResolutionMap(*grid_); // 生成低分辨率地图 }
3.3.2 子图状态管理
1. 状态机设计
2. 状态切换操作
| 状态 | 设置方法 | 特性 |
|---|---|---|
kActive | AddRangeData() | 可更新栅格，不参与回环检测 |
kFinished | FinishSubmap() | 冻结栅格，参与回环检测 |
kDeleted | TrimSubmap() | 标记删除，等待资源回收 |
3.3.3 存储方式与数据结构
1. 内存数据结构
struct SubmapData {std::shared_ptr<const Submap> submap; // 子图元数据transform::Rigid3d global_pose; // 全局位姿ProbabilityGrid high_resolution_grid; // 高分辨率栅格(5cm)ProbabilityGrid low_resolution_grid; // 低分辨率栅格(25cm)
};
2. 磁盘存储格式（.pbstream）
message Submap {SubmapId id = 1; // (trajectory_id, submap_index)Rigid3d local_pose = 2; // 局部坐标系位姿int32 num_range_data = 3; // 插入帧数SubmapState state = 4; // 状态枚举ProbabilityGridProto high_res_grid = 5; // 压缩栅格数据
}
3. 栅格压缩算法
- 游程编码(RLE)：连续相同值压缩
- 分块存储：16x16块独立压缩
- 概率量化：8bit存储(0-255对应概率0-1)
3.3.4 空间关系机制
1. 重叠区域设计
- 固定重叠率：50%重叠（核心设计）
// 双活跃子图维持机制 while (submaps_.size() < 2) {AddSubmap(pose); // 确保始终有两个活跃子图 } - 重叠生成原理：
子图 开始帧 结束帧 重叠区域 Submap_A 1 60 帧30-60 Submap_B 30 90 帧30-60
2. 子图对齐机制
- 对齐定义：通过优化调整子图间的相对位姿，使重叠区域几何一致
- 对齐实现：
void OptimizationProblem2D::Solve(const std::vector<Constraint>& constraints) {// 构建对齐误差项problem.AddResidualBlock(new SubmapMatchCostFunction(relative_pose, covariance),nullptr,prev_submap_pose.data(),next_submap_pose.data()); } - 对齐误差函数：
e = log ( T relative − 1 ⋅ ( T prev − 1 T next ) ) e = \log\left(T_{\text{relative}}^{-1} \cdot (T_{\text{prev}}^{-1} T_{\text{next}})\right) e=log(Trelative−1⋅(Tprev−1Tnext))
3.3.5 高级特性与优化
1. 动态环境适应性
void ProbabilityGrid::UpdateDynamicAreas(const std::vector<DynamicCell>& dynamic_cells) {if (state_ == kFinished) return; // 冻结子图不更新for (const auto& cell : dynamic_cells) {UpdateProbability(cell.x, cell.y, 0.5); // 重置为未知状态}
}
2. 子图裁剪策略
| 策略 | 触发条件 | 操作 |
|---|---|---|
| 边缘裁剪 | 子图使用率<40% | 缩小物理边界 |
| 分辨率降级 | 距离>50m | 切换为低分辨率 |
| 惰性加载 | 内存压力>80% | 卸载非活动子图 |
3. 多机器人协同
- 对齐原理：通过跨轨迹约束实现不同机器人子图的空间对齐
- 数据关联：子图ID包含轨迹ID
(trajectory_id, submap_index)
3.3.6 性能优化实践
1. 参数调优建议
TRAJECTORY_BUILDER_2D.submaps = {num_range_data = 60, -- 增大可提升回环鲁棒性resolution = 0.05, -- 5cm栅格(平衡精度/内存)range_data_inserter = {hit_probability = 0.55, -- 降低可减少动态物体残留miss_probability = 0.49,},
}
2. 内存优化技巧
- 栅格分块加载：
void ProbabilityGrid::LoadOnDemand(int x, int y) {if (!IsBlockLoaded(x,y)) {LoadBlockFromDisk(GetBlockId(x,y));} } - 差分更新：仅存储相对于基础子图的差异
3.3.7 子图机制的核心价值
-
空间分割：
将大环境分解为局部一致的子单元，避免全局优化复杂度爆炸 -
分层优化：
- 前端：保证子图内局部一致性
- 后端：通过约束优化子图间关系
-
计算效率：
- 活跃子图：高频更新
- 冻结子图：低频参与回环检测
-
多尺度处理：
分辨率 用途 计算复杂度 5cm 扫描匹配 高 25cm 回环检测 低 -
系统扩展性：
- 子图ID =
(trajectory_id, submap_index)支持：- 多机器人协同
- 长期增量建图
- 分层地图管理
- 子图ID =
Cartographer 通过这套精妙的子图机制，在保证精度的同时，实现了大规模环境下的实时SLAM能力，成为其区别于其他SLAM系统的核心竞争优势。
3.4 位姿递推器
Cartographer 中的 位姿递推器（Pose Extrapolator） 是多传感器融合的核心模块，负责实时预测机器人的位姿。其计算过程融合了 IMU 角速度积分、里程计运动模型 和 历史位姿插值，以下是详细计算流程与实例分析：
3.4.1 位姿递推器计算流程（6 步分解）
步骤 1：初始化与数据结构
struct ExtrapolationState {common::Time time; // 最新更新时间戳transform::Rigid3d pose; // 当前位姿 (x,y,z,roll,pitch,yaw)Eigen::Vector3d velocity; // 线速度 (vx,vy,vz)Eigen::Vector3d angular_velocity; // 角速度 (ωx,ωy,ωz)
};
std::queue<ExtrapolationState> pose_queue_; // 历史位姿队列
步骤 2：传感器数据输入
步骤 3：时间推进预测（核心方法 Advance(time)）
bool PoseExtrapolator::Advance(const common::Time time) {const double delta_t = common::ToSeconds(time - last_time_);// 1. 角速度积分更新姿态const Eigen::Quaterniond rotation = AngleAxisVectorToRotationQuaternion(Eigen::Vector3d(angular_velocity_ * delta_t));extrapolation_state_.pose.rotation = extrapolation_state_.pose.rotation * rotation;// 2. 线速度积分更新位置extrapolation_state_.pose.translation += extrapolation_state_.velocity * delta_t;// 3. 更新时间戳extrapolation_state_.time = time;return true;
}
步骤 4：传感器数据融合
(1) 处理 IMU 角速度
void AddImuAngularVelocity(const sensor::ImuData& imu_data) {angular_velocity_ = imu_data.angular_velocity; // 直接更新角速度last_imu_time_ = imu_data.time;
}
(2) 处理里程计数据
void AddOdometryData(const sensor::OdometryData& odom_data) {if (!odom_queue_.empty()) {// 计算瞬时速度 v = Δs / Δtconst auto& prev_odom = odom_queue_.back();const double dt = common::ToSeconds(odom_data.time - prev_odom.time);velocity_ = (odom_data.pose.translation - prev_odom.pose.translation) / dt;}odom_queue_.push(odom_data);
}
(3) 处理激光匹配位姿
void AddPose(const transform::Rigid3d& pose, const common::Time time) {// 1. 更新当前状态extrapolation_state_ = {time, pose, velocity_, angular_velocity_};// 2. 维护历史队列（用于插值）if (pose_queue_.size() > 20) pose_queue_.pop();pose_queue_.push(extrapolation_state_);
}
步骤 5：位姿插值（ExtrapolatePose(time)）
transform::Rigid3d ExtrapolatePose(const common::Time time) {Advance(time); // 先预测到目标时间// 历史位姿插值补偿if (!pose_queue_.empty()) {const auto& newest = pose_queue_.back();const auto& oldest = pose_queue_.front();const double ratio = (time - oldest.time) / (newest.time - oldest.time);// 线性插值位置const Eigen::Vector3d interp_trans = oldest.pose.translation * (1 - ratio) + newest.pose.translation * ratio;// 球面插值旋转const Eigen::Quaterniond interp_rot = oldest.pose.rotation.slerp(ratio, newest.pose.rotation);return transform::Rigid3d(interp_trans, interp_rot);}return extrapolation_state_.pose;
}
步骤 6：协方差估计（运动不确定性）
PoseCovariance EstimateCovariance(const common::Time time) {const double delta_t = common::ToSeconds(time - last_time_);// 基础协方差 (单位: m², rad²)PoseCovariance base_cov = PoseCovariance::Identity() * 0.01; // 动态放大因子const double vel_scale = velocity_.norm() * delta_t * 0.5;const double ang_scale = angular_velocity_.norm() * delta_t * 0.3;// 位置协方差放大base_cov.block<3, 3>(0, 0) *= (1 + vel_scale); // 旋转协方差放大base_cov.block<3, 3>(3, 3) *= (1 + ang_scale); return base_cov;
}
3.4.2 计算实例：机器人加速转弯场景
初始状态（t=0s）
位姿： P0 = (0, 0, 0) R0 = (0°, 0°, 0°)
速度： v0 = (0.5, 0, 0) m/s
角速度： ω0 = (0, 0, 0.3) rad/s // 绕Z轴转弯
预测 2 秒后位姿（t=2s）
// 1. 角速度积分
Δθ = ω0 * Δt = (0, 0, 0.3) * 2 = (0, 0, 0.6 rad) ≈ 34.4°
旋转四元数 R = AngleAxisToQuaternion(0.6 rad)// 2. 线速度积分（未考虑旋转）
理论位移 = v0 * Δt = (0.5, 0, 0) * 2 = (1, 0, 0)// 3. 旋转补偿（因转弯需转换速度方向）
实际位移 = R0.conjugate() * (v0 * Δt) = [cos(34.4°)*1, sin(34.4°)*1, 0] = (0.825, 0.565, 0)// 4. 最终预测位姿
位置 = P0 + 实际位移 = (0.825, 0.565, 0)
朝向 = R0 * R = (0°, 0°, 34.4°)
与真实运动对比
| 参数 | 预测位姿 | 真实位姿 (圆周运动) | 误差 |
|---|---|---|---|
| X | 0.825 m | 0.821 m (cos34.4°*r) | +0.5% |
| Y | 0.565 m | 0.564 m (sin34.4°*r) | +0.2% |
| θ | 34.4° | 34.4° | 0 |
注：此简化模型未考虑向心加速度，实际误差随速度增加而增大
3.4.3 关键参数配置
TRAJECTORY_BUILDER_2D.pose_extrapolator = {use_imu_based = false, -- 是否依赖IMU（默认false）imu_gravity_time_constant = 10.0, -- 重力滤波常数odometry_velocity_scale = 1.0, -- 里程计速度缩放pose_queue_duration = 0.5, -- 历史位姿队列时长（秒）
}
3.4.4 特殊场景处理策略
1. 传感器失效
| 失效传感器 | 处理方案 |
|---|---|
| IMU | 切换为纯里程计模型（use_imu_based=false） |
| 里程计 | 启用历史位姿插值作为运动模型 |
| 两者均失效 | 恒定速度假设（velocity_=last_vel） |
2. 高速旋转补偿
// 向心加速度补偿（定制扩展）
if (angular_velocity_.norm() > 0.5) {const Eigen::Vector3d centripetal_acc = angular_velocity_.cross(velocity_);pose.translation -= 0.5 * centripetal_acc * delta_t * delta_t;
}
3.4.5 性能优化技巧
-
历史位姿队列长度
- 室内环境：
pose_queue_duration=0.3（短时运动） - 室外高速：
pose_queue_duration=1.0（长时预测）
- 室内环境：
-
协方差动态缩放
// 增加旋转不确定性权重 cov.block<3,3>(3,3) *= (1 + 2.0 * angular_velocity_.norm()); -
插值算法切换
// 小角度使用线性插值，大角度用球面插值 if (angle < 0.1) interp_rot = linear_interpolate(...); elseinterp_rot = slerp(...);
3.4.6 总结
Cartographer 的位姿递推器通过 三阶段融合 实现高精度预测：
- 运动学积分：
位移 = v·Δt,角度 = ω·Δt - 传感器校正：
- 里程计 → 瞬时速度
- 激光匹配 → 位姿重置
- 历史数据插值：
滑动窗口加权补偿积分误差
预测误差源：
- 未建模的向心加速度（转弯时）
- 瞬时速度突变（急加速）
- IMU 偏置漂移
实际应用中需通过 增大历史队列时长 和 动态协方差缩放 提升鲁棒性，高速场景建议启用向心加速度补偿（需定制开发）。
3.5 Imu_tracker姿态追踪
Cartographer中的IMU Tracker是一个核心模块，用于融合IMU（惯性测量单元）的角速度和加速度数据，实时估计设备的姿态方向（orientation）。其设计重点在于通过重力方向约束优化滚转（roll）和俯仰（pitch）角，但无法消除偏航角（yaw）的漂移。以下是其详细内容解析：
3.5.1 核心功能与设计思想
-
重力方向约束
- 原理：在设备低速运动时，线性加速度的均值近似为重力加速度，因此加速度计可直接测量重力方向，从而稳定
roll和itch角。 - 局限性：偏航角（
yaw）因无绝对参考而产生漂移，需依赖其他传感器（如激光雷达、磁力计）校正。
- 原理：在设备低速运动时，线性加速度的均值近似为重力加速度，因此加速度计可直接测量重力方向，从而稳定
-
时间驱动更新
- 通过
Advance()方法推进时间，利用角速度积分更新姿态，同时反向旋转重力向量以保持一致性。
- 通过
3.5.2 数据成员与初始化
1. 关键成员变量
| 变量名 | 类型 | 作用 |
|---|---|---|
imu_gravity_time_constant_ | double | 重力加速度时间常数（通常设为9.8-10.0），控制加速度融合的平滑度 |
time_ | common::Time | 最新一次IMU数据的时间戳 |
last_linear_acceleration_time_ | common::Time | 上一次加速度观测的时间戳 |
orientation_ | Eigen::Quaterniond | 当前姿态的四元数表示（核心输出） |
gravity_vector_ | Eigen::Vector3d | 估计的重力方向向量（初始为[0,0,1]） |
imu_angular_velocity_ | Eigen::Vector3d | 当前角速度观测值 |
2. 初始化
ImuTracker::ImuTracker(double imu_gravity_time_constant, common::Time time): imu_gravity_time_constant_(imu_gravity_time_constant),time_(time),orientation_(Eigen::Quaterniond::Identity()), // 初始姿态：无旋转gravity_vector_(Eigen::Vector3d::UnitZ()), // 初始重力：Z轴方向imu_angular_velocity_(Eigen::Vector3d::Zero()) {}
初始化时姿态为单位四元数，重力向量指向Z轴正方向（世界坐标系定义）。
3.5.3 核心方法解析
1. 时间推进与姿态更新（Advance()）
void ImuTracker::Advance(common::Time time) {const double delta_t = common::ToSeconds(time - time_);// 计算旋转量：角速度 × 时间差const Eigen::Quaterniond rotation = transform::AngleAxisVectorToRotationQuaternion(Eigen::Vector3d(imu_angular_velocity_ * delta_t));orientation_ = (orientation_ * rotation).normalized(); // 更新姿态gravity_vector_ = rotation.conjugate() * gravity_vector_; // 反向旋转重力向量time_ = time; // 更新时间戳
}
- 作用：根据角速度积分更新姿态，同时修正重力向量方向。
2. 加速度观测融合（AddImuLinearAccelerationObservation()）
void AddImuLinearAccelerationObservation(const Eigen::Vector3d& imu_linear_acceleration) {// 计算时间差const double delta_t = time_ - last_linear_acceleration_time_;// 计算融合权重：α = 1 - exp(-Δt / τ)const double alpha = 1 - std::exp(-delta_t / imu_gravity_time_constant_);// 低通滤波融合重力向量gravity_vector_ = (1 - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;// 根据重力修正姿态（roll/pitch）orientation_ = Eigen::Quaterniond::FromTwoVectors(gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());last_linear_acceleration_time_ = time_;
}
- 作用：通过低通滤波器融合新加速度观测，更新重力向量并修正姿态（仅影响
roll/pitch）。
3. 角速度观测更新（AddImuAngularVelocityObservation()）
void AddImuAngularVelocityObservation(const Eigen::Vector3d& imu_angular_velocity) {imu_angular_velocity_ = imu_angular_velocity; // 直接更新角速度
}
- 作用：仅存储当前角速度值，供
Advance()积分使用。
3.5.4 算法特性与局限
-
优势
- 实时性高：计算复杂度低，适合嵌入式系统。
- 抗运动干扰：低通滤波减少瞬时加速度对重力估计的影响。
-
局限
- yaw漂移问题：无外部参考时偏航角误差累积（需激光/视觉辅助）。
- 动态场景失效：高速运动时加速度计无法分离重力与运动加速度。
3.5.5 实际应用配置
在Cartographer中启用IMU Tracker需在Lua配置中设置：
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 9.81 -- 重力常数
TRAJECTORY_BUILDER_2D.orientation_weight = 1.0 -- 姿态优化权重
同时需确保：
- 坐标系统一：
tracking_frame设置为IMU坐标系（如imu_link）。 - 时间同步：IMU与激光雷达数据需时间对齐（误差<1ms）。
3.5.6 总结
IMU Tracker通过角速度积分与重力方向滤波，实现了对设备姿态的实时跟踪。其设计巧妙利用物理约束稳定了roll/pitch，但yaw漂移问题需多传感器融合解决。实际应用中，参数调优（如重力时间常数）和坐标系校准对精度至关重要。
当设备本身存在非重力加速度（即运动加速度），且在x、y、z轴均有分量时，Cartographer 的 IMU Tracker 和整体SLAM系统采用多层次的策略来处理这种挑战性场景：
3.5.6.1 Cartographer 的系统级解决方案
1. 多传感器优先级覆盖
graph TD
A[IMU原始数据] --> B[IMU Tracker]
B --> C{是否激光匹配可用？}
C -- 是 --> D[使用激光位姿覆盖IMU姿态]
C -- 否 --> E[继续使用IMU估计]
- 激光扫描匹配优先：
当Scan Match成功后，其输出的高精度位姿会直接覆盖IMU Tracker的姿态估计
（源码位置：PoseExtrapolator::AddPose）
2. 运动外推器（Pose Extrapolator）的动态权重
在 PoseExtrapolator 中融合多传感器：
// 计算IMU权重（动态降低运动时权重）
double imu_weight = (angular_velocity.norm() < angular_velocity_threshold) ? high_weight : low_weight;// 融合结果 = w_imu * imu_pose + w_odom * odom_pose + w_scan * scan_pose
- 角速度阈值判断：
当||ω|| > 0.3 rad/s（默认）时，IMU权重从1.0降至0.1 - 加速度辅助判断：
若线加速度||a|| > 1.5g，标记为高动态状态
3. 扫描匹配的鲁棒性增强
针对加速度场景的专门优化：
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 0.5
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 0.1
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 0.1
- 降低运动自由度权重：
当检测到高加速度时，减少优化中对平移/旋转的约束，避免错误匹配
3.5.6.2 不同加速度场景的处理策略
1. 瞬时加速度场景（如避障急停）
- IMU Tracker：
低通滤波有效滤除突发加速度，重力向量基本稳定 - 系统响应：
运动外推器短期依赖里程计，等待下一帧激光匹配校正
2. 持续线性加速度（如直线加速）
| 参数 | 正常值 | 加速度场景调整 | 作用 |
|---|---|---|---|
imu_gravity_time_constant | 10.0 s | → 30.0 s | 降低加速度影响权重 |
orientation_weight | 1.0 | → 0.3 | 减少IMU姿态信任度 |
motion_filter.max_distance_meters | 0.1 m | → 0.05 m | 增加激光插入频率 |
3. 复杂加速度（如三维旋转运动）
- 3D模式专用处理：
TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = true TRAJECTORY_BUILDER_3D.imu_acceleration_weight = 0.5 - 点云预处理：
采用VoxelFilter降采样（voxel_size = 0.05），减少运动畸变影响
3.5.6.3 配置建议
高动态环境参数模板
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 30.0 -- 增大时间常数
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 0.05
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 0.05
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.05
POSE_GRAPH.optimization_problem.acceleration_weight = 0.5 -- 3D专属
传感器冗余方案
3.5.6.4 局限性及应对
-
Z轴观测缺失（2D模式）
- 问题：2D模式无法校正垂直方向运动
- 方案：
- 切換到3D模式
- 融合气压计数据（需定制开发）
-
长时间加速度（＞10s）
- 问题：重力向量持续偏移
- 方案：
// 强制重置重力向量（定制开发） if (acceleration_duration > 10.0) {gravity_vector_ = Eigen::Vector3d(0,0,9.8); }
3.5.6.5 结论
Cartographer 通过 三層防御机制 应对设备加速度：
- IMU Tracker层：
低通滤波抑制瞬时加速度 - 传感器融合层：
动态降低高加速度时IMU权重 - 全局优化层：
激光匹配和位姿图优化校正累积误差
最佳实践：在高动态场景中，结合参数调优（增大gravity_time_constant）与多传感器冗余（视觉+IMU），可实现厘米级精度定位。对于极端场景（如无人机特技），建议扩展气压计或TOF传感器。
3.6 Trajectory轨迹
轨迹（Trajectory）是 Cartographer 中最高维度的数据组织单元，下面我将从多个维度全面解析其工作机制：
3.6.1 轨迹生命周期管理
1. 创建机制
-
核心代码：
int MapBuilder::CreateTrajectoryBuilder(const TrajectoryBuilderOptions& options) {const int trajectory_id = trajectory_id_generator_++;auto trajectory_builder = std::make_unique<TrajectoryBuilder>(...);trajectory_builders_[trajectory_id] = std::move(trajectory_builder);pose_graph_->AddTrajectory(trajectory_id, options);return trajectory_id; } -
创建条件：
- 新设备接入（自动分配ID）
- 加载历史地图后继续建图（ID = max_id+1）
- 显式调用创建API
2. 更新机制
- 更新内容：
- 子图集合（
trajectory_state.submaps） - 位姿节点（
trajectory_state.nodes） - 传感器时间同步状态
- 子图集合（
3. 维护机制
-
内存管理：
struct TrajectoryState {std::deque<std::shared_ptr<Submap>> submaps; // 子图队列std::map<NodeId, TrajectoryNode> nodes; // 节点映射SensorCollator sensor_collator; // 传感器同步State state = ACTIVE; // 状态标志 }; -
自动维护操作：
- 子图冻结：当子图达到
num_range_data阈值 - 节点裁剪：移除优化后的历史节点
- 传感器缓存清理：超时未匹配数据丢弃
- 子图冻结：当子图达到
4. 结束机制
void MapBuilder::FinishTrajectory(int trajectory_id) {trajectory_builders_.at(trajectory_id)->Finish(); // 停止数据接收pose_graph_->FinishTrajectory(trajectory_id); // 标记轨迹完成trajectory_states_[trajectory_id] = FINISHED; // 更新状态
}
- 结束条件：
- 用户显式调用
FinishTrajectory() - 传感器丢失超时（默认30秒）
- SLAM严重失败（连续匹配失败）
- 用户显式调用
5. 删除机制
void PoseGraph::DeleteTrajectory(int trajectory_id) {// 检查约束依赖if (!HasExternalConstraints(trajectory_id)) {// 物理删除数据submap_data_.erase(trajectory_id);trajectory_nodes_.erase(trajectory_id);// 逻辑标记trajectory_states_[trajectory_id] = DELETED;}
}
- 删除条件：
- 轨迹状态为
FINISHED或FAILED - 无其他轨迹依赖其约束
- 未被实时建图使用
- 轨迹状态为
3.6.2 轨迹与子图的关系
1. 从属关系
- 每个子图通过
SubmapId绑定父轨迹：struct SubmapId {int trajectory_id;int submap_index; // 轨迹内唯一索引 };
2. 数据流关系
| 操作 | 轨迹角色 | 子图角色 |
|---|---|---|
| 创建 | 初始化子图容器 | 接收激光数据 |
| 更新 | 传递传感器数据 | 更新栅格地图 |
| 优化 | 提供节点约束 | 提供子图位姿 |
| 序列化 | 维护子图元数据 | 存储栅格数据 |
3.6.3 轨迹与后端的关系
1. 约束管理
- 约束类型：
- 局部约束（同轨迹内）
- 回环约束（同轨迹内）
- 跨轨迹约束（不同轨迹间）
2. 优化参与
void PoseGraph::RunOptimization() {// 构建优化问题for (const auto& constraint : constraints_) {// 轨迹A子图 vs 轨迹B节点problem.AddResidualBlock(new ConstraintCost(constraint),nullptr,GetPose(constraint.submap_id), // 子图位姿GetPose(constraint.node_id) // 节点位姿);}
}
3.6.4 新旧轨迹约束关系求解
1. 跨轨迹约束构建
2. 数学表示
跨轨迹约束残差：
e_{\text{cross}} = \log\left( T_{\text{match}}^{-1} \cdot (T_{\text{old\_submap}}^{-1} \cdot T_{\text{new\_node}} \right)
其中：
- T match T_{\text{match}} Tmatch：BBS匹配得到的相对位姿
- T old_submap T_{\text{old\_submap}} Told_submap：旧子图全局位姿
- T new_node T_{\text{new\_node}} Tnew_node：新轨迹节点位姿
3.6.5 轨迹长度限制与优化
1. 理论限制
| 资源类型 | 理论限制 | 影响因素 |
|---|---|---|
| 内存 | 无硬限制 | 子图分辨率/数量 |
| 计算实时性 | 100km轨迹 | 位姿图优化复杂度 |
| 存储 | 磁盘空间 | .pbstream压缩率 |
2. 实际限制解决方案
-
分段策略：
// 自动轨迹分段条件 if (trajectory_length > 1000m || node_count > 5000 || duration > 3600s) {FinishCurrentTrajectory();CreateNewTrajectory(); // 继承坐标系 } -
优化加速：
POSE_GRAPH.optimize_every_n_nodes = 180 -- 增大优化间隔 POSE_GRAPH.max_num_final_iterations = 50 -- 减少最终优化次数
3.6.6 高级特性
1. 轨迹冻结/解冻
// 冻结轨迹（暂停优化）
pose_graph_->FreezeTrajectory(trajectory_id);// 解冻轨迹
pose_graph_->ThawTrajectory(trajectory_id);
2. 轨迹坐标系对齐
void SetGlobalSlamOptimizationCallback(GlobalSlamOptimizationCallback callback) {// 用于多轨迹全局坐标系对齐global_slam_callback_ = callback;
}
3. 轨迹数据序列化
message TrajectoryData {int32 trajectory_id = 1;repeated Submap submaps = 2;repeated Node nodes = 3;TrajectoryState state = 4;Rigid3d origin_from_trajectory = 5; // 坐标系变换
}
3.6.7 总结
-
轨迹长度：
- ✅ 理论无限长：通过子图分块机制支持
- ⚠️ 实践限制：受内存/计算力约束，建议每1-5km分段
-
跨轨迹交互：
- 通过 跨子图匹配 建立空间约束
- 使用 相同优化器 求解跨轨迹约束
-
生命周期控制：
状态 允许操作 转换条件 ACTIVE 添加数据/局部优化 初始状态 FINISHED 参与全局优化/删除 显式结束或超时 FROZEN 不参与优化 手动冻结 DELETED 资源回收 无依赖约束后 -
设计哲学：
- 逻辑隔离：不同轨迹数据独立存储
- 物理关联：通过约束图实现跨轨迹优化
- 资源可控：支持大规模场景下的渐进式建图
Cartographer 的轨迹机制为 长期建图、多机器人系统 和 大场景SLAM 提供了基础架构支撑，通过合理的分段策略和资源管理，可支持从室内场景到城市级地图的构建需求。
