基于ROS的自主无人机VSLAM研究*

刘峰，吕强，郭峰，王国胜

(装甲兵工程学院 控制工程系, 北京 100072)

导航、制导与控制

基于ROS的自主无人机VSLAM研究*

刘峰，吕强，郭峰，王国胜

(装甲兵工程学院 控制工程系, 北京 100072)

针对未知复杂环境中无人机无法获得外部辅助情况下自主导航所面临的严峻问题，提出在ROS框架下在板运行单目VSLAM算法的自主无人机方案，仅依靠自身摄像机自主地完成SLAM和导航任务。研究VSLAM算法原理与前沿算法ORB-SLAM，设计并搭建了自主导航无人机平台，针对搭建的无人机平台方案和特点完成视觉定位部分的改进设计。实验表明，自主无人机能够在未知环境中，自主实现同时定位和地图构建任务并完成精确的飞行控制与导航。

视觉同时定位于地图构建；自主无人机；ROS；视觉定位；位姿图优化；自主导航

0 引言

微小型空中无人平台在军事侦察、灾难救援、探测和监控等领域都扮演着重要的角色，无人机的自主飞行控制对空中无人平台走向智能化具有里程碑的意义。然而自主无人机在飞行过程中的最大挑战是定位信息的获取，目前仅仅依赖GPS的定位方式缺乏精度和可靠性，GPS信号在有覆盖的环境中容易丢失。视觉传感器能够有效获取周围环境信息，重量轻，功耗低，所以采用视觉方法进行无人机定位正逐渐成为研究热点[1-6]，文献[1]的方法被成功应用于无人机中[7-8]。

对无人机的飞行控制大部分还采用人工远程控制，法国派诺公司的ARDrone四旋翼[9]搭载视觉传感器实现无人机室内定位，通过Wi-Fi将图像数据传回地面站计算机，地面站计算机完成数据处理并控制无人机的飞行。慕尼黑科技大学搭建四旋翼无人机[10]RGB-D摄像头的稠密跟踪与地图构建系统，获取室内环境3D模型完成无人机定位和自主飞行，算法依赖地面站GPU，需要无线模块完成数据传输。这些无线通信的方式、数据和控制命令易受干扰，且存在一定的控制延迟。瑞士苏黎世联邦理工学院提出微型控制无人机半直接单目视觉里程计方法[11]，具有精度高和运行速度快的特点，该方法也实现了在板嵌入式计算机的实时运行，数据和控制不再依赖无线方式。但是在长距离和大尺度的环境中精度与鲁棒性不高，目前还只是纯粹的运动估计，还没有实现环境三维重建。

本文主要研究自主飞行的多旋翼无人机视觉定位与地图构建，设计了多旋翼无人机自主导航控制软硬件平台，研究视觉定位与地图构建算法，并成功利用该方法实现无人机的室内视觉定位，完成无人机的室内自主导航飞行。

1 平台设计

1.1 硬件

本文搭建的多旋翼无人机平台包括Pixhawk飞行控制板、视觉传感器、低功耗在板主控计算机、电机和14.8 V锂电池。能够满足基本的载荷要求，最短续航能力为20 min，最大直径75 cm。图1为该无人机平台。

图1 自主无人机平台Fig.1 Autonomous UAV platform

在板计算机是多旋翼无人机视觉定位与地图构建以及自主飞行控制的核心。本文的在板计算机搭载Intel Core i7处理器，能够满足定位、地图构建、障碍物检测和路径规划等高级任务需求，采用Ubuntu12.04操作系统，搭载Hydro机器人操作系统。该在板计算机的优势在于大小仅为25 mm×103 mm×113 mm，质量为0.270 kg。

飞行控制板是执行自主命令和控制飞行的枢纽，其内部具有16位陀螺仪，一个STM公司的14位加速度计和磁力计，以及MS5611气压计。并提供了多个接口来连接外部传感器，适用于各类无人机。为了增强系统的稳定性和拓展性，本文还连接了数传和GPS等模块。

