一种微型无人机单目视觉SVO/INS组合导航方法

肖 尧，阮晓钢，朱晓庆，董鹏飞，魏若岩

（1.北京工业大学 信息学部，北京 100124；2.河北经贸大学 信息技术学院，石家庄 050061）

微型无人机具有体积小、重量轻、功耗低、机动性好等优点，可以在狭窄的空间中完成探索、搜救、视察等任务。为了完成上述任务，微型无人机需具有在未知环境中自主导航的能力，即通过自身携带的传感器实现运动状态的估计、定位、地图构建、路径规划与避障等功能。

受限于自身尺寸、功耗和负载限制，微型无人机可选的机载传感器十分有限。GPS可以为无人机提供位置信息，但其信号容易受到遮挡，不适用于室内、隧道等环境。此外，GPS不能提供周围环境信息，无法用于后续的路径规划与避障等。单目视觉传感器功耗低、体积小、价格便宜，可以为无人机提供丰富的周围环境信息，但无法感知深度信息，因此基于单目视觉传感器估计的位置带有未知尺度。单目视觉/惯性导航系统（Visual-Inertial Navigation System,VINS）使用的传感器仅包括一个单目视觉传感器和一个惯性测量单元（Inertial Measurement Unit,IMU），其利用了两种传感器的互补特性，通过融合两种传感器的测量结果，可估计未知尺度信息和重力加速度方向[1]，相比于纯单目视觉系统，具有更好的鲁棒性，因此，单目VINS是目前的研究热点[2-15]。

根据视觉和 IMU测量信息的融合方式分类，VINS相关方法可分为两种：紧耦合融合[3-10]和松耦合融合[11-15]。紧耦合方法把视觉和IMU视为耦合系统，基于滤波[9-10]或者优化[3-8]的方法直接融合两者的测量信息。由于紧耦合方法考虑了相机与IMU测量信息之间的相关性，一般来说其估计结果精度较高，但存在计算量大的问题，在机载计算机上实时运行较为困难。松耦合方法把视觉和IMU视为两个独立的系统，通过滤波器将视觉里程计的估计结果和 IMU测量信息进行融合，一方面，视觉里程计前端可通过光束平差（Bundle Adjustment,BA）对多个关键帧的位姿和特征点位置同时进行优化，从而提高估计精度，另一方面，由于滤波器不直接估计特征点的位置，其状态维数较小且恒定，相比紧耦合方法，具有计算速度快的优点，因此更适用于机载计算资源有限的微型无人机。

由于上述优点，松耦合VINS方法近几年获得了较大的发展。Scaramuzza等人[11]提出一种融合单目视觉和 IMU的无人机自主导航系统，视觉前端使用了PTAM（Parallel Tracking and Mapping）[16]，通过扩展卡尔曼滤波，将PTAM的输出和IMU测量数据进行松耦合融合，实现了无人机的位置、姿态、速度等状态估计以及寻迹飞行。Faessler等人[12]提出了一种失败时可以重新初始化和恢复无人机状态的系统，使用半直接视觉里程计SVO（Semi-direct Visual Odometry）[17]作为视觉前端并将其估计结果和IMU融合，通过激光测距传感器对无人机的高度进行估计，进而估计尺度，实现了对无人机的导航。随后，Faessler等人[13]对以上工作进行了扩展，实现了无人机的地图构建和导航功能，但由于SVO本身设计于摄像头下视场景，上述系统并不适用于摄像头前视场景。周等人[14]提出了一种单目视觉ORB-SLAM/INS组合导航方法，基于卡尔曼滤波器对ORB-SLAM[18]和IMU的测量信息进行融合，并进行了跑车实验，该方法假设视觉坐标系的初始姿态和重力方向已知，因此不需要对重力方向做估计。徐等人[15]提出了一种基于图优化的视觉惯性SLAM方法，首先基于图优化方法实现一种视觉SLAM前端，然后将视觉前端和IMU的测量信息进行松耦合融合，由于该方法使用的视觉传感器为RBG-D摄像头，没有考虑未知尺度的问题，且仅用于室内环境。

