CN117315547A - 一种解决动态物体大占比的视觉slam方法 - Google Patents

Abstract

本发明提出了一种解决动态物体大占比的视觉SLAM方法，旨在解决动态物体在图像中占比大时，SLAM算法的定位精度低和实时性差的问题，具体包括：首先输入连续图像帧到改进的轻量化目标检测网络(SG‑YOLOv5s)中，提取图像中动态物体语义信息，获得动态物体自适应锚框，同时提取ORB特征点，然后基于几何约束法剔除锚框内的动态特征点，利用PROSAC算法剔除锚框外的动态特征点，最后利用静态特征点进行位姿估计和建图。本发明准确有效的剔除了动态区域内的动态特征点，保留了静态特征点，减少了动态区域外的误匹配点，降低了动态环境对SLAM系统的影响，提高了系统的实时性，同时提升了系统的位姿估计和建图精度。

Description

一种解决动态物体大占比的视觉SLAM方法

技术领域

本发明属计算机视觉领域，尤其涉及一种解决动态物体大占比的视觉SLAM方法。

背景技术

近年来，视觉SLAM系统的研究发展迅速，视觉SLAM主要分为两类：特征点法和直接法。其中ORB-SLAM2是基于特征点法具有代表的方法，LSD-SLAM是基于直接法中的优秀算法。基于特征点和直接法的视觉SLAM算法在静态环境的假设下有定位精度高、鲁棒性好的特点，但在动态场景下，由于动态物体的特征点不稳定，视觉SLAM的位姿估计和建图精度下降，因此很难应用于动态物体占比大的场景中。针对以上问题，现有的动态视觉SLAM算法主要通过剔除动态特征点来提高系统的鲁棒性和相机位姿估计精度，现有方法能在一定程度上降低动态物体对系统性能的影响，但在动态物体占比大的情况下，仍存在位姿估计精度低和实时性差的问题。

论文《动态场景下融合YOLOv5s的视觉SLAM算法研究》中使用目标检测算法、光流法和几何约束方法相结合来剔除场景中的动态特征点，但论文中方法仅用于室内，而且此方法将检测到的动态目标特征点全部剔除，在动态物体占比较大时，保留的特征点过少，同样会导致SLAM精度下降的问题。

论文《面向复杂动态交通环境的视觉SLAM算法》中改进后的目标检测网络主要是对检测精度进行提升，网络内需要计算的参数数量仍然很大，在实时性问题上效果不佳，而且论文中仅对检测到的动态目标所在的运动区域进行了动静区分，没有考虑到目标检测误检或者漏检的情况。

论文《基于改进YOLOv5s的动态视觉SLAM算法》将目标检测和ORB-SLAM3相结合来解决动态环境下SLAM精度下降问题，但论文中方法仅适用于检测室内动态物体，没有考虑到室外环境下动态物体占比大的情况，并且论文中也没有考虑到目标检测算法误检、漏检的情况，对于检测到的动态物体区域外的特征点没有区分和剔除。

本发明结合了目标检测算法，对骨干网络、颈部网络和损失函数进行了改进，参数数量和模型规模显著降低，在保证检测精度的同时，有效提高了算法的运行速度。本发明考虑到了动态物体占比大情况，对检测到的动态目标区域内的特征点进行动静区分，剔除了动态区域内的动态特征点，尽可能多的保留了静态特征点，本发明还考虑到了目标检测算法出现错检、漏检的情况，对动态目标区域外的动态特征点和误匹配点进行了检测和剔除。本发明尽可能多的保留了静态特征点，提高了系统实时性的同时提升了系统的位姿估计准确度和建图精度。

发明内容

针对现有技术存在的在动态物体占比大的情况下精度不高，实际应用时实时性不好的问题，本发明提出了一种解决动态物体大占比的视觉SLAM方法，利用改进轻量化目标检测网络SG-YOLOv5s获得动态物体的语义信息和自适应锚框，去除锚框中的动态特征点，同时用PROSAC算法剔除锚框外的误匹配点和动态特征点，仅利用静态特征点进行同步定位与建图，有效提高了SLAM在动态环境下的精度和实时性。

