一种路径跟踪避障制导方法与流程

本发明涉及船舶控制工程与船舶自动化航行装备应用领域，尤其是涉及一种路径跟踪避障制导算法。

背景技术：

制导算法是解决船舶运动航迹保持问题的必不可少的技术，传统的基于航路点的船舶路径规划，要求船舶沿着设定航线自动航行，归结于路径跟踪的制导问题^[1]。然而，海洋环境具有多变性，船舶在航行过程中经常会发生避障行为，尤其在穿越渔区和途径狭窄或繁忙水道时，这就要求船舶制导算法具有避障功能。避障算法，起源于机器人控制的研究，常用的方法有势场法，速度避障法和动态窗法，其中动态窗法将船舶前进速度和艏摇速度在采样周内可达到的范围以动态窗的形式进行描述，尤其适合解决高速无人船的多目标避障问题，下面对文献[2]提出的基于动态窗的避障算法进行技术特征的简要介绍。

该算法在对设定航路点进行追踪的同时，对探测范围内的多个目标进行避障。要求已知船舶在采样周期ts内前进速度的动态窗口vt和艏向角θusv的动态窗口vhead。按照式(8)的最大化原则选取最优艏向角

fe(θ)＝ε·fhead(θ)+(1-ε)·fsafe(θ)(8)

其中，θgoal是航路点对于实船的真方位角，bobs是探测半径dnear内障碍物造成的不可行真方位角的集合，αi是探测半径内障碍物中心对于实船的真方位角，ε是可调整参数。根据式(11)插入当前制导坐标点pinsert，dinsert为一个小于探测半径的距离。

进而通过式(12)获得当前制导艏向角作为当前艏向角的跟踪目标。

定义按照式(13),(14)定义当前制导速度。

因为该避障算法仅考虑实船相对方位角范围内的障碍物，所以给出的定义如式(15)所示。

η1的定义如式(16)所示。是当前转艏角速度。

为了防止速度过大造成碰撞，实船速度应当满足式(17)，如图1所示。

化简可得：

则vmax应当满足式(19)，rmax是探测范围内最大障碍物的半径。η2应当满足式(20)。

通过以下流程实时获取船舶目标艏摇角和目标速度对实船进行避障制导。最终可以获得如图3所示的避障效果。

现有面向大型船舶制导算法的研究多是针对路径跟踪提出。其中，los制导算法^[3]发展较为成熟且被广泛应用，然而却存在无法对转向部分的曲线段进行航迹保持，以及难以进行路径规划等缺点。dvs制导算法^[4]通过导引虚拟小船实现直线段及曲线段的路径规划，从根本上克服了los制导算法的缺点，通过生成动态虚拟小船作为直接跟踪目标，避免了执行器输入过大。然而，基于动态虚拟小船的生成方式，传统dvs制导算法难以同避障算法相结合。

在航海领域，避障算法的研究目前主要针对小型无人艇，常用的避障算法有动态窗算法^[2]，速度避障法^[5]，势场避障法^[6]等。其中动态窗算法仅考虑了船舶的前进和艏摇运动，没有考虑横荡运动，故不适应于考虑欠驱动特性的大型船舶。速度避障法有计算简便的数学特性，却仅需要假设船舶运动速度是线性的，且难以同路径跟踪制导算法结合。势场法需要对环境提前预知，且在通过狭窄通道时极易失去稳定，造成振荡，故不适用于具有多变性的真实海况。

在过去的研究中，避障算法和路径跟踪制导算法的研究往往是分开的。从图3可以看出，避障算法往往仅考虑对航路点的跟踪，对于大型商船而言这是不切实际的。同时，大型商船在进行路径跟踪时，经常会遇到作业中的小型渔船群，岛屿礁石等一定程度上不可预测的静止或慢时变目标。一种能够进行路径跟踪同时考虑多目标避障的制导算法成为亟待解决的研究点。文献[7]和[8]分别基于los制导算法和serret-frenet框架提出了一种考虑单目标避障的路径跟踪制导算法，该算法对多目标障碍物无效且仍然没有对曲线段进行路径规划的能力。文献[9]基于龙格库塔法提出了一种具有路径规划和多目标避障能力的制导算法，却无法适用于具有欠驱动特性的大型商船。

