基于双目视觉的SLAM四旋翼无人机

谭凯元，朱嘉林，邓 君，王荔蔷，杨家源

（东莞理工学院机械工程学院，广东东莞 523000）

0 引言

近年来，随着人民生活质量的提高，机器人的应用也越来越广泛，其在不同的领域发挥着重要的作用。而且在许多地方，机器人已经可以代替人类去完成一些工作，特别是可以帮助人类完成复杂的计算工作，或者在一些对人类来说具有安全隐患的工作环境，机器人更是取代人类去完成这些危险的工作。而在诸如此类的工作环境中，机器人需要强大的同步定位和自动建图能力，这能够让机器人在未知的环境下对周边的环境进行实时的地图构建，以便机器人在之后的进行自主导航和路径规划。

为了进行实时地图的构建，所选择的载体机器人应该为较为灵活机动的，所以现在市面上的传统选择为小车。但是这种SLAM 小车最大的缺点是对地形的环境、地貌有很多要求，会使地图的构建产生相当多的缺陷，有许多地方无法建图，大幅度会受限于地形地貌[1]。而运用无人机搭载摄像头进行建图可以克服如此众多缺点，突破地形限制，更加灵活方便，同时构建的地图也会更加立体完全。

无人机又称无人机驾驶飞行器，是近年来发展起来的一个集空气动力学、材料力学、自动控制技术、软件技术为一体的高科技产品。无人机广泛用于空中侦察、监视、通信、反潜、电子干扰等领域。目前世界上有30余个国家和地区研制出了50多种无人机，无人机型号超过300余种，有55个国家装备了无人机[2]。

本文所设计的双目SLAM 无人机是一种应用于复杂地形地貌下建模的机器人[3]，其上面搭载了双目摄像头，进行对周边环境的信息进行采集，并装载了树莓派将双目摄像头采集到的信息筛选处理并转化成点云图和输出，同时树莓派还进行与飞控板之间的信息交流，实时控制无人机位姿、运动位移。最终将地图完整地构建之后，再通过不同的算法规划和筛选出无人机从起点到终点的最优路径解[4]，从而实现双目SLAM 无人机的自主导航功能。

1 双目SLAM无人机整体结构

1.1 控制部分

无人机的控制部分是一块树莓派4B 主控板，树莓派4B 主控板相当于一个微处理器，具有独立的CPU芯片，可以实现本项目所需的建图功能。通过在树莓派中烧录一个Ubuntu16.04系统，并安装ROS 系统，将ORB-SLAM 建图，定位避障，自主导航各种算法集成在一起，无人机能够实现自动建图，自主导航的目标。

1.2 信息采集部分

无人机的信息采集部分主要是采用USB 双目摄像头，USB双目摄像头是一款集双目图像及IMU 为一体的高清图像传感器，该设备支持Windows、Linux、Android 等多种平台，且支持C/C++、Python 等各种编程语言，不仅支持PC 桌面操作系统，还支持嵌入式系统，如树莓派开发板。适合应用于ORBSLAM建图。

1.3 外观部分

其主体外观如图1所示。

图1 双目无人机主体外观

2 双目摄像头的标定与匹配

在无人机自动导航中必需的点云图基于深度图，在室内，自主机器人一般使用深度RGB-D 相机获得深度图；而在室外，双目相机没有尺度问题，安装在无人机上来计算深度图正合适。

2.1 相机标定

在使用摄像头之前，首先要进行相机标定[5]，在各种图像测量的过程中以及机器视觉应用中，为了重建和识别物体，需要确定空间物体表面任意点的三维几何位置与其在图像中的对应点之间的相互关系，进而建立相机成像的几何模型，而这些几何模型参数就是摄像头的相机参数。这些参数分别是相机的内、外参和畸变参数，摄像机内参反映的是摄像机从三维空间到二维图像的投影关系，即坐标系到图像坐标系之间的投影关系；摄像机外参决定了摄像机坐标系和世界坐标系之间相对位置关系，即其旋转R和平移T关系。

