买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：西北工业大学深圳研究院;西北工业大学

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

主权项：1.一种基于USBL距离量测信息的多UUV协同导航方法，其特征在于，用于主航行器与多个从航行器协同导航，包括：对每个从航行器预定航行轨迹，并设定主航行器为初始化状态；建立每个从航行器以及主航行器的运动学模型；基于每个从航行器以及主航行器的运动学模型，根据每个从航行器预定的航行轨迹对主航行器进行运动规划，获取主航行器的航行轨迹；每个从航行器沿着预定的航行轨迹进行航行，同时，主航行器按照规划的航行轨迹运动，主航行器每隔一段时间与每个从航行器进行信息交互，每个从航行器根据交互的信息利用迭代扩展卡尔曼滤波算法修正导航误差；每个从航行器根据修正后的导航继续航行；所述主航行器的航行轨迹是按照以下步骤获取：根据每个从航行器预定的航行轨迹通过强化学习训练，获取主航行器对于每个从航行器的状态动作价值函数；根据主航行器对于每个从航行器的状态动作价值函数，获取主航行器在导航过程中最优轨迹，将最优轨迹作为主航行器的航行轨迹；所述主航行器对于每个从航行器的状态动作价值函数，包括：定义主航行器与每个从航行器之间的状态集合；根据主航行器的偏航角速度范围确定主航行器的动作集合；获取当前时刻每个从航行器在预定航行轨迹上的位置信息，以及主航行器在当前时刻的位置信息；根据当前时刻每个从航行器的位置信息和主航行器的位置信息，从状态集合中，选取当前时刻主航行器与每个从航行器之间的状态；根据当前时刻主航行器与每个从航行器之间的状态，从动作集合中选取动作；主航行器执行选取的动作，根据运动学模型更新主航行器的位置，并获取下一时刻主航行器的位置信息；根据下一时刻主航行器的位置信息和每个从航行器的位置信息，获取下一时刻主航行器与每个从航行器之间的状态；根据当前时刻主航行器与每个从航行器之间的状态，与下一时刻主航行器与每个从航行器之间的状态，获取当前时刻动作的奖励；根据当前时刻动作的奖励更新状态动作价值函数；依次沿着每个从航行器在预定航行轨迹上的位置信息，更新主航行器与每个从航行器之间的状态，达到最大训练次数，获取主航行器对于每个从航行器的状态动作价值函数；所述主航行器在导航过程中最优轨迹是按照以下步骤获取：将整个从航行器的导航过程划分为多个时长相等的子导航过程；在每个子导航过程中，根据上一时刻与当前时刻的主航行器对于每个从航行器的状态，获取上一时刻每个从航行器的代价值；选取最大代价值对应的从航行器，以及与主航行器的状态；获取最大代价值对应的从航行器的状态动作价值函数；根据最大代价值对应的从航行器的状态动作价值函数选取主航行器的动作；主航行器执行该动作，直到该子导航过程结束，更新主航行器位置，并获取主航行器在当前子导航过程中的航行轨迹；以此获取每个子导航过程中主航行器的航行轨迹；即得到主航行器在整个导航过程中的最优轨迹；主航行器每隔一段时间与每个从航行器交互的信息，包括：主航行器与每个从航行器的相对距离，以及主航行器的导航位置；每个从航行器根据交互的信息利用迭代扩展卡尔曼滤波算法修正导航误差，包括：根据主航行器与每个从航行器的相对距离，以及主航行器的导航位置，结合从航行器的运动学模型，利用迭代扩展卡尔曼滤波算法进行从航行器的状态估计；每个从航行器根据对应的状态估计调整导航位置；利用迭代扩展卡尔曼滤波算法进行从UUV的状态估计；初始化状态估计和估计误差协方差矩阵P0，完成以下步骤：a利用扩展卡尔曼滤波算法进行预滤波，执行以下方程： 式中，为扩展卡尔曼滤波下k时刻状态先验估计，为扩展卡尔曼滤波下k时刻状态先验估计误差协方差矩阵，Kk,E为k时刻扩展卡尔曼增益，为扩展卡尔曼滤波下k时刻状态后验估计；b在获得扩展卡尔曼滤波的状态估计后，通过RTS平滑算法求取平滑值 c利用和进行状态估计修正，分别将其作为系统方程和量测方程的泰勒级数展开点，执行以下方程： 式中为迭代扩展卡尔曼滤波下k时刻状态先验估计，为迭代扩展卡尔曼滤波下k时刻状态先验估计误差协方差矩阵，Kk为k时刻迭代扩展卡尔曼增益，为迭代扩展卡尔曼滤波下k时刻状态后验估计，Pk为迭代扩展卡尔曼滤波下k时刻状态后验估计误差协方差矩阵， d返回和Pk，更新采样点k并继续下一次滤波，直到导航过程结束。

全文数据：

权利要求：

百度查询： 西北工业大学深圳研究院 西北工业大学 一种基于USBL距离量测信息的多UUV协同导航方法及其系统