CN113064434A - 一种基于主从式编队的水面目标检测与跟踪控制方法 - Google Patents

Abstract

本发明公开了一种基于主从式编队的水面目标检测与跟踪控制方法，将双目视觉摄像机分别固定在两个主USV上，采用三角测距法获得目标USV和主USV的实时距离。主USV上安装的GPS系统可获得实时定位信息，而目标USV上装有航向、航速、航向角速度传感器，利用主USV和目标USV安装的通讯设备进行交互。当目标USV接收到主USV的测距信息和位置坐标后完成定位处理将结果通过通讯设备发送给两个主USV，两个主USV以目标USV为领航者一起编队运动，并保持与目标USV之间的距离均等于最小安全距离，且两个主USV与目标USV连线之间的夹角为90°。本发明能够优化水面目标USV定位的性能，大大降低光照变化、能见度等外界因素对目标USV定位精确度的影响，提高对水面目标USV检测和跟踪的精度。

Description

一种基于主从式编队的水面目标检测与跟踪控制方法

技术领域

本发明属于目标跟踪技术领域，具体涉及一种水面目标检测与跟踪控制方法。

背景技术

USV(Unmanned Surface Vehicle)作为一种水面自主运载工具，可以在危险的海洋环境中完成多种复杂任务的优势正受到广泛的运用，常用于实现水面目标的检测与跟踪。常用的检测设备有雷达、红外传感器、光视觉等。其中，视觉技术获得的目标和环境等区域更直观、细节等信息更详细，有利于识别和后续处理。

由于单个无人船利用双目视觉获取的信息来源单一，容易受光照变化、能见度、目标遮挡以及风浪导致的图像抖动等因素的影响，产生观测误差而导致跟踪精度的降低。

发明内容

为了克服现有技术的不足，本发明提供了一种基于主从式编队的水面目标检测与跟踪控制方法，将双目视觉摄像机分别固定在两个主USV上，采用三角测距法获得目标USV和主USV的实时距离。主USV上安装的GPS系统可获得实时定位信息，而目标USV上装有航向、航速、航向角速度传感器，利用主USV和目标USV安装的通讯设备进行交互。当目标USV接收到主USV的测距信息和位置坐标后完成定位处理将结果通过通讯设备发送给两个主USV，两个主USV以目标USV为领航者一起编队运动，并保持与目标USV之间的距离均等于最小安全距离，且两个主USV与目标USV连线之间的夹角为90°。本发明能够优化水面目标USV定位的性能，大大降低光照变化、能见度等外界因素对目标USV定位精确度的影响，提高对水面目标USV检测和跟踪的精度。

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

步骤1：所述主从式编队包括两个主USV和一个目标USV，两个主USV分别为USV1和USV2；在两个主USV和目标USV入水之前，对两个主USV和目标USV进行时钟同步；

步骤2：将两个主USV与目标USV放入水中；

两个主USV与目标USV的初始广义位置向量分别定义为：

X_n1(0)＝(x_n1(0) y_n1(0) ψ_n1(0))^T

X_n2(0)＝(x_n2(0) y_n2(0) ψ_n2(0))^T

X(0)＝(x(0) y(0) ψ(0))^T

其中，X_n1(0)和X_n2(0)分别为USV1和USV2的初始广义位置向量，X(0)为目标USV的初始广义位置向量；(x_n1(0) y_n1(0))和(x_n2(0) y_n2(0))分别为USV1和USV2在水平面内的位置，(x(0)y(0))为目标USV在水平面内的位置，三个USV的位置都由安装在自身的GPS测量得到；ψ_n1(0)和ψ_n2(0)分别表示USV1和USV2的初始航向角，ψ(0)表示目标USV的初始航向角，三个初始航向角都由安装在自身的航向传感器测量得到；

步骤3：定义目标USV的状态向量X＝(x y ψ)^T，设定目标USV的扩展卡尔曼滤波器的滤波初值

状态估计误差协方差矩阵的初值为

