MPU 6050姿态角度融合算法

supergiant

1、介绍
1.1 姿态角（Euler角）pitch yaw roll介绍飞行器的姿态角并不是指哪个角度，是三个角度的统称。它们是：俯仰、滚转、偏航。你可以想象是飞机围绕XYZ三个轴分别转动形成的夹角。地面坐标系（earth-surface inertial reference frame）Sg--------OXgYgZg





①在地面上选一点Og②使Xg轴在水平面内并指向某一方向③Zg轴垂直于地面并指向地心(重力方向)④Yg轴在水平面内垂直于Xg轴，其指向按右手定则确定机体坐标系(Aircraft-body coordinate frame)Sb-------OXYZ





①原点O取在飞机质心处，坐标系与飞机固连②x轴在飞机对称平面内并平行于飞机的设计轴线指向机头③y轴垂直于飞机对称平面指向机身右方④z轴在飞机对称平面内，与x轴垂直并指向机身下方欧拉角/姿态角（Euler Angle）












机体坐标系与地面坐标系的关系是三个Euler角，反应了飞机相对地面的姿态。俯仰角θ（pitch）：机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。





偏航角ψ（yaw）：机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。





滚转角Φ（roll）：机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。





首先要明确，MPU6050 是一款姿态传感器，使用它就是为了得到待测物体（如四轴、平衡小车） x、y、z 轴的倾角（俯仰角 Pitch、滚转角 Roll、偏航角 Yaw） 。我们通过 I2C 读取到 MPU6050 的六个数据（三轴加速度 AD 值、三轴角速度 AD 值）经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。

本帖主要介绍三种姿态融合算法：四元数法 、一阶互补算法和卡尔曼滤波算法。

二、四元数法
关于四元数的一些概念和计算就不写上来了，我也不懂。我能告诉你的是：通过下面的算法，可以把六个数据转化成四元数（q0、q1、q2、q3），然后四元数转化成欧拉角（P、R、Y 角）。

  虽然 MPU6050 自带的 DMP库可以直接输出四元数，减轻 STM32 的运算负担， 这里在此没有使用，因为我是用 STM32 的硬件 I2C 读取 MPU6050 数据的，DMP库需要对 I2C 函数进行修改，如 DMP 库中的 I2C 写：i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0]))；有4个输入变量，而 STM32 硬件 I2C 的 I2C 写为：

void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr)，只有 3 个输入量（这之间的差异好像是由于 MPU6050 的 DMP 库是针对 MSP430 单片机写的），所以必须进行修改，但是改固件库是一件很痛苦的事，你们应该都懂。当然，如果你用模拟 I2C 的话，是容易实现的，网上的 DMP 移植几乎都是基于模拟 I2C 的。

