基于STM32的四旋翼无人机项目（二）：MPU6050姿态解算（含上位机3D姿态显示教学）_mpu6050上位机

效果图：

一、飞行器姿态解算

1.1 MPU6050概述

飞行器通常搭载一款姿态传感器（不管是六轴还是九轴姿态传感器），本项目中以最常见的 MPU6050 为例。MPU6050 传感器其实并不能直接输出我们飞行器飞行过程中的欧拉角(Euler-angles)，通过读取它的传感器我们可以得到：3轴角速度+3轴角加速度。得到的角速度和角加速度信息我们是无法直接使用的，这个时候我们可以选择使用 DMP 去解算此时飞行器的欧拉角(Euler-angles)情况。当然，作者在项目中并没有使用 DMP 去解算飞行器的欧拉角(Euler-angles)，而是使用了四元数解算的方法！

**DMP（Digital Motion Processor）**是一种数字运动处理器，它可以从MPU6050等传感器中读取数据，并进行解算以获取姿态信息。下面是DMP解算MPU6050的优缺点：

优点：

1. DMP使用简单，可以直接套用官方库进行开发，无需自己编写解算算法。
2. DMP不会占用太多的资源，因为它只需要读取传感器数据并进行简单的解算。
3. DMP的输出数据经过处理，可以直接用于姿态控制等应用，无需再进行复杂的计算。

缺点：

1. DMP的输出数据精度可能不够高，特别是在高精度传感场景下。
2. DMP的输出数据不稳定，可能会受到噪声等因素的影响。
3. DMP无法测量偏航角，只能获取滚动角和俯仰角的信息。

1.2 四元数姿态解算

本小节将以下方思维导图进行分析讲解：

初次接触的读者朋友可能对四元数较为陌生，这里作者建议大家直接去阅读秦永元的《惯性导航》，里面有非常好的讲解，大家可以直接看绪论和第九章就可以。

《惯性导航》PDF地址：惯性导航（第三版） (sciencereading.cn)")

下面我们根据思维导图用程序来一步一步实现如何求解欧拉角：

**1、**定义初始四元数的值为q0=1,q1=0,q2=0,q3=0。

**2、**读取加速度计值、角速度值，程序定义变量分别为ax、ay、az，gx、gy、gz，将陀螺仪值转为弧度，转换如下：

gx = gx * 0.0174f;        //1度=0.0174弧度
gy = gy * 0.0174f; 
gz = gz * 0.0174f; 
1
2
3

**3、**对加速度值进行归一化

//提取等效旋转矩阵中的重力分量 
Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);								
Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);						  
Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);	

//加速度归一化
NormAcc = 1/sqrt(squa(MPU6050.accX)+ squa(MPU6050.accY) +squa(MPU6050.accZ));
	
//归一化计算
Acc.x = pMpu->accX * NormAcc;
Acc.y = pMpu->accY * NormAcc;
Acc.z = pMpu->accZ * NormAcc;
1
2
3
4
5
6
7
8
9
10
11
12

**4、**提取姿态矩阵中的重力分量，我们已经得到数学计算公式为

//提取等效旋转矩阵中的重力分量 
Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);								
Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);						  
Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);	
1
2
3
4

**5、**求姿态误差，对两向量进行叉乘(定义ex、ey、ez为三个轴误差元素)，数学计算为：

//向量差乘得出的值
AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);
1
2
3
4

**6、**互补滤波，将误差输入PID控制器后与陀螺仪测得的角速度相加，修正角速度值，程序实现如下(Kp为互补滤波系数这里取Kp=0.5，实际值根据需要进行调整)：

//角速度融合加速度积分补偿值
Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x  +  GyroIntegError.x;//弧度制
Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y  +  GyroIntegError.y;
Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z  +  GyroIntegError.z;
1
2
3
4

**7、**解四元数微分方程，其数学计算如下（初始值q0 = 1,q1 = 0,q2 = 0,q3 = 0,为角速度，为周期时间）

// 一阶龙格库塔法, 更新四元数
q
1