点击上方“小白学视觉”，选择加"星标"或“置顶”
重磅干货，第一时间送达

前面文章中我们介绍过SLAM中的两大类传感器：激光雷达和视觉传感器，并详细分析了其优缺点。而实际上，在基于移动机器人的SLAM中，还有一类很重要的传感器就是惯性测量单元（Inertial Measurement Unit，简称IMU）。

单目、双目、RGBD等视觉传感器可以获得非常丰富的环境信息，可以方便的用于三维重建、识别去过的地方（回环检测）等。而IMU传感器则可以获得移动机器人本身的运动信息，比如重心方向、俯仰角、偏航角等。而最近几年基于 camera + IMU的VIO（视觉惯性里程计）SLAM技术方案也是研究的热点，具有代表性的如OKVIS、VI-ORB等。

最近刚好调研了惯性导航系统（Inertial Navigation System，简称INS）相关知识。在此分享一下，希望能对读者有所帮助。

惯性导航系统简介

惯性导航系统是一种利用惯性敏感器件、基准方向及最初的位置信息来确定运载体在惯性空间中的位置、方向和速度的自主式导航系统，有时也简称为惯导。其工作环境不仅包括空中、地面，还可以在水下。

你知道自己的初始位置，知道自己的初始朝向（姿态），知道自己每一时刻如何改变了朝向，知道自己每一时刻相对朝向是怎样走的，把这些加一起不停地推，走一步推一步，在不考虑各种误差时，得出的结果就应该正好是你现在的朝向和位置。

但是要怎么知道自己的方向和位置是怎么改变的呢？不同的导航系统用不同的传感器，有不同的方法，比如里程计用车辆上轮子转的周数，多普勒计程仪像蝙蝠一样往水底发射声波……而惯性导航之所以叫【惯性】导航，就是因为使用的是【惯性器件】，也就是加速度计、陀螺仪、磁力计、气压计等。

因此，惯性导航系统至少需要包括含有加速度计、陀螺仪等的惯性测量单元和用于推理的计算单元两大部分。

惯性测量单元

想象一个笛卡尔坐标系，形如下图所示，具有x轴、y轴和z轴，传感器能够测量各轴方向的线性运动，以及围绕各轴的旋转运动。这就是所有惯性测量单元的根本出发点，所有惯性导航系统都是据此而构建。

加速度计

加速度计测量加速度，利用的原理是 a=F/M，测量物体的“惯性力”。加速度计在惯性参照系中用于测量系统的线加速度，但只能测量相对于系统运动方向的加速度（由于加速度计与系统固定并随系统转动，不知道自身的方向）。  这可以想象成一个被蒙上眼睛的乘客在汽车加速时向后挤压座位，汽车刹车时身体前倾，汽车加速上坡时下压座位，汽车越过山顶下坡时从座位上弹起，仅根据这些信息，乘客知道汽车相对自身怎样加速，即向前、向后、向上、向下、向左 或向右，但不知道相对地面的方向。

可以通过对加速度进行解算，求得角速度，但由于精度不高，不具有很好的使用价值。但是加速度计可以辅助陀螺仪进行角度解算。

陀螺仪

陀螺在惯性参照系中用于测量系统的角速率。通过以惯性参照系中系统初始方位作为初始条件，对角速率进行积分，就可以时刻得到系统的当前方向。这可以想象成被蒙上眼睛的乘客坐在汽车中，感觉汽车左转、右转、上坡、下坡，仅根据这些信息他知道了汽车朝哪里开，但不知道汽车是快，是慢或是否汽车滑向路边。

我们现在智能手机上采用的陀螺仪是采用了MEMS微机电技术的MEMS陀螺仪，它需要参考其他传感器的数据才能实现功能，但其体积小、功耗低、易于数字化和智能化，特别是成本低，非常适合手机、汽车牵引控制系统、医疗器材这些需要大规模生产的设备。

磁力计

磁力计/地磁场传感器，它有个通俗的名字：电子罗盘。当加速度传感器完全水平的时候，可以预料，重力传感器无法分辨出在水平面旋转的角度即绕Z轴的旋转无法显示出来，此时只有陀螺仪可以检测。

 陀螺仪虽然动态十分快速，但由于其工作原理是积分，所以在静态会有累计误差，表现为角度会一直增加或者一直减少。于是我们会需要一个在水平位置能确认朝向的传感器，这就是如今IMU必备的第三个传感器，地磁场传感器，通过这3个传感器的相互校正，我们终于在大的理论上可以得到比较准确的姿态参数了。

气压计

气压传感器用于检测大气压强的仪器,实际应用当中气压传感器可作高度计。在惯导系统中有时通过增加气压计增强Z轴动态与精度。

传感器对比

导航系统传感器功能示意图

每个传感器都有自己的优点和缺点，相互配合，取长补短才能达到比较好的导航效果。

计算单元

计算单元主要由三部分组成：姿态解算单元，积分单元和误差补偿单元。

• 姿态解算单元的主要任务是负责将测量得到的惯性数据由载体自身的坐标系转换到地球坐标系。

• 加速度积分单元负责在系统所提供初始位置及速度的基础上，对运动传感器的信息进行整合计算，不断更新当前位置及速度。

• 误差补偿单元负责对积分单元的输出进行适当的修正，提高定位和姿态精度。

计算单元流程示意图

姿态解算单元

姿态就是指载体的俯仰/横滚/航向情况。姿态是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系。姿态有多种数学表示方式，常见的是四元数、欧拉角、矩阵和轴角。他们各自有其自身的优点，在不同的领域使用不同的表示方式。

