[发明专利]多障碍复杂环境下欠驱动无人艇的自主靠泊方法

权利要求书

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*)，否则直接输出那组解。