基于激光和视觉传感器融合的定位与建图_赵以恒.pdf

文章编号：1009 444X（2022）04 0392 06基于激光和视觉传感器融合的定位与建图赵以恒，周志峰（上海工程技术大学 机械与汽车工程学院,上海 201620）摘要：定位与建图是自动驾驶的关键技术之一.激光传感器或视觉传感器具有局限性，通过多传感器融合可以发挥不同传感器各自的优点，提高定位与建图的精度和鲁棒性.通过优化 Harris 算法对图像进行角点提取，利用关键帧对特征点匹配算法进行优化，然后利用非线性最小二乘法进行后端优化.通过试验平台进行定位与建图试验，对算法进行验证，并用 EVO 工具对定位误差进行分析.结果表明，提出后端优化算法误差比单一传感器定位误差减少 13%.关键词：多传感器融合；角点检测；特征点匹配；非线性最小二乘法中图分类号：TP242 文献标志码：ALocation and mapping of lidar and vision sensor fusionZHAOYiheng，ZHOUZhifeng（School of Mechanical and Automotive Engineering,Shanghai University of Engineering Science,Shanghai 201620,China）Abstract：Location and mapping is one of the key technologies for autonomous driving.With limitations oflidar sensors or vision sensors,the advantages of different sensors can be brought into play through multi-sensor fusion and the accuracy and robustness of location and mapping can be improved.The Harris algorithmwas optimized for corner extraction,the key frame was used to optimize the feature point matching algorithm,and then the nonlinear least square method was used for back-end optimization.The location and mappingexperiments were carried out on the test platform to verify the algorithm,and the positioning error wasanalyzed with the EVO tool.The result shows that the error of the proposed back-end optimization algorithm is13%less than that of a single sensor.Key words：multi-sensor fusion；corner detection；feature point matching；nonlinear least square method 移动机器人近几年得到较快的发展，精准的定位和准确的地图对于移动机器人非常重要.利用自身携带的传感器，通过对周围世界的观测和自身运动的解算，来完成自主定位与建图，这便是同 步 定 位 与 建 图 技 术（Simultaneous Locationand Mapping，SLAM）.在 SLAM 系统中，常见的传感器包括激光传感器和视觉传感器.多线激光雷达稳定性较高，但点云特征不明显，不易对障碍物进行很好的分类.相机传感器的成本低，便携程度好，提供丰富的环境信息，但在夜晚或者光线较弱的情况下，识别能力下降并丢掉了深度信息.将不同传感器进行融合可以解决单一传感器具有局限性的问题.2018 年 Graeter 等1提出激光雷达单目相机 收稿日期：2022 05 05作者简介：赵以恒（1996 ），男，在读硕士，研究方向为多传感器融合建图.E-mail:通信作者：周志峰（1978 ），男，副教授，博士，研究方向为自动驾驶和定位与建图.E-mail: 第 36 卷 第 4 期上 海 工 程 技 术 大 学 学 报Vol.36 No.42022 年 12 月JOURNAL OF SHANGHAI UNIVERSITY OF ENGINEERING SCIENCEDec.2022视 觉 里 程 计（Lidar-Monocular Visual Odometry，LIMO）算法，通过激光雷达恢复图像深度，但算法整体的精度并没有很大提高.DVS（Direct VisualSLAM）算法2采用直接法进行特征点配对，准确性得到提升，但匹配过程中会产生不可靠的约束.Huang 等3提出PL-LMO（Point Line Lidar MonocularOdometry）算 法 融 合 了 Lidar 和 Monocular，使 用RANSAC（Random Sample Consensus）算法提取地面，但平面拟合效果并不是很好.FLCS（FusionLidar Camara Sensor）算法4中用激光雷达初始化检测特征，通过高斯回归算法对缺失值进行插值，但 会 造 成 资 源 冗 余.VILM（Visual IMU LidarSLAM）算法5中雷达通过无损检测（NDT）方式构建，减小额误差，但仅采用一个窗口，进行特征点匹配时准确率较低.V-LOAM（Visual-Lidar Odometryand Mapping）算法6的先验由视觉里程计开始，中间经历 BA（Bundle Adjust）优化7，但激光和视觉的一致性检验没有得到很好验证.VIL-SLAM（Visual Inertial Lidar SLAM）算法8实现了基于关键帧的体系结构，但运动估计会产生漂移.LIC-Fusion(Lidar IMU Camera Fusion)算法9在室内外均有较好效果，但没有维护全局地图.张伟伟等10提出双重闭环检测为位姿图提供约束，但对初值要求较高.激光传感器和视觉传感器融合已经成为一种新的趋势，本研究将基于相机传感器特征识别丰富的特点，优化特征点识别与匹配算法，利用激光雷达进行基于图优化的后端优化，充分发挥各自优点，完成精度更高的定位与建图.1 SLAM 简介同 步 定 位 与 建 图（Simultaneous Localizationand Mapping，SLAM）算法一边估计机器人位姿，一边对环境进行建图，其经典框架如图 1 所示.当传感器采集到数据后，首先进行前端里程计的配准，然后进行后端优化，通过回环检测建立闭环，最后建出可以用来导航决策的地图.前端里程计通过相邻两帧的照片或点云求解传感器运动的位置和姿态，传感器位置和姿态使用旋转矩阵和平移矩阵表示.视觉传感器主要分为特征点法和直接法.激光传感器则通过相邻两帧的匹配，达到恢复激光雷达运动姿态的目的.后端优化是从带噪声的数据中估计机器人内在状态的过程，基于图优化的 SLAM 后端算法如图 2 所示.图中节点表示移动机器人的位置和姿态，边表示各种约束.通过构建图算法，找到一个最优的约束点，达到预测数据和传感器观测得到的数据之间的误差最小的目的.回环检测通过检测一组或多组特征来判断是否之前到达过此处，对定位与建图进行优化.本研究主要通过对前端特征点提取算法和后端算法进行优化完成激光和视觉融合的定位与建图.位置特征U 输入Z 观测xkxkxk+1xk+2xk+3xk+4Zk+1,jZk+2,j+2Zk+3,j+2Zk+3,j+3Zk+3,j+1Zk+1,j+1Zk+2,j+1Zk+4,j+3Zk,jYj+1Yj+3Yj+2Yj Uk+1Uk+2Uk+3Uk+4图 2 图优化示意图Fig.2 Graph optimization diagram 2 特征点提取匹配算法与图优化的后端优化算法 2.1 特征点特征点需满足差异性和重复性两个标准.差异性即特征点应明显区别于非特征点，利用点周围灰度变化是否明显区分.重复性意味着同一个特征点应该在不同视角中被重复检测到，即特征点具有旋转、广度、尺度的不变性.角点属于特征点，是灰度图像中不同方向、亮度信息变化剧烈的点.本研究主要对角点检测达到特征提取的目的.2.2 Harris 角点检测Harris 角点检测主要利用特征点具有局部差异性的原理.具体步骤为：以每个点为中心取一个窗口，描述特征点周围的环境，窗口大小自定义，本研究取 55；若以特征点为中心，将窗口向任意方向移动，周围环境变化都较为明显，即在这个区 回环检测传感器数据前端视觉里程计后端非线性优化建图图 1 SLAM 算法经典框架Fig.1 Classic framework of SLAM algorithm第 4 期赵以恒 等：基于激光和视觉传感器融合的定位与建图 393 域里梯度变化较大，那么这个特征点称为 Harris角点.Harris 角点检测流程图如图 3 所示.图片输入高斯平滑滤波计算图像水平和垂直方向的梯度计算每个像素位置的 Harris 矩阵计算每个像素位置的 Harris 角点响应值找到 Harris 角点响应值大于给定阈值且局部最大的位置作为特征点角点检测完毕图 3 Harris 角点检测流程图Fig.3 Flow chart of Harris corner detection 计算图像的梯度时会产生噪声，首先须对图像进行滤波.本研究使用高斯平滑去除噪声，可避免变化较大的误差点对算法造成影响，高斯滤波通过计算卷积和拉普拉斯矩阵完成对图片的降噪处理.然后计算图像水平和垂直方向的梯度，再计算每个像素位置的 Harris 矩阵，Harris 矩阵通过水平方向和竖直方向的梯度加权得到.得到 Harris矩阵后，计算每个像素位置的 Harris 矩阵角点响应值，通过非极大值抑制的方法，仅保留窗口内最大的一个响应值，最后将这个角点响应值大于设定阈值且在窗口内局部最大的位置作为特征点.Harris 算法主要依据特征点具有局部差异性的性质，其算法的数学表达式为E(u,v)=(x,y)w(x,y)I(x+u,y+v)I(x,y)2(x,y)w(x,y)I(x,y)+Ix(x,y)u+Iy(x,y)v+I(x,y)2（1）图像梯度为I(x,y)=(Ix(x,y),Iy(x,y)（2）Harris 矩阵为H=|(x,y)w(x,y)(Ix(x,y)2(x,y)w(x,y)(Ix(x,y)Iy(x,y)(x,y)w(x,y)(Ix(x,y)Iy(x,y)(x,y)w(x,y)(Iy(x,y)2|（3）1、2直接计算 Harris 矩阵运算量较大，本研究利用特征值对 Harris 角点进行优化.Harris 矩阵是最小二乘的线性系统，对 Harris 矩阵进行特征值分解，得到两个特征值，如图 4 所示.两个特征值反映两个相互垂直方向上的变化情况，一个为变化最快的方向，另一个方向为变化最慢的方向，其余方向的变化程度介于两者之间.特征值越大表示变化越快，特征值越小表示变化越慢.最慢变化方向最快变化方向 21 11图 4 Harris 示意图Fig.4 Harris diagram对齐特征值进行分析SVD(H)=UV（4）1 2 01 0,2 01,2 0当时，位于非特征点区域；当，位于边缘区域；当，位于焦点区域.通过特征值对 Harris 特征点进行优化，可以提高特征点检测效率，增加特征点检测个数.2.3 特征匹配特征点提取之后，需进行特征匹配，常见的特征匹配算法为暴力匹配算法，通过计算一张图像固定特征点与相邻图像每一个特征点之间的得分，完成特征匹配.采用暴力匹配算法的步骤为：首先利用优化的 Harris 算法找到第一张图像的特征点，计