MPU 6050姿态角度融合算法

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 的。

#include<math.h>
#include "stm32f10x.h"
//---------------------------------------------------------------------------------------------------
// 变量定义
 
#define Kp 100.0f                        // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.002f                // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.001f                // 采样周期的一半
 
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差
 
float Yaw,Pitch,Roll;  //偏航角，俯仰角，翻滚角

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
   
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;  
 
        // 测量正常化
        norm = sqrt(ax*ax + ay*ay + az*az);      
        ax = ax / norm;                   //单位化