单场次洪水总量对比选取了1992—2016年系列中，实测流量最大年份1998年的最大实测洪水段，其实测最大流量为流量208 m3/s；汛期总量和年总量选取汛期径流相对丰沛的2011年进行对比分析计算；其中推算流量，高水部分采用1992-2016年历年单值化关系线推算，中水部分多年单值化关系线推算，低枯水部分采用单年率定关系线推算，对比结果见表5。

飞行控制板上的串行通信接口与主控计算机的USB接口连接，建立了主控计算机与飞行控制板之间的数据桥梁。单目视觉传感器通过USB接口直接连接在主控计算机上。

1.2 软件

软件主要基于机器人操作系统(ROS)[12]框架，建立各模块通信、采集传感器数据和设计自主位置控制。

机器人操作系统是一款新颖的主要应用在机器人平台操作与控制系统的软件框架，其优势可以归结为：点对点设计、工具包丰富、代码复用率高、多语言支持(C++，Python，Java)、精简集成化、系统模块化、免费开源和便于测试。

通信接口节点是主控计算机与Pixhawk飞行控制板之间的信息传输枢纽，该节点是实现主控计算机实时获取飞行器飞行状态和对飞行状态进行自主控制的关键。该节点主要通过Mavlink协议接收多旋翼飞行器的心跳包、姿态角数据包和位置数据包等，并按类别以特定消息格式将数据发布到不同的主题上。同时订阅了vision和position等主题，再以Mavlink协议的形式将这些主题数据发送到飞行控制板，获取外部传感器的数据并实现自主飞行控制。自主控制节点的设计原理可以总结为订阅飞行器的当前位置主题，提供位置反馈，再将位置控制命令发布到通信节点的/mavros/setpoint/local_position主题上。通过对无人飞行器状态的实时监测调整位置控制信息。

2 无人机视觉定位与地图构建

本节主要研究一种实时且高精度的视觉simultaneous localization and mapping (SLAM)算法[13]，该单目视觉定位与地图构建方法，能够在室内和室外，大规模和小规模实时运行，可以完成闭环检测和重定位，包括自动的地图初始化，且选取了最合适的方法来选择特征点和关键帧。针对其特点，对算法完成改进工作，并成功运用在搭建的自主四旋翼无人机平台，在无其它定位方法的情况下，完成四旋翼无人机的视觉定位与地图构建。

2.1 地图初始化

使用标准的DLT和八点算法[14]并行计算单应性矩阵Hc和基本矩阵Fc，然后对每个模型计算得

(1)

(2)

采用如下公式的计算方法完成2个模型选择：

(3)

当RH>0.45时即处理的是平面或者低视差的情形，选择单应性矩阵模型。否则选择基本矩阵模型。

模型选择完成后需要从模型中得到相关的运动估计。在单应性矩阵的情况下，利用八点计算的运动假设方法处理，在视差较大的基本矩阵情况下，利用SVD方法计算4种运动假设完成估计。

2.2 跟踪

跟踪线程对每一帧图像完成位姿估计，主要包含5个步骤：ORB特征提取、帧对帧的初始位姿估计、全局重定位的初始位姿估计、跟踪局部地图、确定新的关键帧。其框架如图2所示。

图2 跟踪线程框架Fig.2 Tracking thread frame

采用帧对帧和利用关键帧进行全局重定位2种位姿估计方法大大提高了图像跟踪的鲁棒性。其次跟踪线程中还需要一个插入关键帧的策略来更新全局关键帧。

2.3 局部地图构建

局部地图构建线程主要对每个新的关键帧Ki完成局部地图的构建。可分为5个步骤完成，分别为：插入关键帧，淘汰最近地图点，创建新的地图点，局部光束平差以及局部关键帧淘汰。局部地图中包含当前关键帧K1以及与其关联的其他关键帧组K2(这组关键帧与当前图像帧有共同的地图点)中地图点的集合。跟踪线程中新的特征点跟踪该局部地图，提高了位姿估计的精度。

2.4 环路闭合检测

