IMU选型及其AHRS算法分析

MEMS IMU的型号多种多样，如何给自己的机器人选择一个合适的IMU至关重要。本文从IMU常用参数，选型需要测试哪些IMU特性，以及如何对IMU数据进行处理这三部分进行介绍。

IMU 简介

IMU（Inertial Measurement Unit）即惯性测量单元，能够测量物体三轴加速度及角速度，一般用于系统的测量环节，估计物体位姿。IMU一般包含一个三轴的加速度计和一个三轴的陀螺仪，加速度计检测物体在载体坐标系统独立三轴的加速度信号，陀螺检测载体相对于导航坐标系的角速度信号，根据测得物体在三维空间中的角速度和加速度可以解算出物体的姿态。MEMS IMU价格便宜，体积小，在导航、无人机、VR、机器人以及智能手环等诸多领域得到广泛应用。IMU的检测精度对系统的整体性能非常重要，如果IMU检测数据噪声很大，系统得到的反馈就是错误的，就好比人的眼睛耳朵等感官得到了错误的信息，如何能够行动自如？系统底层是根基，如果系统底层不稳定，上层功能也很难做好。

IMU常用参数

这里列出常用的几款消费级MEMS IMU参数对比，IMU分别是

  • ==BMI055==(Realsense T265 D455使用)
  • ==BMI088==(小觅彩色版和傲览Avia使用)
  • ==ICM-42686==
  • ==ICM-42688==(高通RB3，RB5计算平台使用)

Img

最大量程

ICM-42686的加速度计量程最大是32g，陀螺仪的最大量程是4000°/s，BMI088的加速度计量程是32g, BMI055，ICM-42688都是16g。这里需要根据机器人的应用场景选择合适的量程，量程越大其灵敏度会随之下降，量程太小可能会有超量程情况。

ADC位数

BMI055的加速度计ADC位数最小是12bit，分辨率最差。BMI088、ICM-42686和ICM-42688都是16bit，分辨率一致。

灵敏度

BMI088、ICM-42686和ICM-42688的加速度计灵敏度都是2048 LSB/g，BMI055的加速度计灵敏度最低是128 LSB/g。LSB是指传感器输出数字量的最低有效位。例如，BMI088的ADC位数是16位，能表示的最大数字是65536，其在16g量程时的灵敏度就是65536/32=2048 LSB/g，即加速度是1g时ADC输出的数值就是2048，数值越大代表了灵敏度越高，以上可以看出灵敏度与ADC的位数和量程有关。陀螺仪同理。

零偏

BMI088和ICM-42688的加速度计零偏都是20mg，BMI055的零偏最大是70mg，TDK的陀螺仪零偏比BMI稍微小一点。

零偏温漂系数

零偏温漂系数反映了加速度和陀螺仪零偏的温度敏感性，从手册上看BMI088、ICM-42686和ICM-42688相差不大，有条件最好实际测试对比。

非线性

理想情况下，我们认为在量程范围内传感器数据是线性的，实际中传感器的变化是非线性的如图2所示，在使用IMU之前需要对其非线性特性进行测试，如果非线性太严重，要进行非线性的的校准，这类的校准方法有很多，比如比例校准，二次拟合校准等，下边以最简单的比例校准为例： Img Img

零偏加速度敏感性

理想情况下，陀螺仪的输出应该对加速度不敏感，由于机械设计不对称和/或微加工不够精确，造成陀螺仪的测量可能会受到外部加速度的影响。BMI055和BMI088的g敏感度不差，都是0.1°/s/g 或 360°/h/g，需要注意的是大多数陀螺仪的g敏感度会随振动频率变化而变化，因此补偿方案将变得复杂，要求根据频率改变敏感度。

非正交误差

理想情况下，加速度计和陀螺仪的X，Y和Z轴是完全正交的，即各轴的两两夹角为90度，但是由于结构的加工误差，很难做到完全正交。Cross-Axis Sensitivity为1%表示: 假设X轴的加速度为1g，理想情况下Y轴是无分量的。但是实际有1%的分量耦合到Y轴: 1g*1%=0.01g=10mg。

IMU特性测试

零偏重复性

全称是零偏逐次上电重复性，理想情况下，IMU在相同外界条件下每次上电的零偏不变，但是实际环境中，在相同的外界条件下IMU每次上电的零偏会有差别。测量方法是在工作温度下将IMU多次上电，记录每次上电以后的零偏大小，然后统计其差异。以BMI088为例，其加速度零偏重复性如下。 Img

零偏温漂特性

