4、定位模块排查：定位漂移、重定位跳变、SLAM累积误差、全局与局部坐标转换错误

定位模块出问题，轨迹抖动几乎是必然的。我见过太多团队花几个月调控制参数，最后发现根子在定位上——传感器数据都错了，你控制再稳也没用。

这一章我们聚焦四个核心问题：定位漂移、重定位跳变、SLAM累积误差、坐标转换错误。每个问题我都会结合项目中的真实案例来讲。

4.1 定位漂移：传感器在「说谎」

定位漂移是最常见的抖动来源。说白了，就是机器人明明停着，定位却慢慢跑偏了。或者直线行驶时，轨迹像喝醉了一样左右晃。

典型表现：

• 静止时位置缓慢变化（GPS漂移或IMU零偏）
• 直线行驶轨迹呈S形（航向角漂移）
• 长时间运行后位置误差越来越大（累积漂移）

我在项目中遇到过最坑的一次：AGV在仓库里跑，定位每10分钟偏1米。查了三天，最后发现是IMU的陀螺零偏没有在线校准。温度一变，零偏就飘了。

常见原因与修复：

原因	现象	修复方法
GPS多路径效应	位置在开阔区正常，靠近建筑时抖动	增加多路径检测，降低异常卫星权重
IMU零偏未校准	静止时航向缓慢旋转	启动时做静态零偏校准，运行时在线估计
融合权重不合理	GPS信号差时位置突然跳变	根据卫星数/HDOP动态调整融合权重
磁力计干扰	航向在特定区域异常	使用陀螺积分+磁力计修正，避免纯磁力计航向

4.2 重定位跳变：位置「瞬移」了

重定位跳变比漂移更吓人——轨迹上突然出现一个台阶，位置瞬间跳了几米甚至几十米。这在自动驾驶里是致命问题。

为什么会这样？说白了，就是定位系统在「重新算」位置时，新旧结果不一致，又没有做平滑过渡。

我个人的排查经验：

1. 先看时间戳——重定位触发时，新旧位姿的时间戳是否连续？我曾经发现过时间戳回跳了0.5秒，导致位置直接跳回原点。
2. 再看置信度——重定位模块输出的协方差矩阵是否合理？如果协方差很小但实际误差很大，说明观测模型有问题。
3. 最后看回环检测——视觉SLAM的回环检测如果匹配错了，位置跳变是必然的。

代码示例：重定位平滑过渡

// 重定位时做线性插值，避免跳变
Eigen::Isometry3d SmoothRelocalization(
    const Eigen::Isometry3d& old_pose,
    const Eigen::Isometry3d& new_pose,
    double transition_time,   // 过渡时间，单位秒
    double current_time) {    // 当前时间戳
  
  double alpha = std::clamp(current_time / transition_time, 0.0, 1.0);
  
  // 位置线性插值
  Eigen::Vector3d pos = old_pose.translation() * (1 - alpha) 
                      + new_pose.translation() * alpha;
  
  // 姿态球面线性插值
  Eigen::Quaterniond q_old(old_pose.rotation());
  Eigen::Quaterniond q_new(new_pose.rotation());
  Eigen::Quaterniond q = q_old.slerp(alpha, q_new);
  
  Eigen::Isometry3d result;
  result.translation() = pos;
  result.rotation() = q.toRotationMatrix();
  return result;
}

4.3 SLAM累积误差：跑得越远，偏得越离谱

SLAM的累积误差是个老生常谈的问题。纯里程计做SLAM，误差随距离线性增长；纯视觉SLAM，误差随距离超线性增长。

我记得有一次做园区无人车，跑了500米后定位偏了3米。查下来发现是轮式里程计在草地上打滑，SLAM系统以为车在直行，实际上已经偏了。

累积误差的三大来源：

• 里程计漂移——轮子打滑、IMU零偏、编码器分辨率不足
• 关键帧选择不当——关键帧太密导致计算量爆炸，太疏导致误差累积
• 回环检测失效——没有回环修正，误差只能一直累积下去

我的建议：不要指望SLAM自己搞定一切。在长距离场景下，必须引入绝对观测（GPS、路标、二维码）来定期修正。

4.4 坐标转换错误：最容易被忽视的「隐形杀手」

坐标转换错误，嗯，这个坑我踩过不止一次。明明每个传感器数据看起来都对，但轨迹就是抖。最后发现是TF树里某个坐标系的时间戳没对齐。

常见坐标转换错误：

错误类型	现象	排查方法
TF树断裂	某个坐标系突然丢失，定位跳到原点	用tf2_monitor检查TF树完整性
时间戳错位	轨迹出现周期性抖动	检查各传感器时间戳是否同步到同一时钟
全局/局部混用	规划轨迹与定位轨迹不重合	确认全局坐标系（map/odom）和局部坐标系（base_link）的转换关系
外参标定错误	传感器融合后位置偏移	使用标定工具重新标定传感器外参

我曾经踩过的坑：有一次调试激光雷达和IMU融合，定位在转弯时总是抖动。查了两天，最后发现是IMU的安装角度标定错了2度。2度看起来不大，但在10米外就是0.35米的偏差。

4.5 实战排查流程

说了这么多，实际排查时怎么下手？我总结了一个四步流程：

1. 数据回放——把bag包里的定位数据、传感器数据、控制指令全部回放出来，肉眼观察轨迹抖动的位置和模式。
2. 误差分离——把定位误差分解为高频抖动（传感器噪声）和低频漂移（累积误差）。高频看滤波参数，低频看传感器融合策略。
3. 根因定位——根据抖动模式判断是哪类问题：
   • 周期性抖动 → 坐标转换时间戳问题
   • 突发跳变 → 重定位或回环检测问题
   • 缓慢漂移 → SLAM累积误差或传感器零偏
4. 针对性修复——根据根因选择修复方案，修复后重新回放验证。