一种基于滤波与图优化结合的自动驾驶车辆实时定位与建图方法

本发明提供了一种基于滤波与图优化结合的自动驾驶车辆实时定位与建图方法，前端，基于IMU信息预积分，得到位姿先验状态量，基于LIDAR点云的特征提取与点云配准，得到位姿观测量，位姿先验状态量和位姿观测量基于自适应误差迭代卡尔曼滤波算法进行融合，得到位姿后验状态量；基于自适应误差迭代卡尔曼滤波算法的过程噪声与观测噪声可根据残差与新息进行实时更新；在后端，基于前端位姿估计结果、GPS因子、回环检测因子和地面约束因子的误差函数构建目标函数，求解目标函数的最优解，得到自动驾驶车辆不同时刻位姿的最优估计值。本发明提高了复杂环境中自动驾驶车辆位姿估计动态响应能力以及因子图优化的定位精度与稳定性。