为达到以上目的，本发明采用如下具体技术方案予以解决：

S1：获取输入的图像帧序列；

S2：对输入图像帧进行ORB特征点提取；

S3：改进轻量化目标检测网络YOLOv5s，在YOLOv5s的基准网络中，采用ShuffleNetV2网络的基本单元，记为S-1，代替原YOLOv5s中的CBS模块，采用ShuffleNetV2网络的下采样单元，记为S-2，代替原YOLOv5s中的CSP1_X模块，从而重新构建基准网络，在颈部网络中，用Ghost模块和GhostBottleneck模块替换原有的CBS模块，采用S-1模块替换CSP2_X模块，并且将定位损失函数CIoU替换成SIoU，改进后的轻量化目标检测网络记为SG-YOLOv5s，将图像输入到SG-YOLOv5s中进行动态物体检测，获得潜在运动物体语义信息和自适应锚框；

S4：基于S3获得的运动物体语义信息和运动物体锚框，将落在运动物体锚框内的特征点判定为潜在动态特征点，利用几何约束法将锚框内的特征点区分为静态特征点和动态特征点，剔除动态特征点，具体步骤如下：

S41：使用Lucas-Kanada光流法对特征点进行跟踪，LK光流法把图像看成了关于位置与时间的函数，基于像素在运动过程中的灰度值恒定的假设，可以得到：

I(x+dx,y+dy,t+dt)＝I(x,y,t) (1)

式中，I(x,y,t)为t时刻坐标为(x,y)的特征点的灰度值，I(x+dx,y+dy,t+dt)为t+dt时刻的后续帧中特征点的灰度值，对式(1)进行泰勒展开得：

式中，ε为高阶无穷小项，基于相邻两帧之间运动为小运动的假设，ε可以忽略不计，经过整理得：

式中，为像素在x轴的运动速度，记为u，/>为像素在y轴上的运动速度，记为v，为图像在点(x,y)处x方向的梯度，记为I_x，/>为图像在点(x,y)处y方向的梯度,记为I_y，为图像灰度对时间的变化量，记为I_t，将式(3)整理为矩阵形式得：

基于相邻像素具有相似的假设，可以通过计算出u和v，进而得到像素点在后续帧中的位置；

S42：获得相邻帧中的匹配特征点后，利用匹配特征点计算基础矩阵F；

S43：利用基础矩阵和特征点坐标可得到极线L,设p₁，p₂为一对匹配的特征点，齐次坐标为：

极线L的表达式如下式：

p₂到极线L的距离d可表示为：

当距离d大于预定阈值时，判断p₂为动态特征点；

S5：利用PROSAC算法判断并剔除检测到的动态物体锚框外的误匹配点和动态特征点，具体步骤如下：

S51：计算图像中两匹配特征点的最小欧氏距离d_min、距离比值λ和匹配度因子γ，距离比值λ的表达式如下：

式中，d_1min为图像中某特征点与相邻图像中其他特征点的最小距离，λ和d_min越小，特征匹配的正确率越高，采用匹配度因子γ对匹配点进行质量评估，γ的表达式如下：

γ越大，匹配点的质量越高，选取高质量点对，对其降序排列，计算其单应性矩阵H；

S52：由S51得到的高质量点对的单应性矩阵H，得出相邻图像中其他特征点的投影点，计算投影点与匹配点的误差，若误差小于阈值，归为内点，否则归为外点；

S53：若内点数量小于内点数量阈值时，增加迭代次数，重复S51和S52，若内点数量达到阈值，则退出迭代；

S54：采用达到迭代阈值的内点重新计算单应性矩阵，得到模型估计的内点集合和单应性矩阵H；

S6：利用静态特征点进行位姿估计或重定位，进行局部地图跟踪，确定关键帧，传输给局部建图线程；

