姿态传感器
姿态传感器是基于MEMS技术的高性能三维运动姿态测量系统。它包含三轴陀螺仪、三轴加速度计，三轴电子罗盘等运动传感器，通过内嵌的低功耗ARM处理器得到经过温度补偿的三维姿态与方位等数据，比如角速度，加速度，磁数据等。利用基于四元数的三维算法和特殊数据融合技术，实时输出以四元数、欧拉角表示的零漂移三维姿态方位数据。姿态传感器可广泛嵌入到航模无人机，机器人，机械云台，车辆船舶，地面及水下设备，虚拟现实，人体运动分析等需要自主测量三维姿态与方位的产品设备中。
姿态传感器的产品特点：
✔ 灵活的数据输出接口：I2C，SPI， 串口
✔ 高速数据输出率，最高500Hz
✔ 真正的低功耗，实测17mA
✔ 快速的启动稳定时间，0.1秒
✔ 全角度无盲区三维姿态方位数据输出
✔ 多种数据输出选择（欧拉角，四元数，旋转矩阵等）
✔ 高动态响应与长时间稳定性相结合
✔ 复合型Kalman滤波与数据融合，捷联式惯导技术
✔ 输出绝对方向（地球磁场方向）
✔ 可在动态环境下启动
✔ 跌落检测输出，超动态检测输出
✔ 三轴加速度、三轴角速度和三轴磁场强度计高度集成9DOF
✔ 软件开发编程接口，Demo程序
简单来说，姿态传感器就是能获取三维运动信息的传感器。
姿态传感器在电子产品中处处可见，在平衡车和无人机最为常见。在自主移动机器人中，通过姿态传感器能获取机器人的位姿信息是非常关键的，它会影响到机器人的运功规划及运行状态。
接下来正式进入姿态解算的内容。
姿态解算基础知识
参考文章：
姿态解算基本知识-CSDN博客
坐标系
导航坐标系：在多旋翼中，又叫地球坐标系、地理坐标系。通常，采用北东地(NED)构成坐标系的 X、Y、Z轴。
机体坐标系：固联在多旋翼飞行器上，坐标系原点定位于飞行器中心点(假设中心点与重心点重合)，X轴指向机头方向，Y轴指向机头的右方，Z轴垂直于X，Y轴构成平面垂直向下，这个可以用“右手定则”确定坐标轴的正方向。
可见，不同的系统中，坐标系的方向可以是不同的。
固连坐标系是随着物体运动的，但是参考坐标系是恒定不动的。
导航坐标系一般是作为参考坐标系。
姿态
姿态通俗的说就是指我们站在地面上观察飞行器的俯仰(pitch)/横滚(roll)/航向(yaw)状态。飞行器需要实时知道当前自己的姿态，才能够根据需要操控其接下来的动作，例如保持平稳，翻滚。
MPU6050姿态解算2-欧拉角&旋转矩阵-CSDN博客
记住这几个单词：横滚roll，俯仰pitch，偏航yaw
数学模型:姿态是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系(姿态角)﹐有一些数学表示方法。很常见的就是欧拉角,四元数，矩阵，轴角。
飞行器姿态描述套用上面的数学模型，参考坐标系就对应着导航坐标系是固定不变的。我们通常用坐标系R表示。固连坐标系就对应着机体坐标系，用坐标系r表示。那么我们就可以用欧拉角，四元数等数学方法来描述r与R的角位置关系。这就是飞行器姿态解算的数学模型。
其实就是通过参考坐标系和固连坐标系的相对位置关系来计算三维运动数据的。因此只要有相对运动，就能通过一些方法来计算出发生了什么运动。
欧拉角
莱昂哈德·欧拉用欧拉角来描述刚体在三维欧几里得空间的取向。对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系（就是人们通常所说的地面静止参照系），是静止不动的。而坐标系则固联于刚体，随着刚体的旋转而旋转。如下图的左数第一个图a 、β、y就是欧拉角。后面紧跟的三个图演示了欧拉角是如何产生的，分别对应哪个角度。
注意几个问题：最开始固连参考系和参考坐标系是重合的
然后每一次旋转都是基于前一次的。
理论上，物体的任意一次随意运动都可以分解成这三个方向上的运动，即三个运动方向上的矢量共同合成运动后的状态。
我们具体看下这张图
图中有两组坐标：
小写xyz为全局坐标，即参考坐标系（蓝色部分），保持不动；
大写XYZ为局部坐标，即固连坐标系（红色部分），随着物体一起运动；
设定 xyz-轴为参考系的参考轴。称 xy-平面与 XY-平面的相交为交点线，用英文字母（N）代表。zxz 顺规的欧拉角可以静态地这样定义：
α 是 x-轴与交点线的夹角，β 是 z-轴与Z-轴的夹角，γ 是交点线与X-轴的夹角。
很可惜地，对于夹角的顺序和标记，夹角的两个轴的指定，并没有任何常规。科学家对此从未达成共识。每当用到欧拉角时，我们必须明确的表示出夹角的顺序，指定其参考轴。
实际上，有许多方法可以设定两个坐标系的相对取向。欧拉角方法只是其中的一种。此外，不同的作者会用不同组合的欧拉角来描述，或用不同的名字表示同样的欧拉角。因此，使用欧拉角前，必须先做好明确的定义。
比如：
经过如下旋转步骤可得到三个欧拉角
- 物体绕全局的z（小写）轴旋转得到α角；
- 基于前一步，绕着前一步的X（大写）轴旋转得到β角；
- 基于前一步，绕着前一步的Z（大写）旋转得到γ角；
貌似第3个步骤是第1个步骤在新的平面上的重复动作。
这里的步骤并没有绕y轴旋转，只是饶了x和z(Z)轴，属于经典的欧拉角的一种。
注意，这里只是列举了一种旋转方式，其他更多的旋转方式
可参考：经典欧拉角与泰特布莱恩角_bryant angles-CSDN博客
欧拉角按照旋转轴分为经典欧拉角（Proper Euler Angle）和泰特布莱恩角（Tait–Bryan angles），共 12种旋转方式：
经典欧拉角-Proper Euler angles (z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)
使用两个轴的旋转角度表示，第一个旋转角度和第三个旋转角度都是绕同一个轴。
泰特-布莱恩角-Tait–Bryan angles (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)
使用三个轴的旋转角度表示。
现在用的比较多的是泰特布莱恩角（Tait–Bryan angles），属于欧拉角的一种。
Tait–Bryan angles 也被称为Cardan angles, nautical angles, (heading, elevation, and bank),(yaw, pitch, and roll). 我们接触的比较多的是yaw(偏航), pitch(俯仰), roll(横滚).三个变量一般对应(车体,飞行器的)z,y,x三个坐标轴.
旋转方式的区分
按照旋转的坐标系分为两种旋转方式：
内旋-intrinsic rotation（动态）:相对变换后的（当前的，自身的）坐标系做变换，以z-y′-z′′表示。′上标表达的是该旋转是以上一次旋转为参考。
外旋-extrinsic rotation（静态）:相对初始的（固定的）坐标系做变换，以z-x-z表示。
不考虑内旋与外旋时，经典欧拉角和泰特布莱恩角各有六种绕轴旋转方式;如果考虑内旋与外旋，则各有12种绕轴旋转方式。
欧拉角万向节死锁（Gimbal Lock）
局部坐标是很直观，但是导致了欧拉角有一个重大的缺陷，万向节死锁。
这种围绕选旋转前固定轴的先Z、再X、再Y的旋转操作，与其最终所预期的三个轴向可以旋转的结果并非一定是一对一的映射。某些情况下是多对一的映射，造成一些旋转自由度的缺失，也就是“死锁”
在编程中很难规避死锁问题，所以现在很多时候都使用四元数实现旋转。
其实一般我们用欧拉角是为了方便主观理解,最终我们一般还是要转换为旋转矩阵,或者四元数来参与计算.
四元数
参考：四元数快速教程【3D】 - 知乎
四元数（Quaternion）是用于旋转和拉伸向量的数学运算符。
四元数是使用一组有序的四个数字来描述3D空间中的方向或旋转的另一种方法。 它们能够唯一地描述围绕任意轴的任何三维旋转，并且不会受到万向节锁定的影响。 如果你的应用程序中的传感器或物体能够在 3D 空间中的任何位置移动，那么它们在跟踪物体方面优于欧拉角。
虽然很容易想象滚动、俯仰和偏航，但如果你正在设计一个能够自由指向空间中任何方向的系统，最终会遇到万向节锁定。
威廉·汉密尔顿于 1843 年发明了四元数，作为一种允许他对向量进行乘法和除法、旋转和拉伸的方法。
具体定义及相关概念如下：
四元数转欧拉角
四元数可以方便的表示3维空间的旋转，但其概念不太好理解，可以先类比复数，复数表示的其实是2维平面中的旋转。
四元数虽然方便表示旋转，但其形式不太直观，常常将其转换成pitch、roll、yaw的表示形式，方便观察姿态。
转换公式为：
关于欧拉角、旋转矩阵以及四元数的更多内容可自行查阅资料。
这里提供两篇做个参考:
3维旋转矩阵推导与助记_二维坐标系旋转 三阶矩阵-CSDN博客
欧拉角旋转_marker中设置欧拉角的旋转信息,-CSDN博客
姿态解算方式
MPU6050是一个六轴的姿态传感器，因此在姿态解算的时候能够测到加速度和陀螺仪数据。
DMP方式
MPU6050姿态解算1-DMP方式_mpu6050姿态解算用什么算法比较好-CSDN博客
MPU6050的姿态解算方法有多种，包括硬件方式的DMP解算，软件方式的欧拉角与旋转矩阵解算，软件方式的轴角法与四元数解算。先介绍最易操作的DMP方式。
MPU6050 自带了数字运动处理器，即 DMP，并且，InvenSense 提供了一个 MPU6050 的嵌入式运动驱动库Motion Driver，结合 MPU6050 的 DMP，可以将我们的加速度传感器和角速度传感器的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角：航向角（yaw）、横滚角（roll）和俯仰角（pitch）。
使用内置的 DMP，大大简化了四轴的代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。
DMP就是MPU6050内部的运动引擎，全称Digital Motion Processor，直接输出四元数，可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合。Motion Driver是Invensense针对其运动传感器的软件包，并非全部开源，核心的算法部分是针对ARM处理器和MSP430处理器编译成了静态链接库，适用于MPU6050、MPU6500、MPU9150、MPU9250等传感器。
使用时，将DMP的Motion Driver移植到自己的工程里就行啦。
欧拉角&旋转矩阵方式
MPU6050姿态解算2-欧拉角&旋转矩阵-CSDN博客
IMU全称Inertial Measurement Unit，惯性测量单元，主要用来检测和测量加速度与旋转运动的传感器。其原理是采用惯性定律实现的，这些传感器从超小型的的MEMS传感器，到测量精度非常高的激光陀螺，无论尺寸只有几个毫米的MEMS传感器，到直径几近半米的光纤器件采用的都是这一原理。
具体参考这篇：惯性传感器（IMU）
什么是IMU惯性传感器，它在运动领域有哪些应用？ - 知乎
惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。在导航中有着很重要的应用价值。
本小节将通过软件解算的方式，利用欧拉角与旋转矩阵来对陀螺仪与加速度计的原始数据进行姿态求解，并将两种姿态进行互补融合，最终得到IMU的实时姿态。
具体看上面的文章链接。
姿态融合
由链接文章内的分析可知，加速度计在静止时刻，根据感受到的重力加速度，可以计算出roll和pitch角，并且角度计算只与当前姿态有关。而陀螺仪是对时间间隔内的角速度积分，得到每一次的角度变换量，累加到上一次的姿态角上，得到新的姿态角，陀螺仪可以计算roll、pitch、yaw三个角。
实际上，加速度仅在静止时刻可以得到较准确的姿态，而陀螺仪仅对转动时的姿态变化敏感，且陀螺仪若本身存在误差，则经过连续的时间积分，误差会不断增大。因此，需要结合两者计算的姿态，进行互补融合。当然，这里只能对roll和pitch融合，因为加速度计没有得到yaw。
K为比例系数，需要根据实际来调整，如选用0.4。
总结：姿态解算的目的是获取MPU6050固连坐标系在参考坐标系中的姿态角(也就是欧拉角)，但是欧拉角不是直接测量的而是通过IMU(惯性测量单元)数据（陀螺仪、加速度计、罗盘等）再加上Mahony互补滤波灯算法解算出姿态角，所以姿态解算的基础就是 IMU数据。
对比和总结
对比DMP和自己进行姿态解算：
使用DMP我们可以直接得到四元数，但是如果不用DMP的话，就得自己来进行解算，根据得到的各轴角速度以及加速度，再经过滤波，接着计算出四元数或者欧拉角，然后还得进行姿态融合。前者更便捷，但是灵活度差，精度不是特别高；后者麻烦，但是算法处理得当的话，就能有很高的精度。一般应用使用DMP即可。
MPU6050姿态获取与处理
理论上只用陀螺仪就可以完成姿态导航的任务，只需要对3个轴的陀螺仪角度进行积分，得到3个方向的旋转角度的姿态数据，就可以了。但实际上存在着误差噪声等，对陀螺仪积分并不能得到完全准确的姿态，所以我们就需要用加速度计传感器进行辅助矫正。
数据获取
1、MPU6050的陀螺仪采集物体转动的角速度信号，通过ADC（模拟数字转换器）转换成数字信号采集回来。再通过通信传输给单片机。
2、加速度计则是采集物体加速度信号，并传输回来。
数据处理
通过以上的步骤，我们可以分别得出线加速度传感器与角加速度传感器的数据，接下来就要进行数据的处理与融合。
具体步骤如下
1、校准数据（零点漂移）：传感器安装在设备上总有一个初始的角度，我们设这个角度为0度，我们每一次的数据都要减去这个初始数据，得到一个相对的角度。
2、把测量值换算成相应的单位：原始数据除以它在该量程下的灵敏度就可以获得实际的物理单位。加速度的物理单位为g，角速度的物理单位为°/s。
3、滤波和数据融合：常见方法有三种：互补滤波、卡尔曼滤波、硬件DMP解算四元数。
——互补滤波：因为加速度计有高频噪声，陀螺仪有低频噪声，需要互补滤波融合得到较可靠的角度值。
——卡尔曼滤波：利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
——硬件DMP解算四元数：DMP将原始数据直接转换成四元数输出，运用欧拉角转换算法，从而得到yaw、roll和pitch。
关于姿态解算的实际应用开发，可参考网上大神提供的博客，比如：
基于STM32的四旋翼无人机项目（一）：基础知识篇
基于STM32的四旋翼无人机项目（二）：MPU6050姿态解算（含上位机3D姿态显示教学）-CSDN博客
MPU6050姿态解算和数据融合 · 给新手的两轮自平衡小车开发实战指南
