[發明專利]一種基于主從式編隊的水面目標檢測與跟蹤控制方法

有效

• 申請號: 202110329226.6
• 申請日: 2021-03-27
• 公開（公告）號: CN113064434B
• 公開（公告）日: 2022-07-12
• 發明（設計）人: 王銀濤;閆崢;張冰;韓正卿;嚴衛生;崔榮鑫;張守旭;李宏;王崇武
• 申請（專利權）人: 西北工業大學
• 主分類號: G05D1/02
• 分類號: G05D1/02;G05D1/12
• 代理公司: 西安凱多思知識產權代理事務所(普通合伙) 61290
• 代理人: 劉新瓊

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，開始新的循環，直至目標檢測與跟蹤控制過程結束。