S7：接收S6传输的关键帧，完成关键帧的插入，利用关键帧的共视关系生成新的地图点，搜索融合相邻关键帧的地图点，然后用局部光束平差法进行局部地图优化，筛选插入的关键帧并剔除多余关键帧，最后将处理后的关键帧发送给闭环检测线程；

S8：接收S7处理后的关键帧，先进行闭环检测流程，利用词袋寻找并验证闭环候选关键帧，计算当前位姿的Sim(3)相似变换，然后进行闭环矫正流程，通过之前计算的Sim(3)变换，进行位姿和地图点传播和矫正，最后对所有的地图点和关键帧位姿进行全局BA优化。

本发明具有如下有益效果：

1.本发明利用改进轻量化目标检测网络SG-YOLOv5s检测动态目标，对检测到的动态目标区域内的特征点进行动静区分，利用几何约束法剔除了动态区域内的动态特征点，利用PROSAC算法剔除动态区域外的误匹配点和动态特征点，尽可能多的保留了静态特征点，有效解决了动态物体占比大时SLAM系统位姿估计和建图精度下降的问题；

2.本发明改进轻量化目标检测网络，使网络计算的参数量和网络模型规模均显著降低，利用改进的轻量化目标检测网络SG-YOLOv5s检测动态目标，在保证检测效果的同时提升了检测效率，有效提高了动态物体占比大时SLAM系统的实时性；

3.与传统动态SLAM算法比较，相比于ORB-SLAM2算法，本发明的绝对轨迹误差减少了95％以上，与DS-SLAM算法相比，本发明的绝对轨迹误差减少了35.66％，本发明的平均运行时间降低了44.7％。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为SLAM系统的整体流程框图；

图2为SG-YOLOv5s网络结构图；

图3为GhostBottleneck结构图；

图4为目标检测效果图；

图5为ORB-SLAM2运行效果图；

图6为本发明的SLAM系统运行效果图。

具体实施方式

为使本发明的上述目的、特征和优点更加明显易懂，一种解决动态物体大占比的视觉SLAM方法，包括以下步骤：

S1：获取输入的图像帧序列；

S2：对输入图像帧进行ORB特征点提取；

S3：改进轻量化目标检测网络YOLOv5s，在YOLOv5s的基准网络中，采用ShuffleNetV2网络的基本单元，记为S-1，代替原YOLOv5s中的CBS模块，采用ShuffleNetV2网络的下采样单元，记为S-2，代替原YOLOv5s中的CSP1_X模块，从而重新构建基准网络，在颈部网络中，用Ghost模块和GhostBottleneck模块替换原有的CBS模块，GhostBottleneck模块结构如图3所示，采用S-1模块替换CSP2_X模块，并且将定位损失函数CIoU替换成SIoU，改进后的轻量化目标检测网络记为SG-YOLOv5s，网络结构如图2所示，将图像输入到SG-YOLOv5s中进行动态物体检测，获得潜在运动物体语义信息和自适应锚框；

S4：基于S3获得的运动物体语义信息和运动物体锚框，将落在运动物体锚框内的特征点判定为潜在动态特征点，利用几何约束法将锚框内的特征点区分为静态特征点和动态特征点，剔除动态特征点，具体步骤如下：

S41：使用Lucas-Kanada光流法对特征点进行跟踪，LK光流法把图像看成了关于位置与时间的函数，基于像素在运动过程中的灰度值恒定的假设，可以得到：

I(x+dx,y+dy,t+dt)＝I(x,y,t) (1)

式中，I(x,y,t)为t时刻坐标为(x,y)的特征点的灰度值，I(x+dx,y+dy,t+dt)为t+dt时刻的后续帧中特征点的灰度值，对式(1)进行泰勒展开得：

式中，ε为高阶无穷小项，基于相邻两帧之间运动为小运动的假设，ε可以忽略不计，经过整理得：

式中，为像素在x轴的运动速度，记为u，/>为像素在y轴上的运动速度，记为v，为图像在点(x,y)处x方向的梯度，记为I_x，/>为图像在点(x,y)处y方向的梯度,记为I_y，为图像灰度对时间的变化量，记为I_t，将式(3)整理为矩阵形式得：