[backcolor=inherit !important]#[backcolor=inherit !important]include[backcolor=inherit !important]<math.h>[backcolor=inherit !important]#[backcolor=inherit !important]include[backcolor=inherit !important] [backcolor=inherit !important]"stm32f10x.h"[backcolor=inherit !important]//---------------------------------------------------------------------------------------------------[backcolor=inherit !important]// 变量定义 [backcolor=inherit !important]#[backcolor=inherit !important]define[backcolor=inherit !important] Kp 100.0f                        [backcolor=inherit !important]// 比例增益支配率收敛到加速度计/磁强计[backcolor=inherit !important]#[backcolor=inherit !important]define[backcolor=inherit !important] Ki 0.002f                [backcolor=inherit !important]// 积分增益支配率的陀螺仪偏见的衔接[backcolor=inherit !important]#[backcolor=inherit !important]define[backcolor=inherit !important] halfT 0.001f                [backcolor=inherit !important]// 采样周期的一半 [backcolor=inherit !important]float q0 = [backcolor=inherit !important]1[backcolor=inherit !important], q1 = [backcolor=inherit !important]0[backcolor=inherit !important], q2 = [backcolor=inherit !important]0[backcolor=inherit !important], q3 = [backcolor=inherit !important]0[backcolor=inherit !important];          [backcolor=inherit !important]// 四元数的元素，代表估计方向[backcolor=inherit !important]float exInt = [backcolor=inherit !important]0[backcolor=inherit !important], eyInt = [backcolor=inherit !important]0[backcolor=inherit !important], ezInt = [backcolor=inherit !important]0[backcolor=inherit !important];        [backcolor=inherit !important]// 按比例缩小积分误差 [backcolor=inherit !important]float Yaw[backcolor=inherit !important],Pitch[backcolor=inherit !important],Roll[backcolor=inherit !important];  [backcolor=inherit !important]//偏航角，俯仰角，翻滚角[backcolor=inherit !important]void [backcolor=inherit !important]IMUupdate[backcolor=inherit !important]([backcolor=inherit !important]float gx[backcolor=inherit !important], [backcolor=inherit !important]float gy[backcolor=inherit !important], [backcolor=inherit !important]float gz[backcolor=inherit !important], [backcolor=inherit !important]float ax[backcolor=inherit !important], [backcolor=inherit !important]float ay[backcolor=inherit !important], [backcolor=inherit !important]float az[backcolor=inherit !important])[backcolor=inherit !important]{        [backcolor=inherit !important]float norm[backcolor=inherit !important];        [backcolor=inherit !important]float vx[backcolor=inherit !important], vy[backcolor=inherit !important], vz[backcolor=inherit !important];        [backcolor=inherit !important]float ex[backcolor=inherit !important], ey[backcolor=inherit !important], ez[backcolor=inherit !important];           [backcolor=inherit !important]// 测量正常化        norm = [backcolor=inherit !important]sqrt[backcolor=inherit !important](ax*ax + ay*ay + az*az[backcolor=inherit !important])[backcolor=inherit !important];              ax = ax / norm[backcolor=inherit !important];                   [backcolor=inherit !important]//单位化        ay = ay / norm[backcolor=inherit !important];        az = az / norm[backcolor=inherit !important];               [backcolor=inherit !important]// 估计方向的重力        vx = [backcolor=inherit !important]2*[backcolor=inherit !important](q1*q3 - q0*q2[backcolor=inherit !important])[backcolor=inherit !important];        vy = [backcolor=inherit !important]2*[backcolor=inherit !important](q0*q1 + q2*q3[backcolor=inherit !important])[backcolor=inherit !important];        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3[backcolor=inherit !important];         [backcolor=inherit !important]// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和        ex = [backcolor=inherit !important](ay*vz - az*vy[backcolor=inherit !important])[backcolor=inherit !important];        ey = [backcolor=inherit !important](az*vx - ax*vz[backcolor=inherit !important])[backcolor=inherit !important];        ez = [backcolor=inherit !important](ax*vy - ay*vx[backcolor=inherit !important])[backcolor=inherit !important];         [backcolor=inherit !important]// 积分误差比例积分增益        exInt = exInt + ex*Ki[backcolor=inherit !important];        eyInt = eyInt + ey*Ki[backcolor=inherit !important];        ezInt = ezInt + ez*Ki[backcolor=inherit !important];         [backcolor=inherit !important]// 调整后的陀螺仪测量        gx = gx + Kp*ex + exInt[backcolor=inherit !important];        gy = gy + Kp*ey + eyInt[backcolor=inherit !important];        gz = gz + Kp*ez + ezInt[backcolor=inherit !important];         [backcolor=inherit !important]// 整合四元数率和正常化        q0 = q0 + [backcolor=inherit !important](-q1*gx - q2*gy - q3*gz[backcolor=inherit !important])*halfT[backcolor=inherit !important];        q1 = q1 + [backcolor=inherit !important](q0*gx + q2*gz - q3*gy[backcolor=inherit !important])*halfT[backcolor=inherit !important];        q2 = q2 + [backcolor=inherit !important](q0*gy - q1*gz + q3*gx[backcolor=inherit !important])*halfT[backcolor=inherit !important];        q3 = q3 + [backcolor=inherit !important](q0*gz + q1*gy - q2*gx[backcolor=inherit !important])*halfT[backcolor=inherit !important];           [backcolor=inherit !important]// 正常化四元        norm = [backcolor=inherit !important]sqrt[backcolor=inherit !important](q0*q0 + q1*q1 + q2*q2 + q3*q3[backcolor=inherit !important])[backcolor=inherit !important];        q0 = q0 / norm[backcolor=inherit !important];        q1 = q1 / norm[backcolor=inherit !important];        q2 = q2 / norm[backcolor=inherit !important];        q3 = q3 / norm[backcolor=inherit !important];         Pitch  = [backcolor=inherit !important]asin[backcolor=inherit !important](-[backcolor=inherit !important]2 * q1 * q3 + [backcolor=inherit !important]2 * q0* q2[backcolor=inherit !important])* [backcolor=inherit !important]57.3[backcolor=inherit !important]; [backcolor=inherit !important]// pitch ,转换为度数        Roll = [backcolor=inherit !important]atan2[backcolor=inherit !important]([backcolor=inherit !important]2 * q2 * q3 + [backcolor=inherit !important]2 * q0 * q1[backcolor=inherit !important], -[backcolor=inherit !important]2 * q1 * q1 - [backcolor=inherit !important]2 * q2* q2 + [backcolor=inherit !important]1[backcolor=inherit !important])* [backcolor=inherit !important]57.3[backcolor=inherit !important]; [backcolor=inherit !important]// rollv        [backcolor=inherit !important]//Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;                //此处没有价值，注掉[backcolor=inherit !important]}      要注意的的是，四元数算法输出的是三个量 Pitch、Roll 和 Yaw，运算量很大。而像平衡小车这样的例子只需要一个角（Pitch 或 Roll ）就可以满足工作要求，个人觉得做平衡小车最好不用四元数法。