对于高精度IMU模块，厂家会对每个IMU模块进行温漂矫正，而对于低端MEMS IMU芯片，不可能每个都做温漂标定和补偿，因此厂家往往给个零偏温漂系数。测量方法是将IMU芯片加热，记录整个温度区间内的零偏大小，拟合零偏温度特性曲线，观察零偏在某个温度范围内是否存在异常情况。测试结果可以参考零偏温度滞回特性。

零偏温度滞回特性

零偏温度滞回特性指的是IMU在温度上升阶段和温度下降阶段对应的零偏可能会不一致。测量方法是将IMU芯片加热然后降温，然后再加热降温，多测几次，观察IMU数据在对应温度的零偏是否一致。 Img

以BMI088为例，将其来回加热降温三次，其陀螺仪零偏温度滞回特性如下。 Img

振动特性

振动特性指在振动情况下，零偏随振动频率的变化特性。有些IMU芯片在高频振动下，频率特性会出现异常，对于无人机场景，一般要做振动特性的测试，如果IMU出现异常频率特性，可以考虑加减震装置。

应力特性

应力特性是指IMU芯片贴到PCB板上以后，PCB板会对IMU施加应力，从而造成IMU零偏发生变化。如果IMU受到应力后零偏变化较大，则需要将IMU贴到PCB板上以后再次矫正零偏。以BMI088为例，测试IMU模块贴到PCB板前后的零偏变化如下。 Img

IMU校准标定

十二面体校准步骤：

  1. 将IMU固定在三轴正交的立方体（治具）上;

  2. 将治具一面放置在校准面上，静置，采集一段时间数据；

  3. 将治具原地旋转180度，静置，采集一段时间数据；

  4. 将两次采集到的数据求平均，作为一次测量值；

  5. 重复步骤2～3，测量其它五个面数据 Img

姿态解算

姿态解算的稳定性与否是影响手机自包含传感器定位精度的一个重要因素，在经典的惯导系统中，通常包含高精度的陀螺仪，可直接通过数值积分的方法对载体姿态进行解算。随着MEMS技术的发展，大量低成本的MEMS传感器件开始集成到智能手机中，Apple公司在2010年发布的iPhone 4是最早集成MEMS惯性器件的手机，乔布斯老爷子在发布会现场演示了MEMS陀螺仪如何应用于智能手机的游戏中，引领了MEMS器件与智能手机结合的潮流。

与高精度惯导器件不同的是，MEMS惯性器件的输出常常包含较大的噪声误差，因而会使得姿态解算的结果快速发散，导致精度难以维系。为了弥补MEMS陀螺仪器件的输出噪声，常常把陀螺仪，加速度计和磁力计三者组合在一起进行姿态的解算以实现更高的精度，这类系统称为航姿参考系统（AHRS）。AHRS的应用范围极其广泛，在飞行控制，机器人导航以及SLAM领域均有着它的身影，更是基于智能手机室内定位的核心技术之一。本文主要介绍三类常用的AHRS算法模型以及它们各自的优缺点，对于坐标系、姿态角的描述方法（欧拉角、四元数、旋转矩阵）等基础概念大家可以参阅前文中推荐的教材。

模型一：互补滤波算法

当前的开源PX4源代码里面默认使用的就是基于mahony的互补滤波算法（explicit complement filter）进行姿态解算的，该算法的特点是利用陀螺仪，加速度计和磁力计输出的互补特性进行融合解算，通常陀螺仪的输出具有高频特性，即在载体进行高速运动的情况下较为敏感，而加速度计和磁力计正好相反，在载体运动变化较小的情况下精度较高。 Mahony算法即所谓的互补滤波算法，通过PID反馈控制器把误差量反馈补偿修正陀螺仪的误差。算法原理：根据加速度计和地磁计的数据，转换到地理坐标系后，与对应参考的重力向量和地磁向量进行求叉乘误差，这个误差用来校正陀螺仪的输出，然后用陀螺仪数据进行四元数更新，再转换到欧拉角。通俗一点的解释就是使用加速度计输出来修正陀螺仪的横滚角和俯仰角误差，使用磁力计输出来修正陀螺仪的航向角误差，从而给出一个更加平稳，发散速度更慢的姿态融合结果。 关于这一块，现在研究的比较多就是如何实现自适应调参。固定的参数不能获得所有情况下的最优运动姿态角，可以设计参数可调的自适应算法在不同运动状态下进行调节参数的大小。其参数调节规则为：正常运动状态情况下，Kp和Ki值取为系统初始化值；当运动体具有较大运动加速度或姿态变化剧烈时，应选择较小的Kp值（可取其初始化值的0.1倍），而Ki值应在同一数量级内适当取大一点。具体取值需根据实际应用系统选取。 Mahony互补滤波的原理主要体现在它的"互补"两字上。 一般的惯性测量单元由陀螺仪与加速度计组成。而陀螺仪是低频器件，主要的误差是因为飘移引起；而加速度计是高频器件，加速度数值误差变化量在很短的时间内会变化很大。Mahony就是用加速度计去校正陀螺仪的误差。 应用上，用Mahony的算法运算量很小，速度很快，效果也挺好。此算法最先为了固定翼开发，用在某些载具场景上也是不错的选择。 由于姿态估计的方向余弦矩阵（DCM）/互补滤波方法的计算量比Kalman滤波方法少一个数量级，因此Arducopter的开发人员选择了这种方法。 Mahony互补滤波算法相关论文：《AComplementary Filter for Attitude Estimation of a Fixed-Wing UAV》；《NonlinearComplementary Filters on the Special Orthogonal Group》 Img