技术实现要素：

鉴于已有技术存在的缺陷，本发明公开了一种路径跟踪避障制导方法，包括以下步骤：

一种路径跟踪避障制导方法，包括以下步骤：

(1)执行路径规划并获取导引虚拟小船的时间序列信号；

(2)根据动态虚拟小船和导引虚拟小船的位置信息，计算动态虚拟小船的路径跟踪艏向角ψdp和路径跟踪前进速度udp；

(3)实时监测动态虚拟小船到障碍物安全圆族边界的最小距离lmin，并根据实时监测的最小距离lmin确定动态虚拟小船的最终艏向角ψd；

(4)计算动态虚拟小船的最终前进速度ud和转艏角速度rd；

(5)判断动态虚拟小船与导引虚拟小船间的距离ldr与设定的阈值距离ldrset的大小，如果动态虚拟小船与导引虚拟小船的距离ldr大于等于阈值距离ldrset，则导引虚拟小船下一时间点的运动状态保持当前状态不变；如果动态虚拟小船与导引虚拟小船的距离ldr小于阈值距离ldrset，则导引虚拟小船下一时间点运动状态信息按照其时间序列的排序下移；

(6)将制导信息输入到控制回路中并更新动态虚拟小船下一时间点的位置信息；

(7)测量实船位置，判断是否到达目的地，如果“是”，结束船舶航行；如果“否”，时间更新t＝t+1并进入步骤(2)。

进一步地，在步骤(2)中所述动态虚拟小船的路径跟踪艏向角ψdp采用公式(2)进行计算：

其中，(xr，yr)，(xd，yd)分别为导引虚拟小船和动态虚拟小船的坐标；

所述动态虚拟小船的路径跟踪前进速度udp采用公式(3)进行计算：

udp＝klldr+urcos(ψdp-ψr)(3)

其中ldr为动态虚拟小船与导引虚拟小船间的距离，kl为预设参数，其数值大小是根据动态虚拟小船在路径跟踪情况下对导引虚拟小船位置的收敛速率进行设置，其值越大，收敛越快，但考虑到执行器的限制，应当适当选取。

进一步地，在步骤(3)中所述根据实时监测的动态虚拟小船到障碍物安全圆族边界的最小距离lmin确定动态虚拟小船的最终艏向角ψd包括：

1)当动态虚拟小船到障碍物安全圆族边界的最小距离lmin小于等于船舶障碍物探测设备的激活圈半径ractive时，执行避障算法以确定最终艏向角ψd：

避障算法原理为：根据船舶上障碍物探测设备(如导航雷达，摄像设备等)的作用半径确定避障算法的探测圈半径rtest和激活圈半径ractive，当障碍物进入探测圈时，在探测到的障碍物的边缘生成半径为rs的安全圆族，此时若安全圆族部分或全部进入激活圈，从路径跟踪制导算法给出的动态虚拟小船路径跟踪艏向角ψdp开始，向两侧搜索能够避免与圆族相交的最近的艏向角和

按照评价函数(4)的最小化原则从两个艏向角和中选取最优值作为制导算法中动态虚拟小船的最终艏向角ψd：

其中i＝1,2，ε是自定义的权重值，ψd(j-1)代表上一个采样时间点的最终艏向角；

2)当动态虚拟小船到障碍物安全圆族边界的最小距离lmin大于船舶中障碍物探测设备的激活圈半径ractive时，动态虚拟小船的最终艏向角ψd＝ψdp。

进一步地，在所述步骤(4)中，所述动态虚拟小船的最终前进速度ud采用公式(5)(6)进行计算：

