基于卡尔曼和icp融合的位姿优化方法、系统、设备及介质

本发明公开了一种基于卡尔曼和icp融合的位姿优化方法、系统、设备及介质，属于场景平面拟合技术领域，其目的在于解决现有技术在狭长空间、重复纹理等场景下位姿估计精度低的问题。其通过先构建地图；再利用里程计、里程计运动模型对位姿进行积分得到的标准状态进行修正、迭代，得到真实状态；然后经ICP匹配后，将得到的滤波位姿和匹配位姿进行融合，得到位姿估计结果；最后将激光雷达坐标系下的点云信息通过位姿估计结果变化到世界坐标系下，进行地图更新。通过引入icp辅助滤波器并结合误差卡尔曼滤波对初始位姿进行估计，采用自适应阈值匹配策略和稳健的ICP算法，有效缓解了狭长空间、重复纹理等场景下位姿估计精度低、累积误差的问题。