基于长期趋势因子的因子图优化方法

本公开提供一种基于长期趋势因子的因子图优化方法及程序产品，包括：获取雷达、相机、IMU等传感器数据；对采集到的数据进行数据预处理，等到初始特征矩阵等信息；使用雷达子模块进行初步定位，获得雷达模块初始位姿；使用视觉子模块进行初步定位，获得视觉模块初始位姿；分别针对雷达子模块和视觉子模块，计算和视觉模块置信度；引入长期趋势因子，辅助雷达因子、视觉因子和预积分因子优化机器人位姿；使用最新帧的位姿信息，更新长期趋势因子，并输出最终的机器人位姿。本公开能够在面对弱纹理和光照不足的环境下，实现实时的高鲁棒性和较高精度的定位和建图，提高了SLAM在恶劣环境下的定位精度和鲁棒性。