基于ROS2与EKF的四轮差速机器人里程计精度优化：解决建图漂移与重影问题

摘要 (Abstract)

在移动机器人（特别是四轮差速/滑移转向机器人 Skid-Steer）的建图过程中，单纯依赖编码器会导致严重的航向角漂移，而单纯依赖IMU角速度积分又面临零偏累积问题。本文介绍了一种基于扩展卡尔曼滤波（EKF）的融合方案。通过引入IMU绝对航向角观测 、滑移系数校正 以及速度低通滤波，成功解决了 Gmapping/Cartographer 建图时的"地图模糊"和"转向不足"问题。

---

1. 问题背景 (The Challenge)

在开发四轮差速底盘（如麦克纳姆轮或普通四轮差速）时，我们常遇到以下核心痛点：

1. 里程计转向不准：由于轮胎与地面的横向滑移，物理轮距不等于运动学有效轮距。通常表现为机器人实地转了 90 度，但 Rviz 里只显示转了 60 度。
2. 地图模糊（重影）：编码器计算出的瞬时速度噪声很大，导致里程计数据高频抖动。在构建栅格地图时，激光雷达的扫描帧无法精准拼接，导致墙壁变厚或出现重影。
3. 长时间运行漂移：仅靠编码器或单纯的 IMU 角速度积分，时间久了航向角（Yaw）会发散，导致在大场景建图时"回环"无法闭合。

---

2. EKF 系统建模 (System Modeling)

为了解决上述问题，我们设计了一个 5 维状态量的 EKF 系统。

2.1 状态向量 (State Vector)

X=[x,y,θ,v,ω]T X = [x, y, \theta, v, \omega]^T X=[x,y,θ,v,ω]T

其中：

• x,yx, yx,y: 机器人在世界坐标系下的位置
• θ\thetaθ: 航向角
• vvv: 线速度
• ω\omegaω: 角速度

2.2 运动方程 (预测模型)

我们使用标准的差速运动学模型作为预测步骤：

{xk=xk−1+vk−1cos⁡(θk−1)Δtyk=yk−1+vk−1sin⁡(θk−1)Δtθk=θk−1+ωk−1Δt \begin{cases} x_{k} = x_{k-1} + v_{k-1} \cos(\theta_{k-1}) \Delta t \\ y_{k} = y_{k-1} + v_{k-1} \sin(\theta_{k-1}) \Delta t \\ \theta_{k} = \theta_{k-1} + \omega_{k-1} \Delta t \end{cases} ⎩ ⎨ ⎧xk=xk−1+vk−1cos(θk−1)Δtyk=yk−1+vk−1sin(θk−1)Δtθk=θk−1+ωk−1Δt

注意 ：对于差速车，由于打滑严重，预测模型中的 θ\thetaθ 往往非常不准。这需要依靠观测步骤（Correction Step）来强行修正。

---

3. 关键优化策略 (Key Optimizations)

这是本文的核心部分，针对建图稳定性做了三个关键改进。

3.1 引入 IMU 航向角直接观测 (Direct Yaw Fusion)

传统的 EKF 往往只融合 IMU 的角速度 (ω\omegaω)。但在低速高摩擦场景下，积分误差依然存在。

改进方案：

我们在 EKF 外部预先对 IMU 的角速度进行积分，得到相对的 Yaw 角，并将其直接作为观测变量输入 EKF。

• 观测矩阵 HHH：

H=[0010000001] H = \begin{bmatrix} 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} H=[0000100001]

这意味着我们直接观测状态量中的 [θ,ω][\theta, \omega][θ,ω]。

• 效果：一旦里程计预测的角度发生漂移，高置信度的 IMU 观测值会立即将角度"拉回"正确值。

3.2 滑移系数校正 (Slip Factor Correction)

对于 Skid-Steer 机器人，转向时轮胎必须克服侧向摩擦力，导致"有效轮距"比"物理轮距"大。

代码实现：

# 物理轮距 0.17m，但在地毯上滑移严重，实际有效轮距可能放大到 1.6~2.0 倍
self.effective_track = self.wheel_track * self.skid_steer_slip_factor 

# 计算角速度时使用有效轮距
raw_omega = (v_r - v_l) / self.effective_track

操作建议：调整该系数，直到机器人实地旋转 360 度与 Rviz 显示完全一致。通常取值在 1.5 ~ 2.5 之间。

3.3 速度低通滤波 (Low-Pass Filter)

为了解决地图模糊问题，必须平滑编码器的噪声。原始的 delta_pulse / dt 计算出的速度极其抖动，直接用于建图会导致激光雷达数据在地图上"跳跃"。

算法：

vsmooth=α⋅vraw+(1−α)⋅vlast v_{smooth} = \alpha \cdot v_{raw} + (1 - \alpha) \cdot v_{last} vsmooth=α⋅vraw+(1−α)⋅vlast

参数选择 ：

我们在代码中设置 α=0.6\alpha = 0.6α=0.6。

