[發明專利]多障礙復雜環境下欠驅動無人艇的自主靠泊方法

有效

• 申請號: 201810165108.4
• 申請日: 2018-02-28
• 公開（公告）號: CN108459602B
• 公開（公告）日: 2021-03-30
• 發明（設計）人: 賈知浩;李曄;廖煜雷;張偉;陳啟賢;張偉斌;王磊峰;潘愷文;姜權權;范佳佳
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G05D1/02
• 分類號: G05D1/02;G05D1/00;G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.多障礙復雜環境下欠驅動無人艇的自主靠泊方法，其特征在于，包括以下步驟：

(1)由傳感器獲取無人艇當前的位置P(x_c,y_c)，速度v_c，角速度ω_c和艏向f_c；目標泊位的位置(x_A,y_A)和目標姿態角周圍障礙物的個數n位置(x_i,y_i)，速度v_i，艏向f_i，其中i＝0,1,2…n，并將碰撞障礙數j初始化為0；

(2)根據(x_A,y_A)和以及無人艇的最大速度V_max和最大加速度a_max計算目標泊位的一級引導點Aim1(x_A1,y_A1)；

(3)根據(x_A1,y_A1)、(x_c,y_c)、v_c、ω_c計算二級引導點Aim2(x_A2,y_A2)；

(4)根據(x_c,y_c)和(x_A2,y_A2)判斷當前無人艇是否到達Aim2，是則轉步驟(5)，否則以Aim2的方向為目標艏向Kf；

(5)根據(x_c,y_c)和(x_A1,y_A1)判斷當前無人艇是否到達Aim1，是則以Aim的方向為目標艏向Kf，否則沿回轉引導圓行駛直到抵達Aim1；Aim，Aim1和Aim2分別是目標泊位，一級引導點和二級引導點；

(6)計算當前無人艇(x_c,y_c)與目標泊位間的相對距離Kd；

(7)判斷Kd是否為0，是則結束，否則轉步驟(8)；

(8)根據當前無人艇的速度v_c和艏向f_c，結合LOS視線法計算當前無人艇的靠泊約束集[vd]；

(9)利用公式t_min(i)＝L_u2c(i)/v_u2c(i)分別計算當前無人艇與第i個障礙物的最短碰撞時間t_min(i)，i＝0,1,2…n，其中L_u2c和v_u2c分別是當前無人艇與障礙物i間的相對距離和相對速度；

(10)判斷t_min(i)是否小于時間閾值T_c，是則令j等于j加1轉步驟(16)，否則轉步驟(15)；

(11)根據障礙j與無人艇的相對距離速度艏向信息計算當前情況下的橢圓碰撞錐[vo]＝(Tan1，Tan2)(1)∪(Tan1，Tan2)(2)∪…∪(Tan1，Tan2)(j)，其中Tan1和Tan2是無人艇與障礙j間絕對速度矢量空間內的兩個切線艏向角；

(12)計算[vd]∩[vo]，得到當前無人艇的可行速度矢量集[Cand]；

(13)判斷[Cand]是否為空，是則將無人艇速度置0并轉步驟(16)，否則轉步驟(14)；

(14)利用基于COLREGS的多障礙啟發算方法，在[Cand]集合中選擇合適的無人艇速度矢量(v*，f*)；

(15)判斷i是否等于n，是則令無人艇速度等于缺省值V_def，艏向等于Kf并轉步驟(17)，否則令i等于i加1并返回步驟(10)；

(16)判斷i是否等于n，是則轉步驟(11)，否則令i等于i加1并返回步驟(10)；

(17)根據新的無人艇速度艏向，利用公式計算無人艇下一時刻的位置P*，令P等于P*，返回步驟(1)；

步驟(14)中基于COLREGS的多障礙啟發式算法的步驟包括：

(14.1)根據當前無人艇與障礙物j的相對艏向角f_rel(j)進行COLREGS規則判斷，|180°-f_rel(j)|15°為相遇情況，|f_rel(j)|45°為超越情況，45°≤|f_rel(j)|≤165°為交叉情況；

(14.2)在當前無人艇可行速度矢量集[Cand]中選擇符合COLREGS規則的速度矢量子集[Candson]，即相遇時無人艇應向右舷轉向，超越時應向左舷轉向，交叉時要從障礙后方駛過，若[Candson]存在轉步驟(14.3)，否則轉步驟(14.5)；

(14.3)在[Candson]中選擇橢圓碰撞錐邊界艏向(Tan1或Tan2)上的最大速度并轉步驟(14.6)，若不存在，轉步驟(14.4)；

(14.4)在[Candson]中選擇最靠近橢圓碰撞錐邊界艏向上的最大速度，轉步驟(14.6)；

(14.5)在[Cand]中選擇最靠近橢圓碰撞錐邊界艏向上的最大速度，轉步驟(14.6)；

(14.6)當前無人艇需要避讓的碰撞障礙物數量j等于j減1，若j仍大于1，令k等于k加1(k初始值為0)并返回步驟(14.1)，其中否則轉步驟(14.7)；

(14.7)若存在多組解(多障礙物情況)，則利用公式k＝min(abs(f_c-f*(k)))選擇最接近無人艇當前艏向f_c的那組解作為期望輸出(v*，f*)，否則直接輸出那組解。