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

MPU6050介绍及姿态解算相关推荐

Arduino 与 MPU6050 姿态解算+ PROCESSING
2019独角兽企业重金招聘Python工程师标准>>> 买的MPU6050自带姿态解算大大减轻了上层处理器所做的工作. 通过熟悉了一下processing之后做了一个小例子更是感觉这 ...
从MPU6050了解姿态解算
前言最近正在学习和陀螺仪有关的知识,要将陀螺仪用到期末大作业中,代码还处在调试阶段,目前先总结一下学到的理论知识,学习资料来源三维转动的四元数表述 - 中国知网 (cnki.net) MPU605 ...
MPU6050姿态解算1-DMP方式
MPU6050的姿态解算方法有多种,包括硬件方式的DMP解算,软件方式的欧拉角与旋转矩阵解算,软件方式的轴角法与四元数解算.本篇先介绍最易操作的DMP方式. MPU6050基本功能 3轴陀螺仪陀螺仪 ...
stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
基于STM32的四旋翼无人机项目（二）：MPU6050姿态解算（含上位机3D姿态显示教学）
前言:本文为手把手教学飞控核心知识点之一的姿态解算--MPU6050 姿态解算(飞控专栏第2篇).项目中飞行器使用 MPU6050 传感器对飞行器的姿态进行解算(四元数方法),搭配设计的卡尔曼滤波器与 ...
【毕业设计】MPU6050姿态解算姿态估计 - 物联网单片机 stm32
文章目录 1 简介 2 MPU6050 3 工作原理 4 单片机与MPU6050通信 4.1 mpu6050 数据格式 4.2 倾角计算方法 5 实现代码 6 最后 1 简介 Hi,大家好,这里是丹成 ...
ESP-12F驱动MPU6050使用DMP库姿态解算
一.准备工作主芯片采用是安信可科技的ESF-12F模组(内置ESP8266芯片),使用GPIO模拟I2C驱动MPU6050,i2c驱动部分可直接参考官方例程中的i2c_master.c文件,(附件提 ...
STM32入门笔记(02)：MPU6050、MPU9250、ICM20948及姿态解算（SPL库函数版)
目录 MPU6050 什么是MPU6050? MPU6050的特点 MPU6050框图 MPU6050初始化 MPU6050寄存器电源管理寄存器1(0X6B) 陀螺仪配置寄存器(0X1B) 加速度传 ...
Pixhawk之姿态解算篇（5）_ECF/EKF/GD介绍
一.开篇很久没更新blog了, 最近研究的东西比较杂乱,也整理了很多东西,没有来的及更新,最近发现很多小伙伴都开始写blog了,在不更新就要"被落后了".兄弟们,等等我啊~~~ ...

MPU6050介绍及姿态解算

MPU6050介绍及姿态解算相关推荐

最新文章

热门文章