[发明专利]一种无人水下航行器的目标跟踪控制方法

说明书

本发明公开了一种面向水声探测的水下航行器目标跟踪控制方法，所述方法包括：步骤1)在t_k时刻水下航行器根据水声探测获取目标当前位置；步骤2)以目标当前位置为输入，基于粒子滤波预测目标可能出现的位置，获取目标更新位置；步骤3)建立运动控制惯性坐标系和载体坐标系，由此得到水下航行器运动学的参数；步骤4)根据当前时刻水下航行器的姿态和位置以及目标更新位置，如果偏航角需要调整则对水下航行器的偏航角进行调整；步骤5)如果俯仰角需要调整则对水下航行器的俯仰角进行调整；步骤6)水下航行器基于偏航角和俯仰角确定的方向向目标前进，计算获取水下航行器在t_k+1时刻的位置和姿态，并令k＝k+1，转入步骤1)；直至目标跟踪结束。

技术领域

本发明涉及无人水下航行器建模领域，尤其涉及一种水下航行器的目标跟踪控制方法。

背景技术

无人水下航行器(UUV)是一种海上力量倍增器，拥有广泛而重要的军事、科研用途，在未来的海洋探测中发挥不可替代的作用。UUV搭载多种传感器，可以在复杂海况、恶劣环境下完成水下警戒、跟踪、勘探、中继通信等水下航行任务。随着技术的进步，UUV拥有更多的潜在应用领域，尤其是对人员要求严格的深海环境。

UUV在水下作业过程中，一种重要的探测手段是成像探测，包括光视觉成像和声纳探测成像。在声纳探测成像方面，一种方法是基于侧扫声纳的海底成像方法，利用运动信息和声纳探测模型实现UUV定位。在光视觉成像方面，一种方法是通过分割线性成像、GVF-SNAKE模型、PSO-BP算法实现目标识别。但是基于成像的探测方法，存在时效性差、能耗高的缺点，在实用中还有很多瓶颈。。

发明内容

本发明的目的是服务于水下探测体系中UUV装备部署，满足UUV主动探测时对目标的持续有效跟踪，提出一种水下航行器的目标跟踪控制方法。

为了实现上述目的，本发明提出了一种水下航行器的目标跟踪控制方法，所述方法包括：

步骤1)在t_k时刻，水下航行器根据水声探测获取目标当前位置，k为正整数；

步骤2)以目标当前位置为输入，基于粒子滤波预测目标可能出现的位置，获取目标更新位置；

步骤3)建立运动控制惯性坐标系和载体坐标系，由此得到水下航行器运动学的参数：姿态和位置；

步骤4)根据当前时刻水下航行器的姿态和位置以及目标更新位置，判断水下航行器是否进行偏航角的调整，如果不需要调整，转入步骤5)，否则，对水下航行器的偏航角进行调整，转入步骤5)；

步骤5)判断水下航行器是否进行俯仰角的调整，如果不需要调整，转入步骤6)，如果需要调整，对水下航行器的俯仰角进行调整，转入步骤6)；

步骤6)水下航行器基于偏航角和俯仰角确定的方向向目标前进，计算获取水下航行器在t_k+1时刻的位置和姿态，并令k＝k+1，转入步骤1)；直至目标跟踪结束。

作为上述方法的一种改进，所述步骤1)中水下航行器根据水声探测获取目标位置的具体过程为：基于主动声纳方程，分析目标强度、海洋环境噪声、检测阈和传播损失因素，计算声纳探测作用距离；如果目标在声纳探测的作用范围内，则输出t_k时刻目标当前位置x_k。

作为上述方法的一种改进，所述步骤2)的具体过程为：

选取N个采样点以及权重集合为其中表示t_k时刻根据水声探测得到的目标位置选取的第i个采样点，对应的权重为计算系统状态的后验概率密度p(x_k|y_1:k)为：