IMU四元数解算

前言

Warn

本文章有基础的理论推导，过程可能存在不严谨的地方，着重对四元数的实际应用做讨论此文章的观点或代码仍有瑕疵，若您有更好的解决方案或更正建议欢迎留言讨论

info

本文前部分是关于理论推导，内容比较枯燥。若只关注实际应用请跳转到后文，查阅相关代码本文的代码已通过验证，MCU为AI8051，IMU为imu660ra。角速度通过死区 + 去静态零飘抑制。实际测试静态零漂约为0.046度/秒

关于四元数的介绍和理解

四元数的产生

对于一个复数，可以写成如下形式 ,运用欧拉公式(Euler’s formula) ρe^iα = ρcos α + iρsin α 可将三角函数形式的复数转为指数形式 z = ρe^iα 对该复平面内的向量进行旋转操作，假设旋转角度为ϕ,则旋转过程如下:

该旋转为一个自由度下的变化，对于实际应用来说，一个物品在空间中的矢量有三个自由度。因此我们需要四个变量来描述角度的变化，其中一个实数变量表示旋转的角度大小，剩下三个为虚数，体现旋转轴的方向。我们将这样的四个变量记作： q = (w, x, y, z) 对于一个旋转角度为θ的四元数可表示为：

实部（w）：与旋转的角度相关，体现旋转的“量”。
虚部（x, y, z）：与旋转轴的方向相关，体现旋转的“方向”。

为了方便，我们有时将其简写为：其中

这也就是我们常说的轴角对：

旋转轴：一个单位向量，表示旋转的轴线方向。
旋转角：一个标量，表示绕该轴旋转的角度。

用四元数表示3D旋转矩阵

并不是所有的四元数都能表示三维空间中的一个旋转，只有单位四元数才能正确表示一个旋转。所谓单位四元数即： $满足$ 对一个3D向量：其旋转后的向量可表示为：

计算旋转矩阵

(1)计算q⋅p

设：则记 (2) 计算 (q⋅p)⋅q−1 $所以：展开后为：$ (3)整理成矩阵形式

将a,b,c,d代入上方程组中，可整理出旋转矩阵：

info

该旋转矩阵较为重要，在本文后续还会用到

四元数的关键公式推导

四元数和角速度的关系

对于给定一个角速度，我们需要找到四元数如何随时间变化。角速度 ω 描述了旋转的变化率。四元数的导数 q’ 可以通过以下方式与 ω 关联：

设角速度为： ω = [ω_x, ω_y, ω_z]
1. 对于一个旋转矩阵 R(t)，其时间导数满足： $其中，为的反对称矩阵$
2. 四元数的倒数
  
  通过微分 R 并利用 R=[ω]×R，可以推导出四元数的导数 q’。这个过程涉及大量的代数运算，最终得到： $其中是一个纯四元数（无实部），表示为：$

从角速度到四元数

假设当前四元数为q = (w, x, y, z),角速度为ω = (0, ω_x, ω_y, ω_z),则四元数倒数为：，展开运算可得：这些导数被用来更新四元数的各个分量，然后进行归一化处理，将会被用于后续的代码中

关于欧拉角的介绍和理解

什么是欧拉角

欧拉角是一种直观描述物体在三维空间中姿态（Orientation） 的方法，通过绕三个相互垂直的坐标轴依次旋转来表示。最常用的是 俯仰角（Pitch）、偏航角（Yaw）、翻滚角（Roll） 系统。

三个旋转轴：在物体自身坐标系中定义：

X轴（翻滚轴）：指向物体正右方（如飞机右翼方向）。
Y轴（俯仰轴）：指向物体正前方（如飞机机头方向）。
Z轴（偏航轴）：指向物体正上方（如飞机顶部方向）。

注意：必须按固定顺序旋转（如 Z→Y→X 或 Y→X→Z），不同顺序会得到不同结果。

