基于视觉延时补偿的无人机室内实时导航系统

许承宇 徐绍凯

（1.中国航空工业集团公司洛阳电光设备研究所，洛阳 471000）（2.普联技术有限公司，杭州 310000）

引言

近几十年来，随着传感器技术的发展和控制理论的完善，无人机的应用越来越广泛.导航定位的高精度和高性能是实现无人机（Unmanned Aerial Vehicle，UAV）广泛应用的重要前提，在导航系统中，通常需要使用多种类型的传感器来实现不同的运动感知目的.因此，传感器数据融合是必不可少的，但同时在融合的过程中也存在传感器输出数据延时的问题.

对于无人机的室内导航系统而言，惯性测量单元（Inertial measurement unit，IMU）能够提供惯性信息[1]，是无人机最常用的传感器之一.另外，由于视觉传感器可以提供丰富的信息量，近年来得到了广泛应用，视觉数据中不仅包括彩色图像信息，还可以计算出包括位移、速度、目标位置等运动信息.将视觉传感器与惯性传感器相结合，组成视觉辅助惯性导航系统，是目前无人机导航十分重要的方法.但是惯性传感器和视觉传感器需要做预先的同步和融合才能作为控制系统的输入，因此必须解决每个传感器之间的时间延时差问题.

传感器延时主要是指视觉传感器的输出延时.一方面，因为视觉信息通常需要经过复杂的操作处理，比如在用到实时定位和地图重建（Simultaneous localization and mapping，SLAM）[2]等技术时，需要进行大量图像数据计算工作，耗费巨大的计算资源，在硬件性能有限时会有很大的时间延迟.另一方面，如果无人机机载系统无法满足图像处理的硬件要求，就必须通过无线模块将视觉数据发送回地面站，这就需要一定的通信时间，不可避免地导致更大的延时.

为了实现合理的传感器数据融合，必须准确估计这几个传感器之间的输出延时差.对于视觉与IMU相结合的传感器系统而言，视觉数据延时通常难以准确估算[3]，使用扩展卡尔曼滤波器（Extended Kalman filter，EKF）创建了一种视觉辅助惯性导航系统（Visual-Inertial navigcation system，V-INS），它可以有效地融合IMU和视觉数据，但是并没有考虑到视觉延时[4].提出了一种改进的卡尔曼滤波器，在假设延时为常数的情况下补偿了延时，并且构造了周期函数作为测量协方差，用以提高融合精度.在文献[5]中开发了一种名为时间延时迭代最临近点（Time Delay Iterative Closest Point，TD-ICP）的方法，该方法在3-D空间中找到两条曲线的对应关系以找到最临近点，将其中的时间差看作视觉数据延时.然而，TD-ICP需要大量的计算时间，并且需要精确的校准和初始化，本文参考了该方法并加以改进.针对分布式网络控制系统[6]，提出了一种延时补偿方法，但在整体化模型中应用效果较差[7].在视觉延时估计中使用了积分方法，为本文提供了灵感，但是在使用传感器计算运动速度时，它没有正确地整合加速度计数据和视觉数据，估计精度还有改进的空间.

本文提出了迭代最邻近点（Tterative Closest Point，ICP）的一种变体算法来估计延时，可以简化传统算法并大大减少计算时间.在此估计的时间延时内，根据加速度计的测量结果，预测出延时补偿后无人机的速度，并进一步估计出运动的位移.在统一好各传感器的坐标系方向后，使用EKF将IMU测量值和补偿后的视觉数据融合在一起，用于无人机的定位和导航.

本文在第1部分中，提供了IMU的数据模型；在第2部分中给出了ICP方法的简化变体，运用迭代求解的方法估算出视觉数据的延时；在第3部分中，本文使用EKF融合了IMU数据和补偿后的视觉数据；第4部分是实验和分析，在这部分中使用了不同数据源的输入进行对比，给出了几组对照实验的结果；第5部分总结了本文.

1 IMU模型

IMU包含加速度计和陀螺仪，分别提供加速度和角速度.在无人机航姿参考系统（Attitude and Heading Reference System，AHRS）[8]中可以通过融合加速度(ax，ay，az)和角速度(ϕ，θ，ψ)来估计无人机的姿态.

1.1 加速度计测量模型

加速度计可以测量物体的线性加速度.加速度计的输出通常包括瞬时加速度以及无法避免的偏差和噪声，可表示为

其中，a′是实际的加速度，而a是传感器测量出的加速度；Ka是一个比例参数，为常数，由传感器自身特性决定；ba是偏差项，包含零偏和由温度变化引起的偏差；na是高斯白噪声.通常可假定ba中的零偏为常数，因此可以在初始化期间对其进行补偿.温度变化引起的偏差会随环境温度而变化，因此该偏差可以通过测温模块进行校准；g是重力加速度，在计算运动加速度时，首先应消除重力影响；Τα是一个旋转矩阵，用于对齐加速度计和无人机机体的正方向.

