两轮差速小车二维里程计：从 ROS robot_pose_ekf 到纯 C 的 EKF + Mahony 实现

摘要

本文介绍如何基于 ROS 的 robot_pose_ekf 思路，用纯 C 语言实现一套无第三方依赖的二维里程计融合库。传感器包括两轮差速小车的 TLE5012 编码器、MPU6500 IMU 与 IST8310 磁力计；姿态部分采用 Mahony 滤波将 IMU 与磁力计先融合为三维姿态，再将其航向作为 EKF 的单一姿态观测。库内全部使用 float 运算，适合嵌入式与实时系统，并提供 CMake / Makefile 与示例程序。

关键词：EKF；里程计；两轮差速；Mahony；TLE5012；MPU6500；IST8310；C 语言；传感器融合

---

一、为什么要做这个库？

在移动机器人或两轮差速小车上，我们经常需要估计机器人在平面内的位置 (x, y) 和航向 (θ)。ROS 里经典的 robot_pose_ekf 用扩展卡尔曼滤波（EKF）融合轮式里程计、IMU 等，但依赖 BFL 库和 ROS，不利于直接移植到裸机或 RTOS 环境。

因此希望得到：

• 精简 C 实现：仅依赖 C99 和标准库（math.h、string.h），无第三方库；
• 传感器明确：TLE5012 双轮编码器 + MPU6500 IMU + IST8310 磁力计；
• 输出明确：EKF 融合后的二维平面位姿 (x, y, θ)；
• 姿态更稳：IMU 与磁力计不直接各自进 EKF，而是先经 Mahony 融合成三维姿态，再只把 yaw 喂给里程计 EKF，避免双源航向打架。

下文按「算法来源 → 整体架构 → 核心模块 → 使用方式 → 编译踩坑」展开，并给出可直接复用的工程结构。

---

二、从 robot_pose_ekf 里拿了什么？

在动手写 C 之前，先梳理了 robot_pose_ekf 的核心逻辑：

• 状态：原版是 6 维 (x, y, z, roll, pitch, yaw)，我们只做平面，故降维为 3 维 (x, y, theta)。
• 运动模型：差速/单轨模型，与 nonlinearanalyticconditionalgaussianodo 一致：
  • x += cos(θ) * v * dt
  • y += sin(θ) * v * dt
  • theta += omega * dt
    并配合雅可比矩阵更新协方差。
• 测量：轮式里程计提供 (x, y, θ)；IMU/磁力计主要提供 θ 的约束。我们选择「IMU+磁 → Mahony → 一个 θ」再进 EKF，而不是两个 θ 分别更新。

这样 EKF 部分就变成「3 维状态 + 轮式 3 维观测 + 姿态 1 维观测」的经典结构，易于在 C 里手写矩阵运算。

---

三、整体架构与目录结构

库按三层拆分，便于替换传感器或只复用其中一层：

层级	作用	头文件 / 源文件
差速运动学	左右轮角速度/角增量 → 车体 v、ω 及位姿增量	diff_drive.h/c
轮式里程计	TLE5012 左右轮角度积分 → 世界系 (x, y, θ)	wheel_odom.h/c
EKF 融合	预测 + 轮式/姿态测量更新，输出 (x, y, θ)	odom_ekf.h/c
姿态 AHRS	IMU + 磁力计 → Mahony → 四元数/欧拉角，供 EKF 取 yaw	mahony.h/c

目录结构如下：

ekf_odom/
├── include/
│   ├── odom_ekf.h      # EKF 融合 API
│   ├── diff_drive.h    # 两轮差速：编码器 → v/ω、位姿增量
│   ├── wheel_odom.h    # 轮式里程计积分（TLE5012 → x,y,θ）
│   └── mahony.h        # Mahony AHRS：IMU+磁 → 三维姿态
├── src/
│   ├── odom_ekf.c
│   ├── diff_drive.c
│   ├── wheel_odom.c
│   └── mahony.c
├── example/
│   └── main.c          # 示例：模拟数据跑通整条链路
├── CMakeLists.txt
├── Makefile
└── README.md

---

四、核心设计要点

4.1 EKF 状态与测量

• 状态向量：[x, y, theta]，单位 m、m、rad。
• 预测：输入为车体线速度 v、角速度 omega 与周期 dt，按差速模型更新状态和 3×3 协方差矩阵 P（含雅可比 F）。
• 测量更新：
  • 轮式：观测 (x, y, θ)，测量矩阵 H 为单位阵，噪声对角阵 R 可配置。
  • 姿态：仅观测 θ，由 Mahony 输出的 yaw 填入 odom_ekf_attitude_meas_t，调用 odom_ekf_update_attitude()。也可保留仅 IMU 或仅磁的接口 update_imu / update_mag 做回退。

所有浮点统一用 float，数学函数用 cosf/sinf/sqrtf/atan2f 等，以减小计算量和内存。

4.2 两轮差速与 TLE5012

