两轮差速小车C语言里程计实现与传感器融合

1. 项目概述：两轮差速小车的纯C语言里程计实现

在移动机器人开发领域，两轮差速驱动平台是最基础也是最常见的机械结构之一。这类机器人通常需要实时估计自身在二维平面内的位置(x,y)和航向角(θ)，这就是所谓的里程计(Odometry)功能。ROS生态中的robot_pose_ekf包提供了成熟的解决方案，但其依赖ROS框架和BFL库，难以直接移植到资源受限的嵌入式平台。

本项目实现了一个完全用C99标准编写的二维里程计库，核心特点包括：

• 零第三方依赖：仅使用标准数学库(math.h)和字符串库(string.h)
• 传感器针对性适配：专为TLE5012编码器、MPU6500 IMU和IST8310磁力计优化
• 双层传感器融合：Mahony算法融合IMU与磁力计数据，EKF融合轮式里程计与姿态数据
• 嵌入式友好：全程使用float单精度浮点运算，避免double带来的性能损耗

这个实现特别适合以下场景：

1. 需要脱离ROS环境运行的嵌入式系统
2. 资源受限的实时操作系统(RTOS)环境
3. 对第三方库依赖有严格限制的项目
4. 需要高度定制化传感器融合算法的场合

2. 算法架构解析

2.1 从ROS到裸机的算法迁移

原版robot_pose_ekf采用6维状态向量(x,y,z,roll,pitch,yaw)，而我们的实现做了两个关键简化：

1. 降维处理：仅保留平面运动的3个自由度(x,y,θ)
2. 解耦姿态融合：将IMU和磁力计的数据融合与位姿估计分离

状态预测模型采用经典的差速运动学模型：

code复制x_k+1 = x_k + cos(θ_k) * v * Δt
y_k+1 = y_k + sin(θ_k) * v * Δt 
θ_k+1 = θ_k + ω * Δt

其中v和ω分别表示车体坐标系下的线速度和角速度。

2.2 传感器数据处理流程

整个系统的数据流遵循以下顺序：

1. 编码器原始数据 → 轮速计算 → 轮式里程计
2. IMU+磁力计 → Mahony滤波 → 三维姿态
3. 轮式里程计 + 姿态航向 → EKF融合 → 最终位姿

这种分层处理有三大优势：

• 模块化程度高，各环节可独立调试
• 计算负载分散，避免单一模块过载
• 传感器故障时可部分降级运行

3. 核心模块实现细节

3.1 差速运动学计算

差速驱动模型的核心是将左右轮的旋转运动转换为车体的平移和旋转。关键计算公式如下：

code复制// 轮子圆周运动弧长计算
s_left = wheel_radius * (current_left_angle - last_left_angle)
s_right = wheel_radius * (current_right_angle - last_right_angle)

// 车体位移和转向
ds = (s_left + s_right) / 2
dθ = (s_right - s_left) / (2 * wheel_base)

// 世界坐标系下的位移增量
dx = ds * cos(θ + dθ/2)
dy = ds * sin(θ + dθ/2)

这里采用中点角度(θ + dθ/2)来计算位移增量，比直接用θ更精确，特别是当dθ较大时。

3.2 扩展卡尔曼滤波实现

EKF的实现包含以下几个关键部分：

状态向量：

c复制typedef struct {
    float x;     // 世界坐标系x位置(m)
    float y;     // 世界坐标系y位置(m) 
    float theta; // 航向角(rad)
} odom_ekf_state_t;

协方差矩阵：
使用3x3矩阵表示状态变量的不确定性：

code复制P = [ σ_x²    σ_xy   σ_xθ
      σ_yx    σ_y²   σ_yθ
      σ_θx    σ_θy   σ_θ² ]

预测步骤：

c复制void odom_ekf_predict(odom_ekf_t *ekf, float v, float omega, float dt)
{
    // 状态预测
    ekf->state.x += cosf(ekf->state.theta) * v * dt;
    ekf->state.y += sinf(ekf->state.theta) * v * dt;
    ekf->state.theta += omega * dt;
    
    // 协方差预测 (F是状态转移雅可比矩阵)
    // P = F * P * F^T + Q
    mat3f_mult_f(ekf->F, ekf->P, ekf->temp33_1);
    mat3f_mult_tf(ekf->temp33_1, ekf->F, ekf->temp33_2);
    mat3f_add(ekf->temp33_2, ekf->Q, ekf->P);
}

测量更新：
针对轮式里程计和姿态测量分别实现更新函数，核心是计算卡尔曼增益K并更新状态和协方差：

c复制// 轮式里程计更新
void odom_ekf_update_odom(odom_ekf_t *ekf, const odom_ekf_odom_meas_t *z);

// 姿态测量更新 
void odom_ekf_update_attitude(odom_ekf_t *ekf, const odom_ekf_attitude_meas_t *z);

3.3 Mahony姿态滤波实现

Mahony滤波是一种轻量级的互补滤波算法，相比更复杂的Kalman滤波实现，它在保持不错精度的同时计算量小得多。核心更新步骤如下：

c复制void mahony_update(mahony_t *m, float