一种基于动态特征补偿与弱约束复用的视觉SLAM方法

本发明涉及计算机视觉与同时定位与地图构建，尤其涉及一种基于动态特征补偿与弱约束复用的视觉slam方法。

背景技术：

1、视觉slam是移动机器人在未知环境中实现自主定位与环境建模的核心技术，在自动驾驶、增强现实、机器人导航等领域具有广泛的应用前景。传统视觉slam方法依赖于环境静态假设，即场景中的特征点在帧间仅随相机运动而发生投影变化。然而，真实场景中不可避免地存在行人、车辆等动态物体，当动态物体占据较大视野时，传统slam系统的静态假设被严重违背，动态特征点在帧间匹配中引入大量外点，导致位姿估计漂移甚至跟踪丢失。如何在高动态环境中保持鲁棒的定位与建图能力，如何在高动态环境中保持鲁棒的定位与建图能力，是视觉slam技术从实验室环境走向实际部署所面临的关键障碍。

2、针对上述问题，现有研究主要形成三类技术路线：

3、基于检测与剔除的策略：现有方法大多采取“先检测后剔除”的思路，将识别出的动态特征直接排除。该策略虽简洁有效，但在高动态场景中大量特征被移除后，可用几何约束显著减少，容易导致局部光束法平差退化甚至跟踪丢失。

4、基于对极几何的动静分类策略：该类方法利用极线约束区分动态点与静态点，但当相机发生纯旋转运动时，基础矩阵退化，对极几何约束失效，难以可靠区分动态点与静态点。纯旋转是真实机器人运动中的常见工况（如原地转向、云台旋转等），严重制约了该类方法在实际部署中的可靠性。

5、基于显式运动建模的策略：部分方法尝试对动态物体进行显式运动建模并纳入联合优化，但每增加一个动态物体即引入至少6个附加自由度，计算负担随场景中动态物体数量线性增长，难以满足实时性要求。

6、综上所述，现有动态slam方法在应对高动态场景时，面临“特征剔除导致约束不足”“纯旋转导致几何分类失效”以及“显式建模导致算力爆炸”三重困境。如何在不过度增加计算负担的前提下有效维系高动态场景中的几何约束，并在极端动态场景中防止跟踪丢失，是当前视觉slam领域亟待解决的技术难题。

技术实现思路

1、针对现有方法在动态场景中因特征剔除导致几何约束不足、纯旋转引起对极约束失效等问题，本发明提出一种基于动态特征补偿与弱约束复用的视觉slam方法。该方法有别于先检测后剔除策略，对动态特征进行运动补偿后以降权形式保留复用，在高动态场景中有效维系场景几何约束。

2、首先，利用imu辅助的几何约束与高斯混合模型实现自适应动静分类。读取相邻两帧图像，提取orb特征点并进行匹配，获得匹配特征点对。利用imu预积分计算相邻帧相对位姿，通过平移模长判定运动模式，若平移模长大于阈值则选择基础矩阵模型，否则选择单应矩阵模型，避免纯旋转退化。基于选定的几何模型计算各匹配特征点的极线距离或投影距离，对全体特征点的距离误差采用两分量高斯混合模型进行分布拟合，通过em算法求得自适应阈值，完成初步动静分类。以初步静态点为种子通过ransac重新估计几何模型并迭代精化，消除imu初始误差，得到最终的静态点集与动态候选点集。

3、其次，结合yolo语义先验与像素级运动前景进行双重验证，抑制单帧误判。利用静态点集估计仿射变换矩阵，消除相机运动引起的背景偏移，生成与参考帧对齐的补偿图像。在补偿图像上采用pbas算法提取运动前景掩膜，结合yolo目标检测框进行重合率匹配，筛选出真实运动目标区域。取动态候选点中落在真实运动目标区域内的部分，作为后续处理对象。

4、再次，引入运动方向约束的密度聚类算法，在二维运动残差层面将动态特征按运动模式自动分解为局部一致子簇。对筛选所得特征点，利用仿射变换逆变换提取局部运动残差向量，定义运动差异度量并运行运动一致性约束的密度聚类算法，划分为局部运动一致子簇。

5、然后，利用深度信息与已知相机运动，通过svd分解解耦物体自身运动并执行三维补偿以恢复为准静态观测。对各子簇通过当前帧像素坐标与深度值反投影至相机坐标系，得到三维观测点，获取上一帧对应三维点。根据已知帧间相机运动，将上一帧点变换至当前帧坐标系，构建运动解耦目标函数，通过svd分解闭式求解物体自身刚体运动参数。利用物体自身运动参数的逆变换将物体运动分量从混合观测中剥离，计算假设物体静止时的三维坐标，经相机投影函数重投影至图像平面，得到补偿后的准静态观测点。

