C/C++2026年11月21日· 约 5 分钟

互补解算、四元数与欧拉角:姿态估计的完整链路

姿态估计的核心在于将角速度与加速度两路看似矛盾的数据,转化为物体相对重力方向的三个具体角度。在云台、平衡车或飞控系统中,实时获取设备当前的倾斜状态是基础需求。直接对欧拉角积分并不可行,因为欧拉角存在旋

#姿态估计#四元数#互补滤波#嵌入式
Twitter 微博

封面

姿态估计的核心在于将角速度与加速度两路看似矛盾的数据,转化为物体相对重力方向的三个具体角度。在云台、平衡车或飞控系统中,实时获取设备当前的倾斜状态是基础需求。直接对欧拉角积分并不可行,因为欧拉角存在旋转顺序依赖,在某些极端姿态下会发生轴重合,导致两个角无法独立描述旋转,即万向节锁问题。因此,内部姿态计算必须使用四元数这种不带顺序且不会退化的表示方法,仅在显示或送入控制环时才转换为欧拉角。

整个解算链路分为三步:首先通过互补解算利用加速度将姿态拉回重力方向,其次通过四元数解算利用角速度推动姿态向前,最后计算欧拉角供屏幕显示。这里采用 Mahony 互补滤波结构,通过叉乘计算误差,结合比例与积分反馈修正角速度。

坐标系与基础定义#

姿态是相对于参考系的,此处以重力方向为参考。定义三个欧拉角:俯仰 pitch 绕 Y 轴旋转,范围 -90 到 90 度;横滚 roll 绕 X 轴旋转,范围 -180 到 180 度;偏航 yaw 绕 Z 轴旋转,范围 -180 到 180 度。俯仰角的范围限制源于其通过反正弦函数计算,受限于三角函数值域。坐标系遵循芯片手册约定,Z 轴垂直芯片表面向外,X、Y 在板面内,构成右手系。芯片平放静止时,加速度计读数为 (0, 0, +1g),注意加速度计测量的是比力,静止时指向支撑方向而非地面。内部计算使用弧度,显示时转换为角度。四元数 q 表示从机体系到世界系的旋转,初始状态 q = (1,0,0,0) 代表水平对齐。

#define ATTITUDE_RAD_TO_DEG   57.2957795f
#define ATTITUDE_DEG_TO_RAD   0.01745329252f

#define ATTITUDE_ACC_TOL_G          0.15f
#define ATTITUDE_INTEGRAL_MAX       0.5f
#define ATTITUDE_YAW_DEADZONE_DPS   0.5f

互补解算核心逻辑#

互补解算的关键在于利用加速度计修正由陀螺仪积分积累而产生的漂移。首先,从四元数中提取当前姿态认为的重力方向。

gravity_x = 2.0f * ((q1 * q3) - (q0 * q2));
gravity_y = 2.0f * ((q0 * q1) + (q2 * q3));
gravity_z = 1.0f - (2.0f * ((q1 * q1) + (q2 * q2)));

这三行代码提取旋转矩阵的第三行,即世界系 Z 轴在机体系中的表现。接下来处理加速度计数据。必须对加速度向量进行归一化,以消除车辆加速或减速带来的模长变化,仅保留方向信息。如果加速度模长偏离 1g 超过 ATTITUDE_ACC_TOL_G,则判定该时刻存在线性加速,不信任此加速度数据用于姿态修正。

norm = sqrtf((accel_x * accel_x) + (accel_y * accel_y) + (accel_z * accel_z));
if ((norm > 0.001f) && (isfinite(norm)) && (fabsf(norm - 1.0f) < ATTITUDE_ACC_TOL_G))
{
    use_accel = 1U;
    accel_x /= norm;
    accel_y /= norm;
    accel_z /= norm;
}

归一化后,计算实测重力方向与姿态预测重力方向的叉乘,得到误差向量。

error_x = (accel_y * gravity_z) - (accel_z * gravity_y);
error_y = (accel_z * gravity_x) - (accel_x * gravity_z);
error_z = (accel_x * gravity_y) - (accel_y * gravity_x);

该误差向量指示了姿态需要修正的方向和幅度。最后,将误差通过比例增益 kp 和积分增益 ki 反馈到角速度中。积分项用于消除静态偏差,但必须限制在 ATTITUDE_INTEGRAL_MAX 范围内,防止积分饱和导致系统失控。

gx = gyro_x_dps * ATTITUDE_DEG_TO_RAD;
if (use_accel != 0U)
{
    gx += (att->kp * error_x) + att->integral_x;
    att->integral_x += att->ki * error_x;
    if (att->integral_x > ATTITUDE_INTEGRAL_MAX)  { att->integral_x = ATTITUDE_INTEGRAL_MAX; }
    if (att->integral_x < -ATTITUDE_INTEGRAL_MAX) { att->integral_x = -ATTITUDE_INTEGRAL_MAX; }
}

修正后的角速度随后用于更新四元数。不同实现的参数约定可能不同,例如 Adafruit 实现中使用“半个误差”,其增益值约为本文实现的两倍,且积分项处理细节略有差异。

flowchart TD A[读取陀螺仪与加速度数据] --> B{加速度模长接近1g?} B -- 否 --> C[标记 use_accel 为 0] B -- 是 --> D[归一化加速度向量] D --> E[计算预测重力方向] E --> F[叉乘计算误差向量] F --> G[比例与积分修正角速度] G --> H[更新四元数] H --> I[计算欧拉角]

该流程确保了在动态环境下,滤波器能根据加速度的可信度动态调整对陀螺仪积分路径的修正力度,从而获得稳定且准确的全向姿态估计。

评论

© 2026 松岛川树