其中，lmin为障碍物安全圆族边界到动态虚拟小船的最小距离，rc∈[0,ractive/2]，为手动设置的一个阈值，udo为手动设置的避障过程的最小值，ldbset为手动设置的实船到动态虚拟小船间距离上界，ldb为实船距离动态虚拟小船的实际距离，u′dp值计算过程如下：首先设置动态虚拟小船路径跟踪速度最大值udpmax，按照公式(3)计算动态虚拟小船路径跟踪速度udp，若udp≥udpmax，则保持u′dp＝udpmax，否则令u′dp＝udp；

所述动态虚拟小船的转艏角速度rd采用如公式(7)所示的1阶滤波器进行估计：

其中，βψd为动态虚拟小船艏向角ψd的估计值，τ为设定参数。

本发明具有以下有益效果：

1)本发明的制导算法解决了船舶在路径跟踪过程中对多目标进行避障的现实问题，提高了船舶的自动化程度。基于就近原则的避障算法简单易于实现，避免了航程的浪费且保证了航行安全。

2)本发明继承了传统dvs算法的优点，解决了“los算法无法进行曲线段路径规划”的问题，适用于高精度航迹保持控制中，同时，易于同欠驱动问题的先进控制算法结合。考虑船舶执行装置的限制，本发明进行了dvs的速度规划，相比传统dvs制导算法，具有异曲同工之妙。

附图说明

图1是现有技术中最小半径限制原理图；

图2是现有技术中基于动态窗的避障算法执行流程图；

图3是现有技术中基于动态窗的避障算法2维效果图；

图4是本发明的船舶路径跟踪控制逻辑结构图；

图5是现有技术中导引虚拟小船的航路点处弧线路径规划示意图；

图6是本发明改进的动态虚拟小船制导算法基本原理；

图7是本发明的避障算法基本原理；

图8是本发明中u′d速度规划示意图；

图9是本发明跟踪避障制导算法执行流程图；

图10为本发明实施例的船舶示意图；

图11为实施例中实验海浪模型干扰三维视图；

图12为实施例中模拟真实海洋环境下路径跟踪避障2-d效果图；

图13为实施例中船舶运动姿态变量u,v,ψ时间变化曲线示意图；

图14为实施例中执行装置控制输入n,δ时间变化曲线示意图。

具体实施方式

为使本发明的技术方案和优点更加清楚，下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚完整的描述。

如图4和图9所示为本发明公开的路径跟踪避障制导算法的船舶路径跟踪控制逻辑结构图和跟踪避障制导算法执行流程图(图中gvs表示导引虚拟小船，dvs表示动态虚拟小船)。如图4所示船舶的整个执行模块分为制导系统和控制系统两部分，制导模块采用本发明的提出的路径跟踪避障制导算法。

如图9所示本发明提出的路径跟踪避障制导算法具体步骤如下：

(1)执行路径规划并获取导引虚拟小船的时间序列信号。

根据驾驶员设定的航路点位置信息计算出船舶的路径跟踪参考路径，根据导引虚拟小船的运动学特性(1)，获取导引虚拟小船运动状态信息的时间序列信号(导引虚拟小船的艏向角ψr、前进速度ur、位置坐标(xr,yr)按照时间顺序的排列)；

如图6所示从左至右依次为实船、动态虚拟小船和导引虚拟小船。导引虚拟小船和动态虚拟小船均具有如公式(1)的运动学特性：

其中(xr，yr)、(xd，yd)分别为导引虚拟小船和动态虚拟小船的坐标，ur、ud分别为导引虚拟小船的前进速度和动态虚拟小船的最终前进速度，ψr、ψd分别为导引虚拟小船的艏向角和动态虚拟小船的最终艏向角。导引虚拟小船的前进速度ur和艏向角ψr与现有技术中动态虚拟小船路径跟踪制导算法的计算方法相同。

选取导引虚拟小船运动状态信息时间序列的第一组数据作为导引虚拟小船初始状态信息；实船所有运动状态信息可以通过船用导航设备(gps、计程仪等)测得；对动态虚拟小船的位置信息进行设定，其位置处于实船和导引虚拟小船的连线中点上，其全部运动状态信息可通过下述步骤补全。

