基于关键约束帧的前视成像声呐水下定位与导航方法

摘要

本发明属于水下自主定位导航技术领域，公开了一种基于关键约束帧的前视成像声呐水下定位与导航方法。其目的在于识别并充分利用过往声呐图像中的有效约束信息对当前位姿和水下特征进行定位。主要步骤有：前端数据处理，包括二维声呐图像的特征提取与匹配、惯性测量数据处理；后端弹性滑动窗口优化，包括声呐关键约束帧的识别与存储、从关键约束帧库中选择可加入滑动窗口优化的过往帧、窗口大小控制与优化、更新机器人位姿和水下特征位置。本发明方法可提高水下机器人定位和导航系统的精度，同时保证有限的优化规模，在声呐图像前端出现异常值时也可以及时在后端检测并处理，以保证系统的鲁棒性。