基于单目视觉的四轴飞行器体素地图重建系统的研究

赵艺博 霍冬浩 陈彦钦 李天城 朱晓佳

（中国民航大学 天津市 300300）

同时定位与地图构建(simultaneous localization and mapping, SLAM)最早源于机器人领域, 其目标是在一个未知的环境中实时重建环境的三维结构并同时对机器人自身进行定位，并且根据标记点建图。从SLAM 系统的角度看，“建图”是服务于“定位”的；但从应用层看，“建图”有着许多其他的应用，这些应用大致可以划归为：定位、导航、避障、重建、交互。近年来，随着自动驾驶技术的兴起以及社会对有自动导航、轨迹规划等功能的智能设备的需求越来越高，SLAM 问题得到了越来越多的关注，也使得SLAM 技术得到快速的发展。

SLAM 技术由Smith Self 和Cheeseman 于1986 年首次提出，距今为止已经发展了30 多年。同时无人机也是机器人技术应用的重要平台之一，因此无人机视觉SLAM 也随之受到了大量的关注。由于无人机自身可以携带不同种类的传感器，大量工作者在无人机SLAM 领域做出了优异的成果，例如可以适用于装备有单目、双目或者深度相机的无人机的ORB-SLAM3算法，专为多旋翼系统设计来实现无人机实时建图的框架OpenREALM，可以用于单目 SLAM无人机图像拼接的Map2DFusion 算法，可以用于无人机导航的SLAM 框架openvslam，利用了激光传感器进行稠密重建的RTAB-Map。然而，大多数无人机的应用多集中于双目、深度或是添加激光等其他传感器的环境。这是由于单目摄像头不带有深度信息，无法直接应用于导航等问题。本文就单目摄像头添加深度信息，在ORB-SLAM2[7]上创新融合，实现了基于单目视觉的四轴飞行器体素地图重建，可用于下一步的路径规划。

1 ORB-SLAM2算法的选择

比较各种SLAM 优势与特点，例如基于单目、双目的ORB-SLAM 系列等算法，融合深度学习的TANDEM、MonoRec等 算 法， 基 于 激 光 雷 达 的range-mcl、MULLS等SLAM 算法，亦或者是多传感器融合的R3LIVE等算法。可以看出，基于单目的SLAM 算法的开发成本是很低的，即便是考虑应用层面上的硬件设施，一个携带单目摄像头的小型四轴无人机相较于其他携带各种传感器的设备也是一种成本极低的工具。

ORB-SLAM2算法主要由三个线程组成：跟踪(Tracking)、建图(Local Mapping)、闭环检测(Loop Closing)。如图1 所示，在图片帧传入后，首先跟踪图像，从中提取ORB 特征，并进行初始化位姿，优化位姿，确定关键帧；之后进行关键帧的插入，验证筛选出地图点，优化并完成局部地图构建；最后闭环检测进行校正。

图1： ORB-SLAM2 流程

2 基于ORB-SLAM2算法的稠密重建与保存

2.1 世界坐标的获取

稠密重建需要一种以.pcd 格式存储的点云图，点云图主要包括了点云数量以及点云的世界坐标。因此需要获取图像中空间点的世界坐标。

2.1.1 关键帧图像的保存

在实现稠密建图之前需要图像帧与对应的相机位姿。可以通过修改ORB-SLAM2 算法代码对关键帧进行保存。修改ORB-SLAM2 的源码System.cc 来保存关键帧图片，通过接口获取关键帧对应的位姿并保存为文本文件即可。

2.1.2 世界坐标的计算

在针孔相机模型中，我们可以得到公式：

其中K 为相机内参，R、t 叫做相机的外参，P为世界坐标，P为像素坐标，Z 原本为世界坐标的深度信息。

但是在单目摄像头上显然无法得到深度信息，所以需要在公式中做归一化处理，将Z 消除掉，这样得到像素坐标后，通过计算就可以得到世界坐标下的X、Y 值。但是丢失了深度的地图是无法进行导航与路径规划的。因此需要做稠密重建来建立具有导航功能的地图，通过在重建中将深度信息计算并加入，进行稠密建图。