采样周期为T；

步骤4：目标USV开始在水面航行；

步骤5：在第k个采样时刻，装在USV1和USV2上的双目摄像头分别测量出目标USV的相对方位信息，并分别将自身的位置信息(x_n1(k) y_n1(k))、(x_n2(k) y_n2(k))发送给目标USV；同时，目标USV上安装的速度传感器、航向传感器和航向角速度传感器分别测量出目标USV在k时刻的航速V(k)、航向ψ(k)和航向角速度ω(k)，目标USV的测量噪声协方差矩阵为

分别为航速V(k)、航向ψ(k)和航向角速度ω(k)的噪声方差；

步骤6：利用卡尔曼滤波器对目标USV状态进行预测；

步骤6-1：利用式(1)计算目标USV在k时刻的状态预测值：

其中，

表示目标USV在k-1时刻的状态预测值；

步骤6-2：利用式(2)计算目标USV在k时刻状态误差协方差矩阵的预测值：

其中

P(k-1|k-1)表示目标USV在k-1时刻状态误差协方差矩阵的预测值；

步骤7：USV1和USV2上的双目摄像头通过目标USV在左右摄像头中成像的视差进行三角测距，分别获得USV1和USV2与目标USV的实时距离；

步骤8：利用距离量测对目标USV状态进行滤波；

步骤8-1：利用式(3)计算目标USV在k时刻的卡尔曼滤波增益矩阵：

其中，

R(k)表示量测噪声，

和

分别表示目标USV在x和y方向状态的预测值；

步骤8-2：利用式(4)计算目标USV在k时刻状态的最优滤波值：

其中

Z(k)表示量测值；

步骤8-3：利用式(5)计算目标USV在k时刻状态误差协方差矩阵的滤波值：

P(k|k)＝P(k|k-1)-K(k)H(k)P(k|k-1) (5)

步骤9：目标USV将k时刻的速度、航向角速度及状态滤波值通过通讯设备分别发送给USV1和USV2；

步骤10：USV1和USV2以目标USV作为领航者，并分别采用主从式编队控制律实现队形编队航行；

步骤10-1：利用式(6)分别计算USV1和USV2与目标USV之间的距离：

其中，

表示目标USV在y方向状态的最优滤波值，i为主USV的序号，i＝1,2；

步骤10-2：利用式(7)分别计算USV1和USV2与目标USV之间的夹角；

其中，

表示目标USV的航向；

步骤10-3：利用式(8)分别计算USV1和USV2与目标USV间距离与最小安全距离之间的误差：

其中，r_min为主USV与目标USV之间的最小安全距离，ρ_i为主USV与目标USV之间的距离；

步骤10-4：利用式(9)分别计算USV1和USV2与目标USV之间的夹角与期望夹角之间的误差：

其中，α₁ ^*(k)＝ε，

其中ε为常值，α_i(k)表示主USV与目标USV之间的方位角；

步骤10-5：利用式(10)分别计算USV1和USV2和目标USV之间的航向角偏差：

步骤10-6：利用式(11)分别计算USV1和USV2的速度：

其中，

为设计的控制增益；ψ_i(k)表示主USV的航向；

步骤10-7：利用式(12)分别计算USV1和USV2的航向角速度：

其中，d_i>0为主USV定位装置沿纵轴距主USV重心的距离；

步骤11：返回步骤5，开始新的循环，直至目标检测与跟踪控制过程结束。

本发明的有益效果如下：

由于两个主USV以目标USV为领航者，随目标USV一起编队航行，保持与目标USV之间的距离均等于最小安全距离，且两个USV与目标USV连线之间的夹角为90度，从而可以满足目标USV移动定位的需要，且目标USV和USV在这种编队队形下的定位精度优于一般的编队队形。

附图说明

图1为本发明中双水面跟踪主USV与目标USV间相对方位示意图。

