UAV爱上了VIO

资料来源：《面向无人机的视觉-惯性里程计算法研究》

近年来随着计算机和人工智能的发展，涌现出了一大批高新产业，例如智能机器人、自动驾驶、AR、VR，无人机…这些产品的出现不仅在一定程度上减轻了人们的负担，同时又可以代替人类去做那些比较危险的工作，不断提高人们生活的智能性。

扫地机器人的出现极大的便利了人们日常生活，然而早期的扫地机器人智能实现室内的简单清扫，在房间内随机游走，经常出现碰壁现象。如今随着定位与构图技术的不断发展及其在扫地机器人中的应用，目前的扫地机器人已变得非常智能，可通过传感器对室内的环境进行扫描建图，并实现自主规划式清扫，还能做到自主回充断点续扫等功能。

扫地机器人要真正实现智能清扫，至少要做到以下几点：

1、 知道自己在哪（定位）：也就是扫地机器人在工作过程中需要清除自己所在房间的具体位置；

2、 了解周围环境是什么样子（建图）：也就是需要知道整个房间的地面结构信息；

3、 该如何到达指定地点（路径规划）：当扫地机器人要到达某个指定地点时，它能以最优路线到达目的地，并绕开障碍物。

有以上三大能力的扫地机器人就能变得非常智能了，不会像无头苍蝇一样在室内随机乱跑，而是可以从任意位置出发，根据建立好的地图实现规划式清扫。

同时近些年随着大疆、parrot、AEE等品牌的发展，无人机这一新兴事物也在不断增加对人们生活的影响。无人机的导航需要依靠周围环境的地图和无人机对自身的有效定位这两个必要条件。目前无人机在已知周围环境的情况导航的技术已经比较成熟，因而人们的研究热点逐渐转移到实现无人机在未知环境下的导航。而定位是导航的前提，这就回到了跟扫地机器人一样的问题。

SLAM的全称（Simultaneous Localization And Mapping）即（同时定位与地图构建）是无人机在未知环境中定位的一种解决方案，对无人机自主飞行的实现具有重要意义。SLAM就是当无人机身处一个对于自己来说十分陌生的环境，并从环境中未知地点开始运动，运动过程中通过传感器感知周围环境的特征，结合自身运动状态和从环境中的观测量恢复出环境的3D结构，进而到构建未知环境地图的目的。通过该技术无人机可以再不依靠GPS信号的基础之上，根据周围的环境地图进行自主定位。

目前用于SLAM的传感器主要分为激光雷达及视觉传感器两种，其中激光雷达常用于地面端，视觉传感器常用于空中端设备。

激光SLAM采用单线或多线激光雷达，它具有测量迅速、信息丰富的特点。激光雷达目前广泛应用于机器人以及无人驾驶领域。它可以由自身发射激光，并且在碰到周围物体后反射回来，反射回来的激光就带来了物体的信息。从中可以提取出距离和角度信息。激光雷达发发射出大量的激光，每条激光都带回来了距离和角度信息，这样每条激光的信息浓缩为一个点，这么多条激光就构成了点云。通过不同点云之间进行匹配对比，可以从中计算出激光雷达在两个点云的时间间隔内的运动状态和距离，进而得到了无人机或无人车的运动状态。

如图所示用激光雷达测量得到的距离较为准确，因此用激光来构建的环境地图也较为准确，地图中的距离信息更具有说服力。同时激光雷达测距的误差计算较为简单，可以很容易的精确实际的测量数据。根据反射回来的激光带带来的距离和角度信息形成的不同点云之间进行匹配，可以容易得出环境的3D结构。这给机器人和无人驾驶汽车的路径规划和导航带来了很大的便利，但同时激光带回来的信息太过杂乱，数据处理较为困难。

跟激光雷达相比较SLAM也有其独特的优势。视觉SLAM通过视觉传感器可以获取到周围环境中大量的文理信息，针对不同场景，它具有比激光SLAM更强的辨识能力。近年随着相机硬件技术和非线性优化理论的不断发展，视觉SLAM吸引了越来越多的研究人员。

