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

摘要

本发明公开了多障碍复杂环境下欠驱动无人艇的自主靠泊方法，属于水面无人艇局部动态靠泊规划领域。包括：计算目标泊位的一级引导点，二级引导点；判断无人艇是否到达一级引导点，二级引导点；计算无人艇当前位置与目标泊位间的距离；结合LOS视线法计算当前无人艇的靠泊约束集；计算当前无人艇与周围障碍物的最短碰撞时间；计算当前情况下无人艇的椭圆碰撞锥；利用基于COLREGS的多障碍启发式算法，选择无人艇速度矢量；计算无人艇下一时刻的位置。在传统速度障碍法中加入多级目标引导和靠泊约束集，成功实现多障碍复杂环境下欠驱动无人艇的自主靠泊，充分考虑了无人艇自身动力学、运动学和目标泊位的约束特性，使无人艇在自主靠泊中遵守海事规则。

主权项

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)根据(xA1,yA1)、(xc,yc)、vc、ωc计算二级引导点Aim2(xA2,yA2)；(4)根据(xc,yc)和(xA2,yA2)判断当前无人艇是否到达Aim2，是则转步骤(5)，否则以Aim2的方向为目标艏向Kf；(5)根据(xc,yc)和(xA1,yA1)判断当前无人艇是否到达Aim1，是则以Aim的方向为目标艏向Kf，否则沿回转引导圆行驶直到抵达Aim1；(6)计算当前无人艇(xc,yc)与目标泊位间的相对距离Kd；(7)判断Kd是否为0，是则结束，否则转步骤(8)；(8)根据当前无人艇的速度vc和艏向dc，结合LOS视线法计算当前无人艇的靠泊约束集[vd]；(9)利用公式tmin(i)＝Lu2c(i)/vu2c(i)分别计算当前无人艇与第i个障碍物的最短碰撞时间tmin(i)，i＝0,1,2…n，其中Lu2c和vu2c分别是当前无人艇与障碍物i间的相对距离和相对速度；(10)判断tmin(i)是否小于时间阈值Tc(由无人艇动力学特性获得)，是则令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，是则令无人艇速度等于缺省值Vdef，艏向等于Kf并转步骤(17)，否则令i等于i加1并返回步骤(10)；(16)判断i是否等于n，是则转步骤(11)，否则令i等于i加1并返回步骤(10)；(17)根据新的无人艇速度艏向，利用公式计算无人艇下一时刻的位置P*，令P等于P*，返回步骤(1)。