摄像机标定就是从世界坐标系转换为摄像机坐标系，再由摄像机坐标系转换为图像坐标系的过程，也就是求得最终投影矩阵的过程。

（1）从世界坐标系到摄像机坐标系：是三维点到三维点的转换，包括旋转R和平移T（相机外参）等参数。如图2所示。

图2 世界坐标系到摄像机坐标系的变换

（2）相机坐标系转换为图像坐标系：这是三维点到二维点的转换，包括焦距f（相机内参）等参数。如图3所示。

图3 相机坐标系到图像坐标系的变换

2.2 相机匹配

为了得到深度图需要使用立体匹配算法，利用了双目立体匹配原理。图4所示为双目图像立体匹配的具体流程。

图4 双目立体匹配原理流程

利用双目相机计算稠密的深度需要经过两个步骤，为图像矫正和视差计算，具体如下。

步骤1：图像矫正分为图像去畸变矫正和图像立体匹配[6]。图像畸变指的是图像边缘的直线由于相机的畸变导致在成像的时候变成了曲线。图像去畸变则是利用图像的畸变参数对图像畸变进行校正，检测方法是将原本畸变的图像在通过畸变校正后，变回直线，如图5 所示。这个也称作相机的内参数校准（焦距为fx、fy；相机中心为cx、cy），通常调用ROS 里面的包就可以实现了。

图5 图像去畸变

步骤2：立体畸变校正。立体匹配是指把左右图像旋转到同一个平面（这里说同一个平面不太准确，应该是两个平行的平面），使得左右两幅图片的光轴平行，检测方法是判断左右目图像中的同一个像素点是否在同一水平线上面，是立体视觉研究中的关键部分，其目标是在两个或多个视点中匹配相应像素点，计算视差。通过建立一个能量代价函数，对其最小化来估计像素点的视差，求得深度。

立体矫正是校准相机的外参数实现的，这个也可以通过ROS 下的相机校准包实现，只需要输入棋盘格的size 即可。校准的参数包括基线fx、相机的R矩阵、投影矩阵P矩阵。

一般来说epi＜0.2就已经可以进行建图了，epi＜0.15算是很好了，如图6所示。

图6 立体矫正

在获得相机的内参数和外参数以后，可以调用opencv的函数对双目的图像进行矫正，如下所示[7]：

本项目采用（9×7）的黑白格棋盘，如图8 所示。未标定前双目相机的左右视角画面如图7 所示，黑白格以及环境有明显的扭曲变形，且从棋盘可以看出左右视角画面不在同一平面上。标定校准之后的黑白格如图8所示。

图7 未标定前双目相机

图8 标定校准后双目摄像机

3 ORB-SLAM算法

ORB-SLAM 算法是一种基于ORB 特征的三维定位与地图构建算法（SLAM），具有地图初始化和回环检测的功能，并优化了关键帧选取和地图构建的方法[8]。

ORB-SLAM 共有3 个线程，分别是跟踪线程（Tracking）、地图构建线程（Local Mapping）和回环检测线程（Loop Closing）。

3.1 跟踪线程（Tracking）

跟踪线程运用ORB（Oriented Fast and Rotated Brief）算法对图像中的关键点快速创建特征向量。第一步是使用的是FAST 算法查找图像中的关键点，提取出ORB 特征；第二步是初始姿态估计，根据相邻帧之间的信息估计出相机的初始位姿，并通过全局重定位来初始化位姿；第三步是姿态优化，利用相邻帧寻找更多的特征匹配，优化当前帧的位姿；第四步是选取关键帧，通过筛选冗余关键帧来决定当前帧是否可以作为关键帧。

3.2 地图构建线程（Local Mapping）

