一种结合体素地图联合优化的视觉惯性定位方法及装置

摘要: 本发明属于slam,尤其涉及一种结合体素地图联合优化的视觉惯性定位方法及装置。背景技术:1、传统的视觉-惯性地图构建slam系统在后端主要针对相机位姿与稀疏特征...
一种结合体素地图联合优化的视觉惯性定位方法及装置

本发明属于slam,尤其涉及一种结合体素地图联合优化的视觉惯性定位方法及装置。

背景技术:

1、传统的视觉-惯性地图构建slam系统在后端主要针对相机位姿与稀疏特征点进行联合优化,而稠密地图的构建通常依赖前端的深度融合过程,将多帧图像和惯性测量单元imu信息生成的深度结果直接集成到tsdf(truncated signed distance function,截断符号距离函数)体素网格中。这种方式虽然能够快速生成稠密地图,但由于缺乏后端的全局几何约束,一旦出现遮挡、动态物体干扰或深度估计误差累积,就容易导致稠密地图的几何结构与相机轨迹之间产生不一致,从而影响整体系统的鲁棒性和精度。

2、此外,现有的视觉–惯性地图构建slam系统大多采用静态边缘化策略。当滑动窗口不断推进,早期观测到的体素和状态变量会逐渐失效,但系统在边缘化过程中往往使用固定的先验线性化点,这种做法难以适应后续状态的变化。随着优化迭代的进行,固定先验与当前状态之间可能产生偏差,从而造成收敛精度下降,甚至影响长期一致性。在复杂场景下,这些问题会进一步放大,使系统难以同时保持高精度的定位和高保真度的地图重建。

3、现有技术主要存在以下问题:

4、1、tsdf地图未参与后端优化,几何约束缺失:稠密地图精度依赖于前端深度融合,后端优化未能利用tsdf信息约束相机位姿,缺乏闭环反馈与约束增强;

5、2、早期体素信息冗余,未有效边缘化:滑动窗口推进后,部分体素不再参与观测更新,未能及时边缘化,导致系统内存开销持续增大。

技术实现思路

1、为解决以上技术问题,本发明提供一种结合体素地图联合优化的视觉惯性定位方法及装置,其步骤简单,使用方便;能够有有效解决现有技术中tsdf地图未参与后端优化导致几何约束缺失,以及静态边缘化策略导致先验不一致、定位精度下降的问题。

2、为实现上述技术目的,本发明公开一种结合体素地图联合优化的视觉惯性定位方法,其特征在于,包括如下步骤:

3、s1、针对图像数据进行特征点提取,在体素网格中计算对应特征点的截断符号距离函数;

4、s2、针对图像数据设置滑动窗口,提取滑动窗口中的相机位姿、惯性测量单元状态、相机到惯性测量单元的外参以及局部的截断符号距离函数,构建相应的联合优化变量;

5、s3、基于局部截断符号距离函数构建残差模型与目标优化函数,设计对应相机位姿的残差函数,并设计对应的正则项以提升算法的鲁棒性;

6、s4、对滑动窗口进行边缘化,记录边缘化时的线性化点状态,在每轮非线性迭代后计算当前状态与线性化点之间的偏差,若偏差超过设定阈值,则进行在线动态更新。

7、进一步,s1具体过程为:

8、输入图像数据,提取特征点并计算特征点的方向向量,结合深度得到特征点在相机坐标系下的坐标;

9、通过前端深度融合多帧图像和惯性测量单元imu数据生成素网格,对于体素网格中任意中心点,将中心点投影至像素平面得到投影函数;基于投影式截断符号距离函数tsdf定义,体素的有符号距离为:

10、,

11、其中为相机的投影函数,反映空间点在深度图像上的深度信息,为相机坐标系下的z轴坐标;

12、体素的截断符号距离函数tsdf值:,

13、其中,为截断阈值。

14、进一步,s2中联合优化变量包括:

15、相机位姿为每帧相机的世界坐标系下位姿:,其中表示第i帧相机坐标系到世界坐标系的变换矩阵,为旋转参数,为平移参数,旋转参数采用四元数表示;