在实际应用惯性导航的时候，惯性测量器件是直接安装在运动载体上的，因此惯性器件测得的角速度和加速度的数值都是在载体运动坐标系下的量。而研究者在实际的操控过程中是在一个静止的坐标空间中来衡量这些被测数值的，所以这里涉及到不同坐标系之间的相互转换问题。

例如，地球坐标系是固定的R，载体上有自身的坐标系r，r在R中运动，那么如何知道r和R的角位置关系呢，也就是怎么知道载体相对于地球这个固定坐标系R转动了多少航向，侧翻了多少机身或者调整了多少俯仰。这就是姿态解算。

姿态解算是一个非常复杂的过程，并不像我们认为的那么简单。

姿态解算（altitude algorithm）,也叫做姿态分析、姿态估计、姿态融合。姿态解算是指根据IMU数据求解出姿态。载体根据陀螺仪的三轴角速度对时间积分得到的俯仰/横滚/航向角，这是快速解算。快速解算得到的姿态是存在误差的，而且误差会累加，如果再结合三轴地磁和三轴加速度数据进行校正，得到准确的姿态，这就是深度解算。

快速解算得到的数据一般是不能够直接使用的，因为误差太大。从惯性导航出现、发展到现在，人们总结出了多种姿态矩阵的解算方法，目前主要应用的有欧拉角、方向余弦、旋转矢量和四元数等算法。

姿态矩阵算法对比

加速度积分以及位置计算

姿态解算是惯性导航系统的第一步，加速度积分以及位置计算是第二步，且必须在第一步之后完成。

主要分为

1）、根据姿态解算得到的姿态矩阵，将载体坐标系下的加速度转换为地球坐标系下的三轴加速度

2）、在给定初始位置的基础上，对1）步输出的加速度关于时间进行积分，得到速度

3）、对速度关于时间再次积分，得到位置

而关于如何积分，属于数值计算部分的内容，在此就不展开了。

误差补偿算法

在惯性导航系统中存在不可消除的位置积分误差和传感器误差,在导航过程中，小误差会随时间累积成大误差，其误差大体上与时间成正比，因此需要不断进行修正。现代惯性导航系统使用各种信号（例如全球定位系统及磁罗盘等）对其进行修正，采取控制论原理对不同信号进行权级过滤，保证惯性导航系统的精度及可靠性。

惯性导航系统由很多部分组成，误差补偿算法是惯性导航系统中不可或缺的一部分。为了解决误差累积问题可以考虑设计滤波环节，算法的输入分别为姿态角值和位置，输出为最优估计姿态角和位置值。

常用的滤波算法包括：卡尔曼滤波算法、UKF滤波 、平方根无迹卡尔曼（SRUKF）滤波算法、GP-SRUKF滤波算法等。

组合导航

组合导航的基本原理是利用信息融合技术，通过最优估计、数字滤波等信号处理方法把各种导航系统如无线电、卫星、天文、地形及景象匹配等导航系统的结合。以发挥各种导航技术优势，达到比任何单一导航方式更高的导航精度和可靠性。

常见有以惯性导航和 GPS 卫星导航组合的（INS/GPS）导航系统。与惯导相比 GPS 具有成本低，导航精度高，且误差不随时间积累等优点，GPS 导航系统输出的导航信息作为系统状态的观测量，通过卡尔曼滤波对系统的状态（位置、速度等）及误差进行最优估计，以实现对惯导系统的校准和误差补偿。而惯导系统自主、实时、连续等优点可弥补 GPS 易受干扰、动态环境可靠性差的不足。

惯性导航系统应用

惯性系统最先应用于火箭制导，美国火箭先驱罗伯特•戈达尔试验了早期的陀螺系统。二战期间经德国人冯布劳恩改进后，应用于V-2火箭制导。战后美国麻省理工学院等研究机构及人员对惯性制导进行深入研究，从而发展成应用于飞机、火箭、航天飞机、潜艇的现代惯性导航系统，然而成本及复杂性限制了其可以应用的场合。

国家的很多战略、战术武器，以及洲际飞行的民航飞机等，都必须依赖惯性导航系统或者惯导系统与其他类型的导航系统的组合。它的造价比较昂贵，一台导航级的惯导系统，至少要几十万，而这种精度的导航系统已经足够配备在波音747这样的飞机上了。现在，随着MEMS惯性器件技术的进步，商业级、消费品级的惯性导航才逐渐走进寻常百姓家。

仅采用手机里的加速度计和陀螺仪，通过多传感器融合算法，在多数应用条件下（载体受到的加速度在大多数时间内主要是重力加速度的情况下），可以获得较好的姿态信息。但如果仅依靠这两个传感器来做传统的惯性导航，由于长期精度问题，无法获得有价值的位置数据。如果在手机上实现类似应用的话，通常配合GPS来实现此类功能。某些没有GPS信号的地方，比如大型场所的室内手机定位，目前通常通过惯导来做动态，通过室内基站（Cell/WiFi/BT）来修正长期精度，某些新型手机增加的气压计也主要是为了此类场合使用。

下载1：OpenCV-Contrib扩展模块中文版教程

在「小白学视觉」公众号后台回复：扩展模块中文教程，即可下载全网第一份OpenCV扩展模块教程中文版，涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。

下载2：Python视觉实战项目52讲

在「小白学视觉」公众号后台回复：Python视觉实战项目，即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目，助力快速学校计算机视觉。

下载3：OpenCV实战项目20讲

在「小白学视觉」公众号后台回复：OpenCV实战项目20讲，即可下载含有20个基于OpenCV实现20个实战项目，实现OpenCV学习进阶。

交流群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~