针对上述问题，本文在文献[12-13]的基础上，提出了一种微型无人机单目视觉SVO/INS组合导航方法。首先对视觉里程计前端 SVO[17]的初始化模块、关键帧选择及跟踪失败处理策略进行了重新设计，使其可用于摄像头前视场景，同时设计了误差状态卡尔曼滤波器（Error-State Kalman Filter,ESKF），所设计滤波器将重力方向和尺度纳入状态量，对单目视觉和IMU的测量信息以松耦合的方式进行融合，实现了无人机的位置、姿态、速度等状态估计，其结果可为后续的无人机自主导航控制提供依据。相对于文献[12-13]，首先，本文方法不需要激光测距仪对无人机高度进行测量以实现尺度的估计；其次，摄像头朝前安装，不仅可以用于视觉里程计的输入，还可以为无人机提供更丰富的周围环境信息，这些信息可以用于后续的障碍检测、路径规划和避障等；最后，本文所设计的滤波器在状态空间中加入了重力方向的估计，无需对无人机初始姿态进行初始化。

1 算法框架

本文所提导航方法的框架如图1所示，其中：p表示微型无人机的位置；q为单位四元数，用于表示姿态；v、ω分别表示无人机的速度和角速度；am、ωm表示加速度计和陀螺仪的测量值；λ为未知尺度，表示视觉里程计估计的位移与米制位移的比例。算法的输入为单目相机所拍摄的序列帧图像和 IMU的测量值，输出为无人机的位置、姿态和速度估计。算法主要分为两个模块：视觉里程计模块和滤波器模块。其中，视觉里程计模块使用修改后的SVO算法，其输出为带有未知尺度的无人机位置和姿态估计值；滤波器模块则使用 IMU测量值对系统状态进行预测，使用SVO的估计结果作为观测值对状态进行更新。此外，算法在运行过程中还会对陀螺仪和加速度计的零偏、重力加速度的方向和未知尺度进行在线估计。

2 视觉里程计

本文所提算法的视觉里程计前端基于SVO算法。SVO基于半直接法，具有计算速度快的特点，十分适用于机载计算资源有限的微型无人机。SVO算法主要分为两个线程：第一个线程用于运动估计，主要包含基于模型的稀疏图像对齐（Sparse Model-based Image Alignment）、特征点对齐（Feature Alignment）、位姿和结构精调（Pose and Structure refinement）3个步骤；第二个线程用于建图，基于深度滤波器对特征点的深度进行估计，当待估计特征点的深度收敛后，将这些特征点加入到地图以用于后续帧的运动估计。由于SVO主要设计于摄像头下视的场景，本文重新设计了SVO的初始化模块、关键帧选择以及跟踪失败处理策略，使其可以使用于摄像头前视场景。

2.1 初始化模块

微型无人机视觉传感器朝向地面时，所拍摄的场景多为平面，此时，SVO初始化直接选取具有足够大视差的两帧图像作为初始关键帧并计算对应的单应矩阵。当视觉传感器朝前时，因场景深度分布较为复杂，该方法很容易初始化失败或者由于初始化精度不足进而导致后续跟踪失败。本文针对摄像头前视的情况，对SVO初始化过程进行了重新设计，具体如下：

Step 1.确定初始的两帧关键帧，根据匹配的特征点坐标同时计算两帧图像对应的单应矩阵和本质矩阵；

Step 2.通过SVD分解，分别获得单应矩阵和本质矩阵对应的旋转矩阵和位移；

Step 3.通过三角化算法，基于Step 2得到的两组旋转矩阵和位移，分别计算特征点的深度；

Step 4.将特征点进行反投影，如果投影误差超过一定阈值（算法实现中阈值设置为2），则标记为外点，否则为内点，同时计算各自内点的数量；

Step 5.如果使用单应矩阵计算得到的内点数比较大，则使用单应矩阵的估计结果，否则，使用本质矩阵的估计结果。

重新设计的初始化模块同时考虑了场景存在平面和场景深度分布较为复杂的情况，具有更好的鲁棒性。

2.2 关键帧选择

