[发明专利]一种基于数据驱动的水下航行器目标跟踪控制方法

权利要求书

1.一种基于数据驱动的水下航行器目标跟踪控制方法，其特征在于，包括以下步骤：

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

步骤2：根据步骤1获取的目标当前位置采用扩展卡尔曼滤波方法预测并更新目标位置；

步骤2-1：定义包含加性噪声形式的目标系统运动模型如式(1)所示：

式中，y_k为k时刻目标系统观测值，f(·)为服从于高斯概率分布的运动学状态方程和量测方程的系统噪声，h(·)为服从于高斯概率分布的运动学状态方程和量测方程的量测噪声，w_k-1和v_k均为服从于零均值的高斯白噪声分布，即w_k-1～N(0,Q_k-1)，v_k～N(0,R_k)，Q_k-1和R_k为高斯分布的标准差；

步骤2-2：水下航行器获取目标位置时间更新；

其中，为k-1时刻系统状态估计值，为k时刻系统状态估计值，P_k-1为k-1时刻系统滤波方差，P_k/k-1为k时刻系统滤波方差，为线性化状态转移矩阵，为线性化状态转移矩阵的转置，Q_k-1为高斯分布的标准差；

步骤2-3：目标位置量测更新；

其中，为k时刻系统观测估计值，为k时刻目标位置计值，K_k为增益矩阵，为k时刻系统滤波方差估计值，为服从于高斯概率分布的运动学状态方程和量测方程的量测噪声估计值，为h(x)对x_k求偏导的雅可比矩阵，为h(x)对x_k求偏导的雅可比矩阵；

以探测到的目标位置为输入，经过获取目标位置的时间更新和目标位置量测更新，预测目标出现的位置，依次迭代，保持对目标运动的预测；

步骤3：设计运动控制过程，控制水下航行器的偏航运动，保持对目标的持续跟踪；

步骤3-1：根据水下航行器最小偏航转弯半径，计算惯性坐标系下水下航行器转弯圆心O_y＝(x_y,y_y,z_y)^T的坐标：

式中，为惯性坐标系原点O指向转弯圆心O_y的向量，为惯性坐标系原点O指向水下航行器位置P_r的向量；为水下航行器的当前航向，为当前航向的法向量，R_t为水下航行器最小偏航转弯半径，与R_t对应的最大偏航角为ψ_max，P_r＝(x_r,y_r,z_r)^T为惯性坐标系下水下航行器的当前位置；P_t＝(x_t,y_t,z_t)^T为惯性坐标系下目标更新位置；

步骤3-2：判断(x_t-x_y)²+(y_t-y_y)²+(z_t-z_y)²R²：如果成立，则水下航行器按照现有偏航角继续航行；否则，水下航行器调整偏航角为ψ_max；

步骤3-3：在运动控制过程中，水下航行器在t_k时刻的位置为S_k，水下航行器的速度为v，水下航行器在载体坐标系下的速度为v₁＝(v,0,0)^T，由此得到水下航行器在惯性坐标系下的速度为由式(11)计算：

v₀＝T₁^-1(φ_k,θ_k,ψ_k)v₁ (11)

式中，φ_k,θ_k,ψ_k分别表示t_k时刻的水下航行器的横滚角、俯仰角以及偏航角，当进行偏航角调整时，ψ_k＝ψ_max，则t_k+1时刻水下航行器的位置为：S_k+1＝S_k+v₀(t_k+1-t_k)；

当AUV到达指定位置(x_p,y_p,z_p)^T后，即满足：

水下航行器获取新的目标位置并更新AUV的偏航角。