标志辅助的多特征融合定位算法_刘嘉敏.pdf

2023，59（6）同步定位与地图构建（SLAM）能帮助传感器定位自身位置，并对感知的环境进行地图构建，在机器人定位与导航、自动驾驶、增强现实等任务中有广泛的应用。视觉SLAM中常选择单目相机作为主要传感器来采集数据，是因为单目相机具有便捷、功耗小、成本低的优点，但其成像图片丢失了场景的深度信息，因而单目SLAM需要通过前后帧特征的提取和匹配完成定位和建图任务。这其中由于提取和匹配的精度易产生累积误差，从而影响最终估计的精度，所以如何更好地利用特征以提高精度一直是单目SLAM中一个很大的挑战。近年来，研究人员在单目SLAM技术1上做了大量的工作，MonoSLAM2、PTAM3、ORB-SLAM24使用点特征进行定位，点特征可以方便地从图像中提取，但常受到弱纹理结构和光照挑战的影响。为了丰富特征，减少对点特征的依赖，PL-SLAM5加入线特征降低遮挡对特征的影响，文献6加入面特征表示环境结构特征，使用少量的参数表示更大的结构，文献7添加物体特征，使用CNN网络预测物体尺度进一步修正位姿。标志辅助的多特征融合定位算法刘嘉敏，陈圣伦，王智慧，李豪杰大连理工大学 软件学院，辽宁 大连 116620摘要：单目同步定位与地图构建的精度依赖于图像中特征的提取与匹配算法，最终估算的轨迹常因其中误差的累积而有所偏移。针对此问题，提出一种标志辅助的多特征融合定位算法，结合标志所处的环境平面结构信息辅助定位。该算法使用点、标志、平面多个特征提高位姿估计的精度，标志特征提供更鲁棒的特征点，面特征以更少的参数表达更大的结构，减少遮挡对特征匹配的影响；且通过环境结构中的平面关系建立标志间关联，使标志在优化中更满足相互之间的几何位置关系，从而减少累积误差造成的漂移。实验结果表明，该算法可以有效地在含有标志的环境中定位相机，且在困难环境中能更好地校正回环，与同类方法相比，精度明显提升。关键词：同步定位与建图；单目相机；误差累积；标志特征；平面特征文献标志码：A中图分类号：TP391.4doi：10.3778/j.issn.1002-8331.2111-0316Marker-Assisted Multi-Feature Fusion Localization MethodLIU Jiamin,CHEN Shenglun,WANG Zhihui,LI HaojieSchool of Software Technology,University of Dalian Technology,Dalian,Liaoning 116620,ChinaAbstract：Monocular simultaneous localization and mapping accuracy relies on the feature extraction and association algo-rithm in image.The estimated trajectory will draft because of the error accumulation.Aiming at the problem,this paperproposes a marker-assisted multi-feature fusion localization algorithm,which combines the environment planar structurewhere the markers are located to assist localization.The algorithm uses point,marker and plane features to improve theaccuracy of pose estimation.Marker feature provides more robust points.Plane feature uses fewer parameters to representlarger structure,reducing the impact of occlusion on feature matching.It establishes the relationships between markers bythe planar structures,which can make markers more satisfy the geometric position relationship between each other in theoptimization,thereby reducing the drift caused by the accumulated error.Experimental results show that the algorithm caneffectively track in the environment containing markers,and can better correct the loop in difficult environments.Com-pared with similar methods,the accuracy is significantly improved.Key words：simultaneous localization and mapping;monocular camera;error accumulation;marker feature;plane feature图形图像处理基金项目：国家自然科学基金（61976038，61932020，61772108）。作者简介：刘嘉敏（1997），女，硕士研究生，研究方向为视觉SLAM，E-mail：；陈圣伦（1994），男，博士研究生，研究方向为深度补全；王智慧（1982），女，博士，教授，研究方向为图像处理；李豪杰（1973），男，博士，教授，CCF会员，研究方向为多媒体信息检索。收稿日期：2021-11-17修回日期：2022-01-04文章编号：1002-8331（2023）06-0187-09Computer Engineering and Applications计算机工程与应用187Computer Engineering and Applications计算机工程与应用2023，59（6）除了直接从图像提取更多种特征辅助定位之外，在环境中加入人工路标，使用人工路标特征也可有效提升单目SLAM中的定位和建图精度，减少误差的影响。人工路标即平面人造标志，可提供边长和角点信息先验，从而有效约束尺度。使用标志特征的方法可以分为两类，一类是使视觉图像中的提取的标志信息跟IMU传感器测量值结合。RCARS8系统通过IMU测量信息预测位姿状态，并用检测的标志更新状态，但因其使用扩展卡尔曼滤波的方法，受计算量的限制只能处理有限的标志数量。文献9对上面的方法进行了改进，使用了IMU预积分，并使用增量平滑器联合视觉惯性状态，系统可以保存更多的标志，但因其处理输入的每一帧导致计算量很大。另一类是只使用视觉方法，SPM-SLAM10是一个可以实时快速处理标志的系统，但是只能处理标志，对环境中标志的数量要求高，若图像中没有标志则追踪失败。UcoSLAM11克服了上述工作对标志强依赖的问题，是一个点特征和标志特征结合的SLAM系统，可使用点特征或者标志特征单独进行定位，该系统有很好的稳定性。为了更有效利用标志，处理更多的标志和降低计算量，本文进一步探索标志存在的特性。在文献8-11中都是单独利用提取到的每一个标志对位姿进行约束，而忽略了标志常粘贴在环境中同一墙面或垂直墙面上的情况，发现这一特性可以有效利用，可加入面特征，联合同一平面内的标志位姿使之与该平面更靠近，并使垂直面间的标志互相垂直，平行面间的标志互相平行，这样有利于更好的约束标志特征与辅助相机定位。本文提出一种利用平面特征建立标志间关联的算法。该算法通过提取标志所在平面结构信息，使用点、标志、平面特征一起估计相机位姿，面特征显式利用了标志的平面属性，使用更少的参数表达更大的结构辅助定位；且建立标志间可能存在的共面、平行、垂直关系，在优化中对特征进行约束，从而减小累积的误差。算法使用图优化对位姿和特征进行优化，所以对标志特征的数量不做约束，实验结果也表明此算法满足SLAM系统对实时性的要求。1系统简介图1展示了所提出框架的流程，包括预处理、追踪、局部地图、闭环和地图管理五大模块。在此框架中，首先对输入的图像进行预处理，提取出关键点和标志信息。追踪模块利用点、标志、面特征对当前输入帧进行定位，当地图还未初始化时可同时分别用点和标志特征进行初始化，以及判断是否创建关键帧。局部地图模块根据标志信息，提取出环境面特征和建立其中包含的标志间关系，并对局部地图内的特征和位姿进行联合优化，若地图未包含尺度信息会进行尺度校正。闭环模块会进行检测和校正操作，使用标志优先判断是否形成回环，再对回环进行校正消除漂移误差，接着全局优化进一步修正位姿和特征。地图模块保存着所有关键帧和特征的信息，并完成建图任务。2多特征观测模型本章将介绍系统中使用的符号和特征，并对特征的观测模型进行建模。2.1符号坐标系定义：w代表世界坐标系，m代表标志坐标系，c代表相机坐标系。估计的 6 自由度位姿表示成 44 的转换矩阵TSE(3),T由旋转矩阵RSO(3)和平移向量t3组成。转换矩阵的下标代表了坐标系的转移，例如，Tcw代表了从世界坐标系到相机坐标系的转换关系。2.2特征2.2.1点特征为了快速提取图像中的特征并使特征具有一定的旋转和尺度不变性，本文使用提取更快速、准确的ORB特征点。特征点的二维观测为u=(u,v)T，对应的三维空间点p=(X,Y,Z)T3。点特征的集合P=p。本文使用ORB-SLAM24中特征点的提取与匹配策略。2.2.2标志特征标志是张贴在环境中的人造平面标识，其角点或边长相较点特征可以提供更为鲁棒的信息。从 Matrix12之后，有许多标志被设计使用，包括ARToolKit13、AptilTag14、Chromatag15、ArUco16-17、Circular Marker18，同时也有精度很高的u-LentiMark19。图2展示了各个标志，表1介绍了各标志的特点和不足，为了使用更方便、可区分、成本低、精度高的标志，选择使用ArUco标志。地图初始化关键帧地图点标志平面追踪关键帧判断重定位追踪预处理图像帧局部地图点的剔除和创建标志有效性判断平面提取和更新尺度校正局部地图优化关键帧剔除回环回环检测（标志优先）回环校正全局地图优化地图关键帧关键帧图1系统框架Fig.1System pipeline1882023，59（6）标志特征的集合为M=m。对于每一个标志特征，m=nid,l,Twm,nid为标志m的 ID 号，l为实际边长，Twm为标志坐标系到世界坐标系的转换矩阵。对于ID号为i的标志，构建以其自身中心为原点的标志坐标系，见图3，标志四个角点位置pi1=-l2,l2,0T,pi2=l2,l2,0T,pi3=l2,-l2,0T,pi4=-l2,-l2,0T四点对应在成像平面上的像素集合为ui，可通过标志四点在同一平面的性质得到从标志坐标系到相机坐标系的位姿TcmSE(3)，再利用看到该标志的第一帧得到标志的Twm，即Twm=T-1cwTcm。值得注意的是，标志特征中包含位姿信息而不是四点位置，是因为四个角点位置相对标志坐标系位置固定，优化标志位姿即可得角点位置，若优化角点位置，则需要加入标志在空间中为方形等更多约束，不利于优化。在追踪过程中，使用标志特征的ID号进行帧间数据关联。标志特征位姿计算属于基于平面的位姿估计（PPE），在PPE中存在着位姿模糊的问题20，这是由于在测量中易受噪声影响，无法准确地估计出平面到相机坐标系中z轴的转换。如图4，成像平面上的红框为带有噪声的测量值，空间蓝色平面为真实位姿Tm，空间绿色平面是可得相近重投影误差的模糊