摄像头朝下时，SVO的关键帧选择策略为：如果当前帧相对所有关键帧的移动距离超过场景深度的12%，那么该帧为关键帧[17]。但是，当摄像头朝前时，场景深度较深，分布更复杂，且无人机机动引起的视觉传感器视角变化很大，此时，继续使用之前的SVO算法将导致没有足够的图像被选择为关键帧，进而导致算法跟踪失败。此外，SVO的图像对齐或者特征点对齐通过最小化光度误差（Intensity Residuals）实现，导致其相对于其它基于特征点的方法如ORB-SLAM[18]等对环境光照变化更加敏感，因此，需要将光照变化作为关键帧选择的依据之一。

综上所述，摄像头朝前时，SVO关键帧选择可通过考虑以下几个因素决定：视觉传感器的视角变化超过一定阈值；无人机的移动距离超过一定阈值；环境的光照变化超过一定阈值。但是，现实情况下上述阈值很难确定。此外，实验显示，视角变化、移动距离或光照变化均会导致一定比例的特征点跟踪失败。本文通过大量实验研究分析，将关键帧选择策略确定为位姿和结构精调结束后，当所跟踪的特征点丢失比例超过15%时，将当前帧选取为关键帧。

2.3 跟踪失败处理

实验表明，图像跟踪失败一般是由于连续的几帧图像失焦或者运动模糊引起的。SVO作为一种视觉里程计，没有通过回环检测进行重定位的功能，如果一帧图像跟踪失败，之前SVO的处理方式为：将新的一帧图像与上一帧关键帧进行对齐实现重定位，但该方法只有在视觉传感器的帧率较高（＞50 Hz）时有较大的成功率，当帧率较低（≤20 Hz）时，以上策略将不再有效，究其原因主要如下：

1）相比于高帧率的视觉传感器，低帧率传感器相邻两帧图像之间的采样时间间隔长，视角或位移变化大；

2）SVO算法出于计算速度等考虑，相邻两帧图像的特征点对应的图像块对齐时没有进行仿射变换预处理；

3）相比于连续的两帧图像，新一帧图像与关键帧之间的视角或位移变化更大，容易造成对齐失败；

4）当连续两帧图像跟踪失败时，后续图像帧相对上一关键帧之间的视角或位移变化将会越来越大，对新一帧图像重定位的难度越来越高。

基于以上原因，将 SVO跟踪失败处理策略修改为：当一帧图像跟踪失败时，将新到来的图像帧与上一跟踪成功的图像帧进行对齐，以减少两帧图像之间的视角或者位移变化幅度，提高重定位的成功率。

3 滤波器设计

3.1 坐标系定义

图2为本文所使用的坐标系定义，下标w、v、b、i、c分别表示世界坐标系、视觉坐标系、机体坐标系、IMU坐标系和相机坐标系。本文假设IMU与相机通过刚体固连，IMU坐标系和相机坐标系由IMU和相机硬件确定，同时假设两者之间的相对位移pic和姿态qic已提前标定。机体坐标系定义为与IMU坐标系重合，下文公式中，主要使用IMU坐标系i作为下标表示无人机的相关状态，pwi、qwi、vwi分别表示无人机（机体坐标系）相对世界坐标系的位置、姿态和速度。视觉坐标系由视觉里程计前端内部确定，其与起始帧时刻的相机坐标系重合。定义世界坐标系的方向与起始帧时刻机体坐标系的方向重合，坐标系的原点与视觉坐标系的原点重合，即世界坐标系与视觉坐标系之间的相对位移pwv为0，相对姿态qwv等于qic。由上述定义，可以直接将视觉前端的测量结果pvc、qvc转换到世界坐标系下表示，因此下文中不显式使用视觉坐标系。

3.2 IMU模型

IMU的测量结果受到测量噪声和零偏的影响：

其中，ω、a分别表示实际的角速度和加速度，gi为重力加速度在IMU坐标系下的投影，ωm、am分别为陀螺仪和加速度计测量值，bω、ba分别表示陀螺仪和加速度计的零偏；nω、na分别表示陀螺仪和加速度计的测量噪声，假设其从高斯分布，即nωN(0,σω2)，( 0,)。

将零偏建模为布朗运动，其导数服从高斯分布：

3.3 运动学方程

定义系统的状态如下：

