MPU6050介绍及姿态解算

1、介绍：MPU6050 是 InvenSense 公司推出的全球首款整合性 6 轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了安装空间。

 

（1）绕X轴旋转角度为roll，绕Y轴旋转角度为pitch，绕Z轴旋转角度为yaw。

（2）加速度传感器，本质是力传感器。用来检查上下左右前后哪几个面都受了多少力（包括重力），然后计算角度。

（3）陀螺仪，本质是角速度检测仪。比如，一块板，以X轴为轴心，在一秒钟的时间转到了90度，那么它在X轴上的角速度就是 90度/秒  （DPS, 角速度单位，Degree Per Second的缩写°/S ，体现了转动的快慢）

（4）MPU分辨率：3轴加速度 和3轴陀螺仪分别用了3个16位的ADC, 也就是说，加速度有3个16位ADC，其中每个轴使用了一个。也是说，每个轴输出的数据，是2^16 也就是 -32768 ---- +32768。陀螺仪也是一样。

2、扩展性：MPU6050 内部整合了 3 轴陀螺仪和 3 轴加速度传感器，并且含有一个第二 IIC 接口，可用于连接外部磁力传感器即AUX_CL 和 AUX_DA，并利用自带的数字运动处理器（DMP: DigitalMotion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。

3、姿态解算：我们可以使用 InvenSense 公司提供的DMP非常方便的实现姿态解算，降低了运动处理运算对操作系统的负荷。

4、MPU6050原理图

其中，SCL 和 SDA 是连接 MCU 的 IIC 接口，MCU 通过这个 IIC 接口来控制 MPU6050，另外还有一个 IIC 接口：AUX_CL 和 AUX_DA，这个接口可用来连接外部从设备，比如磁传感器，这样就可以组成一个九轴传感器。

5、初始化步骤

（1）初始化 IIC  接口

（2）复位 MPU6050

MPU6050 内部所有寄存器恢复默认值

（3）设置角速度传感器（陀螺仪）和加速度传感器的满量程范围

上面说的-32768 --- +32768 ，那么这个数字到底代表了什么呢？比如陀螺仪 32768 到底是指角速度达到多少度/秒 ？这个其实是根据MPU6050设置的量程来决定的，量程不一样，32768代表的值就不一样。按陀螺仪来说，MPU6050 有四个量程可选：
±250，±500，±1000，±2000 度/s
比方说，设置了是 ±250 , 那么-32768  ---- +32768 就代表了 -250 ---- +250 。此时它的LSB(最低有效位) 是 131 LSB/(度/s)

（4）设置其它参数

     关闭中断、关闭 AUX IIC 接口、禁止 FIFO、设置陀螺仪采样率和设置数字低通滤波器（DLPF）等。

（5）配置系统时钟源并使能角速度传感器和加速度传感器

6、DMP（Digital Motion Processing ）

我们可以读出 MPU6050 的加速度传感器和角速度传感器的原始数据。DMP可以将原始角速度数据转换为4元组数据，这正是DMP的意义所在。进而完成欧拉角的计算。

MPU6050 的 DMP 输出的四元数是 q30 格式的，也就是浮点数放大了 2 的 30 次方倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以 2 的 30 次方，然后再进行计算，计算公式为：

q0=quat[0] / q30; //q30 格式转换为浮点数

q1=quat[1] / q30;

q2=quat[2] / q30;

q3=quat[3] / q30;

 

//计算得到欧拉角：俯仰角/横滚角/航向角

pitch=asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //俯仰角

roll=atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; //横滚角

yaw=atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //航向角

其中 quat[0]~ quat[3]是四元组，57.3 是弧度转换为角度，即 180/π，这样得到的结果就是以度（°）为单位的。

7、MPU6050的四元数解算姿态方法

我们先来看看如何用欧拉角描述一次平面旋转(坐标变换)：

设坐标系绕旋转α角后得到坐标系，在空间中有一个矢量在坐标系中的投影为，在内的投影为由于旋转绕进行，所以Z坐标未变，即有。

转换成矩阵形式表示为：

 

也就是

所以从旋转到可以写成上面仅仅是绕一根轴的旋转，如果三维空间中的欧拉角旋转要转三次:

上面得到了一个表示旋转的方向余弦矩阵。

不过要想用欧拉角解算姿态，其实我们套用欧拉角微分方程就行了：

上式中左侧，是本次更新后的欧拉角，对应row、pit、yaw。右侧，是上个周期测算出来的角度，三个角速度等于三轴陀螺仪在这个周期转动的角度，单位为弧度，计算间隔时T陀螺角速度，比如0.02秒0.01弧度/秒=0.0002弧度。间因此求解这个微分方程就能解算出当前的欧拉角。

我们为什么不用欧拉角来表示旋转而要引入四元数呢？

一方面是因为欧拉角微分方程中包含了大量的三角运算，这给实时解算带来了一定的困难。而且当俯仰角为90度时方程式会出现神奇的“GimbalLock”（万向锁）。所以欧拉角方法只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。

四元数法只求解四个未知量的线性微分方程组，计算量小，易于操作，是比较实用的工程方法。

我们知道在平面(x,y)中的旋转可以用复数来表示，同样的三维中的旋转可以用单位四元数来描述。我们来定义一个四元数：

一个四元数可以表示一个完整的旋转。只有单位四元数才可以表示旋转，至于为什么，因为这就是四元数表示旋转的约束条件。

而刚才用欧拉角描述的方向余弦矩阵用四元数描述则为：

要首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模，传入参数是陀螺仪x、y、z值和加速度计x、y、z值：

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; 

ay = ay / norm; 

az = az / norm;  

下面把四元数换算成方向余弦中的第三行的三个元素。刚好vx、vy、vz 。其实就是上一次的欧拉角（四元数）的机体坐标参考系换算出来的重力的单位向量。

estimated direction of gravity vx = 2*(q1*q3 - q0*q2);

vy = 2*(q0*q1 + q2*q3); 

vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; 

axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。

axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。

那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。

向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。

这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。

integral error scaled integral gain exInt = exInt + ex*Ki;

eyInt = eyInt + ey*Ki; 

ezInt = ezInt + ez*Ki; 

用叉积误差来做PI修正陀螺零偏

integral error scaled integral gain exInt = exInt + ex*Ki; 

eyInt = eyInt + ey*Ki; 

ezInt = ezInt + ez*Ki;  // adjusted gyroscope measurements 

gx = gx + Kp*ex + exInt; 

gy = gy + Kp*ey + eyInt; 

gz = gz + Kp*ez + ezInt; 

四元数微分方程，其中T为测量周期，为陀螺仪角速度，以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程：

integrate quaternion rate and normalise 

q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; 

q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; 

q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; 

q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;   

最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角：

所以有：

ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw 

ANGLE.Y= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch 

ANGLE.X= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

8、MEMS传感器

（1）加速度传感器

通过微机械加工技术在硅片上加工形成了一个机械悬臂。它与相邻的电极形成了两个电容。由于加速度使得机械悬臂与两个电极之间的距离发生变化，从而改变了两个电容的参数。通过集成的开关电容放大电路量测电容参数的变化，形成了与加速度成正比的电压输出。

 

参考博客：http://blog.sina.com.cn/s/blog_c5a00db10102wd7d.html