一种基于双目视觉的无人机自主导航系统

侯永宏，刘 艳，吕华龙，吴 琦，赵 健，陈艳芳

(1. 天津大学电气自动化与信息工程学院，天津 300072；2. 天津航天中为数据系统科技有限公司，天津 300072)

无人机在荒野搜救[1]、环境探测[2]、农林植保、电力能源巡检、消防救灾、国土测绘、包裹派送等领域有着广阔的应用前景.提高无人机在复杂环境下的自主飞行能力，快速高效地自动规避障碍物，对拓宽无人机应用领域、提高无人机工作效率具有重要意义.

根据对障碍物的感知方式区别，目前的无人机避障方案主要有基于超声波传感器的避障方案[3]、基于激光传感器的避障方案[4]以及视觉传感器避障方案.其中超声传感器存在感知范围窄、空间分辨率差、无法感知吸收声波的物体，在大风干扰的情况下无法工作等缺点，只能作为一种辅助手段.激光传感器障碍物感知精度高，但体积庞大、价格昂贵，不适用于载荷能力受限的低成本无人机系统.视觉传感器具有感知范围宽、空间分辨率高、成本低的优势，其主要缺点是计算复杂度高、实时性差.近年来得益于计算机处理能力的快速提高，特别是图像处理单元(graphic processing unit，GPU)并行处理技术的普及，使得在嵌入式平台上完成复杂的实时视频处理成为可能.

在无人机精确定位方面，目前无人机大多采用GPS+惯性测量单元进行无人机的位姿估计，但在一些楼宇建筑物或者丛林等复杂场景中会存在没有GPS信号的情况，使得无人机无法完成精准定位.虽然惯性测量单元可以进行辅助定位，但是惯性测量单元由于长时间的运行，逐渐累积误差会产生严重的定位漂移.而采用基于视觉传感器的里程计进行无人机定位，既可以不依赖于外部 GPS，也能避免严重定位漂移问题.

目前，有多种基于视觉的机器人自主感知与导航算法已被提出.从研究方法来说，可分为基于深度学习的算法[5]与基于经典的同时定位与建图[6](simultaneous localization and mapping，SLAM)框架的算法.基于深度学习的定位与导航算法因为其计算量庞大，目前不适用于实时导航系统.基于SLAM框架的算法根据视觉传感器的类型，又可分为单目SLAM[7]系统、双目 SLAM[8]系统、多目 SLAM 系统以及基于RGB-D相机[9-11]的SLMA系统.单目相机对计算资源要求不高，但缺少物体实际的尺度信息，造成系统初始化困难[12]；多目系统在场景的三维重建方面有更好的精度，但是很难做到无人机上的实时应用；RGB-D相机能直接获得深度信息，但易受日光干扰，不适用于室外环境.双目相机SLAM系统不存在尺度不确定性问题且可用于室外环境，适用于在未知室外环境的实时无人机导航系统.

本文设计并实现了一种基于双目视觉的小型旋翼无人机自主导航系统.该系统利用双目相机获取环境信息，在机载的嵌入式处理平台上完成了无人机飞行的视觉里程计、障碍物感知、局部环境地图的构建、以及飞行路径的自主规划等自主避障所需的全部实时信号处理，取得了较好的避障效果.

1 系统结构

本系统的软硬件架构如图 1所示.系统分为多旋翼无人机平台、飞行控制器、机载信号处理平台和视觉感知模组 4个部分.为了满足无人机的有效载荷，使得无人机既能挂载视觉感知模组、信号处理平台同时又可以执行救援等其他任务，系统选用了额定载荷2kg的Tarot 650四旋翼机架.通用的Pixhawk飞行控制器不仅能够保证无人机的平稳飞行与运动控制，而且还有丰富的外部控制接口.本系统通过Pixhawk的串口采用基于机器人操作系统(robot operation system，ROS)的 MavLink协议与机载信号处理平台进行数据通信.信号处理平台采用 NVIDIA Jetson TX2模组，其具有4颗ARM A57 CPU核、256颗CUDA核以及8GB内存空间，在功耗仅为7.5W的情况下，处理速度达到1.3TOPS(trillion operations per second).不仅运算能力强劲，而且体积小、重量轻，目前被广泛应用于各类无人驾驶系统作为信号处理平台.视觉感知模组采用 StereoLabs ZED双目立体视觉相机，其具有高分辨率、较佳的低光灵敏度，并且检测距离可达到20m.

