基于视觉—惯性的四旋翼无人机自主导航技术的研究

何玲

摘 要 随着科技的飞快发展，无人机的应用场合日益增多，同时也给人类带来诸多便利。文章着力于四旋翼无人机自主导航技术的初步研究，结合国内外对无人机自主导航的研究提出常见的研究问题和方法。

关键词 自主导航；定位；SLAM；路径规划

中图分类号 TP3 文献标识码 A 文章编号 1674-6708（2018）221-0133-04

1 研究意义

随着科技的进步，无人机逐渐走入人们的生活。无人机可以被快递公司用于主站到分站货物运输工作。无人机也可承担高速公路或城市拥堵道路的监控和疏导工作。携带摄像机的无人机的出现，给影视行业提供了极大便利的同时也大大降低了制作成本，也使本无法企及的复杂危险环境的拍摄成为可能，比如活跃火山口的拍摄。装上激光测距或双目摄像头等具有深度信息的无人机甚至可以完成大型建筑、山脉的三维扫描及测量。

然而，无人机很难操控，即使专业的飞手也经常因失误而出现坠机的情况。因此，排除人为因素，研究出安全稳定的自主飞行无人机成为人们的一致追求。当前主流自主导航方法是GPS—惯性系统。这在多数情况下是可行的，但在室内GPS没有信号，因此无法导航。本课题拟使用视觉—惯性系统组合的导航方式，辅以超声波测距对距离矢量的修正，从而实现较高精度的自主飞行。

2 国内外研究现状

在过去的几年里，微型飞行器激起了人们对无人机自主导航的极大兴趣。美国宾夕法尼亚大学的GRASP实验室首次利用四旋翼无人机成功实现室内躲避障碍物的自主导航飞行实验。GRASP实现利用深度像机和激光测距仪实时构建三维空间地图来引导四旋翼无人机在室内导航、翻转及侧向飞行通过狭小的窗口。在平衡控制上，该实验室成功控制了飞行中的四旋翼无人机上直立木棍的平衡。而在多机配合飞行控制上，该实验室实现了机群跟随音乐进行编队飞行、协同飞行等形式的表演。

2012年，德国慕尼黑工业大学Jakob Engel等人利用单目视觉SLAM技术，融合IMU及气压计数据，成功实现四旋翼无人机固定轨迹的飞行[1]，这也是首次在廉价微型旋翼无人机上实现的自主飞行实验。

2013年，清华大学在国际空中机器人大赛（IARC）第六代比赛中获得冠军。该校使用一家德国公司生产名为Pelican的四轴飞行器，越过已经成熟的飞行器硬件及飞控系统的开发工作，将主要精力集中在室内自主导航的研究上。该团队利用机载处理器采集前向摄像机、底部摄像机及激光测距仪等传感器的数据，通过无线网络传输到地面控制站，并由地面控制站进行数据融合、滤波、构图、预测并生成最终设定时间点的导航控制命令，再次通过无线网络传输回传到无人机，进而实现导航。

2015年，复旦大学在大疆公司的Matrice 100无人机上为停车场管理者打造出了一款简单易用、意义非凡的智能城市解决方案。该系统可以帮助交警或其他管理者识别违章停放的车辆。无人机沿着事先设定好的航线上飞行并通过机载摄像机实时识别每一辆汽车。该系统内置英特尔技术，可以识别车辆的停放位置及方向，从而检测出车輛是否按规定停好。如果发现了违章停放的车辆，无人机将拍下该车辆车牌照片，并传回地面控制中心。

3 常见研究点与经典方法

一般情况下，要想在未知环境中实现导航，必须具备这些能力：构建地图、地图定位和运动控制。这三大要素可以解决导航的4个基本问题： “世界看起来是什么样子的？”“我在哪里？”“我要去哪里？”和“我要怎么去那里？”。“世界看起来是什么样子的？”和“我在哪里？”由SLAM（Simultaneous Localization and Mapping，同步定位与构图）解决。而“我要去哪里？”和“我要怎么去那里？”则被归纳到路径规划中完成。

3.1 同步定位与构图

同步定位与构图（SLAM）并不是单个的算法，而是一个系统框架，定义了一个宽泛的思想性的概念。研究者需要利用不止一种算法并相互配合来完成。SLAM系统主要有4个部分组成：问题建模、传感器数据、地图描述以及回环检测，下面将分别对其介绍。

3.1.1 问题建模

SLAM 研究源起于Smith1986年发表的空间不确定性估计与表达[2]一文。在该论文中，定位问题是指机器人的运动轨迹，地图则由一系列路标点构成。该文所述方法是基于滤波的。这类基于滤波的方法一直在早期的 SLAM 中占据主导地位，例如SIFT特征的提出者Lowe曾用EKF和SIFT实现了视觉SLAM[3]，最早的实时视觉SLAM系统即是基于EKF[4]。而后，粒子滤波器，FastSLAM以及基于优化的方法才逐渐兴起。这个建模的过程，通常称为SLAM 的基础理论。由于传感器和惯性测量设备均存在误差，得到的轨迹与地图通常与真实情况存在着一定的偏差。如何减少噪声带来的影响，建立准确、一致的地图，是 SLAM 研究者们关心的主要问题。

