姿态角的关系

坐标系间的旋转角度	说明	载体自身旋转
偏航角(Yaw)	Y 轴与标准方向的夹角	绕载体Z 轴旋转 可改变
俯仰角(Pitch)	Z 轴与标准方向的夹角	绕载体X 轴旋转可改变
横滚角(Roll)	X 轴与标准方向的夹角	绕载体Y 轴旋转可改变

利用陀螺仪检测角度

最直观的角度检测器就是陀螺仪了，见图 47-3，它可以检测物体绕坐标轴转动的“角速度”，如同将速度对时间积分可以求出路程一样，将角速度对时间积分就可以计算出旋转的“角度”。

陀螺仪检测的缺陷：

由于陀螺仪测量角度时使用积分，会存在积分误差，若积分时间单位 Dt 越小，误差就越小。

同样地，提高陀螺仪传感器的采样频率，即可减少积分误差，目前非常普通的陀螺仪传感器的采样频率都可以达到 8KHz，已能满足大部分应用的精度要求。

利用加速度计检测角度

但由于陀螺仪器件本身误差的存在，我们引入检测倾角的传感器。

当传感器的姿态不同时，它在自身各个坐标轴检测到的重力加速度是不一样的，利用各方向的测量结果，根据力的分解原理，可求出各个坐标轴与重力之间的夹角。

因为重力方向是与地理坐标系的“天地”轴固连的，所以通过测量载体坐标系各轴与重力方向的夹角即可求得它与地理坐标系的角度旋转关系，从而获知载体姿态。

加速度传感器检测的缺陷：

由于这种倾角检测方式是利用重力进行检测的，它无法检测到偏航角(Yaw)，原理跟 T字型水平仪一样，无论如何设计水平仪，水泡都无法指示这样的角度。
另一个缺陷是加速度传感器并不会区分重力加速度与外力加速度，当物体运动的时候，它也会在运动的方向检测出加速度，特别在震动的状态下，传感器的数据会有非常大的数据变化，此时难以反应重力的实际值。

利用磁场检测角度

为了弥补加速度传感器无法检测偏航角(Yaw)的问题，我们再引入磁场检测传感器，它可以检测出各个方向上的磁场大小，通过检测地球磁场，它可实现指南针的功能，所以也被称为电子罗盘。由于地磁场与地理坐标系的“南北”轴固联，利用磁场检测传感器的指南针功能，就可以测量出偏航角(Yaw)了。

磁场检测器的缺陷：
与指南针的缺陷一样，使用磁场传感器会受到外部磁场干扰，如载体本身的电磁场干扰，不同地理环境的磁铁矿干扰等等。

利用GPS 检测角度

使用 GPS 可以直接检测出载体在地球上的坐标，假如载体在某时刻测得坐标为 A，另一时刻测得坐标为 B，利用两个坐标即可求出它的航向，即可以确定偏航角，且不受磁场的影响，但这种检测方式只有当载体产生大范围位移的时候才有效(GPS 民用精度大概为10 米级)。

姿态融合与四元数

可以发现，使用陀螺仪检测角度时，在静止状态下存在缺陷，且受时间影响，而加速度传感器检测角度时，在运动状态下存在缺陷，且不受时间影响，刚好互补。
假如我们同时使用这两种传感器，并设计一个滤波算法，当物体处于静止状态时，增大加速度数据的权重；当物体处于运动状时，增大陀螺仪数据的权重，从而获得更准确的姿态数据；同理，检测偏航角，当载体在静止状态时，可增大磁场检测器数据的权重；当载体在运动状态时，增大陀螺仪和 GPS 检测数据的权重。
这些采用多种传感器数据来检测姿态的处理算法被称为姿态融合。

