CN116563341A - 一种处理复杂环境下动态物体的视觉定位与建图方法 - Google Patents
一种处理复杂环境下动态物体的视觉定位与建图方法 Download PDFInfo
- Publication number
- CN116563341A CN116563341A CN202310528902.1A CN202310528902A CN116563341A CN 116563341 A CN116563341 A CN 116563341A CN 202310528902 A CN202310528902 A CN 202310528902A CN 116563341 A CN116563341 A CN 116563341A
- Authority
- CN
- China
- Prior art keywords
- map
- key frame
- points
- current
- frame
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 53
- 238000013507 mapping Methods 0.000 title claims abstract description 31
- 230000000007 visual effect Effects 0.000 title claims abstract description 27
- 238000012545 processing Methods 0.000 title claims abstract description 15
- 230000004927 fusion Effects 0.000 claims abstract description 65
- 238000001514 detection method Methods 0.000 claims abstract description 35
- 238000005457 optimization Methods 0.000 claims abstract description 31
- 230000003068 static effect Effects 0.000 claims abstract description 26
- 230000011218 segmentation Effects 0.000 claims abstract description 18
- 238000013528 artificial neural network Methods 0.000 claims abstract description 4
- 230000008569 process Effects 0.000 claims description 18
- 238000012795 verification Methods 0.000 claims description 15
- 238000012360 testing method Methods 0.000 claims description 9
- 230000004807 localization Effects 0.000 claims description 8
- 239000011159 matrix material Substances 0.000 claims description 7
- 230000009466 transformation Effects 0.000 claims description 4
- 238000007689 inspection Methods 0.000 claims description 3
- 230000003044 adaptive effect Effects 0.000 claims description 2
- 230000033001 locomotion Effects 0.000 description 9
- 230000000694 effects Effects 0.000 description 5
- 230000007774 longterm Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 238000013527 convolutional neural network Methods 0.000 description 3
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 238000003062 neural network model Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 1
- 238000012217 deletion Methods 0.000 description 1
- 230000037430 deletion Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/02—Neural networks
- G06N3/04—Architecture, e.g. interconnection topology
- G06N3/0464—Convolutional networks [CNN, ConvNet]
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/02—Neural networks
- G06N3/08—Learning methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30244—Camera pose
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
本发明公开了一种处理复杂环境下动态物体的视觉定位与建图方法，包括：获取图像数据中的ORB特征点并基于神经网络对图像数据进行实例分割；计算语义ORB特征点；将语义ORB特征点分为静态特征点与潜在动态特征点，基于静态特征点匹配结果进行帧间位姿估计，并根据特征点匹配的自适应阈值极线约束算法进行动态特征点剔除，获得特征点队列；根据特征点队列跟踪相机位姿，获得帧间位姿信息，根据局部地图进一步跟踪并优化位姿信息；根据关键帧策略选取关键帧，并基于关键帧进行地图点生成和当前关键帧的局部地图优化；选择并生成回环与地图融合候选关键帧，基于改进的共同区域检测算法进行回环优化与地图融合，获得优化后的关键帧位姿与全局地图。
Description
技术领域
本发明涉及视觉处理及状态估计技术领域，更具体的说是涉及一种处理复杂环境下动态物体的视觉定位与建图方法。
背景技术
SLAM(Simultaneous localization andmapping)即同步定位与建图，指的是在未知环境中，机器人或车辆通过感知外部环境信息来确定自身位姿、并以自身位姿为基础完成环境建图。
在智能驾驶、自主移动机器人和无人机等实际应用过程中，现有视觉SLAM方案一些关键问题需要解决。其中一个关键问题是在复杂环境中，动态物体会影响视觉定位与建图算法的整个过程，由于经典SLAM算法是假设环境是静态的，那么动态物体的出现就会被算法理解为自身位姿改变引起观察到的路标在像素坐标系中坐标的改变。另外，复杂环境中其他因素，如人为移动了某些物体，也会影响视觉定位与建图算法的结果，如果对上述因素进行针对性处理，就可以实现更准确地定位与建图。
综上，如何提供一种处理复杂环境下动态物体的视觉定位与建图方法是本领域技术人员亟需解决的问题。
发明内容
有鉴于此，本发明提供了一种处理复杂环境下动态物体的视觉定位与建图方法，实现了复杂环境下动态物体的视觉定位与建图。
为了实现上述目的，本发明采用如下技术方案：
一种处理复杂环境下动态物体的视觉定位与建图方法，包括：
获取图像数据，提取图像数据中的ORB特征点并基于神经网络对图像数据进行实例分割；
基于ORB特征点和实例分割结果计算语义ORB特征点；
将语义ORB特征点分为静态特征点与潜在动态特征点，基于静态特征点匹配结果进行帧间位姿估计，并根据特征点匹配的自适应阈值极线约束算法从潜在动态特征点中剔除动态特征点，获得用于定位与建图的特征点队列Kp；
基于特征点队列Kp得到当前帧的局部地图，根据特征点队列Kp跟踪相机位姿，获得帧间位姿信息Pi，根据当前帧的局部地图进一步跟踪并优化位姿信息；
根据关键帧策略选取关键帧KF，并基于关键帧KF进行地图点生成和当前关键帧的局部地图优化；
从关键帧KF中选择并生成回环与地图融合候选关键帧KFl，基于改进的共同区域检测算法进行共同区域检测，共同区域检测成功后根据回环与地图融合候选关键帧KFl对应的位姿信息进行回环优化与地图融合，获得优化后的关键帧位姿PKi与全局一致性地图。
优选地，基于ORB特征点和实例分割结果计算语义ORB特征点具体包括：
将实例分割结果中各目标语义及掩膜作为提取语义ORB特征点的参数；
根据ORB特征点位置读取各目标语义及掩膜对应的像素值，再根据实例分割结果确定各目标语义对应的特征点，得到ORB特征点。
优选地，基于静态特征点匹配结果进行帧间位姿估计，并根据特征点匹配的自适应阈值极线约束算法从潜在动态特征点中剔除动态特征点，具体包括：
静态特征点与潜在动态特征点分别进行特征点匹配获得静态特征点队列和潜在动态特征点队列；
基于静态特征点队列进行帧间位姿估计，获得基础矩阵；
根据特征点匹配的最小距离区间确定极线约束阈值；
根据基础矩阵和上一帧潜在动态特征点队列的特征点像素坐标，计算上一帧潜在动态特征点队列在当前帧的极线方程；
计算当前帧潜在动态特征点队列与极线方程在像素坐标系的距离，根据极线约束阈值判断是否为动态特征点，若距离大于等于极线约束阈值则是，否则将潜在动态特征点从队列中删除。
优选地，关键帧策略包括：
a.当前帧距离上一关键帧产生了超过25个普通帧；
b.当前帧与参考关键帧跟踪到地图点的比例小于0.85；
c.当前帧距离重定位超过1s；
d.当前帧跟踪到的的近点少于50个，近点是指深度小于相机基线长度40倍的特征点；
e.成功跟踪到的特征匹配点数大于15；
f.待处理的关键帧数量不超过2帧；
g.在剔除动态特征点超过一定数量m时，提高当前帧与参考关键帧跟踪到的地图点比例阈值，使得b策略更易于判断成功；
根据上述策略，决定是否为关键帧的判断方法为：满足策略a.c.d之一，以及满足条件b.d之一，并且同时满足策略e和f则为关键帧，g策略是在动态特征点剔除时对b策略作调整。
优选地，地图点生成具体过程为：
从关键帧队列中取出一帧待处理关键帧，计算其ORB特征点对应的单词特征；
剔除在跟踪过程中初始地图中不合格的地图点；
基于一定数量的共视关键帧与当前关键帧的匹配关系，进行三角化生成三维点；
对三维点作质量检验后确定为当前关键帧的地图点，添加关键帧与其地图点的观测关系。
优选地，当前关键帧的局部地图优化具体过程为：
通过当前关键帧的一级、二级关键帧以及各关键帧对其地图点的观测关系，对当前关键帧的局部地图中所有地图点坐标和当前关键帧的一级关键帧位姿做局部BA优化；
局部BA优化时，假设参与优化的变量中当前关键帧的局部地图点有n个，观测到当前关键帧的局部地图的关键帧有m个，使用最小化重投影误差的思想构建BA代价函数CostFunction：
其中，h(ξj,pj)是基于相机观测模型得到第j个地图点对应的图像中ORB特征点的像素坐标，相机位姿对应的李代数为ξi，路标对应的地图点坐标pj，i,j分别表示关键帧编号与当前关键帧的局部地图点编号。zij为预测数据，利用跟踪阶段估计得到的帧间位姿信息Pi与路标点空间坐标Mi得到，K为相机内参，si为尺度因子。
优选地，基于改进的共同区域检测算法进行共同区域检测包括：
根据回环与地图融合候选关键帧与当前关键帧是否处于同一全局地图，将所有的回环与地图融合候选关键帧KFl分成回环候选关键帧组和地图融合候选关键帧组；
在回环与地图融合候选关键帧中，寻找与当前关键帧没有连接关系且具有公共单词特征的所有回环与地图融合候选关键帧，记为KeyFramCand，计算KeyFramCand中各候选关键帧与当前关键帧共同单词数的最大值，该值的0.8倍记为minCommonWords，最后KeyFramCand中只保留与当前关键帧共同单词数大于minCommonWords的部分；
计算上述KeyFrameCand中各个具有共视关系的关键帧组的共同单词总数，选取前3个组中共同单词数最多的单个回环与地图融合候选关键帧作为初始候选关键帧；
利用词袋模型计算初始候选关键帧与当前关键帧的相对位姿变换，再利用投影匹配的方法优化二者的相对位姿Tam；
基于相对位姿Tam，利用当前关键帧共视程度最高的5个回环与地图融合候选关键帧对KeyFrameCand进行几何一致性检验，如果几何一致性检验成功则共同区域检测成功，根据当前回环与地图融合候选关键帧处于回环候选关键帧组或地图融合候选关键帧组，确定进行回环优化与地图融合。
优选地，共同区域检测成功后根据生成回环与地图融合候选关键帧KFl对应的位姿信息进行回环优化与地图融合，获得优化后的关键帧位姿PKi与全局一致性地图，包括：
对回环与地图融合候选关键帧中的半静态特征点及半静态特征点对应的地图点进行处理，分别标记半静态特征点及半静态特征点对应的地图点的首次观测时间戳，在当前回环与地图融合候选关键帧进行共同区域检测前，根据首次观测时间戳与当前时间的对比，将达到条件的半静态特征点及半静态特征点对应的地图点删除；
当前回环与地图融合候选关键帧KFl如果不在当前全局地图，则进行地图融合，若在当前全局地图，则进行回环优化，获取全局一致性地图，并消除定位的累计误差，获得优化后的关键帧位姿PKi。
优选地，对半静态特征点及半静态特征点对应的地图点进行处理，包括：
将当前回环与地图融合候选关键帧分别与共同区域检测用到的回环候选关键帧组和地图融合候选关键帧组进行时间间隔比较，对删除的半静态特征点对应的地图点做标记；
将上述半静态特征点对应的地图点加入到半静态地图点待删除队列中；
遍历半静态地图点待删除队列，对每个半静态特征点对应的地图点标记数进行检查，当标记数大于等于总观测数的50％后，认定该半静态特征点已经超过了设定的有效时间，将其在全局地图中删除。
优选地，几何一致性检验过程为：
利用投影匹配判断是否有共视关系，5个中有3个回环与地图融合候选关键帧检验成功则证明共同区域检测成功；若验证成功数大于0小于3，则进入时序几何校验：时间上连续新增的关键帧重复上述检验，验证成功数达到3时，证明共同区域检测成功，若连续两个新增关键帧都时序几何校验失败，则当前回环与地图融合候选关键帧验证失败。
本发明具有以下优点：
(1)对于复杂环境中AGV运行问题，动态物体以及半静态物体会影响AGV定位与建图效果，本发明结合实例分割网络Mask R-CNN，提出一种可去除动态物体且优化中长期数据关联的视觉SLAM算法。
(2)针对环境中的动态物体，提出基于语义信息的动态特征点去除方法，提取特征点时利用实例分割结果获取语义特征点；抑制潜在动态特征利用特征点匹配来估计帧间位姿；利用求得的基础矩阵对潜在动态特征点使用自适应阈值的极线约束方法判定其状态，实现了较精确地剔除不同运动程度的特征点。
(3)为使算法在中长期数据关联中保持较优效果，根据动态特征去除情况改进关键帧选取策略，进而提升了局部建图效果，提高了跟踪稳定性和整体定位精度；针对复杂环境中易被人为移动的静态物体，在回环与地图融合候选帧中结合时间一致性处理半静态信息，避免长期数据关联时半静态物体的移动对系统精度的影响，提高了回环优化效果与地图复用性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1附图为本发明提供的处理复杂环境下动态物体的视觉定位与建图方法流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例公开了一种处理复杂环境下动态物体的视觉定位与建图方法，如图1所示，包括：
获取图像数据，提取图像数据中的ORB特征点并基于神经网络对图像数据进行实例分割；
基于ORB特征点和实例分割结果计算语义ORB特征点；
将语义ORB特征点分为静态特征点与潜在动态特征点，基于静态特征点匹配结果进行帧间位姿估计，并根据特征点匹配的自适应阈值极线约束算法从潜在动态特征点中剔除动态特征点，获得用于定位与建图的特征点队列Kp；
基于特征点队列Kp得到当前帧的局部地图，根据特征点队列Kp跟踪相机位姿，获得帧间位姿信息Pi，根据当前帧的局部地图进一步跟踪并优化位姿信息；
根据关键帧策略选取关键帧FK，并基于关键帧FK进行地图点生成和当前关键帧的局部地图优化；
从关键帧FK中选择并生成回环与地图融合候选关键帧KFl，基于改进的共同区域检测算法进行共同区域检测，共同区域检测成功后根据生成的回环与地图融合候选关键帧KFl对应的位姿信息进行回环优化与地图融合，获得优化后的关键帧位姿PKi与全局一致性地图。
在本实施例中，基于Mask R-CNN实例分割神经网络模型获取图像中各目标语义信息，针对环境中的动态目标，利用Mask R-CNN实例分割神经网络模型识别出潜在动态目标后，得到各目标的实例分割结果。
在本实施例中，语义ORB特征点分为潜在动态、半静态和纯静态三种，其中半静态语义指的是容易被人为移动的物体。
基于静态特征点匹配结果进行帧间位姿估计，并根据特征点匹配的自适应阈值极线约束算法从潜在动态特征点中剔除动态特征点，包括：
静态特征点与潜在动态特征点分别进行特征点匹配获得静态特征点队列和潜在动态特征点队列；
基于静态特征点队列进行帧间位姿估计，获得基础矩阵；
根据特征点匹配的最小距离区间确定极线约束阈值；
根据基础矩阵和上一帧潜在动态特征点队列的特征点像素坐标，计算上一帧潜在动态特征点队列在当前帧的极线方程；
计算当前帧潜在动态特征点队列与极线方程在像素坐标系的距离，根据极线约束阈值判断是否为动态特征点，若距离大于等于极线约束阈值则是，否则将潜在动态特征点从队列中删除。
本发明方法利用特征点的语义信息和相机运动过程中的几何约束，将潜在动态特征点进行动静态判断，可以保留更多的静态特征点用于定位，并且，基于自适应阈值极线约束算法进行动态特征点剔除，实现了精确剔除不同运动程度的特征点，因此可以提高定位与建图算法的精确度和鲁棒性。
在本实施例中，动态特征点剔除后，系统正式进入跟踪过程，先利用帧间匹配估计位姿(Scan-Scan)，作为第一阶段跟踪.。初始位姿跟踪用到三种模型：恒速运动模型估计位姿、参考帧估计位姿、重定位估计位姿。即检测系统是否完成初始化，系统初始化会得到局部地图，完成初始化后，使用恒速模型、参考关键帧跟踪模型、重定位跟踪模型三种方法，根据特征点队列跟踪相机位姿，获取帧间位姿变换。
第一阶段跟踪成功后将进入当前帧的局部地图跟踪，与第一阶段跟踪不同的是，当前帧的局部地图跟踪思想是基于ORB特征点和地图点的匹配(Scan-Map)。由于局部优化后的当前帧位姿以及局部地图点坐标的漂移被纠正，误差较小，因此在当前帧的局部地图跟踪阶段利用当前帧之前的关键帧对局部地图的观测信息进一步优化位姿，可以实现更高的当前帧定位精度。
在局部地图跟踪并优化当前帧位姿后，统计匹配并跟踪成功的特征点总数目来判断是否跟踪成功。
在本实施例中，根据关键帧策略选取关键帧，每个关键帧会计算更新当前关键帧的局部地图，获得当前关键帧的局部地图,各当前关键帧的局部地图组成全局地图，具体指标为：
关键帧策略包括：
a.当前帧距离上一关键帧产生了超过25个普通帧；
b.当前帧与参考关键帧跟踪到地图点的比例小于0.85，参考关键帧是关键帧中选出来的，与当前帧的关系是，参考关键帧是关键帧中与当前帧的共视关系最强的关键帧，一般是当前帧的上一个关键帧；
c.当前帧距离重定位超过1s；
d.当前帧跟踪到的的近点少于50个，近点是指深度小于相机基线长度40倍的特征点；
e.成功跟踪到的特征匹配点数大于15；
f.待处理的关键帧数量不超过2帧；
g.在剔除动态特征点超过一定数量m时，提高当前帧与参考关键帧跟踪到的地图点比例阈值，使得b策略更易于判断成功；
根据上述策略，决定是否为关键帧的判断方法为：满足策略a.c.d之一，以及满足条件b.d之一，并且同时满足策略e和f则为关键帧，即(a||c||d)&&(b||d)&&e&&f，g策略是在动态特征点剔除时对b策略作调整。
在本实施例中，地图点生成具体过程为：
从关键帧队列中取出一帧待处理关键帧，计算其ORB特征点对应的单词特征；
剔除在跟踪过程中初始地图中不合格的地图点；
基于一定数量的共视关键帧与当前关键帧的匹配关系，进行三角化生成三维点；
对三维点作质量检验后确定为当前关键帧的地图点，添加关键帧与其地图点的观测关系。
进一步地，上述过程中会出现同一路标点被不同关键帧观测时生成了多个地图点。因此，要进行检查并融合当前关键帧与相邻关键帧的地图点，此过程要完成合并重复的地图点，统一更新地图点的描述子、深度、平均观测方向等属性。
在本实施例中，当前关键帧的局部地图优化具体过程为：
通过当前关键帧的一级、二级关键帧，当前关键帧以及当前关键帧的一级、二级关键帧对地图点的观测关系，对当前关键帧的局部地图中所有地图点坐标和当前关键帧的一级关键帧位姿做局部BA优化；
局部BA优化时，假设参与优化的变量中当前关键帧的局部地图点有n个，观测到当前关键帧的局部地图的关键帧有m个，使用最小化重投影误差的思想构建BA代价函数：
其中，一级关键帧指与当前关键帧具有直接共视关系的关键帧，二级关键帧为“间接共视关系”：一级关键帧中观测到的其他地图点形成直接共视关系的关键帧。
在本实施例中，当复杂环境中有暂时静止的潜在动态目标以及易被人为移动目标(本发明定义其为半静态目标)，如静止的AGV车辆和仓库中的货物，它们不会在动态特征剔除阶段被去除，因此会干扰视觉SLAM算法的中长期数据关联，在一段时间后上述两类目标的移动可能会导致回环检测准确率降低、检测出回环或地图融合后参与优化的地图点位置改变引起的系统精度降低等。考虑到半静态目标对长期数据关联的影响，提出一种基于语义特征点的长期数据关联方法，即从关键帧KF中选择并生成回环与地图融合候选关键帧FKl，基于改进的共同区域检测算法进行共同区域检测，共同区域检测成功后根据回环与地图融合候选关键帧FKl对应的位姿信息进行回环优化与地图融合，主要步骤为：
基于语义特征点，对回环与地图融合检测的候选关键帧中的半静态特征点及其对应的地图点进行处理，分别标记半静态特征点及其对应的地图点的首次观测时间戳，在当前回环与地图融合候选关键帧进行共同区域检测前，根据时间关系制定抑制策略，即根据首次观测时间戳与当前时间的对比将达到条件的半静态特征点对应的地图点删除；
当前回环与地图融合候选关键帧KFl如果不在当前全局地图，则进行地图融合，若在当前全局地图，则进行回环优化，获取全局一致性地图，并消除定位的累计误差，获得优化后的关键帧位姿PKi
在本实施例中，基于语义特征点的半静态信息处理：
在复杂环境中，半静态物体的移动具有随机性，可能是暂时静止，也可能会长期保持静止，因此将当前系统时间戳加入到关键帧信息中，在回环与地图融合检测前，作出如下改进：
1.当前回环与地图融合候选关键帧与共同区域检测用到的回环候选关键帧组和地图融合候选关键帧组进行时间间隔比较，时间间隔比较为首次观测时间与当前时间的比较，对删除的半静态特征点对应的地图点做标记；
2.将上述半静态地图点加入到半静态地图点待删除队列中；
3.遍历半静态地图点删除队列，对每个半静态特征点对应的地图点标记数进行检查，当标记数大于等于总观测数的50％后，认定该半静态特征点已经超过了设定的有效时间，将其在全局地图中删除。
在本实施例中，改进的共同区域检测算法包括：
基于上述长期数据中半静态信息的处理，对当前帧进行共同区域检测。共同区域检测是为了同时检测机器人运动过程中可能发生的闭环以及与历史地图的融合，具体任务是从候选帧中找到与当前关键帧有回环或地图融合关系的关键帧。算法流程如下：
根据回环与地图融合候选关键帧与当前关键帧是否处于同一全局地图，将所有回环与地图融合候选关键帧分成回环候选关键帧组和地图融合候选关键帧组；
在回环与地图融合候选关键帧中，寻找与当前关键帧没有连接关系且具有公共单词特征的所有回环与地图融合候选关键帧，记为KeyFramCand，计算KeyFramCand各候选关键帧与当前关键帧共同单词数的最大值，该值的0.8倍记为minCommonWords，最后KeyFramCand中只保留与当前关键帧共同单词数大于minCommonWords的部分；
计算上述KeyFrameCand中各个具有共视关系的关键帧组的共同单词总数，选取前3个组中共同单词数最多的单个回环与地图融合候选关键帧作为初始候选关键帧；
利用词袋模型计算初始候选关键帧与当前关键帧的相对位姿变换，再利用投影匹配的方法优化二者的相对位姿Tam；
基于上一步求得的相对位姿，使用当前关键帧共视程度最高的5个回环与地图融合候选关键帧对KeyFrameCand进行几何一致性检验：即利用投影匹配判断是否有共视关系，5个中有3个检验成功则证明共同区域检测成功，根据当前回环与地图融合候选关键帧处于回环候选关键帧组或地图融合候选关键帧组，确定进行回环优化与地图融合；若验证成功数大于0小于3，则进入时序几何校验：时间上连续新增的关键帧重复上述检验，验证成功数达到3时，证明共同区域检测成功，若连续两个新增关键帧都时序几何校验失败，则当前回环与地图融合候选关键帧验证失败。
共同区域检测成功后，如果共同区域是当前关键帧与当前活跃地图中关键帧建立的匹配，则进行回环优化；如果共同区域是当前关键帧与历史地图中关键帧建立的匹配，则进行地图融合；如果同时检测到地图融合和回环，则忽略回环，由与地图融合通常涉及到更大范围的数据关联，且地图融合后可获取全局一致的更完整地图，因此地图融合优先级更高。
本发明优化了关键帧选取策略：考虑动态特征点删除导致的特征点减少，进而导致局部地图点更加稀疏，鉴于此作相应调整，使得动态特征点剔除时，选取的关键帧数量在合理范围内增加，保证了跟踪的稳定性以及局部优化效果；优化了回环与地图融合检测算法：在回环与地图融合候选帧中标记半静态特征点的时间戳，在共同区域检测时根据时间戳判断是否需要抑制半静态特征点与地图点，从而避免了人为移动的半静态物体对定位与建图结果的影响。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
Claims (10)
1.一种处理复杂环境下动态物体的视觉定位与建图方法，其特征在于，包括：
获取图像数据，提取图像数据中的ORB特征点并基于神经网络对图像数据进行实例分割；
基于ORB特征点和实例分割结果计算语义ORB特征点；
将语义ORB特征点分为静态特征点与潜在动态特征点，基于静态特征点匹配结果进行帧间位姿估计，并根据特征点匹配的自适应阈值极线约束算法从潜在动态特征点中剔除动态特征点，获得用于定位与建图的特征点队列Kp；
基于特征点队列Kp得到当前帧的局部地图，根据特征点队列Kp跟踪相机位姿，获得帧间位姿信息Pi，根据当前帧的局部地图进一步跟踪并优化位姿信息；
根据关键帧策略选取关键帧KF，并基于关键帧KF进行地图点生成和当前关键帧的局部地图优化；
从关键帧KF中选择并生成回环与地图融合候选关键帧KFl，基于改进的共同区域检测算法进行共同区域检测，共同区域检测成功后根据回环与地图融合候选关键帧KFl对应的位姿信息进行回环优化与地图融合，获得优化后的关键帧位姿PKi与全局一致性地图。
2.根据权利要求1所述的一种处理复杂环境下动态物体的视觉定位与建图方法，其特征在于，基于ORB特征点和实例分割结果计算语义ORB特征点具体包括：
将实例分割结果中各目标语义及掩膜作为提取语义ORB特征点的参数；
根据ORB特征点位置读取各目标语义及掩膜对应的像素值，再根据实例分割结果确定各目标语义对应的特征点，得到ORB特征点。
3.根据权利要求1所述的一种处理复杂环境下动态物体的视觉定位与建图方法，其特征在于，基于静态特征点匹配结果进行帧间位姿估计，并根据特征点匹配的自适应阈值极线约束算法从潜在动态特征点中剔除动态特征点，具体包括：
静态特征点与潜在动态特征点分别进行特征点匹配获得静态特征点队列和潜在动态特征点队列；
基于静态特征点队列进行帧间位姿估计，获得基础矩阵；
根据特征点匹配的最小距离区间确定极线约束阈值；
根据基础矩阵和上一帧潜在动态特征点队列的特征点像素坐标，计算上一帧潜在动态特征点队列在当前帧的极线方程；
计算当前帧潜在动态特征点队列与极线方程在像素坐标系的距离，根据极线约束阈值判断是否为动态特征点，若距离大于等于极线约束阈值则是，否则将潜在动态特征点从队列中删除。
4.根据权利要求1所述的一种处理复杂环境下动态物体的视觉定位与建图方法，其特征在于，关键帧策略包括：
a.当前帧距离上一关键帧产生了超过25个普通帧；
b.当前帧与参考关键帧跟踪到地图点的比例小于0.85；
c.当前帧距离重定位超过1s；
d.当前帧跟踪到的的近点少于50个，近点是指深度小于相机基线长度40倍的特征点；
e.成功跟踪到的特征匹配点数大于15；
f.待处理的关键帧数量不超过2帧；
g.在剔除动态特征点超过一定数量m时，提高当前帧与参考关键帧跟踪到的地图点比例阈值，使得b策略更易于判断成功；
根据上述策略，决定是否为关键帧的判断方法为：满足策略a.c.d之一，以及满足条件b.d之一，并且同时满足策略e和f则为关键帧，g策略是在动态特征点剔除时对b策略作调整。
5.根据权利要求1所述的一种处理复杂环境下动态物体的视觉定位与建图方法，其特征在于，地图点生成具体过程为：
从关键帧队列中取出一帧待处理关键帧，计算其ORB特征点对应的单词特征；
剔除在跟踪过程中初始地图中不合格的地图点；
基于一定数量的共视关键帧与当前关键帧的匹配关系，进行三角化生成三维点；
对三维点作质量检验后确定为当前关键帧的地图点，添加关键帧与其地图点的观测关系。
6.根据权利要求5所述的一种处理复杂环境下动态物体的视觉定位与建图方法，其特征在于，当前关键帧的局部地图优化具体过程为：
通过当前关键帧的一级、二级关键帧以及各关键帧对其地图点的观测关系，对当前关键帧的局部地图中所有地图点坐标和当前关键帧的一级关键帧位姿做局部BA优化；
局部BA优化时，假设参与优化的变量中当前关键帧的局部地图点有n个，观测到当前关键帧的局部地图的关键帧有m个，使用最小化重投影误差的思想构建BA代价函数CostFunction：
其中，h(ξj,pj)是基于相机观测模型得到第j个地图点对应的图像中ORB特征点的像素坐标，相机位姿对应的李代数为ξi，路标对应的地图点坐标pj，i,j分别表示关键帧编号与当前关键帧的局部地图点编号,zij为预测数据。
7.根据权利要求6所述的一种处理复杂环境下动态物体的视觉定位与建图方法，其特征在于，基于改进的共同区域检测算法进行共同区域检测包括：
根据回环与地图融合候选关键帧与当前关键帧是否处于同一全局地图，将所有的回环与地图融合候选关键帧KFl分成回环候选关键帧组和地图融合候选关键帧组；
寻找与当前关键帧没有连接关系且具有公共单词特征的回环与地图融合候选关键帧，记为KeyFramCand，计算KeyFramCand中各候选关键帧与当前关键帧共同单词数的最大值，该值的0.8倍记为minCommonWords，最后KeyFramCand中只保留与当前关键帧共同单词数大于minCommonWords的部分；
计算上述KeyFrameCand中各个具有共视关系的关键帧组的共同单词总数，选取前3个组中共同单词数最多的单个回环与地图融合候选关键帧作为初始候选关键帧；
利用词袋模型计算初始候选关键帧与当前关键帧的相对位姿变换，再利用投影匹配的方法优化二者的相对位姿Tam；
基于相对位姿Tam，利用当前关键帧共视程度最高的5个回环与地图融合候选关键帧对KeyFrameCand进行几何一致性检验，如果几何一致性检验成功则共同区域检测成功，根据当前回环与地图融合候选关键帧处于回环候选关键帧组或地图融合候选关键帧组，确定进行回环优化与地图融合。
8.根据权利要求7所述的一种处理复杂环境下动态物体的视觉定位与建图方法，其特征在于，共同区域检测成功后根据生成回环与地图融合候选关键帧KFl对应的位姿信息进行回环优化与地图融合，获得优化后的关键帧位姿PKi与全局一致性地图，包括：
对回环与地图融合候选关键帧中的半静态特征点及半静态特征点对应的地图点进行处理，分别标记半静态特征点及半静态特征点对应的地图点的首次观测时间戳，在当前回环与地图融合候选关键帧进行共同区域检测前，根据首次观测时间戳与当前时间的对比，将达到条件的半静态特征点及半静态特征点对应的地图点删除；
当前回环与地图融合候选关键帧KFl如果不在当前全局地图，则进行地图融合，若在当前全局地图，则进行回环优化，获取全局一致性地图，并消除定位的累计误差，获得优化后的关键帧位姿PKi。
9.根据权利要求8所述的一种处理复杂环境下动态物体的视觉定位与建图方法，其特征在于，对半静态特征点及半静态特征点对应的地图点进行处理，包括：
将当前回环与地图融合候选关键帧分别与共同区域检测用到的回环候选关键帧组和地图融合候选关键帧组进行时间间隔比较，对删除的半静态特征点对应的地图点做标记；
将上述半静态特征点对应的地图点加入到半静态地图点待删除队列中；
遍历半静态地图点待删除队列，对每个半静态特征点对应的地图点标记数进行检查，当标记数大于等于总观测数的50％后，认定该半静态特征点已经超过了设定的有效时间，将其在全局地图中删除。
10.根据权利要求7所述的一种处理复杂环境下动态物体的视觉定位与建图方法，其特征在于，几何一致性检验过程为：
利用投影匹配判断是否有共视关系，5个中有3个回环与地图融合候选关键帧检验成功则证明共同区域检测成功；若验证成功数大于0小于3，则进入时序几何校验：时间上连续新增的关键帧重复上述检验，验证成功数达到3时，证明共同区域检测成功，若连续两个新增关键帧都时序几何校验失败，则当前回环与地图融合候选关键帧验证失败。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310528902.1A CN116563341A (zh) | 2023-05-11 | 2023-05-11 | 一种处理复杂环境下动态物体的视觉定位与建图方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310528902.1A CN116563341A (zh) | 2023-05-11 | 2023-05-11 | 一种处理复杂环境下动态物体的视觉定位与建图方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116563341A true CN116563341A (zh) | 2023-08-08 |
Family
ID=87485625
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310528902.1A Pending CN116563341A (zh) | 2023-05-11 | 2023-05-11 | 一种处理复杂环境下动态物体的视觉定位与建图方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116563341A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117274620A (zh) * | 2023-11-23 | 2023-12-22 | 东华理工大学南昌校区 | 基于自适应均匀划分特征点提取的视觉slam方法 |
-
2023
- 2023-05-11 CN CN202310528902.1A patent/CN116563341A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117274620A (zh) * | 2023-11-23 | 2023-12-22 | 东华理工大学南昌校区 | 基于自适应均匀划分特征点提取的视觉slam方法 |
| CN117274620B (zh) * | 2023-11-23 | 2024-02-06 | 东华理工大学南昌校区 | 基于自适应均匀划分特征点提取的视觉slam方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111127513B (zh) | 一种多目标跟踪方法 | |
| CN112132893B (zh) | 一种适用于室内动态环境的视觉slam方法 | |
| CN110335319B (zh) | 一种语义驱动的相机定位与地图重建方法和系统 | |
| CN111693047B (zh) | 一种高动态场景下的微小型无人机视觉导航方法 | |
| CN112132897A (zh) | 一种基于深度学习之语义分割的视觉slam方法 | |
| CN110717927A (zh) | 基于深度学习和视惯融合的室内机器人运动估计方法 | |
| WO2023065395A1 (zh) | 作业车辆检测与跟踪方法和系统 | |
| CN114638855A (zh) | 一种多目标跟踪方法、设备及介质 | |
| CN113516664A (zh) | 一种基于语义分割动态点的视觉slam方法 | |
| CN114677323A (zh) | 一种室内动态场景下基于目标检测的语义视觉slam定位方法 | |
| CN112446882A (zh) | 一种动态场景下基于深度学习的鲁棒视觉slam方法 | |
| CN111666860A (zh) | 一种车牌信息与车辆特征融合的车辆轨迹跟踪方法 | |
| CN116563341A (zh) | 一种处理复杂环境下动态物体的视觉定位与建图方法 | |
| CN114998276A (zh) | 一种基于三维点云的机器人动态障碍物实时检测方法 | |
| Yang et al. | Road constrained monocular visual localization using Gaussian-Gaussian cloud model | |
| CN110176022B (zh) | 一种基于视频检测的隧道全景监控系统及方法 | |
| Zhou et al. | Robust global localization by using global visual features and range finders data | |
| CN116592897B (zh) | 基于位姿不确定性的改进orb-slam2定位方法 | |
| CN113807457A (zh) | 确定路网表征信息的方法、装置、设备及存储介质 | |
| Chen et al. | Monocular semantic SLAM in dynamic street scene based on multiple object tracking | |
| CN111882663A (zh) | 一种融合语义信息完成视觉slam闭环检测方法 | |
| CN116385493A (zh) | 野外环境下多运动目标检测与轨迹预测方法 | |
| CN116758153A (zh) | 用于机器人精准位姿获取的基于多因子图的后端优化方法 | |
| Islam et al. | A semantically aware place recognition system for loop closure of a visual SLAM system | |
| CN115962773A (zh) | 一种移动机器人同步定位与地图构建方法、装置和设备 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |