本发明属于水面无人艇局部动态靠泊规划领域，具体涉及多障碍复杂环境下欠驱动无人艇的自主靠泊方法。
背景技术：
无人艇作为一种典型的欠驱动系统，在多障碍复杂环境下的局部动态靠泊规划一直是其研究领域的核心之一。该问题的主要难点在于多数局部动态规划方法没有考虑目标点对无人艇速度艏向的约束以及靠泊过程中的避障效率，这些方法在视野开阔的海域也许可以得到令人满意的结果，但是在交通复杂的港口码头里面，这些方法并不能完成无人艇的自主靠泊任务。
公开日2017年6月13日，公开号cn106845716a，发明名称为“一种基于导航误差约束的水面无人艇局部分层路径规划方法”的专利申请采用了分层思想，将局部路径规划划分为静态路径规划和动态路径规划两个层面，即解决了静态障碍物规避问题，又解决了动态障碍物规避问题，考虑了水面无人艇自身的动力学约束，并融合了海事规则，提高了规划路径的安全性。该方法虽然结合了海事规则和无人艇自身动力学约束，却没有考虑目标点约束和靠泊过程中无人艇的运动学约束，无法直接用于无人艇低速状态下的自主靠泊任务。
公开日2018年1月5日，公开号cn107544500a，发明名称为“一种考虑约束的无人艇靠泊行为轨迹规划方法”的专利申请采用了改进人工势场法，考虑无人艇自身运动约束，为无人艇的自主靠泊控制问题提供便利。但该方法没有考虑无人艇靠泊过程中遇到的动态障碍，在规划的过程中也没有遵守海事规则，因此在复杂的动态环境中很难保证其规划的有效性。
文献“基于速度障碍原理的无人艇自动避碰算法”中，吴博等人提出了一种考虑风浪流影响下的无人艇操纵运动特性的改进速度障碍法，有效克服了局部路径规划的局限性，并且更符合无人艇实际航行避碰操纵要求，能够使无人艇在具有多个动态障碍物的复杂环境中安全避让。该方法虽然经过了仿真验证，但它没有结合colregs海事规则对无人艇的避让机动进行调整，且没有考虑目标点对无人艇的速度艏向约束。
本发明针对多障碍复杂环境下欠驱动无人艇的自主靠泊实际问题，在经典速度障碍方法的基础上，加入目标泊位对无人艇的艏向和速度约束，并在满足欠驱动无人艇靠泊过程中运动特性的前提下，利用基于colregs的多障碍启发式算法，完成多障碍复杂环境下欠驱动无人艇的自主靠泊任务并通过仿真试验取得了优良的效果。
技术实现要素：
本发明的目的在于提供使无人艇在遵守国际海事公约的基础上以尽可能高的效率进行自主靠泊的多障碍复杂环境下欠驱动无人艇的自主靠泊方法。
本发明的目的通过如下技术方案来实现：
多障碍复杂环境下欠驱动无人艇的自主靠泊方法，包括以下步骤：
(1)由传感器获取无人艇当前的位置p(xc,yc)，速度vc，角速度ωc和艏向fc；目标泊位的位置(xa,ya)和目标姿态角
(2)根据(xa,ya)和
(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)根据新的无人艇速度艏向，利用公式
目标泊位的一级二级引导点和回转引导圆是本发明的一个核心，其计算步骤包括：
(a)根据无人艇最大速度vmax和最大加速度amax计算无人艇舵效消失的最短距离lmin，其中lmin＝vmax2/(2amax)；
(b)根据目标姿态角
(c)计算当前无人艇与目标泊位的相对角度kf，公式为
(d)计算回转引导圆的圆心(xo,yo)等于(xor,yor)，其中
(e)计算回转引导圆的圆心(xo,yo)等于(xol,yol)，其中
(f)若flag为’r’，计算当前无人艇与回转引导圆的左切点为aim2，否则计算当前无人艇与回转引导圆的右切点为aim2。
结合los视线法的靠泊约束集是本发明的第二个核心，其计算步骤包括：
(8.1)根据kd利用视线法计算目标点对无人艇的速度约束losv，公式为
(8.2)根据vc和fc计算当前无人艇运动学艏向约束(fkinmin，fkinmax)，公式为
(8.3)根据vc，ωc和fc计算当前无人艇的动力学速度约束(vdymin，vdymax)和艏向角约束(fdymin，fdymax)，公式为
(8.4)比较vdymax与losv的大小，若losv小于vdymax，则转步骤(8.5)，否则转步骤(8.6)；
(8.5)比较vdymin与losv的大小，若losv小于vdymin，则令vdymin等于0，vdymax等于losv，否则令vdymax等于losv；
(8.6)(fkinmin，fkinmax)与(fdymin，fdymax)取交集，与(vdymin，vdymax)共同构成无人艇的靠泊约束集[vd]；
基于colregs的多障碍启发式算法是本发明的第三个核心，其主要步骤包括：
(14.1)根据当前无人艇与障碍物j的相对艏向角frel(j)进行colregs规则判断，|180°-frel(j)|<15°为相遇情况，|frel(j)|<45°为超越情况，45°≤|frel(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(fc-f*(k)))选择最接近无人艇当前艏向fc的那组解作为期望输出(v*，f*)，否则直接输出那组解。
本发明的有益效果在于：
本发明针对多障碍复杂环境中欠驱动无人艇的自主靠泊问题，提出无人艇多级目标引导方法和结合los视线法的靠泊约束集，考虑靠泊过程中目标点对无人艇的速度艏向约束，得到的轨迹满足欠驱动无人艇的运动特性，便于与运动控制系统相结合，具有良好的实时性和应用性。
本发明结合colregs海事规则，提出基于colregs的多障碍启发式算法，使无人艇的避让机动更加规范和高效，有效提高了无人艇在多障碍复杂环境中的自主性和安全性。
附图说明
图1为多障碍复杂环境下欠驱动无人艇自主靠泊方法主流程图；
图2为一级二级引导点和回转引导圆的计算流程图；
图3为结合los视线法的靠泊约束集计算流程图；
图4为基于colregs的多障碍启发式算法的计算流程图；
图5为一级二级引导点和回转引导圆的详细说明；
图6为仿真效果图：图(a)为无人艇靠泊后期艏向曲线；图(b)为无人艇靠泊期望轨迹；图(c)为无人艇靠泊后期速度曲线。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明：
结合图1，该方法包括如下步骤：
(1)由传感器获取无人艇当前的位置p(xc,yc)，速度vc，角速度ωc和艏向fc；目标泊位的位置(xa,ya)和目标姿态角
(2)计算目标泊位的一级引导点aim1(xa1,ya1)和二级引导点aim2(xa2,ya2)；
(3)判断当前无人艇是否到达aim2，是则转步骤(4)，否则以aim2的方向为目标艏向kf；
(4)判断当前无人艇是否到达aim1，是则以aim的方向为目标艏向kf，否则以aim1的方向为目标艏向kf；
(5)计算当前无人艇(xc,yc)与目标泊位(xa,ya)间的相对距离kd；
(6)判断kd是否为0，是则结束，否则转步骤(8)；
(7)结合los视线法计算当前无人艇的动态约束集[vd]；
(8)计算当前无人艇与第i个障碍物的最短碰撞时间tmin(i)，i＝0,1,2…n；
(9)判断tmin(i)是否小于时间阈值tc(由无人艇动力学特性获得)，是则令j等于j加1转步骤(15)，否则转步骤(14)；
(10)计算当前情况下的椭圆碰撞锥[vo]＝(tan1，tan2)(1)∪(tan1，tan2)(2)∪…∪(tan1，tan2)(j)；
(11)计算[vd]∩[vo]，得到当前无人艇的可行速度矢量集[cand]；
(12)判断[cand]是否为空，是则将无人艇速度置0并转步骤(16)，否则转步骤(13)；
(13)利用基于colregs的多障碍启发式算法，在[cand]集合中选择合适的无人艇速度矢量(v*，f*)；
(14)判断i是否等于n，是则令无人艇速度等于缺省值vdef，艏向等于kf并转步骤(16)，否则令i等于i加1并返回步骤(9)；
(15)判断i是否等于n，是则转步骤(10)，否则令i等于i加1并返回步骤(9)；
(16)根据新的无人艇速度艏向，利用公式
结合图2，一级二级引导点和回转引导圆的计算步骤如下：
(1)计算无人艇舵效消失的最短距离lmin，其中lmin＝vmax2/(2amax)；
(2)根据目标姿态角
(3)计算当前无人艇与目标泊位的相对角度kf；
(4)判断kf是否大于
(5)根据flag计算回转引导圆的圆心位置(xo,yo)；
(6)判断flag值，若为’r’，计算当前无人艇与回转引导圆的左切点为aim2，否则计算当前无人艇与回转引导圆的右切点为aim2；
对一级二级引导点和回转引导圆的详细说明：
如图5所示，图中的aim，aim1和aim2分别是目标泊位，一级引导点和二级引导点，(xol,yol)和(xor,yor)分别是目标泊位左右两个引导圆的圆心，由于引导圆的曲率半径大于等于无人艇抵达aim2时的最小回转半径，因此无人艇可沿回转引导圆的边界做定长回转运动，直到抵达aim1，在此之后虽然无人艇的舵效无法保证，但由于不需要再改变艏向，所以无人艇做直线运动直到抵达目标泊位aim。仿真程序效果如图6所示。
图(a)为无人艇靠泊后期艏向曲线，可以看出艏向角逐渐稳定到90度附近；图(c)为无人艇靠泊后期速度曲线，可以看出其速度逐渐下降到0；图(b)为无人艇靠泊期望轨迹，图中的轨迹平滑，满足欠驱动无人艇的运动特性。
结合图3，计算无人艇靠泊约束集的步骤包括：
(1)根据kd利用los视线法计算目标点对无人艇的速度约束losv；
(2)根据vc和fc计算当前无人艇运动学艏向约束(fkinmin，fkinmax)；
(3)根据vc，ωc和fc计算当前无人艇的动力学速度约束(vdymin，vdymax)和艏向角约束(fdymin，fdymax)；
(4)比较vdymax与losv的大小，若losv小于vdymax，则转步骤(5)，否则转步骤(6)；
(5)比较vdymin与losv的大小，若losv小于vdymin，则令vdymin等于0，vdymax等于losv，否则令vdymax等于losv；
(6)计算[vd]＝(fkinmin，fkinmax)∩(fdymin，fdymax)∪(vdymin，vdymax)；
结合图4，基于colregs的多障碍启发式算法的计算步骤如下：
(1)根据当前无人艇与障碍物j的相对艏向角frel(j)进行colregs规则判断；
(2)在当前无人艇可行速度矢量集[cand]中选择符合colregs规则的速度矢量子集[candson]，若存在转步骤(3)，否则转步骤(5)；
(3)在[candson]中选择椭圆碰撞锥边界艏向(tan1或tan2)上的最大速度并转步骤(6)，若不存在，转步骤(4)；
(4)在[candson]中选择最靠近椭圆碰撞锥边界艏向上的最大速度，转步骤(6)；
(5)在[cand]中选择最靠近椭圆碰撞锥边界艏向上的最大速度，转步骤(6)；
(6)当前无人艇需要避让的碰撞障碍物数量j等于j减1，若j仍大于1，令k等于k加1(k初始为0)返回步骤(1)，否则转步骤(7)；
(7)若k>1，则选择最接近无人艇当前艏向fc的那组解作为期望输出(v*，f*)，否则直接输出那组解。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。