基于飞行器平台的室内地图构建

分类号：单位代码： 申 请 号： 本科学位论文 中文论文题目中文论文题目:? 英文论文题目英文论文题目:Indoor space mapping based on aircraft platform 申请人姓名：? 指导教师：? ：专业名称：? 研究方向：? 所在学院：? 论文提交日期论文提交日期 ? 近年来，飞行器自主导航中的同步定位与地图构建（SLAM，Simultaneous Localization and Mapping）成为了研究的热点。由于其重要的理论与应用价值, 被很多学者认为是实现 全自主移动机器人的关键。SLAM 算法目前正逐步向飞行器领域拓展，对引导无人机自主 探索未知环境具有极其重要的意义。 本文首先对机器人 SLAM 问题进行了概述，给出了 SLAM 问题的概率模型，介绍了 基于滤波的方法和基于图优化的方法。然后本文对图优化的方法进行了详细的描述。本文 介绍了基于图优化的 SLAM 框架，对图优化问题进行建模，使用了一种基于最小二乘的优 化算法解决图优化 SLAM 问题。然后本文使用公开的数据集对这一优化算法进行测试。 在算法测试完成之后，本文将该算法做了实际的应用。本文前端借助单目视觉方法， 后端使用基于图的优化方法，对室内环境进行地图构建。首先使用手持摄像头对桌面环境 进行了实时地图构建。然后搭建飞行器平台，使用飞行器自带的前置摄像头作为传感器， 通过 wifi 将图像信息会传到地面站，地面站作为处理单元，对飞行器所处环境进行地图构 建。 关键词： SLAM，图优化，飞行器地图构建 i Abstract Recent years, Simultaneous Localization and Mapping (SLAM) has become a research hot spot. Due to its important application value, SLAM is considered as a major point of autonomous mobile robot. SLAM algorithm is gradually developing in the field of aircraft, which has signifi- cance to guide in UAV autonomously exploring in unknown environment. First, this paper gives a brief review of SLAM, forms up a probability model, introduces filter- based method and graph-based method. Second, this paper describes the graph-based SLAM in detail. We introduce the framework of the graph-based SLAM, model the optimization problem, and use the least squares algorithm to solve this problem. Third this paper uses public data sets to test this optimization algorithm. After the algorithm test is completed, this paper applies this algorithm in real environment. We map the indoor environment using monocular vision method to process data, and graph-based method to optimize the map. At first, we hold the camera in hand, and build the real-time map of the desktop environment. Then we construct an aircraft platform, using aircrafts front-facing camera as main sensor to transmits image data to the ground station which builds the real-time map of the environment. Keywords:SLAM, Graph-based optimization, Aircraft map-building iii 目?目? ? .i Abstractiii 目? 1?论 .1 1.1?题?1 1.2SLAM ?2 1.3?文?3 2SLAM ?题? .5 2.1SLAM ?题?5 2.2SLAM ?题?6 3? SLAM ? 9 3.1? SLAM ?.9 3.2? SLAM ?. 10 3.3? 12 3.4?题? 13 3.5? 14 3.6? 19 4? SLAM ? 23 4.1? 23 4.2? 24 4.3? 29 4.4? 29 5? . 37 5.1? 37 5.2? 37 ?文? . 39 v 1?论?论 1.1?题?题? 由于体积小、机动性能好，空中机器人成为了大量室内和室外任务的理想平台。尤其 在不适宜地面机器人行走的复杂环境中，空中机器人的优势更加明显。在这些任务中，空 中机器人自主飞行的能力至关重要。由于 GPS 技术，我们看到了很多成功的空中飞行器的 户外应用，如航拍、运输、侦查、搜救等。但在缺少 GPS 信号的室内环境中，仅依靠自身 惯性测量系统，飞行器无法自身定位更无法估计周围环境。因此，为实现空中机器人在缺 少 GPS 信号的未知环境中的自主导航，我们需要飞行器对自身状态信息和环境的感知，建 立周围环境的模型。 在未知环境中，移动机器人为了能够探索未知区域并顺利完成给定的任务，需要具有 自主导航能力。图 1.1 描述了基于地图的移动机器人导航结构，其中定位（Localization）是 指机器人确定自身在工作环境中的精确位姿；地图构建（Mapping）是指建立机器人所工 作环境的各种物体，如障碍、路标等的准确的空间位置描述，即环境地图。如图 1.1 所示， 移动机器人精确的定位以及环境地图创建是实现机器人自主导航的前提和基础。 图 1.1基于地图的机器人导航结构 移动机器人的精确定位是基于对环境的感知和理解，需要创建环境地图，而环境地 图的创建又需要精确的定位。因此，移动机器人的定位和地图创建是一个既矛盾又相 1 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 关的过程。为了克服这个问题，一些研究者提出了移动机器人的同步定位与地图构建1 （SLAM，Simultaneous Localization and Mapping） 。由于其重要的理论与应用价值, 被很多学 者认为是实现真正全自主移动机器人的关键。SLAM 问题，最早是由 Randall Smith 和 Peter Cheseseman 在 1988 年发表的论文1当中提出来。在他们的论文中，利用移动机器人的运 动方式和装备的传感器获得的测量数据，分别设计了移动机器人的运动模型和观测模型， 结合概率学的贝叶斯理论，实现了对轮式移动机器人在未知环境中的运动状态进行实时估 计。所有移动机器人导航的基本过程可以总结为三个最基本的关键问题，即“Where am I now?” 、 “What is the structure of my environment? ”以及“How can I get that target position?” 这三个问题的实质指的就是机器人定位和地图创建， 以及机器人导航路径规划问题。 SLAM 问题的解决直接影响后面路径规划的问题能否正确处理，所以说，移动机器人的同步定位 与地图构建是实现机器人自主导航，提高机器人的智能化水平的关键和首要解决的基础性 难题。 SLAM 算法目前正逐步向飞行器领域拓展，对引导无人机自主探索未知环境具有极其 重要的意义。 1.2SLAM ? 移动机器人 SLAM 问题的研究始于上世纪八十年代中期。在 1986 年的 ICRA（Interna- tional Conference of Robotics and Automation）会议上，Peter Cheeseman, Jim Crowley, Hugh Durrant-Whyte2提出了利用贝叶斯估计理论完成移动机器人状态和地图创建同时估计方 法的可能性。 1988 年，Smith,Self 和 Cheeseman1将 SLAM 问题系统地表示为随机估计问题，用一 个通用的状态向量以及该向量的协方差矩阵表示环境路标位置和机器人位置。该论文奠定 了移动机器人 SLAM 问题以后研究实现的总体方法。1995 年，Durrant-Whyte3首次提出 解决 SLAM 问题的一种系统框架，并证明 SLAM 最终会收敛。文章同时指出：一旦将机 器人状态与路标位置放入全状态估计器中，最终状态是收敛的，路标间多次观测的关联性 越大，SLAM 将获得更好的收敛性能。然而，在 SLAM 问题中，由于全状态估计使得被估 状态维数随着路标数成平方增长，因而必须面对计算效率低的问题，这也就极大地限制了 SLAM 技术的发展和应用。 本世纪初，研究者对 SLAM 问题的兴趣空前高涨，主要致力于如何有效的减少计算 量，研究更高效的算法以及扩大 SLAM 技术的应用环境。目前降低 SLAM 计算复杂度的方 法主要集中于减少次要特征、改进地图表示方法以及更高效的算法等方面4。在当前研究 2 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 领域，一般将 SLAM 问题划分为在线 SLAM 问题（on-line SLAM problem）和全局 SLAM 问题（full SLAM problem） 。对于在线 SLAM 问题，待估计状态量由机器人当前位姿以及 构成地图的各路标位置两部分组成。解决这类问题比较典型的做法就是采用滤波技术对状 态量进行迭代估计，每当机器人获取到新的观测信息时，在线 SLAM 算法将对状态量进行 更新。在线 SLAM 问题的相关研究中，比较有代表性的方法包括基于卡尔曼滤波器的方 法5，基于信息滤波器的方法6以及基于粒子滤波器的方法7。这些方法均以增量式的形 式进行计算与更新，因此具备在线计算的特点。 与在线 SLAM 问题相对应，全局 SLAM 问题要求对机器人整个导航任务期间各个时 刻的位姿以及各路标的位置进行估计，这类问题在计算过程中主要包括数据关联（Data Association）和优化（Optimization）两个过程。其中数据关联过程负责将机器人状态，路 标以及里程计信息整理成因子图的形式。因子图一般包含两类节点，它们分别表示机器人 各个时刻的位姿状态或者路标的位置状态；因子图中的边则表示由其相连节点对应时刻传 感器测量信息匹配生成的空间约束条件。由于传感器的测量过程中包含噪声，因子图中对 应的空间约束往往同节点之间的相对空间关系不一致的。而 SLAM 中优化中优化过程则正 是针对这种“不一致” ，借助优化的方法，重新配置图中节点位置，使上述“不一致”最小 化。由于全局 SLAM 问题通常可利用因子图模型进行描述，因此一般也将该类 SLAM 问 题成为基于图的 SLAM 问题。在基于图的 SLAM 问题中，最具代表性的为 g2o（General Framework for Graph Optimiazion） 8。g2o 是一个用于处理图优化问题的开源框架，它通过 非线性最小二乘优化和稀疏线性代数，实现了对基于图的 SLAM 问题的高效求解。 在过去，由于基于图的方法需要解决多元变量非线性最小二乘优化问题，所以被认为 复杂度高，计算量大，太耗费时间，无法满足 SLAM 的实时性要求。但随着计算机计算能 力的增强以及高效求解方法的出现，基于图优化的方法在近几年重新得到重视。目前，该 算法的应用较少。本文尝试使用 11 年德国弗莱堡大学自主智能实验室提出了 g2o 方法，做 一次飞行器平台的实际应用。 1.3?文?文? 本论文围绕室内环境下飞行器的 SLAM 问题展开，选择合适的飞行器，利用单目摄像 机作为移动机器人的外传感器，提取室内深度信息及视觉里程计数据，应用基于图优化的 算法，搭建一套完整的能够完成室内地图构建的飞行器平台。 具体内容安排如下： 第一章首先介绍本论文的研究背景及意义，接着介绍移动机器人 SLAM 问题的研究进 3 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 展，最后给出本论文的研究内容与结构。 第二章对 SLAM 问题进行了分析，介绍了 SLAM 问题的一般模型和基本原理。 第三章分析了基于图的 SLAM 算法，详细介绍了该算法的思想与实现方法，并进行仿 真实验。 第四章搭建飞行器平台，并利用单目摄像机作为传感器，使用基于图的 SLAM 算法， 构建室内环境地图。 第五章为总结与展望。首先对本文所作的工作进行总结，然后对后继工作的研究内容 进行展望。 4 2SLAM ?题?题? 2.1SLAM ?题?题? 同步定位和地图构建（Simultaneous Localization and Mapping, SLAM）, 其基本思想是 自主机器人在全然未知的环境中根据其自身的定位信息对地图进行增量式的更新，并使用 已创建的地图进行自我定位。图 2.1 为同步定位和地图构建的过程示意图。 图 2.1同步定位和地图构建过程示意图 SLAM 问题可以定义为一个最大后验概率估计。记地图为 m,t 时刻机器人的位姿为 xt= (xt,yt,t)T, 观测信息为 zt, 控制信息为 ut, 记 Xt= x0,x1,.,xt, Zt= z0,z1,.,zt, Ut1= u0,u1,.,ut1. 在实际问题中，机器人根据观测信息及控制信息，求位姿信息的后验概率分布，即 Xt, Ep(Xt|Zt,Ut1,m), 通常简化为 xt, Ep(xt|Zt,Ut1,m) 根据贝叶斯估计理论，有： p(xt|Zt,Ut1,m) = p(zt|xt,Zt1,Ut1,m)p(xt|Zt1,Ut1,m)(2.1) 5 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 又因为 SLAM 问题符合马尔科夫性质，及当一个随机过程在给定现在状态及所有过去状态 情况下，其未来状态的条件概率分布仅依赖于当前状态，可得： p(xt|Zt,Ut1,m) = p(zt|xt,m)p(xt|Zt1,Ut1,m)(2.2) 将（2.2）式中 p(xt|Zt1,Ut1,m) 用全概率公式展开，可得： p(xt|Zt,Ut1,m) = p(zt|xt,m) p(xt,xt1|Zt1,Ut1,m)dxt1(2.3) 对（2.3）式再用一次贝叶斯公式和马尔科夫性质简化，可得： p(xt|Zt,Ut1,m) = p(zt|xt,m) p(xt|xt1,ut1,m)p(xt1|Zt1,Ut1,m)dxt1(2.4) 其中 p(zt|xt,m) 为观测模型，p(xt|xt1,ut1,m) 为运动模型。 这就是 SLAM 问题的一般模型。 2.2SLAM ?题?题? 2.2.1? 基于滤波器的方法主要是利用了递归贝叶斯估计原理，在假定从 0 到 t 时刻的观测信 息以及控制信息已知的条件下，对系统状态（包括机器人当前位姿以及所有地图特征位置） 的后验概率进行估计。根据后验概率表示方式的不同，存在多种基于滤波器的方法，常用 的包括扩展卡尔曼滤波（EKF） 5、扩展信息滤波（EIF）6、以及粒子滤波（PF）7 方法等。 为强调其增量式特性，基于滤波的 SLAM 方法通常被称为在线 SLAM（on-line SLAM） 。 2.2.2? 与滤波方法中每一步只考虑机器人当前位姿不同，基于平滑的方法通过所有的观测信 息估计机器人完整的运动轨迹和地图，因而也被称为完全 SLAM 方法（full SLAM） 。由于 地图特征可以通过边缘化方法转化为位姿间的约束，从而简化为位姿序列的估计。这类方 法可以用图的方式作直观描述，所得的图被称为位姿图（pose graph） 。图中的节点对应机 器人在不同时刻的位置和姿态，而边则描述了位姿与位姿间的空间约束关系。这种约束可 以通过里程计或观测信息的配准得到。在图构造好后，对图中节点所处的位姿进行优化， 使其最好地满足边所表示的约束关系，优化的结果即对应机器人的运动轨迹。由于平滑方 法的这种直观特性，又被称为基于图优化的方法。过去，基于图优化的方法被认为太耗费 6 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 时间，无法满足 SLAM 的实时性要求，但随着高效求解方法的出现，基于图优化的方法重 新得到重视，在经历一个复兴后，其成为当前 SLAM 研究的一个热点。 在第三章，我们将详细介绍一种最具代表性的基于图优化的方法，g2o（General Framework for Graph Optimization） 8。 7 3? SLAM ? 3.1? SLAM ? 基于图优化的 SLAM 方法由 Lu 与 Milios9首先提出。通过保留所有的观测数据帧以 及帧间的空间约束关系，并将约束看作是随机观测，然后采用最大似然方法估计机器人 的位姿。这种思想可以通过图的方式形象地表现出来。用图中的节点表示待求解的机器 人位姿，用节点间的边来描述位姿间的空间约束关系，则对位姿序列的估计可转化为图 的优化问题，即通过调整图中的节点的位置使其最好地满足边的约束关系。Gutmannn 和 Konolige10在 Lu 与 Milios 的基础上提出了高效的闭环检测及图构建方法，从而形成了基 于图优化的增量式 SLAM 算法框架，其主要包括顺序配准、环形闭合检测以及图优化 3 大 部分，它们之间的关系可以通过下图来表示。 图 3.1基于图优化的 SLAM 框架 顺序配准主要考虑连续数据帧间的匹配及相对姿态估计问题，而环形闭合检测则主要 根据观测数据判断机器人是否处在之前已访问区域。这两部分都与观测数据密切相关。从 数据处理的观点来看，两者都是要解决数据关联问题，前者考虑局部数据关系，而后者则 处理全局数据关系。从基于图的表示上看，顺序配准与环形闭合检测都是根据观测信息建 立图节点间的约束，即完成图的构建。两者一起被称为 SLAM 前端（SLAM front-end） 。 由于观测噪声以及配准误差的存在，通过观测信息配准得到的位姿图往往不具备一致 性。图中的边与边之间常常存在“冲突” 。举例来说，若用 Ti来表示数据帧间的相对变换 9 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 矩阵而且 T0,T1,.,Tn构成一个闭环的话，从理论上讲，必然存在 T0T1.Tn= I, 其中 I 为 单位矩阵。但通过观测信息配准得到的相对变换矩阵通常不满足该理论的约束。在基于图 的表示中，机器人的位姿被看作是随机变量，位姿间的约束则是与随机变量相关的观测， 那么图优化结果则对应于位姿的最大似然估计。与顺序配准及环形闭合检测不同，图优化 部分一般不直接处理观测数据，而是对 SLAM 前端构造的图进行优化，因而，图优化通常 又被称为 SLAM 后端（SLAM back-end）, 与 SLAM 前端相区分。 3.2? SLAM ? 在基于图的 SLAM 中，机器人的位姿被图中的节点描述。观测值或者里程计测量值引 起的位姿之间的空间约束通过节点间的线段表示。更细节的是，一个基于图的 SLAM 算法 构建了一个不含原始传感器测量值的图。图中的每一个节点代表一个机器人的位姿。节点 间的线代表两个机器人位姿间的空间约束。一个约束包含两点间相对转移的可能性分布。 如下图所示： 图 3.2节点图 图中的节点表示机器人在某个时刻的位姿。邻近的节点通过边相连，这些边是由于测 量值产生的两个位姿之间的空间约束。两个连续节点之间的边 et1,t来自于里程计信息， 其它的边是由各种传感器对相同环境观察而产生的空间约束。 一旦图被构建， 我们去寻找最能满足约束的机器人位姿的结构。 因此， 基于图的 SLAM 10 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 问题解耦为两个任务：从原始测量值中构建图（图构建） ，通过图中的线段，决定最可能的 位置结构（图优化） 。 图 3.3 取参数向量 x = (x1,.,xt)T, 这里 xi代表机器人在 i 时刻的位姿。zij和 ij分别作为 节点 i 和节点 j 间测量值的均值和测量值的信息矩阵。 zij(xi,xj) 作为节点 zi和 xj之间的 期望测量值。所以，测量值 zij的对数似然函数 Iij满足以下形式 Iij zij zij(xi,xj)Tijzij zij(xi,xj)(3.1) 取 e(xi,xj) 作为期望的观测值 zij和机器人真实的观测值 zij的差。为了简化记号，我 们将这种测量值的标号写成误差函数 e(xi,xj) = zij zij(3.2) 最大似然方法的目标是去找到节点 x的配置，使得所有观测值的负 log 对数似然函数 F（x）最小 F(x) = (i,j)C eT ijijeij (3.3) 即解决以下方程： x= argmin x F(x)(3.4) 接下来，我们可以采用 Gauss-Newton 或者 Levenberg-Marquardt 算法11处理这种非线 性最小二乘优化问题。 11 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 3.3? 如果对机器人的位姿有一个好的初始位置的估计 x，那么能用 Gauss-Newton 或者 Levenberg-Marquardt 算法11求得方程（3.4）的数值解（该方法来源于参考文献 8）主要 思想就是在初始估计附近通过一阶泰勒级数展开近似误差函数 ek= ( xk+ xk) = ek( x + x) = ek+ Jkx (3.5) 这里 Jk是 ek(x) 在 x 处的雅克比矩阵，把方程（3.5）代入方程（3.3） ，得到 Fk( x + x) = ek( x + x)Tkek( x + x) (ek+ Jkx)Tk(ek+ Jkx) = eT kkek+ 2e T kkJkx + x TJT kkJkx = ck+ 2bkx + xTHkx (3.6) 这样，方程 F(x) 可以写为 F( x + x) = (k)C Fk( x + x) kC ck+ 2bkx + xTHkx = c + 2bTx + xTHx (3.7) 这里，记 c = ck,b = bk,H = Hk. 为了使目标函数最小，通过求导，可知 x 满足 Hx= b(3.8) 通过解线性方程组，我们可以得到 x接下来，我们把 x加到初始估计值中，对初 始估计值进行更新 x= x + x(3.9) 接下来进行迭代，重新得到公式（3.7）的线性化结果，求解公式（3.8） ，更新公式 （3.9） 。在每次迭代中，上一次结果被当做新的初始估计。 以上过程是一个通用的多元函数最小化的求解方法。然而，这种方法假设参数 x 处于 欧氏空间。对于 SLAM 问题，机器人的位置通常包括坐标和姿态，所以位姿 x 处于非欧氏 空间。对此，我们引入流形12的方法去求解此问题。 12 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 为了解决参数处于非欧氏空间问题，通常的做法是把误差最小化问题放在流形中解决。 流形是一个数学空间，在全局范围内流形不一定是欧氏的，但是在局部可以被看做欧氏13。 在 SLAM 问题中，每一个位姿 xi都包含一个坐标向量 ti和一个旋转向量 i, 坐标向量 显然属于欧氏空间，而旋转向量 i存在于非欧氏空间中。为了避免奇异，这些空间用旋转 矩阵或者四元数表示。在流形空间中，定义一个操作符号 ?，用来处理非欧氏空间中的运 算 ek(xk) def. = ek( xk? xk) ek+ Jkx (3.10) 这里 x 是在初始位置 x 附近的微小增量。雅克比矩阵 Jk为 Jk= ek( x ? x) x |x=0(3.11) 同样，经过线性化后，我们能得到公式（3.8）同样的结果, 求导后得到 Hx= b(3.12) 相应的，对初始估计进行更新 x= x ? x(3.13) 如此迭代下去，直至满意的结果。 3.4?题?题? 一开始，本文对 30 个节点进行图优化处理，采用高斯牛顿方法迭代，时间复杂度为 O(n3), 计算机每次迭代的时间在 300ms 左右，可以接受。但当本文使用公开数据集测试算 法时，该官方数据集有 934 个节点，计算量爆炸，计算机过了相当长的时间都没有给出计 算结果。 对此，本文想到了对整图分解，局部优化的方法。首先将图中的每 100 个连续节点分 为一个子图，然后对子图中的这 100 个节点进行一次图优化，将每一个子图优化的结果拼 接起来作为最后的优化结果。虽然该思想不能保证严格意义上的全局优化，但是能够使计 算量显著下降，保证实时性。对这 934 个节点分为 10 组，最后每次迭代的时间在 500ms 左 右。优化效果在下一节中展示。 13 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 3.5? 这里，我们以一个 2D SLAM 问题，并使用 Intel Research Lab、ManhattanOlson 数据集 为例，展开这一仿真过程。 3.5.1? 图 3.42D SLAM 节点图 图 3.4 描述了一个 SLAM 图。机器人的位姿用节点 xs t 表示，路标用节点 xl i 表示。我 们假设路标只有位置信息而没有方向信息，所以路标属于欧氏空间。机器人的位置既有 x y 坐标，又包含旋角度 , 所以位姿属于 SE(2) 空间。机器人在某一位时刻对路标的测 量值用 zl 表示，连续节点间的里程计测量值用 z s 表示。 3.5.2? ? 在连续的两个机器人位姿节点 xs t 和 xs t+1中，存在的约束为里程计测量值 z s t,t+1。因为 传感器误差的存在，所以里程计测量值往往与真值有差距。我们假设噪声是高斯白噪声， 那么节点 xs t 和 xs t+1之间的边可以用以下两个参数描述 14 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 1. zs t,t+1 SE(2) 代表连续节点间里程测量值 2. s t,t+1 33代表里程计测量值的协方差矩阵 ? 在机器人位姿节点 xs t 与路标 xl i间也存在一种约束，这种约束来自观测传感器 z l t,i。同 样，由于传感器存在误差，测量值与真值之间存在偏差。同样假设这种噪声是高斯白噪声， 那么节点 xs t 与路标 xl i间的约束可以用以下两个参数描述 1. zl t,i 2代表在 xs t 对路标 xl i的观测值 2. l t,i 22代表测量值的协方差矩阵 3.5.3? 构建误差函数常用的方法是根据初始估计的 xk，预测期望的观测值 zk的，然后误差 函数即为真实观测值 zk与预测观测值 zk的差。对于机器人节点 xs t 与路标 xl i 之间约束误 差，首先预测一个期望的测量值 hl t,i(x s t,x l i)，期望的测量值是指从机器人节点 x s t 看路标 xl i 存在的位置，所以， zt,i(xs t,x l i) 形式如下 zt,i(xs t,x l i) = (xi xs t)cos s t + (yi ys t)sin s t (xi xs t)sin s t + (yi ys t)cos s t (3.14) 误差为真实的测量值与预测的测量值之差，那么，节点与路标之间的误差函数如下 el t,i(x s t,x l i) = zt,i zt,i(x s t,x l i) (3.15) 同样，我们可以定义机器人连续节点 xs t 和 xs t+1 的里程计约束误差。首先预测一个期 望的里程计数值， zt,t+1(xs t.x s t+1) = x s t+1 x s t (3.16) 误差为真实的里程计测量值与预测的里程计测量值之差，那么连续节点间的误差函数 如下 el t,t+1(x s t,x s t+1) = zt,t+1 zt,t+1(x s t.x s t+1) (3.17) 有了 (3.15) 和（3.17）两个误差函数，我们可以利用公式（3.7） （3.8） （3.9）迭代求解。 15 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 3.5.4? 下面我们以 Intel Research Lab 公开数据集做一个数据处理 图 3.5Intel Research Lab 该数据集中共有 934 个节点，2780 条约束，其中 934 条约束来自于里程计信息，即连 续机器人位姿间的约束；1837 条约束来自于观测器信息，即非连续机器人位姿间的约束。 最开始，我们将里程计信息作为初始估计 x, 得到一张初始环境地图，如图 3.6 所示， 初始地图大致满足实验室环境，但中间走廊处效果较差。 图 3.6初始环境地图 16 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 然后我们通过误差最小二乘的方法对以上数据进行迭代优化，得到以下的结果 图 3.7一次迭代后的环境地图 图 3.8十次迭代后的环境地图 图 3.7 图 3.8 分别是一次迭代和十次迭代后的结果。与初始估计对比可知，优化后的 地图结构清晰，特别是地图中间的走廊，经过优化后更加清晰明显。由于里程计信息误差 17 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 不大，所以收敛速度较快，一次迭代后就逼近最优结果。 3.5.5? 我们使用 ManhattanOlson 数据集，该数据集中共有 3500 个节点，9098 条约束，其中 3500 条约束来自于里程计信息，即连续机器人位姿间的约束；5598 条约束来自于观测器 信息，即非连续机器人位姿间的约束。 最开始，我们将里程计信息作为初始估计 x, 得到一张初始环境地图，如图 3.9 所示， 地图右下角区域里程计误差较大，地图较为杂乱。 图 3.9初始环境地图 然后，我们通过误差最小二乘的方法对以上数据进行迭代优化 图 3.10一次迭代后的环境地图 18 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 图 3.11十次迭代后的环境地图 图 3.10 图 3.11 分别是一次迭代和十次迭代后的结果。与初始估计对比可知，优化后 的地图结构清晰，特别是地图右下角较为复杂的地区，经过优化后结构更加清晰明显。 通过 Intel Research Lab 数据和 ManhattanOlson 数据的仿真，可以看出基于最小二乘的 图优化方法具备较好的效果。 3.6? 这里，我们将图优化算法与光流法得到的二维地图进行一个比较。 光流法为实验室其他同学在做的项目，光流法基本原理为分析连续两帧图片光强度的 变化来判断两帧图片的运动，进而推算出摄像机的运动情况，从而生成摄像机的运动轨迹。 我们控制飞行器做一个规则的矩形运动，如图 3.12 所示。 图 3.12飞行器运动示意图 19 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 图 3.13 为光流法得到的机器人位姿信息，由于光流法存在测量误差，所有最终结果有 一定的累积偏差。 图 3.13光流法结果 接下来，我们要使用图优化方法对同样的信息进行处理。首先要构建一张节点图，初 始节点的位置我们使用光流法得到的结果，接下来我们要计算节点间的约束信息，这需要 一些图像处理的技术。为了方便图像处理，我们事先在地板上放置了一些绿色的方块作为 路标，如图 3.14 中的绿色方块所示。通过 HSV 色域空间，我们很容易在图像中识别出这 些路标。如果我们在两帧图片中发现了相同的路标，那么我们就很容易获得这两帧图片所 在位置的空间约束。以图 3.14 为例，假定图像的中心点为飞行器的位置，在右图中，飞行 器距绿色路标为 a1, 在左图中，飞行器距绿色路标为 a2, 由于路标位置固定，那么我们很容 易得到这两个位姿间的约束为 a2 a1。 图 3.14通过同一路标计算位置关系 通过以上图像处理的方法，我们能够得到不同位姿间的约束关系，即节点图中的边， 这样我们可以使用图优化来对所有位姿进行优化，优化结果如图 3.15 所示。 20 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 图 3.15图优化结果 通过图 3.13 与图 3.15 对比可以看出，图优化得到的结果更加符合实际情况，图优化 方法具有良好的效果。 21 4? SLAM ? 4.1? 一个完整的图优化 SLAM 问题分为前端和后端，前端负责处理传感器数据，得到 初始的位姿估计及约束，后端对位姿进行图优化。这里，我们前端借助于 LSD-SLAM （Large-Scale Direct Monocular SLAM）方法14，后端使用上文介绍过的基于最小二乘的图 优化算法。如图 4.1 所示，摄像头获取数据，然后由 LSD-SLAM 处理生成节点图，节点图 由 Graph-based Optimation(即图优化) 处理。 图 4.1整体架构 LSD-SLAM 是一种单目视觉 SLAM 方法15，该方法直接进行连续图像匹配获得当前 环境的深度信息图，并选取关键帧，通过连续图像与关键帧的匹配得到摄像头的运动轨迹， 如图 4.2 所示。该方法选取图像上所有的点进行比较，而不是仅仅选取角点，所以得到的 深度图相比角点方法信息丰富。这里直接使用该算法的开源程序，对此算法不再展开详细 论述。 23 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 图 4.2LSD-SLAM 对于后端，我们使用基于图优化的 g2o 方法8，处理前端数据，得到满足全局误差最 小的环境地图。 本次毕业设计使用 ROS（机器人操作系统，Robot Operating System）平台（4.1.3 节将 对此平台进行介绍） ，在 ROS 平台下将飞行器控制程序、LSD_SLAM、图优化程序整合起 来, 如图 4.3 所示。 图 4.3ROS 程序架构 4.2? 4.2.1? 由于进行了手持和机载两种试验，所以选取了两种摄像头。一种是普通 USB 摄像头， 罗技 C270USB；另外一种是 AR.Drone 飞行器上的前置摄像头。 24 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 图 4.4罗技摄像头 图 4.5AR.Drone 飞行器前置摄像头 首先要分别对图 4.4、图 4.5 这两款摄像头进行标定，拿到相机内参数，去除畸变。这 里，我们使用 ROS 平台 (Robot Operating System) 下的 camera_calibration 包对摄像头进行 校正。通过摄像头对棋盘格不同角度的拍摄，获取摄像头内参数。如图 4.6 所示。 25 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 图 4.6摄像头标定过程 得到摄像机的内参数矩阵，下表为 AR.Drone 飞行器前置摄像头内参数： image width: 640 image height: 360 camera matrix: rows: 3 cols: 3 data: 561.126855, 0, 344.468073, 0, 558.137502, 163.498467, 0, 0, 1 distortion model: plumb bob distortion coefficients: rows: 1 cols: 5 data: -0.518857, 0.279803, 0.001078, -0.002978, 0 rectification matrix: rows: 3 cols: 3 data: 1, 0, 0, 0, 1, 0, 0, 0, 1 projection matrix: rows: 3 cols: 4 data: 455.355591, 0, 350.400233, 0, 0, 523.73053, 161.213323, 0, 0, 0, 1, 0 26 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 图 4.7矫正前后摄像头图像对比 图 4.7 为矫正前后摄像图头像对比图，上图为矫正之前摄像头得到的图像，下图为矫 正之后摄像头得到的图像。摄像头矫正结束后，我们就可以使用 LSD-SLAM 对图像进行 处理。 4.2.2? 飞行器使用欧洲 Parrot 公司生产的 AR.Drone2.0 飞行器，该飞行器在 LINUX 下有一套 完善的开发平台，方便开发者使用该飞行器做各种应用的开发。同时该飞行器还自带光流 悬停算法，为室内地图构建提供了一个可靠的平台。 AR.Drone2.0 飞行器与计算机以网络的方式进行通信，所有的计算程序在计算机上运 行，计算机再发送控制指令返回到飞行器上。飞行器把摄像头读到的图像通过 wifi 发送给 计算机，在计算机上进行地图构建。 27 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 图 4.8AR.Drone 飞行器 4.2.3ROS ? ROS（机器人操作系统，Robot Operating System） ，是专为机器人软件开发所设计出来 的一套电脑操作系统架构。这里，我们对飞行器的操作、视频的读取、LSD-SLAM 算法的 使用以及图优化均在 ROS 平台下完成。 ROS 系统的一大特点是具有很好的模块化设计功能，它通过将各个模块封装为节点， 并在节点之间使用话题或者服务的方式进行通信，达到独立各个模块而又整合所有功能的 目的。 图 4.9ROS topic 通信方式 图 4.9，可以很好的说明 ROS 系统的信息交互方式，图中 ardrone_drive 和 key- board_contorller 为两个互相独立的节点，从 ardrone_drive 到 keyboard_contorller 的信息通 过 ardrone/image_raw 这一话题传递。从 keyboard_contorller 到 ardrone_drive 的信息通过 ardrone/land 这一话题传递。这样实现了两个节点既能独立工作，又能相互传递信息。 28 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 4.3? 整个软件程序框架如图 4.10 所示，摄像头得到的图片首先进行追踪，然后判断是否需 要选取新的关键帧，将当前帧加入到地图当中，最后用 g2o 去优化处理。 图 4.10基于图优化的单目视觉 slam 流程图 4.4? 4.4.1? 选取实验室桌面环境，如图 4.11 所示，我们手持摄像头，对桌面物体进行实时地图构 建。这里摄像头围绕桌面物体均匀移动。该桌面上放有书本、纸巾、水杯等杂物，特点清 晰，层次鲜明，有利于基于视觉的地图构建。 29 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 图 4.11实验室桌面 图 4.12 为摄像头拍摄并处理后的画面，该图像经过去畸变矫正处理，并且被转化为黑 白图像，以减少计算量。 图 4.12摄像头拍摄处理后图像 30 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 图 4.13实验室桌面构建过程 图 4.13 为实验室桌面地图构建过程，红色方框为摄像头当前时刻位姿，蓝色方框为摄 像机过去时刻位姿，每一个方框代表一个关键帧。当摄像头运动超过一定距离后，增添一 个新的关键帧。每增添一个关键帧，对采集到的数据进行一次全局优化。 图 4.14桌面地图及摄像头运动轨迹 1 31 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 图 4.15桌面地图及摄像头运动轨迹 2 实验视频连接：/v_show/id_XOTUzODU1MDU2.html 得到的三维地图较好的符合实际情况。 4.4.2? 我们使用机载摄像头，操纵 AR.Drone 飞行器，对智控所一楼大厅中的物体进行拍摄 和地图构建。在实验的过程中，保持飞行器的飞行稳定。我们选取了特点较为鲜明的阶梯 状物体进行地图构建。 图 4.16智控所 1 楼大厅 32 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 图 4.17AR.Drone 拍摄处理后图像 图 4.17 为 AR.Drone 前置摄像头处理后的画面，原始图像通过 wifi 传到地面站，传输 频率为 30Hz。在地面站此图像进行去畸变校正和黑白化处理，然后进行基于图的 SLAM 运算。 图 4.18智控所大厅物体构建过程 33 浙江大学本科生毕业论文基于飞行器平台的室内地图构建 图 4.18 为智控所局部大厅地图构建过程，红色方框为摄像头当前时刻位姿，蓝色方框 为摄像机过去时刻位姿，每一个方框代表一个关键帧。当摄像头运动超过一定距离后，增 添一个新的关键帧。每增添一个关键帧，对采集到的数据进行一次全局优化。 图 4.19智控所大厅局部地图及摄像头运动轨迹 1 图 4.20智控所大厅局部地图及摄像头运动轨迹 2 实验视频链接：/v_show/id_XOTUzODY1MTY4.html 34 基于飞行器平台的室内地图构建浙江大学本科生毕业论文 图 4.19、图 4.20 为两个不同的角度观看模型地图。该地图不仅画出了摄像头移动的轨 迹，还基本复原了原始图像，图中阶梯明显，深度信息