图1 系统软件硬件框架Fig.1 Framework of the system software hardware

2 双目视觉避障算法

所设计的无人机自主导航系统，使用 ZED双目摄像机获取场景信息，无人机定位部分采用 ORB SLAM2算法，得到无人机位姿信息，再利用改进的SAD算法检测障碍物点，结合障碍物点与无人机位姿生成局部障碍物地图，再根据本地轨迹库，规划航迹，实现无人机自主导航.

2.1 视觉里程计

无人机在复杂环境下飞行，会存在无法使用GPS进行定位的情况，为了实现在无 GPS信号的环境下平稳、正确飞行，无人机需要对自己的飞行位姿(位置和姿态)进行精确估计.视觉里程计就是利用连续视频帧内同一路标的位置变化来估计相机的运动轨迹.本系统视觉里程计(visual odometry，VO)模块采用ORB SLAM[12-13]算法来进行无人机精确定位.

视觉里程计的基本思想是，相机在运动过程中会观测到三维世界中的物体并提取特征组成路标点地图，不同帧图像会观测到路标点地图中的同一路标点.通过当前帧图像中特征点与路标点地图中特征点的匹配，利用双目相机的成像几何模型，直接得到特征点的深度信息，进而求得其世界坐标，之后根据同一路标点在下一帧图像中的坐标，可以求出不同帧之间的相机位姿变换(旋转矩阵R和平移向量t).经过累积和非线性优化准确估计出无人机在整体运动过程中的状态.

ORB SLAM 算法为降低视觉信息冗余度，从视频序列中抽取一些帧进行位姿的计算，所选取的帧称为关键帧.在关键帧的选取策略上，由于双目视觉里程计能获得深度信息，所以图像首帧即为关键帧，从立体点中可以直接构建初始化地图，后续每隔固定时间间隔选取一次关键帧，特殊情况是当场景发生较大变化时，也需选取关键帧.

ORB-SLAM 算法采用 ORB特征点作为前端匹配特征点.ORB特征由关键点和描述子两部分组成[14]，关键点称为“oriented FAST”，它改进了FAST[15]不具有方向性的问题；描述子称为BRIEF[16](binary robust independent elementary feature)，是一种快速的二进制描述子，用于匹配原始关键点.得到匹配点后，即可计算出关键点在世界坐标系中的坐标.

由匹配点计算物体实际坐标的具体方法.首先采用PnP(perspective-n-point)几何约束关系初步得到两帧之间的相对位姿.由于匹配的关键点会存在噪声，这样计算出来的位姿误差较大，需要使用光束法平差(bundle adjustment，BA)进一步优化无人机位姿.BA算法是非线性优化中计算最小化重投影误差的方法.本系统采用的BA只对两帧图像的运动进行优化，可以认为是纯运动 BA.定义旋转矩阵R∈SO(3)，平移向量t∈ℝ3，χ是所有匹配点集合，BA的优化目标是最小化在世界坐标系中匹配到的路标点Xi∈ℝ3与关键点xi∈ℝ3之间的重投影误差为

式中：ρ为稳健的 Huber损失函数；Δ为协方差矩阵，利用Δ对重投影误差求椭球范数；(fx,fy)为焦距；b为基线；(cx, cy)为像素坐标系下坐标原点从图片中心平移到图片左上角造成的平移量，以上参数均可通过相机标定获得.该式实现相机坐标系下的路标点投影到像素坐标系下.在相机坐标系中，通常Z轴指向相机前方，X轴向右，Y轴向下，坐标原点在相机中心，则Z轴数据大小代表路标点深度信息.

由于长时间的相对位姿误差累积会带来定位漂移误差，ORB SLAM 算法后端优化应用局部 BA优化算法，减少漂移误差.采用与 ORB-SLAM[12]中基本相同的关键帧插入与剔除策略，同时增加一个判断标准，即当前帧的关键点小于一定数目或者帧中出现的新关键点大于一定数目时插入新的关键帧.对于新场景内的关键帧KL，进行局部BA优化得

局部BA相对于纯运动BA，添加优化KL帧中所有关键点PL在地图中的 3D坐标，该优化需利用本次地图时间内所有的关键帧K.集合kχ中的点表示第k个关键帧中与PL匹配的关键点.