此处我们使用ZYX顺序的旋转矩阵，旋转矩阵 RR 由三个基本旋转矩阵相乘得到：

则最终R为：读者看到这么长的旋转矩阵可能会大脑发怵，这么长的矩阵，这么多三角函数应该怎么去使用呢？对此不必担心，因为我们只需要用到该矩阵的一部分元素。相信读者应该注意到四元数也有一个旋转矩阵，我们就要用四元数求出欧拉角。

从四元数到欧拉角

为了方便表示，设旋转矩阵中的元素为：

计算俯仰角θ（Pitch）:

从 RR 的第三行第一列 r31=−sin⁡θ，得：θ = arcsin (−r₃₁)
计算偏航角ψ（Yaw）：

从 RR 的第一行和第二行的第一列：ψ = atan2(r₂₁, r₁₁)

info

其中 atan2(y,x)atan2(y,x) 是四象限反正切函数。
计算翻滚角ϕ（Roll）：

从 RR 的第三行第二列和第三列：θ = atan2(r₃₂, r₃₃)

恭喜各位读者，到此时已完成了理论部分的学习，接下来是应用的部分。因为主要应用在于IMU的角度解算，所以我给出了C语言代码。

实际应用于代码中

初始化四元数，读取IMU的角速度值

// 四元素参数初始化，需加入staic，保证变量的生命周期
static float q_w = 1.0f;
static float q_x = 0.0f;
static float q_y = 0.0f;
static float q_z = 0.0f;

//辅助变量
float qa, qb, qc;

float gx = DEG_TO_RAD(imu_trans_data.GTX); // 单位需转为rad/s;
float gy = DEG_TO_RAD(imu_trans_data.GTY);
float gz = DEG_TO_RAD(imu_trans_data.GTZ);

计算四元数的倒数，并积分求得四元数

// 四元数导数计算
gx *= (0.5f * T);     // 预乘以减少操作，其中T代表代码执行的周期
gy *= (0.5f * T);
gz *= (0.5f * T);
qa = q_w;
qb = q_x;
qc = q_y;
q_w += (-qb * gx - qc * gy - q_z * gz);
q_x += (qa * gx + qc * gz - q_z * gy);
q_y += (qa * gy - qb * gz + q_z * gx);
q_z += (qa * gz + qb * gy - qc * gx);

将四元数进行归一化

单位四元数才能正确表示一个旋转，我们需要单位四元数才能求解出欧拉角

// 归一化四元数
recipNorm = 1.0f / sqrt(q_w * q_w + q_x * q_x + q_y * q_y + q_z * q_z);
q_w *= recipNorm;
q_x *= recipNorm;
q_y *= recipNorm;
q_z *= recipNorm;

计算欧拉角

我们已经获得单位四元数了，根据上文的转换公式可以求得欧拉角

// 计算欧拉角
euler_angle.Roll = RAD_TO_DEG(atan2(2.0f * (q_w * q_x + q_y * q_z), 1.0f - 2.0f * (q_x * q_x + q_y * q_y)));
euler_angle.Pitch = RAD_TO_DEG(asin(2.0f * (q_w * q_y - q_z * q_x)));
euler_angle.Yaw = RAD_TO_DEG(atan2(2.0f * (q_w * q_z + q_x * q_y), 1.0f - 2.0f * (q_y * q_y + q_z * q_z)));

到此，我们已经完成了基础的IMU四元数解算，并且求得了欧拉角的值。

代码改进

普通IMU，例如MPU6050会提供六个数据，分别是三轴的加速度和三轴的角速度。读者可以看到，上述文章中只使用了三轴的角速度，而没有使用三轴的加速度。我们可以用三轴的加速度来修正角速度误差。本文将采用Mahony互补滤波算法，通过加速度计数据修正陀螺仪的角速度误差。重力方向的测量值（来自加速度计）与当前姿态估计的重力方向之间的误差来校正陀螺仪的漂移。接下来我将直接写出完整的代码，方便各位读者使用。