其中，gw为重力加速度在世界坐标系下的投影，由于起始帧时刻IMU的z轴方向相对重力方向未知，由世界坐标系的定义可知，gw未知且需要估计，λ为未知尺度。

系统的运动学方程表示如下：

其中，Rwi为qwi对应的旋转矩阵，Ω(ω)为由角速度ω构造的四元数右乘矩阵，其定义如下：

忽略式(4)～(9)中的噪声项，通过IMU的测量值对其进行积分，可以得到系统状态的预测。一般来说，由于IMU的测量频率远高于视觉传感器帧率，滤波器需要进行多次预测之后才进行一次更新。

3.4 状态误差与协方差矩阵

定义系统状态x和预测状态之间的误差为δx=x-，由于四元数表示的姿态存在参数冗余，为防止协方差矩阵出现奇异，需要特殊处理。令四元数的误差为δθ，δθ为3× 1的矢量，其定义如下：

定义系统的状态误差为：

根据状态误差的定义，对其进行建模：

式中，[r]×表示由向量r构成的叉乘矩阵，其定义如下：

式(13)～(19)可写成矩阵形式的连续线性时变微分方程：

对式(21)进行离散化，保留一阶项，得到：

其中，Δt为IMU采样间隔，I3分别3阶单位矩阵，wd=wΔt。

由式(22)，可以计算协方差矩阵：

其中，Qn、Qwd分别为n、wd对应的协方差矩阵，

其中，03表示3阶零矩阵。

Qd为12× 12的协方差矩阵。将Qn、Qwd从数学形式上合并为Qd是为了编程时方便处理和计算，合并后，Bd矩阵中的旋转矩阵在Fn矩阵中变成了单位阵。

对两帧图像之间的多个IMU测量值，重复使用式(23)对协方差矩阵进行迭代计算。

3.5 更 新

系统的测量模型为：

式中，np、nq分别为视觉里程计的位置和姿态测量噪声。

位置的测量误差为：

对式(28)进行线性化，得到：

按式(11)四元数误差定义，可以定义姿态的测量误差为：

故

由式(29)(31)可得：

滤波器更新计算：

文献[11]假设世界坐标系z轴竖直向上且世界坐标系与视觉坐标系之间的相对位姿pwv、qwv已知，故重力加速度在世界坐标系的投影已知，但需要在无人机起飞前通过外部设备对pwv、qwv进行估计。

文献[12-13]在所估计的状态空间中加入了pwv、qwv的估计，实验表明，该方法对qwv初始值设置较为敏感，不准确的初始值很容易导致滤波器发散。

本文所设计的滤波器直接对重力加速度方向进行估计，其初始值可以直接通过短时间内加速度计测量值估计得到，避免了上述问题。

4 实验与讨论

本文使用 EuRoc[19]公共数据集中的 Machine Hall 01子集对算法有效性进行验证。EuRoc数据集由苏黎世联邦工业大学 ASL实验室使用无人机现场采集得到，其所用传感器包括双目视觉传感器（Aptina MT9V034全局快门，WVGA单色，2×20 FPS）和IMU（ADIS16448，200 Hz）。此外，EuRoc数据集还提供了Ground Truth数据，其精度为毫米级，可用于对估计结果进行评估。本文算法通过单目视觉和IMU实现无人机的状态估计和导航，故实验时仅使用数据集中左摄像头（cam0）采集的图像，另外，实验所需的视觉传感器和IMU之间的相对位姿pic、qic由数据集提供。

4.1 SVO初始化实验

为了证明本文修改后的SVO初始化更具鲁棒性，将Machine Hall 01数据集的3682帧图像最后100帧和约400～900帧之间的静止帧去掉后，从中随机抽取50帧图像作为SVO输入的起始帧进行了50次实验，如果前两个关键帧初始化完成之后可以继续保持跟踪100帧，则判定为初始化成功。实验结果如表1所示。

由表1可知，SVO初始化模块修改后，其初始化成功率更高。本文所提算法对SVO作了3处修改，对关键帧选择策略进行修改后，初始化成功的SVO可以在Machine Hall 01上运行，但依然存在跟踪失败的现象，而对 SVO跟踪失败处理策略的修改最终实现了SVO在Machine Hall 01上的完整运行，由此证明了修改的有效性。