图2为本发明中对两个水面目标USV的图像及其处理，其中图2(a)为原图像，图2(b)为融合HSV通道的二值图像，图2(c)为筛选轮廓后的画框图像，图2(d)为最终目标检测框图。

图3为本发明中移动背景下的包含遮挡的水面目标USV跟踪效果示意图。

图4为本发明中在不同大小的分离角下的系统定位误差图，其中图4(a)表示系统分离角γ₁₂＝60°，图4(b)表示系统分离角γ₁₂＝50°，图4(c)表示系统分离角γ₁₂＝40°。

图5为本发明实验I中目标USV及主USV的真实轨迹示意图和目标USV定位误差，其中图5(a)为实验I目标USV及主USV的真实轨迹示意图，图5(b)为实验I目标USV定位误差。

图6为本发明实验II中目标USV及主USV的真实轨迹示意图和目标USV定位误差，其中图6(a)为实验II目标USV及主USV的真实轨迹示意图，图6(b)为实验II目标USV定位误差。

具体实施方式

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

为解决单个无人船利用双目视觉获取的信息来源单一易产生观测误差而导致跟踪精度降低的问题，本发明在单无人船实现水面目标USV检测跟踪的基础上，进一步通过信息交互与融合研究了多艘无人船下目标USV检测与跟踪问题，提高对水面目标USV检测和跟踪的精度。

本方法的定位需要两艘搭有GPS和双目视觉相机的USV，使用时，USV可以通过GPS实时测量出自身的位置，并使用相机测算出目标USV与USV的距离，从而解算出目标USV的位置，并对目标USV进行跟踪。

如图1所示，一种基于主从式编队的水面目标检测与跟踪控制方法，包括以下步骤：

步骤1：所述主从式编队包括两个主USV和一个目标USV，两个主USV分别为USV1和USV2；在两个主USV和目标USV入水之前，对两个主USV和目标USV进行时钟同步；

步骤2：将两个主USV与目标USV放入水中；

两个主USV与目标USV的初始广义位置向量分别定义为：

X_n1(0)＝(x_n1(0) y_n1(0) ψ_n1(0))^T

X_n2(0)＝(x_n2(0) y_n2(0) ψ_n2(0))^T

X(0)＝(x(0) y(0) ψ(0))^T

其中，X_n1(0)和X_n2(0)分别为USV1和USV2的初始广义位置向量，X(0)为目标USV的初始广义位置向量；(x_n1(0) y_n1(0))和(x_n2(0) y_n2(0))分别为USV1和USV2在水平面内的位置，(x(0)y(0))为目标USV在水平面内的位置，三个USV的位置都由安装在自身的GPS测量得到；ψ_n1(0)和ψ_n2(0)分别表示USV1和USV2的初始航向角，ψ(0)表示目标USV的初始航向角，三个初始航向角都由安装在自身的航向传感器测量得到；

步骤3：定义目标USV的状态向量X＝(x y ψ)^T，设定目标USV的扩展卡尔曼滤波器的滤波初值

状态估计误差协方差矩阵的初值为

采样周期为T；

步骤4：目标USV开始在水面航行。

步骤5：在第k个采样时刻，装在USV1和USV2上的双目摄像头分别测量出目标USV的相对方位信息，并分别将自身的位置信息(x_n1(k) y_n1(k))、(x_n2(k) y_n2(k))发送给目标USV；同时，目标USV上安装的速度传感器、航向传感器和航向角速度传感器分别测量出目标USV在k时刻的航速V(k)、航向ψ(k)和航向角速度ω(k)，目标USV的测量噪声协方差矩阵为

分别为航速V(k)、航向ψ(k)和航向角速度ω(k)的噪声方差；

步骤6：利用卡尔曼滤波器对目标USV状态进行预测；

步骤6-1：利用式(1)计算目标USV在k时刻的状态预测值：

其中，

表示目标USV在k-1时刻的状态预测值；

步骤6-2：利用式(2)计算目标USV在k时刻状态误差协方差矩阵的预测值：

其中

