一，四元数

        1.四元数基础结构体定义    

typedef struct
{
   float q0; //实部，旋转角度分量
   float q1; //q1,q2,q3 是虚部，旋转轴x/y/z分量
   float q2;
   float q3;
}Quaternion;

        2.四元数归一化函数

#include <math.h>

void quat_normalize(Quaternnion *qt)
{
   //计算模长
   float norm = sqrtf(qt->q0*qt->q0 + qt->q1*qt->q1 + qt->q2*qt->q2 + qt->q3*qt->q3);

   //防止模长为零
   if(norm < 1e-6f)return;
   
   //归一化
   qt->q0 /= norm;
   qt->q1 /= norm;
   qt->q2 /= norm;
   qt->q3 /= norm;

}

        3.四元数乘法函数

                ①四元数乘法规则：q(new) = q1⊗q2, 代表先执行q2旋转，再q1旋转

                

Quaternion quat_mul(Quaternion q1, Quaternion q2)
{
   Quaternion res;
   res.q0 = q1.q0*q2.q0 - q1.q1*q2.q1 - q1.q2*q2.q2 - q1.q3*q2.a3;
   res.q1 = q1.q0*q2.q1 + q1.q1*q2.q0 + q1.q2*q2.q3 - q1.q3*q2.q2;
   res.q2 = q1.q0*q2.q2 - q1.q1*q2.q3 + q1.q2*q2.q0 + q1.q3*q2.q1;
   res.q3 = q1.q0*q2.q3 + q1.q1*q2.q2 - q1.q2*q2.q1 + q1.q3*q2.q0;
   return res;
}

               ②姿态更新公式 ：q(new) = q  +  0.5 * q ⊗ [0, gx, gy, gz] * dt

二，四元数 <---->欧拉角（roll, pitch, yaw）

        ①四元数——>欧拉角

                设四元数 q = [w, x, y, z]

                roll = arctan2( 2(wx + yz) , 1 - 2(x*x + y*y))

                pitch = arcsin( 2(wy-zx))

                yaw = arctan2( 2(wz+xy) , 1 - 2(y*y + z*z))

        ②欧拉角——>四元数

                r = roll/2,  p = pitch/2,  y = yaw/2

                w = cosr*cosp*cosy + sinr*sinp*siny 

                x = sinr*cosp*cosy - cosr*sinp*siny

                y = cosr*sinp*cosy + sinr*cosp*siny

                z = cosr*cosp*siny - sinr*sinp*cosy

                技巧：两边对称用加号，不对称用减号

三，互补滤波代码解析

        ①宏定义

#define GYRO_SENS_250        131.0f
#define ACC_SENS             16384.0f // ±2g量程 LSB/g
#define PI                   3.1415926535f
// 互补滤波基础系数 0.95~0.99 陀螺权重越大动态越好漂移越大
#define ALPHA_COMPLEMENT     0.98f

        ②全局变量

Quaternion q_att = {1.0f, 0.0f, 0.0f, 0.0f};
uint32_t tick_last = 0;
int16_t gx_off, gy_off, gz_off;

        ③获取欧拉角函数

void quat_to_euler(Quaternion q, float *roll, float *pitch, float *yaw)
{
    // Roll X
    float sinR = 2.0f * (q.q0 * q.q1 + q.q2 * q.q3);
    float cosR = 1.0f - 2.0f * (q.q1 * q.q1 + q.q2 * q.q2);
    *roll = atan2f(sinR, cosR);

    // Pitch Y
    float sinP = 2.0f * (q.q0 * q.q2 - q.q3 * q.q1);
    if(fabsf(sinP) >= 1.0f)
        *pitch = copysignf(PI / 2.0f, sinP);
    else
        *pitch = asinf(sinP);

    // Yaw Z 加速度计无法修正航向，仅陀螺积分
    float sinY = 2.0f * (q.q0 * q.q3 + q.q1 * q.q2);
    float cosY = 1.0f - 2.0f * (q.q2 * q.q2 + q.q3 * q.q3);
    *yaw = atan2f(sinY, cosY);
}

        ④互补滤波

函数定义：

void complementary_filterm(float ax, float ay, float az, float raw_norm, float dt)

函数体：

Ⅰ：计算出加速度可信度

    // 1.  防止除0
    if(raw_norm < 0.01f) return;
    
    // 2. 归一化加速度矢量
    ax /= raw_norm;
    ay /= raw_norm;
    az /= raw_norm;

    // 3. 根据重力偏离1g程度计算加速度可信度
    float acc_trust = 1.0f - fabsf(raw_norm - 1.0f);
    if(acc_trust < 0.0f) acc_trust = 0.0f;
    if(acc_trust > 1.0f) acc_trust = 1.0f;

Ⅱ：

1. norm 越接近 1g → 设备静止，加速度可信度高，alpha减小，多靠加速度修正漂移
2. norm 远离 1g → 剧烈运动，加速度不可信，alpha 拉高，几乎只用陀螺仪

   // 动态alpha：运动时陀螺权重升高，静止时加速度权重升高
   float alpha = ALPHA_COMPLEMENT + (1.0f - ALPHA_COMPLEMENT) * (1.0f - acc_trust);

Ⅲ：计算融合后的角度

    // 1. 由当前四元数获取陀螺积分欧拉角
    float q_roll, q_pitch, q_yaw;
    quat_to_euler(q_att, &q_roll, &q_pitch, &q_yaw);

    // 2.  加速度计解算静态倾角
    float acc_roll  = atan2f(ay, az);
    float acc_pitch = atan2f(-ax, sqrtf(ay*ay + az*az));

    // 3. 互补融合角度
    float fuse_roll  = alpha * q_roll  + (1.0f - alpha) * acc_roll;
    float fuse_pitch = alpha * q_pitch + (1.0f - alpha) * acc_pitch;

Ⅳ：限制单次最大修正10°，防止剧烈跳变

    float roll_diff = fuse_roll - q_roll;
    float pitch_diff = fuse_pitch - q_pitch;
    const float max_correction = 10.0f * PI / 180.0f;
    if(roll_diff > max_correction) roll_diff = max_correction;
    if(roll_diff < -max_correction) roll_diff = -max_correction;
    if(pitch_diff > max_correction) pitch_diff = max_correction;
    if(pitch_diff < -max_correction) pitch_diff = -max_correction;
    fuse_roll = q_roll + roll_diff;
    fuse_pitch = q_pitch + pitch_diff;

Ⅴ：融合欧拉角转回四元数更新姿态（Yaw保留陀螺积分值）

    float cr = cosf(fuse_roll * 0.5f);
    float sr = sinf(fuse_roll * 0.5f);
    float cp = cosf(fuse_pitch * 0.5f);
    float sp = sinf(fuse_pitch * 0.5f);
    float cy = cosf(q_yaw * 0.5f);
    float sy = sinf(q_yaw * 0.5f);

    q_att.q0 = cr * cp * cy + sr * sp * sy;
    q_att.q1 = sr * cp * cy - cr * sp * sy;
    q_att.q2 = cr * sp * cy + sr * cp * sy;
    q_att.q3 = cr * cp * sy - sr * sp * cy;

    quat_normalize(&q_att);

四，总结

本篇内容主要是针对对互补滤波不太理解的网友们，希望能给大家带来帮助。        

完整 CubeMX 工程、全部驱动源码已整理，仓库地址：https://gitee.com/iiddoo/flight-control-embedded-system，下载后打开.ioc 文件重新生成工程即可直接编译运行。