一种位姿估计方法

摘要: 本发明涉及计算机视觉、机器人导航与定位的交叉,尤其涉及一种适用于城市道路、园区巡检、室内外移动机器人等高动态场景的vio(visual-inertialodom...
一种位姿估计方法

本发明涉及计算机视觉、机器人导航与定位的交叉,尤其涉及一种适用于城市道路、园区巡检、室内外移动机器人等高动态场景的vio(visual-inertialodometry,视觉惯性里程计)鲁棒位姿估计方法。

背景技术:

1、传统的vio(visual-inertial odometry,视觉惯性里程计)系统通常基于静态世界假设,即认为环境中路标点的空间位置是固定不变的,其在不同图像帧中的观测变化完全由相机自身运动和传感器噪声引起。在此假设下,系统通过最小化视觉重投影残差和imu(inertial measurement unit,惯性测量单元)预积分残差来估计载体的位姿、速度和偏置。然而,当环境中存在行人、行驶车辆等动态物体时,位于这些动态物体上的特征点(路标)的观测变化会混入物体自身的运动,导致前端匹配、位姿初始化、后端优化等环节受到干扰甚至失效,严重影响vio系统在高动态场景下的定位精度和鲁棒性。

2、针对动态场景下的vio问题,现有技术主要存在以下几类解决方案:第一类是基于几何约束的鲁棒估计方法,如ransac或鲁棒核函数,但这类方法在动态物体占比较高时,其内点集合可能被动态物体主导,导致基准模型本身失真,存在“用受污染视觉结果验证视觉特征”的循环依赖问题。第二类是基于语义分割的动态区域硬掩码方法,直接删除位于车辆、行人等预设动态类别区域内的所有特征点。然而,语义类别不等同于物理运动状态,例如停放的车辆、等待红灯的行人,它们在语义上属于“动态类别”,但实际是静止的,直接删除会导致可用视觉约束骤减,尤其在特征稀疏场景下会引发系统退化。第三类是引入惯性信息进行辅助判断,但现有方案多停留在简单的阈值化剔除层面,缺乏一种将语义先验、惯性运动约束和轨迹时序信息进行统一置信度融合的机制,也缺少根据视觉观测质量动态调整后端优化权重的自适应调度策略。

3、因此,如何有效区分语义上的“动态类别”与物理上的“真实运动”,如何保留静止的“假动态”目标上的有效特征,以及如何在视觉观测质量退化时自适应地调整优化策略,是本领域亟待解决的技术问题。

4、以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案,其并不必然属于本专利申请的现有技术,在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下,上述背景技术不应当用于评价本技术的新颖性和创造性。

技术实现思路

1、为解决现有vio系统在动态场景中因循环依赖、语义误判和缺乏自适应调度而导致的鲁棒性差、定位精度低的问题,本发明提出了一种能够准确区分真实动态特征与静态特征,并能在视觉质量退化时自适应调整约束权重的位姿估计方法。

2、为了达到上述目的,本发明采用以下技术方案:

3、第一方面,本发明公开了一种位姿估计方法,包括以下步骤:

4、s1:对图像帧进行语义分割,获得像素级语义信息,并基于预定义的动态类别集合生成候选动态特征点集合;

5、s2:利用imu预积分获得相邻图像帧之间的惯性运动预测,计算候选动态特征点的惯性预测像素坐标与实际观测像素坐标之间的运动一致性残差;

6、s3:对于具有历史数据的特征点轨迹,基于所述运动一致性残差的历史序列和连续跟踪状态,计算轨迹静态置信度;

7、s4:融合所述像素级语义信息、所述运动一致性残差和所述轨迹静态置信度,生成所述候选动态特征点的动态风险评分,并将所述动态风险评分映射为视觉重投影残差的特征级连续软权重;

8、s5:计算当前图像帧的视觉质量评分,并将所述视觉质量评分映射为视觉重投影残差的帧级整体权重;

9、s6:结合所述特征级连续软权重和所述帧级整体权重,对视觉重投影残差和imu预积分残差进行联合优化,以估计载体位姿。

10、优选地,步骤s1具体包括:

11、将图像帧输入语义分割网络,获得像素级语义类别概率图;

12、提取图像帧的特征点,查询特征点所在位置的语义类别概率;

13、将最大预测概率属于预定义的动态类别集合、且概率值超过预设语义筛选阈值的特征点,纳入所述候选动态特征点集合。

14、优选地,所述语义分割网络采用pidnet结构,包括比例分支、积分分支和微分分支,并通过边界引导融合模块输出像素级语义类别概率图。

15、优选地,步骤s2具体包括:

16、利用双目视差或深度信息,将特征点从图像坐标系反投影至相机坐标系,得到特征点的三维坐标;

17、利用imu预积分推导出相邻帧相机之间的相对位姿变换;

18、将所述三维坐标根据所述相对位姿变换投影至下一帧图像平面,得到惯性预测的像素坐标;

19、计算所述惯性预测的像素坐标与特征匹配得到的实际像素坐标之间的欧氏距离,作为所述运动一致性残差。

20、优选地,步骤s3具体包括:

21、为每条特征点轨迹维护一个先进先出的历史残差队列,用于存储近期若干帧的运动一致性残差;

22、计算所述历史残差队列中的运动一致性残差的平均值;

23、基于所述平均值和该轨迹的连续跟踪帧数,计算所述轨迹静态置信度。

24、优选地,步骤s4具体包括:

25、将所述运动一致性残差归一化到预设区间内;

26、将归一化后的运动一致性残差、从像素级语义信息中获得的语义动态置信度、以及所述轨迹静态置信度进行加权融合,得到所述动态风险评分;

27、通过一个单调递减的映射函数,将所述动态风险评分转换为介于0和1之间的所述特征级连续软权重。

28、优选地,步骤s5中评估当前图像帧的视觉质量,具体包括:

29、计算当前图像帧有效跟踪特征点数量与参考数量的比值,得到数量评分;

30、将图像平面网格化,计算特征点在不同网格分布的熵值,得到分布均匀度评分;

31、基于语义分割结果排除潜在动态目标区域,计算当前帧与上一帧静态图像区域的灰度分布差异,得到跨帧光度稳定性评分;

32、计算图像拉普拉斯响应的方差与参考方差的比值,得到图像清晰度评分;

33、对所述数量评分、分布均匀度评分、跨帧光度稳定性评分和图像清晰度评分进行加权或综合,以评估当前图像帧的视觉质量。

34、优选地,步骤s5中所述视觉重投影残差的帧级整体权重根据所述视觉质量评分在预设低阈值和高阈值之间进行线性或非线性映射调节。

35、优选地,步骤s6具体包括:在滑动窗口优化中,使用所述特征级连续软权重和所述帧级整体权重加权视觉重投影残差,并结合imu预积分残差和边缘化先验残差,进行联合优化以估计载体位姿。

36、第二方面,本发明公开了一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现如第一方面所述的位姿估计方法的步骤。

37、与现有技术相比,本发明的有益效果在于:本发明提供的位姿估计方法,通过将语义信息、惯性信息与历史跟踪信息进行多源融合,生成动态风险评分,并映射为特征级连续软权重,同时结合由视觉质量评分映射得到的帧级整体权重,对视觉重投影残差和imu预积分残差进行联合优化。该方案首先利用独立于视觉的imu惯性运动预测,对基于语义信息生成的候选动态特征点进行几何一致性验证,有效排除了“假动态”语义物体的干扰,并识别出“真动态”特征。其次,时序轨迹静态置信度的引入,使得长期稳定跟踪的特征(如停放的车辆)能够获得更高的置信度,从而被有效保留和利用,解决了传统硬掩码方法导致有效特征丢失的问题。最后,特征级的连续软权重与帧级的整体视觉质量权重协同作用,不仅实现了对单个特征点风险程度的精细化处理,还使得系统能根据整体视觉观测质量动态调整视觉与惯性约束的比重。这种双重自适应机制使得优化过程在动态物体多、光照变化大或图像模糊等复杂场景下,能够平滑地降低不可靠视觉信息的影响,并强化惯性约束的支撑作用,从而显著提升了高动态和退化场景下位姿估计的精度、连续性和鲁棒性。

38、在进一步的方案中,本发明还具有以下有益效果:

39、(1)通过采用特定的比例-积分-微分网络结构进行语义分割,能够兼顾局部细节、全局上下文和边缘信息,从而更精确地提取动态类别区域,为后续处理提供更可靠的语义先验,间接提升了动态特征识别的准确性。

40、(2)通过具体定义运动一致性残差的计算方式,即利用双目深度和imu预积分进行三维坐标预测与投影,提供了一种量化、可靠的几何一致性度量标准,为后续的置信度评估和风险评分提供了精确的依据。

41、(3)通过定义轨迹静态置信度的具体计算方式,特别是引入历史残差队列和连续跟踪成熟度,能够有效地区分“刚进入视野的不稳定特征”、“长期低残差跟踪的稳定特征”和“运动轨迹异常的特征”,使时序维度的评估更具科学性和鲁棒性。

42、(4)通过具体定义多源信息融合生成连续软权重的过程,将语义、几何、时序三类异构信息统一到同一数值区间并进行加权融合,实现了对不同风险等级特征点的精细化、平滑化权重分配,避免了权重突变带来的优化问题。

43、(5)通过从特征数量、空间分布、光照稳定性和清晰度四个维度综合评估视觉质量,并据此动态映射视觉约束的整体权重,使得系统能够对多种类型的视觉退化场景做出准确、及时的响应,从而在光照变化、运动模糊、弱纹理等挑战性环境下保持鲁棒的位姿估计性能。