P(k-1|k-1)表示目标USV在k-1时刻状态误差协方差矩阵的预测值；

步骤7：USV1和USV2上的双目摄像头通过目标USV在左右摄像头中成像的视差进行三角测距，分别获得USV1和USV2与目标USV的实时距离；

步骤8：利用距离量测对目标USV状态进行滤波；

步骤8-1：利用式(3)计算目标USV在k时刻的卡尔曼滤波增益矩阵：

其中，

R(k)表示量测噪声，

和

分别表示目标USV在x和y方向状态的预测值；

步骤8-2：利用式(4)计算目标USV在k时刻状态的最优滤波值：

其中

Z(k)表示量测值；

步骤8-3：利用式(5)计算目标USV在k时刻状态误差协方差矩阵的滤波值：

P(k|k)＝P(k|k-1)-K(k)H(k)P(k|k-1) (5)

步骤9：目标USV将k时刻的速度、航向角速度及状态滤波值通过通讯设备分别发送给USV1和USV2；

步骤10：USV1和USV2以目标USV作为领航者，并分别采用主从式编队控制律实现队形编队航行；

步骤10-1：利用式(6)分别计算USV1和USV2与目标USV之间的距离：

其中，

表示目标USV在y方向状态的最优滤波值，i为主USV的序号，i＝1,2；

步骤10-2：利用式(7)分别计算USV1和USV2与目标USV之间的夹角；

其中，

表示目标USV的航向；

步骤10-3：利用式(8)分别计算USV1和USV2与目标USV间距离与最小安全距离之间的误差：

其中，r_min为主USV与目标USV之间的最小安全距离，ρ_i为主USV与目标USV之间的距离；

步骤10-4：利用式(9)分别计算USV1和USV2与目标USV之间的夹角与期望夹角之间的误差：

其中，α₁ ^*(k)＝ε，

其中ε为常值，α_i(k)表示主USV与目标USV之间的方位角；

步骤10-5：利用式(10)分别计算USV1和USV2和目标USV之间的航向角偏差：

步骤10-6：利用式(11)分别计算USV1和USV2的速度：

其中，

为设计的控制增益；ψ_i(k)表示主USV的航向；

步骤10-7：利用式(12)分别计算USV1和USV2的航向角速度：

其中，d_i>0为主USV定位装置沿纵轴距主USV重心的距离；

步骤11：返回步骤5，开始新的循环，直至目标检测与跟踪控制过程结束。

具体实施例：

1、本发明中采用的扩展卡尔曼滤波原理描述如下：

首先，本发明针对二维平面内的目标USV定位问题，建立了目标USV在地面坐标系下的运动学方程，并在此基础上设计了扩展卡尔曼滤波器。在地面坐标系下，目标USV的运动学状态方程为：

上式可简写为：

X(k+1)＝F(X(k),U(k),W(k))

式中，X(k)＝(x(k) y(k) ψ(k))^T为k时刻目标USV的运动状态，T为采样周期，X(k)＝(V(k) ψ(k) ω(k))^T为k时刻目标USV的航速、航向和航向角速度，其噪声协方差：

由运动学状态方程，计算雅可比矩阵；

选取k时刻目标USV与两个主USV间的距离值作为量测方程:

式中，w_z为量测噪声，其噪声协方差为：

由上式可得雅可比矩阵：

由此可得目标USV状态的扩展卡尔曼滤波器为：

给定卡尔曼滤波器的初始状态

滤波状态初始协方差矩阵P(0|0)，以及第k个采样时刻的系统噪声协方差矩阵Q(k)、测量噪声协方差矩阵R(k)及距离的测量值Z(k)，就可用下列扩展卡尔曼滤波器方程进行迭代求解。

状态预测：

状态预测误差协方差矩阵：

卡尔曼滤波增益矩阵：

K(k)＝P(k|k-1)H^T(k)[H(k)P(k|k-1)H^T(k)+R(k)]^-1

状态滤波：

状态滤波误差协方差矩阵：