近年来随着技术不断的更新发展，定位和导航在生活中的广泛应用中引起了越来越多的关注，特别是对于自动驾驶汽车，服务机器人和无人驾驶飞行器等。用于定位和导航的几种类型传感器：全球导航卫星系统（GNSS），激光雷达，惯性测量单元（IMU）和相机。然而，它们各自有明显的缺点：在晴朗的天空下，GNSS可以提供可靠的定位信息；激光雷达遇到玻璃表面物体会产生反射；来自IMU的测量结果是有噪声的，因此惯性导航系统可能由于误差累积而快速漂移；单目SLAM只能恢复到一定比例的运动轨迹，并且当相机快速移动或光照变化剧烈时，往往会丢失跟踪信息。因此，传感器的融合方法，特别是视觉-惯性导航系统，引起广泛关注。来自IMU的加速度和角速度信息可以明显改善视觉SLAM系统无法应对快速移动场景的困难，此外，IMU和相机重量轻且成本低，因此广泛用于民用领域。

单一的传感器不能适用所有的场景，比如视觉传感器在大多数文理丰富的场景中效果很好，但是如果遇到玻璃，白墙等特征较少的场景，基本上无法工作，而通过多个传感器的融合可以达到理想的定位效果。

虽然IMU长时间使用有非常大的累积误差，但是在短时间内，其相对位移数据又有很高的精度，所以当视觉传感器失效时，IMU数据能够提高系统定位的精度。视觉和惯性测量的互补特性，使它们特别适合融合。此外，这两种传感器在大多数智能手机中都有，可以有效应用于移动手机进行视觉-惯性联合定位、建图。

VIO的全称是Visual-Inertial Odometry，中文名为视觉惯性里程计。对无人机的定位来说，实时性是一个重要的条件，视觉里程计常常要在计算代价和精确度之间做权衡。VIO可以在同样的条件下，获得更好的效果。同时价格成本也是重要的指标，相比于昂贵的激光雷达，摄像机和IMU是低价普适的传感器。

Mono SLAM是A.J.Davison在2007年提出的第一个实时的单目视觉SLAM系统。Mono SLAM以EKF（扩展卡尔滤波）滤波作为系统的处理后端，以视觉传感器当前的运动状态和运动过程中观测到的所有路标点作为优化的状态量。在优化过程中随着传感器的运动，不断更新优化状态量的均值和方差。在EKF中，每个特征点的位置服从高斯分布，可以用一个椭球表示它的均值和不确定性，它们在某个方向上特长，说明在该方向上越不稳定。该方法的特点：场景窄、路标树有限、稀疏特征点易丢失等。

为了提高系统的处理效率，Klein在2007年提出PTAM，这是第一个将跟踪和建图分成两个线程同时进行，并行化处理，而且首次区分出系统的前后端。前段是实时性较强的图像数据处理，后端为计算量巨大的优化部分。后续许多视觉SLAM系统设计也采取了类似的方法。PTAM是第一个使用非线性优化作为后端的方案，而不是滤波器的后端方案。提出了关键帧（key frames）策略。视觉传感器会带来大量的图片，每个图片上又有丰富的信息，全部处理这些信息对计算设备压力太大，因此PTAM提出在大量图片中挑选具有代表意义的图片作为关键帧处理，从这些关键帧中恢复出其运动轨迹，大大较小了计算压力。

ORB-SLAM是继承PTAM思路并表现最好的一位，是Mru-Artal在2015年提出的。它围绕ORB特征计算，包括视觉里程计与回环检测的ORB字典。ORB特征计算效率比SIFT或SURF高，又具有良好的旋转和缩放不变性。ORB-SLAM首次创新地使用了三个线程完成SLAM：1、实时跟踪特征点的跟踪线程；2、局部光速发平差（Bundle Adjustment）的优化线程；3、全局位姿图的回环检测与优化线程。该方法的缺点：每幅图像计算一遍ORB特征非常耗时，三线程结构给CPU带来了较重负担。稀疏特征点地图只能满足定位需求，无法提供导航、避障等功能。

RichardA在2011年提出的DTAM是单目VSLAM系统，同时它也是首个可以构建稠密地图的系统。他是通过计算系统提取到的关键帧中的空间规范能量范数来实现的。该方法的缺点是：计算量非常大，需要GPU并行计算。DTAM假设光度恒定，对全局照明处理不够鲁棒。

LSD-SLAM是J.Engel等在2014年提出的。它不是提取特征点而是直接提取特征块，通过关键帧中的特征块匹配对比，恢复出环境的3D结构和自身的运动状态在CPU上实现了半稠密场景的重建。该方法的缺点：相机内参对系统的准确性有致命影响，而且相机如果进行快速运动，没有办法跟踪到特征快，容易跟踪丢失。

SVO（Senmi-direct Visual Odoemtry）是一种半直接法的视觉里程计，它由Forster在2014年提出，该系统是特征点和直接法的混合使用：跟踪了一些角点，然后像直接法那样，根据关键点周围信息估计相机运动及位置。由于不需要计算大量描述子，因此速度极快，在消费级笔记本电脑上可以达到每秒300帧，在无人机可以达到每秒55帧。该方法缺点是：舍弃了后端优化和回环检测，位姿估计存在累积误差，丢失后定位困难。