• α\alphaα 过大：平滑效果差，地图依然模糊。
• α\alphaα 过小：延迟过高，机器人停下了，里程计还在走。
• 0.6 是一个经验值，有效过滤了机械震动引起的高频噪声，让 Gmapping 的粒子滤波器更容易收敛。

---

4. 噪声协方差矩阵调优 (Covariance Tuning)

EKF 的性能本质上取决于我们"信谁"。对于四轮差速车，物理特性决定了我们应极度信任 IMU 的角度，而不信任轮子的转向。

• 过程噪声 Q (Process Noise) ：

  我们大幅增大了 θ\thetaθ 的过程噪声，这在数学上等同于告诉 EKF："预测模型算出的角度大概率是错的，请忽略它，多听观测值的"。

  self.Q = np.diag([
      1e-4,   # x
      1e-4,   # y
      0.05,   # theta (显著增大，信任观测)
      1e-2,   # v
      0.05    # omega
  ])

• 观测噪声 R (Measurement Noise)：

  • 编码器 ：信任线速度 vvv，极不信任 角速度 ω\omegaω（因为会打滑）。
  • IMU ：极度信任 航向角 θ\thetaθ 和角速度 ω\omegaω。

  self.R_enc = np.diag([0.02, 0.5])   # 编码器角速度噪声设大 (0.5)，表示很不准
  self.R_imu = np.diag([0.005, 0.01]) # IMU 噪声设极小，表示非常准

---

5. 实际建图效果对比 (Results)

优化前 (Before Optimization)

• 现象：原地旋转时，激光雷达扫描的墙壁轮廓会随着旋转发生偏移，无法重合。
• 建图：走廊变弯，直角转弯变成钝角，构建的地图边界模糊不清（俗称"双眼皮"现象）。
• 原因：编码器速度抖动导致位姿跳变，且转向角度滞后。

优化后 (After Optimization)

• 现象：无论底盘如何剧烈晃动或旋转，Rviz 中的 TF 坐标系始终紧跟真实朝向。
• 建图：墙壁线条锐利，直角分明，闭环检测成功率大幅提升，地图不再出现重影。
• 原因：IMU 锁住了航向角，低通滤波消除了位置抖动。

---

6. 核心代码片段 (Core Code Implementation)

以下是实现上述逻辑的关键 Python 代码片段。

6.1 低通滤波与编码器解析

    def parse_encoder(self, data):
        # ... (省略数据解析部分) ...
        
        # 1. 计算原始速度
        pulses_per_sec = d_pulses / dt
        wheel_v = pulses_per_sec * self.distance_per_pulse
        
        # 2. 差速运动学模型 (计算左右轮速度)
        v_l = (wheel_v[0] + wheel_v[1]) / 2.0
        v_r = (wheel_v[2] + wheel_v[3]) / 2.0
        
        raw_v = (v_l + v_r) / 2.0
        # 使用有效轮距计算角速度
        raw_omega = (v_r - v_l) / self.effective_track

        # 3. 【关键优化】低通滤波 (Low Pass Filter)
        # alpha = 0.6, 平滑速度，消除地图抖动
        smooth_v = self.lpf_alpha * raw_v + (1.0 - self.lpf_alpha) * self.last_v_enc
        smooth_omega = self.lpf_alpha * raw_omega + (1.0 - self.lpf_alpha) * self.last_omega_enc
        
        self.last_v_enc = smooth_v
        self.last_omega_enc = smooth_omega

        # 存入 Buffer 供 EKF 使用
        self.encoder_buffer.append({
            'v': smooth_v,
            'omega': smooth_omega,
            # ...
        })

6.2 融合 IMU 航向角的 EKF 更新

    def ekf_update_imu(self, yaw, omega):
        # 观测矩阵 H: 观测 [theta, omega] - 强行融合角度
        H = np.zeros((2, 5))
        H[0,2] = 1.0 # 观测 theta
        H[1,4] = 1.0 # 观测 omega
        
        z = np.array([yaw, omega])
        z_pred = np.array([self.state[2], self.state[4]])
        
        y = z - z_pred
        
        # 【关键】处理角度残差的跨越问题 (如 3.14 -> -3.14)
        # 必须确保误差在 -pi 到 pi 之间，否则滤波器会发散
        y[0] = self.normalize_angle(y[0])
        
        # 标准 EKF 更新步骤
        S = H @ self.P @ H.T + self.R_imu
        K = self.P @ H.T @ np.linalg.inv(S)
        self.state += K @ y
        self.P = (np.eye(5) - K @ H) @ self.P
        
        # 归一化状态中的角度
        self.state[2] = self.normalize_angle(self.state[2])

---

7. 总结 (Conclusion)

通过 ROS2 与 Python 实现的这套 EKF 系统，证明了对于低成本差速底盘：

1. 模型不如传感器：差速运动学模型在滑移严重时非常不准，必须依赖高频 IMU 对航向角进行直接修正。
2. 数据预处理至关重要：简单的低通滤波（LPF）就能极大提升建图的清晰度。
3. 观测维度的选择：直接观测 Yaw 角比单纯观测角速度能更有效地抑制漂移。

这套方案不仅适用于四轮差速车，同样适用于履带车等高滑移率的移动机器人平台。