地图构建线程ORB-SLAM 关键步骤，即建立点云地图。第一步是加入跟踪线程中筛选出的关键帧；第二步是验证最近加入的地图点（去除异常）；第三步是生成新的地图点，利用三角法生成新的地图云点，三角化的云点必须经过地图点云筛选测试，保证留下的点云都是能被跟踪的；第四步是局部束调整（Local Bundle Adjustment），添加待优化的位姿顶点，进行优化迭代，检测和排除误差较大的异常；第五步是验证关键帧，构建局部地图时检测并删除重复帧。

3.3 回环检测线程（Loop Closing）

回环检测可以解决累计漂移，校正整个轨迹的形状[9]。第一步是选取相似帧，在相似帧提取关键点和局部特征，判断是否先前已经构建过地图的位置；第二步是检测闭环，获得回环的累积误差；第三步是融合三维点，融合重复的点云，并且在Covisibility Graph 中插入新的边以连接闭环；第四步是图优化（传导变换矩阵），用本质图（Essential Graph）去优化位姿图，将闭环的误差分散到整个图中。

4 ORB-SLAM建图

4.1 稀疏点云地图

首先进行位姿的初始化和优化，通过提取图像中的特征点，根据相邻帧之间的信息，将其各自的特征点进行匹配，从而估计出相机的初始位姿，并通过全局重定位来初始化位姿。

得出相机的位姿后，可以通过计算得出视觉里程计。图10 中相机图像上的绿色小方块为当前提取的图像特征点，即ORB特征。图中的黑点和红点代表环境的稀疏地图（红色为局部地图点，表示当前路标；黑色为全局地图点，代表历史路标）。图中的绿线为相机的运动轨迹，绿色方框代表相机的实时位姿；蓝色方框为相机的历史位姿，代表相机运动过程中的空间位置（即关键帧）[10]。最后将生成的ORB-SLAM2 文件修改，基于ROS系统，将相机的位姿和视觉里程计生成为节点并发布，用于后面的稠密点云地图、八叉树地图及代价地图建图。

图10 实验室稀疏点云图

4.2 稠密点云地图

稠密点云地图的建图系统分为了3 个节点，如图11 所示。第一个节点作为驱动节点，负责采集图像的数据；第二个节点为姿态节点，主要运行ORB-SLAM 做姿态估计；第三个节点作为建图节点，是系统核心，收集第一和第二节点的建图节点并接收摄像头传回的数据和实时位姿，最后进行点云拼接。

图11 稠密点云地图建图步骤

4.3 八叉树地图

相较于各种点云图，八叉树地图更加灵活且易于压缩，并且还可以随时更新[11]。如图12 所示，每个立方体都会不断地均匀分成8 块，直到变成最小的方块为止，而整个最大的方块，可以看成是一个根节点，而最小的方块就是末端的“叶子节点”。于是在八叉树地图中，当从下一层的节点往上走时，这个地图的体积每经过一层就可以扩大8倍。如图12所示。基于八叉树地图的特性，八叉树地图有以下4个优点。

图12 八叉树地图

（1）八叉树地图可以描述所建地图中的任意位置的状态，可以用树中的节点来表示空间中的任意位置。

（2）同时，八叉树地图更易于更新和维护，他将各个位置的状态信息保存于节点，当需要修改这些位置的状态时，则只需要修改节点中的变量即可。

（3）八叉树地图还可以根据所需要得到地图的精度而随时调整。

（4）八叉树地图需要的内存相较于点云地图而言小很多，当对地图的精度要求不高时，可以用低分辨率的构建方式来节省空间。这更加有利于用在机器人的导航上。

5 路径规划

路径规划是双目SLAM 无人机能够实现自主导航的重要过程，主要功能是让无人机在地图内自主规划出一条从起点到终点的无碰撞的最优路径解。路径规划由两部分组成，分别是全局路径规划和局部路径规划。全局路径规划是当已经知道了起点、终点的位置和无人机所处的地图时，通过一系列算法计算出一条从起点到终点的最优路径解，更加适用于静态的环境；而局部路径规划则是依靠摄像头等传感器模块感知实时的信息，通过一系列算法实现对不断变化的障碍物和突发事件进行及时的避险，更具实时性和实用性，更加适用于动态的环境[12]。全局路径规划和局部路径规划这两种路径规划方法的组成了无人机完备的路径规划系统。

