📚 课程目录第2章加速度计数据融合姿态估计

2. 加速度计数据融合姿态估计

任务说明

本案例实现运动相机EIS中的基础姿态估计模块：融合三轴陀螺仪角速度与三轴加速度计测量值，通过互补滤波在频域上结合两者优势——陀螺仪短时精度高但存在积分漂移，加速度计长期稳定但易受运动加速度干扰——输出相机当前姿态的欧拉角（横滚角φ、俯仰角θ）。该输出将作为后续防抖算法中坐标系变换与画面补偿的输入。

核心算法与实现

互补滤波的核心思想是：姿态角由陀螺仪积分得到高频分量，由加速度计静态解算得到低频分量，两者按权重系数α融合。α由滤波时间常数τ与采样周期dt决定：α = τ / (τ + dt)。本实现采用旋转矩阵形式的互补滤波，避免欧拉角奇异性问题，计算流程如下：

由加速度计读数归一化后，通过叉积计算当前姿态与重力参考方向之间的误差；
将误差经比例增益Kp补偿到陀螺仪角速度上，修正积分漂移；
对修正后的角速度进行四元数积分更新；
将四元数归一化后转换为欧拉角输出。

代码实现


#include <math.h>
#include <stdio.h>

typedef struct {
    float w, x, y, z;  // 四元数
} Quat;

typedef struct {
    float phi, theta;  // 横滚角、俯仰角（弧度）
} Euler;

// 采样周期与滤波参数
#define DT      0.01f   // 100Hz采样
#define TAU     0.5f    // 滤波时间常数
#define Kp      2.0f    // 比例增益

static Quat q = {1.0f, 0.0f, 0.0f, 0.0f};

// 四元数归一化
static void quat_normalize(Quat *q) {
    float n = sqrtf(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z);
    if (n < 1e-6f) { q->w = 1.0f; return; }
    q->w /= n; q->x /= n; q->y /= n; q->z /= n;
}

// 互补滤波单步更新
Euler complementary_filter(float gx, float gy, float gz,
                           float ax, float ay, float az) {
    // 1. 加速度计归一化
    float a_norm = sqrtf(ax*ax + ay*ay + az*az);
    if (a_norm < 1e-6f) a_norm = 1.0f;
    ax /= a_norm; ay /= a_norm; az /= a_norm;

    // 2. 由当前四元数计算重力参考方向（旋转矩阵第三行）
    float gx_ref = 2.0f*(q.x*q.z - q.w*q.y);
    float gy_ref = 2.0f*(q.y*q.z + q.w*q.x);
    float gz_ref = q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z;

    // 3. 叉积计算误差
    float ex = ay*gz_ref - az*gy_ref;
    float ey = az*gx_ref - ax*gz_ref;
    float ez = ax*gy_ref - ay*gx_ref;

    // 4. 比例补偿到角速度
    gx += Kp * ex;
    gy += Kp * ey;
    gz += Kp * ez;

    // 5. 四元数积分（一阶龙格-库塔）
    Quat dq;
    dq.w = 0.5f * (-q.x*gx - q.y*gy - q.z*gz) * DT;
    dq.x = 0.5f * ( q.w*gx + q.y*gz - q.z*gy) * DT;
    dq.y = 0.5f * ( q.w*gy - q.x*gz + q.z*gx) * DT;
    dq.z = 0.5f * ( q.w*gz + q.x*gy - q.y*gx) * DT;

    q.w += dq.w; q.x += dq.x; q.y += dq.y; q.z += dq.z;
    quat_normalize(&q);

    // 6. 转换为欧拉角（ZYX顺序）
    Euler e;
    float sinr = 2.0f*(q.w*q.x + q.y*q.z);
    float cosr = 1.0f - 2.0f*(q.x*q.x + q.y*q.y);
    e.phi = atan2f(sinr, cosr);
    float sinp = 2.0f*(q.w*q.y - q.z*q.x);
    if (fabsf(sinp) >= 1.0f)
        e.theta = copysignf(M_PI/2.0f, sinp);
    else
        e.theta = asinf(sinp);
    return e;
}

// 测试主函数
int main() {
    // 模拟静止状态：陀螺仪零角速度，加速度计仅受重力
    float gx=0.0f, gy=0.0f, gz=0.0f;
    float ax=0.0f, ay=0.0f, az=9.81f;
    Euler e;
    for (int i = 0; i < 100; i++) {
        e = complementary_filter(gx, gy, gz, ax, ay, az);
    }
    printf("静止姿态: phi=%.4f rad, theta=%.4f rad\n", e.phi, e.theta);

    // 模拟绕X轴旋转：角速度0.5 rad/s，持续2秒
    // 加速度计仍近似指向重力（忽略运动加速度）
    gx = 0.5f; gy = 0.0f; gz = 0.0f;
    ax = 0.0f; ay = 0.0f; az = 9.81f;
    for (int i = 0; i < 200; i++) {
        e = complementary_filter(gx, gy, gz, ax, ay, az);
    }
    printf("旋转2秒后: phi=%.4f rad (理论约1.0 rad)\n", e.phi);
    return 0;
}

编译与运行


gcc -o attitude_filter attitude_filter.c -lm
./attitude_filter

验证方法

运行程序后观察输出：

静止测试：前100次迭代后，phi和theta应接近0.0 rad（误差小于0.01 rad），证明滤波器无漂移且能收敛到水平姿态；
旋转测试：以0.5 rad/s绕X轴旋转2秒，理论横滚角变化为1.0 rad。输出phi应接近1.0 rad（允许0.05 rad误差），证明陀螺仪积分路径正确且加速度计修正未过度干扰动态响应。

判定标准：两项测试输出均在上述容差范围内，即验证互补滤波实现正确。若静止测试出现明显漂移，检查Kp是否过小；若旋转测试角度偏差过大，检查四元数积分符号或DT设置。