一种基于USBL距离量测信息的多UUV协同导航方法及其系统

本发明涉及水下航行器导航技术领域，具体涉及一种基于USBL距离量测信息的多UUV协同导航方法及其系统，包括对每个从航行器预定航行轨迹，并设定主航行器为初始化状态；建立每个从航行器以及主航行器的运动学模型；获取主航行器的航行轨迹；每个从航行器沿着预定的航行轨迹进行航行，同时，主航行器按照规划的航行轨迹运动，每个从航行器根据交互的信息利用迭代扩展卡尔曼滤波算法修正导航误差；每个从航行器根据修正后的导航继续航行。本发明基于预先设定的从UUV轨迹，综合时间差分法与分层强化学习算法，对主UUV进行运动规划，之后在导航过程中利用迭代扩展卡尔曼滤波(IEKF)算法进行状态估计，从而有效提升从UUV的定位精度。