5.1 全局路径规划

传统使用的全局路径规划算法有4 种，分别是Dijkstra 算法、RRT 算法、BFS 算法和A*算法。Dijkstra 算法的特点是其支持的搜索地图面积较大，该算法从起点向外扩散，生成最优路径解，但是其缺点明显，开始建图之前会将整张地图全部搜索完，所以效率相较于其他3 种算法比较低，但仍然可以生成最优路径；RRT算法是一种基于树状结构的路径规划算法，从起点开始向外扩散出一个个的树状结构，但这种算法最大的缺点是无法对规划的结果进行预判，并且每次规划的结果可能都不一样；BFS 算法的特点是搜索空间小，但是效率高，不能保证找到一条最优路径解；A*算法在Dijkstra 算法的基础上加入了启发函数，用于引导其搜索方向；并且结合了BFS算法的优点，相较于其他路径规划算法，使用更加普遍，因此本项目中使用A*算法作为无人机的全局路径规划。

5.2 局部路径规划

当无人机在建立好了所需地图后，通过A*算法可以规划出一条大致可行的路线，但面对实际环境中各种突发的状况，例如突然出现了动态障碍或者建图时未被检测到的障碍，就需要通过局部的路径规划来进行路径的调整。于是本项目采用DWA 算法进行局部路径规划。DWA 算法会先建立机器人的速度运动模型，建立之后对机器人的速度进行采样，最后使用评估函数完成局部规划。

6 双目SLAM无人机优化与测试

6.1 系统测试环境

无人机以ROS 系统为载体，将ORB-SLAM 建图，定位避障，自主导航各种算法集成在一起。在ROS系统中，最小的进程单元就是节点，一个软件包里可以有多个可执行文件，可执行文件在运行之后就成了一个进程，这个进程在ROS中就叫做节点[13]。在无人机中建图、定位、循迹等就是不同的节点。ROS系统将节点声明为话题，然后以发布话题的形式将数据传输，最后使用不同的ROS通信机制实现数据的交互。本项目采用双目摄像头，以双目坐标系作为点云地图的基础坐标系，图13所示为本项目的双目SLAM无人机。

图13 双目SLAM无人机

6.2 双目摄像头的标定与使用

双目摄像头的标定与使用在第2节已叙述，图14所示为标定后的拍摄效果。

图14 摄像头标定后的拍摄效果

6.3 ORB-SLAM 地图创建

实验室稀疏点云地图构建如图15 所示。图中相机图像上的绿色小方块为当前提取的图像特征点，即ORB 特征。图中的黑点和红点代表环境的稀疏地图（红色为局部地图点，表示当前路标；黑色为全局地图点，代表历史路标）。绿色方框代表相机的实时位姿；蓝色方框为相机的历史位姿，代表相机运动过程中的空间位置（即关键帧）[10]。

图15 实验室稀疏点云地图

7 结束语

本文设计了一种基于双目视觉的SLAM 四旋翼无人机，可以在未知的环境下对周边的环境进行实时的地图构建，并且完成自主定位导航。并且由于无人机灵活机动的特性，可以不受限于地形，在许多传统SLAM 小车去不了的地方，也能进行建图。在设计的过程中通过各种渠道查阅关于ORB-SLAM、无人机的知识；对双目摄像头的标定匹配，以及矫正；通过使用树莓派将双目摄像头采集到的信息进行筛选处理后，转化成点云图再输出，同时树莓派还进行与飞控板之间的信息交流，实时控制无人机位移；将ORB-SLAM 算法融入到ROS 系统与建图当中，最终实现实时点云地图的建立。