在姿态融合解算的时候常常使用“四元数”来表示姿态，它由三个实数及一个虚数组成，因而被称之为四元数。使用四元数表示姿态并不直观，但因为使用欧拉角(即前面说的偏航角、横滚角及俯仰角)表示姿态的时候会有“万向节死锁”问题，且运算比较复杂。
所以一般在数据处理的时候会使用四元数，处理完毕后再把四元数转换成欧拉角。
在这里我们只要了解四元数是姿态的另一种表示方式即可。

传感器

1. 传感器工作原理

我们讲的传感器一般是指把物理量转化成电信号量的装置。

2. 传感器参数

传感器一般使用精度、分辨率及采样频率这些参数来进行比较，衡量它的性能。

参数	说明
线性误差	指传感器测量值与真实物理量值之间的拟合度误差。
分辨率	指传感器可检测到的最小物理量的单位。
采样频率	指在单位时间内的采样次数。

3. 物理量的表示方法

大部分传感器的输出都是与电压成比例关系的，电压值一般采用 ADC 来测量，而ADC 一般有固定的位数，如 8 位 ADC、12 位 ADC 等，ADC 的位数会影响测量的分辨率及量程。
假设用一个 2 位的 ADC 来测量长度，2 位的 ADC 最多只能表示 0、1、2、3 这四个数，假如它的分辨率为 20 厘米，那么它最大的测量长度为 60 厘米。
假如它的分辨率为 10 厘米，那么它的最大测量长度为 30 厘米，由此可知，对于特定位数的ADC，量程和分辨率不可兼得。

在实际应用中，常常直接用 ADC 每位表征的物理量值来表示分辨率，如每位代表 20厘米，我们称它的分辨率为 1LSB/20cm，它等效于 5 位表示 1 米：5LSB/m。其中的 LSB（Least Significant Bit），意为最 ADC 的低有效位。

使用采样得到的 ADC 数值，除以分辨率，即可求取得到物理量。例如使用分辨率为5LSB/m、线性误差为 0.1m 的传感器进行长度测量，其 ADC 采样得到数据值为“20”，可计算知道该传感器的测量值为 4 米，而该长度的真实值介于 3.9-4.1 米之间。

MPU6050

简介

MPU6050 芯片，它是一种六轴传感器模块，采用 InvenSense 公司的 MPU6050 作为主芯片，能同时检测三轴加速度、三轴陀螺仪(三轴角速度)的运动数据以及温度数据。利用 MPU6050 芯片内部的 DMP 模块（Digital Motion Processor 数字运动处理器），可对传感器数据进行滤波、融合处理，它直接通过 I2C 接口向主控器输出姿态解算后的姿态数据，降低主控器的运算量。其姿态解算频率最高可达 200Hz，非常适合用于对姿态控制实时要求较高的领域。常见应用于手机、智能手环、四轴飞行器及计步器等的姿态检测。

特性参数

加速度与陀螺仪传感器的 ADC 均为 16 位，它们的量程及分辨率可选多种模式，量程越大，分辨率越低。

加速度配置跟量程的关系：

AFS_SEL	Full Scale Range	LSB Sensitivity
0	±2g	16384 LSB/g
1	±4g	8192 LSB/g
2	±8g	4096 LSB/g
3	±16g	2048 LSB/g

陀螺仪的几种量程配置：

FS_SEL	Full Scale Range
0	±250°/s
1	±500°/s
2	±1000°/s
3	±2000°/s

从表中还可了解到传感器的加速度及陀螺仪的采样频率分别为 1000Hz 及 8000Hz，它们是指加速度及角速度数据的采样频率，我们可以使用 STM32 控制器把这些数据读取出来然后进行姿态融合解算，以求出传感器当前的姿态(即求出偏航角、横滚角、俯仰角)。

而如果我们使用传感器内部的 DMP 单元进行解算，它可以直接对采样得到的加速度及角速度进行姿态解算，解算得到的结果再输出给 STM32 控制器，即 STM32 无需自己计算，可直接获取偏航角、横滚角及俯仰角，该 DMP 每秒可输出 200 次姿态数据。

具体功能实现代码在此。