2.2 单目稠密重建

一幅图像的像素只能提供物体与成像平面的角度以及物体的亮度，无法提供距离。因此需要通过立体视觉的方式来获得距离。本文对于深度估计分为以下四个步骤：

Step1 假设所有像素的深度满足某个初始的高斯分布；Step2 读入当前帧，通过极线搜索和块匹配确定投影点位置；

Step3 根据几何关系计算三角化后的深度及不确定性；Step4 将当前观测融合进行上一次的估计中。若收敛则停止计算，否则返回Step2。

2.2.1 极线匹配

在稠密图深度估计中，是无法直接将所有特征点拿来直接做匹配的，所以如何确定第一幅图的某像素在其他图的位置是一个需要解决的问题。本文采取极线搜索和块匹配的方法。

如图2 所示，左边相机观测到了某一个像素p，由无人机单目相机观测所得。根据相机模型，该地图点位于从O出发的射线上的某个区间之内。从右侧相机视角来看，这条射线的在成像平面上的投影叫做极线。显然，当已知两部相机的相对运动时，极线是可以确定的。 接下来，只需要确定p 对应的是极线上的哪个点即可。而因为单个像素的亮度没有区分性，故选择比较像素块。在p 周围取一个w×w 的小块，然后在极线上也取很多相同大小的小块进行比较，即块匹配。

图2： 极线匹配

其中 表示图像块中像素点 的灰度值，通过去均值可有效改善图像块由于光照变亮或变暗导致的相似性降低。相关性越高，则图像块间相似度越高，取极线上相关性最高的搜索点作为匹配点。极线匹配算法具体过程如表1 所示。

表1：

2.2.2 三角测量

在得到两帧上的一对匹配点P，P后，可以通过三角测量来计算空间点的深度。如图3 所示，根据其观测角度及平移距离可构建三角并相交于空间点P，但由于噪声的影响通常会存在些许偏差。设空间点P 在两帧相机坐标系下的深度分别为d，d，其归一化图像平面坐标分别为P，P，则根据位姿变化可得：

图3： 三角测量

3 地图重建

3.1 八叉树地图

在导航问题中，对地图建模一般采用一种本身有较好的压缩性能的地图形式：八叉树地图，它是一种体素地图。如图4 所示，如果把一个小方块的每个面平均切成两块，那么这个小方块就会变成同样大小的八个方块，直到最后的方块大小达到建模的最高精度。这个从最大空间细分到最小空间的过程，就是一颗八叉树。在八叉树中，当我们由下一层节点往上走一层时，地图就能扩大为原来的八倍。从存储层面来讲，可以用0 表示空白，1 表示被占据。这种0-1 的表示可以用一个比特来存储，节省空间。

图4： 八叉树地图

3.2 地图可视化

通过安装octomap 库，可实现pcd 点云图转化为八叉树地图，再利用rvis 可视化工具可观察生成的八叉树地图。建立地图的流程如图5 所示。

图5： 建图流程

如图6 所示，场景1、2 为使用特洛无人机携带的单目相机所拍摄的数据集场景以及使用重建系统转化后的体素地图。场景1中相机位于场景前方，场景2中相机位于场景上方。

图6： 转化出的体素地图

4 结束语

本文对基于单目相机的ORB-SLAM 算法进行了改进，使得无人机通过单目相机能够建立可用于导航的八叉树地图。但是仍然存在不足之处，建立的体素地图细节不够丰富，且存在深度错误估计的问题。并且只对单一场景进行重建是不足以应用于导航的。下一步，笔者将深入学习视觉 SLAM框架以及点云库，寻找更加合适的深度估计方法，并研究如何建立维护全局地图。在保证低成本传感器的同时，尽可能建立高质量的地图，提高单目ORB-SLAM2 建图算法的实用性。