林晓华，王茂森，戴劲松

( 南京理工大学 机械工程学院， 南京 210094)

随着人工智能技术的快速发展，使得小型无人机在军用和民用两个领域均表现出非常重要的应用价值，包括侦察、搜救、探测、追踪以及监控等[1-4]。为了达到无人机自主飞行控制的目的，需要完成两个前提任务，即定位与建图[5]。最早源于机器人领域，Smith和Cheeseman便提出了同时定位与建图(Simultaneous Localization and Mapping, SLAM)[6]，其目的是当机器人在一个未知的环境中移动时能够做到实时重建环境的三维结构并同时估计自身的状态和位置信息。

现有的无人机定位方式多依赖于全球定位系统(Global Positioning System，GPS)，倘若在GPS信号遮蔽严重的区域,这种方法便不再可靠甚至完全失效。但随着视觉技术的不断发展，基于视觉技术的定位方式愈发成为热门的研究方向。文献[7]第一个提出基于EKF的实时单目SLAM系统，成为V-SLAM研究的先驱，但实时运行时间较短且无法处理无人机的速度突变。文献[8]提出了一种基于特征点与直接法混合的半直接法，处理速度快，但缺少回环检测和后端优化部分，使得位姿估计累计误差较大。文献[9]中将直接法应用于半稠密建图的V-SLAM系统中，使得系统可以实时运行在大场景环境中，构建半稠密的地图，但对光线比较敏感。文献[10]提出一种融合IMU的单目SLAM系统，克服了单目SLAM的固有弊端，目前在多旋翼无人机平台上得到较多的运用。除此之外，由于视觉传感器具有重量轻、功耗低、小范围内定位精度高、硬件成本低廉、获取信息更为丰富等优点，使得越来越多的视觉同时定位与地图构建[11](Visual Simultaneous Localization and Mapping，V-SLAM)方案得到开发与应用。本文主要针对无人机在室内无GPS信号情况下无法完成自身定位导航的问题，提出一种基于ROS框架下运行双目SLAM算法进而构建无人机环境占据地图方案，并在计算资源有限的情况下，仅采用关键帧信息进行建图以减少对CPU的占用率。

1 平台搭建

本文方案搭建的硬件平台基于Linux内核，使用ubuntu16.04操作系统，处理器型号为Intel Corei5-8300H，RAM为16.0 GB，能够满足SLAM算法以及地图构建等任务需求。

传感器硬件模块为两个平行放置的杰锐微通HF867- 480P型USB高清摄像头，如图1所示。该摄像头支持UVC通信协议，帧率为30帧/秒，适用于Linux内核下的嵌入式开发。工作电压为5V，无人机搭载的锂电池能够满足其与飞控板的基本载荷需求。

图1 双目摄像头硬件

本文方案的软件模块主要依托于机器人操作系统[12](Robot Operating System,ROS)，建立双相机图像获取模块、算法运行模块以及建图模块之间的数据通信。

ROS是一个适用于机器人的开源的元操作系统。它采用的是一种分布式框架，提供了操作系统应有的服务，包括硬件抽象，底层设备控制，常用函数的实现，进程间消息传递，以及包管理。它也提供用于获取、编译、编写、和跨计算机运行代码所需的工具和库函数。其具有代码复用率高、多语言支持、精简集成化、系统模块化以及便于测试等优点。基于ROS这种分布式框架的优点，本方案将构建地图分为3个节点。

第1个节点：首先通过改写USB相机驱动并调用USB相机功能包，使其能够同时同步调用两个相机以获取双目图像信息，然后利用ROS的“话题发布-订阅”模型，将两个相机所获取的图像数据消息同步发布到usb_cam1/image和usb_cam2/image两个话题。

第2个节点：算法模块通过订阅第一节点所发布的两个相机图像话题，将图像数据、相机参数、词典文件作为输入参数，运行SLAM算法，实现相机轨迹定位及跟踪。此外，还增加点云计算函数，并将点云信息发布到pointcloud_outout话题，同时也将位姿估计信息发布到camera_pose话题。

第3个节点：建图模块通过订阅pointcloud_outout话题以及camera_pose话题，得到点云信息与相机位姿信息，利用Octomap工具构建出占据地图，完成任务。节点关系如图2所示。

图2 三节点关系示意图

2 ORB-SLAM算法框架探析