P(k)＝P(k|k-1)-K(k)H(k)P(k|k-1)

2、本发明方法中采用的最优定位原理描述如下：

由于本文采用了两个主USV的距离信息来估计目标USV的状态，因此主USV的Fisher信息矩阵可定义为：

由于目标USV的状态滤波误差协方差矩阵满足：

P(k|k)≥J^-1(k)

因此，J(k)越大，则目标USV的状态的估计精度就越高。

先假定目标USV能同时测量两个主USV的距离和方位，即：

其中，v_i(k)是不相关的高斯白噪声，该式的雅可比矩阵为：

此时，可得：

计算该式的行列式可得

上式可简化为：

式中，n和m为大于0的常数。为了得到最优定位性能，需要：

式中，r_min为目标USV与USV之间的最小安全距离。其最优值为：

3、本发明方法中水面目标USV测距原理描述如下：

本发明中，目标USV与主USV间的距离由双目视觉摄像头获取，原理在于使用目标USV在左右摄像头中成像的视差进行三角测距来获得实时距离。本发明中使用的ZEDSteroLab双目摄像头使用张正友标定法进行矫正和极线对准，对20m内物体的测距误差在5％以内。在本发明中实现该方法的关键在于如何检测和跟踪图像中运动目标USV，以及获取视差图过程的准确性和实时性。

本发明将原图像进行HSV通道分离，并统计获得水面波纹干扰在各通道中的噪声密集区间，再将去除该区间后的各通道图像使用阈值分割、图像形态学等方法尽可能去除噪声，保留目标USV图像。最后将二值图转化后的各通道图像合并，实现水面目标USV的快速检测。该方法对含水面目标USV的视频的检测效果如图2所示。

本发明使用KCF算法目标USV实现跟踪，并在提取候选区域的步骤中引入了尺度因子以解决目标USV尺度变换问题，同时在提取候选区域特征点描述算子引入主成分分析法进行降维以减少计算量，保证检测和跟踪过程的实时性。该方法对含水面目标USV的视频的跟踪效果如图3所示，改进后方法的每秒平均帧数为23.65，满足实时性要求。

本发明选用SGBM算法进行立体匹配以获得视差图，并通过使用分割出含有目标USV区域的原图、使用基于积分图法的高斯滤波进行快速空洞填充等方法在保证足够精确度的前提下降低了计算量以保证实时性，最后通过视差计算出目标USV与USV的距离。

4、本发明方法中采用的主从式编队控制原理描述如下：

从上面的分析可知:当目标USV与主USV之间的距离为最小安全距离且两个主USV与目标USV连线之间的夹角为

时，可得到最优的定位性能。为了得到最优的定位性能，以目标USV作为领航者，两个主USV作为跟随者，形成固定的编队一起航行，且满足上述的距离与角度要求。

设目标USV当前所在的位置为(x,y)、航向角为ψ、速度为V、航向角速度为ω。主USV的位置为(x_i,y_i),航向角为ψ_i、速度为V_i、航向角速度为ω_i。定位与通讯设备沿纵轴距重心的距离为d_i,i＝1,2。则位定位与通讯设备的位置的运动学方程可表示为：

主USV与目标USV之间的距离ρ_i以及夹角α_i的运动学方程可表示为：

ρ_i＝V_i cos(α_i+ψ_i)-Vcosα_i+d_iω_i sin(α_i+ψ_i)

领航-跟随者的相互位置关系由(ρ_i,α_i)确定。因此编队控制的目标USV为：

当t→∞时，有：

和

分别为期望的距离和角度。令：

可将运动学方程表示为：

其中

显然，

因此，主USV的输入u_i可以描述为：

若使

可选择

K＝diag[k₁,k₂]，k₁,k₂∈R^*为设计的控制增益，从而可将主USV的控制输入选择为：

5、仿真实验和水面实验。