(2)根据动态虚拟小船和导引虚拟小船的当前位置信息，计算动态虚拟小船的路径跟踪艏向角ψdp和路径跟踪前进速度udp。动态虚拟小船的路径跟踪艏向角ψdp采用公式(2)进行计算：

动态虚拟小船的路径跟踪前进速度udp采用公式(3)进行计算：

udp＝klldr+urcos(ψdp-ψr)(3)

其中ldr为动态虚拟小船与导引虚拟小船间的距离，kl为预设参数，其数值大小是根据动态虚拟小船在路径跟踪情况下对导引虚拟小船位置的收敛速率进行设置，其值越大，收敛越快，但考虑到执行器的限制，应当适当选取。

区别于传统动态虚拟小船路径跟踪制导算法，实船相对导引虚拟小船位置不限制动态虚拟小船的生成。在该算法中，其本身按照(1)所示的运动学关系独立航行，从公式(2)和(3)可知，动态虚拟小船的路径跟踪艏向角ψdp和路径跟踪速度udp通过其本身对于导引虚拟小船的关系生成，其运动过程是连续的，便于同后续避障策略结合。

(3)实时监测动态虚拟小船到障碍物安全圆族边界的最小距离lmin，并根据实时监测的动态虚拟小船到障碍物安全圆族边界的最小距离lmin确定动态虚拟小船的最终艏向角ψd；

根据实时监测的动态虚拟小船到障碍物安全圆族边界的最小距离lmin确定动态虚拟小船的最终艏向角ψd包括：

1)当动态虚拟小船到障碍物安全圆族边界的最小距离lmin小于等于船舶障碍物探测设备的激活圈半径ractive时，执行避障算法以确定最终艏向角ψd：

如图7所示避障算法原理为：根据船舶上障碍物探测设备(如导航雷达，摄像设备等)的作用半径确定避障算法的探测圈半径rtest和激活圈半径ractive。因为本制导算法要求通过控制算法保证实船对动态虚拟小船位置的快速收敛，所以假设探测圈和激活圈均是以动态虚拟小船的位置为圆心的。当障碍物进入动态虚拟小船探测圈时，在探测到的障碍物的边缘生成半径为rs的安全圆族，此时若安全圆族全部或部分进入激活圈(lmin≤ractive)，则从路径跟踪制导算法给出的动态虚拟小船路径跟踪艏向角ψdp开始，向两侧搜索能够避免与圆族相交的最近的艏向角和

按照评价函数(4)的最小化原则从两个艏向角和中选取最优值作为制导算法中动态虚拟小船的最终艏向角ψd：

其中i＝1,2，ε是自定义的权重值，ψd(j-1)代表上一个采样时间点的最终艏向角；

该评价函数不仅保证了最终艏向角ψd距离ψdp偏差小，同时具有连续性，避免了最优值来回切换引起的抖振。另外，因为障碍物边界的圆族是逐渐进入激活圈的，所以路径跟踪和避障算法的切换是光滑的。

2)当动态虚拟小船到障碍物安全圆族边界的最小距离lmin大于船舶中障碍物探测设备的激活圈半径ractive时，动态虚拟小船的最终艏向角ψd＝ψdp。当障碍物在动态虚拟小船探测圈外时，理论上lmin是无法测得的，此时定义lmin＝rtest。

(4)计算动态虚拟小船的最终前进速度ud和转艏角速度rd；

在该步骤中，出于安全性的考虑，船舶在遇到障碍物时需要降速，同时，考虑到执行器有界性，动态虚拟小船在距离实船较远时，应当降速等待实船追踪，本发明将采用公式(5)和(6)对动态虚拟小船的最终前进速度ud进行规划：

