[发明专利]一种基于主从式编队的水面目标检测与跟踪控制方法

权利要求书

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的航向，i＝1,2；

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

其中，r_min为从USV与目标USV之间的最小安全距离，ρ_i为从USV与目标USV之间的距离，i＝1,2；

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

其中，α₁^*(k)＝ε，其中ε为常值，α_i(j)表示从USV与目标USV之间的方位角，i＝1,2；

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

其中，i＝1,2；

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

其中，为设计的控制增益；ψ_i(k)表示从USV的航向，i＝1,2；

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

其中，d_i0为从USV定位装置沿纵轴距从USV重心的距离，i＝1,2；

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