主要记录一下这两天学到的 IMU 的原理、算法、安装，以及 MPU6050 的 DMP 模式使用。

原理

IMU，Inertial Measurement Unit，全称是惯性导航系统，主要元件有陀螺仪、加速度计和磁力计等。

陀螺仪测量沿 X、Y 和 Z 轴的角位置随时间的变化率，角度需要在此基础上进行积分，主要使用科里奥利效应进行测量；加速度计测量沿 3 个轴的加速度，当 Z 轴向上放置的时候，Z 方向的加速度是+9.8m/s²。

总结来看，常见的力信号转换为电信号的基本方法都是利用电容大小与极板距离的关系，很有意思。

不同传感器有各自的特点，例如陀螺仪有温漂、积分累计误差，加速度计会受震动、物体运动影响，将这些传感器的数据融合，就能得到较为准确的姿态信息。

MPU6050 IMU 在单芯片上集成了一个 3 轴加速度计和一个 3 轴陀螺仪。以及一个可扩展的数字运动处理器 DMP（Digital Motion Processor）。它也被称为六轴运动跟踪设备或 6 DoF 设备，因为它有 6 个输出，即 3 个加速度计输出和 3 个陀螺仪输出。

关于详细原理推荐看 How MEMS Accelerometer Gyroscope Magnetometer Work & Arduino Tutorial，B 站大学也给了翻译版本：MEMS 加速度计陀螺仪磁力仪如何工作+Arduino 教程（中英字幕）[How To Mechatronics]；关于性能参数以及 STM32 下的代码（也有 DMP），推荐野火的第 44 章 MPU6050 传感器—姿态检测。

表达

IMU 获取的是目标物体的姿态，这里就设计到角度的表达方法。

从坐标系开始，这里沿用了一些航空中的定义，但是各种手册、博客、论坛没有统一的标准，大家自己写的时候一定要仔细辨别。

首先用右手定制定义一个地面坐标系，然后以载体（飞机）的质心为原点，根据运载体自身结构方向构成坐标系，如 Z 轴上由原点指向载体顶部，Y 轴指向载体头部，X 轴沿载体两侧方向。

然后采用某种飞控的定义方式（不唯一）：绕 X 轴为俯仰 Pitch，绕 Y 轴为桶滚/横滚 Roll，绕 Z 轴为偏航 Yaw，称为三个欧拉角。

但这样还不够，三个角度在叠加的时候是和顺序相关的，一种定义是先 Yaw（y 轴），再 Pitch（x 轴），最后 Roll（z 轴），得名 YPR。

用欧拉角表示姿态还有一个坑，Gimbal Lock。所以一般在数据处理的时候会使用四元数，它由三个实数及一个虚数组成，处理完毕后再把四元数转换成欧拉角。

Gimbal Lock

Gimbal Lock，翻译成万向节死锁。一句话概括：中间层级的旋转达到 90°，将内层外层自由度重合了，就会发生 gimbal lock。

首先推荐这个视频——欧拉角旋转以及万向节死锁，然后可以看看马同学的回答：如何通俗地解释欧拉角？之后为何要引入四元数？

总结来说，欧拉角的“万向节死锁”问题，是欧拉旋转定义本身造成的。在旋转前定义了三个轴的叠加顺序，会导致在某些特殊位置丢失自由度。如果改变叠加顺序（总共有 6 种），可以暂时避免，但又会出现新的死锁角。

算法

如上文所说，IMU 的多个输出需要进行数据融合，加速度计不足的地方陀螺仪正好能补上，陀螺仪欠佳的地方加速度计正好能补上，“互补”就是最简单的一个思想。

常用的算法包括一阶互补滤波、卡尔曼滤波（还没学明白，下次补上），引用几篇博文。

mpu6050 姿态解算原理 https://blog.csdn.net/qq_29413829/article/details/60973915 （四元数、索引篇）深度解析卡尔曼滤波在 IMU 中的使用 https://zhuanlan.zhihu.com/p/36374943 （卡尔曼推导） MPU6050 滤波、姿态融合（一阶互补、卡尔曼） https://www.cnblogs.com/qsyll0916/p/8030379.html （代码调试）

安装

前两天在测试的时候，由于手上 MPU6050 的排针是朝下的，所以安装的时候偷懒，直接倒装，没想到调用第三方库的时候出了不少莫名其妙的问题，气得把 IMU 拆了，没想到这时候随手开机竟然能成功，果然是倒装的问题。

假如在零重力情况下，IMU 应该是“各向同性”的，因此任意的安装姿态应该是等效的；但受到重力影响，导致 Z 轴有初始加速度，所以在一些算法里有特殊处理。特别是上文还提到欧拉角不适用于大角度，对于 Gimbal Lock 不同算法给出的叠加顺序也不一定合适。所以为了更方便地调用现成库，不给自己找麻烦，还是尽量正面向上安装吧。当然如果自己写滤波，或者只需要用到其中某一个轴的数据，完全不需要考虑这么多，只要在数据处理的时候小心各轴对应即可。