Klein于2007年提出PTAM[13](Parallel Tracking and Mapping)并开源。ORB-SLAM作为对PTAM的改进[14-15]，被研究者称为第一个现代SLAM方法，在PTAM分离了追踪和地图构建线程的基础上，把回环检测和世界地图的优化从局部地图优化中独立出来，另辟一个线程，形成三线程并行运行结构，使得在线SLAM应用到更大的场景中，并可以应用到室外。

Tracking线程即跟踪线程，主要涵盖SLAM前端，也即视觉里程计(VO)部分。该线程的主要工作是提取ORB特征、初始跟踪估计相机位姿、跟踪局部地图、决定新的关键帧以及当前帧相关信息保存，核心任务是根据图像来估计相机运动。

ORB-SLAM的前端采用特征点法，基于ORB(Oriented FASTand Rotated BRIEF)特征进行特征点提取以及匹配。通过运动模型或者跟踪参考关键帧得到每一帧对应的初始相机位姿估计，倘若跟踪失败，则通过重定位得到位姿估计。同时在该线程中还通过判断是否需要插入新关键帧来更新全局关键帧。

LocalMapping线程即局部地图构建线程，其主要任务是插入关键帧、抛弃外点、创建新的地图点、局部BA(Buddle Adjustment)优化以及淘汰局部关键帧。

由于从双目相机中不断获取得到新图像传入系统，并从Tracking线程中得到新的关键帧输入，局部地图中将会包含当前关键帧和与其成功匹配的其他关键帧，通过优化算法不断剔除坏点以及误差较大的关键帧，从而提高了位姿估计的精度。

LoopClosing线程即闭合环路检测线程，其主要任务是检测闭环以及计算闭环，最后对位姿图进行优化，以达到全局一致性。

线程中，通过遍历附近地图中的关键帧，通过视觉词袋向量，计算检测出候选关键帧。再对候选关键帧生成Sim3求解器，计算其相似变换对当前关键帧的位姿进行修正，得到最佳的闭环匹配。最后，采用g2o(general graph optimization)框架[16]，对系统进行优化。

3 算法改进

原有算法框架通过System∷TrackStereo()这个接口函数实现ORB-SLAM库的调用，在System∷TrackStereo()中调用了Tracking∷GrabImageStereo()函数，最后在Tracking这个类中，mpTracker->GrabImageStereo()最终调用函数Tracking∷Track()计算相机位姿，同时调用NeedNewKeyFrame()函数，决定是否插入关键帧。

对于关键帧的筛选，首先有3种直接不插入关键帧的情况：第1种情况，当前模式处于定位模式下，定位模式下只定位不建图，仅利用已有的地图点对当前相机位姿进行估计，并不更新地图点，因此没有插入关键帧的必要；第2种情况，局部地图处于全部闭环条件下，当局部地图被占用时，插入关键帧反而会影响全局闭环优化；第3种情况，距离上一次重定位较近的条件下，由于重定位成功后，刚刚插入了关键帧，所以不需要频繁插入。

在排除了上述3种情况后，需要对关键帧进行进一步的检测。首先需要保证内点数超过设定的最小阈值以保证跟踪质量，同时避免引入过多的冗余信息，其次需要满足以下3个条件之一：其一，距离上次插入关键帧已经过去了最大限定帧数，目的是防止跟踪丢失；其二，距离上次插入关键帧至少过去了最小限定帧数，主要是为了防止图像重叠度太高，并且使得局部建图线程有足够的空闲处理新的关键帧；其三，局部建图线程中关键帧队列中关键帧数量不超过3个，主要也是考虑到局部建图线程的处理能力以及信息冗余度问题。

关键帧相当于SLAM系统的骨架，是在局部一系列普通帧中作为局部帧的代表，记录着局部信息。在相机保持静止的状态下，普通帧也会一直增加，而关键帧因为总是看到原场景，因此不会改变。所以在普通帧每两帧之间会存在大量的信息冗余，如果所有帧都参与计算，不仅浪费算力，对处理器内存也是极大的考验。此外，关键帧选择时还会对图片质量、特征点质量等进行考察，一定程度上发挥了滤波作用，可以防止无用或者错误的信息进入优化过程而破坏定位建图的准确性。

因此，便可利用关键帧信息进行占据地图的构建工作。在原有算法框架基础上，增加关键帧判断标志位“isKeyframe”，加入到各层函数入口参数，在ROS节点订阅源文件中使用带有关键帧的库调用入口，实现算法改进。最终调用接口流程如图3所示。

图3 改进后函数调用接口流程框图

