一起自学SLAM算法：4.1 惯性测量单元

连载文章，长期更新，欢迎关注：

---

写在前面

第1章-ROS入门必备知识

第2章-C++编程范式

第3章-OpenCV图像处理

第4章-机器人传感器

        4.1 惯性测量单元

        4.2 激光雷达

        4.3 相机

        4.4 带编码器的减速电机

第5章-机器人主机

第6章-机器人底盘

第7章-SLAM中的数学基础

第8章-激光SLAM系统

第9章-视觉SLAM系统

第10章-其他SLAM系统

第11章-自主导航中的数学基础

第12章-典型自主导航系统

第13章-机器人SLAM导航综合实战

---

IMU（Inertial measurement unit，惯性测量单元）是用来测量惯性物理量的设备，比如测量加速度的加速度计、测量角速度的陀螺仪等。有些IMU上除了包含加速度计和陀螺仪，还集成有磁力计和气压计。利用加速度、角速度、磁力和气压信息，通过物理学的基本知识就能计算出传感器的运动状态。由于IMU具有非常高的测量频率，而相机、轮式里程计等测量频率较低，往往将IMU与相机或轮式里程计做融合会得到更好的测量效果。

在航空航天、无人机、自动驾驶汽车、机器人、智能穿戴等领域广泛使用IMU进行运动测量和状态估计，一般从IMU设备采集到的原始数据都存在较大的误差，所以需要对测量到的原始数据进行标定和滤波。经过标定和滤波之后的数据，就可以用来做姿态融合，求出传感器的运动状态，也就是在空间中的姿态角。

4.1.1 IMU工作原理

要灵活使用IMU做各种应用开发，并开发惯性导航相关的算法，就需要先了解IMU是怎么工作的。这里首先介绍加速度、角速度和磁力的测量方法，然后讨论测量过程的性能指标，最后对芯片选型进行讲解。

1.测量方法

测量大致分为机械方法、光学方法和微机电方法。机械方法就是用弹簧质量块装置测量力学量的方法；光学方法用在高精度角速度测量上，测量基于闭合光路的萨格奈克效应；在对体积要求严格的消费电子领域，机械法中的弹簧质量块装置直接被集成到单芯片中，这就是微机电方法，也是应用最广泛的方法。下面就依次介绍加速度、角速度和地磁的各种测量方法。

（1）加速度测量

利用物体的惯性，通过牛顿第二定律F=ma就能求得物体运动的加速度a。在已知物体质量m的情况下，关键是测量出作用在物体上的力F。这里有两种测量方法，一种是利用线位移的弹簧测量，另一种是利用摆式测量，如图4-1所示。

 图4-1  测量加速度的力学原理

由于加速度是一个三维空间的矢量，所以常常用三轴加速度计进行测量，三轴加速度计模型，如图4-2所示。

图4-2  三轴加速度计模型

 测量作用在物体各个轴上的力，可以用带滑动变阻器的弹簧进行实现，也可以使用压电式器件或压阻式器件直接测量。在实际产品中最常见的是MEMS（Micro-Electro-Mechanical System，微机电系统）实现方案，也就是将物体质量块、力测量器件、机械结构等直接集成到一个芯片里面。MEMS实现方案的加速度计具有体积小、使用方便、便于生产等众多优点，广泛应用于工业和消费产品中。图4-3所示为几种常见的MEMS加速度计实现方式。

 图4-3  MEMS加速度计原理

（2）角速度测量

高速旋转的陀螺具有定轴性，也就是陀螺指向空间中的固定方向。利用这个性质，可以测量物体在空间中的旋转运动，这就是机械陀螺仪，如图4-4所示。

图4-4  机械陀螺仪

为了追求极高的精度，在航空航天和军事等领域使用的是激光陀螺仪。在任意几何形状的闭合光路中，从某一观察点出发的一对光波沿相反方向运行一周后又回到该观察点时，这对光波的相位将由于该闭合光路相对于惯性空间的旋转而不同，相位差与闭合光路的转动角速率成正比。该方法是法物理学家萨格奈克提出的，后来就称为萨格奈克效应。严格上说，萨格奈克效应要在广义相对论中进行讨论，不过光路旋转的速率远远小于光速，所以直接采用经典物理的方法计算也能得到同样的结果。激光陀螺仪正是利用萨格奈克效应设计出来的，如图4-5所示。

 图4-5  激光陀螺仪