6、最后，根据场景中静态特征点的充足程度，自适应调节补偿观测在跟踪线程中的参与程度。计算各簇运动残差方差与语义置信因子，确定补偿观测的几何/语义联合权重。静态点充足时仅依赖静态点进行位姿求解；静态点数量低于预设阈值时，将补偿观测以联合权重作为弱约束纳入位姿估计，为跟踪提供补充约束，防止因静态点不足导致前端退化。

7、为实现上述目的，本发明提供一种基于动态特征补偿与弱约束复用的视觉slam方法，具体步骤包括：

8、s1：获取相邻两帧图像，提取特征点并进行匹配，获得匹配特征点对；

9、s2：利用imu预积分获取相邻两帧间的相对位姿信息，根据相对位姿信息中的平移模长判定当前帧的运动模式，并根据运动模式自适应选择基础矩阵模型或单应矩阵模型作为初始几何模型；

10、s3：基于所述初始几何模型，计算各匹配特征点的几何距离误差，并对全体特征点的距离误差分布进行自适应动静分类，得到初步静态点集与动态候选点集；

11、s4：以所述初步静态点集为种子，通过ransac算法重新估计几何模型并迭代精化，消除imu初始误差，得到最终的静态点集与动态候选点集；

12、s5：基于所述动态候选点集，结合语义先验信息与像素级运动前景进行交叉验证，筛选出真实运动目标区域内的特征点作为有效动态特征点，并对所述有效动态特征点进行运动模式聚类，划分为若干局部运动一致子簇；

13、s6：针对各所述局部运动一致子簇，利用深度信息与已知相机运动进行三维运动解耦，将物体自身运动分量从混合观测中剥离，对剥离后的特征点执行三维坐标补偿，恢复为准静态观测点；

14、s7：根据当前帧中所述最终的静态点集的特征点数量，自适应调节所述准静态观测点在位姿估计中的参与程度；当静态特征点数量充足时，优先采用最终的静态点集进行位姿估计，准静态观测点不参与位姿估计优化函数；当静态特征点数量低于预设阈值时，将所述准静态观测点作为弱约束纳入位姿估计优化函数，为跟踪线程提供补充约束。

15、进一步地，步骤s1中获取相邻两帧图像，提取特征点并进行匹配，获得匹配特征点对，具体过程如下：

16、获取视频流，依次读取相邻两帧图像，对前一帧提取orb特征点并计算描述子，在后一帧中基于描述子距离进行最近邻与次近邻搜索，若最近距离与次近距离之比小于预设阈值则初步判定为匹配点对；

17、采用对称约束进行双向匹配验证，仅保留正向匹配与反向匹配一致的特征点对，得到稳健的相邻帧匹配点对集合。

18、进一步地，步骤s2中初始几何模型选取过程包括：

19、利用imu预积分获取相邻两帧之间的相对旋转和平移，并计算平移模长：；

20、根据平移模长判定运动模式，若，判定相机存在明显平移，选择基础矩阵模型作为初始几何模型，表达式为：；若，判定相机近似纯旋转状态，选择单应矩阵模型作为初始几何模型，表达式为：；

21、其中，表示相机内参矩阵，表示由三维向量构造的反对称矩阵算子，表示预设平移模长阈值，表示旋转矩阵，表示平移向量。

22、进一步地，步骤s3中所述对全体特征点的距离误差分布进行自适应动静分类，得到初步静态点集与动态候选点集，具体包括：

23、采用两分量高斯混合模型对距离误差分布进行建模，表达式为：，其中，表示几何距离误差，、分别表示两分量的混合权重，、分别表示各高斯分量的均值与方差，表示高斯分布函数；

24、通过期望最大化算法迭代估计模型参数；

25、根据贝叶斯最小错误率准则，基于所述模型参数求解最优决策边界，生成自适应分类阈值，其中，所述边界满足：；

26、将距离误差大于所述自适应分类阈值的特征点标记为动态候选点，其余标记为初步静态点，得到初步静态点集与动态候选点集。

27、进一步地，步骤s5中所述基于所述动态候选点集，结合语义先验信息与像素级运动前景进行交叉验证，筛选出真实运动目标区域内的特征点，具体包括：

28、s51：利用所述最终的静态点集估计仿射变换矩阵，消除相机运动引起的背景偏移，生成与参考帧对齐的补偿图像；

29、s52：在所述补偿图像上提取像素级运动前景掩膜，并获取所述运动前景掩膜的外接矩形区域

d'；