3.1.2 传感器数据处理

目前，SLAM研究中使用的传感器包括激光测距仪、单目相机、立体相机、深度相机、声呐以及多种传感器信息融合。SLAM的求解方式与传感器有着重要的关联，因为从本质上说，SLAM所有的计算都是对传感器数据的处理。通常来说，传感器的选择不同，SLAM基本方程的参数化会有很大的差别。如基于激光传感器的 SLAM 一般使用EKF实现，基于相机的则用姿态图（Pose Graph）进行优化，这是由于不同传感器的特性不同而造成的，如表1。

3.1.3 地图描述

建图的问题则主要取决于地图的类型。不同类型的地图各有侧重点，应用于不同的场合。主要有以下几种类型。

1）度量地图（Metric Map）。度量地图利用物体间的精确位置关系来构图。度量地图的具体形式可分为稀疏形式（Sparse Map）与密集形式（Dense Map）。稀疏形式主要指由路标组成的地图，而密集形式主要指占据网格地图（Occupancy Grid Map）。稀疏路标图主要用于早期研究，由于计算性能的限制，机器人的运动非常缓慢，地图的规模也相对较小。目前常用的是密集型的占据网格地图。通常把地图按照某种分辨率分割成许多个小块，以矩阵（对应二维情形）或八叉树（对应三维情形）来表示。度量地图简单易于理解，但每一个网格点的状态都需要保存，因此会消耗非常大的存储空间。

2）拓扑地图（Topological Map）。与度量地图的精确性相比，拓扑地图的表达角度完全不同。它采用由顶点和边组成的图（Graph）：G={V，E}的方式表达，因而更强调地图元素的独立性，以及元素之间的连通关系。其中顶点表示特征点的姿态，边表示运动。拓扑地图是一种比较紧凑的地图表达方式。它可以放松地图对精确位置的需要，去掉了地图的很多细节问题。但是，拓扑地图很难表达具有复杂结构的环境。拓扑地图下一步研究的主要方向是对地图进行分割形成结点与边的问题。使用拓扑地图进行导航与路径规划也是后续需要解决的重要一环。

3）混合地图（Hybrid Map）。由于上述地图表达方式各有优劣，因而有研究者认为应该构建带有层次模型的地图，混合使用不同的表达方式来处理地图。其核心思想是：用度量地图表达局部结构，用拓扑地图表达各个小地图之间的连通关系。此类方法有成功的案例，但由于技术复杂，不易于推广。

3.1.4 回环检测

回环检测（Loop Closure Detection），是指机器人识别是否曾经到过某个地方。如果检测成功，将进行环路融合计算，找出一个最优的更加接近真实的位置，从而显著地减小累积误差。

回环检测本质上是通过检测观测数据相似性来实现的一种算法。对于视觉 SLAM，有的研究者使用传统的模式识别的方法，把回环检测建构成一个分类问题，训练分类器进行分类。但多数的SLAM使用词袋模型（Bag-of-Words）[5]。词袋模型通过机器学习对从周围环境里大量图像提取的视觉特征进行聚类，利用特征类别建立较为精简的词典，然后将每幅图都用词典中的单词进行表达。回环检测的过程就是将当前的图像中的单词与历史图像中的单词进行比较，相似度达到指定的阈值即视为回环检测成功。

然而，错误的回环检测结果可能会使地图变得更糟糕。该错误分为两类：一类是假阳性（False Positive），也称作感知偏差（Perceptual Aliasing），即实际上不同的场景被归为同一场景；另一类是假阴性（False Negative），又称感知变异（Perceptual Variability），即事实上同一个场景被分成了两个不同场景。感知偏差将使地图质量变差，是希望避免的。尽量多地检测出真实回环是检验回环检测算法的重要标准。通常用准确率-召回率曲线来量化一个检测算法的好坏。

3.2 路径规划（Path Planning）

路径规划是自主移动机器人的一个非常重要的组成部分。对于机器人来说，只有地图还不行，它需要知道在地图上如何实现从起始位置到达目标位置。路径规划就是在地图已经存在的前提下，按照一定的标准要求，在充满障碍的环境中寻找从起始状态到达目标状态的没有碰撞的符合要求的路径。

随着不同领域对移动机器人的广泛需求，越来越多的研究人员投入到了路径规划领域的研究，进而提出很多有效的路径规划算法。比如概率路线图法、快速搜索随机树法等等。

3.2.1 概率路线图

