Python算法
基于 ROS2 与 EKF 的四轮差速机器人里程计精度优化：解决建图漂移与重影问题
对四轮差速机器人在建图中因滑移导致的里程计漂移和地图模糊问题，提出了一种基于扩展卡尔曼滤波（EKF）的融合方案。通过引入 IMU 绝对航向角观测、滑移系数校正以及速度低通滤波，有效修正了航向角发散和瞬时速度噪声。实验表明，该方案显著提升了 Gmapping/Cartographer 建图的清晰度与闭环检测成功率，解决了墙壁变厚和重影现象。
对四轮差速机器人在建图中因滑移导致的里程计漂移和地图模糊问题，提出了一种基于扩展卡尔曼滤波（EKF）的融合方案。通过引入 IMU 绝对航向角观测、滑移系数校正以及速度低通滤波，有效修正了航向角发散和瞬时速度噪声。实验表明，该方案显著提升了 Gmapping/Cartographer 建图的清晰度与闭环检测成功率，解决了墙壁变厚和重影现象。
在移动机器人（特别是四轮差速/滑移转向机器人 Skid-Steer）的建图过程中，单纯依赖编码器会导致严重的航向角漂移，而单纯依赖 IMU 角速度积分又面临零偏累积问题。本文介绍了一种基于扩展卡尔曼滤波（EKF）的融合方案。通过引入 IMU 绝对航向角观测、滑移系数校正以及速度低通滤波，成功解决了 Gmapping/Cartographer 建图时的'地图模糊'和'转向不足'问题。
在开发四轮差速底盘（如麦克纳姆轮或普通四轮差速）时，我们常遇到以下核心痛点：
为了解决上述问题，我们设计了一个 5 维状态量的 EKF 系统。
$$ X = [x, y, \theta, v, \omega]^T $$
其中：
我们使用标准的差速运动学模型作为预测步骤：
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）来强行修正。
这是本文的核心部分，针对建图稳定性做了三个关键改进。
传统的 EKF 往往只融合 IMU 的角速度 (ω)。但在低速高摩擦场景下，积分误差依然存在。
改进方案： 我们在 EKF 外部预先对 IMU 的角速度进行积分，得到相对的 Yaw 角，并将其直接作为观测变量输入 EKF。
$$ H = [[0, 0, 1, 0, 0], [0, 0, 0, 0, 1]] $$
这意味着我们直接观测状态量中的 [θ, ω]。
对于 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 之间。
为了解决地图模糊问题，必须平滑编码器的噪声。原始的 delta_pulse / dt 计算出的速度极其抖动，直接用于建图会导致激光雷达数据在地图上'跳跃'。
算法：
$$ v_{smooth} = \alpha \cdot v_{raw} + (1 - \alpha) \cdot v_{last} $$
参数选择： 我们在代码中设置 α=0.6。
EKF 的性能本质上取决于我们'信谁'。对于四轮差速车，物理特性决定了我们应极度信任 IMU 的角度，而不信任轮子的转向。
观测噪声 R (Measurement Noise)：
self.R_enc = np.diag([0.02, 0.5]) # 编码器角速度噪声设大 (0.5)，表示很不准
self.R_imu = np.diag([0.005, 0.01]) # IMU 噪声设极小，表示非常准
过程噪声 Q (Process Noise)： 我们大幅增大了 θ 的过程噪声，这在数学上等同于告诉 EKF：'预测模型算出的角度大概率是错的，请忽略它，多听观测值的'。
self.Q = np.diag([
1e-4, # x
1e-4, # y
0.05, # theta (显著增大，信任观测)
1e-2, # v
0.05 # omega
])
以下是实现上述逻辑的关键 Python 代码片段。
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, ...})
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])
通过 ROS2 与 Python 实现的这套 EKF 系统，证明了对于低成本差速底盘：
这套方案不仅适用于四轮差速车，同样适用于履带车等高滑移率的移动机器人平台。
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
解析常见 curl 参数并生成 fetch、axios、PHP curl 或 Python requests 示例代码。 在线工具，curl 转代码在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML 转 Markdown 互为补充。 在线工具，Markdown 转 HTML在线工具，online
将 HTML 片段转为 GitHub Flavored Markdown，支持标题、列表、链接、代码块与表格等；浏览器内处理，可链接预填。 在线工具，HTML 转 Markdown在线工具，online