三、一阶互补算法
       MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是说有两组 Pitch、Roll 角，到底应该选哪组呢？别急，先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，且通过算 tan()  可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起，进行修正。得到 Pitch 角的程序如下：

[backcolor=inherit !important]//一阶互补滤波[backcolor=inherit !important]float K1 =[backcolor=inherit !important]0.1[backcolor=inherit !important]; [backcolor=inherit !important]// 对加速度计取值的权重[backcolor=inherit !important]float dt=[backcolor=inherit !important]0.001[backcolor=inherit !important];[backcolor=inherit !important]//注意：dt的取值为滤波器采样时间[backcolor=inherit !important]float angle[backcolor=inherit !important]; angle_ax=[backcolor=inherit !important]atan[backcolor=inherit !important](ax/az[backcolor=inherit !important])*[backcolor=inherit !important]57.3[backcolor=inherit !important];     [backcolor=inherit !important]//加速度得到的角度gy=[backcolor=inherit !important]([backcolor=inherit !important]float[backcolor=inherit !important])gyo[backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]]/[backcolor=inherit !important]7510.0[backcolor=inherit !important];       [backcolor=inherit !important]//陀螺仪得到的角速度Pitch = [backcolor=inherit !important]yijiehubu[backcolor=inherit !important](angle_ax[backcolor=inherit !important],gy[backcolor=inherit !important])[backcolor=inherit !important]; [backcolor=inherit !important]float [backcolor=inherit !important]yijiehubu[backcolor=inherit !important]([backcolor=inherit !important]float angle_m[backcolor=inherit !important], [backcolor=inherit !important]float gyro_m[backcolor=inherit !important])[backcolor=inherit !important]//采集后计算的角度和角加速度[backcolor=inherit !important]{     angle = K1 * angle_m + [backcolor=inherit !important]([backcolor=inherit !important]1-K1[backcolor=inherit !important]) * [backcolor=inherit !important](angle + gyro_m * dt[backcolor=inherit !important])[backcolor=inherit !important];     [backcolor=inherit !important]return angle[backcolor=inherit !important];[backcolor=inherit !important]}    互补算法只能得到一个倾角，这在平衡车项目中够用了，而在四轴飞行器设计中还需要 Roll 和 Yaw，就需要两个 互补算法，我是这样写的，注意变量不要搞混： 复制代码[backcolor=inherit !important]//一阶互补滤波[backcolor=inherit !important]float K1 =[backcolor=inherit !important]0.1[backcolor=inherit !important]; [backcolor=inherit !important]// 对加速度计取值的权重[backcolor=inherit !important]float dt=[backcolor=inherit !important]0.001[backcolor=inherit !important];[backcolor=inherit !important]//注意：dt的取值为滤波器采样时间[backcolor=inherit !important]float angle_P[backcolor=inherit !important],angle_R[backcolor=inherit !important];[backcolor=inherit !important]float [backcolor=inherit !important]yijiehubu_P[backcolor=inherit !important]([backcolor=inherit !important]float angle_m[backcolor=inherit !important], [backcolor=inherit !important]float gyro_m[backcolor=inherit !important])[backcolor=inherit !important]//采集后计算的角度和角加速度[backcolor=inherit !important]{     angle_P = K1 * angle_m + [backcolor=inherit !important]([backcolor=inherit !important]1-K1[backcolor=inherit !important]) * [backcolor=inherit !important](angle_P + gyro_m * dt[backcolor=inherit !important])[backcolor=inherit !important];         [backcolor=inherit !important]return angle_P[backcolor=inherit !important];[backcolor=inherit !important]} [backcolor=inherit !important]float [backcolor=inherit !important]yijiehubu_R[backcolor=inherit !important]([backcolor=inherit !important]float angle_m[backcolor=inherit !important], [backcolor=inherit !important]float gyro_m[backcolor=inherit !important])[backcolor=inherit !important]//采集后计算的角度和角加速度[backcolor=inherit !important]{     angle_R = K1 * angle_m + [backcolor=inherit !important]([backcolor=inherit !important]1-K1[backcolor=inherit !important]) * [backcolor=inherit !important](angle_R + gyro_m * dt[backcolor=inherit !important])[backcolor=inherit !important];         [backcolor=inherit !important]return angle_R[backcolor=inherit !important];[backcolor=inherit !important]}单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用。



