一种用于无人机室内定位的改进ORB光流算法

於小杰，贺 勇，刘盛华

长沙理工大学 电气与信息工程学院，长沙 410000

无人机传统导航方法中GPS定位是首选，但其在室内由于建筑物的阻挡，导致信号弱，定位差，不能满足导航需求。因此，在室内采用视觉导航[1-2]方案成为近几年众多院校研究热点，其着重于：（1）如何快速地提取、跟踪稳定特征点；（2）如何高效地处理特征点错误匹配的情况；（3）如何解决光流跟踪失败的问题。其中大部分研究基础源自Rublee 提出的ORB（Oriented FAST and Rotated BRIEF）特征提取算法[3]：将FAST（Features From Accelerated Segment Test）特征点检测器与BRIEF（Binary Robust Independent Elementary Features）描述符相结合的一种算法。该算法的特征点匹配速度远远高于SIFT（Scale Invariant Feature Transform）和SURF（Speeded Up Robust Features）[4]，保证了特征提取的实时性，但匹配精度较差，误匹配率高。因此，在满足实时性的同时，提高基于ORB特征的光流精度就变得有意义了。

为了提高ORB 光流的精度，很多人员对其展开研究。唐浩等[5]将ORB 特征与FAST 角点光流融合，利用Hessian矩阵提取ORB特征，以此获得尺度不变性，通过循环迭代获得相应的速度估计，但其误匹配度仍然较高，导致定位精度较低；王亭亭等[6]采用基于原始ORB特征的LK 光流法，利用随机抽样一致滤波（Random Sample Consensus，RANSAC）来提高光流场的精度，再融合里程计、惯导等信息，实现了无人机室内导航，拥有较高的实时性，但其光流速度估计处理较为简单，采用传统的前向-后向追踪策略，滤除误匹配效果不理想；郝志洋等[7]结合SURF算法的尺度不变特点和ORB算法的快速性特点，创新性地提出了SURB 算法，并利用渐进一致采样算法（Progressive Sample Consensus，PROSAC）去除误匹配，实现了室内定位功能，但其算法较为复杂，尤其是PROSAC算法对所有匹配点不断迭代筛选，导致整体实时性较低。以上算法相较于原始ORB算法有一定程度的改进，但无人机在三维空间中是运动的，拥有6个自由度，对算法的实时性、鲁棒性要求较高，必须在延时性、准确性和兼容性等方面综合考虑可行的视觉导航方案。

针对实时性、准确性和鲁棒性等问题，本文提出一种改进的ORB 特征光流算法。首先，采用ORB 算法提取每帧图像的特征点，送入金字塔中估计下一帧点集坐标；其次，采用前后双向单追踪策略进行粗匹配；最后，采用FLANN-KNN 匹配规则和前后双向双追踪策略进行精细匹配，达到滤除误匹配点集的目的。

1 实验平台搭建

本文搭建的实验平台包括以下部分：华骏HJ816四轴无人机、基于树莓派3 的BirdVisionV3 机器视觉模组、匿名科创地面站、遥控器、keil5 编译软件以及Open Vision Studio IDE编译软件等。

BirdVisionV3 机器视觉模组有两个作用：第一，作为相机，为验证本文算法的实际效果提供平台；第二，在实际飞行实验时，置于无人机底部作为光流传感器，摄像头采集图像数据，树莓派微型计算机负责数据处理。这样做的好处是可以将光流信息单独处理，计算速度大幅提升，同时极大地减轻了飞控板的运算压力。机器视觉模组的主要硬件有：800 万像素、3.15 mm 焦距的Sony IMX219 摄像头，四核BCM2837 处理器（主频1.2 GHz），1 GB LPDDR2内存，16 GB MicroSD存储内存，USB-WIFI 模块等。HJ816 无人机用于验证本文算法在实际应用中的性能，其核心控制器是新唐MINI58-ZDE，配有 SPL06-001 气压计、MPU6050 惯性测量单元和LC12S收发一体串口无线通信模块等。

实验平台的系统结构如图1所示，由安装在无人机底部的机器视觉模组采集图像信息，经过树莓派微型计算机计算处理后，将解算得到的速度信息、目标信息通过I2C 接口传输到飞控板中。飞控中心再利用输入信号（如角速度、加速度、光流信息等）进行无人机的姿态估计，输出适当的PWM给电机，用以调节转速。气压计则将测量到的高度信息反馈至飞控板，经过控制环节实现无人机的定高，以此减少高度变化对光流准确度的影响，从而达到使无人机能够在室内定位的目的。