• TLE5012 给出左右轮累积角度（单位 rad）。
• 每周期做差分：delta_left = left - last_left，同理右侧。
• 由轮径 r、半轮距 L 得到：
  • 弧长：s_left = r * delta_left，s_right = r * delta_right
  • 车体位移与航向增量：ds = 0.5*(s_left + s_right)，dtheta = (s_right - s_left) / (2*L)
  • 世界系位移：dx = ds*cos(θ+0.5*dθ)，dy = ds*sin(θ+0.5*dθ)
• 同时用本周期角增量与 dt 算出 v、omega，供 EKF 预测使用。

4.3 Mahony 姿态融合

• 输入：MPU6500 的加速度 (ax, ay, az)、角速度 (gx, gy, gz)，IST8310 的磁力计 (mx, my, mz)，均为机体系；以及周期 dt。
• 使用互补滤波思路：用加速度估计重力方向修正 roll/pitch，用磁力计水平分量约束 yaw，对陀螺积分做 PI 校正，四元数积分后归一化。
• 接口：mahony_init(&m, Kp, Ki)；每周期 mahony_update(..., use_mag, dt)；取航向 yaw = mahony_get_yaw(&m)，再填入 EKF 的 odom_ekf_attitude_meas_t 并调用 odom_ekf_update_attitude()。

这样 EKF 只接收「一个已经融合好的航向」，避免 IMU 与磁力计在 EKF 内互相抢权。

---

五、使用流程（按周期调用）

典型周期 10 ms，顺序如下。

1. 初始化

odom_ekf_pose_t initial_pose = { 0.0f, 0.0f, 0.0f };
odom_ekf_init(&state, NULL, &initial_pose);  // NULL 表示用默认噪声配置

wheel_odom_init(&wo, 0.05f, 0.1f, 0.0f);     // 轮径 0.05 m，半轮距 0.1 m
mahony_init(&mahony, 1.5f, 0.05f);           // Kp=1.5, Ki=0.05

2. 每周期

// 编码器 → 轮式里程计
wheel_odom_update(&wo, left_rad, right_rad, dt);

// 轮速 → EKF 预测
float v, omega;
wheel_odom_get_velocity(&wo, &v, &omega);
odom_ekf_predict(&state, v, omega, dt);

// 轮式位姿 → EKF 更新
odom_ekf_pose_t wheel_pose;
wheel_odom_get_pose(&wo, &wheel_pose);
odom_ekf_odom_meas_t odom_meas = { wheel_pose.x, wheel_pose.y, wheel_pose.theta, t };
odom_ekf_update_odom(&state, &odom_meas);

// IMU + 磁 → Mahony → 航向 → EKF 更新
mahony_update(&mahony, ax,ay,az, gx,gy,gz, mx,my,mz, 1, dt);
float yaw = mahony_get_yaw(&mahony);
odom_ekf_attitude_meas_t att_meas = { .theta = yaw, .time_s = t };
odom_ekf_update_attitude(&state, &att_meas);

// 输出融合位姿
odom_ekf_pose_t fused;
odom_ekf_get_pose(&state, &fused);
// 使用 fused.x, fused.y, fused.theta

3. 配置

若需调节过程/测量噪声，可填充 odom_ekf_config_t（如 process_std_x/y/theta、odom_std_*、attitude_std_theta）并在 odom_ekf_init 时传入，否则传 NULL 使用默认值。

---

六、编译与运行

CMake（推荐）

cd ekf_odom
mkdir build && cd build
cmake ..
cmake --build .
./ekf_odom_example

Makefile

make libekf_odom.a && make ekf_odom_example
./ekf_odom_example

示例程序用模拟的编码器与 IMU/磁数据跑通「轮式积分 → Mahony → EKF」整条链路，可直接作为对接真实传感器的模板。

---

七、编译踩坑：undefined reference to cosf/sinf/sqrtf

在 Linux 下用 CMake 构建时，若出现：

undefined reference to `cosf'
undefined reference to `sinf'
undefined reference to `sqrtf'
undefined reference to `atan2f'
...

原因是这些数学函数在 libm 中，需要显式链接。解决：在 CMakeLists.txt 里为可执行目标加上 m：

target_link_libraries(ekf_odom_example ekf_odom m)

若用自带 -lm 的 Makefile 则不会遇到该问题。

---

八、小结

• 本文基于 robot_pose_ekf 的思想，用纯 C 实现了一个无第三方依赖的二维 EKF 里程计库，传感器为 TLE5012 + MPU6500 + IST8310，姿态由 Mahony 先融合再以单一 yaw 输入 EKF。
• 库采用 float、模块化设计（差速 / 轮式 / EKF / Mahony），便于移植到嵌入式或替换传感器。
• 提供 CMake 与 Makefile、示例程序及可选的噪声配置；Linux 下记得链接 libm。

若你也在做两轮差速或小型机器人的本地定位，希望这份从 ROS 到纯 C 的落地过程能提供参考。如有问题或改进建议，欢迎在评论区交流。

---

相关标签：#EKF #里程计 #两轮差速 #Mahony #传感器融合 #C语言 #嵌入式 #TLE5012 #MPU6500 #IST8310