一种动态场景下基于观测可信度重构与状态一致鲁棒估计的RGB-DSLAM方法及系统

摘要: 本发明属于控制调节系统,具体来说,特别涉及一种动态场景下基于观测可信度重构与状态一致鲁棒估计的rgb-dslam方法及系统。背景技术:1、同步定位与建图(sla...
一种动态场景下基于观测可信度重构与状态一致鲁棒估计的RGB-DSLAM方法及系统

本发明属于控制调节系统,具体来说,特别涉及一种动态场景下基于观测可信度重构与状态一致鲁棒估计的rgb-d slam方法及系统。

背景技术:

1、同步定位与建图(slam)是移动机器人、服务机器人和室内自主导航系统的基础能力。rgb-d 传感器能够同时提供彩色图像和深度观测,相比单目系统更容易获得可恢复尺度的几何约束,因此在室内机器人视觉中被广泛使用。然而,许多稀疏特征式rgb-d slam系统仍隐含依赖静态世界假设。当行人穿行、可移动物体遮挡、视角快速切换或相机运动加剧时,进入前端位姿优化的特征观测不再只受纹理质量、深度噪声和视角变化影响,还会受到动态区域错误关联、局部遮挡和残差长尾分布的共同扰动。主要原因为动态目标会破坏局部地图点投影和当前帧匹配之间的一致性,使重投影残差出现长尾甚至局部多峰分布。其次,遮挡切换会导致可用几何约束数量和空间覆盖在短时间内显著变化,固定鲁棒核阈值和固定关键帧策略难以及时适配。更重要的是,当语义分割或实例识别被引入前端时,语义结果通常由独立线程异步产生;若过期语义掩码被直接复用于当前帧,遮挡边界、深度层切换和快速视角变化可能使原本用于抑制动态干扰的语义先验转化为新的误导源。

2、现有的解决方法大概有三类:第一类方法直接检测并剔除动态区域。如dynaslam利用语义分割与多视图几何检测抑制动态物体,并结合背景修补改善建图结果;ds-slam将语义分割和运动一致性检测结合,以减少动态特征对位姿估计的影响;这类方法证明了语义先验对动态slam的有效性,但对象级或区域级硬剔除也可能删除动态实例内部短时间内仍稳定、仍能提供位姿约束的局部纹理。第二类方法进一步融合语义、光流、极线约束、深度一致性或静态概率,以获得更细粒度的观测判断,能将动态观测处理从二值剔除推进到概率化或证据融合,但多数工作仍主要关注哪些点应被剔除,较少同时处理异步语义先验的时效边界和动态区域内部局部稳定观测的恢复。第三类方法从鲁棒估计角度改善非线性最小二乘在外点污染下的稳定性,例如自适应鲁棒核和数据驱动残差统计,然而,鲁棒核主要作用于优化阶段的残差降权;若观测构造、先验融合和关键帧写入仍采用固定策略,前端估计与地图刷新之间仍可能发生状态不一致。

3、基于此,本技术提供一种能够重构观测可信度、实现状态一致调节控制的鲁棒跟踪方案。

技术实现思路

1、针对相关技术中的问题,本发明提出一种动态场景下基于观测可信度重构与状态一致鲁棒估计的rgb-d slam方法及系统,以克服现有相关技术所存在的上述技术问题。

2、为解决上述技术问题,本发明是通过以下技术方案实现的:

3、本发明为一种动态场景下基于观测可信度重构与状态一致鲁棒估计的rgb-dslam方法,包括以下步骤:

4、获取当前图像中帧的初始观测集合及异步语义线程输出的语义数据包;

5、根据所述语义数据包对应帧与当前跟踪帧的时间差,执行有界时效门控,确定先验使用模式,所述先验使用模式包括同帧先验模式、单帧滞后补偿先验模式及纯几何退化模式;

6、在所述先验使用模式下,结合动态语义区域内的局部稳定观测恢复结果,构造观测先验权重;

7、基于当前帧的匹配充足度和分位数残差统计计算条件评分;

8、基于所述条件评分,利用预设的自适应控制律联动调节鲁棒核阈值、观测先验权重与几何权重的软融合系数以及关键帧接纳门槛;

9、基于所述观测先验权重、几何权重及调节后的鲁棒核阈值,执行迭代重加权位姿优化,输出当前帧位姿;

10、根据当前帧位姿得到高权重观测集合和高权重观测覆盖率;

11、基于当前帧位姿的高权重观测集合,根据所述关键帧接纳门槛与高权重观测覆盖率自适应接纳关键帧。

12、其中,本发明通过有界时效门控机制执行先验使用模式,将异步语义先验严格限制在可控的滞后边界内,有效解决了因语义线程延迟导致的跨帧语义失配问题,避免了过期掩码在遮挡边界和快速视角变化下转化为新的误差源;同时摒弃了简单的二值化剔除策略,在动态语义区域内挖掘并恢复具有短期运动一致性的局部稳定观测,实现了动态干扰抑制与有效几何约束保留的细粒度平衡;还构建了条件评分机制,将匹配充足度与残差分布特征量化为单一状态量,并利用条件评分,通过预设的自适应控制律联动调节鲁棒核阈值、观测先验权重与几何权重的软融合系数以及关键帧接纳门槛,使得位姿优化、观测赋权与地图刷新在动态退化条件下始终保持一致的控制逻辑;从而通过有界时效门控机制、局部稳定观测以及条件评分机制的配合显著降低了动态场景下的绝对轨迹误差和相对位姿误差,提高了鲁棒跟踪精确度。

13、优选地,所述确定先验使用模式包括如下步骤:

14、通过语义线程对输入的图像执行实例分割与类别识别,并输出语义数据包,具体如下:

15、

16、其中,表示语义线程针对第q帧生成并输出的语义数据包,s表示该数据包来源于语义线程,为该数据包对应的帧序号,为语义数据包时间戳;、和分别表示实例掩码集合、实例类别集合与类别置信度集合;与分别为语义静态可信度图和时序一致性图;为高可信支持点集合;为不直接参与前端优化的辅助元信息;

17、设相机输入频率为,则相机输入周期为:

18、

19、设当前线程正在处理第帧,最新可用语义数据包对应的帧号为,定义帧差和时间差为:

20、

21、

22、其中,为当前跟踪帧时间戳,为帧差,为时间差,表示帧号为的最新可用语义数据包的时间戳,c和s分别表示相机图像时间戳和语义线程时间戳,t表示当前跟踪帧序号;

23、定义先验使用模式为:

24、

25、其中,为辅助时间门限,为先验使用模式,且为三种受控状态:当时表示采用同帧先验模式,即采用同帧语义信息;当时表示采用单帧滞后补偿先验模式,使用上一帧语义信息并经过补偿后参与当前帧优化;当时表示纯几何退化模式,此时先验不可用,系统退化为纯几何跟踪。

26、优选地,所述局部稳定观测恢复具体包括如下步骤:

27、在动态语义区域内,利用前后一致性约束和小位移累积约束筛选出具有时序一致性的局部支持点,并生成离散评分,具体如下

28、;

29、其中,i表示动态语义区域内被跟踪特征点的索引,表示第t帧第i个特征点的离散评分,表示第t-1帧中第i个被跟踪特征点的时序可靠性离散评分,为最大离散评分值,表示第t帧第i个特征点前后向一致性检测结果,表示第t帧第i个特征点相邻帧像素位移,为小位移阈值;

30、经描述子复核后,将离散评分映射为特征点级运动一致性权重:

31、;

32、其中,为第t帧第i个特征点级运动一致性权重;

33、将特征点级运动一致性权重投影为图像级运动一致性图。

34、优选地,所述构造观测先验权重包括如下步骤:

35、首先对语义静态可信度进行基于几何可见性的重投影补偿,设当前帧第个观测点的像素位置和深度分别为和,由当前深度恢复该点在相机坐标系下的三维位置为:

36、;

37、其中,为相机内参矩阵,和分别表示第t帧第i个观测点在图像坐标系中的横向像素坐标和纵向像素坐标,x、y分别表示二维像素坐标的水平分量和竖直分量,为第i个观测点的二维像素位置,且,t表示转置,设由上一帧估计位姿和当前预测位姿得到的相对变换为:

38、;

39、其中,w表示世界坐标系,分别表示上一帧和当前帧相机坐标系,表示世界坐标系w到上一帧相机坐标系的位姿变换,表示世界坐标系w到当前帧预测相机坐标系的预测位姿变换,pred表示该位姿来自当前帧优化前的运动预测,表示由当前预测相机坐标系到上一帧相机坐标系的预测相对变换,则该点在上一帧图像中的预测重投影位置为:

40、;

41、仅当落在图像有效区域内,且满足深度一致性条件:

42、;

43、才认为该观测的重投影补偿有效,并定义补偿后的语义静态可信度为:

44、;

45、其中,为观测点在上一帧中的预测深度,表示上一帧深度图在对应预测像素位置处实际观测到的深度,为深度一致性阈值,用于判断预测深度与上一帧深度图观测是否一致,为补偿衰减系数,表示单帧滞后补偿模式下,第t帧第i个观测点经重投影补偿后的语义静态可信度,表示上一帧语义静态可信度图在预测重投影位置处的取值,comp表示该量为补偿后的结果,该式通过衰减上一帧语义先验相对于完全静态可信度1的偏离程度,降低跨帧传播语义信息的不确定性影响;

46、接着对运动一致性补偿,将当前帧运动一致性补偿图定义为:

47、;

48、其中,表示当前第t帧像素位置u处的运动一致性补偿值,表示能够从上一帧有效传播至当前帧的特征点索引集合,表示以为中心、预设半径r内的像素邻域,表示该特征点在上一帧的运动一致性权重,表示跨帧传播衰减系数,取最大值表示当多个有效传播特征点对同一像素提供支持时采用其中最强的时序支持证据,该式表示只有当存在使得时,该特征点才可向像素u传播运动一致性权重,若当前像素u的领域不存在有效传播点,则令。

49、于是,当前帧第个观测点的语义权重与时序权重分别定义为:

50、;

51、;

52、最终先验权重为:

53、。

54、其中,表示当前第t帧第i个观测点最终采用的时序运动一致性权重,表示第t帧i个观测点经单帧滞后重投影补偿后得到的语义静态可信度,表示第t帧运动一致性图在当前观测位置处的取值,表示当前第t帧运动一致性补偿图在第i个观测点当前像素位置处的取值,表示根据当前先验使用模式来确定第t帧i个观测点的运动一致性权重,表示根据当前先验使用模式来确定第t帧i个观测点的运动一致性权重,表示第t帧第i个观测点最终采用的观测先验权重,即从该观测点的语义静态可信度和时序运动一致性可信度中选择较高者作为最终先验可信度,从而使动态语义区域内虽然语义静态可信度较低、但具有较强短期运动一致性的局部稳定观测仍能够被保留并参与后续优化。

55、优选地,所述计算条件评分包括以下步骤:

56、定义条件评分,且满足:

57、;

58、其中,表示有效匹配充足度,表示几何置信度;

59、设未加权有效匹配数为,安全阈值为,则满足:

60、;

61、其中,为曲率参数;

62、在预测位姿处统计几何置信度,定义第个观测的像素域重投影误差为:

63、;

64、其中,表示在当前第t帧预测位姿处,第i个有效观测的像素域重投影误差,表示与当前第i个图像观测相匹配的地图点在世界坐标系w下的三维坐标,m(i)表示与第i个观测建立对应关系的地图点索引,x表示地图点三维坐标,在所有上计算第分位数误差:

65、;

66、其中,表示当前帧参与统计的有效观测集合,i为观测索引,p为分位数等级,且p∈{20,50,80},quantilep(·)表示计算样本集合的第p百分位数,表示当前第t帧重投影误差分布的第p百分位数误差,反映高质量匹配的误差区,表示整体匹配的典型误差区,表示长尾误差区,将不同分位数映射到统一尺度,对每个分位数采用分段线性归一化:

67、;

68、其中,表示当前第t帧第p分位数误差对应的归一化几何置信度,第p分位数对应的最大可接受误差阈值,取px、px、px,最终,几何置信度定义为:

69、。

70、优选地,所述联动调节具体包括以下步骤:

71、调节tukey鲁棒核阈值,设基准阈值为,上一帧位姿优化结束后的最终内点数为,参考内点规模为,则历史内点保持度 定义为:

72、;

73、最终,tukey鲁棒核阈值 定义为:

74、;

75、其中,与分别为条件评分增益系数和历史内点保持度增益系数;

76、调节软融合系数,其满足:

77、

78、调节关键帧接纳门槛,其满足:

79、;

80、其中,表示观测先验权重与几何权重软融合时使用的融合系数,表示跟踪状态健康时偏向精确约束的融合系数下限,表示退化状态下偏向保留有效边缘观测的救援型融合系数上限,表示由条件评分决定的关键帧接纳最小高权重观测数量,为退化状态下采用的接纳门槛,为健康状态下采用的接纳门槛,β为控制门槛随变化曲线形状的指数参数。

81、优选地,所述执行迭代重加权位姿优化具体包括以下步骤:

82、计算观测点的平方mahalanobis误差为:

83、;

84、归一化误差为:

85、;

86、计算几何权重为:

87、;

88、构造和,其中,满足:

89、;

90、满足:

91、;

92、最终融合权重为:

93、

94、第次迭代的位姿增量为:

95、;

96、并更新位姿为:

97、;

98、其中,表示利用李代数增量更新相机位姿的运算,k 表示irls迭代次数,表示第t帧第i个观测的二维重投影残差,表示该观测对应的像素测量协方差矩阵;表示几何鲁棒权重,表示观测先验权重,表示第k次irls迭代中第i个观测最终参与优化的融合权重,表示当前帧有效观测集合,表示第k次迭代时当前帧相机位姿估计,表示第k次迭代求得的李代数位姿增量。

99、优选地,所述自适应接纳关键帧包括以下步骤:

100、设最终融合权重大于阈值的为高权重位姿,统计高权重观测集合,且满足:

101、;

102、其中,表示irls收敛后的最终权重,其数量记为,且满足:

103、;

104、其中,为第t帧高权重观测数量,表示当前第t帧在irls位姿优化收敛后,最终融合权重不低于阈值的高权重观测点数量,即高权重观测集合的基数,将图像划分为网格,记被高权重点覆盖的网格数为,则覆盖率为:

105、

106、其中,表示沿图像水平方向划分的网格列数,表示沿图像竖直方向划分的网格行数,因此图像共包含个网格单元,表示当前第t帧中至少包含一个高权重观测点的网格单元数量,cell表示网格单元、cover表示被高权重观测覆盖,表示当前帧高权重观测覆盖率,关键帧接纳条件为,当条件满足时接纳当前帧为关键帧,否则拒绝。

107、优选地,所述方法还包括初始化增强和地图点可靠性维护两项辅助机制;所述初始化增强为:

108、在启动阶段维护长度为  的候选帧缓冲区,并为每个候选帧计算综合质量分数:

109、;

110、其中,、、和分别表示归一化后的静态特征点数量、静态特征点的空间分布均匀度、有效深度比例和模糊强度,为加权系数,当当前输入的候选帧同时满足快速触发条件:

111、;

112、则直接执行初始化,否则从缓冲区中选择  最大的候选帧建立初始地图;其中,、、和分别为静态特征点数量、空间分布均匀度、有效深度比例和模糊强度对应的快速初始化阈值;

113、所述地图点可靠性维护为:

114、将观测先验权重和关键帧接纳信息发送至局部建图线程,并在局部建图线程中维护地图点的动态评分,若地图点被观测且权重低于低权重惩罚阈值或被判为外点则加分,权重高于高权重奖励阈值则减分。

115、本发明还公开了一种动态场景下基于观测可信度重构与状态一致鲁棒估计的rgb-d slam系统,包括数据获取模块、时效门控模块、观测重构模块、状态评估模块、参数调节模块、位姿优化模块、关键帧管理模块以及地图维护模块;

116、数据获取模块,用于获取当前图像中帧的初始观测集合及异步语义线程输出的语义数据包;

117、时效门控模块,用于根据语义数据包与当前帧的帧差和时间差执行有界时效门控,确定先验使用模式;

118、观测重构模块,用于在动态语义区域内恢复局部稳定观测并构造观测先验权重;

119、状态评估模块,用于基于匹配充足度和分位数残差统计计算条件评分;

120、参数调节模块,用于利用条件评分联动调节鲁棒核阈值、软融合系数及关键帧接纳门槛;

121、位姿优化模块,用于基于观测先验权重、几何权重及调节后的鲁棒核阈值执行迭代重加权位姿优化,输出当前帧位姿;

122、关键帧管理模块,用于根据高权重观测集合及覆盖率自适应接纳关键帧,并将相关信息发送至局部建图线程;

123、地图维护模块,用于根据观测权重和关键帧接纳信息对地图点进行动态评分与删除维护。

124、本发明具有以下有益效果:

125、1.本发明通过有界时效门控机制执行先验使用模式,将异步语义先验严格限制在可控的滞后边界内,有效解决了因语义线程延迟导致的跨帧语义失配问题,避免了过期掩码在遮挡边界和快速视角变化下转化为新的误差源;同时摒弃了简单的二值化剔除策略,在动态语义区域内挖掘并恢复具有短期运动一致性的局部稳定观测,实现了动态干扰抑制与有效几何约束保留的细粒度平衡;还构建了条件评分机制,将匹配充足度与残差分布特征量化为单一状态量,并利用条件评分,通过预设的自适应控制律联动调节鲁棒核阈值、观测先验权重与几何权重的软融合系数以及关键帧接纳门槛,使得位姿优化、观测赋权与地图刷新在动态退化条件下始终保持一致的控制逻辑;从而通过有界时效门控机制、局部稳定观测以及条件评分机制的配合显著降低了动态场景下的绝对轨迹误差和相对位姿误差,提高了鲁棒跟踪精确度。

126、2.本发明中通过引入帧差和时间差双重判断准则,将语义先验的使用严格划分为同帧先验、单帧滞后补偿和纯几何退化三种受控状态,使得系统不再盲目等待语义结果或无限复用历史掩码,而是设定了严格的容忍边界,当语义延迟在容忍范围内时,系统通过后续的弱补偿机制修正误差;一旦延迟超出边界,系统果断退化为纯几何模式,切断错误语义的传播路径;不仅防止了异步系统固有的延迟问题演变为系统性的定位漂移,还通过短等待窗口的设计,在有限的时间代价下最大化了有效语义先验的利用率,相比现有无门控的语义融合方案,大幅降低了因语义滞后引发的异常残差,为后续观测可信度重构提供了干净的输入环境。

127、3.本发明中通过时序可靠性模块在动态语义区域内恢复局部稳定观测,解决了现有动态slam方法因对象级硬剔除而过度丢弃有效约束的问题,显著提升了动态场景下的几何约束利用率。现有技术通常将整个被分割为人或椅的区域视为动态干扰并整体剔除,然而在人体关节、衣物褶皱或家具边缘,往往存在大量在短时间内与相机保持相对静止或匀速运动的局部纹理,这些纹理蕴含了宝贵的位姿约束信息,时序可靠性模块通过证据累积而非瞬时判决的方式处理这些区域,利用前后向光流一致性、小位移约束和描述子复核,对动态区域内的特征点进行多帧验证,逐步累积其可靠性评分,相比硬剔除策略,时序可靠性模块能提供额外的有效内点,增强了位姿优化的可观测性和数值稳定性,从而有利于提高系统定位精度。

128、4.本发明构建的条件评分综合了匹配充足度和几何置信度两个维度,前者反映约束的数量基础,后者反映残差分布的健康程度,同时利用条件评分这一单一状态量驱动了三个关键环节的协同变化:当条件评分降低时,鲁棒核阈值放宽以避免优化病态,融合系数偏向稳健模式以保留边缘约束,关键帧接纳门槛降低以允许刷新参考帧;反之,当系统健康时,则收紧控制以追求精度和纯净度,从而通过联动调节确保了前端优化与地图刷新在同一风险评价下工作,避免了某一环节过于激进或保守导致的状态失配,相比单一的自适应鲁棒核方法,这种全局联动策略显著提升了系统在极端动态条件下的适应能力,使定位轨迹更加平滑、连续,有效抑制了因局部退化引发的跟踪丢失。

129、当然,实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。