【《视觉与激光融合的室内机器人建图算法分析案例》8200字】

视觉与激光融合的室内机器人建图算法分析案例目录TOC\o"1-3"\h\u76501.1引言 162561.2未知环境下的二维栅格地图构建 2122741.3视觉与激光融合的室内建图算法 5317691.3.1相机标定 6100011.3.2激光位姿估计 9249751.3.3视觉位姿估计 117281.3.4基于位姿图的激光位姿优化 11255691.3.5基于BA的视觉位姿优化 1232091.3.6扩展卡尔曼滤波位姿融合 1481801.4激光与视觉的回环融合 15111321.4.1激光雷达回环检测 15117071.4.2视觉回环检测 17144341.4.3激光雷达与视觉相机回环融合 181.1引言现如今，机器人技术之所以受到如此高的关注度，很大原因来自机器人建图技术的发展，通过建图，机器人能够认识自己工作的环境从而在自己熟悉的环境中进行诸多种类的工作。机器人自主导航功能更是依赖于机器人的建图技术，建图技术的核心任务是利用其自身携带的传感器在未知的环境中对环境信息进行获取，并对环境进行地图构建，同时得到机器人在地图中实时的位置和姿态。现如今，主要有两种主流的建图方式，第一种是直接利用激光雷达对环境进行建图，但是这种方式无法获取到丰富的环境信息，无法形成三维的环境信息，无法对机器人的功能进行有效拓展，第二种是使用三维激光雷达或者以计算机视觉的方式进行环境建图，但是这种方法因为要先提取图像特征，进行特征匹配、估算变换矩阵，因此运算量巨大，无法保证建图的实时性。因此单一传感器建图的方式无论是在运算成本还是建图效果上都无法满足现有的要求，所以有必要进行多传感器融合建图方式的研究，本章针对单传感器在构建室内环境地图时由于累计误差大导致地图质量不高的问题提出一种激光与视觉融合的建图方法。1.2未知环境下的二维栅格地图构建机器人在未知的环境中实现导航等功能，首先需要准备一张可以进行导航的地图。二维栅格地图由于其容易构建、表示、保存、便于路径规划等优点成为目前SLAM领域主流的导航地图，如图3-1所示。二维栅格地图是一种尺度地图，与其类似的还有特征点图、点云地图等，如图3-2和3-3所示。图3-1二维栅格地图图3-2特征地图图3-3点云地图二维栅格地图广泛应用于机器人领域，是机器人实现路径规划、实时避障功能的基础。其构建方法主要有两种，一种是使用激光雷达传感器进行绘制，另一种是使用相机获取环境的三维点云，然后对点云进行降维处理形成二维栅格地图。本节主要介绍基于激光雷达的二维栅格地图生成过程。使用激光雷达构建二维栅格地图时，首先需要较精确的激光雷达数据，但是激光雷达传感器由于硬件等原因总是会存在着很大的噪声，机器人在同一个位姿下的不同时刻，激光雷达对某个障碍物进行测距的结果不一致，一帧可能是4m，另一帧可能是4.1m。但是不能将其位置都标记为障碍物，为解决噪声带来的使地图与实际环境匹配度差的问题，因此出现了占据栅格地图构建算法来解决这一问题。首先将环境未知地图栅格化，将整个环境区域划分成很多个小格子，对于每个小格子的状态有三种，分别是占用（障碍物）、空闲、未知（初始化状态），如图3-4所示。图3-4栅格地图状态在整张栅格地图中，针对每一个单独的格子，将其空闲状态的概率用来表示，若其被障碍物占据，则使用来表示其概率，两者之和为1。使用值来表示一个栅格的状态会存在较大的偶然性，误差大。因此引入两者的比值来对栅格的状态进行判定，如下式。（3-1）当激光雷达获取到环境障碍物信息得到观测结果后，需要对相关的栅格进行状态更新。假设在激光雷达获取到环境信息之前的栅格状态为，则在获取到某些栅格在实际环境中的实际障碍物信息后，其状态则更新为：（3-2）状态估计根据贝叶斯公式可得如下3-3和3-4式的形式。（3-3）（3-4）将上述两个式子，代入，得出如下3-5式至3-8式。（3-5）（3-6）（3-7）（3-8）此时对上式两边取对数即二维栅格状态发生再一次变化，采用表示某个某个栅格的状态，如下式3-9所示。（3-9）此时只有项含有测量值，将这个比值称为测量值模型，因为激光雷达对某一个栅格的观测结果只有两种，因此的选项只有两种状态，分别是free和occu，代表空闲和占据，如下式：（3-10）（3-11）此时，用表示栅格s的状态S，则栅格状态更新进一步简化为：（3-12）其中和分别表示测量值之后和之前栅格S的状态。经过建模，只需要使用加法即可完成一个栅格状态的更新，如下式。（3-13）（3-14）假设occu=0.9，free=-0.7。显而易见，栅格状态的值越大，栅格占用的概率就越大。相反，值越小，网格越空闲。如下图3-5所示是用两次激光扫描数据更新地图的过程。在结果中，颜色越深越表示栅格是空闲的，颜色越浅越表示是占据的。图3-5二维栅格地图更新过程1.3视觉与激光融合的室内建图算法相机和激光雷达是目前SLAM领域应用较广泛的两种传感器。二维激光雷达通过对环境某一个平面进行扫描能够构建出精确的地图，为机器人导航提供地图，但是激光雷达因为其构造只能扫描一个平面，因此忽略了大量的环境信息。视觉传感器具有广阔的检测领域，根据相机成像原理，能够获取到更加丰富的环境信息，可同时获取环境障碍物的颜色和深度信息，但是纯视觉易受光照影响，也会影响建图质量。激光雷达和视觉传感器进行融合能够有效避免单个传感器的局限性。图3-6激光雷达与相机融合算法流程图如上图所示，首先利用激光雷达数据和里程计数据采用迭代最近点算法估算出激光雷达传感器在运动时的初始位姿，然后使用位姿图对初始位姿进行优化；双目相机采集到的是图像信息，采用ORB特征点提取算法，利用对极几何方法估计出双目相机的初始位姿，最后使用BA（BundleAdjustment）算法进行优化。得到两种传感器优化后的位姿以后，采用扩展卡尔曼滤波（ExtendedKalmanFliter,EKF）对两个位姿进行融合，融合后的位姿作为机器人真实的位姿。在得到机器人融合后的位姿后，还需要进一步的优化，本文采用提取特征点建立词袋检测回环，然后再与激光回环检测进行融合，在融合过程中使用相机位姿来修正机器人位姿，于此同时对视觉回环检测和激光回环检测进行融合可以有效修正由于累计误差导致地图无法形成闭环的现象。综上，本文提出的融合方法主要有两个特点：（1）采用扩展卡尔曼滤波对激光雷达位姿和视觉位姿进行融合，然后使用视觉来修正激光雷达得到的位姿，从而使机器人的位姿精度更高。（2）在位姿优化阶段，通过将激光回环检测与视觉回环检测进行融合，从而可以利用视觉更加丰富的语义信息来补充激光雷达信息不足的缺点，提高回环检测的精度，构建全局一致的地图。1.3.1相机标定将两种不同类型的传感器进行融合，首先面临的问题就是传感器的校准。为了使两种传感器能在同一时间同一环境下对环境信息进行有效的描述，必须对传感器进行精确的标定。在标定前首先要准备一块规格为A0尺寸的硬纸板（900*1200mm），如下图所示。图3-7双目相机标定板采用外部标定法，利用机器人操作系统（ROS）中的参数服务器工具进行标定。设点云在相机坐标系下的空间位置为，点云在激光雷达坐标系下的空间坐标为，其中K为相机的内参数，为点云在图像上的投影点。将点云与激光雷达坐标系转换到相机坐标系下，如下式：（3-15）再由相机坐标系变换到归一化平面坐标系并投影到像素平面上，如下式：（3-16）最后根据图像的大小判断投影点是否在图像上，保留投影在图像上的投影点并在图像上标出，建立二维投影点和三维投影点的对应关系，如下图所示。图3-8激光与相机联合标定在标定之前需要进行动态参数的配置，配置的参数有九个，如下所示：Roll：外参中旋转矩阵对应的滚转角，默认为0。Pitch：外参中旋转矩阵对应的俯仰角，默认为0。Yaw：外参中旋转矩阵对应的偏航角，默认为0。X：外参中平移向量对应的x分量，默认为0。Y：外参中平移向量对应的y分量，默认为0。Z：外参中平移向量对应的z分量，默认为0。Dis_filter：勾选后表示按照给定的距离对点云进行过滤，只保留一定距离内的点云。Next：表示只对当前图像与点云进行处理。Get_point：手动在图像上进行取点。标定前的准备工作完成后使用SLAM主流测试数据集KITTL数据进行标定，首先在ROS系统中启动标定程序，再播放数据包。刚开始由于外参参数默认是0，所以图像上看不到点云的投影点，此时需要手动调整“roll”，“pitch”，“yaw”，“x”，“y”，“z”这六个参数，将点云的投影点向图像上调整。如下图所示。图3-9相机标定初始图像观察上图可以看到，点云在投影后失去了深度信息，因此无法在图像上对不同深度的点云进行投影，此时通过调整点云过滤距离找到点云投影点和图像像素点的对应关系，如下图所示。图3-10过滤后的点云投影图像图3-11过滤后局部放大图通过调整外参参数，将右侧广告牌的投影点和对应图像像素重合，如下图所示图3-12投影点和像素点重合通过手动调参后，就可以获取点云和像素点之间的对应关系，即完成相机标定。1.3.2激光位姿估计激光雷达通过ICP算法匹配相邻激光帧数据来求得激光雷达的位姿，这种方式使用的数据为激光雷达采集到的原始数据，因此鲁棒性更高。在激光帧之间进行激光点的匹配，其基本原理是通过两帧点集之间的对应点进行配准，从而找出两帧激光点之间的运动关系，如下3-13所示，通过两帧之间相应点之间的配准可以建立两帧激光点集之间的关系，从而可以继续推导出两帧点集之间的旋转矩阵和平移向量。图3-13ICP算法匹配示意图ICP算法主要是通过寻找最近邻点匹配对和最近邻点匹配来计算出其旋转矩阵R和平移向量t。在寻找最近邻点匹配对时，通常采用暴力匹配法。在暴力匹配方法中，当前点云集的每一个点与激光雷达之间的距离与参考点云集所有点的距离值之间进行作差，其结果取绝对值，最后选取两点之间距离最小的点用作匹配点对，匹配点对就是在空间中的某一个点在激光雷达不同时刻扫描到的点。在ICP算法计算旋转矩阵R和平移向量t的过程中，设P是激光雷达传感器在t-1时刻采集到的点集，这个点集中一共有个点，其中每个点用表示，Q为激光雷达在t时刻采集到的点集，此时的点集共有,其中每一个点用表示，通过下式求解两个点集之间的位置变换参数R和t。（3-17）由上式可知，当目标函数的值最小时得到的旋转矩阵R和平移向量t就是激光雷达相应的位姿变化。此算法的实现流程框架如下图所示。图3-14ICP算法实现流程框架图1.3.3视觉位姿估计视觉相机估计位姿就是通过视觉里程计算法估计出相机的运动过程，其原理如2.1.2节所述，首先采用ORB特征点提取算法提取出相邻图像帧中的特征点，然后同样对两帧中提取出的特征点进行匹配，利用RANSAC算法剔除误匹配点，筛选出一些质量较高的特征点，最后采用对极几何求解相机的位姿，得到两帧图像之间的运动关系，具体的计算过程在第二章SLAM传感器数据处理的2.1.2节中做了详细的推导，本节不在做详细说明。1.3.4基于位姿图的激光位姿优化由1.3.2和1.3.3节可知，此时已经得到了激光雷达和双目摄像头的位姿，但是由于硬件或者人为操作以及环境等因素，得到的位姿结果会有误差，且会随着时间的累计，累计误差会越来越大，因此为得到精度更高的位姿信息为后面的建图以及导航提供更好的数据，此时需要对其进行优化。本文采用图形优化方法对激光雷达的初始姿态进行优化。图优化是一种将变量和成本函数放入图中的方法。这里的图是图论意义上的图。这里的图形由顶点和连接顶点的边组成。在本节中，位姿由顶点表示，对于优化变量，误差项由边表示，如下式所示（3-18）上式中，E代表图中的边，V代表图中的顶点。位姿图由边和顶点组成，通过构建一个只有轨迹的图优化，可以让两个关键帧之间通过特征匹配之后得到的运动估计来给定位姿初始值。如图3-15所示，激光雷达的位姿用图中的节点表示，相邻的激光雷达位姿用边进行连接，而边为相邻激光雷达位姿之间的相对运动估计的误差。图3-15位姿图激光雷达的位姿集合一般用角度和平移向量描述，比如，设某个顶点用表示，位置上的观测信息作为对邻近的另一个顶点的约束条件，则两顶点的位姿偏移量如下式所示。（3-19）上式中，表示旋转后的旋转矩阵，表示观测方程函数。根据观测方程函数和观测信息，可以推导出其误差函数，如下式所示。（3-20）位姿图整体目标函数为：（3-21）上式中，表示所有边的集合，最小化上式所示的误差就可以计算出优化后的位姿，通过最小化上式中的目标函数计算优化后的估计值，通过L-M（Levenberg-Marquardt）方法求解，其计算公式如下式。（3-22）上式中表示拉格朗日乘子，表示雅克比矩阵，diag表示构造对角函数，H为海塞矩阵，表示引入的增量，引入方式如下：（3-23）1.3.5基于BA的视觉位姿优化由1.3.3节得到双目相机的位姿，本节采用BA算法对该位姿进行优化。BA算法的最终目的是最小化重投影误差，对对极几何给出的结果进行优化。采用特征点匹配证实了P1和P2是同一个空间点的投影，是点P的重投影点，如下图3-16所示，在现实情况下，和之间并不是重合的，二者之间有一定的距离，最小化重投影误差就是让和之间的差值最小。图3-16重投影示意图首先将和的坐标进行归一化处理，如下式所示。（3-24）然后表示重投影误差：（3-25）最后总体重投影误差为：（3-26）这里仍然是一个最小化二乘问题，同样采用L-M方法进行求解。1.3.6扩展卡尔曼滤波位姿融合本文采用扩展卡尔曼滤波算法对激光雷达姿态和相机姿态进行融合。扩展卡尔曼滤波是由卡尔曼滤波发展而来的。卡尔曼滤波应用于线性系统，但不能应用于非线性系统。激光雷达位姿与相机位姿融合系统属于非线性系统，因此扩展卡尔曼滤波更为方便。根据扩展卡尔曼滤波算法的原理，想要估算出某时刻的机器人实际位姿，就需要根据t-1时刻的位姿进行预测，此时，用来估计t时刻的位姿来自两个不同的传感器，一种是激光雷达，另外一种是双目摄像头。假设激光雷达传感器的位姿作为预测值，其噪声为将此时位姿的噪声协方差矩阵记为，得到的双目相机的位姿作为实际测量值，其噪声为，对应的位姿噪声协方差为，预测值通过双目相机的位姿来进行纠正。预测值因为是采用的激光雷达传感器的，其获取到的信息量较少并且只有几何信息，因此在几何相似度非常高的情况下，会导致测量数据误差较大不可靠，而实际测量值使用了双目相机，通过加入视觉的估算位姿增加了纯激光雷达位姿的先验条件。对于非线性系统，其对应的系统状态方程和测量方程可描述为：（3-27）上式中，表示控制输入量，表示测量函数，表示转移矩阵，，均为高斯噪声，且都符合正态分布。为了为下一时刻提供先验估计，首先需要计算出当前的状态变量和误差协方差。根据系统上一时刻的状态来对当前的状态进行预测，如下式所示为状态预测方程。（3-28）上式中表示当前时刻的状态是根据前一时刻的状态进行预测的。预测之后可以对对应的协方差进行更新，下式为更新方程。（3-29）上式中是的协方差矩阵，是噪声协方差为状态转移方程的雅克比矩阵。在融合过程中，更新部分为：（3-30）（3-31）（3-32）其中，是误差方程，表示误差的协方差，表示卡尔曼增益。最后通过测量值修正预测值，此时可计算出当前状态的估计值和更新相对应的协方差，如下式所示。（3-33）（3-34）其中，是通过拓展卡尔曼滤波算法融合激光位姿和相机位姿后的融合位姿，通过不断的更新误差估计，便可以实现扩展卡尔曼滤波方程的不断更新，从而获取最优融合位姿。1.4激光与视觉的回环融合1.4.1激光雷达回环检测在使用激光雷达进行机器人定位与地图构建时，随着时间的累计，其激光雷达的测量数据也会有越来越大的累计误差，为了得到全局一致的轨迹和质量较好的地图，需要消除随着时间的累计而不断增大的累计误差，此时引入回环检测来解决这个问题。激光的回环检测就是通过将历史的激光帧数据利用起来，当前的雷达激光数据帧与某一历史激光数据帧具有很高的相似性时，此时则认为机器人正在经过之前已经经过的地方，此时若能及时检测到回环就能实现轨迹和地图的修正。激光雷达数据中有激光点的距离信息和角度信息，信息表达简单，因此可以考虑在回环检测时使用暴力匹配的方式进行检测。首先，把当前点云集中每个点的距离值和参考帧中的所有点的距离值进行做差，然后取绝对值。其次，筛选出做差之后绝对值最小的点建立匹配对关系，再对匹配对之间的角度值进行比较并设置阈值，只有在两帧激光点集匹配对的数量大于设定阈值且匹配对之间对应的角度信息也相等时，才能判定回环检测成功，否则失败，如图下图所示。3-17回环检测成功3-18回环检测失败1.4.2视觉回环检测视觉回环检测的思路和激光回环检测差不多，目前在视觉信息进行回环检测时，最常用的方法是词袋法。词袋就是将图像中提取的特征点当成一个个单独的单词放入一个袋子里，当需要进行回环检测时，将当前帧图像特征点与词袋中的单词一一进行比对，找出相似度最高的那一个，通过相似度与阈值的比较来判断是否成功检测到了回环。3-19视觉回环检测流程（1）提取图像特征点本文在提取图像特征点时采用的是ORB（OrientedFASTandRotatedBRIEF）算法。其主要步骤主要分两步，首先是提取特征点，然后是对提取到的特征点进行描述，使每一个特征点都具有唯一性和明显的特征。（2）选取关键帧选取关键帧的目的就是为了在做帧间匹配时，只需要对那些匹配效率和正确率看起来较高的图像进行匹配，这样既能降低算法计算量又能够保证较高的匹配精度。本文通过两帧图像之间的ORB特征的变化程度进行筛选。筛选过程需满足几个规则，首先两帧图像之间的距离要合适不能太近，也不能太远，本文经过实验验证，在使用间隔为20帧的距离时，具有较好的效果，这样能够保证两帧之间没有过多的冗余信息。其次本文选取的关键帧上必须有超过60个特征点才有可能被选做关键帧，最后，两帧之间的特征变化率必须要低于85%，以此来保证两个相邻的关键帧上有足够的特征变化。当某一帧满足以上条件时才可以被选做关键帧，进行后面的操作。（3）建立视觉词袋回环检测过程一般和建图过程是同时在进行的，计算量大，实时性较差，为了避免由于回环检测和主要进程即建图进程发生算力的竞争，所以使用词袋模型来降低回环检测过程的计算量，提升整个建图以及优化的实时性。词袋就是将获取到的图像特征采用k-means聚类算法进行聚类，然后在回环检测时采用高效的搜索算法在词袋中搜索视觉单词。（4）视觉回环检测在获取机器人位姿阶段，本文采用了融合的思路将激光雷达的位姿和相机的位姿进行融合，得到了精度较高的位姿，但是随着时间累积效应，建图的累计误差也越来越大，因此在回环检测环节，本文也采用融合的思路将激光回环检测和视觉回环检测融合起来，提升回环的效率。在视觉SLAM中，实现回环检测需要匹配任意的两幅图像上提取的特征点，根据特征点的匹配结果来判断机器人此时是否经过了之前经过的地方，产生了回环。1.4.3激光雷达与视觉相机回环