IMU原始数据经过四元数计算出欧拉角度

陀螺仪：靠三轴角速度推着四元数实时转动，算动态姿态加速度计：算出水平姿态偏差，拉住四元数扶正，消漂移融合迭代不断刷新四元数最后用固定三角公式，把四元数翻译成单片机好控制的角度值。

时空自由民.

115人浏览 · 2026-05-18 09:58:59

时空自由民. · 2026-05-18 09:58:59 发布

完整链路：IMU原始XYZ数据 → 四元数 → 欧拉角

整体流程

三轴加速度ax/ay/az + 三轴角速度gx/gy/gz
→ 姿态融合算法更新四元数
→ 四元数公式换算 → Roll/Pitch/Yaw欧拉角

一、先明确原始数据含义（右手IMU坐标系）

物理量	含义
$g_x,g_y,g_z$	三轴角速度，单位rad/s，代表当前绕各轴转动快慢
$a_x,a_y,a_z$	三轴加速度，含重力加速度，静止时只反映重力分量

核心作用

陀螺仪(角速度)：短时姿态变化极准，积分能算角度变化，长期会漂移
加速度计：静止/低速时能算出水平姿态角（横滚、俯仰），修正陀螺漂移，无法测航向yaw

二、第一步：角速度积分，更新四元数微分

四元数随时间变化的微分方程（姿态核心递推式）
$\begin{cases} \dot{q_0} = -\dfrac12(q_1g_x + q_2g_y + q_3g_z)\\[4pt] \dot{q_1} = \dfrac12(q_0g_x - q_3g_y + q_2g_z)\\[4pt] \dot{q_2} = \dfrac12(q_3g_x + q_0g_y - q_1g_z)\\[4pt] \dot{q_3} = \dfrac12(-q_2g_x + q_1g_y + q_0g_z) \end{cases}$

离散积分（采样周期 $d t$ ）：
$q_{new} = q_{old} + \dot{q} \cdot dt$
这一步只用三轴角速度，纯陀螺解算姿态，缺点：慢慢飘

三、第二步：用加速度计数据做姿态误差修正

由当前四元数，把载体坐标系重力向量转到地理坐标系
由原始ax/ay/az，算出实测重力向量
求两个向量的偏差，得到姿态误差
用PI/比例系数修正角速度，再代入上面微分方程修正四元数

主流就是 Mahony互补滤波 / 卡尔曼滤波 做融合

本质：用加速度计稳住水平角度，用陀螺仪保证动态响应，双向校准四元数

简易逻辑

静止时：加速度主导，校准四元数，消除陀螺漂移
运动时：陀螺仪主导，加速度权重降低，避免运动加速度干扰姿态

四、第三步：四元数归一化

融合更新完四元数后必须归一化，保证模长为1

$\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}$
$q_0=\frac{q_0}{norm},\ q_1=\frac{q_1}{norm},\ q_2=\frac{q_2}{norm},\ q_3=\frac{q_3}{norm}$

五、第四步：归一化四元数 → 欧拉角（Z-Y-X顺序）

弧度公式

$\begin{align*} roll\ (\text{绕X横滚})&=\arctan2\big(2(q_0q_1+q_2q_3),\ 1-2(q_1^2+q_2^2)\big)\\ pitch\ (\text{绕Y俯仰})&=\arcsin\big(2(q_0q_2-q_1q_3)\big)\\ yaw\ (\text{绕Z偏航})&=\arctan2\big(2(q_0q_3+q_1q_2),\ 1-2(q_2^2+q_3^2)\big) \end{align*}$

转角度

$\times 57.2958$

完整执行时序（代码运行逻辑）

定时采样：读取 IMU 原始 ax,ay,az,gx,gy,gz
角速度转弧度，带入四元数微分公式，预更新四元数
利用加速度数据计算姿态误差，修正角速度
再次修正四元数，完成姿态融合
四元数归一化
套公式算出 Roll、Pitch、Yaw 欧拉角
输出欧拉角给PID、姿态显示、运动控制

一句话串通

拿三轴角速度推着四元数实时转动，再用三轴加速度校准摆正四元数，最后把摆正后的四元数反解三角函数，算出人能看懂的三个姿态欧拉角。

需要我给你一份不带数学公式、纯代码逐行注释的完整融合+转欧拉角工程代码吗？