使用 ORB SLAM2算法，设计的无人机系统在没有 GPS的情况下也能实现精确的定位，得到无人机在世界坐标系中的位姿，提高本系统的稳定性.

2.2 基于双目视觉的障碍物感知

在无人机自主导航中，无人机对周围环境的精确感知是定位与构建地图的基础与前提.

对于实际飞行中的无人机系统，要达到有效躲避障碍物的需求，必须有较高的计算效率，稳定的计算精度来自主感知周围环境.

在估计环境深度信息时，一般采用对双目成像像素匹配的方法完成环境深度的估计.但如果对无人机双目成像的所有像素点做立体匹配，会造成大量信息冗余，浪费机载计算机有限的计算资源，难以达到实时性的要求.所以在识别障碍物时，只需计算障碍物的轮廓信息，并且只把关注的焦点集中在飞行前方一定范围内，在双目视图的左右图像中匹配障碍物的轮廓像素点即可识别障碍物的位置和大小.

本系统中使用二阶微分的拉普拉斯算子(LOG)进行边缘检测，获取障碍物的边缘信息为

式中：I( x, y)表示原始图像中坐标是(x, y)的像素点；L( x, y)表示进行拉普拉斯变换后的像素点.

根据初步提取的障碍物对应像素点在左目图像中的位置，为了获得障碍物实际的位置与深度信息，需要在右目图像中寻找匹配的像素点.在这一步中采用经过改进的图像块匹配算法 SAD(sum of absolute differences).其基本思想是取左、右目图像像素块中对应像素的差的绝对值之和，据此评估两个图像块的相似度[17].

传统的SAD匹配估计的是一张图像中所有像素点的视差信息.每一个像素点的视差信息的计算，需要通过对右目图像对应的极线上所有像素周围的像素块进行匹配，选取像素差值最小的点作为匹配点，再计算相应视差.这样一个匹配过程的时间复杂度是相当高的，很难保证足够的帧率.

改进的SAD算法仅仅需要均匀稀疏的像素块视差和每一帧左、右目图像中的固定视差大小的像素块匹配，同时整个匹配过程使用 CUDA加速.①均匀稀疏是指对一张 1280×720的图片，将其分割成12×12大小的网格，随后只求这些网格中心点(同时也应当是边缘点)对应的像素块的视差，有效减少将近两个数量级的匹配时间复杂度；②固定视差大小匹配是指，假定无人机只关心前方固定距离处的障碍物，在视频流中也就是固定深度处的像素块，深度反推出在左、右目图像中的固定视差，通过计算固定视差处的像素块的像素差值是否在一定阈值内来确定是否匹配，通过这种方法又可以降低一个数量级左右的时间复杂度；③同时将整个视差估计以及预先的滤波(拉普拉斯边缘提取设定阈值以及双边滤波去除噪点)过程都放在NVIDIA Jetson TX2的GPU当中进行并行运算，有效提升障碍物感知过程的效率，保障系统的实时性.

经典SAD算法流程如下.

从左到右、从上到下依次扫描左目图像中像素.

(1) 选定图像像素点，并以此为中心，构造一个n×n的窗口，用窗口分别覆盖左、右目图像对应区域，窗口中心点在选中像素点的坐标上；

(2) 左目覆盖区域减去右目覆盖区域，并求出所有像素点灰度差的绝对值之和；

(3) 在同一水平线上移动右目图像的窗口，在一定搜索范围d内重复上一步的处理；

(4) 找到这个范围内SAD值在规定阈值内的窗口，即找到了左目图像边缘点的最佳匹配的像素块.

SAD值的求解公式为

式中L( i)表示拉普拉斯变换后的像素点.

本系统根据无人机的飞行速度及系统响应时间，采用“推扫式”障碍物深度感知方法.即设定对于每帧图像，只寻找无人机前方固定范围处(即图片中固定深度)的障碍物，无人机在持续飞行过程中，如图 2所示，根据当前范围(深色部分)所捕捉到的障碍物信息，结合之前(浅色部分)的障碍物信息，逐步累积获得周围环境的整体感知.

图2 “推扫式”图像感知Fig.2 Pushbroom image perception

根据经典的双目摄像机针孔模型的三角相似原理，可以得到固定深度下的视差大小为