表1 初始化实验结果对比Tab.1 Comparison of initialization experiment results

4.2 无人机状态估计

为了评估本文算法状态估计结果的精度，使用所提算法完整跑完Machine Hall 01数据集，并将估计结果与 VINS-Mono[3]的估计结果以及数据集提供的Ground Truth进行对比。其中，位置和姿态初始值设置为起始帧时刻 IMU相对于世界坐标系的位置和姿态，由世界坐标系的定义，可以计算得到位置初始值为 -Rcipic，姿态初始值为单位四元数。速度、加速度计和陀螺仪偏差的初始值都设置为 0，重力方向初始值设置为起始帧时刻的IMU测量值，尺度初始值设置为0.5。VINS-Mono使用程序默认的配置。使用均方根误差（RMSE）对结果进行统计分析和对比。

图3 飞行轨迹Fig.3 Trajectory

图3为飞行轨迹，相比于Ground Truth，本文算法和 VINS-Mono的估计结果都存在误差。为了进一步对估计误差进行分析，图4给出了位置误差分别在x、y、z三个方向上的变化曲线，相比于VINS-Mono的估计结果，本文所提算法的位置误差变化相对平缓。

图4 位置误差Fig.4 Translation error

表2为位置误差的统计结果，可以看出，本文所提算法在y、z方向上的误差均小于 VINS-Mono。对位置误差的欧式距离进行统计，本文所提算法的平均误差为0.151 m，VINS-Mono的平均误差为0.187 m，可见本文算法的估计结果优于VINS-Mono。

表2 位置误差对比Tab.2 Comparison of the translation error

图5 姿态误差Fig.5 Attitude error

图5为所估计的四元数姿态转换为欧拉角后的误差曲线，表3为对应的姿态误差统计结果，从中可以看出，本文算法的姿态估计误差整体小于VINS-Mono。图5中，本文所提算法的姿态估计误差在约84 s处有一个突变，这是由于此时 SVO出现了跟踪失败的情况，但通过跟踪失败处理算法，视觉前端里程计实现了重定位，证明了本文所提跟踪失败处理算法的有效性。

表3 姿态误差对比Tab.3 Comparison of the attitude error

图6和表4分别为速度估计的误差曲线和误差的统计结果，结果表明，本文所提算法的速度估计误差小于VINS-Mono。图6中y方向的速度估计在大约84 s处出现了较大的偏差，是由于SVO视觉前端跟踪失败导致。

表4 速度误差对比Tab.4 Comparison of the velocity error

图6 速度误差Fig.6 Velocity error

对比图4～6中约84 s处前端跟踪失败重定位对位置、姿态和速度的影响可以发现，重定位过程对姿态和速度的估计产生较大的影响，而图4中位置误差在仅在y轴方向在约84 s处仅看到一个小的突变。这是由于跟踪失败发生时，无人机主要进行旋转运动，此时视角变化较大，从而造成了跟踪失败，同时也造成图5中姿态估计误差出现明显的突变。

由图6可知，此时在y轴方向产生的最大速度误差约为0.15 m/s，此外，从跟踪失败到重定位成功间隔为3帧，即大约0.15 s，由此产生的位置误差约为0.02 m，因此，图4中的位置误差并没有显示出明显的突变。

图7为尺度的估计变化曲线。实验结果表明，本文算法在对IMU和SVO的测量结果进行融合后，可以对尺度进行恢复。实验中，大约经过15 s后，尺度基本收敛，由于单目视觉前端的估计会随着距离产生一定漂移，所以尺度收敛结果不为恒定值。

图8为陀螺仪零偏的估计，可见其结果基本收敛于真实值，证明了本文所设计的滤波器可以有效地对陀螺仪偏差进行在线估计。

图7 尺度Fig.7 Scale

图8 陀螺仪零偏Fig.8 Gyroscope bias

图9 加速度计零偏Fig.9 Acceleration bias