其中，lmin为上述步骤定义的最小距离，rc∈[0,ractive/2]，为手动设置的一个阈值，其值的大小决定了过渡区间(如图8所示，过渡区间表示u′dp过渡到udo对应的lmin区间)的大小，为了保证避障的安全性，选取时应在保证一定过渡区间的原则下尽量取大，udo为手动设置的避障过程的动态虚拟小船速度最小值，选取时应满足避碰过程中船舶安全性的要求，ldbset为手动设置的实船到动态虚拟小船间距离上界，其值的选取要满足控制算法作用下实船对动态虚拟小船追踪的要求，实船对动态虚拟小船追踪越快，ldbset可选取越大。ldb为实船距离动态虚拟小船的实际距离。

公式(6)中的u′d采用公式(5)进行规划，图8所示为采用公式(5)对u′d进行规划的示意图。其中，公式(5)中的u′dp值计算过程如下：首先设置动态虚拟小船路径跟踪速度最大值udpmax，按照公式(3)计算动态虚拟小船路径跟踪速度udp，若udp≥udpmax，则保持u′dp＝udpmax，否则令u′dp＝udp。跟据公式(6)可知当ldb等于ldbset时ud＝0，动态虚拟小船保持原地不动，又知控制回路会保证实船收敛到动态虚拟小船的位置上，由此保证了动态虚拟小船距离实船不会超过ldbset。

采用如公式(7)所示的1阶滤波器进行估计动态虚拟小船的转艏角速度rd：

其中，βψd为动态虚拟小船艏向角ψd的估计值，τ为设定参数，其值的大小是根据滤波器的滤波性能和对rd估计的准确性进行设定，其值越大，滤波性能越好，但估计的准确性越低，选取时要两者兼顾。

(5)判断动态虚拟小船与导引虚拟小船间的距离ldr与设定的阈值距离ldrset的大小，如果动态虚拟小船与导引虚拟小船的距离ldr大于等于阈值距离ldrset，则导引虚拟小船的下一时间点运动状态保持当前采样时间点的运动状态不变，如果动态虚拟小船与导引虚拟小船的距离ldr小于阈值距离ldrset，则导引虚拟小船下一时间点的运动状态按照其时间序列的排序下移。

在动态虚拟小船执行避障算法的过程中，前进速度会下降，而导引虚拟小船的速度始终为ur，如果不对导引虚拟小船加以限制，则会造成动态虚拟小船距离导引虚拟小船过远，影响路径跟踪效果。因此，设置动态虚拟小船与导引虚拟小船间的阈值距离ldrset，通过限制动态虚拟小船与导引虚拟小船的距离ldr，可以实现精确路径跟踪，当ldr≥ldrset时，导引虚拟小船的下一时间点运动状态会保持当前采样时间点的状态不变，这相当于导引虚拟小船会对动态虚拟小船进行等待，避免动态虚拟小船距离导引虚拟小船过远。

(6)将制导信息输入到控制回路中并更新动态虚拟小船下一时间点的位置信息。

将动态虚拟小船的当前状态变量(最终艏向角ψd，转艏角速度rd，最终速度ud，位置坐标xd,yd)作为制导信息代入到控制器中，控制实船收敛到动态虚拟小船的位置上。根据动态虚拟小船的运动学公式(1)和当前状态变量信息计算下一时间点的动态虚拟小船的位置信息。

(7)测量实船位置判断是否到达目的地，如果“是”，结束船舶航行；如果“否”，时间更新t＝t+1并进入步骤(2)。

实施例：为了验证本发明所提出制导算法的有效性，这部分将以大连海事大学科研实习船“育鲲”轮为被控对象(见图10)，开展系统仿真实验进行算法测试。表1给出了“育鲲”轮主要参数，其数学模型采用式(21)所示的三自由度非线性机理模型，其水动力系数基于相关实船操纵性试验数据进行系统辨识获取，关于模型辨识建模的理论成果已在国际期刊isatransactions公开发表，在此不做赘述。

表1.“育鲲”轮主要参数

其中，高阶流体动力项表达式为

