摘要
在移动机器人（特别是四轮差速/滑移转向机器人 Skid-Steer）的建图过程中，单纯依赖编码器会导致严重的航向角漂移，而单纯依赖 IMU 角速度积分又面临零偏累积问题。本文介绍了一种基于扩展卡尔曼滤波（EKF）的融合方案。通过引入 IMU 绝对航向角观测、滑移系数校正以及速度低通滤波，成功解决了 Gmapping/Cartographer 建图时的'地图模糊'和'转向不足'问题。
1. 问题背景
在开发四轮差速底盘（如麦克纳姆轮或普通四轮差速）时，我们常遇到以下核心痛点：
- 里程计转向不准：由于轮胎与地面的横向滑移，物理轮距不等于运动学有效轮距。通常表现为机器人实地转了 90 度，但 Rviz 里只显示转了 60 度。
- 地图模糊（重影）：编码器计算出的瞬时速度噪声很大，导致里程计数据高频抖动。在构建栅格地图时，激光雷达的扫描帧无法精准拼接，导致墙壁变厚或出现重影。
- 长时间运行漂移：仅靠编码器或单纯的 IMU 角速度积分，时间久了航向角（Yaw）会发散，导致在大场景建图时'回环'无法闭合。
2. EKF 系统建模
为了解决上述问题，我们设计了一个 5 维状态量的 EKF 系统。
2.1 状态向量
$$ X = [x, y, \theta, v, \omega]^T $$
其中：
- x, y: 机器人在世界坐标系下的位置
- θ: 航向角
- v: 线速度
- ω: 角速度
2.2 运动方程（预测模型）
我们使用标准的差速运动学模型作为预测步骤：
x_k = x_{k-1} + v_{k-1} * cos(theta_{k-1}) * dt
y_k = y_{k-1} + v_{k-1} * sin(theta_{k-1}) * dt
theta_k = theta_{k-1} + omega_{k-1} * dt
注意：对于差速车，由于打滑严重，预测模型中的 θ 往往非常不准。这需要依靠观测步骤（Correction Step）来强行修正。
3. 关键优化策略
这是本文的核心部分，针对建图稳定性做了三个关键改进。
3.1 引入 IMU 航向角直接观测
传统的 EKF 往往只融合 IMU 的角速度 (ω)。但在低速高摩擦场景下，积分误差依然存在。
改进方案： 我们在 EKF 外部预先对 IMU 的角速度进行积分，得到相对的 Yaw 角，并将其直接作为观测变量输入 EKF。
- 观测矩阵 H：
$$ H = [[0, 0, 1, 0, 0], [0, 0, 0, 0, 1]] $$
这意味着我们直接观测状态量中的 [θ, ω]。
- 效果：一旦里程计预测的角度发生漂移，高置信度的 IMU 观测值会立即将角度'拉回'正确值。
3.2 滑移系数校正
对于 Skid-Steer 机器人，转向时轮胎必须克服侧向摩擦力，导致'有效轮距'比'物理轮距'大。
代码实现：
# 物理轮距 0.17m，但在地毯上滑移严重，实际有效轮距可能放大到 1.6~2.0 倍
.effective_track = .wheel_track * .skid_steer_slip_factor
raw_omega = (v_r - v_l) / .effective_track