1.2 陀螺仪数据模型

陀螺仪可测量绕三个正交轴上的角速度.与加速度计类似，陀螺仪的数据模型可表示为

这里，ω′是实际角速度，而ω是测量出的结果；bω是偏差；nω是高斯白噪声；同样地，Kω和Τω表示比例参数和相应的旋转矩阵.

1.3 IMU测量延时

由于IMU是微型机械电子系统（Micro Electro Mechanical System，MEMS）[9]，在测量过程中会因为自身的机电构造而存在固有的输出延时.并且通常原始的IMU输出噪声较大，需要在使用过程中进行滤波处理，这样更会导致延时变大.加速度计和陀螺仪的数据输出延时可以从IMU的数据手册中找到，以目前常用的惯性传感器MPU6000为例，可以从其数据手册中找到如表1所示的输出延时.其中tda和tdω分别代表加速度计和陀螺仪的输出延时.从表1中可以看出，如果选择了相同的滤波参数fp，则tda和tdω之间只有很小的差异.

随着计算机硬件性能的迅猛发展，尤其是CPU中央运算处理器的发展,计算机视觉技术和生物识别技术得到了飞速的发展，各种各样的人机交互软件不断推陈出新，生物特征识别技术也逐渐应用到智能门禁系统中。当下、各小区车库道闸淘汰射频卡技术，进出小区只需刷车脸，道闸均会自动起降。究其原因是利用图像识别技术，性能稳定可靠。基于人脸识别(刷脸识别)的智能门禁系统由于友好的用户体验以及成熟的技术积淀而受到广泛认可。

表1 MPU6000数据输出延时Table 1 MPU6000 data output delay

2 视觉数据模型

通过使用恰当的视觉里程计或SLAM算法[10]，可以利用视觉传感器系统来较准确地计算出无人机的实时位姿.本文利用ORB-SLAM2[11]算法来计算视觉位姿，该算法可以通过估计两个连续图像帧之间的姿态变化来跟踪摄像机的运动，并将关键帧存储起来以创建本地地图.在经过ORB-SLAM2算法的处理后，可以由图像数据计算出无人机的运动状态量Χt，Χt中包括位置坐标信息和无人机的姿态角信息，如下所示：

其中，Τv是旋转矩阵，用于统一相机坐标系和无人机机体坐标系，可表示为：

在上式中，β表示两坐标轴间的旋转，γ表示两个姿态轴之间的旋转.

2.1 视觉数据延时

本文使用的延时估计算法是基于ICP的一种简化变体.我们将所有由视觉数据计算出的姿态角信息保存为数据集Vt，将所有由AHRS估计出的无人机姿态角数据保存为数据集At，使用迭代的方法求解数据集Vt与数据集At之间对应的最邻近点.需要注意的是数据集中每项数据都包含时间信息，也就是数据集Vt和At之间的延时差可以视为时间轴上的平移运动，如果不考虑两个数据集间存在旋转关系，就可以将延时TD初步表示为：

上式中，NA代表At中有效数据的个数，下标t和t+TD表示数据采集的时刻.求解上式需要运用ICP的思路，首先分别计算两个数据集的去质心点集，数据集Vt和At中的姿态角用欧拉角的形式表示为如下的3×3矩阵：

分别计算出两个数据集的质心，用μV和μA表示

则数据集的去质心点集为

接下来将去质心点集代入公式（3）中，就可以使用奇异值分解的方法搜索最优解：

至此一次估计过程结束，如果Vt+TD，i和At，i之间的差E(V，A)大于阈值（Threshold，TH），所有点将根据TD进行更新，算法将再次搜索对应的点，直到E(V，A)

图1 延时标定算法逻辑图Fig.1 Logic diagram of delay calibration algorithm

2.2 视觉延时补偿

AHRS的姿态角输出本身就具有内部延时tdF，但是由于TD比tdF和tda都要大得多，并且EKF的方法可认为是实时更新了来自AHRS的数据，因此AHRS的输出可以被大致认为是IMU数据的无延时估计，也就是tdF≈tda≈tdω，各传感器数据在时间轴上的关系如图2所示.根据图2可以看出AHRS估计的数据相对于IMU数据有微小的延时，而视觉数据的延时远远大于AHRS估计数据所产生的延时，因此可以得到如下的关系式：

图2 延时示意图Fig.2 Time delay diagram

其中，td1表示视觉数据滞后于惯性传感器数据输出的延时，而TD+tdF是视觉数据滞后于实际运动发生时刻的实际延时，通过减去加速度计的输出延时，求得的td1在涉及加速度的控制闭环中基本满足准确度要求.使用td1进行延时补偿，可以将视觉数据补偿到与IMU数据基本相同的时间点.

