流速自适应的无人船自主规划路径和运动精确跟踪方法
技术领域
本发明涉及一种流速自适应的无人船自主规划路径和运动精确跟踪方法，属于无人船自主控制海上作业技术领域。
背景技术
无人船作为一种新型海洋观测平台，具有体量小、速度快、吃水浅的优势，可进入传统海洋调查船所不能及的特殊水域，在近海环境调查、管线巡检等领域已有广泛的应用。无人船按照指定的路径运行是执行各项任务的基础，实际运动路径与指定路径的一致可使得无人船各项任务的数据准确性提升。
无人船在作业前会指定作业区域，然后根据作业需求进行作业区域的路径规划，而无人船在航行中会遇到各种不定因数的影响，如河流的水流力、风力、波浪力等，这些外界干扰会使运行在规划路线中的无人船偏离规划路径，导致无人船工作产生较大的误差，又由于水流力、风力、波浪力的方向和大小的不确定性，很难在规划路径前进行预先设定，目前消除这种误差的方式多为预先设定偏移量，预先设置偏移需要知道此段水域的水文数据，而工作的目标区域多数为待测量区域还没有任何资料，因此开发一种自适应的路径规划算法极其重要。
目前，自主规划路径方法中多数为在复杂的地形或区域中解算出最优路径，基本没考虑有外界因素干扰下路径如何调整，更没有对复杂多变的外部干扰做到自适应式调整。
发明内容
针对现有技术上存在的不足，本发明的目的在于提出一种流速自适应的无人船自主规划路径和运动精确跟踪方法，据运行状态进行运动补偿调整，从而使无人船的实际运行轨迹和规划路径一致。
为了实现上述目的，本发明是通过如下的技术方案来实现：
流速自适应的无人船自主规划路径和运动精确跟踪方法，其特征在于：
无人船通过其上设置的GPS定位器实时传输所述无人船的经纬度位置及高程信息至基站，在基站端使用软件对无人船的经纬度位置及高程信息进行计算和对无人船的航行艏向角和速度进行调控；
选定无人船的作业区域，为了对整个区域进行扫描，并根据无人船的检测宽度进行航行间距划分，将选定的多边形的作业区域切割成条状；
计算所述作业区域内最远的直线距离，以规划区域离所述无人船最近的端点为起点，向所述规划区域内部生成间隔大小为无人船扫描宽度的大于等于2个平行线，根据无人船作业方向设置切割线的方向，所述规划区域为手动点击输入的一个要作业的多边形作业区域；
通过一条直线与多边形相交判定算法来计算切割线与多边形的交点，通过交点把原线段分成大于等于2个段线，一条切割线与多边形的两个交点分别为一段规划线的起点和终点；同理，大于等于2条切割线与多边形的相交得到大于等于2段规划线，逐个将每段规划线的终点和最近一条线的起点依次连接成一条完整的规划路径；
为了保证无人船能够按照所述规划路径航行，无人船在规划路径跟踪过程中进行速度控制，在直线运行时采用匀加速到设定航速，当无人船将要到达拐点或终点时进行减速，有利于减小船体的惯性运动；
由于在外界有干扰情况下，无人船会偏离规划路径，所述外界有干扰情况下为有水流、风的情况下(本发明以水流为例，其他干扰力控制方法相同)，采用调整艏向角和跟踪虚拟航线的方式使无人船跟踪规划路径。
进一步地，以时间周期T来不断计算无人船艏向角，根据无人船与规划线的位置关系来计算无人船艏向角，其具体计算公式如下；
vit＝(xt-xi,yt-yi)(1)
vi(i+1)＝(xi+1-xi,yi+1-yi) (2)
(xlos,ylos)为制导点坐标，(xt,yt)为无人船位置，l为前视距离，为手动设定的参数，α为当前无人船参考艏向角，vit为点(xi,yi)到无人船位置(xt,yt)的向量，vi(i+1)为跟踪路径线段向量，(xp,yp)为无人船与路径的垂点，Δp(i+1)为垂点到线段末端的长度；根据上述艏向角控制规则不断的调整艏向角α，直到无人船的垂直于航线的分速度等于水流的分速度，无人船在外力作用下稳定运行形成固定偏移D；
当无人船的规划路径的航线或水流方向改变时，无人船按照调控艏向角α的方法来调整无人船船体的艏向角达到稳定航向，所述稳定航向，指无人船的垂直距离D变化幅度较小时的无人船的船体艏向角α；优选地，变化幅度为0。
进一步地，在调控艏向角α过程中，由于无人船的惯性存在左右震荡，为了防止无人船在调控过程中震荡，采用预先调节，时间周期T的计算无人船艏向角和采集转向角速度；根据无人船角速度大小对无人船进行预控制，促使无人船平滑过渡到稳定跟踪，预艏向角调节方法如下：
β＝(D-Dold)*k(8)
αd＝α-β,|β|＜|α| (9)
其中，Dold为上周期垂直距离，αd为采用预艏向控制后的期望艏向角，k为增益系数。
进一步地，当无人船船体达到所述稳定航向时会存在固定偏差，所述固定偏差通过虚拟调整规划路径的航线来消除；所述虚拟调整规划路径的航线的具体方法为：周期的计算无人船船体与规划路径的航线的垂直偏差距离D，并进行累计，根据累计的垂直偏差距离大小，虚拟调整规划路径航线的坐标使其垂直移动得到虚拟航线，使无人船运行虚拟航线稳定的偏移位置与规划路径的航线相重合；当无人船调整到逼近规划航线时，累计的垂直偏差不再变化表示虚拟航线趋于稳定；
(xit,yit)＝(xi,yi)+ki(Ix,Iy) (10)
(xit+1,yit+1)＝(xi+1,yi+1)+ki(Ix,Iy) (11)
式中，Ix为偏差在x方向的误差积累，式中Iy为偏差在y方向的误差积累，ki为积累的增益系数，(xit,yit)、(xit+1,yit+1)为虚拟航线的位置，当外界干扰改变时无人船发生新的偏差，虚拟航线根据所述调整规划路径的航线的方法自动调整，以此达到自适应的路径规划控制。
进一步地，无人船转向时会由于速度较快而影响跟踪的稳定性，降低转向过程的速度有利于减小跟踪误差，其减小的速度与转向航线角度相关，速度控制的具体方法如下：
式中，γ为上一条跟踪线段和现跟踪线段之间的夹角，v0为无人船设定速度，v为无人船实际控制速度。
进一步地，所述大于等于2个平行线覆盖的面积大于等于作业区域最远直线距离的平方，保证了切割线能够全部覆盖规划区域。
进一步地，所述时间周期为T＝1s。
进一步地，每一段的所述规划线用弓字形或Z字形的连接方式形成不同的规划路径。
本发明的有益效果为：
(1)无人船在水流作用下能够根据偏移航线的距离和前进方向自动调整首向来适应水流，使无人船在复杂多变的水流条件下能够自适应的调整，无需预先知道水流状况；
(2)无人船在调节的同时根据船体的角速度预先控制无人船的惯性作用，使在调节的过程中均匀过度、不过度、不震荡、保证无人船的运行的稳定性；
(3)对无人船偏离规划线的距离进行累积，根据累积大小设置虚拟航线，无人船以虚拟航线为目标航线，使无人船在水流的作用下的实际航线与规划航线相重合，从而达到精确跟踪规划航线进行工作。
附图说明
下面结合附图和具体实施方式来详细说明本发明：
图1为本发明规划区域切割线示意图(两种不同航线切割的示意图)；
图2为本发明切割线生成规划路径示意图(左侧为往返式路径右侧为Z字路径)；
图3为本发明无人船艏向控制规则图；
图4为本发明无人船在外力作用下逐渐稳定示意；
图5为本发明采用预艏向控制后控制规则图；
图6为本发明无人船使用预艏向控制角度变化图；
图7为本发明无人船未使用预艏向控制轨迹图；
图8为本发明无人船使用预艏向控制轨迹图；
图9为本发明无人船跟踪虚拟航线原理图(虚拟航线未稳定)；
图10为本发明无人船跟踪虚拟航线原理图(虚拟航线稳定)。
图11为本发明全局虚拟航线图。
图12为本发明无人船速度控制示意图。
图13为本发明无人船控制流程图。
具体实施方式
为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。
参见图13，本发明的流速自适应的无人船自主规划路径和运动精确跟踪方法，包括：
步骤1：无人船通过其上设置的GPS定位器实时传输所述无人船的经纬度位置及高程信息至基站，在基站端使用软件对无人船的经纬度位置及高程信息进行计算和对无人船的航行艏向角和速度进行调控。
步骤2：选定无人船的作业区域，为了对整个区域进行扫描，并根据无人船的检测宽度进行航行间距划分，将选定的多边形的作业区域切割成条状。
步骤3：计算作业区域内最远的直线距离，以规划区域离所述无人船最近的端点为起点，向规划区域内部生成间隔大小为无人船扫描宽度的大于等于2个平行线，根据无人船作业方向设置切割线的方向，如图1所示；规划区域为手动点击输入的一个要作业的多边形作业区域。
步骤4：通过一条直线与多边形相交判定算法来计算切割线与多边形的交点，通过交点把原线段分成大于等于2个段线，一条切割线与多边形的两个交点分别为一段规划线的起点和终点；同理，大于等于2条切割线与多边形的相交得到大于等于2段规划线，逐个将每段规划线的终点和最近一条线的起点依次连接成一条完整的规划路径，如图2所示。
步骤5：为了保证无人船能够按照所述规划路径航行，无人船在规划路径跟踪过程中进行速度控制，在直线运行时采用匀加速到设定航速，当无人船将要到达拐点或终点时进行适当的减速，有利于减小船体的惯性运动。
步骤6：由于在外界有干扰情况下，无人船会偏离规划路径，外界有干扰情况下为有水流、风的情况下(本发明以水流为例，其他干扰力控制方法相同)，采用调整艏向角和跟踪虚拟航线的方式使无人船跟踪规划路径。
进一步地，以时间周期T来不断计算无人船艏向角，根据无人船与规划线的位置关系来计算无人船艏向角，其具体计算原理如图3所示，具体计算公式如下；
vit＝(xt-xi,yt-yi)(1)
vi(i+1)＝(xi+1-xi,yi+1-yi) (2)
(xlos,ylos)为制导点坐标，(xt,yt)为无人船位置，l为前视距离，为手动设定的参数，α为当前无人船参考艏向角，vit为点(xi,yi)到无人船位置(xt,yt)的向量，vi(i+1)为跟踪路径线段向量，(xp,yp)为无人船与路径的垂点，Δp(i+1)为垂点到线段末端的长度；根据上述艏向角控制规则不断的调整艏向角α，直到无人船的垂直于航线的分速度等于水流的分速度，无人船在外力作用下稳定运行形成固定偏移D，如图4所示。
当无人船的规划路径的航线或水流方向改变时，无人船按照调控艏向角α的方法来调整无人船船体的艏向角达到稳定航向，稳定航向是指无人船的垂直距离D变化幅度较小时的无人船的船体艏向角α。
进一步地，在调控艏向角α过程中，由于无人船的惯性存在左右震荡，为了防止无人船在调控过程中震荡，采用预先调节，时间周期T的计算无人船艏向角和采集转向角速度；根据无人船角速度大小对无人船进行预控制，促使无人船平滑过渡到稳定跟踪，预艏向角调节的具体实现如图5所示，预艏向角调节方法如下：
β＝(D-Dold)*k(8)
αd＝α-β,|β|＜|α|(9)
其中，Dold为上周期垂直距离，αd为采用预艏向控制后的期望艏向角，k为增益系数，控制效果如图6-8所示。
进一步地，当无人船船体达到所述稳定航向时会存在固定偏差，固定偏差通过虚拟调整规划路径的航线来消除；虚拟调整规划路径的航线的具体方法为：周期的计算无人船船体与规划路径的航线的垂直偏差距离D，并进行累计，根据累计的垂直偏差距离大小，虚拟调整规划路径航线的坐标使其垂直移动得到虚拟航线，使无人船运行虚拟航线稳定的偏移位置与规划路径的航线相重合；当无人船调整到逼近规划航线时，累计的垂直偏差不再变化表示虚拟航线趋于稳定，如图9-11所示；
(xit,yit)＝(xi,yi)+ki(Ix,Iy) (10)
(xit+1,yit+1)＝(xi+1,yi+1)+ki(Ix,Iy)(11)
式中，Ix为偏差在x方向的误差积累，式中Iy为偏差在y方向的误差积累，ki为积累的增益系数，(xit,yit)、(xit+1,yit+1)为虚拟航线的位置，当外界干扰改变时无人船发生新的偏差，虚拟航线根据所述调整规划路径的航线的方法自动调整，以此达到自适应的路径规划控制。
进一步地，无人船转向时会由于速度较快而影响跟踪的稳定性，降低转向过程的速度有利于减小跟踪误差，其减小的速度与转向航线角度相关，如图12所示，速度控制的具体方法如下：
式中，γ为上一条跟踪线段和现跟踪线段之间的夹角，v0为无人船设定速度，v为无人船实际控制速度。
作为进一步改进，大于等于2个平行线覆盖的面积大于等于作业区域最远直线距离的平方，保证了切割线能够全部覆盖规划区域。
作为进一步改进，时间周期为T＝1s。
作为进一步改进，每一段的规划线用弓字形或Z字形的连接方式形成不同的规划路径，如图2所示。
本发明自主规划路径和运动控制流程如图13所示，其详细过程如下：
步骤A：手动选择规划区域，在地图上点击多个点形成闭合多边形，在此多边形内执行图1切割规划航线，形成无人船航行使用的规划航线，沿着此航线可完成区域的全覆盖作业。
步骤B：根据无人船误差积累通过式(8～11)计算虚拟航行相对于规划航线的位置，通过无人船内部定位系统获取定位信息(xt,yt)，计算无人船在跟踪虚拟航线上的垂点(xp,yp)，运行通过式(1～6)计算无人船在大地坐标系上航行艏向角α，同时根据无人船运动状态式(7～9)计算预控制后的艏向期望值αd，无人船沿着该期望值αd方向运行。
步骤C：无人船更新误差积累Ix、Iy、Dold参数。同时检测垂点(xp,yp)与此段路径末点(xi+1,yi+1)距离小于某设定值，则进行减速控制执行式(12)，并切换下一条规划航线进行跟踪，如此航线为最后一条航线则路径跟踪结束，否则执行步骤B。
步骤D：无人船区域作业完成。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。