该实验中，计划航线由5个航路点w1(200,0),w2(200,600),w3(800,900),w4(800,1500),w5(1400,1800)确定。考虑“育鲲”轮穿过渔船作业区的情况，渔船在作业过程中可以视为静止或慢时变的目标，随机在计划航线附近生成多个障碍物。船舶初始状态为[x,y,ψ,u,v,r]t＝0＝[0m,0m,90deg,0m/s,0m/s,0deg/s]，制导算法参数设置为：rtest＝130m,ractive＝100m,rs＝10m,ε＝0.6,rc＝30m,udpmax＝20m/s,udo＝4m/s,ldbset＝80m,ldrset＝100m,kl＝0.2,τ＝0.1。为了更加贴近真实环境，实验中考虑了风、浪、流对控制系统的扰动，相关干扰机理模型参见文献[10]。控制算法采用文献[4]提出的鲁棒神经自适应控制律，这一应用体现了本发明与先进控制算法的良好结合。

仿真实验所使用环境干扰为：风速(蒲福风7级)vwind＝15.25m/s，风向ψwind＝50deg；海浪干扰由风干扰模型耦合产生，即为在蒲福风7级情况下充分成长生成的海浪，图11给出了试验海浪干扰的三维视图；海流vcurrent＝0.5m/s，流向βcurrent＝280deg。图12-14给出了在上述实验条件下，利用动态虚拟小船dvs路径跟踪避障制导算法得到的结果。从图12可以看出，在导引虚拟小船gvs进行直线和曲线路径规划的基础上，动态虚拟小船dvs能够有效跟踪gvs轨迹并灵活地进行多障碍物的避障，导引船舶实现有效的航迹保持及避障控制。图13和图14分别给出了船舶运动姿态变量u,v,ψ和控制输入n,δ的时间变化曲线，可以看出，船舶姿态变化平稳，基于该制导算法的控制输入符合航海实践中执行装置的要求，通过推进装置与舵机的配合，可实现精确的路径跟踪及避障控制。

文献1：张显库.船舶运动简捷鲁棒自适应控制.北京:科学出版社,2012.

文献2：tangp,zhangr,liud,etal.localreactiveobstacleavoidanceapproachforhigh-speedunmannedsurfacevehicle[j].oceanengineering,2015,106:128-140.

文献3：fossenti,breivikm,andskjetner.lineofsightpathfollowingofunderactuatedmarinecraft[c]//ifacconferenceonmanoeuveringandcontrolofmarinecraft.2003:244-249.

文献4：guoqingzhang,xiankuzhang.anoveldvsguidanceprincipleandrobustadaptivepath-followingcontrolforunderactuatedshipsusinglowfrequencygain-learning[j].isatransactions,2015,56:75-85.

文献5：kuwatay,wolfmt,zarzhitskyd,etal.safemaritimeautonomousnavigationwithcolregs,usingvelocityobstacles[j].ieeejournalofoceanicengineering,2014,39:110-119.

文献6：koreny,borensteinj.potentialfieldmethodsandtheirinherentlimitationsformobilerobotnavigation[c]//ieeeinternationalconferenceonroboticsandautomation.1991:1398-1404.

文献7：moes,pettersenky.set-basedline-of-sight(los)pathfollowingwithcollisionavoidanceforunderactuatedunmannedsurfacevessel[c]//mediterraneanconferenceoncontrolandautomation.2016.

文献8：zereike,sorbaraa,bibulim,etal.prioritytaskapproachforusvs’pathfollowingmissionswithobstaclesavoidanceandspeedregulation[c]//ifacconferenceonmanouveringandcontrolofmarinecraft.2015,48:25-30.

文献9：soltanra,ashrafiuonh,muskekr.ode-basedobstacleavoidanceandtrajectoryplanningforunmannedsurfacevessels[j].robotica,2011,29(5):691-703.

文献10：贾欣乐,杨盐生.船舶运动数学模型.大连:大连海事大学出版社,1999。