一种融合物体级静态地标的前端视觉里程计方法

本发明涉及视觉里程计定位，具体涉及一种融合物体级静态地标的前端视觉里程计方法。

背景技术：

1、同时前端视觉里程计是同步定位与地图构建（simultaneous localization andmapping，slam）系统获取传感器实时位姿、构建环境地图的核心模块，其精度直接决定了系统的整体性能。传统基于静态环境假设的视觉里程计方法，如基于定向快速旋转简报特征的同步定位与地图构建（orb-slam2）在动态场景中易受动态物体干扰，导致特征的错误匹配从而引起位姿估计漂移。

2、现有动态视觉里程计方法主要存在两方面不足：其一，动态物体识别方法多依赖单一观测信息，如仅基于几何约束或语义先验进行判断，在缓慢运动、同向运动、启停遮挡等复杂场景中容易出现动态物体识别不准确的问题，导致动态特征剔除不稳定；其二，现有方法大多仅简单去除动态物体上的特征，未能进一步挖掘场景中可利用的物体级结构信息，在纹理稀疏或点特征不足的场景下容易出现几何约束不足；同时部分研究将物体跟踪加入局部地图优化，但动态物体的不确定性难以建模，反而易增大定位误差。

3、针对静态物体地标建模，现有方法存在初始化速度与精度难以兼顾、多帧优化仅依赖单一二维投影约束易出现尺度漂移与视角退化等问题，因此，亟需一种能够兼顾动态目标鲁棒识别、静态物体可靠建模以及局部地图联合优化的视觉里程计方法。

技术实现思路

1、有鉴于此，本发明的目的在于提供一种融合物体级静态地标的前端视觉里程计方法，通过多源信息融合实现精准动态物体分割，同时对判定为静态的物体进行椭球参数化建模并纳入局部地图联合优化，从而在动态场景和特征稀疏场景下提高前端视觉里程计的定位精度与稳定性。

2、为达到上述目的，本发明提供如下技术方案：

3、一种融合物体级静态地标的前端视觉里程计方法，以orb-slam2为基础框架，核心在于通过多源信息融合实现鲁棒动态特征剔除，并构建静态物体椭球地标补充结构化几何约束，最终实现局部地图联合优化。

4、首先对输入连续帧图像进行多源数据预处理，得到语义分割结果、深度图、3d检测框与稠密光流场；分别从语义先验、场景流三维一致性、帧间运动一致性三个维度构建物体动态概率模型；将三类概率转换为对数几率形式，基于信息源可靠性自适应加权融合，经时序平滑与双阈值判决得到稳定的物体动静标签，剔除动态区域特征。

5、其次对判定为静态的物体进行椭球参数化建模，采用3d检测框快速初始化结合特征点pca拟合优化的单帧初始化策略；利用连续多帧的2d检测框与3d检测框构建联合观测误差，迭代优化椭球参数，筛选高质量椭球作为静态物体地标存入全局地图。

6、最后筛选与当前局部关键帧强关联的椭球地标，将椭球参数与关键帧位姿、静态地图点纳入统一优化框架，构建联合优化目标函数，通过光束法平差同步优化所有变量，提升位姿估计精度。

7、本发明提出的一种融合物体级静态地标的前端视觉里程计方法，包括以下步骤：

8、s1、图像数据预处理：对输入的连续帧双目图像依次执行语义实例分割、双目深度估计、3d目标检测与稠密光流计算、特征点提取与帧间匹配，得到各帧的实例语义掩膜、像素级深度图、物体3d检测框、帧间稠密光流场与帧间匹配特征点；

9、s2、多源动态概率独立建模：分别基于语义类别信息构建物体动态先验概率，基于场景流三维一致性构建像素级动态概率并聚合为物体级动态概率，基于帧间运动一致性构建物体级动态概率；

10、s3、多源概率融合与时序稳定判决：将三类动态概率统一转换为对数几率形式，根据各信息源的观测可靠性自适应分配融合权重进行加权叠加，对融合结果进行判决，输出物体的最终动静属性，剔除动态物体区域内的特征点；

11、s4、静态物体椭球单帧初始化：针对判定为静态的物体，融合3d检测框先验初始化结果与语义掩膜内特征点云的pca拟合结果，得到椭球初始参数；

12、s5、连续帧椭球参数优化：基于连续多帧的2d检测框与3d检测框构建二维投影-三维几何联合观测误差，迭代优化椭球参数，筛选被有效观测总数超过预设阈值的参数收敛椭球存入全局静态地标地图；

13、s6、局部地图联合优化：筛选与当前局部关键帧存在强观测关联的椭球地标，构建包含静态地图点重投影误差与椭球投影误差的联合优化目标函数，同步优化关键帧位姿、静态地图点坐标与椭球参数。

14、进一步，所述步骤s1包括:

15、s11、采用实例分割网络对rgb图像执行实例分割，输出物体类别、像素级掩膜、二维检测框与检测置信度；

16、s12、采用双目深度估计网络输出视差图，结合相机内参与基线参数转换为真实深度图；

17、s13、结合二维检测框、深度图与相机内参，反投影得到物体三维中心，结合预存的物体类别-尺寸对应关系与点云pca估计目标尺寸与朝向，生成3d检测框；

18、s14、采用稠密光流估计网络计算相邻帧间的稠密光流场；

19、s15、提取图像特征点并进行帧间匹配，得到用于位姿估计和动态特征剔除的帧间匹配特征点。

20、进一步，所述步骤s2中，基于场景流三维一致性构建物体级动态概率的具体过程为：

21、s21、根据帧间稠密光流场匹配前后帧对应像素，结合前后帧深度图分别反投影得到相机坐标系下的三维点；