基于相邻像素具有相似的假设，可以通过计算出u和v，进而得到像素点在后续帧中的位置；

S42：获得相邻帧中的匹配特征点后，利用匹配特征点计算基础矩阵F；

S43：利用基础矩阵和特征点坐标可得到极线L,设p₁，p₂为一对匹配的特征点，齐次坐标为：

极线L的表达式如下式：

p₂到极线L的距离d可表示为：

若距离d大于预定阈值时，则将特征点p₂判断为动态特征点；

S5：利用PROSAC算法判断并剔除检测到的动态物体锚框外的误匹配点和动态特征点，具体步骤如下：

S51：计算图像中两匹配特征点的最小欧氏距离d_min、距离比值λ和匹配度因子γ，距离比值λ的表达式如下：

式中，d_1min为图像中某特征点与相邻图像中其他特征点的最小距离，λ和d_min越小，特征匹配的正确率越高，采用匹配度因子γ对匹配点进行质量评估，γ的表达式如下：

γ越大，匹配点的质量越高，选取高质量点对，对其降序排列，计算其单应性矩阵H；

S52：由S51得到的高质量点对的单应性矩阵H，得出相邻图像中其他特征点的投影点，计算投影点与匹配点的误差，若误差小于阈值，归为内点，否则归为外点；

S53：若内点数量小于内点数量阈值时，增加迭代次数，重复S51和S52，若内点数量达到阈值，则退出迭代；

S54：采用达到迭代阈值的内点重新计算单应性矩阵，得到模型估计的内点集合和单应性矩阵H；

S6：利用静态特征点进行位姿估计或重定位，进行局部地图跟踪，确定关键帧，传输给局部建图线程；

S7：接收S6传输的关键帧，完成关键帧的插入，利用关键帧的共视关系生成新的地图点，搜索融合相邻关键帧的地图点，然后用局部光束平差法进行局部地图优化，筛选插入的关键帧并剔除多余关键帧，最后将处理后的关键帧发送给闭环检测线程；

S8：接收S7处理后的关键帧，先进行闭环检测流程，利用词袋寻找并验证闭环候选关键帧，计算当前位姿的Sim(3)相似变换，然后进行闭环矫正流程，通过之前计算的Sim(3)变换，进行位姿和地图点传播和矫正，最后对所有的地图点和关键帧位姿进行全局BA优化。

如图4所示，改进的轻量化目标检测网络SG-YOLOv5s在KITTI数据集中能够准确识别动态目标。如图5所示，在动态物体占比大时，ORB-SLAM2算法没有完全剔除动态物体上的动态特征点，动态区域外的误匹配点较多。如图6所示，动态物体占比大时，本发明能有效剔除动态特征点，准确保留动态区域内的静态特征点，极大地减少了动态区域外的误匹配特征点数量，验证了本发明的有效性。在动态物体占比大的场景中，ORB-SLAM2绝对轨迹误差的均方根误差为0.3512，DS-SLAM的绝对轨迹误差的均方根误差为0.0258，本发明绝对轨迹误差的均方根误差为0.0166，相比于ORB-SLAM2算法，本发明的绝对轨迹误差减少了95％以上，相比于DS-SLAM算法，本发明的绝对轨迹误差减少了约35.66％。DS-SLAM算法的平均运行时间为73.44ms，本发明平均运行时间为40.61ms，本发明的平均运行时间降低了约44.7％。本发明在实验设备(CPU为Intel Core i5-12500H，显卡为Nvidia GeForceRTX3050Ti)上能够实时运行。以上所述具体实施方案，对本发明的发明目的、技术方案和有益效果进行了进一步说明，以上实施例仅用于说明本发明的技术方案，而非对本发明创造保护范围的限制，本领域的普通技术人员应当理解，凡在本发明的技术方案进行修改、等同替换，均包含在本发明的保护范围内。

Claims (1)

1.一种解决动态物体大占比的视觉SLAM方法，其特征在于，包括以下步骤：

S1：获取输入的图像帧序列；

