CN112684809A - 一种基于数据驱动的水下航行器目标跟踪控制方法 - Google Patents

Abstract

本发明公开了一种基于数据驱动的水下航行器目标跟踪控制方法，包括水声探测、位置预测以及运动控制。水声探测根据AUV当前位姿信息通过机载自动感知模块获取目标的状态信息，位置预测融合机载自动感知模块产生的目标状态信息，通过扩展卡尔曼滤波进行位置预测，运动控制过程在位置预测的驱动下，调整运动控制策略，不断输出位姿到运动控制系统。本发明方法采用的数据驱动控制算法融合机载自动感知模块产生的目标状态信息，对目标跟踪控制具有较好的自适应能力和较强的鲁棒性，可有效提高海流条件下水下自主航行器的目标跟踪控制效果。

Description

一种基于数据驱动的水下航行器目标跟踪控制方法

技术领域

本发明属于水下航行器技术领域，具体涉及一种水下航行器目标跟踪控制方法。

背景技术

在各种海洋技术中，水下自主航行器(AUV)具有高机动性，高稳定性，高续航性的优点，被广泛应用在科研考察、水下作业和军事活动中，水下目标跟踪逐渐成为AUV水下自主作业的关键技术之一。AUV目标跟踪技术对于海洋国土监视、近岸预警或近海防御、海底管线跟踪与维修，具有重要的研究价值。

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

发明内容

为了克服现有技术的不足，本发明提供了一种基于数据驱动的水下航行器目标跟踪控制方法，包括水声探测、位置预测以及运动控制。水声探测根据AUV当前位姿信息通过机载自动感知模块获取目标的状态信息，位置预测融合机载自动感知模块产生的目标状态信息，通过扩展卡尔曼滤波进行位置预测，运动控制过程在位置预测的驱动下，调整运动控制策略，不断输出位姿到运动控制系统。本发明方法采用的数据驱动控制算法融合机载自动感知模块产生的目标状态信息，对目标跟踪控制具有较好的自适应能力和较强的鲁棒性，可有效提高海流条件下水下自主航行器的目标跟踪控制效果。

本发明解决其技术问题所采用的技术方案包括如下步骤：

步骤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,O_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的偏航角。

由于采用了本发明的一种基于数据驱动的水下航行器目标跟踪控制方法，带来了如下有益效果：

1.本发明的水下自主航行器的目标跟踪控制方法可在有海流的情况下实现水下自主航行器的目标跟踪控制。

2.本发明方法采用的数据驱动控制算法融合机载自动感知模块产生的目标状态信息，对目标跟踪控制具有较好的自适应能力和较强的鲁棒性。

3.本发明方法简单可行，工作可靠，可有效提高海流条件下水下自主航行器的目标跟踪控制效果。

附图说明

图1为本发明方法的水下航行器目标跟踪控制框图。

图2为本发明方法的扩展卡尔曼滤波算法流程图。

图3为本发明方法的水下航行器偏航控制的基本过程图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

本发明的一种基于数据驱动的水下航行器目标跟踪控制方法，包括水声探测、位置预测以及运动控制。水声探测根据AUV当前位姿信息通过机载自动感知模块获取目标的状态信息；位置预测融合机载自动感知模块产生的目标状态信息，通过扩展卡尔曼滤波进行位置预测；运动控制过程在位置预测的驱动下，调整运动控制策略，不断输出位姿到运动控制系统。

如图1所示，本发明提供一种基于数据驱动的水下航行器目标跟踪控制方法，包括如下步骤：

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

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

步骤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所示。

步骤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的偏航角。

Claims (1)

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的偏航角。

Images