飞控学习笔记-姿态角解算（MPU6050 加速度计加陀螺仪）

本文持续更新…
I2C通信
AHRS是自动航向基准系统(Automatic Heading Reference System)的简称。目前，使用四元数来进行AHRS姿态解算的算法被广泛采用于四轴飞行器上。

IMU部分：
IMU是惯性测量装置(Inertial Measurement Unit)的简称，通常包含陀螺仪和加速度计。

1.陀螺仪：测量的是角速度，即物体转动的速度，把速度和时间相乘，即可以得到某一时间段内物体转过的角度。（但是积分运算得来的角度本身就存在误差，随着时间的累加，误差会加剧，此时就需要加速度计辅助计算出姿态角度）
2.加速度计：测量的是物体的加速度，我们知道，重力加速度是一个物体受重力作用的情况下所具有的加速度。当物体处于静止状态时，加速度计测量出来的值就等于重力加速度1g, 约等于9.8米每平方秒。重力加速度g的方向总是竖直向下的，通过获得重力加速度在其X轴，Y轴上的分量，我们可以计算出物体相对于水平面的倾斜角度。典型的IMU惯性测量芯片为MPU6050，它被广泛采用在四轴飞行器上。
mpu6050便是这两种传感器结合测出姿态角，通常运用卡尔曼滤波得出最终角度

根据加速度计和地磁计的数据，转换到地理坐标系后，与对应参考的重力向量和地磁向量进行求误差，这个误差用来校正陀螺仪的输出，然后用陀螺仪数据进行四元数更新，再转换到欧拉角

陀螺仪的角速度测量：假如他的速度是1度没秒，那么通过速度乘以时间，我们就可以知道他距离起点走了多少度。

加速度计来测量倾角：一个简单的例子如下: 一个单轴的加速计位于重力水平面上的时候，它在垂直方向上受到的加速度为1g，在水平方向上受到的加速度为0。当我们把它旋转一个角度的时候，就会在水平轴上产生一个加速度分量。通过它们的关系，就可以计算出该单轴加速计的倾角。

1.通过陀螺仪的积分来获得四轴的旋转角度
2.然后通过加速度计的比例和积分运算来修正陀螺仪的积分结果。
下面代码中的gx，gy，gz分别代表陀螺仪在X轴，Y轴和Z轴三个轴上的分量
ax，ay，az分别代表加速度计在在X轴，Y轴和Z轴三个轴上的分量。

MPU6050（陀螺仪 加速度计）

MPU6050只包含陀螺仪和加速计共六个轴，
而MPU9150还包含磁力计，共九个轴。

陀螺仪

陀螺仪是测试角速度的传感器，也有人把角速度说成角速率，说的是一样的物理量。拿电 机做例子，当我们说一个电机10转每秒。一转是360度,那么它的主轴在一秒内转过3600 度。也就是说这个电机在转动时的角速度是3600dps.dps 就是dergee per second 度每秒 (或者写成 deg/s).
MPU6050 集成了三轴的陀螺仪.角速度全格感测范围为±250、±500、±1000与± 2000°/sec (dps)（度/秒）.当选择量程为±250dps（度/秒）的时候，将会得到分辩率为131LSB/(º/s).也就 是当载体在X+轴转动1dps时，ADC将输出131.

上述的说明,已经知道陀螺仪测量角度的变化率.为了解释这一点，
让我们假设，我们已经测量围绕Y轴的旋转角（这将是Axz角）在时刻t0，我们将其定义为Axz0，接下来，我们测量这个角度是在稍后的时间t1是Axz1。
变化率将被计算如下： RateAxz =（Axz1 - Axz0）/（t1 - t0）
如果Axz单位是度，并以秒为时间单位，那么RateAxz将以度 / s表示。 MPU6050并不会以 度/ s 单位输出，我们需要在读完后进行转换。先来看看各个量程对 应的灵敏度

从ADC值到 dps
通过I2C接口读出来的转换结果ADC值，并不是以度每秒为单位。一般按以下公式进行转 换：Anglerate = ADCrate /灵敏度 以量程为±1000º/s为例，说明如何转换。假设读取x轴的ADC值为200,从上表中得知在 ±1000º/s下的灵敏度为32.8LSB/(º/s) 。根据上面的公式： Anglerate = 200/32.8 = 6.09756º/s 这就是说,MPU6050检测到模块正在以约6度每秒的速度绕X轴(或者叫在YZ平面上)旋转.
ADC值并不都是正的,请注意,当出现负数时,意味着该设备从现有的正方向相反的方向旋转.
如下：=顺时针为正

最后，还要将角度换算成弧度制，便于计算

综上做个陀螺仪的总结：
首先 传感器测量出当前姿态角，并且将数据寄存在一个16位的寄存器中
其次我们要选择精度，便于将16位的数据换算成角度
最终将角度转化为弧度制
输出姿态角=寄存器ADC的值/（灵敏度+2PI/360）

加速度计：

加速度计

全球首例整合性6轴运动处理组件
3轴陀螺仪＋3轴加速度计
IIC接口，可用于扩展外部传感器
数据格式：旋转矩阵、四元数(quaternion)、欧拉角格式
体积小

无人机传感器
MPU6050:3轴陀螺仪＋3轴加速度（姿态）
HMC5883:3轴数字罗盘（精确方向控制）
MS5611:气压计（高度计）

Pitch:俯仰角（抬头低头）
Roll:滚转角（翻身）
Yaw：偏航角（转弯）

三个角度组合，可唯一确定一个姿态
（MPU6050带有三个陀螺仪，每个陀螺仪各自负责检测相应轴的转动速度，也就是检测围绕 各个轴转动的速度。像三轴的陀螺仪将同时检测 X Y Z轴的旋转。 由上面这个模型图，首先我们定义：
Rxz - 是R向量在XZ平面上的投影
Ryz - 是R向量在XY平面上的投影
Rxz和Rz所形成的直角三角形，
利用勾股定理，我们得到： Rxz ^ 2 = RX ^ 2 + RZ ^ 2，
和同样： Ryz ^ 2 = RY ^ 2 + RZ ^ 2
还要注意的是：
R ^ 2 = RXZ ^ 2 + Ry^ 2，这可以来自从公式1和上面的等式，或它可以是来自于由R和Ryz
R ^ 2 = Ryz ^ 2 + Rx^ 2 形成的直角三角 我们不会在本文中使用这些公式，只是让读者认识到所有值之间的关系。 同时我们将定义Z轴和Rxz 、RyZ之间的夹角。 Axz - Rxz和Z轴间的夹角 Ayz - Ryz和Z轴间的夹角 现在看看,从这个模型中,陀螺仪测量什么?）