与加速度一样，要测量物体在三维空间的旋转运动，常常用三轴陀螺仪进行测量，也就是每一个轴方向上都要放一个陀螺仪。三轴陀螺仪的具体模型跟加速度中的模型非常相似，就不赘述了。

与加速度计一样，陀螺仪在实际产品中最常见的是MEMS实现方案。将物体质量块、力测量器件、机械结构等直接集成到一个芯片里面。物体质量块被控制来回振荡，振荡过程的径向速度为v，同时以w角速度转动，这个时候物体会产生一个垂直于振荡方向的力F，这个力称为科里奥利力。只要用前面加速度计中测量力的方法测量出科里奥利力，利用其他已知条件，就能计算出物体转动角速度w，如图4-6所示。

图4-6  MEMS陀螺仪原理

 （3）地磁测量

利用地磁进行导航，已经有了悠久的历史。从古代就开始使用的指南针，是测量地磁最简单的方法，如图4-7所示。

 图4-7  指南针

在电子产品中使用的是电子磁力计，最常见的电子磁力计是根据霍尔效应设计的传感器。霍尔效应是一种磁电效应，即处在磁场中的通电导体块的两个侧边会产生电压差。利用霍尔效应，将磁场测量转化为电压测量，如图4-8所示。

 图4-8  电子磁力计

与加速度角速度一样，要测量三维空间的磁场，常常用三轴磁力计进行测量，也就是每一个轴方向上都要放一个磁力计。

2.性能指标

考虑到实用性，下面的内容将只讨论应用最为广泛的MEMS三轴加速度计、MEMS三轴陀螺仪和三轴电子磁力计。对不同性能的传感器进行比较，来帮助大家理解IMU中的各种性能指标。常见IMU芯片型号，如表4-1所示。

表4-1  常见IMU芯片型号

芯片型号	三轴加速度计	三轴陀螺仪	三轴电子磁力计	气压计
LSM303	有		有
MPU9250	有	有	有
BMA180	有
L3G4200D		有
MAG3110			有
ADIS16405	有	有	有
ADIS16488A	有	有	有	有

从表4-1中看出，有的芯片只集成了单独的加速度计或陀螺仪或磁力计，有的芯片则同时集成加速度计、角速度计和磁力计，有的芯片甚至还集成了气压计。根据不同的应用需求，选择对应的芯片型号。如果应用场景中只需要测量加速度，则选择只集成加速度计的芯片就满足需求了。如果应用场景中需要同时获取加速度、角速度和磁力计数据，则可以选择同时集成了这些传感器的芯片。

首先来对比一下各个加速度计的性能指标吧，如表4-2所示。

表4-2  加速度计性能对比

表4-3  陀螺仪性能对比然后来对比一下各个陀螺仪的性能指标吧，如表4-3所示。

陀螺仪	量程（dps）	非线性度	零偏（dps）	轴间灵敏度	噪声密度（）	温偏 dps/℃	价格
L3G4200D	±2000	0.2%	±75		0.03	±0.04	7.5元
MPU9250	±2000	±0.1%	±5	±2%	0.01	±30	18元
ADIS16405	±350	±0.1%	±3	±0.05°	0.05	±0.01	4.5千元
ADIS16488A	±480	±0.01%	±0.2	±0.05°	0.0059	±0.0025	1.5万元

最后来对比一下各个磁力计的性能指标吧，如表4-4所示。

表4-4  磁力计性能对比

3.芯片选型

上面已经对常见IMU芯片型号的性能指标进行了对比讨论，这里推荐几种选型方案。首先是消费级别应用的话，可以选择LSM303+L3G4200D组成的方案来做九轴IMU传感器，这个方案比较灵活，价格也不贵。另一种消费级别应用的推荐是直接采用MPU9250，优点是单芯片直接集成了九轴IMU传感器，并且价格非常便宜。如果是工业级的应用，可以选择ADIS16405或ADIS16488A这两款高端的芯片，当然价格会贵很多。

由于MPU9250在单芯片上集成了三轴加速度计、三轴陀螺仪和三轴磁力计，并且价格非常便宜，开发资料丰富等众多优点。下面将以MPU9250为例，展开讲解IMU原始数据采集、IMU参数标定、IMU数据滤波和IMU姿态融合这些步骤的内容。

4.1.2 IMU原始数据采集

考虑到MPU9250在广泛使用性、单芯片九轴传感器集成性和价格实惠的特点，本书选取MPU920芯片来获取三轴加速度计、三轴陀螺仪和三轴磁力计数据。怎么样才能通过MPU9250芯片得到三轴加速度计、三轴陀螺仪和三轴磁力计数据呢？这里分三个部分进行讲解，依次是硬件电路搭建、固件驱动开发和上位机ROS驱动程序。