除此之外，IMU 要尽量安装在机器人可能的旋转中心，例如做直立平衡车的时候可以把 IMU 安装在车轴中心；对于无人机则是安装在重心位置，因为 IMU 希望采集的加速度是机身整体的，不希望受到自身姿态影响。

还有一点就是振动，这是在这篇 imu 减振中学到的，里面提到了很详细的机械减振方法，推荐阅读。

减振模块的固有振动频率远离机身振动的频率，避免和减振模块共振，从而达到减振的效果。可以将减振模块看做一个受迫振动系统，机身振动作为驱动输入。当减振模块的固有频率和输入频率相同时，二者共振，振动被放大。当二者远离时，输入频率被抑制，相当于减振。设计好的减振模块并不容易。简单的加橡胶管或泡棉，有时候非但起不了减振的效果，反而会将振动放大。IMU 模块越轻，减振越难，需要的减振垫要越小、越软。如果可能，最好将电池、飞控和所有其它可能的重量集中到一起来做减振。

DMP 模式

DMP，Digital Motion Processor，就是 IMU 内部的处理器，可以减轻外围微处理器的工作负担，且避免了繁琐的滤波和数据融合。

这次使用 Arduino 测试，本以为高度封装，用起来顺心很多，但没想到一些高级操作和官方库函数共用会出现奇怪的问题，总之很头疼，后续再写一篇博客记录下吧。

打开 Arduino IDE，可以在项目→加载库→管理库找到一个叫做“MPU6050”的库，其中就提供了开启 DMP 模式的“MPU6050_DMP6”例程，具体寄存器什么的就不管了，文档里都有详细说明，整理一下 demo 里开启 DMP 的整个流程：

setup(): 初始化 mpu→初始化传感器→开启传感器→设置 FIFO 的来源为 dmp→初始化 dmp→加载 dmp 程序→重置 FIFO 开启 dmp

loop(): 收到 dmp 数据已准备中断→检验 FIFO 的数据字节大小是否>=一个 dmp packet 的字节数大小→读取 dmp 数据

此外注意到接线说明，需要连接一个 INT 引脚作为外部中断。

NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch depends on the MPU-6050’s INT pin being connected to the Arduino’s external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is digital I/O pin 2.

悬空这个中断引脚的话，跑起来是这样的，DMP 能正常启动，但系统一直在等待触发。

检查了一下代码就不难理解，INT 中断输出引脚，是用来给处理器判断 DMP 的中断状态的，实现更高效、不遗漏的数据读取。

在setup()里也开启了中断检测，把 INTERRUPT_PIN 脚的上升沿绑定到 dmpDataReady 函数。

Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

dmpDataReady()只是用来修改一个标志位，其在主函数loop()中被检查。

volatile bool mpuInterrupt = false; {% raw %}// indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}{% endraw %}

所以可以在loop()里把所有相关标志位都删了，不停地访问，检查是否有有效数据，不出意外就可以在串口里看到返回值。

DMP 输出的是四元数，例程也支持多种其他返回值。

//#define OUTPUT_READABLE_QUATERNION // 返回四元数
//#define OUTPUT_READABLE_EULER // 返回欧拉角
#define OUTPUT_READABLE_YAWPITCHROLL // 返回 YPR 角度
//#define OUTPUT_READABLE_REALACCEL // 返回加速度 去掉重力
//#define OUTPUT_READABLE_WORLDACCEL // 返回加速度 去掉重力 补偿旋转
//#define OUTPUT_TEAPOT // 一个可视化的 demo 可以参考 https://youtu.be/kyX9cRxJNdo

此时也可以验证上文中提到的 Gimbal Lock，使用 YPR 模式时，当 Pitch 轴旋转约 90 度时，另两轴的数据就会出现明显异常。小心，例程的输出：Z 轴旋转是 Yaw；Y 轴旋转是 Pitch；X 轴旋转是 Roll。

此外注意到 demo 在初始化时，还手动加了一个补偿量。

// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(51);
mpu.setYGyroOffset(8);
mpu.setZGyroOffset(21);
mpu.setXAccelOffset(1150); 
mpu.setYAccelOffset(-50); 
mpu.setZAccelOffset(1060);

关于补偿量的确定，可以参考这篇 Calibrating & Optimising the MPU6050，相关讨论 How to decide Gyro and Accelerometer offsett。

和这些相关的一个奇怪现象就是，DMP 输出的数据往往需要四五秒左右稳定下来，特别是 Yaw 轴，猜测可能是一个自动校准过程。

调试完之后也发现了一位网友提供的精简版 DMP 代码（不需外部中断）。

最后附一些学习 DMP 过程中参考过的博客：对于 MPU6050 的一些问题汇总；请问如何使用 MPU6050 的 FIFO 和 DMP；mpu6050 报错 fifo overflow 解决办法。

原理#

表达#

Gimbal Lock#

算法#

安装#

DMP 模式#

原理

表达

Gimbal Lock

算法

安装

DMP 模式