22、s22、根据相机帧间初始位姿计算静态刚体假设下的场景流一致性残差向量，将所述场景流一致性残差向量建模为零均值三维高斯分布；利用语义分割得到的绝对静态背景区域，通过中位数绝对偏差 mad 在线估计残差的尺度参数，并设置上下界抑制退化场景的估计偏差；

23、s23、对残差进行尺度归一化，构建服从自由度为3的卡方分布的归一化统计量，通过卡方分布累积分布函数将残差大小映射为像素级动态概率；采用鲁棒中位数聚合将像素级动态概率聚合为物体级场景流动态概率。

24、进一步，所述步骤s2中，还包括：基于扩展卡尔曼滤波的帧间运动一致性构建物体级动态概率，具体过程为：

25、s24、采用匀速运动模型构建物体状态转移方程，状态量包含物体世界坐标系下的位置、朝向与运动速度；以每帧世界坐标系下的3d检测框位置与朝向作为观测值，观测噪声由语义检测置信度自适应调整；

26、s25、结合光流传播掩膜交并比与3d检测框交并比构建匹配得分，结合类别一致性约束建立帧间物体数据关联；

27、s26、通过扩展卡尔曼滤波对物体运动状态进行递推预测与更新，计算观测值与预测值的创新残差，构建残差的归一化马氏距离nis统计量，该统计量服从自由度为3的卡方分布；通过卡方分布累积分布函数将nis统计量映射为物体级帧间运动动态概率。

28、进一步，所述步骤s3中，多源概率自适应融合的具体过程为：

29、s31、将语义先验动态概率、场景流动态概率、帧间运动动态概率分别转换为对数几率值，将概率映射至实数域以实现加性融合；

30、s32、分别计算三类信息源的可靠性评分：语义信息可靠性由实例分割置信度直接表征，帧间运动可靠性由语义置信度与深度可靠性联合表征，场景流可靠性由静态背景残差尺度与参考尺度的偏差表征；

31、s33、根据可靠性评分归一化得到各信息源的自适应融合权重，对三路对数几率进行加权叠加并加入偏置项，得到融合对数几率，经sigmoid函数逆映射得到融合后的物体动态概率。

32、进一步，所述步骤s3中，时序平滑与双阈值迟滞判决具体为：

33、采用指数滑动平均对融合结果进行时序滤波，抑制单帧检测抖动与遮挡导致的瞬时误判；

34、设置高低两个判决阈值，当动态概率高于高阈值时判定为动态物体，低于低阈值时判定为静态物体，处于两阈值中间区间时保持上一时刻的判定结果。

35、进一步，所述步骤s4中，椭球单帧初始化的具体过程为：

36、s41、将相机坐标系下的3d检测框通过初始位姿转换至世界坐标系，以检测框中心坐标、绕竖直轴的旋转姿态与半长宽高作为椭球初始参数，完成基于3d检测框的快速初始化；

37、s42、提取语义掩膜内的图像局部特征点，结合深度图反投影得到三维点云，筛选落在3d检测框内的有效点集，经pca分解得到点云的质心、主方向姿态矩阵与三轴半轴长度，完成特征点拟合初始化；

38、s43、当掩膜内有效特征点数量大于设定阈值时，以3d检测置信度为融合权重，对两类初始化结果的中心、姿态、尺度参数进行加权融合，得到最终椭球初始参数；有效特征点不足时直接采用3d检测框初始化结果。

39、进一步，所述步骤s5中，连续帧椭球参数优化的联合观测误差函数为：

40、

41、其中，

q为椭球参数集合，表示连续观测帧集合；表示2d观测误差项、表示3d观测误差项；和分别为2d观测误差项与3d观测误差项的权重。

42、进一步，所述步骤s6中，局部地图联合优化的目标函数为：

43、

44、其中，表示局部关键帧集合；表示第帧关键帧位姿；表示第帧观测到的静态地图点集合；表示第帧中满足筛选条件的椭球集合；表示地图点的三维坐标；表示三维椭球参数；和分别表示地图点和椭球在第帧中的图像观测；、分别为地图点和椭球的投影函数。

45、进一步，所述椭球集合的筛选规则为满足以下任一条件：

46、椭球内部包含的局部静态地图点数量超过3个；

47、椭球投影至当前关键帧的投影椭圆包围框与该帧任一2d检测框的交并比大于设定阈值；

48、椭球对应的三维包围盒与当前关键帧中任一3d检测框的交并比大于设定阈值。

49、本发明的有益效果在于：

50、1、动态分割鲁棒性显著提升。本发明融合语义先验、场景流三维一致性、帧间运动一致性三类互补信息，结合自适应权重与时序平滑机制，有效解决单一信息源在复杂场景下的退化问题。在instancemotseg数据集高动态序列上，miou可达0.755，相比ds-slam提升16.06%，相比dyna-slam提升9.67%，对同向运动、启停频繁的物体识别准确率提升显著。

51、2、里程计定位精度大幅提高。在剔除动态特征的基础上，进一步引入静态物体椭球作为结构化地标，为局部地图优化补充物体级几何约束。在高动态场景下，绝对位姿误差均值相比orb-slam2降低25.22%，相比仅动态去除方案进一步降低3.23%，有效抑制累计漂移。

52、3、物体地标建模稳定性提高。单帧初始化融合3d检测先验与特征点几何拟合，在有效特征点充足时利用点云结构细化椭球参数，在有效特征点不足时保留三维检测框初始化结果；多帧优化进一步结合二维投影与三维几何双约束，降低仅依赖二维投影时产生尺度漂移和视角退化的风险，使静态物体地标能够为后续局部地图优化提供稳定的物体级结构约束。