16、惯性测量单元状态:包括速度、加速度偏置、陀螺仪偏置;

17、相机到惯性测量单元的外参:相机与惯性测量单元之间的相对位姿用表示;

18、局部截断符号距离函数:从滑动窗口内观测到的所有三维点中提取体素插值所涉及的中心体素,记为,其中表示第k个体素的tsdf值。

19、进一步,s3中,目标优化函数为对联合优化变量最小化以下残差平方和:

20、,

21、其中,为联合优化变量的集合,为视觉重投影残差,为截断符号距离函数几何一致性残差,为截断符号距离函数正则化残差,为惯性测量单元残差。

22、进一步,视觉重投影残差计算方法为:

23、对于任意一个特征点,特征点在多个帧中的投影误差定义为:

24、,

25、其中为第帧中的三维点的坐标,为投影函数,为第帧中该特征点的观测值,和分别为第帧和第帧相机坐标系到世界坐标系的变换矩阵。

26、进一步,截断符号距离函数几何一致性残差的计算方法为:

27、对于每个三维点,获取三维点的当前估计位置,在截断符号距离函数网格中通过三维三线性插值获取表面的截断符号距离函数值:

28、,

29、其中,为插值系数,为邻域体素值;理想表面对应截断符号距离函数值为零,残差为。

30、进一步,截断符号距离函数正则化残差具体为:

31、,

32、其中,为初始化或上一帧的tsdf融合结果,为正则化强度,用于防止截断符号距离函数漂移。

33、进一步,s4具体为:

34、边缘化后,保存保留变量在边缘化时的状态,,其中分别为边缘化时保留的相机位姿、惯性测量单元速度与体素值状态;

35、每轮非线性迭代后计算当前状态与线性化点状态之间的平移偏差、旋转偏差或截断符号距离函数漂移,若任一偏差超过设定阈值,则判定先验失效,并对先验残差与雅可比矩阵进行更新;

36、若失效,将当前迭代步的联合优化变量作为新的线性化点,重新计算先验残差与雅可比矩阵,更新公式如下:

37、,

38、,

39、其中,为更新后的残差值,为原雅可比矩阵,为更新后的雅可比矩阵,为当前迭代步的联合优化变量,为线性化点状态,无需重建整个信息矩阵或因子图,即可保持先验一致性。

40、一种结合体素地图联合优化的视觉惯性定位方法定位装置,包括:

41、数据预处理模块,针对图像数据进行特征点提取,并计算对应特征点的截断符号距离函数;

42、联合优化变量构建模块,针对图像数据设置滑动窗口,提取滑动窗口中的相机位姿、惯性测量单元状态、相机到惯性测量单元的外参以及局部的截断符号距离函数,构建相应的联合优化变量;

43、目标优化函数模块,根据部分点的体素值构建相应的残差模型与目标优化函数,基于局部截断符号距离函数构建残差模型,设计对应相机位姿的残差函数,并设计对应的正则项以提升算法的鲁棒性;

44、先验一致性更新模块,记录边缘化时的线性化点状态,在迭代后计算偏差,超过阈值时对先验残差与雅可比矩阵进行一阶更新。

45、一种电子设备,包括:一个或多个处理器;存储器,用于存储一个或多个程序,其中,当所述一个或多个程序被所述一个或多个处理器执行时,使得所述一个或多个处理器实现结合体素地图联合优化的视觉惯性定位方法。

46、有益效果:本发明将tsdf体素值直接引入到后端联合优化中,使稠密几何信息能够反向约束相机轨迹,有效消除了由于遮挡或噪声引起的几何偏差,显著提升了定位与建图的一致性。与此同时,本技术摒弃了固定先验的静态边缘化策略,引入动态边缘化与在线先验刷新机制,在检测到当前状态与先验不一致时可即时更新残差与雅可比,从而保持系统在长时间运行下的收敛精度和稳定性。本发明采用tsdf与位姿的联合优化机制:将tsdf体素值引入后端联合优化,与传统slam重投影误差协同构造目标函数。本发明采用tsdf残差的插值与正则构造方法:使用8体素插值计算tsdf误差,并添加正则项避免tsdf变量漂移。