2 基于改进ORB特征光流法的速度估计

2.1 相关算法

2.1.1 ORB特征提取与描述

ORB 是将加速角点提取与二进制独立基本特征结合的特征检测算法[8]。具体算法实现如下：

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

（2）向FAST特征点添加方向因子。定义图像矩为：

式中，I(x,y)为图像灰度值；a、b为矩的阶次。利用图像矩计算质心坐标：

定义方向因子为图像块中心到质心的向量，即：

（3）引入旋转因子。利用BRIEF描述符来描述特征点，具体方法为，取M×M的窗口P，平滑处理后，有如下定义：

式中，I(x)表示点x处的灰度值。n个特征点生成的BRIEF描述符为n维向量：

图1 无人机实验平台及系统结构图

为描述符引入旋转不变性，定义一个2×n的矩阵：

由方向因子θ获得的旋转矩阵Rθ，通过计算获得新的变换矩阵Sθ=RθS。只要θ确定，即可获得描述符：

2.1.2 LK光流算法简述

经典的LK（Lucas-Kanade）光流算法[9]是利用两帧之差来追踪特征点瞬时速度的估计方法，通过迭代计算可以得到一种稀疏光流场。但是LK光流法必须满足三条前提假设[10]：第一，相邻帧之间的亮度、颜色不发生变化；第二，相邻帧之间的像素点运动是足够小且连续的；第三，相邻帧之间的像素点运动一致，具有相同的光流。基于此，可得亮度守恒约束方程[11]：

式中，vx和vy表示图像x、y方向上的光流值；Ix和Iy分别表示图像像素点在x、y方向上的偏导数；It表示图像像素点亮度关于时间的导数。vx和vy将直接参与速度解算。

LK 算法的三条前提假设都很难满足，其中尤为严重的是第二条假设像素点运动都是“小运动”。通常的解决办法是引入高斯金字塔[12]（如图2所示），缩小图像尺寸，降低分辨率，此时的像素运动就变成了“小运动”，再利用泰勒级数展开式进行计算，从而保证了光流提取的连续性。

图2 金字塔示意图

2.2 改进的ORB-LK光流算法

在无人机悬停时，光流传感器开始启用，共分两步：第一，特征点识别与跟踪；第二，光流计算及速度转换。之后将获得的速度值通过I2C接口送入无人机控制系统中，进行定位调节，迭代计算直至定位完成。在整个过程中，特征点是前提，其准确性决定了光流速度的精确性。

在ORB 特征点识别与跟踪过程中，需要进行前后帧图像特征点匹配。通常采用暴力匹配法[13]，即通过计算前一帧所有特征点（以第p个特征点第k维特征向量Xpk为例）与后一帧所有特征点（以第q个特征点第k维特征向量Xqk为例）之间的欧式距离Dpq：

距离Dpq越小，两个特征点越相似，距离最近的则保留为该特征点的匹配点。但该方法过度依靠不稳定的特征描述子，当图像中存在纹理相似区域时，会导致即便不是前后帧中同一点，也会计算出欧式距离，并且认为是匹配的情况，使得前后帧匹配结果出现大量错误匹配，影响后续跟踪效果与光流计算。因此，本文以此为重点，对匹配规则进行改进。

在原始ORB 光流算法的基础上，结合无人机定位时的飞行速度，提出适用于无人机室内定位的细分匹配规则算法——改进ORB-LK算法：通过飞行时测得的水平速度vu设置阈值，并利用投影公式[14]计算此时的光流阈值：

式中，h为无人机飞行高度，f为摄像头焦距。当光流值高于阈值Δ时，采用前后双向单追踪策略来筛选ORB 特征点，称为粗匹配；当光流值低于阈值Δ时，采用前后双向双追踪策略和FLANN-KNN 匹配规则来筛选特征点，称为精匹配。粗匹配的目的在于快速获得大量特征点，便于无人机快速进入室内定位状态，为精匹配提供输入保障。精匹配的目的在于优化特征匹配，在小位移中获得准确的光流值，从而提高无人机室内定位的准确度。具体内容如下：

（1）粗匹配：ORB 提取和前后双向单追踪策略。如图3所示，首先利用ORB算法对前一帧图像取特征点集合A，然后通过金字塔LK算法获得在后一帧图像上对应的特征点集合B（前向跟踪），之后再将集合B通过金字塔返回到第一帧中，获得对应特征点集C（后向跟踪）。由于存在噪声影响，点集A和C不会完全相同。因此遍历集合A、C中所有点对，计算出距离误差d，选取小于阈值ε的点对，再结合对应点集B，最终保存筛选后的点对集AiBi+1SET,i为帧数。