在td1这段时间内，无人机的实际运动由加速度计实时获取，但由于视觉数据的延时，此时并未获取到由视觉传感器计算得到的运动数据.根据运动定理，可以将线性加速度的积分视为无人机的速度va，视觉数据的位置微分也可被视为速度vb.相对而言，va具有良好的实时响应，但由于误差在积分过程中会产生漂移，尤其是对于长时间操作漂移会更加明显.而vb在整个飞行过程中相对准确，但其实时性能却很差.

因此，融合va和vb以获得更为准确的速度数据是非常有必要的.将va作为td1时间内的有效实时速度，并利用vb在td1时刻之后的高可靠性，可以得到的补偿后速度v′为：

其中，ts是传感器的采样周期，而xt是来自Xt中的位置坐标.假设在飞行过程中环境温度没有变化，在初始化过程中无人机处于静止状态，则取前200次加速度数据的平均值来作为加速度计的零偏：

同样的，位移的实际延时td2≈td1，由速度积分得到的位移数据在td2这段时间内具有更好的动态性能，而在td2时刻之后，视觉数据可以给出更准确的位移信息.因此，可以通过融合来自这两个数据源的位移数据来计算延时补偿后的位置：

其中v′和x′分别是补偿后的速度和位移，经过补偿的速度和位移用于后续数据融合滤波和导航，可以明显提高状态估计器的准确性.

3 基于EKF的位置融合

4 实验结果与验证

本文的无人机实验平台如图3中所示.图中1部分所示为Intel电脑棒，用于传输视觉数据给地面站，并担任部分图像处理任务；2部分表示的是无人机的飞控单元，负责无人机的位姿控制，并担任了传感器数据融合的工作；3部分所示是无人机的IMU模块；4部分是一双目摄像头，采集视觉数据通过1传给地面站，执行SLAM任务.摄像头和IMU相互独立，二者之间无硬件同步.红外测距模块和其他辅助测试传感器安装在无人机的底部.

图3 无人机实验平台Fig.3 UAV experimental platform

首先，运用本文的延时标定和补偿方法对视觉数据进行补偿.通过融合在延时时间内的加速度，可以大大提高补偿后速度的估计精度.在图4中，采用了一个红外测距模块作为对比传感器，该模块精度很高，并且仅有几毫秒的延时，因此可以将红外测量数据作为对比的真实值.可以观察到，经过补偿后的速度v′和红外数据之间只有很小的误差，估计精度明显优于未经过补偿的速度v.精确估计的速度v′可进一步用于补偿位移.如图5所示，补偿后的位移l非常接近真实位移，估计精度明显优于未经过补偿的位移l′.该实验也有红外测距模块的测量数据做对比.

图4 速度的延时补偿Fig.4 Delay compensation of speed

图5 位移的延时补偿Fig.5 Delay compensation of displacement

图4和图5表明，本文描述的延时补偿方法可以很好地将视觉数据延时补偿到正确的时间点，这对于实时运动估计是非常有意义的，可以提高无人机实时室内导航系统的控制稳定性和普适性.

延时补偿还有助于提高无人机的定位精度.无人飞行器沿着同一个预设矩形的边缘飞行，实际飞行位置如图6所示.图6中三组独立实验分别使用了三个不同的数据输入源，raw为使用原始视觉数据进行定位的飞行结果，L1为使用未经延时补偿的融合数据进行定位的飞行结果，L2则为使用了补偿后的融合数据的飞行结果.

图6 无人机的实际飞行位置Fig.6 Actual flight position of UAV

运用软件对三种数据源得到的飞行结果进行APE误差分析，相对误差表现如图7所示.图7中各线条按顺序分别表示APE误差、均方根误差、误差中位数、误差均值和标准偏差.由图可知，使用了补偿后的融合数据的飞行结果相对误差是三组实验中最小的，可以证明补偿后的数据具有更好的定位精度.具体误差数值如表2所示.

图7 三种数据源输入的定位误差对比Fig.7 Comparison of positioning errors of three data sources input

表2 三种数据源输入的定位误差数值Table 2 Positioning error values of three data sources

5 结论

本文提出了一种基于视觉系统延时补偿的无人机实时室内导航方法.首先，通过ICP的简化变体估算出IMU数据和视觉数据之间的延时.基于该估计的延时，使用加速度信息来补偿视觉估计的速度.然后，对补偿后的速度求积分，进一步融合积分结果和视觉估计的位置，得到补偿后的位置信息.补偿后的速度和位置在精度上有了很大的提高，并且使其与实际的速度和位移保持同步.最后，利用EKF将补偿后的视觉数据与IMU数据融合，从而使定位性能显著优于原始视觉数据.更重要的是，该方法不仅提高了定位精度，还满足了室内无人机控制的实时性要求.