[發明專利]欠驅動AUV水下動態目標跟蹤中的期望路徑構造方法

在審

• 申請號: 201810771536.1
• 申請日: 2018-07-13
• 公開（公告）號: CN109141421A
• 公開（公告）日: 2019-01-04
• 發明（設計）人: 李曄;孫葉義;曹建;王友康;姜言清;安力;何佳雨;周子燁;王汝鵬;崔林濤
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種欠驅動AUV水下動態目標跟蹤中的期望路徑構造方法，其特征是：

步驟一：基于交互多模型得到當前時刻目標位置的估計值；通過超短基線和雙目視覺定位系統得到當前時刻目標位置的觀測值；根據得到估計值和觀測值，通過卡爾曼濾波得到當前時刻目標位置的最優估計值；

步驟二：將當前時刻AUV位置與目標位置的最優估計值之間的部分或全部歷史目標點作為所選歷史目標點序列，選擇當前時刻AUV位置、所選歷史目標點序列、當前時刻目標位置的最優估計值作為期望目標點序列；在所選期望目標點序列的基礎上，引入幾何約束條件，形成一條光滑的三次樣條曲線路徑；

步驟三：選擇步驟二中的三次樣條曲線路徑作為當前時刻的期望路徑，隨著時間的推移，通過步驟一、步驟二期望路徑不斷更新。

2.根據權利要求1所述的欠驅動AUV水下動態目標跟蹤中的期望路徑構造方法，其特征是：設在k時刻，將AUV尚未經過的歷史目標點位置信息作為已知信息，設歷史目標點序列為(a_k-n,a_k-n+1,a_k-n+2,...,a_k-3,a_k-2,a_k-1)，即AUV尚未經過的歷史目標點的數目為n，當AUV距離目標較遠、即n≥10時，取首端和末端各5個點，即(a_k-n,a_k-n+1,a_k-n+2,a_k-n+3,a_k-n+4,a_k-5,a_k-4,a_k-3,a_k-2,a_k-1)為所選歷史目標點序列，此時期望目標點序列為當前時刻AUV位置、所選歷史目標點序列、當前時刻目標位置最優估計值，共12個點；當AUV距離目標較近、即n＜10時，則所選歷史目標點序列為(a_k-n,a_k-n+1,a_k-n+2,...,a_k-3,a_k-2,a_k-1)，此時期望目標點序列為當前時刻AUV位置、所選歷史目標點序列、當前時刻目標位置最優估計值，共n+2個點，，其中a_i＝(ξ_i,η_i,ζ_i)表示大地坐標系下點的位置坐標，設期望目標點序列數目為m、其序列為(p₁,p₂,...,p_m)，則表示為：

當n≥10時，m＝12；

(p₁,p₂,...,p_m)＝(a_AUV,a_k-n,a_k-n+1,a_k-n+2,a_k-n+3,a_k-n+4,a_k-5,a_k-4,a_k-3,a_k-2,a_k-1,a_k)當n＜10時，m＝n+2；

(p₁,p₂,...,p_m)＝(a_AUV,a_k-n,a_k-n+1,a_k-n+2,...,a_k-3,a_k-2,a_k-1,a_k)。

3.根據權利要求1或2所述的欠驅動AUV水下動態目標跟蹤中的期望路徑構造方法，其特征是：三次樣條曲線的幾何約束包括起始約束與末段約束，

起始段約束要求路徑在η_AUV處的切線方向與當前時刻的AUV艏向一致，且以當前時刻AUV的位置為路徑起點，即：

其中：p₁為第一個期望目標點，(ξ_AUV,η_AUV,ζ_AUV)表示當前時刻AUV的坐標，表示路徑曲線在起點處的斜率，ψ表示AUV的艏向向量，α為正實數；

末段約束要求所規劃路徑的期望艏向不超過塢站錐形開角的一半，且以當前時刻目標位置為路徑終點，即

其中：p_m為最后一個期望目標點，(ξ_k,η_k,ζ_k)表示當前時刻目標的坐標，α_塢站為塢站錐形開角大小，ψ為AUV艏向角。