图3 前后双向单追踪策略示意图

（2）精匹配：FLANN-KNN 特征匹配和前后双向双追踪策略。当粗匹配光流值vx、vy小于阈值Δ时，进入精匹配模式，假设此时为第m帧。从粗匹配保存的点对集AiBi+1SET中，获得第m帧的特征点集Am=Bm⊆Am-1BmSET，第m+1 帧特征点通过ORB 算法获得，为点集Am+1。

为预防图像出现投影畸变，本文采用FLANN-KNN算法进行特征匹配：①建立FLANN 匹配器；②利用KNN 算法初步筛选点集Am和Am+1所对应的描述符集；③再利用特征向量之间的欧式距离筛选好的描述符集及对应点集A′m和A′m+1；④通过随机抽样一致算法（RANSAC）消除点集中的误匹配点，计算出单应性矩阵H，获得最终匹配对集A″m和A″m+1。图4 显示了FLANN-KNN算法前后的匹配效果图。

图4 FLANN-KNN算法前后匹配效果图

基于此，再进行前后双向双追踪策略，如图5所示。利用第m帧筛选后的特征点集A″m进行前向跟踪，获得点集Mm+1，再进行后向跟踪，获得点集Nm，同样由于噪声的存在，记点集A″m和点集Nm之间的特征向量距离误差为dm；同理，点集A″m+1向前获得点集Mm，向后获得点集Nm+1，记A″m+1和Nm+1之间的距离误差为dm+1。由于光流运动是建立的三条前提假设上的，那么第m+1 帧上点集A″m+1与Mm+1存在距离误差，不能完全重合，记误差dn+1；同理，第m帧上点集A″m和点集Mm的误差记为dn。经过实验测试，像素距离阈值ε取10，计算匹配点对过滤函数Π：

能够获得最终适用于无人机室内导航的准确特征点集，进一步使得光流值vx和vy更加准确，为后续光流解算机体速度消除错误信息。

图5 前后双向双追踪策略示意图

2.3 改进ORB-LK光流算法实现流程

本文设计的改进ORB-LK光流算法流程如图6所示。

（1）无人机搭载的单目摄像机在飞行过程中实时采集图像序列。对图像进行ORB 特征提取，得到一系列特征点。

（2）将上一帧特征点和当前帧图像带入金字塔LK算法中，定位到当前帧与上一帧同名的特征点，并计算光流值。

（3）根据步骤（2）中确定的同名特征点，利用前后双向单追踪策略，返回前一帧中再次搜寻同名特征点，并计算与原始特征点的像素差值，做第一次特征点筛选，保留筛选后的对应光流值。

（4）步骤（3）中保留的光流值若大于阈值Δ，则进入光流与惯导数据处理环节，最后送入参数解算、姿态补偿中。

（5）步骤（3）中保留的光流值若小于阈值Δ，则进行ORB 特征提取，再次进入金字塔中，对其采用FLANNKNN 匹配和前后双向双追踪策略进行特征点筛选，删除错误匹配的特征点集和对应的光流值，保存本次筛选的光流值进入光流与惯导融合、参数解算中，并最后做姿态补偿。

3 实验验证分析

本文对改进ORB-LK 特征光流提取效果（在Bird-Vision3 模块上实验）以及在实际无人机室内导航应用中的算法性能（在HJ816无人机上实验）进行验证与分析。

3.1 改进ORB-LK特征光流提取效果分析

图6 算法步骤示意图

表1 三种算法各项数据指标对比

为了提高无人机光流信息的准确度，本文根据金字塔LK 光流跟踪特性，采用粗匹配和精匹配的方式对ORB特征进行了误匹配筛选改进。其中光流阈值Δ由设定的飞行速度经验阈值vu得到，为5；像素阈值ε设为经验值10。为了检验改进算法的效果，本文从主观视觉效果和客观数据指标两方面对改进的ORB特征光流提取算法进行评价。

（1）主观视觉效果

实验中分别采用Shi-Tomasi 光流提取算法[15]、原始ORB 光流提取算法和本文改进ORB-LK 算法分别对少纹理、多纹理、强光和弱光四个场景进行特征提取，并选取其中各自具有代表性的一帧图像进行视觉效果分析，结果如图7 所示。图（a）是Shi-Tomasi 光流法在四种场景中的特征提取效果，图（b）是原始ORB光流法的提取结果，图（c）是本文改进ORB-LK 光流算法的提取结果。从图中可以直观地看出Shi-Tomasi 光流法提取的特征点最少，主要集中在图像物体的边缘等特征明显区域；原始ORB光流法提取的特征点数量最多，同时也包含许多误匹配点，误差高，离散性较差；本文改进ORBLK光流算法是在原始ORB光流法上进行筛选，滤除误匹配后特征点数量有所减少，但仍能保留图像重要特征（尤其是图像中心特征），具有较好的分布性和离散性。