模型二：梯度下降算法

The Gradient Descent是madgwick在2010提出来的一套算法，其核心思想是将用加速度计和磁力计通过梯度下降法得到的姿态四元数与由陀螺仪积分得到的姿态，进行线性融合，得到最优的姿态。在算法实现过程中需要了解目标函数的建立，梯度下降法是根据目标函数求取误差向量的最小值。 该算法首先是只用陀螺仪数据根据一阶毕卡更新四元数，然后才是梯度下降法对加速度数据和磁力计数据进行处理以便求出最小的误差四元数，最后根据权重值（线性）把两者融合到一起得到最终的姿态四元数。使用梯度下降法就是对加速度数据和磁力计数据和相应参考向量求取误差量，这一部分和 mahony互补滤波算法非常类似，mahony_base是利用向量的叉乘求取量测值和参考向量的误差的，而梯度下降法是根据反梯度方向迭代求取最小误差的（理论基础就是构造目标函数使用梯度下降法求取误差向量的最小值）。过程中最重要就是参数β需要通过实验进行确定gyro的测量噪声（measurement error）。 梯度下降是一种在机器学习里用的比较多的优化算法，用来减小真实值与测量值之间的误差。数学建模的时候偶尔也会见到，基本上就是一种最小化误差的优化算法，Madgwick的算法用加速度计和磁力计的数据对IMU数据进行补偿，因此可以上电自动收敛到当前姿态。 采Madgwick梯度下降姿态解算算法，较以前的卡尔曼滤波方法大幅降低了下位机对数据迭代的次数，采用此方法仅需10Hz的采样速度即可收敛至当前载具的姿态 首先它的好处就是对迭代次数、传感器采样频率要求很低，而这恰好弥补了Kalman滤波器的缺点——吃性能。也就是说，梯度下降中的"下降"可以理解为使一个函数的输出值与理论值之间误差逐步下降，用在姿态解算上也就是使解算姿态与真实姿态之间误差逐步下降。 Img 该算法适用于IMU和AHRS系统，其明显特点就是计算量小（computationally inexpensive），低频有效性（比如10hz）。Madgwick说该算法最后的精度比kalman_base算法要好，我也不知道，没实际测试过；基于梯度下降的数据融合算法能够显著降低对处理器速度的要求。

对梯度下降姿态解算算法感兴趣的可参考Madgwick论文：《An efficient orientation filter for inertial and inertial magneticsensor arrays》的和《Estimation of IMU and MARG orientation using agradient descent algorithm》。

模型三：EKF姿态融合算法

EKF是出了名的计算量大，因为算法实现过程中包含了各种矩阵运算，对处理器的运算速度和精度要求很高对MCU的主频要求比较大，再加上系统的非线性，难以建立稳定可靠的状态方程以及合适的过程噪声协方差阵Q以及测量噪声协方差阵R。一言以蔽之，就是CPU计算负荷大。但是理解了KF以后，对整合滤波过程还是比较清晰的，很多公司都要求必须会EKF算法，优势就是在于处理过程中的预测和修正，其中一点就是可以估计陀螺仪的bias；在算法的计算处理过程中每一次都在不停的修正K和bias，相当于动态自适应算法了，主要就是调好Q、R。所以终极目标就是把EKF搞懂，虽然比较难，但是必须知难而上。

关于EKF有许许多多的模型，有直接四元数建立模型的，也有通过姿态角和bias建立模型的，在这里讲解一篇论文中的模型，希望能够抛砖引玉。（相关论文：《A Double-Stage Kalman Filter for Orientation TrackingWith an Integrated Processor in 9-D IMU》）

按照习惯，推导过程中使用的导航坐标系为东北天坐标系（ENU），IMU的载体坐标系为载体坐标系为：右、前，上,欧拉角的旋转顺序为(偏航、俯仰，滚转)。算法主要框架如图： Img 状态转移矩阵推导： Img 观测矩阵推导： Img 参考链接