环路闭合检测线程主要是针对局部地图构建线程中最后处理的关键帧Ki来进行。通过关键帧Ki的视觉词袋向量计算其与系统中关键帧的相似性，完成候选的闭环检测。倘若有多个位置的关键帧与Ki的场景外观相似，则有多个闭环候选。

在确定了闭环后，则需要完成闭环修正，通过计算得到的相似性变换对当前关键帧的位姿进行修正，同时这个修正将影响到与当前关键帧相邻的关键帧节点。最后进行位姿图优化达到全局一致性，优化后，每个地图点根据修正的关键帧进行变换。系统中所有的优化都采用了g2o(general graph optimization)框架[15]。

2.5 参数设定

针对本文的无人机平台，需要对视觉定位与地图构建算法的相关参数进行设置。具体参数的值如表1所示。参数的设置主要根据在板主控计算机的处理能力、实时性和精度等进行综合考虑，使系统的性能和稳定性更高。其中ORB特征点数为每个图像帧中特征点的个数，FAST检测阈值保证了最低的检测特征点数。

表1 算法参数设置Table 1 Algorithm parameter settings

3 实验分析

本节将对无人机视觉导航进行实验分析，验证自主导航控制精度，并完成室内环境的地图构建。

由于在ROS中rosrun命令只能单次运行一个节点，实验平台需要对多个节点同时运行且完成相关参数设置，所以采用.launch启动文件来编排启动节点和初始化参数，同时还能运行ROS内核。图3表示的是节点关系，其中圆圈表示主题。

图3 节点关系图Fig.3 Relationship diagram between nodes

验证实验为实验室真实环境中的定点导航飞行，同时对环境进行地图构建。无人机的飞行区域在实验室中央3 m×6 m的无障碍区域。单目视觉传感器的视野水平朝向飞行器侧面且飞行过程中保持偏航角不变。自主导航飞行的高度设置为1.5 m。

图4 自主导航水平位置精度Fig.4 Horizontal position accuracy of autonomous navigation

图5 自主导航三维位置精度Fig.5 Three-dimensional positional accuracy of autonomous navigation

实验中地图构建的真实环境部分图像如图6所示。图7为导航与地图构建实验关键帧的轨迹和构建的真实环境三维地图点，观察视角为俯视图，其中蓝色的轨迹为算法关键帧轨迹，黑色的点为空间三维地图点，红色的点为当前传感器视野图像中的空间地图点，图6中A和B位置分别对应图7中的2幅环境图像。系统共构建了5 722个环境地图点，包含131帧图像关键帧，飞行过程中平均每帧图像跟踪约150个特征点。三维环境地图点构建的区域约为10 m2的真实实验室办公环境。

图6 地图构建环境部分真实图像Fig.6 Partially true image of maps built environment

图7 关键帧轨迹和环境特征点地图Fig.7 Keyframe track and environmental characteristics point map

4 结束语

多旋翼无人机灵活机动、已经广泛应用于军事侦察和灾害救援等领域。在缺乏GPS信号的未知环境中，基于视觉的飞行器定位导航和地图构建逐渐成为研究热点。本文主要研究自主的多旋翼无人机视觉定位与地图构建，利用视觉方法完成了无人机的定点导航和地图构建。实验验证结果表明本文的系统能够在完全未知的环境中，利用视觉的方法自主完成精确的导航飞行控制和地图构建任务，不需要其他辅助定位系统，且具有较高的导航飞行控制精度。

[1] KLEIN G, MURRAY D. Parallel Tracking and Mapping for Small AR Workspaces[C]∥Mixed and Augmented Reality, 2007. ISMAR 2007. 6th IEEE and ACM International Symposium on. IEEE, 2007: 225-234.

[2] MUR-ARTAL R, TARDOS J D. ORB-SLAM: Tracking and Mapping Recognizable Features[C]∥Robotics: Science and Systems (RSS) Workshop on Multi View Geometry in Robotics，2014.

[3] CONCHA A, CIVERA J DPPTAM. Dense Piecewise Planar Tracking and Mapping from a Monocular Sequence[C]∥Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on. IEEE, 2015: 5686-5693.