图9和图10分别为加速度计零偏和重力加速度的估计。图9中，加速度计零偏的估计值基本收敛，但收敛速度较为缓慢，且相比于陀螺仪零偏的估计结果有较大的波动，其主要原因分析如下：1）加速度计自身的测量噪声比陀螺仪大，导致了加速度计零偏的估计难度较陀螺仪零偏估计难度高；2）单目VINS需要在至少两个轴向有足够的加速度才能保证加速计零偏可以观测[1]，这个条件在实际系统较难以持续满足；3）当运动激励不足时，加速度计零偏与重力加速度相互耦合。

一般来说，重力加速度远大于无人机的加速度，其在加速计各轴会产生较大的投影，而姿态估计存在误差将会导致不能将重力加速度从加速度计测量值中完全去除，这些没有去除的重力加速度分量会被当成加速度计零偏或者噪声处理，进一步提高了加速度计零偏的估计难度。图10中，重力加速度方向的估计在大约10 s后基本收敛，但此时gx有大约0.1 m/s2的估计误差，在约100 s后基本收敛到真实值，与图9中加速度计零偏的收敛速度基本一致，进一步证明了上述关于加速度计零偏与重力加速度相互耦合的分析结果。

图10 重力加速度Fig.10 Gravity

4.3 尺度收敛性实验

相比于纯视觉的单目视觉里程计或SLAM，单目VINS的最大优势在于可以通过融合视觉传感器和IMU的测量结果恢复尺度。由于滤波器的收敛性在一定程度上依赖于初始值的设置，为了验证本文所设计的滤波器在尺度设置不同初始值下均可以收敛，其他参数保持不变，分别将尺度初始值设置为 0.1、0.3、0.5、0.7、0.9、1.0。

图11为对应的实验结果，其中，尺度的真实值约为0.4，从结果可以看出，尺度在不同初始值下，大约经过15 s后都基本收敛于真实值，但收敛速度略有差别，初始尺度越接近真实值，收敛速度越快。图11中，尺度在大约0.6 s之前没有变化的原因是此时SVO还没有完成初始化，滤波器没有开始更新。

图11 尺度不同初始值的收敛曲线Fig.11 Convergence of scale with different initial value

4.4 运行时间统计

本文所提方法基于滤波器对视觉里程计和 IMU的测量信息进行基于松耦合融合，具有计算速度快的优点。对算法各步骤的运行时间进行统计，结果如图12所示。实验所用 PC的 CPU为 Intel i7-7700 3.60GHz，内存8GB，操作系统为Ubuntu 16.04，算法使用C++实现。

图12中，预测表示使用相邻两帧图像之间的多个IMU 测量值进行状态预测和协方差估计所耗时间统计，更新表示滤波器使用视觉前端的估计结果进行一次状态更新的耗时，SVO 表示视觉前端处理一帧图像所消耗时间。从图12 中，预测、更新、SVO 对应的中位数统计结果分别为 0.05 ms、0.01 ms、4.14 ms，总耗时 4.20 ms，由此可以看出，算法运行时，视觉前端占用了算法的大部分计算量，相比之下，滤波融合算法运行所需要的时间可以忽略，此外，算法在大部分时间可以以超过200帧/s的频率进行状态估计，远超相机的帧率，体现了本文所提算法的优越性。图12中 SVO 处理一帧图像所需的时间有较大波动，这是因为算法处理的图像帧分为普通帧和关键帧两种，当处理关键帧时需要进行局部光束平差，此时算法需要消耗更多的时间。

图12 运行时间统计Fig.12 Timing static results

5 结 论

本文提出了一种针对未知环境的微型无人机单目视觉SVO/INS组合导航方法。使用SVO作为视觉前端，并对SVO进行了修改，使其成功应用于摄像头前视场景。设计了一种ESKF滤波器，可对SVO输出和IMU测量结果进行松耦合融合，实现无人机的位置、姿态和速度估计，同时可对尺度、重力方向、陀螺仪和加速度计的零偏进行在线估计。使用EuRoc数据集进行实验，证明了该方法的有效性，且其估计结果具有较高的精度。所提算法不需要对尺度和姿态做初始化，具有实时性能好的优点，可用于无人机控制、路径规划、轨迹跟踪、地图创建等后续任务。进一步提高算法在快速运动和复杂光照环境下的鲁棒性以及重力方向、加速度计零偏估计的收敛速度和精度将是下一步的研究重点。