DSO（Direct Sparse Odometry）是基于高度精确的稀疏直接结构和运动公式的视觉里程计的方法，是不考虑几何先验信息，能够直接优化光度误差。而且系统还进行了光度标定的模型构建，首次将滑窗应用到SLAM系统中来，对以后的SLAM系统发展产生了深远的影响。滑窗的理论提出主要是为了降低优化计算的复杂度，使整个系统的计算量不会随着时间的增长无休止的增加。滑窗只计算相邻的几个关键帧，并且随着时间的增加窗口中的帧数并不会变化。除此之外DSO还能计算仿射的亮度变化，对深度计算也进行了优化，但是该方法没有回环检测。

OKVIS是由Stefan Leutenegge提出的基于双目+惯导的视觉里程计，属于VIO（Visual Inertial Odometry）。OKVIS的算法流程是通过IMU测量值对当前状态做预测，根据预测进行特征提取和特征匹配，3D特征和二维图像特征构成优化中重投影，同时预测的IMU的状态量和优化的参数之间构成IMU测量误差，这两项误差放在一起做优化。

ROVIO是由Bloesch Michael 等人在2015年提出的，它阐述了一个将视觉信息和IMU信息进行紧耦合的一种视觉惯性测量单元。数据融合的方法主要是通过迭代卡尔曼滤波来进行的。对于视觉方面的信息，作者主要是通过将路标点在图像中对应的点周围的图像块作为路标点的描述子，从而得到光度误差。然后光度误差进行变换得到卡尔曼滤波中的innovation term，进而进行滤波状态的更新。

随着人们越来越多地将SLAM技术应用到如小型无人机（UAV）或手持移动设备这样的小型平台上，相机和惯性测量单元（IMU）传感器渐渐成为定位和构图领域的研究焦点。得益于芯片和微机电系统（MEMS）器件工业的快速发展，这些传感器具有高精度，小型化和低成本等特点，通过各个传感器联合提供的信息来补偿它们各自信息的不足。此外，生物学的研究表明，人类和一些动物的导航也会部分依赖运动感知形式的视觉的各种各样的组合。

根据传感器的融合测量的方式不同，视觉惯性里程计（VIO）系统可分为两个主要方式：一、间接融合的松耦合方法；二；直接融合的紧耦合方法。松散耦合方法通过两个估计器处理图像和IMU测量，这两个估计器分别估计相对运动并融合来自两个估计器的估计以获得最终结果。紧耦合方法使用一个估计器通过直接融合来自相机和IMU的原始测量来找到最佳估计。与松散耦合方法相比，紧耦合方法通常准确和更鲁棒。关于紧耦合VIO方法的相关工作又通过测量模型中的线性化数量来分类。这些基于扩展卡尔曼滤波器（EKF）的只会在卡尔曼滤波的更新过程中处理一次测量值，与此不同的是非线性优化的过程中多次反复的更新测量。基于滤波方式的预测和更新正好分别采取IMU和视觉测量值来进行，即采用IMU的测量来进行预测，视觉测量来更新。由于三维3地标的坐标包括在状态向量中，也就意味着滤波的计算复杂度会随着时间的增加而指数增加。为了解决这个问题Mourikis提出了采用边缘化的方式来降低计算量，滤波器并不用计算所有的视觉测量。

基于优化的方法是通过最小化具有IMU测量残差和视觉重投影残差的联合非线性成本函数来获得最优估计。因此相比于滤波的一次处理，优化在处理中会将状态向量进行多次线性化处理，这样的好处是得到的数据准确度更高。通过在帧之间积分IMU测量来计算IMU测量约束，然而IMU的积分量与IMU在第一帧时的数据密切相关，当传播过程改变时，需要从最开始的帧计算IMU的积分。Lupton和Sukkarieh提出一种IMU预积分技术，可以避免这种重复集成。IMU预积分已广泛用于基于优化的VIO。Forster等通过在流行上处理旋转组而不是使用欧拉角来重新构造IMU预积分。虽然优化相对于滤波因为处理过程的原因已经实现了较高的精度，但是随着提取到的路标越来越多，计算能力又变得相形见绌了。OKVIS使用现金先出滑动窗口方法通过边缘化最优状态的测量来进行绑定计算。G.Sibley等提出了双向边缘化以选择性地边缘化身体状态和地标。

