本发明涉及水面无人艇自主靠泊领域，尤其涉及一种考虑约束的无人艇靠泊行为轨迹规划方法。
背景技术：
无人艇是一种强非线性的欠驱动系统，由于其无人艇在靠泊时面临的限制水域环境较为复杂，因此无人艇的自主靠泊是一大难题；另外无人艇在靠泊时还会受到因速度低导致的舵效差、因岸壁效应导致的扰动大等影响，这些都使自动控制变得更加困难。
自主靠泊系统可以使欠驱动无人艇实现自主的安全的靠泊，它可以根据无人艇、目标泊位和障碍物的位置自主规划出可行的靠泊轨迹，然后交由控制系统自动控制无人艇舵、桨等执行机构，使无人艇遵循所规划轨迹完成靠泊。
发明名称为“一种用于多种泊车场景的车辆自主泊车路径规划方法”(公开号cn1o5857306a，公开日2016年8月17日)的专利申请适用于多种泊车场景，设计合理，能够提供丰富的信息控制车辆自主泊车，安全系数高。但是车不存在漂角以及横漂的现象，该方法并不适用于无人艇的路径规划，而且该方法需要倒车等欠驱动无人艇较难完成的操作，无法适用于欠驱动无人艇自主靠泊的轨迹规划中。
发明名称为“一种基于邻域智能水滴算法的水面无人艇路径规划方法”(公开号cn103744428a，公开日2014年4月23日)的专利申请针对基本iwd方法存在的易陷入局部最优解导致方法停滞及收敛速度较慢的问题进行了改进，可以避免该方法陷入局部最优导致早熟，提高了方法寻优的收敛速度。但是该方法没有考虑无人艇靠泊过程中岸边的码头对无人艇航向航速的影响，无法实现无人艇的安全自主靠泊。
文献“基于改进人工势场法的无人船路径规划算法”中，刘琨等人提出了一种改进的人工势场法，用指数函数代替二次函数构造势场函数，降低了势场强度的变化幅度，并在斥力势场函数中增加无人船与目标点的相对位置的一个因子，解决目标不可达问题；同时设置势场系数调整因子，引入2个判断条件确定无人船是否陷入局部最小值，在此基础上选择相应的势场系数，从而跳出局部极小值点。
该方法虽然经过了仿真验证，但它并没有考虑无人艇的欠驱动特性和规划轨迹的光顺问题，且计算成本相对较高，缺乏实用性。
综上所述，现有的轨迹规划方法或没有考虑无人艇的欠驱动特性，规划出的轨迹转角过大；或需要较大的计算量以保证轨迹可行；或没有考虑靠泊过程中岸壁对无人艇的影响，均无法很好地解决无人艇靠泊过程中存在的实际问题。
技术实现要素：
本发明的目的是为了提供一种考虑约束的无人艇靠泊行为轨迹规划方法，是一种考虑近障碍约束、码头终端约束和无人艇自身运动约束的轨迹规划方法。
本发明的目的是这样实现的：包括远端规划阶段和近岸规划阶段，
远端规划阶段步骤如下：
步骤一：根据无人艇当前位置信息计算出当前的期望艏向；
步骤二：选择最近的障碍物对无人艇的期望艏向进行近障碍前瞻补偿，得到补偿后的期望艏向；
步骤三：根据无人艇当前速度和补偿后的期望艏向更新无人艇位置；
步骤四：考察更新后的无人艇位置是否到达已知的远端规划目标点，如果到达则结束远端规划进入近岸规划阶段，否则返回步骤一；
近岸规划阶段步骤如下：
(1)规划出指向第i个虚拟码头的期望艏向，i初始值为1；
(2)计算无人艇当前位置距离目标码头的距离，为当前期望速度添加码头约束；
(3)根据期望速度和艏向更新无人艇位置信息；
(4)判断当前无人艇是否抵达第i个虚拟码头，是则转步骤(5)，否则返回步骤(2)；
(5)判断第i个虚拟码头是否为目标码头，如果是目标码头则规划完毕程序结束，否则令i＝i+1，返回步骤(1)。
本发明还包括这样一些结构特征：
1.近障碍前瞻补偿具体为：
(1)计算出无人艇与障碍物之间的相对距离d；转步骤(2)；
(2)判断d是否大于阈值d，是则转步骤(3)，否则转步骤(5)，阈值d由障碍物的影响半径r和障碍物本身的尺寸r共同决定，即d＝f(r,r)；
(3)根据无人艇、目标点和障碍物三者之间的相对位置，确定近障碍前瞻补偿量α的正负号，转步骤(4)；
(4)计算补偿量α，结合正负号对当前期望艏向进行补偿，转步骤(5)；
(5)返回新的期望艏向。
与现有技术相比，本发明的有益效果是：1.本发明针对无人艇的靠泊规划克服了局部极小点，轨迹拐角过大和岸避效应问题，而且由于该方法本身计算量相对较小，计算成本低，因此对无人艇自身硬件系统的要求较低，具有较广的应用范围。
2.本发明将无人艇自身运动约束、近障碍约束与码头终端约束考虑在内，可实时精准地规划出靠泊轨迹，同时规划得到的轨迹光顺实用，有助于更好地开展下一步跟踪控制工作，在仿真和外场试验下均得到了有效验证。
本发明考虑到无人艇靠泊过程中周围环境的特征变化，本发明将无人艇的靠泊行为分成了远端规划和近岸规划两个阶段；为了避免轨迹陷入局部极小点和拐角较大，本发明在远端规划阶段加入了近障碍前瞻补偿；为了降低无人艇在停靠过程中因速度慢、舵效差而增加的控制难度，本发明在近岸规划阶段加入了虚拟码头约束。
附图说明
图1为主程序远端规划阶段流程框图；
图2为主程序近岸规划阶段流程框图；
图3为近障碍前瞻补偿函数流程框图；
图4为无人艇的受力分析示意图；
图5为近障碍前瞻补偿方法的详细分析图；
图6为虚拟码头目标点引导法的示意图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述。
结合图1至3，该规划方法根据无人艇周围环境的特征变化将无人艇靠泊行为分成了远端规划和近岸规划两个阶段，远端规划阶段无人艇主要受到多障碍物的约束；近岸规划阶段主要受到码头约束。
所述的远端规划阶段，其特点在于采用基于近障碍前瞻补偿的改进人工势场法对多障碍物环境下的无人艇进行航迹规划，以解决人工势场法存在的局部极小点问题和规划轨迹的转角过大问题，且远端规划阶段包括如下步骤：
(1)由传感器获取无人艇的起点位置(x0,y0)，远端规划阶段目标点位置(xf,yf)，障碍物的个数n和位置[xo1,yo1；...；xon,yon]，障碍物影响半径r，转步骤(2)；
(2)利用改进人工势场法计算出当前无人艇的引力，斥力，合力和期望艏向θ，转步骤(3)；
a.引力场
无人艇在工作区域的位置用x＝(x,y)t表示，则引力势函数可以定义为：
式中：uatt为目标点所产生的引力场，k为增益常数，x为无人艇实时的位置，xg为目标点的位置，则引力可以表示为：
fatt＝-grad(uatt)＝k(xg-x)(1-2)
b.斥力场
对于斥力势函数表达式如下。
式中：urep为障碍物所产生的斥力场，η为增益常数，ρ为无人艇与障碍物之间的距离，ρ0为障碍物的影响半径，当无人艇在障碍物影响半径ρ0之外时，该障碍物将不对无人艇产生斥力的作用。斥力的计算方法表示为：
无人艇受到的合力为f＝fatt+frep，该力决定了无人艇的运动方向，如图4所示。
(3)选择距离无人艇最近的障碍物作为优先进行近障碍前瞻补偿的对象障碍物ot，转步骤(4)；
(4)利用勾股定理计算判断无人艇当前位置(xk,yk)与对象障碍物ot位置间的距离dot和ot的前瞻补偿半径r，并判断dot是否小于r，是则转步骤(5)，否则转步骤(8)；其中r与d、r0、rz的大小有关，表示为r＝g(d,rz,r0)，式中r0为障碍物的半径；rz为障碍物的影响半径；d为障碍物到无人艇与目标点间连线的距离。
(5)利用近障碍前瞻补偿方法对无人艇当前期望艏向θ进行补偿，转步骤(6)；
(6)利用公式
(7)无人艇移动至(xk+1,yk+1)，无人艇运行一步后步数k＝k+1，无人艇的位置由(xk,yk)变成了(xk+1,yk+1)，转步骤(8)；
(8)判断无人艇当前位置(xk,yk)距离远端规划目标点(xf,yf)的距离是否小于阈值g(g一般取值3到6)，是则视为抵达目标点并转入近岸规划阶段，否则转步骤(2)；
远端规划阶段步骤(5)中的近障碍前瞻补偿方法是本发明的一个核心，其主要步骤为：
(1)根据勾股定理计算无人艇当前位置(xk,yk)与补偿对象障碍物(xot,yot)之间的相对距离d；转步骤(2)；
(2)判断d是否大于阈值d，该阈值d一般由障碍物的影响半径r和障碍物本身的尺寸r共同决定，即d＝f(r,r)，是则转步骤(3)，否则转步骤(7)；
(3)利用勾股定理计算出目标点(xf,yf)与无人艇当前位置(xk,yk)间的相对角度a(程序中a>0)以及障碍物与无人艇之间的相对角度b，转步骤(4)；
(4)如果a>0且b<0，则认为目标点在无人艇右翼而障碍物在无人艇左翼，补偿量取负号并转步骤(6)，否则转步骤(5)；
(5)如果b>0且a>b，则认为目标点和障碍物都在无人艇右翼且目标点更靠右，补偿量取正号并转步骤(6)，否则补偿量取负号并转步骤(6)；
(6)计算补偿量α＝t*r，其中r是前瞻补偿角速度，t表示每步长的时间，结合正负号对当前期望艏向θ进行补偿，转步骤(7)；其中r的大小受到d、r0、r的影响，即r＝f(r,d,r0)，式中r0为障碍物的半径；r为近障碍前瞻补偿半径；d为障碍物到无人艇与目标点连线的距离。
(7)返回新的期望艏向θ’，令θ＝θ’；
关于近障碍前瞻补偿方法的详细分析如图5所示：
如图5所示，首先判断障碍物位于起始点与目标点连线的哪一侧，确定补偿角速度的正负号(由于仿真程序中将目标点设置在了无人艇右翼，因此说明书中只考虑目标点在右翼的情况即a>0，左翼同理)；根据影响障碍物前瞻补偿程度的因素d、r0、rz，分别确定近障碍前瞻补偿的半径r(r1≤r≤r2)和前瞻补偿角速度r。
设无人艇与障碍物实时距离为l：
当无人艇靠近障碍物时：
当l＞r2时，无人艇艏向只受到目标点的影响，方向为引力方向，前瞻补偿角速度r＝0，艏向角求解公式为：
ψ＝θ1
式中，ψ表示无人艇艏向角，θ1表示引力方向。
当rz＜l≤r2时，无人艇艏向受到目标点、补偿角速度的共同影响，前瞻补偿角速度为r，艏向角求解公式为：
ψ＝θ1+rt
式中，t表示每步长所需的时间。
当r1＜l≤rz时，无人艇艏向受到目标点、障碍物、补偿角速度的共同影响，前瞻补偿角速度为r，艏向角求解公式为：
ψ＝θ0+rt
式中，θ0表示引力与斥力的合力方向。
当l≤r1时，无人艇艏向受到目标点、障碍物共同影响，艏向角求解公式为：
ψ＝θ0
①当无人艇远离障碍物时：
这里规定只考虑无人艇第一次到达近障碍前瞻补偿区域最小边界(即l＝r1)的情况，之后即便无人艇再次进入该区域，也不对该障碍做前瞻补偿。
当l≤rz时，无人艇艏向受到目标点、障碍物共同影响，艏向角求解公式为：
ψ＝θ0
当l＞rz时，无人艇艏向受到目标点引力的影响，艏向角求解公式为：
ψ＝θ1
所述的近岸规划阶段，其特点在于引入了虚拟码头概念，从而降低了无人艇在低速靠泊时的控制难度，且近岸规划阶段步骤包括：
(1)由传感器获取无人艇当前位置(xk,yk)，目标码头位置(xl,yl)，并设置虚拟码头的个数m(m＝4)和位置[xd1,yd1；...；xdm,ydm]，转步骤(2)；
(2)根据改进人工势场法计算当前位置无人艇受到第i(i初始值为1)个虚拟码头的引力和期望艏向θ，转步骤(3)；
(3)根据勾股定理计算无人艇当前位置(xk,yk)距离最终目标码头(xl,yl)的距离l，根据los视距法利用公式
(4)利用公式
(5)利用勾股定理计算无人艇当前位置(xk,yk)与第i个虚拟码头(xdi,ydi)之间的距离li，判断li是否小于g(g取值一般为3到6)，是则转步骤(6)，否则返回步骤(2)；
(6)判断第i个虚拟码头是否为目标码头，是则规划完毕程序结束，否则令i＝i+1，返回步骤(1)；
其中引入的虚拟码头是本发明的另一个核心，关于虚拟码头的概念分析：
码头终端约束中要求无人艇的最终姿态要与岸边平行，由于无人艇进入码头区域之后速度大小受到一定的限制，而速度减小时无人艇舵效会变差，艏向将不易调节，因此应尽早调节艏向，本发明提出虚拟码头目标点引导法，添加若干个(一般取3到5个)隐形目标点，如图6所示(图中i＝1到4为虚拟码头，i＝5时为最终目标码头)，并结合los视距法实现无人艇靠泊后期对艏向和速度的高精度调节。
一个成熟的无人艇系统包括探测系统，规划系统和控制系统，无人艇通过探测系统与外界环境进行信息交换；通过规划系统计算出合理的速度，正确的航向和可行的路线；最终通过控制系统驱动无人艇执行计划。好的规划系统既能降低对探测系统的要求，又能降低控制难度，因此一个良好的规划系统是成熟无人艇系统中必不可少的一环。
综上，本发明提出了一种考虑约束的无人艇靠泊规划方法。该规划方法根据无人艇所处环境的特征变化将无人艇的靠泊行为分成了远端规划和近岸规划两个阶段。远端规划包括：(1)计算当前的期望艏向；(2)对期望艏向进行近障碍前瞻补偿；(3)更新无人艇位置；(4)判断无人艇是否到达远端规划目标点，是则结束远端规划转入近岸规划阶段，否则返回步骤(1)。近岸规划包括：(1)计算出指向第i(i的初始值为1)个虚拟码头的期望路径；(2)计算无人艇距离目标码头的距离，为当前期望速度添加码头约束；(3)更新无人艇位置；(4)判断无人艇是否抵达第i个虚拟码头，是则转步骤(5)，否则返回步骤(2)；(5)判断第i个虚拟码头是否为目标码头，是则规划完毕程序结束，否则令i＝i+1，返回步骤(1)。本发明采用了改进人工势场法，考虑了靠泊过程中岸壁效应对无人艇的影响并有效地解决了规划路径的转角过大问题，为无人艇的自主靠泊控制问题提供便利。