一种基于开关约束的水下航行器定位鲁棒优化方法

摘要

本发明涉及一种基于开关约束的水下航行器定位鲁棒优化方法，针对传统水下航行器导航控制方法未能对错误闭环实现鲁棒性的缺点，基于General Graph Optimization(g2o)和开关约束的导航控制方法，采用一种开关约束后端优化方法，利用节点因子图，设计了一种消除地图构建过程中产生的错误闭环的方法。本发明解决现有水下航行器在同步定位与地图构建过程中产生错误闭环而导致其所构建地图出现错误的局限，使得航行器在地图构建过程中对错误闭环进行处理并加权为零，并且添加开关先验约束，以消除错误闭环带来的不利影响。理论证明了该发明的鲁棒性，实验验证了该发明的有效性，对减小水下航行器地图构建错误率有良好的效果。