六轴传感器基础知识学习：MPU6050特性，四元数，姿态解算，卡尔曼滤波

实际上，只要说到多少轴的传感器一般是就是指加速度传感器（即加速计）、角速度传感器（即陀螺仪）、磁感应传感器（即电子罗盘）。这三类传感器测量的数据在空间坐标系中都可以被分解为X,Y,Z三个方向轴的力，因此也常常被称为3轴加速度计、3轴陀螺仪、3轴磁力计。

上面3类传感器有其各自的功能特点及应用，比如加速度计可以测量设备的测斜情况，陀螺仪可以测量设备自身的旋转运动，还有磁力计可以定位设备的方位。通过它们相互组合和匹配融合又可以衍生出更多的不同应用，在叫法上通过这样简单的轴数上相加的可以变成六轴传感器或者九轴传感器。
六轴传感器：通常指的是三轴加速度计+三轴陀螺仪，三轴加速度计是检测横向加速的，三轴陀螺仪是检测角度旋转和平衡的，可以用在体感游戏上。
九轴传感器：就是三轴加速度计+三轴陀螺仪+三轴磁强计的组合，在飞行器上是广泛应用的。另外，在九轴传感器基础上在加入气压传感器，从而能获取海拔高度的数据，也有人把这些传感器组合称为十轴传感器。

认识MPU6050

MPU6050是InvenSense公司推出的全球首款整合性6轴运动处理组件。下面以这个芯片为例分析六轴传感器相关的知识点，其他的三轴或六轴传感器很多特性都是与之类似的。

某MPU6050模块：

MPU6050芯片的检测轴和方向：

图示向右为 X 轴正方向，向前为 Y 轴正方向，垂直模块向上为 Z 轴正方向。

旋转的方向按右手法则定义，即右手大拇指指向轴向，四指弯曲的方向即为绕该轴旋转的方向。
X 轴角度（滚转角 Roll）即为绕 X 轴旋转方向的角度，
Y 轴角度（俯仰角 Pitch）即为绕 Y 轴旋转方向的角度，
Z 轴角度（偏航角 Yaw）即为绕 Z 轴旋转方向的角度，
三者合称姿态角/欧拉角（Euler angles）。

滚转角、俯仰角、偏航角通过一架飞机的运动形态就容易理解为啥这样命名了。

• 偏航角Yaw：绕Z轴转动（机头水平转）
  

• 俯仰角Pitch：绕Y轴转动（飞机上下抬头）

• 横滚角Roll：绕X轴转动（飞机左右翻滚）

根据上面的信息我们应该要意识到：在项目中应用六轴传感器时，需要注意芯片的放置方向与整机前进方向的关系，否则将对软件解读运动状态造成麻烦。

下面我把 MPU6050 的有代表性的几个特性摘出来了：
1.以数字形式输出 6 轴或 9 轴（需外接磁传感器）（通过XCL、XDA进行通信）的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据（需 DMP 支持）

2.具有 131 LSBs/° /sec 敏感度与全格感测范围为±250、±500、±1000 与±2000°/sec 的 3 轴角速度感测器(陀螺仪)

3.集成可程序控制，范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器

4.自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算数据、感测器同步化、姿势感应等的负荷

5.自带一个数字温度传感器

6.可程序控制的中断(interrupt)（INT接口的功能），支持姿势识别摇摄、画面放大缩小、滚动、快速下降中断、high-G 中断、零动作感应、触击感应、摇动感应功能

7.自带 1024 字节 FIFO，有助于降低系统功耗

8.高达 400Khz 的 IIC 通信接口

系统框图：
MPU-60X0对陀螺仪和加速度计分别用了三个16位的ADC，将其测量的模拟量转化为可输出的数字量。从系统框图中可以看出加速度计和陀螺仪都有3个ADC模块，转化后的数字量最后保存到寄存器中供用户读取。

加速度计

加速度计顾名思义,就是测量加速度的.那么,我们如何认识这个加速度呢?在此用一个盒子形状的立方体来做模型，认识加速度。
我们可以把加速度计想象为一个正方体盒子里放了一个球如图所示，假设盒子没有任何受力时（包括重力），小球将处于正中心的位置，那么六个壁面感受到的压力都是0，可以理解为三轴的加速度均为0。

如果我们突然将立方体向左侧移动（我们加快加速，1G =9.8米/ S ^ 2），皮球打在了墙上X-。然后，我们测量球适用于在X轴上的壁和输出-1g值的压力。如下图

