面向动态环境的视觉惯性导航系统

本申请提出一种面向动态环境的视觉惯性导航系统，包括视觉惯性状态估计器和鲁棒的位姿图优化部分，视觉惯性状态估计器包括动态物体特征过滤模块，动态物体特征过滤模块用于对相机采集的当前图像进行语义分割，得到语义图像；并对图像特征进行运动一致性检测，得到一致性检测结果；基于带有语义标签的语义图像和一致性检测结果，过滤掉当前图像中的动态物体上的特征点；鲁棒的位姿图优化部分用于采用非线性因子恢复NFR从基于滑动窗口优化的VIO中提取序列约束的最优协方差；以及对闭环约束添加选择性先验因子构建选择性闭环约束；再将最优协方差和选择性闭环约束输入鲁棒的位姿图优化部分的位姿图优化模块，以提高系统对动态环境的适应性。