实验一：通过MATLAB仿真实验分析主USV与目标USV间的距离对定位性能的影响。设水面目标USV初始位置为原点，将进行航向角ψ_k＝45°，航行速度恒为3m/s的匀速直线运动。两艘主USV初始位置分别在x轴和y轴正轴10m处，并和水面目标USV保持着相同的航向和速度。初始状态下两艘主USV与目标USV的距离均为10m，分离角γ₁₂＝90°，系统已达到最优队形配置。为了便于计算，采用多边定位法通过目标USV和两点之间的观测距离直接获得目标USV的估计位置坐标，设距离量测的误差协方差

该系统定位误差的仿真结果如图4(a)所示，经过1000次蒙特卡洛测试，求出最优观测下系统的定位误差均值为0.7634m。

表1不同分离角下对应的系统定位误差均值

仿真通过改变分离角γ₁₂验证其对目标USV定位误差的影响，在其他条件不变的前提下把分离角γ₁₂从120°逐渐降低到30°，该情况下系统的定位误差仿真结果示例图(选取了γ₁₂＝60°和40°两组)如图4(b)和(c)所示。经过1000次蒙特卡洛测试，仿真得出不同分离角下对应系统的定位误差均值，如表1所示。显然，系统在分离角γ₁₂＝90°时对目标USV定位性能最佳，而角度偏差越大，系统的定位性能也就越差。而当分离角γ₁₂偏差超过40°时，系统的误差便会显著地增加。

实验二：通过水面实验验证上述结论以及系统的可行性。首先进行实验I，在一个湖中以湖心预设位置为原点，正北方向为y轴建立惯性坐标系，目标USV与两艘主USV的初始的位置坐标分别为(-40m,67m)、(-52m,62m)和(-59m,55m)。主USV与目标USV设定的初始速度为0.5m/s，初始航向角为东偏南45°。实验的采样间隔T＝0.2s，采样时间为220s。

水面目标USV及主USV的初始位置和真实运动轨迹如图5(a)所示，考虑到实验应尽可能选取系统分离角γ₁₂＝90°的区段，截取图中黑框内的实验内容为有效数据。系统误差如图5(b)所示，此时系统的平均定位误差为0.648m。

然后进行对照实验II，该实验以岸边基站位置为原点和运动终点，正北方向为y轴建立惯性坐标系，目标USV与主USV的初始的位置与运动轨迹如图6(a)所示。主USV与目标USV设定的初始速度为0.5m/s，初始航向角为西偏南45°，该实验的前半段主USV与目标USV均进行定向航行，保持推力不变。实验的采样间隔T＝0.2s，采样时间为500s。为了计算方便，截取目标USV运动非常接近匀速直线运动的前半段的实验内容为有效数据，此时系统的分离角γ₁₂明显较最优分离角度偏大。计算得到系统的定位误差，如图6(b)所示，此时系统的平均定位误差为0.819m。显然，实验I对水面目标USV的定位误差低于实验II的误差。即最优分离角下系统的定位性能更加。

由以上两个实验可以看出，本发明具有最优的目标检测和跟踪性能。

Claims (1)

1.一种基于主从式编队的水面目标检测与跟踪控制方法，其特征在于，包括以下步骤：

步骤1：所述主从式编队包括两个主USV和一个目标USV，两个主USV分别为USV1和USV2；在两个主USV和目标USV入水之前，对两个主USV和目标USV进行时钟同步；

步骤2：将两个主USV与目标USV放入水中；

两个主USV与目标USV的初始广义位置向量分别定义为：

X_n1(0)＝(x_n1(0) y_n1(0) ψ_n1(0))^T

X_n2(0)＝(x_n2(0) y_n2(0) ψ_n2(0))^T

X(0)＝(x(0) y(0) ψ(0))^T