注意，加速度计反应的加速向量与当前的受力方向是相反的.如上图所示,受力方向向左,但是加速度的向量方向为右.

对于MPU6050，寄存器0x3B到0x40的6个寄存器分别保存了XYZ轴的加速度值，每个轴的值分为高八位和低八位，但注意这个加速度计的读数，以LSB为单位的，不是g（9.8米/秒^ 2），需要做转换。

MPU6050的加速度计的三轴分量ACC_X、ACC_Y和ACC_Z均为16位有符号整数，其均以重力加速度g的倍数为单位，有四个可选量程：±2g、±4g、±8g、±16g。

为了得到最终的力值，单位为g，我们
用下面的公式：
RX = ADCRx /灵敏度 （RX是转换为g单位后的结果，ADCRx是从sensor读出的加速度原始值）

此时以X轴ACC_X为例，量程选择±2g，则实际加速度转换参考下面公式：

ACCx是X轴的读数，注意最高位是符号位。
后面这部分实际上是LSB灵敏度，不同的加速度范围对应的灵敏度如下：

量程选择±2g，当x轴的计数为ADCx 时，那么对应的加速度值就是 (ADCx/16384)g
这个灵敏度与寄存器原始值支持的位数有关，上面这个表仅代表MPU6050。

加速度计的标定

当MPU6050水平放置时，±2g量程，理论上Z轴感受到重力 将读出16384。前面说到，加速度计反应的加速向量与当前的受力方向是相反的，所以重力的方向是Z轴的负方向，则加速度计的值就是正的1g，即读出正16384。同时X 轴和Y轴的读数将是0。可实际会有误差。这是由于每个芯片在制作时都不一样，数据手册上的都是理论的值，真正的芯片在水平时Z轴可能并不是16384。我们需要找到当各个轴在0g重力时的计数，1g时的读数，以及-1g时的读数，得到一个补偿值，在每次读取ADC结果后都进行补偿。这个过程我们称之为标定。用数学公式表示为:
ADCx = K*Gx + Offset
ADCx 传感器输出
Gx 真实的加速值
Offset 加速度为0g时传感器的输出
K 标度因数

陀螺仪

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

对于MPU6050，寄存器0x43到0x48的6个寄存器分别保存了XYZ轴的陀螺仪测量值，每个轴的值分为高八位和低八位，但注意这个陀螺仪的读数，以LSB为单位的，不是 deg / s，需要做转换。

每个 16位陀螺仪测量值的满量程定义在 FS_SEL（寄存器 27）。对于每个满量程的设置，GYRO_xOUT里陀螺仪测量值的灵敏度最低分辨率（ LSB）如下表：

从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值并不都是正的，请注意，当出现负数时，意味着该设备从现有的正方向往相反的方向旋转。

DMP

DMP就是MPU6050内部的运动引擎，全称Digital Motion Processor，直接输出四元数，有了四元数就可以转换成欧拉角，可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合。Motion Driver是Invensense针对其运动传感器的软件包，并非全部开源，核心的算法部分是针对ARM处理器和MSP430处理器编译成了静态链接库，适用于MPU6050、MPU6500、MPU9150、MPU9250等传感器。

FIFO

MPU-60X0包含一个可通过串行接口访问的1024字节FIFO寄存器。 FIFO配置寄存器决定哪个数据写入FIFO。 可能的选择包括陀螺仪数据，加速计数据，温度读数，辅助传感器读数和 FSYNC 输入。 FIFO 计数器跟踪 FIFO 中包含的有效数据字节数。 FIFO寄存器支持突发读取。 中断功能可用于确定新数据何时可用。

姿态融合

MPU6050的正式名称为六轴运动姿态陀螺仪传感器，之所以称其为“六轴”，是因为MPU6050是一个6轴运动处理组件，包含了3轴加速度计和3轴陀螺仪，可分别输出三轴加速度和三轴角速度，然后再根据以上原始数据在DMP(Digital Motion Processor，数字运动处理器)中使用算法进行姿态融合解算出三轴的角度数据。
常用的姿态融合算法有四元数法 、一阶互补算法和卡尔曼滤波算法，其中第三者最为常用。