概率路线图（Probabilistic RoadMaps，PRM）算法[6]是在1996年由Lydia Kavraki、JeanClaude Latombe提出的。概率路线图按处理流程的先后分学习和查询两个阶段。学习阶段构建路线图，查询阶段则是在构建好的路线图中找出合适的路线。学习阶段在位姿空间中概率随机地采样，并将位于自由位姿空间中，即无碰撞的采样点连线成边，由无碰撞的采样点和边共同组成了一张较为复杂的随机路线图。查询阶段将初始点和目标点分别连接到前一阶段生成好的路线图中，然后使用局部规划器，在路线图中随机查找连接初始点和目标点的连接路径。当初始点和目标点在同一个连通分支内时，一种可行的路径很大几率能被查找到。当初始点和目标点不在同一个连通分支内时，则回到学习阶段，增加更多的采样点，从而增加路径联通的可能性，再次进入查询阶段进行路径搜索。循环进行这两个阶段的操作，一直到路径被找到为止。

与之前的算法相比，概率路线图克服了局部极小问题，且计算量小，适合多自由度的机器人路径规划。但是当环境中有窄道时，非常有可能出现采样率低的现象，导致无法找到合适的路径。

3.2.2 快速搜索随机树

快速搜索随机树（Rapidly-Exploring Random Tree，RRT）算法[7]由Steven M. Lavalle在1998年提出。该方法与概率路线图（PRM）不同，它不需要PRM学习阶段那样先要对整个空间建模，能够有效地解决高维空间和复杂约束条件下的路径规划问题。RRT算法是基于最优控制理论思想建立的，从起始状态开始通过应用控制输入，在最短时间内递增地生长一棵搜索树到达新的状态。树上的顶点表示状态，有向边表示控制输入。控制输入作用在先前的状态从而得到新的状态。当一个顶点到达目标状态或期望的目标区域时，路径搜索成功。快速搜索随机树算法引入微分约束，以使規划的路径更加平滑，更适合机器人行进，也更具合理性。快速探索随机树算法被广泛用在复杂的工作环境、存在微分约束的环境及高维状态空间中。

快速搜索随机数算法仍然不够完美，当出现较多狭窄空间时，会导致收敛特别慢。有人采用双向搜索技术，从起始状态和目标状态同时相向地增长搜索树，从而提高搜索效率。也有人通过增加导向因子来引导节点扩展方向，从而使搜索树有目标地生长，而不是漫无目标，这大大缩短了生长时间。

3.2.3 RRT*

虽然RRT算法具有运算速度快的优点，并且能在较短时间内找到解，但是由于RRT算法随机性很大，已经证明是无法找到最优解的。为了解决这个问题，Karaman和Frazzol在 RRT 算法基础上，引入节点选择机制，提出了RRT*（RRT Star）算法[8]，这种算法虽然同样无法找到最优解，但是可以保证找到一个接近最优的解。

4 预期创新点

4.1 地图创新点

基于特征的SLAM一般会因为计算效率的问题而采用关键帧的方法，因而生成的地图一般是稀疏的点云地图。在稀疏点云地图上生成的占据栅格地图，一般在复杂的环境中不足以实现无碰撞导航。如何能在保证计算效率（实时）的前提下，保证复杂环境的无碰撞导航是本文要实现的目标之一。

4.2 路径规划创新点

基于RRT*路径规划算法获得的导航路径是一个渐进最优的图，包含多个顶点和有向边。每个顶点为无人机姿态，有向边为控制输入。实现导航就是让无人机根据有向边的控制输入，达到每个顶点的姿态，最终到达目标区域。事实RRT*给出的路径图并不是平滑的，是一个个离散状态的集合，连接两个顶点的边是直线，即只对应一个输入，因此无人機很难无偏跟随这条导航路径。在无人机输入极限的范围内，如何优化（平滑）这条导航路径，使其可以无偏跟踪是本文要实现的另一个目标。

参考文献

[1]Jakob Engel， Jürgen Sturm， and Daniel Cremers， “Camera-Based Navigation of a Low-Cost Quadrocopter，” in Intelligent Robots and Systems（IROS）， 2012.

[2]Smith R C， Cheeseman P， “On the representation and estimation of spatial uncertainty，” in International Journal of Robotics Research， 1986.

[3]Se S， Lowe D， and Little J， “Mobile robot localization and mapping with uncertainty using scale-invariant visual landmarks，” in International Journal of Robotics Research， 2002.

[4]Davison A， Reid I， Molton N， et al.， “Monoslam： Real-time single camera SLAM，”in IEEE Transactions on Pattern Analysis and Machine Intelligence， 2007.

[5]Filliat D， “A visual bag of words method for interactive qualitative localization and mapping，”in IEEE International Conference on Robotics and Automation （ICRA）， 2007.

[6]Kavraki L E et al.， “Probabilistic roadmaps for path planning in high-dimensional configuration spaces，” in IEEE Transactions on Robotics and Automation， 1996.

[7]Steven M. Lavalle， “Rapidly-exploring random trees： A new tool for path planning，” in Technical Report No. 98-11， 1998.

[8]Sertac Karaman， Emilio Frazzoli，”Sampling-based Algorithms for Optimal Motion Planning，” in The International Journal of Robotics Research， 2011.