30、s53：对当前帧进行目标检测，获取语义目标检测框，将所述语义目标检测框与所述运动前景掩膜进行重合率匹配，筛选出真实运动目标区域；

31、s54：将所述动态候选点集中落在真实运动目标区域内的特征点，作为有效动态特征点。

32、进一步地，步骤s52中采用pbas算法实现像素级运动前景掩膜提取；步骤s53中目标检测采用yolo系列神经网络实现；所述重合率匹配的计算方式为：

33、计算每个yolo目标检测框与运动前景掩膜外接矩形区域

d'的重合率：，其中，s（）表示区域面积函数；若大于预设重合率阈值

δ，则判定该语义目标检测框对应的区域为真实运动目标区域，否则作为静态背景剔除。

34、进一步地，步骤s5中所述对所述有效动态特征点进行运动模式聚类，划分为若干局部运动一致子簇，具体包括：

35、利用所述仿射变换矩阵的逆变换提取每个有效动态特征点的局部运动残差向量，表达式为：，其中，表示当前帧实际观测坐标，表示上一帧匹配点坐标，a表示仿射变换矩阵；

36、基于所述局部运动残差向量定义运动差异度量，运行基于运动一致性约束的密度聚类算法，将所述有效动态特征点划分为若干局部运动一致子簇。

37、进一步地，所述运行基于运动一致性约束的密度聚类算法，将所述有效动态特征点划分为若干局部运动一致子簇，具体包括：

38、采用马氏距离量化两点间运动残差的相似程度：，其中，表示特征点

i和特征点

j之间的运动差异得分，和分别表示点

i和点

j的局部运动残差向量，表示协方差矩阵的逆矩阵；

39、基于卡方分布性质，以第一显著性水平确定初始聚类阈值，以第二显著性水平确定簇合并阈值，其中；

40、执行初始成簇与基于所述初始聚类阈值的簇扩张，并利用所述簇合并阈值对空间相邻且运动相似的簇进行合并，剔除噪声点，得到局部运动一致子簇。

41、进一步地，步骤s6具体过程包括：

42、s61：对于局部运动一致子簇中的第

i个特征点，依据其在当前帧的像素坐标及对应深度值，利用相机内参矩阵执行反投影变换，将其映射至当前帧相机坐标系下，得到该特征点在当前帧相机坐标系下的三维观测点，并通过orb描述子匹配获取该特征点在上一帧中匹配点对应的三维点；

43、s62：根据imu预积分或初始位姿估计提供的帧间相机运动的旋转矩阵和平移向量，将上一帧三维点变换至当前帧坐标系，得到纯相机运动下的静态期望位置，表达式为：，其中，表示旋转矩阵，表示平移向量；

44、s63：构建运动解耦目标函数，通过奇异值分解闭式求解物体自身的刚体运动参数；

45、s64：利用物体自身运动参数的逆变换，将物体运动分量从混合观测中剥离，计算假设物体静止时的三维坐标：，其中，表示补偿后的三维坐标，即准静态点，表示物体的自身平移量，表示物体的自身旋转量；并通过相机投影函数重投影至图像平面，得到补偿后的准静态观测点。

46、进一步地，步骤s7中所述将所述准静态观测点作为弱约束纳入位姿估计优化函数之前，还包括联合权重计算的步骤，具体为：

47、计算各局部运动一致子簇的簇内运动残差方差，以第k个动态子簇为例，表达式为：，其中，表示子簇内第

i个特征点的运动残差向量，表示子簇内所有特征点的平均运动残差向量，表示第

k个独立子簇，表示该子簇内特征点的总数；

48、根据几何一致性与语义先验确定补偿观测的复用权重：，其中，为语义置信因子，根据局部运动一致子簇所对应运动目标的类别确定，为归一化系数；

49、将复用权重作为所述准静态观测点在位姿估计优化函数中的信息矩阵缩放系数或鲁棒核函数权重，以实现弱约束接入。

50、与现有技术相比，本发明有益效果如下：

51、本发明通过imu辅助的平移模长判定自适应选择几何模型，有效解决了纯旋转运动下对极几何约束失效导致动静态分类不可靠的问题。通过运动补偿与三维解耦，将动态特征修复为准静态观测点并降权复用，避免了高动态场景中因大量特征剔除导致的几何约束不足和ba退化。通过静态点充足时依赖静态点、不足时接入补偿观测的自适应切换策略，在保证常规场景定位精度不受影响的前提下，显著降低了极端动态场景下的跟踪丢失率。同时，本发明采用svd分解闭式求解物体运动参数，无需迭代优化，避免了显式运动建模中变量维度随动态物体数量线性增长的计算负担问题。