卡尔曼滤波算法简述：对于姿态传感器模块来说，角度解算来源于两种类型的数据，第一种是通过加速度和磁场数据进行运动学解算可以求出三轴的姿态角，优点是没有长期漂移和误差累计，但缺点是精度差，而且仅在静态时能保证精度。第二种是通过陀螺仪测量出来的角速度进行积分运算，可以得出三轴姿态角，优点是精度高，可以进行动态测量，对加速度不敏感，缺点是有累积误差和漂移，时间越长累积误差越大。卡尔曼滤波就是一种数据融合算法，结合加速度计和陀螺仪的信息，共同来解算姿态，集合二者的优点获得在动态环境下可以准确测量姿态的方法。

欧拉角

以下图的坐标为例，飞机头是X轴方向，两翼是Y轴方向，垂直是Z轴方向，
假想当前的飞机自身有个随运动而动的坐标，然后与当前一个固定不动的坐标重合，就像动图中的初始状态，
分别对应绕Z轴转ψ，再绕Y轴转θ，再绕X轴转φ，注意这里的轴指的是随自身运动的坐标
完成后飞机处于一个新的动作状态
(ψ, θ, φ)就是ZYX欧拉角

欧拉角是用来 唯一的 确定定点转动刚体位置的 三个一组的 独立角参量。

通俗来讲的话有：

eg：你去问路得到了两种回答

1 往东经104°04′北纬30°40′走

2 往前走100m后右转

第一种令人难以理解，第二种则是相对坐标，比较具有操作性

欧拉角的思想就是采用第二种回答的方式，优点在于较好理解
可以说欧拉角每次转动的角度都是根据自身上一次动作生成的新的坐标而言的，就像一个人每次左转或右转都是相对自己面向的方向来说

相对欧拉角，还有一种固定角的思路就是对应第一种回答。
固定角，每一次旋转都是绕固定的坐标系的轴，即所谓“固定”。
每一次都是绕运动的坐标系B的轴旋转而不是绕固定的坐标系A的轴旋转，这样的三个一组的旋转称为欧拉角。

关于旋转顺序

eg：问路时候先左转30度后右转60度 和 先右转30度后左转60度 有着根本上的区别
想要让两个人动作一致，除了报先转30度再转60度还不行，必须说明先左转30度后右转60度

旋转顺序 (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z).

我们指定绕x轴旋转α,绕y轴旋转β.但是可以有多个旋转顺序, 比如:

情况1:先绕x轴旋转α,再绕y轴旋转β.得到姿态 O1

情况2:先绕y轴旋转β,再绕x轴旋转α,得到姿态 O2

得到的 O1 一般是不等于 O2 的.

对于x,y,z三个轴的不同旋转顺序一共有(x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)六种组合.我们需要明确旋转顺序,才能确定欧拉角所指的姿态.

在很多不太专业的博客中，关于欧拉角上来就是Yaw，Pitch，Roll，都没有搞清楚欧拉角是怎么定义的，是绕哪个轴在转动。

通过对比不同领域的文献，我发现关于欧拉角的定义似乎有所差别，比如在一些文献当中，是不区分固定角和欧拉角的，绕固定轴和绕运动轴的区别称为外旋和内旋。因此，为了和上面保持一致，这里就以Z-Y-X绕运动轴旋转的角度称为欧拉角，一般用Yaw，Pitch，Roll表示。偏航角yaw，俯仰角pitch，滚动角 roll

偏航角，俯仰角，滚动角似乎都是以飞机的动作来命名，即使不讲究XYZ轴，也知道是往哪个方向转动。
只能说偏航角，俯仰角，滚动角是用了欧拉角的表达方式，但是最好不要把欧拉角和偏航角，俯仰角，滚动角划等号。

四元数

提出意义：上述欧拉角是多次旋转后才能得到，那么为什么不一步到位，只旋转一次呢？那么四元数应运而生。
定义：
对于一个物体的旋转，我们只需要知道四个值：一个旋转的向量 + 一个旋转的角度。而四元数也正是这样的设计：

其中x,y,z 代表的是向量的三维坐标，w代表的是角度

四元数和欧拉角间的相互转化

1、四元数转为欧拉角

2、欧拉角转为四元数

https://blog.csdn.net/qq_40604876/article/details/119024790

https://blog.csdn.net/weixin_45263626/article/details/105230225

https://blog.csdn.net/weixin_55073640/article/details/125757469

https://blog.csdn.net/absll/article/details/124835737

https://blog.csdn.net/weixin_55073640/article/details/125757469

https://blog.csdn.net/thefist11cc/article/details/126595776