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

摘要

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