IMU的测量是有噪声的，因此惯性导航系统会由于误差累积而产生快速漂移，单目SLAM又会丢失尺度信息，而且相机快速移动或光照变化剧烈时，跟踪效果会大大降低。因此无人机在未知环境自主定位时单一的传感器并不能适用，而通过多个传感器的融合可以达到理想的定位效果。视觉-惯性里程计不仅具有视觉传感器在大多数纹理丰富的场景中定位效果很好的优点，还具有IMU在短时间内相对位移数据精度很高的优势，所以视觉和惯性测量的互补特性使它们特别适合融合。此外，相对于昂贵的激光雷达，IMU和相机都成本低、重量低、体积小，对于载荷要求较高的无人机无疑是更合适的选择。

同时系统的定位精度也与相机获取的视觉信息密切相关，目前主流的视觉SLAM方案在相机获取视觉信息均是提取特征点来进行处理，但是无人机处于楼道、管道间、线路房等缺乏特征点的环境中自主定位时面临着巨大的挑战。这时考虑增加线特征来帮助SLAM系统提高定位精度，因为线特征相对于点特征具有丰富的边缘信息并且具有光照不变性，在SLAM方案中增加线要素不仅有助于恢复更加丰富的环境地图，还可以提高系统对光照和视角变化明显场景的定位精度。

图像匹配技术是计算机视觉的重要组成部分, 在目标识别、图像拼接、运动跟踪、图像检索、自动定位等研究中起着重要作用。图像特征匹配是图像匹配的一种重要方法，图像进行特征匹配分三个关键步骤：检测关键点、提取描述向量和特征匹配；通过检测关键点和提取描述向量构造出局部特征描述子，然后进行特征匹配。

上图金色小点均为特征点。特征点的作用是：目标跟踪。关于特征点有许多提取算法，包括SIFT,SURF,BRISK,ORB,FREAK。FAST具有很强的实时性，但FAST不具备尺度不变性，且不包含方向因子。BRIEF用简单的二进制的形式描述特征，实时性比较高，但BRIEF无 法保证旋转和尺度不变形，对噪声比较敏感。Tango便是利用Freak算法提取特征点。

ORB是一种实时性较高的特征提取与描 述方法，它融合了FAST和BRIEF两种描述子。

今天我们便以ORB特征提取方法为例，详细讲解VIO的一种用于无人机导航算法实现。

（1）建立图像高斯金字塔，对每一层图像提取FAST特征点；

（2）为特征点添加方向因子：以特征点为中心建立一个图像块，定义图像块各像素点的矩。

(3) 引入旋转因子：利用 BRIEF 方法描述特征点，并在此基础上采用方向因子驱动法实现二 进制字符串向量的旋转。

接下来进行Lucas-Kanade 光流提取，当摄像机移动的时候，特征光点将拖拽一条"光流"

而且摄像机移动的时候不仅会水平挪动，还会有俯仰的姿态变化，这就要结合IMU惯性元件进行"融合惯导角速率的速度解算"

解决室内定位问题，就要对里程精确计数。

可以采用"图标法里程计"，对摄像头获取的地板灰度图像进行滤波，利用 Canny 算子进行边缘提取，然后对获取的边缘图像利用 Hough 变换提取线段，找到标记点，比方说地板砖的交点。

利用图标里程计，只要保证视野里始终有标记点，便可以实现对飞机的有效定位。

之前的步骤中，主要依赖视觉传感器——摄像头，通过对外部特征的提取和计算，得到自身的速度和位置数据。

但是机器由于各种原因，会出现视觉传感器失效的状况，大概是转晕了？

这个时候，IMU又派上用场了。此时，将里程计、光流和惯导做数据融合，克服这一毛病。

所谓VIO，精髓的地方就在于此，和VO相比较，加入IMU单元后，功能可是强大了不少。

这样无人机就能愉快的飞行了。

香港科技大学的沈老师 (Prof. Shaojie Shen) 团队发布了一个令人振奋的VIO示例，并开源了全部代码。

图片中首先无人机在室内高速做8字运动，红色的定位数据与绿色的motion capture下真值的偏差很小，体现了定位算法的稳定性。

接着给出了在室内和室外环境中利用VIO的定位，产生mesh map，并基于此实现了实时的避障规划。一个摄像头+IMU的组合完成了之前需要RGB-D摄像机或者3D激光雷达才能实现的任务，令人惊叹。

图片中无人机在室内高速做8字运动时红色的定位数据与绿色的motion capture下真值是光学动捕系统测出来的，这样有助于更高效研究、开发。光学动捕系统工作原理如图。