/**
 * @brief 四元素法+动态互补滤波s
 * @note 更大的浮点计算量
 * 
 */
void IMU_Get_EularAngle_Plus(void)
{
    // 四元素参数
    static float q_w = 1.0f;
    static float q_x = 0.0f;
    static float q_y = 0.0f;
    static float q_z = 0.0f;

    // 控制器参数
    static const float imu_Kp = 2.0f;    // 比例增益
    static const float imu_Ki = 0.005f;  // 积分增益
    static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;

    // 将原始数据转换为弧度每秒和g
    float ax = imu_trans_data.ATX;
    float ay = imu_trans_data.ATY;  
    float az = imu_trans_data.ATZ;
    float gx = DEG_TO_RAD(imu_trans_data.GTX); // 转为rad/s;
    float gy = DEG_TO_RAD(imu_trans_data.GTY);
    float gz = DEG_TO_RAD(imu_trans_data.GTZ);

    // 辅助变量
    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;
    float qa, qb, qc;

    // 加速度计数据校验
    if(ax*ay*az == 0) return;

    // 归一化加速度计测量值（必须先归一化）
    recipNorm = 1.0f / sqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // 估计重力的方向
    halfvx = q_x * q_z - q_w * q_y;
    halfvy = q_w * q_x + q_y * q_z;
    halfvz = q_w * q_w - 0.5f + q_z * q_z;

    // 误差是估计方向和测量方向的叉积
    halfex = (ay * halfvz - az * halfvy);
    halfey = (az * halfvx - ax * halfvz);
    halfez = (ax * halfvy - ay * halfvx);

    // 计算并应用积分反馈
    if(imu_Ki > 0.0f) {
        integralFBx += imu_Ki * halfex * halfT;    // 积分误差比例增益
        integralFBy += imu_Ki * halfey * halfT;
        integralFBz += imu_Ki * halfez * halfT;
        gx += integralFBx;    // 应用积分反馈
        gy += integralFBy;
        gz += integralFBz;
    } else {
        integralFBx = 0.0f;   // 防止积分饱和
        integralFBy = 0.0f;
        integralFBz = 0.0f;
    }

    // 应用比例反馈
    gx += imu_Kp * halfex;
    gy += imu_Kp * halfey;
    gz += imu_Kp * halfez;

    // 四元数导数计算
    gx *= (0.5f * halfT);     // 预乘以减少操作
    gy *= (0.5f * halfT);
    gz *= (0.5f * halfT);
    qa = q_w;
    qb = q_x;
    qc = q_y;
    q_w += (-qb * gx - qc * gy - q_z * gz);
    q_x += (qa * gx + qc * gz - q_z * gy);
    q_y += (qa * gy - qb * gz + q_z * gx);
    q_z += (qa * gz + qb * gy - qc * gx);

    // 归一化四元数
    recipNorm = 1.0f / sqrt(q_w * q_w + q_x * q_x + q_y * q_y + q_z * q_z);
    q_w *= recipNorm;
    q_x *= recipNorm;
    q_y *= recipNorm;
    q_z *= recipNorm;

    // 存储四元数
    quaternion.qw = q_w;
    quaternion.qx = q_x;
    quaternion.qy = q_y;
    quaternion.qz = q_z;

    // 计算欧拉角
    euler_angle.Roll = RAD_TO_DEG(atan2(2.0f * (q_w * q_x + q_y * q_z), 1.0f - 2.0f * (q_x * q_x + q_y * q_y)));
    euler_angle.Pitch = RAD_TO_DEG(asin(2.0f * (q_w * q_y - q_z * q_x)));
    euler_angle.Yaw = RAD_TO_DEG(atan2(2.0f * (q_w * q_z + q_x * q_y), 1.0f - 2.0f * (q_y * q_y + q_z * q_z)));
}

Eviarch's blog