4 实验参数与结果分析

首先采用张氏标定法对双目相机进行标定。张氏标定法首先要从至少两个以上角度对棋盘格进行拍摄，将棋盘格的角点作为标定点。将一个标定点记为Q，其在相机坐标系下的坐标记为Q=(x,y,z)，转换成对应的齐次坐标形式为Q′=(x,y,z,1)，假设q为Q对应在相机成像平面的点，q在图像坐标系下的齐次坐标值记为q=(m,n,1)。那么空间中点Q和成像平面上的q之间的转换关系如下：

(1)

其中λ≠0为比例系数，矩阵M为相机内参数矩阵，矩阵R和矩阵T为旋转矩阵和平移矩阵。因为标定棋盘为一个平面，故把世界坐标系构建在Z=0的平面上，那么上式可改写为如下形式：

(2)

式中r1、r2、r3分别代表旋转矩阵的第一、二、三列。设

(3)

式中μ≠0为比例系数，因为r1、r2为旋转矩阵中的2个向量，因此r1和r2必正交，故推导得：

(4)

为方便求解上式，设一对称矩阵A=M-TM-1如下：

(5)

显然矩阵A为一个对称矩阵，现定义一个6维向量a来表示A，公式如下：

(6)

接着进行下一步的数学化简有：

(7)

将式(7)改为以a为未知数的2个其次方程：

(8)

迭代n组拍摄的图像，可得以下方程组：

(9)

整理后可得：

Vb=0

(10)

式(10)中，V是一个2n×6的矩阵。当n≥3时，a是唯一的；当n=2时，可使A12=0。该式的解为VTV的最小特征值所对应的特征向量。根据求得的a可根据式(7)得到矩阵A，这样就可以求得所需要的参数矩阵M。此外平移矩阵T和旋转矩阵R可通过内参数矩阵M计算求得，公式如下：

(11)

进行标定时，标定界面如图4所示，使用8×6标定板，其每个方格边长为24 mm，得到最终标定结果如表1、表2所示。

图4 双目相机标定界面

表1 右相机标定参数

表2 左相机标定参数

在对实际场景建图前，首先采用TUM数据集的freiburg1_xyz序列对算法改进前后的建图效果，如图5、图6所示。其中两幅图的左半部分为改进前的建图效果，右半部分为改进后的建图效果。可见，算法改进前后建图效果总体上保持一致，仅在一些小细节上有细微差别，但对于构建无人机环境地图，这些细小的区别并不会产生实质的影响。

实际室内场景实验时，在ROS框架下依次启动相机驱动模块、算法模块、建图模块3个节点，运行改进后的算法，在走廊、楼梯口实验场景中，仅对关键帧进行跟踪，构建得到的效果图与实际场景如图7、图8所示。可见实际场景中占据地图构建效果良好。

图5 改进前后点云地图效果

图6 改进前后占据地图效果

图7 走廊场景建图效果

图8 楼梯场景建图效果

在运行整体方案的同时，利用系统命令Top，监测各个进程实时占用内存以及CPU占用率，监测界面如图9所示。其中PID为进程ID号，%CPU为当前进程的CPU占用率，%MEM为当前进程占用的物理内存和总内存的百分比，COMMAND为当前进程的启动命令名称。

图9 线程监测界面

对于改进前后算法，分别在实际场景中运行并构建占据地图，同时在相机移动时以及移动后静止时分别监测建图进程的CPU占用率20 s，得到数据用软件绘制得到如图10、图11所示。其中CPU超过100%的占用率是因为实验中所使用的CPU具有四核，满载负荷运行理论上能够达到400%。

由图10、图11分析可知：无论是在相机移动时或者静止时，本文所利用改进的仅跟踪关键帧线程构建占据地图方案总体上看对CPU占用率有较为明显的降低，这为无人机板载计算机减负能够起到一定的作用。

图10 移动建图时占用率比较

图11 静止建图时占用率比较

5 结论

针对无人机在室内无法使用GPS的情况，设计了一种仅利用关键帧进行占据地图构建的VSLAM方案。利用ROS操作系统的分布式框架特点，设计了驱动、算法、建图3个信息节点接口用于信息交互，使用双目相机作为环境信息采集入口，最终利用ORB-SLAM获取的关键帧信息，完成占据地图的构建。实验结果表明，该设计方案能够实现无人机占据地图构建，并且可以有效地减少构建地图线程的CPU占用率，为无人机自主导航提供了一种参考方案。