嵌入式实战：IMU原始六轴数据 → 四元数 → 欧拉角

一、前置准备

1. 坐标系（工程标准右手系）

载体系：X右、Y前、Z上
旋转顺序：Z-Y-X（yaw→pitch→roll）
采样周期：固定dt（如10ms=0.01s）

2. 原始数据预处理

IMU读出的是传感器原始整型值，先转物理量

// 陀螺仪：原始值 → 角速度 rad/s
gx = (float)raw_gx * GYRO_SENS;
gy = (float)raw_gy * GYRO_SENS;
gz = (float)raw_gz * GYRO_SENS;

// 加速度计：原始值 → 加速度 g
ax = (float)raw_ax * ACC_SENS;
ay = (float)raw_ay * ACC_SENS;
az = (float)raw_az * ACC_SENS;

灵敏度查表：如MPU6050 ±250°/s → GYRO_SENS = 0.0087266f
加速度±2g → ACC_SENS = 0.000598f

3. 全局姿态变量

// 四元数 初始姿态水平朝前 q0=1,其余0
float q0=1.0f, q1=0.0f, q2=0.0f, q3=0.0f;
// 欧拉角 角度制
float roll,pitch,yaw;
// 滤波修正量
float ex=0,ey=0,ez=0;
// 采样周期
#define dt 0.01f
// 滤波参数 Mahony常用
#define Kp 2.0f
#define Ki 0.005f

二、核心计算步骤（嵌入式逐行逻辑）

步骤1：加速度计求理论重力姿态向量

静止时加速度就是重力在三轴投影，先归一化加速度

// 1.加速度模长
float norm = sqrtf(ax*ax + ay*ay + az*az);
if(norm < 0.001f) return; // 防止除0
// 归一化 单位重力向量
float axn = ax/norm;
float ayn = ay/norm;
float azn = az/norm;

步骤2：用当前四元数，推算理论重力向量

把地理坐标系竖直向下重力，转到当前载体坐标系

// 四元数解算出来的理论重力分量
float vx = 2*(q1*q3 - q0*q2);
float vy = 2*(q0*q1 + q2*q3);
float vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

步骤3：计算姿态误差（实测重力 vs 理论重力）

ex += (ayn*vz - azn*vy);
ey += (azn*vx - axn*vz);
ez += (axn*vy - ayn*vx);

误差含义：当前姿态偏了多少，用来修正陀螺仪漂移

步骤4：误差补偿角速度（融合核心）

// PI修正陀螺
gx += Kp*ex + Ki*ex*dt;
gy += Kp*ey + Ki*ey*dt;
gz += Kp*ez + Ki*ez*dt;

静止：误差大，大幅修正陀螺
运动：误差小，陀螺主导姿态

步骤5：角速度积分更新四元数（最关键递推）

嵌入式离散迭代公式

float q0_dot = 0.5f*(-q1*gx - q2*gy - q3*gz);
float q1_dot = 0.5f*( q0*gx - q3*gy + q2*gz);
float q2_dot = 0.5f*( q3*gx + q0*gy - q1*gz);
float q3_dot = 0.5f*(-q2*gx + q1*gy + q0*gz);

// 欧拉积分迭代更新四元数
q0 += q0_dot * dt;
q1 += q1_dot * dt;
q2 += q2_dot * dt;
q3 += q3_dot * dt;

步骤6：四元数归一化（必须做，防止漂移畸变）

float qnorm = sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= qnorm;
q1 /= qnorm;
q2 /= qnorm;
q3 /= qnorm;

步骤7：四元数 → 欧拉角（直接嵌入式可用公式）

// 俯仰角 pitch
pitch = asinf(2.0f*(q0*q2 - q1*q3));
// 横滚角 roll
roll  = atan2f(2.0f*(q0*q1 + q2*q3), 1.0f-2.0f*(q1*q1+q2*q2));
// 偏航角 yaw
yaw   = atan2f(2.0f*(q0*q3 + q1*q2), 1.0f-2.0f*(q2*q2+q3*q3));

// 弧度转角度 嵌入式常用常量
#define RAD_TO_DEG 57.29578f
roll  *= RAD_TO_DEG;
pitch *= RAD_TO_DEG;
yaw   *= RAD_TO_DEG;