[4] DAVISON A J, REID I D, MOLTON N D, et al. MonoSLAM: Real-Time Single Camera SLAM[J]. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 2007, 29(6): 1052-1067.

[5] YANG S, SCHERER S A, SCHAUWECKER K, et al. Autonomous Landing of MAVs on an Arbitrarily Textured Landing Site Using Onboard Monocular Vision[J]. Journal of Intelligent & Robotic Systems, 2014, 74(1-2): 27-43.

[6] 林辉灿，吕强，张洋，等.稀疏和稠密的VSLAM的研究进展[J].机器人，2016，38(5)：621-631. LIN Hui-can,LÜ Qiang,ZHANG Yang,et al.The Sparse and Dense VSLAM:A Survey[J].Robot,2016,38(5):621-631.

[7] SCARAMUZZA D, ACHTELIK M C, DOITSIDIS L, et al. Vision-Controlled Micro Flying Robots: From System Design to Autonomous Navigation and Mapping in GPS-denied Environments[J]. Robotics & Automation Magazine, IEEE, 2014, 21(3): 26-40.

[8] ENGEL J, STURM J, CREMERS D. Scale-Aware Navigation of a Low-Cost Quadrocopter with a Monocular Camera[J]. Robotics and Autonomous Systems, 2014, 62(11): 1646-1656.

[10] ENGEL J, STURM J, CREMERS D. Camera-Based Navigation of a Low-Cost Quadrocopter[C]∥Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conference on. IEEE, 2012: 2815-2821.

[11] FORSTER C, PIZZOLI M, SCARAMUZZA D. SVO: Fast Semi-Direct Monocular Visual Odometry[C]∥Robotics and Automation (ICRA), 2014 IEEE International Conference on. IEEE, 2014: 15-22.

[12] MARTINEZ A,FERNANDEZ E.Learning ROS for Robotics Programming[M].Birmingham:Packt Publishing Ltd.,2013.

[13] MUR-ARTAL R, MONTIEL J M M, TARDOS J D. ORB-SLAM: A Versatile and Accurate Monocular SLAM System[J]. Robotics, IEEE Transactions on, 2015, 31(5): 1147-1163.

[14] HARTLEY R, ZISSERMAN A. Multiple View Geometry in Computer Vision[M].New York: Cambridge University Press, 2003.

[15] KÜMMELE R, GRISETTI G, STRASDAT H, et al. g2o: A General Framework for Graph Optimization[C]∥Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011: 3607-3613.

VSLAM of Autonomous UAV Based on ROS

LIU Feng，LÜ Qiang，GUO Feng，WANG Guo-sheng

(Academy of Armored Forces Engineering,Department of Control Engineering, Beijing 100072，China)

Inunknown complex environments, unmanned aerial vehicles(UAV) faces the problem of autonomous navigation without external aid. To solve the problem, an autonomous UAV program with monocular visual simultaneous localization and mapping(VSLAM) algorithms running onboard under ROS framework is proposed. It can autonomously achieve SLAM and navigation task only relying on its own camera. The principle of VLSAM algorithms and the state-of-the-art ORB-SLAM algorithm are studied and a UAV autonomous navigation platform is designed and built. Aiming at the platform and its characteristics, the visual positioning design is improved. Experiments show that the autonomous UAV can achieve autonomous simultaneous positioningand complete map building task and precise flight control and navigation in unknown environments.

visual simultaneous localization and mapping(VSLAM)； autonomous unmanned aerial vehicles(UAV); robot operating system(ROS); visual localization; general graph optimization;autonomous navigation

2016-01-22；

2016-03-11

刘峰(1987-)，男，江西吉安人。博士生，主要研究方向为机器人控制，计算机视觉，SLAM。

10.3969/j.issn.1009-086x.2016.06.011

V279；V448.22+4；TP391.41

A

1009-086X(2016)-06-0061-06

通信地址：100072 北京市丰台区杜家坎21号院

E-mail：18606921232@163.com