1.硬件电路搭建

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.固件驱动开发

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

3.上位机ROS驱动程序

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

4.1.3 IMU参数标定

从IMU中测量到的数据精度对实际应用系统（如无人机、自动驾驶汽车、机器人、智能穿戴等）的整体性能影响非常大，因此需要对IMU采集到的原始数据做尽可能的补偿，以消除已知的测量误差项。如果IMU测量数据本身就存在很大的误差，也就是说输入到系统的就是错误信息，上层实际应用系统的算法做的再好也会输出错误结果。

1.良率检测

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.内参标定过程

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

3.标定模型改进

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

4.1.4 IMU数据滤波

经过标定后的IMU数据，依然存在抖动毛刺之类的噪声。因此还需要对数据进行滤波，使数据变得平滑。图4-27是截取IMU其中一个轴数据的波形，波形中明显可以看到很明显的毛刺。这里介绍几种传感器去噪的常用滤波器：均值滤波、滑动滤波、滑动中值滤波、RC低通数字滤波和IIR数字滤波。

 图4-27  IMU数据噪声

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

4.1.5 IMU姿态融合

IMU在各种惯性导航和传感器融合中应用非常广泛，在正式讲解本节的姿态融合前，有必要对一些令大家感到疑惑的名词进行解释。IMU是指惯性测量单元，测量惯性系统中的运动，即直线运动和旋转运动，对应的物理量为加速度和角速度。MARG是指磁力、角速度、重力，就是能测量这三个物理量的传感器，可以说IMU是6轴的传感器，而MARG是9轴的传感器，MARG比IMU多了一个3轴的磁力计。AHRS是指航姿参考系统，利用地球重力场和磁场为参考，计算导航物体的空间姿态（也就是“航姿”），AHRS比MARG多了一个航姿计算的过程。INS是指组合导航系统，是将两种或以上的导航设备组合在一起使用的导航系统，能够提供更精确全面的导航信息，比如航姿信息、位置信息、速度信息等。

这里有必要把IMU和AHRS拿出来讨论一下，以帮助大家理解即将要讲到的姿态融合算法的重要性。假如我们有一个理想的IMU，可以完全无误差测量运动过程中的加速度和角速度，通过姿态的相对运动推算，总能知道自己在宇宙中的实时位置。遗憾的是平时使用的都是MEMS传感器，测量到的加速度和角速度有很大误差，单纯靠相对运动推算，过不了多久推算结果就漂移到无法接受了。为了解决漂移问题，需要找一个固定的参考才行，这就是AHRS要做的事情。AHRS一方面采用陀螺仪对航姿变化进行推算，另一方面利用地球重力场和磁场的绝对参考对推算出的航姿做修正，这样就能减小漂移问题，提高航姿计算的精度。这里特别说明一下，一般情况下IMU都是广义层面的意思，同时指代IMU、MARG、AHRS等多种意思，如表4-7所示。可以根据具体讨论的内容，来判断IMU的具体范畴。本节将要讨论的IMU姿态融合，其实是AHRS讨论的范畴。

表4-7  名词解释

名词	全称	解释
IMU	惯性测量单元	三轴加速度 三轴角速度
MARG	磁力、角速度、重力	三轴加速度 三轴角速度 三轴磁力
AHRS	航姿参考系统	三轴加速度 三轴角速度 三轴磁力 航姿
INS	组合导航系统	全球卫星导航 惯性导航 视觉导航 ...... 等等

为了更进一步说明导航系统的结构，可以结合一个典型的导航系统框图来展开，如图4-30所示。导航系统的输入是各个传感器的数据，输出是各种导航信息，系统部分负责数据采集、数据校准、滤波、数据融合等处理。如果考虑所有的传感器，那么讨论的就是INS问题。下面只考虑三轴加速度、三轴角速度和三轴磁力，所以这就是AHRS问题。也就是输入三轴加速度、三轴角速度和三轴磁力，计算出航姿，行业中最通用的做法是采用卡尔曼滤波融合算法或互补滤波融合算法，下面就来分别分析这两种融合算法。

 图4-30  典型的导航系统框图

1.卡尔曼滤波

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.互补滤波

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

源码仓库

• Github下载：github.com/xiihoo/Books_Robot_SLAM_Navigation

• Gitee下载（国内访问速度快）：gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation

参考文献

【1】 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.