[發明專利]一種考慮混合多目標避障的水面無人艇路徑跟蹤制導方法

有效

• 申請號: 201710504472.4
• 申請日: 2017-06-28
• 公開（公告）號: CN107168335B
• 公開（公告）日: 2020-06-23
• 發明（設計）人: 張國慶;鄧英杰;吳曉雪;張顯庫;田佰軍;黃晨峰;關巍
• 申請（專利權）人: 大連海事大學
• 主分類號: G05D1/02
• 分類號: G05D1/02;G05D1/12
• 代理公司: 大連至誠專利代理事務所(特殊普通合伙) 21242
• 代理人: 楊威;涂文詩

1.一種考慮混合多目標避障的水面無人艇路徑跟蹤制導方法，其特征在于包括以下步驟：

S1：設定航路點信息W₁,W₂,…,W_n，分別建立導引虛擬小船和動態虛擬小船的運動數學模型；其中，所述導引虛擬小船和動態虛擬小船的運動數學模型的表達式為：

其中，d代表動態虛擬小船，g代表導引虛擬小船，(x,y)為位置坐標，分別為距離的一階導數，ψ為艏向角，u為前進速度，r為轉艏角速度；

S2：導引虛擬小船根據制導算法規劃出參考路徑，并始終沿參考路徑行駛，啟動路徑跟蹤制導模式，動態虛擬小船的艏向角時刻等于導引虛擬小船相對于動態虛擬小船的真方位角，并向實船發出制導命令，以引導實船進行路徑跟蹤，實船不斷獲取動態虛擬小船的導引變量；其中，所述動態虛擬小船的導引變量包括艏向角ψ_d和前進速度u_d，艏向角ψ_d的計算公式為：

其中，d代表動態虛擬小船，g代表導引虛擬小船，(x,y)為位置坐標；

前進速度u_d的計算公式為：

其中，k_d為用于調整收斂速度快慢的設定參數，l_dg為動態虛擬小船到導引虛擬小船的距離，u_g為導引虛擬小船的前進速度，ψ_g為導引虛擬小船的艏向角，ψ_d為動態虛擬小船的艏向角，l_dbset為實船到動態虛擬小船的距離的上限設定，l_db為實船到動態虛擬小船的距離；

S3：在采樣時間點進行采樣，判斷動態虛擬小船是否進入障礙物探測環，如進入障礙物探測環，進入障礙物排序過程，執行S4；如未進入障礙物探測環，保持路徑跟蹤制導模式，執行S6；

S4：進入障礙物排序過程：對避障物的優先級進行排序，得到障礙物的優先級序列，選取最高優先級障礙物作為當前避障目標，判斷當前避障目標是否滿足避障操縱條件，若當前避障目標滿足避障操縱條件，則進入對當前障礙物的避障制導模式，即執行S5；若當前避障目標不滿足避障操縱條件，選取次優先級的障礙物作為當前避障目標；若所有障礙物都不滿足避障操縱條件，則進入路徑跟蹤模式，執行S6；

S5：進入避障制導模式：在每一采樣時間點，判斷當前障礙物的避障操縱條件是否滿足，如滿足避障操縱條件，則啟動對當前障礙物的避障制導模式，獲取動態虛擬小船的導引變量，引導實船；如不滿足避障操縱條件，則從障礙物優先級序列中去除當前障礙物，并執行S4；其中，S5所述的避障制導模式中，動態虛擬小船的速度u_d調整為避障速度，艏向角按照下列方法選取：

S51：如當前避障目標為靜態障礙物，則

其中，ψ_do為動態虛擬小船在避障制導模式中的艏向角，φ為靜態障礙物相對動態虛擬小船的真方位角，e為動態虛擬小船到該靜態障礙物的安全極限環的距離，Δ為設定的前向距離，λ＝±1決定動態虛擬小船避障時環繞靜態障礙物的方向，+1為順時針方向環繞靜態障礙物的安全極限環，-1為逆時針方向環繞靜態障礙物的安全極限環；

S52：如當前避障目標為動態障礙物，則

其中，φ為動態障礙物相對動態虛擬小船的真方位角，Δ為設定的前向距離，e為動態虛擬小船到該動態障礙物的安全極限環的距離，λ＝±1，+1為順時針環繞動態障礙物的安全極限環，-1為逆時針環繞動態障礙物的安全極限環；補償參數k用于補償會遇船舶運動對艏向角造成的影響，其定義為：

其中，b＝-2eV₀²，c＝-(Δ²+e²)V₀²且u_do≥u_c≥V₀；；

u_c為當前動態障礙物的速度；u_do表示動態虛擬小船的避障速度；

S6：根據動態虛擬小船的導引變量和控制策略，實時調整實船的執行器輸入，控制實船跟蹤動態虛擬小船；

S7：測量實船位置，判斷是否到達終點，如果是，結束船舶航行，如果否，則執行S2。