其中，X_n1(0)和X_n2(0)分别为USV1和USV2的初始广义位置向量，X(0)为目标USV的初始广义位置向量；(x_n1(0) y_n1(0))和(x_n2(0) y_n2(0))分别为USV1和USV2在水平面内的位置，(x(0)y(0))为目标USV在水平面内的位置，三个USV的位置都由安装在自身的GPS测量得到；ψ_n1(0)和ψ_n2(0)分别表示USV1和USV2的初始航向角，ψ(0)表示目标USV的初始航向角，三个初始航向角都由安装在自身的航向传感器测量得到；

步骤3：定义目标USV的状态向量X＝(x y ψ)^T，设定目标USV的扩展卡尔曼滤波器的滤波初值

状态估计误差协方差矩阵的初值为P(0|0)＝

采样周期为T；

步骤4：目标USV开始在水面航行；

步骤5：在第k个采样时刻，装在USV1和USV2上的双目摄像头分别测量出目标USV的相对方位信息，并分别将自身的位置信息(x_n1(k) y_n1(k))、(x_n2(k) y_n2(k))发送给目标USV；同时，目标USV上安装的速度传感器、航向传感器和航向角速度传感器分别测量出目标USV在k时刻的航速V(k)、航向ψ(k)和航向角速度ω(k)，目标USV的测量噪声协方差矩阵为

分别为航速V(k)、航向ψ(k)和航向角速度ω(k)的噪声方差；

步骤6：利用卡尔曼滤波器对目标USV状态进行预测；

步骤6-1：利用式(1)计算目标USV在k时刻的状态预测值：

其中，

表示目标USV在k-1时刻的状态预测值；

步骤6-2：利用式(2)计算目标USV在k时刻状态误差协方差矩阵的预测值：

其中

P(k-1|k-1)表示目标USV在k-1时刻状态误差协方差矩阵的预测值；

步骤7：USV1和USV2上的双目摄像头通过目标USV在左右摄像头中成像的视差进行三角测距，分别获得USV1和USV2与目标USV的实时距离；

步骤8：利用距离量测对目标USV状态进行滤波；

步骤8-1：利用式(3)计算目标USV在k时刻的卡尔曼滤波增益矩阵：

其中，

R(k)表示量测噪声，

和

分别表示目标USV在x和y方向状态的预测值；

步骤8-2：利用式(4)计算目标USV在k时刻状态的最优滤波值：

其中

Z(k)表示量测值；

步骤8-3：利用式(5)计算目标USV在k时刻状态误差协方差矩阵的滤波值：

P(k|k)＝P(k|k-1)-K(k)H(k)P(k|k-1) (5)

步骤9：目标USV将k时刻的速度、航向角速度及状态滤波值通过通讯设备分别发送给USV1和USV2；

步骤10：USV1和USV2以目标USV作为领航者，并分别采用主从式编队控制律实现队形编队航行；

步骤10-1：利用式(6)分别计算USV1和USV2与目标USV之间的距离：

其中，

表示目标USV在y方向状态的最优滤波值，i为主USV的序号，i＝1，2；

步骤10-2：利用式(7)分别计算USV1和USV2与目标USV之间的夹角；

其中，

表示目标USV的航向；

步骤10-3：利用式(8)分别计算USV1和USV2与目标USV间距离与最小安全距离之间的误差：

其中，r_min为主USV与目标USV之间的最小安全距离，ρ_i为主USV与目标USV之间的距离；

步骤10-4：利用式(9)分别计算USV1和USV2与目标USV之间的夹角与期望夹角之间的误差：

其中，α₁ ^*(k)＝ε，

其中ε为常值，α_i(k)表示主USV与目标USV之间的方位角；

步骤10-5：利用式(10)分别计算USV1和USV2和目标USV之间的航向角偏差：

步骤10-6：利用式(11)分别计算USV1和USV2的速度：

其中，k₁，

为设计的控制增益；ψ_i(k)表示主USV的航向；

步骤10-7：利用式(12)分别计算USV1和USV2的航向角速度：

其中，d_i＞0为主USV定位装置沿纵轴距主USV重心的距离；

步骤11：返回步骤5，开始新的循环，直至目标检测与跟踪控制过程结束。

Images