式中：B为基线，代表双目摄像机左右摄像头之间的距离；f为摄像头的焦距.B与f均是摄像机的基本参数信息，本方法中设定取摄像机镜头前5 m的障碍物进行检测，d则是同一物体在左右两幅图中的成像视差值，也是本文改进的SAD算法的搜索范围.

障碍物边缘点匹配成功后，根据相机标定获得的内参矩阵，把障碍物点的像素坐标转换到相机坐标.对于远处的地面水平线和具有重复纹理信息的背景，系统可能会将其当做障碍物，因此，本系统加入水平一致性检测消除该影响.本系统所采用的改进的 SAD匹配方法避免传统匹配中的遍历过程，极大地降低计算复杂度，提高处理速度，适用于无人机飞行过程中的实时障碍物检测.

2.3 点云地图

无人机自主规划飞行轨迹，需要当前环境地图.传统方法需要先获得当前环境的先验地图，使得无人机在已知环境下进行导航，此方法具有局限性的，不适用于所有场景.较好的解决方案是无人机在飞行的过程中进行地图的构建.受限于机载计算机的内存大小，本方法不会对地图进行本地化的存储，所以本系统认为所有场景均是未知的.无人机不断进行局部地图的增量式构建，以此作为导航的依据.

结合第 2.1节中得到的无人机在世界坐标系中的位姿与第 2.2节中得到的障碍物在相机坐标系中的位置，便可得到障碍物在世界坐标系中的三维坐标.本系统采用 PCL[18]中提出的基于点云的八叉树结构地图.八叉树地图(octomap)是一种可压缩、可更新并且分辨率可调的适用于导航的稠密地图.八叉树地图把空间建模成许多立方体，大的立方体被逐渐均分成同等大小的 8个小立方体.用概率对数值描述某个小立方体中含有障碍物的概率，即

式中：x为0～1的概率；y为概率对数值.当不断观测到某个节点被障碍物占据时，让y增加一个值，否则减少一个值.对获得的 y，根据式(7)求逆变换可以得到相应的概率值.因此可以查询某个空间范围“可以通过的概率”，从而实现无人机飞行过程中的导航.

为了避免在八叉树中长时间积累大规模地图信息，同时有效缓解处理负担，本系统采用地图生存期与关键帧机制，同时构建两个八叉树地图，如图 3所示：一幅八叉树地图的生存时间是4s，当前运行的地图是工作地图，下一时刻的地图是构造地图.为了确保在丢弃一个八叉树时不会错过一个潜在的障碍，在工作地图运行 2s时，构造地图开始同步构建，工作地图和构造地图之间至少存在2s的重叠期.

通过上述方法，无人机就获得了周围环境的局部障碍物地图信息.

图3 地图生存期Fig.3 Map lifetime

2.4 基于轨迹库的路径规划

无人机的轨迹规划是指计算机根据周边障碍物信息规划出一条能有效躲避障碍物的安全飞行路径.由于无人机在局部地图中只需进行局部路径规划，本系统根据已生成的局部障碍物地图，采用一种简化的基于本地轨迹库的航迹规划方法.

具体过程如下.

(1) 首先选择无人机与当前地图障碍物的最近距离dobs当做判断当前路径上是否有障碍物的标准，并且设定一个参考阈值dref，如果dobs＞dref，则认为当前执行路径上没有障碍物(即当前执行路径安全)，反之则认为是有障碍物.

(2) 状态集合S={s0, s1}代表当前无人机的飞行状态，s0表示当前路径安全，s1代表路径不安全.动作集合A={a0, a1, a2, a3}中的元素分别代表无人机沿直线飞行、向左飞行、向右飞行以及朝终点飞行的飞行指令.其中a3表示无人机根据当前飞行位姿、速度以及终点坐标，在不考虑未观测区域障碍物的情况下，直接朝向终点飞行的路径.假设a代表无人机当前动作，具体的轨迹库遍历顺序见表1.

表1 轨迹库遍历顺序Tab.1 Traversal order of trajectory library

2.5 系统优化

自主导航的难点之一在于规划有限的机载计算资源[19].前文中的各个模块都占用一定的 CPU资源，尤其是地图的构建需要占用更大的处理资源.考虑到本系统所使用的处理器 NVIDIA Jetson TX2中带有GPU模块(具有256个CUDA内核)，为了降低CPU的工作负担并充分利用处理器的资源，实验中使用大规模并行计算工具 CUDA，优化系统的建图框架，缩短程序运行时间，达到实时运行效果.