四、卡尔曼滤波
      其实卡尔曼滤波和一阶互补有些相似，输入也是一样的。卡尔曼原理以及什么5个公式等等的，我也不太懂，就不写了，感兴趣的话可以上网查。在此给出具体程序，和一阶互补算法一样，每次卡尔曼滤波只能得到一个方向的角度。

[backcolor=inherit !important]#[backcolor=inherit !important]include[backcolor=inherit !important]<math.h>[backcolor=inherit !important]#[backcolor=inherit !important]include[backcolor=inherit !important] [backcolor=inherit !important]"stm32f10x.h"[backcolor=inherit !important]#[backcolor=inherit !important]include[backcolor=inherit !important] [backcolor=inherit !important]"Kalman_Filter.h"[backcolor=inherit !important]//卡尔曼滤波参数与函数[backcolor=inherit !important]float dt=[backcolor=inherit !important]0.001[backcolor=inherit !important];[backcolor=inherit !important]//注意：dt的取值为kalman滤波器采样时间[backcolor=inherit !important]float angle[backcolor=inherit !important], angle_dot[backcolor=inherit !important];[backcolor=inherit !important]//角度和角速度[backcolor=inherit !important]float P[backcolor=inherit !important][[backcolor=inherit !important]2[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]2[backcolor=inherit !important]] = [backcolor=inherit !important]{[backcolor=inherit !important]{ [backcolor=inherit !important]1[backcolor=inherit !important], [backcolor=inherit !important]0 [backcolor=inherit !important]}[backcolor=inherit !important],                 [backcolor=inherit !important]{ [backcolor=inherit !important]0[backcolor=inherit !important], [backcolor=inherit !important]1 [backcolor=inherit !important]}[backcolor=inherit !important]}[backcolor=inherit !important];[backcolor=inherit !important]float Pdot[backcolor=inherit !important][[backcolor=inherit !important]4[backcolor=inherit !important]] =[backcolor=inherit !important]{ [backcolor=inherit !important]0[backcolor=inherit !important],[backcolor=inherit !important]0[backcolor=inherit !important],[backcolor=inherit !important]0[backcolor=inherit !important],[backcolor=inherit !important]0[backcolor=inherit !important]}[backcolor=inherit !important];[backcolor=inherit !important]float Q_angle=[backcolor=inherit !important]0.001[backcolor=inherit !important], Q_gyro=[backcolor=inherit !important]0.005[backcolor=inherit !important]; [backcolor=inherit !important]//角度数据置信度,角速度数据置信度[backcolor=inherit !important]float R_angle=[backcolor=inherit !important]0.5 [backcolor=inherit !important],C_0 = [backcolor=inherit !important]1[backcolor=inherit !important];[backcolor=inherit !important]float q_bias[backcolor=inherit !important], angle_err[backcolor=inherit !important], PCt_0[backcolor=inherit !important], PCt_1[backcolor=inherit !important], E[backcolor=inherit !important], K_0[backcolor=inherit !important], K_1[backcolor=inherit !important], t_0[backcolor=inherit !important], t_1[backcolor=inherit !important]; [backcolor=inherit !important]//卡尔曼滤波[backcolor=inherit !important]float [backcolor=inherit !important]Kalman_Filter[backcolor=inherit !important]([backcolor=inherit !important]float angle_m[backcolor=inherit !important], [backcolor=inherit !important]float gyro_m[backcolor=inherit !important])[backcolor=inherit !important]//angleAx 和 gyroGy[backcolor=inherit !important]{        angle+=[backcolor=inherit !important](gyro_m-q_bias[backcolor=inherit !important]) * dt[backcolor=inherit !important];        angle_err = angle_m - angle[backcolor=inherit !important];        Pdot[backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]]=Q_angle - P[backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]] - P[backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]][backcolor=inherit !important];        Pdot[backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]]=- P[backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]][backcolor=inherit !important];        Pdot[backcolor=inherit !important][[backcolor=inherit !important]2[backcolor=inherit !important]]=- P[backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]][backcolor=inherit !important];        Pdot[backcolor=inherit !important][[backcolor=inherit !important]3[backcolor=inherit !important]]=Q_gyro[backcolor=inherit !important];        P[backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]] += Pdot[backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]] * dt[backcolor=inherit !important];        P[backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]] += Pdot[backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]] * dt[backcolor=inherit !important];        P[backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]] += Pdot[backcolor=inherit !important][[backcolor=inherit !important]2[backcolor=inherit !important]] * dt[backcolor=inherit !important];        P[backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]] += Pdot[backcolor=inherit !important][[backcolor=inherit !important]3[backcolor=inherit !important]] * dt[backcolor=inherit !important];        PCt_0 = C_0 * P[backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]][backcolor=inherit !important];        PCt_1 = C_0 * P[backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]][backcolor=inherit !important];        E = R_angle + C_0 * PCt_0[backcolor=inherit !important];        K_0 = PCt_0 / E[backcolor=inherit !important];        K_1 = PCt_1 / E[backcolor=inherit !important];        t_0 = PCt_0[backcolor=inherit !important];        t_1 = C_0 * P[backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]][backcolor=inherit !important];        P[backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]] -= K_0 * t_0[backcolor=inherit !important];        P[backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]] -= K_0 * t_1[backcolor=inherit !important];        P[backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]0[backcolor=inherit !important]] -= K_1 * t_0[backcolor=inherit !important];        P[backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]][backcolor=inherit !important][[backcolor=inherit !important]1[backcolor=inherit !important]] -= K_1 * t_1[backcolor=inherit !important];        angle += K_0 * angle_err[backcolor=inherit !important]; [backcolor=inherit !important]//最优角度        q_bias += K_1 * angle_err[backcolor=inherit !important];        angle_dot = gyro_m-q_bias[backcolor=inherit !important];[backcolor=inherit !important]//最优角速度         [backcolor=inherit !important]return angle；[backcolor=inherit !important]}
      作个总结：三种融合算法都能够输出姿态角（Pitch 和 Roll ），一次四元数法可以输出 P、R、Y 三个倾角，计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。