S2：提取输入图像帧的ORB特征点；

S3：改进轻量化目标检测网络YOLOv5s，引入ShuffleNetV2网络基本单元和Ghost卷积模块来改进骨干网络和颈部网络，并且将定位损失函数CIoU替换成SIoU，改进后的轻量化目标检测网络记为SG-YOLOv5s，将图像输入到SG-YOLOv5s中检测运动物体，获得潜在运动物体语义信息和自适应锚框；

S4：基于S3获得的运动物体语义信息和运动物体锚框，将落在运动物体锚框内的特征点判定为潜在动态特征点，利用几何约束法将锚框内的特征点区分为静态特征点和动态特征点，剔除动态特征点，具体步骤如下：

S41：使用Lucas-Kanada光流法对特征点进行跟踪，LK光流法把图像看成了关于位置与时间的函数，基于像素在运动过程中的灰度值恒定的假设，可以得到：

I(x+dx,y+dy,t+dt)＝I(x,y,t) (1)

式中，I(x,y,t)为t时刻坐标为(x,y)的特征点的灰度值，I(x+dx,y+dy,t+dt)为t+dt时刻的后续帧中特征点的灰度值，对式(1)进行泰勒展开得：

式中，ε为高阶无穷小项，基于相邻两帧之间运动为小运动的假设，ε可以忽略不计，经过整理得：

式中，为像素在x轴的运动速度，记为u，/>为像素在y轴上的运动速度，记为v，/>为图像在点(x,y)处x方向的梯度，记为I_x，/>为图像在点(x,y)处y方向的梯度,记为I_y，/>为图像灰度对时间的变化量，记为I_t，将式(3)整理为矩阵形式得：

基于相邻像素具有相似的假设，可以通过计算出u和v，进而得到像素点在后续帧中的位置；

S42：获得相邻帧中的匹配特征点后，利用匹配特征点计算基础矩阵F；

S43：利用基础矩阵和特征点坐标可得到极线L,设p₁，p₂为一对匹配的特征点，齐次坐标为：

极线L的表达式如下式：

p₂到极线L的距离d可表示为：

当距离d大于预定阈值时，判断p₂为动态特征点；

S5：利用PROSAC算法判断并剔除检测到的动态物体锚框外的误匹配点和动态特征点，具体步骤如下：

S51：计算图像中两匹配特征点的最小欧氏距离d_min、距离比值λ和匹配度因子γ，距离比值λ的表达式如下：

式中，d_1min为图像中某特征点与相邻图像中其他特征点的最小距离，λ和d_min越小，特征匹配的正确率越高，采用匹配度因子γ对匹配点进行质量评估，γ的表达式如下：

γ越大，匹配点的质量越高，选取高质量点对，对其降序排列，计算其单应性矩阵H；

S52：由S51得到的高质量点对的单应性矩阵H，得出相邻图像中其他特征点的投影点，计算投影点与匹配点的误差，若误差小于阈值，归为内点，否则归为外点；

S53：若内点数量小于内点数量阈值时，增加迭代次数，重复S51和S52，若内点数量达到阈值，则退出迭代；

S54：采用达到迭代阈值的内点重新计算单应性矩阵，得到模型估计的内点集合和单应性矩阵H；

S6：利用静态特征点进行位姿估计或重定位，进行局部地图跟踪，确定关键帧，传输给局部建图线程；

S7：接收S6传输的关键帧，完成关键帧的插入，利用关键帧的共视关系生成新的地图点，搜索融合相邻关键帧的地图点，然后用局部光束平差法进行局部地图优化，筛选插入的关键帧并剔除多余关键帧，最后将处理后的关键帧发送给闭环检测线程；

S8：接收S7处理后的关键帧，先进行闭环检测流程，利用词袋寻找并验证闭环候选关键帧，计算当前位姿的Sim(3)相似变换，然后进行闭环矫正流程，通过之前计算的Sim(3)变换，进行位姿和地图点传播和矫正，最后对所有的地图点和关键帧位姿进行全局BA优化。