为验证 CUDA提高系统运行速度的效果，在NVIDIA Jetson TX2上分别进行 3种实验，如表 2 所示.

表2 GPU加速对比实验Tab.2 GPU comparative experiment

实验 1～3是本系统的不同方案，实验 1代表不使用CUDA进行加速，分辨率是640× 480，实验2代表经过CUDA加速，分辨率同实验1，实验3代表经过 GPU 加速，分辨率是1280× 720，经过对比可以看出，在同样的视频质量下，使用 GPU加速优化系统，障碍物感知速度提高了4倍左右，达到了明显的加速效果.

与文献[20]中的实验方法进行对比.换算成同样的分辨率，本系统在640× 480分辨率下，处理帧率与其相比提升 4倍；当分辨率提高到1280× 720时，虽然处理帧率有所下降，但是系统整体性能仍有大幅提高，从一定程度上验证了本系统的优化效果.

3 实验结果与分析

对所设计方案分别进行仿真和真实飞行两种实验进行验证，实验中所有的算法均在嵌入式处理器NVIDIA Jetson TX2上完成，实验平台如图4所示.

图4 实验平台Fig.4 Experimental platform

3.1 仿真验证

仿真实验在能渲染 3D世界的虚幻引擎 Unreal Engine 4(UE4)上搭配Microsoft开源框架AirSim的环境下进行[21].Airsim提供了多种无人机、无人车模型，能够虚拟出 GPS、IMU、气压计和磁力计等多种传感器.配合 UE4引擎使用，能够渲染出非常详细的3D城市环境.在Airsim仿真中，通过API接口获取仿真环境中虚拟相机图片，通过本系统的算法处理，将最终的无人机飞行指令反馈给仿真环境的无人机模型执行.

图5 仿真试验展示Fig.5 Simulation display

图 5(a)显示出无人机对距离 5m的障碍物进行了标注，用多个蓝色方框覆盖树干.最终建立的点云局部地图如图5(b)所示.图5(c)、(d)是第 3视角的不同场景轨迹图，图 5(d)中上方的立方体堆积物是检测出的障碍物，目的地设定在地图的右上方.左半部分的3条直线是世界地图坐标系，右半部分较短的线是从当前位置指向当前规划航线最远航点的矢量线，连续的线代表实际飞行的轨迹.从图 5中可以看出无人机在避开前面的障碍物时，首先不停地向右飞行并且旋转偏航角，随后选择沿着障碍物直线飞行，直到指向终点的飞行路径是可以安全飞行的，则选择向终点飞行的路径.

3.2 真实飞行

基于安全性考虑，飞行实验选择在航天中为数据系统有限公司的无人机专用实验场地进行.

图 6是测试时的真实场景，由于环境所限，无人机设定为定高飞行，目标是自主绕过无人机的飞行路线上的一排树木.实验中设定相机采集的视频分辨率是1280× 720，实际处理帧率达到60帧/s.

图6 真实飞行展示Fig.6 Real flight show

图 6(b)是无人机飞行时摄像头拍摄的实时场景，红色方框代表识别出的障碍物点(上方阴影是无人机螺旋桨).图 6(c)是真实场景测试时记录下的飞行轨迹和建立的地图，无人机从图 6(c)下方出发，飞行终点在图 6(c)的左上方，其中图片中间偏左部分的像素点区域是无人机识别出的障碍物区域，围绕障碍物区域的绿色线段是无人机飞行的实际轨迹.无人机先是直线飞行，在距离树木大约5m的时候识别出前方的障碍物，随即不断进行航迹的规划，多次试探着改变方向角，同时继续观察周围环境，从而避开障碍物，成功飞向终点.

4 结 语

本文介绍了一种无人机在复杂场景下的自主感知与路径规划系统.首先采用 ORB视觉里程计获取无人机位姿，再使用 SAD障碍物点提取搭配“推扫式”感知技术获得障碍物信息，上述两种方法结合生成局部八叉树地图，其次使用基于本地轨迹库的路径规划算法，最后在NVIDIA Jetson TX2平台上实现了所有的信号处理，并用 GPU对关键算法进行了加速.实验结果表明，所设计的方案能实时处理双目视频信息，感知飞行环境，避开飞行路线上的障碍物.为提高低成本多旋翼无人机自主飞行能力，提高其在未知环境下的避障能力做了积极的探索.