图7 三种算法提取结果对比

（2）客观数据指标

为了进一步从实际数据指标客观地分析算法的性能，实验多次测量三种算法在上述四种场景中的光流提取时间、特征点数以及误匹配率，做平均处理后如表1所示。从表中可以看出，Shi-Tomasi光流法特征点数较少，误匹配率适中，但时间开销较大，与另两种算法相比近乎相差一个量级，难以保证无人机定位的实时性要求。原始ORB算法时间开销虽然较短，特征点数多，但误匹配率也随之增加，影响了无人机定位的准确性。本文改进算法由于增加了误匹配筛选步骤，其时间开销与原始ORB相差约30 ms，但带来的益处是误匹配率的下降，实时性和准确性都有保障。在少纹理场景和弱光场景中，本文改进算法仍能有效地滤除原始ORB 的误匹配点，同时保留其快速性，满足了无人机室内定位中实时性高、时间开销少的要求。

3.2 实际应用中的算法性能分析

为验证本文提出的改进ORB-LK 特征光流算法实际应用中的性能，利用HJ820无人机搭载BirdVision3视觉模块，分别采用Shi-Tomasi 光流提取算法、原始ORB光流提取算法和本文改进算法进行室内定位实验。无人机控制系统中，高度环、姿态环均采用串级PID控制，光流环采用单环PID 控制。各环PID 参数如表2 所示，其中kp表示比例（P）的参数，ki表示积分（I）的参数，kd表示微分（D）的参数。期望悬停时高度为1.6 m，x、y轴位置偏移量为0.1 m。手控操纵无人机，控制无人机的起飞、悬停、降落，利用串口无线通信，上位机实时记录飞行时x、y轴位置偏移量和飞行高度等数据，并进行多次定位误差的归一化数据处理。

表2 无人机室内定位实验PID参数

无人机整体飞行时间约为180 s，起飞后第10～15 s内进行悬停定位，第170～180 s 进行降落。图8、图9 是无人机在x、y方向上位置偏移量随时间变化的曲线对比图，图10、图11是无人机多次定位后x、y方向上的归一化误差对比图。可以看出无人机在悬停过程中，Shi-Tomasi 光流法定位效果不理想，位移偏差高于10 cm时，对应的归一化误差变化较大，有效特征点数量降低，提取的有效光流场质量随之下降，导致高度不稳定，上下起伏大，定位不准。相比之下，原始ORB算法稍好一点，由于特征点数量繁多，能够将偏差大的部分快速拉回，但同时存在误匹配，导致定位误差稍大，精度不够。反观之，本文算法由于经过先粗匹配，后精匹配两步（取光流阈值Δ=5，像素阈值ε=10），滤除了大量误匹配点，使得位置偏差近乎稳定于0，定位归一化误差的波动较小，定位精度随之提高。图12是三种算法下无人机飞行高度随时间变化曲线对比图，从图中数据可以看出，本文方法相较于其余算法，在实际飞行悬停中高度是较为稳定的，悬停效果较好，在期望高度160 cm附近悬停。通过分析实验结果可知，本文算法一定程度上能够满足无人机定位精度的需求，可以尝试用于室内自主导航。

图8 无人机x 轴位置偏移量对比图

图9 无人机y 轴位置偏移量对比图

图10 x 轴多次定位归一化误差对比图

图11 y 轴多次定位归一化误差对比图

图12 无人机飞行高度对比图

4 结论

在室内定位研究中，由于GPS 室内信号弱，一般选择光流传感器作为主要的无人机定位器件，其具有体积小、成本低等优点，被广泛使用。但传统光流算法定位精度不高，为此本文结合原始ORB 特征和LK 光流法，提出一种改进的ORB特征光流算法。该算法优越性在于，采用粗细匹配规则，删除误匹配点集，提升光流质量，提高定位精度。从本文实验可知，在搭载独立运算的树莓派机器视觉模块后，改进ORB-LK光流法能够达到厘米级定位，精确性、实时性和鲁棒性均较好，能用于移动载体的室内实时导航定位，具有较高的应用与参考价值。