【《基于经典RTAB-MAP框架的三维重建系统分析》4800字】

基于经典RTAB-MAP框架的三维重建系统分析目录TOC\o"1-3"\h\u27422基于经典RTAB-MAP框架的三维重建系统分析 1285311.1RTAB-MAP算法介绍 1281261.1.1图slam 1154371.1.2传统RTAB-MAP算法框架 2286991.2关键算法描述 4171021.1.1坐标系转换 5240821.1.2ICP算法 6310061.1.3TSDF点云融合算法 7114231.1.4g2o后端优化算法 81.1RTAB-MAP算法介绍RTAB-MAP是一个目前广泛流行的开源SLAM算法，它是一种基于环境的闭环检测方法。其具有内存管理功能来处理大规模和长期的在线计算。之后，它逐渐发展为在各种机器人和移动平台上实行SLAM算法。由于每个应用程序在传感器，处理能力和运算能力方面都有自己的优缺点，因此，如何根据所用的传感器选择合适的算法极为重要。RTAB-MAP算法涵盖了大部分传感器及复杂环境的应用。用户可以使用不同的机器人和传感器来运行并比较各种方案来选择最合适自己的算法工具。接下来主要介绍RTAB-MAP的背景知识及算法分析。1.1.1图slam图SLAM(GraphSLAM)是一种SLAM算法，可以解决完整的SLAM问题，该算法可以恢复整个地图和路径而不是仅仅是最近的位姿和地图。图SLAM的优点之一是减少了对车载处理器计算能力的需求，另一个是图SLAM的图形比FastSLAM的地图精度更高。快速FastSLAM使用粒子来估计机器人最可能的位姿，但是在任何时间点，都有可能在最可能的位置没有粒子。由于图SLAM解决了完整的SLAM问题，因此这意味着它可以一次处理所有数据以找到最佳解决方案。在图SLAM中，其思想是在图中组织信息。图中的节点表示在特定时间步长t上的机器人姿态或环境中的特征的位置。图中的边代表姿势和特征之间的测量约束或两个连续姿势之间的运动约束。由于空间约束较弱，可以将它们视为连接两个带有质量的弹簧。以此类推，完整的SLAM问题可以作为全局图优化问题来解决。最佳的配置是使弹簧松弛并且使每个节点上的力最小的结构。最大似然原理（MLE）用于优化地图。当应用于SLAM时，似然度会尝试根据给定的运动和测量观测值来估计状态和特征位置的最可能配置。图SLAM的目的是创建一个环境中遇到的所有机器人姿势和特征以及最可能的机器人路径和环境图的图。该任务可以分为两部分，前端和后端。图SLAM的前端着眼于如何使用机器人收集的里程计和测量值来构建图。这包括解释传感器数据，创建图形并在机器人穿越环境时继续向其添加节点和边。当然，这取决于期望的目标，包括应用的精度，所使用的传感器和其他因素，例如，前端可能因应用场景而异。在办公室中使用激光测距仪应用SLAM的移动机器人的前端与在大型室外环境中使用立体摄像机的车辆的前端有很大不同。图SLAM的前端也面临着解决数据关联问题的挑战。用简单的术语来说，这意味着可以准确地识别以前是否已经看到过环境中的特征。图SLAM的后端的输入是带有所有约束的完整图形，输出是机器人姿势和地图特征最可能的配置。后端是一个优化过程，它接受所有约束并找到产生最小错误的系统配置。后端在各个应用程序之间更加协调。前端和后端可以连续完成，也可以迭代执行，后端将更新后的图形馈送到前端以进行进一步处理。本文所利用的RTAB-MAP便是基于图的SLAM。基于图的SLAM意味着该算法使用从视觉传感器收集的数据来定位机器人并绘制环境图。在基于图的方法中，称为“回环检测”的过程用于确定机器人之前是否曾看到过某个位置。随着机器人前往其环境中的新区域，地图会扩展，每个新图像必须与之进行比较的图像数量也会增加。这导致回环检测会花费更长的时间，并且复杂度线性增加。RTAB-MAP通过使用多种策略来实时关闭环路，针对大规模和长期SLAM进行了优化。1.1.2传统RTAB-MAP算法框架图STYLEREF1\s2SEQ图表\*ARABIC\s11RTAB-MAP框架图图STYLEREF1\s21所示，RTAB-MAPROS节点所需的输入是：任何来源输入的里程计（可以是3个自由度或6个自由度）；视觉数据输入（来自摄像头获取的RGB-D图像或立体图像）以及相应的校准消息，可选输入是来自2D激光雷达的激光扫描或来3D激光雷达的点云。然后，将这些输入的所有消息同步并传递到图-SLAM算法。输出为：地图数据；没有任何数据的地图；里程计校正发布在TF上；可选的OctoMap（3D占用网格）；可选的密集点云；可选的2D占用栅格。图STYLEREF1\s21为RTAB-MAP的主要ROS节点。由图可知里程计只是RTAB-MAP的外部输入，RTAB-MAP几乎可以兼容任意一种里程计设备。各种传感器数据同步后，短期记忆模块将创建一个节点，用于存储里程计姿态、传感器的原始数据以及对下一个模块有用的其他信息。根据从节点创建的数据相互重叠的多少，以毫秒为单位设置固定速率来创建节点。例如，如果机器人运动迅速且传感器范围较小，则应提高检测率以确保连续节点的数据重叠，但是将其设置得过高则会不必要地增加内存使用量和计算时间。两个节点之间的刚性转换关系存在链接中，一共有三种链接：邻居链接，闭环链接和邻近链接。通过里程计转换，在相邻节点之间的短期内存中添加邻居链接。回环和相邻链接分别通过回环检测和邻近检测来添加。所有的链接都用作图优化的约束。当向图添加新的闭环或邻近链接时，图优化会将计算出的误差传播到整个图，以减少里程计的漂移。通过图优化，占据地图，点云和2D栅格能够组合输出，并将其发布到外部模块。此外，还可以利用测距法在地图框架中进行定位。RTAB-MAP的内存管理方法在图形管理模块上运行[20]。它用于限制图形的大小，以便可以在大型环境中进行在线SLAM计算。随着图形的不断增长，在闭环检测、图优化和全局地图组装等模块的处理时间会变长，为了避免这段处理时间大于节点获取的周期时间，RTAB-MAP引入了内存管理的理念RTAB-MAP的内存分为工作内存（WM）和长期内存（LTM）两种。当节点数过多导致定位匹配的时间超出固定时间阀值“RTAB-MAP/TimeThr”时，RTAB-MAP就将工作内存中不太可能形成闭环的节点转移到长期内存中，而这些节点就不参与下次闭环检测的运算了，这样可以有效避免由于工作内存过大导致更新时间过长。除此之外，还有一个内存阈值“RTAB-MAP/MemoryThr”用于设置工作内存可以容纳的最大节点数。要确定哪一个节点转移到长期内存，需使用启发式方法（例如，观察到的位置越长，则越重要）就显得尤为重要，因此应将其留在工作内存中。为此，在创建新节点时，短期内存将该节点的权重初始化为0，然后将其可视化地进行比较（得出相应的可视化单词的百分比）与图形中的最后一个节点进行比较。RTAB-MAP设置了一个相似阈值“Mem/RehearsalSimilarity”，当新节点与图形中最后一个节点的相似度超出这个阈值，则将这两个节点的权重相加赋值给新节点，图形中最后一个节点的权重将重置为0。如果机器人没有移动，则最后一个节点将被丢弃，以避免无用地增加图形大小。当达到时间或内存阈值时，最小的加权节点中最早的节点将首先传输到长期内存。当工作内存中某一节点检测到闭合环路时，可以将该节点的邻居节点从长期内存重新带回到工作内存中，以进行更多的回环检测和邻近检测。当机器人在先前访问的区域中移动时，它可以逐渐记住过去的位置，以扩展当前的组合图并使用过去的位置进行定位。RTAB-MAP可用于对不同的传感器进行试验，并尽早确定传感器是否适合目标应用。根据论文提供的结果，可以导出有关在室内环境中使用SLAM（无外部全局定位）的准则。除非使用远程激光雷达，否则对于鲁棒的自主导航，必须具有来自本体感受传感器（例如惯性导航，车轮编码器）的里程输入。当仅依靠短距离传感器时，机器人很可能会终止于传感器无法看到足够特征以能够在地图上定位自己的区域。对于相机，看到白色的墙壁，无纹理的区域或黑暗的区域会导致定位不准确。对于短距离激光雷达，较大的空白空间或几何形状复杂度较低的长走廊也可能会产生问题。两种传感器的问题都取决于环境。1.2关键算法描述基于Kinect2的室内三维重建在经典架构上主要分为前端VO部分，中间的关键帧提取，角点检测，ICP匹配，后端优化与回环检测等几个部分。下文将对本文用到的一些算法进行数学模型的分析，方便对算法进行进一步的介绍。首先介绍在重建过程中需要用到的几个坐标，接着阐述各个坐标系之间的转换关系，以及相机位姿的变换矩阵，这些转换关系是联系相机与最终的重建结果的基础。然后我将介绍点云配准的方法ICP算法与TSDF点云融合的算法，这部分用于完成从一帧的点云到整个的点云的过程。最后介绍以下g2o的后端优化框架用来进一步融合UWB数据提高最后建图的精度。1.1.1坐标系转换在整个重建系统中包含着三种坐标系，全局的世界坐标系（机器人的运动轨迹等都是在世界坐标系中观测的）、相机坐标系以及图像坐标系，通过相机的内参矩阵可以将图像坐标系转换到相机的坐标系上，然后通过刚体变化将相机坐标系再转换到最后的世界坐标系上。（2-1）上述公式描述了图像坐标与像素坐标的关系，其中（u，v）为像素坐标系，（x，y）为图像坐标系。（2-2）上述公式描述了相机坐标系与图像坐标系的转化关系。其中（XC，YC，ZC）坐标为相机坐标系，（x，y）为图像坐标系.，相机的光轴为XZ，XCYC与图像坐标系的xy轴平行，为光轴与相机物理坐标系的夹角，f为相机的焦距。（2-3）上述公式描述了世界坐标系（XWYWZW）与图像像素坐标系（u，v）的关系即通过外参矩阵得到相机坐标系与世界坐标的关系然后再通过公式（2-2）以及（2-1）得到在假设相机光轴与图像物理坐标系夹角为90度的情况下得到的。相机运动可以用欧拉角和四元数进行描述，其中欧拉角通过roll，pitch，yaw三个角度来描述物体在不同坐标轴上的旋转分量来描述物体整体的旋转状况，但是容易出现万向节死锁问题。旋转过程中的坐标轴重合会使得得到的旋转状态失去在某些坐标轴上的分量。四元数则使用了冗余的九个相关量来描述旋转过程，使得整个旋转过程的描述不会出现欧拉角的万向节死锁问题具有奇异与冗余的性质。四元数q的表达式为一个实部加上3个虚部，如下表达式：（2-4）描述旋转矩阵可以由四元数的参数表示如下式所示（2-5）1.1.2ICP算法ICP（IterativeClosestpoint）算法由Besl和Mckay提出。ICP算法通过两个点集之间距离最近为原则建立误差函数。然后通过使得优化函数取最小值来获取变换矩阵使得点云能够进行精确匹配。ICP算法在执行过程中主要分为以下四步：1）对于匹配点进行计算，找到源点集对应的目标点集。2）通过源点集与目标点集估计变换矩阵并求目标函数最小值。3）对源点集进行转换得到新的点集作为新的源点集4）对上述过程进行迭代计算直到目标函数值变化小于阈值。目标函数由下式给出（2-6）其中d是目标函数N为点的个数W为匹配点集U为当前的源点集T为目标点集R为转换矩阵。在实际使用Kinect2进行slam重建的时候不可能使用整张深度图上的所有点云数据进行ICP算法匹配。所以需要选取一些关键点进行帧间匹配。首先通过图像金字塔对图片进行降采样的处理得到分辨率较小的图片，然后通过UWB对于相机姿态的估计来判断关键帧之间可能匹配的点的区域然后再在这些区域选取关键点进行ICP算法匹配以降低运算成本。1.1.3TSDF点云融合算法点云通过ICP算法完成匹配后通过TSDF(TruncatedSignedDistanceFunction)算法来进行全局的重建，这个算法能够使得重建出来的目标的细节得到更多的保留。TSDF算法需要先创建一个全局立方体用来储存接受到点云中的点。按所需要重建的范围大小来划分全局立方体成对应的小立方体然后按早以下步骤来完成对于一帧点云的融合。首先通过ICP算法拿到变化矩阵并且将立方体中间的方块通过坐标系转换的方法得到其相机坐标。通过标定得到的相机内参矩阵。将1）得到的相机坐标转换到图像坐标。获取该点的深度值与相机坐标中的值进行比较判断该立方体在重建的表面的位置。在内部还是外部。4）更新立方体的权重与距离权重与距离的计算公式如下(2-7)上面第一个式子用来计算相邻两帧之间的小立方体的重建时候的表面距离。第二个式子则是计算其权重。第三个式子中的maxtruncation和mintruncation则表示其截断阈值。1.1.4g2o后端优化算法SLAM中间很重要的一部分就是对于机器人位姿的估计，这一过程在本质上也是一个对于非线性误差函数求解最小值的问题，在数学上这一问题有很多通用方法比如高斯-牛顿法，LM法等，但是这些方法在用于工程上的时候相当的困难。g2o则是一个图优化的开源