基于改进关键帧决策及姿态优化的水下定位与建图方法

本发明公开了基于改进关键帧决策及姿态优化的水下定位与建图方法。首先获取AUV所配置传感器的数据；处理得到的传感器的数据，初始化系统参数，进行局部地图的跟踪，在基于ORB‑SLAM3关键帧选择的基础上，根据IMU的观测信息判断当前是否为动态环境，AUV是否剧烈运动，再继续进行关键帧的选择；对选择的关键帧的姿态进行优化，使用双约束进行姿态校正，恢复大视角快速弯曲运动下的局部轨迹，生成新的地图点；进行闭环检测与校正；生成全局轨迹与地图。本发明解决了自主水下航行器可能遇到动态环境或者本身运动剧烈导致位姿漂移的问题，同时使得视觉惯性同步定位与地图构建系统的准确性和鲁棒性大幅提高。