CN107168335B - 一种考虑混合多目标避障的水面无人艇路径跟踪制导方法 - Google Patents

Abstract

本发明公开了一种考虑混合多目标避障的水面无人艇路径跟踪制导方法,将制导过程分为路径跟踪和避障操纵制导两种模式，以改进DVS制导算法为基本构架，由GVS动态规划出由直线和曲线组成的光滑参考路径，路径跟踪模式和避障操纵模式分别对应不同的DVS的引导变量。对于多个或混合的障碍物的避障制导，首先将根据优先级排序及避障操纵条件评判确定当前的避障目标，启动避障操纵模式，过渡函数保证DVS引导变量在不同模式切换之间的光滑性。本发明对多种控制策略具有适用性，便于同现有先进控制算法结合，控制算法的作用在于保证实船对DVS的收敛，保证制导策略的有效性。

Description

一种考虑混合多目标避障的水面无人艇路径跟踪制导方法

技术领域

本发明涉及船舶控制工程与船舶自动化航行领域，更具体地，涉及一种考虑混合多目标避障的水面无人艇路径跟踪制导方法。

背景技术

通过制导策略实现船舶的航迹保持及避碰/避障是船舶运动控制的关键技术。船舶路径跟踪的制导算法目前应用较广泛的主要有可视距离(LOS)制导算法和动态虚拟小船(DVS)制导算法。避障算法可以借鉴移动机器人和陆上小车的控制，采用基于非线性控制理论稳定极限环(stable limit cycles)概念的障碍物规避算法，即极限环避障算法，该避障算法具有抖振小、安全性能高、避障轨迹平滑的优点。将上述船舶路径跟踪的制导算法同避障算法相结合的制导策略成为近年来研究的热点。下面就现有的路径跟踪的船舶制导算法同极限环避障算法相结合的制导策略进行简要介绍。

文献[1]中给出了LOS制导算法结合避障算法的制导策略。LOS制导算法的基本原理如图1所示：在前向距离▽固定的前提下，LOS制导算法根据实船位置与直线参考路径上的点A_F之间的几何关系获得每一采样时间点的路径跟踪期望艏摇角ψ_los，前进速度u_p保持不变，进而引导船舶实现对直线参考路径的跟踪。该制导的策略能够使实船指数收敛到参考直线路径上。

图1中，P_i-1P_i为实船当前跟踪的直线路径，d为实船到路径的垂线距离，(x,y)为实船的当前位置坐标，则可以得到：

考虑实船的欠驱动特性，在期望艏向角中引入漂角的补偿，则可以得到路径跟踪期望艏向角：

当船舶进入航路点P_i的转向边界环(circle of acceptance)时，船舶的跟踪路径切换为P_iP_i+1。

从上述推导可以看出：LOS制导算法不具备转向点处的路径规划功能，如果在转向点处遇到障碍物会出现制导算法的失效。同时，该算法还不满足“一切参考路径均可以由虚拟小船产生的假设”，难以同路径跟踪控制的相关研究结果结合。

当单船会遇时，通过虚拟定义在会遇船舶上的探测环感知其相对于实船的位置。在路径跟踪模式下，若避障操纵条件符合，则实船由路径跟踪模式切换到避障操纵模式，前进运动由u_p加速到避障速度u_oa，且按照避障期望艏向角ψ_oa的导引航行。避障期望艏向角可以导引实船收敛到会遇船舶半径小于探测半径的稳定安全极限环上，确保了实船和会遇船舶之间的安全距离。该避障策略的参数说明如图2所示,其中，G表示参考路径。

如图2所示，探测环半径为R_m，安全极限环半径为R_o，当实船进入到探测环内部时，若实船到来船距离σ满足如下避障操纵条件则启动避障操纵模式。

由图2所示的避障策略几何关系可知：

避障操纵模式下，避障期望艏向角ψ_oa选取如下：

式(5)中，误差e＝σ-R_o；Δ为避障操纵模式下手动设定的前向距离(Lookingahead distance)；λ根据COLREGs的要求选取为±1，+1表示以顺时针的方向环绕安全极限环，-1表示以逆时针的方向环绕；k用于补偿会遇船舶运动对艏向角ψ_oa造成的影响，定义为：

其中，

该避障策略为了保证避障过程的快速性，避障速度一定满足u_oa≥u_c≥V₀。

在控制算法能够保证实船实际艏向角和实际前进速度对期望艏向角和前进速度的有效收敛前提条件下，本制导策略根据当前障碍物的位置及避障操纵条件，在路径跟踪模式和避障操纵模式之间来回切换。当两船会遇时，首先执行避碰任务，避障期望艏摇角的选取策略会保证实船收敛到来船的安全极限环上，当避障操纵条件不满足时，实船从新回到直线参考路径的路径跟踪任务上。与之对应，实船的期望前进速度也会根据任务模式的不同而进行切换。该制导策略的执行过程如流程图3所示。

具备曲线段路径规划能力非常重要。对于路径跟踪控制部分，不仅需要对直线参考路径进行规划，而且还需要在转向点附件进行曲线段路径规划；对于避障控制部分，由于障碍物出现的位置不确定，所以，曲线段路径规划对避障的有效性尤为重要。

另外，船舶避障/避碰行为主要发生在狭窄繁忙水道或渔船作业区等复杂海洋环境下，避障策略应尽可能应对多种避碰/避障条件并具有多适应性，以体现船舶高度智能化航行的需求。

同时，船舶路径跟踪及避障制导的有效性只有通过控制策略得以保证，制导算法应便于同控制算法相结合。

因此，文献[1]中所阐述的LOS制导算法综合避障控制的制导策略存在的缺陷，总结如下：

(1)该制导算法基于LOS路径跟踪制导算法建立，因此也沿袭了LOS制导算法的缺陷。该算法不具备转向点处的路径规划功能，如果在转向点处遇到障碍物会出现制导算法的失效；该算法不满足“一切参考路径均可以由虚拟小船产生的假设”，难以同欠驱动船舶路径跟踪控制相关研究结果结合。

(2)该制导算法不具备对多障碍物避障的功能，仅仅适用于单船会遇避碰的情况，不适用于复杂海洋环境下及高速船的避障控制。另外，该制导算法路径跟踪模式和避障操纵模式的切换过程存在阶跃，切换过程不平滑。

参考文献

[1]S.Moe,K.Y.Pettersen.Set-Based Line-of-Sight(LOS)Path Followingwith Collision Avoidance for Underactuated Unmanned Surface Vessel[C].24^thMediterrean Conference on Control and Automation,2016:402-409.

发明内容

本发明的目的在于克服现有技术存在的上述缺陷，提供一种便于同控制算法相结合且具有曲线段路径规划能力的DVS制导算法，该制导算法不仅克服了LOS制导算法的缺陷，还在一定程度上满足了路径跟踪的时间要求。另外，本发明设置了避碰优先级及相应的艏向角及速度规划策略，实现了船舶对多静止障碍物、多动态障碍物及混合障碍物的有效避障。

为实现上述目的，本发明的技术方案如下：

一种考虑混合多目标避障的水面无人艇路径跟踪制导方法，包括以下步骤：

S1：设定航路点信息W₁,W₂,…,W_n，分别建立导引虚拟小船和动态虚拟小船的运动数学模型；

S2：导引虚拟小船根据制导算法规划出参考路径，并始终沿参考路径行驶，启动路径跟踪制导模式，动态虚拟小船的艏向角时刻等于导引虚拟小船相对于动态虚拟小船的真方位角，并向实船发出制导命令，以引导实船进行路径跟踪，实船不断获取动态虚拟小船的导引变量；

S3：在采样时间点进行采样，判断动态虚拟小船是否进入障碍物探测环，如进入障碍物探测环，进入障碍物排序过程，执行S4；如未进入障碍物探测环，保持路径跟踪制导模式，执行S6；

S4：进入障碍物排序过程：对避障物的优先级进行排序，得到障碍物的优先级序列，选取最高优先级障碍物作为当前避障目标，判断当前避障目标是否满足避障操纵条件，若当前避障目标满足避障操纵条件，则进入对当前障碍物的避障制导模式，即执行S5；若当前避障目标不满足避障操纵条件，选取次优先级的障碍物作为当前避障目标；若所有障碍物都不满足避障操纵条件，则进入路径跟踪模式，执行S6；

S5：进入避障制导模式：在每一采样时间点，判断当前障碍物的避障操纵条件是否满足，如满足避障操纵条件，则启动对当前障碍物的避障制导模式，获取动态虚拟小船的导引变量，引导实船；如不满足避障操纵条件，则从障碍物优先级序列中去除当前障碍物，并执行S4；

S6：根据动态虚拟小船的导引变量和控制策略，实时调整实船的执行器输入，控制实船跟踪动态虚拟小船；

S7：测量实船位置，判断是否到达终点，如果是，结束船舶航行，如果否，则执行S2。

进一步地，S1中，所述导引虚拟小船和动态虚拟小船的运动数学模型的表达式为：

其中，d代表动态虚拟小船，g代表导引虚拟小船，(x,y)为位置坐标，

分别为距离的一阶导数，ψ为艏向角，u为前进速度，r为转首角速度。

进一步地，S4中，所述的障碍物优先级序列的生成方法为：

S41：将障碍物分为静态障碍物和动态障碍物，静态障碍物的优先级高于动态障碍物的优先级；

S42：静态障碍物的优先级与动态虚拟小船到该静态障碍物安全极限环的距离e大小成反比，e最小的障碍物拥有最高的优先级；

S43：动态障碍物的优先级由下列公式确定

其中，F为评价函数，R_m为障碍物探测环的半径，R_o为安全极限环的半径，e为动态虚拟小船到该动态障碍物的安全极限环的距离，

为动态虚拟小船在路径跟踪模式下其与障碍物距离的导数，u_do表示动态虚拟小船的避障速度，为一个常数，其值要大于所有动态障碍物的速度，

为权重参数。

进一步地，S4中，所述避障操纵条件分为对静态障碍物的避障操纵条件和对动态障碍物的避障操纵条件，所述对静态障碍物的避障操纵条件为：动态虚拟小船的艏向在动态虚拟小船到静态障碍物的安全极限环的两条切线之间；所述对动态障碍物的避障操纵条件为下列公式：

其中，σ为动态虚拟小船到动态障碍物的距离，R_o为动态障碍物的极限安全环的半径，R_m为动态障碍物的探测环的半径，

为路径跟踪模式下动态虚拟小船到动态障碍物距离的导数。

进一步地，路径跟踪制导模式和避障制导模式切换时以及不同障碍物的避障制导模式切换时，采用如下时间过渡函数：

其中，α(t)为时间过渡函数，t_c为起始切换时间点，t_s为手动设定的过渡时间。

进一步地，S5所述的避障制导模式中，动态虚拟小船的速度u_d调整为避障速度，艏向角按照下列方法选取：

S51：如当前避障目标为静态障碍物，则

其中，ψ_do为动态虚拟小船在避障制导模式中的艏向角，φ为静态障碍物相对动态虚拟小船的真方位角，Δ为设定的前向距离，λ＝±1决定动态虚拟小船避障时环绕静态障碍物的方向，+1为顺时针方向环绕静态障碍物的安全极限环，-1为逆时针方向环绕静态障碍物的安全极限环；

S52：如当前避障目标为动态障碍物，则

其中，φ为动态障碍物相对动态虚拟小船的真方位角，Δ为设定的前向距离，k为补偿参数，λ＝±1，+1为顺时针环绕动态障碍物的安全极限环，-1为逆时针环绕动态障碍物的安全极限环。

进一步地，S2中，所述动态虚拟小船的导引变量包括艏向角ψ_d和前进速度u_d，艏向角ψ_d的计算公式为：

其中，g代表GVS，d代表DVS，(x,y)为位置坐标；

前进速度u_d的计算公式为：

其中，k_d为用于调整收敛速度快慢的设定参数，l_dg为动态虚拟小船到导引虚拟小船的距离，u_g为导引虚拟小船的前进速度，ψ_g为导引虚拟小船的艏向角，ψ_d为动态虚拟小船的艏向角，l_dbset为实船到动态虚拟小船的距离的上限设定，l_db为实船到动态虚拟小船的距离。

进一步地，S3中，所述障碍物探测环的半径R_m根据障碍物的大小确定，障碍物越大，其值越大。

从上述技术方案可以看出，本发明通过将制导过程分为路径跟踪和避障操纵制导两种模式，以改进DVS制导算法为基本构架，由GVS动态规划出由直线和曲线组成的光滑参考路径，路径跟踪模式和避障操纵模式分别对应不同的DVS的引导变量。对于多个或混合的障碍物的避障制导，首先将根据优先级排序及避障操纵条件评判确定当前的避障目标，启动避障操纵模式，过渡函数保证DVS引导变量在不同模式切换之间的光滑性。本发明对多种控制策略具有适用性，便于同现有先进控制算法结合，控制算法的作用在于保证实船对DVS的收敛，保证制导策略的有效性。

附图说明

图1是现有技术中LOS制导算法的基本原理图；

图2是现有技术中单船会遇的避障策略的示意图；

图3是现有技术中LOS路径跟踪及避障的制导算法的流程图；

图4是本发明的改进的DVS制导算法的基本原理图；

图5是一种多静态目标避障态势的示意图；

图6是一种多动态目标避障态势的示意图；

图7是一种混合多目标避障态势的示意图；

图8是本发明的混合多目标避障态势中的参数示意图；

图9是本发明的考虑混合多目标避障的水面无人艇路径跟踪制导方法的流程图；

图10是大连海事大学教学实习船“育鲲”轮的示意图；

图11是蒲福风6级海况下的三维波面示意图；

图12～图15分别是t＝240s,290s,345s,600s时的实船轨迹图；

图16是一具体实施例中的DVS引导变量的时间变化曲线；

图17是一具体实施例中的控制输入一螺距P和舵角δ的时间变化曲线。

具体实施方式

下面结合附图，对本发明的具体实施方式作进一步的详细说明。

需要说明的是，在下述的具体实施方式中，在详述本发明的实施方式时，为了清楚地表示本发明的结构以便于说明，特对附图中的结构不依照一般比例绘图，并进行了局部放大、变形及简化处理，因此，应避免以此作为对本发明的限定来加以理解。

在以下本发明的具体实施方式中，请参阅图9，图9是本发明的考虑混合多目标避障的水面无人艇路径跟踪制导方法的流程图,结合参考图4～图8，对本发明进一步理解。如图9所示，本发明的考虑混合多目标避障的水面无人艇路径跟踪制导方法,包括以下步骤：

S1：设定航路点信息W₁,W₂,…,W_n，分别建立导引虚拟小船和动态虚拟小船的运动数学模型。

S2：导引虚拟小船根据制导算法规划出参考路径，并始终沿参考路径行驶，启动路径跟踪制导模式，动态虚拟小船的艏向角时刻等于导引虚拟小船相对于动态虚拟小船的真方位角，并向实船发出制导命令，以引导实船进行路径跟踪，实船不断获取动态虚拟小船的导引变量。

在上述步骤中，请参阅图4，图4是改进的动态虚拟小船(DVS)的制导算法的原理图。在本制导算法中，导引虚拟小船(GVS)和DVS均为不考虑惯性力和阻尼力的理想小船，仅考虑平面运动，它们分别采取独立的运动数学方程为：

其中，d代表动态虚拟小船，g代表导引虚拟小船，(x,y)为位置坐标，

分别为距离的一阶导数，ψ为艏向角，u为前进速度，r为转首角速度。

GVS根据设定航路点W_i-1,W_i,W_i+1的信息，依据传统DVS制导算法规划出参考路径，即GVS的位置和姿态时间序列。参考路径分为直线参考路径部分和航路点处的曲线参考路径部分，曲线段参考路径通过圆弧的内插法产生，该曲线段参考路径解决了现有技术中在航路点附件不能引导的问题。设定GVS的纵向速度u_r和转艏角速度r_r，满足实船执行器性能，按照公式(7)的运动学关系沿着直线及曲线段参考路径航行。

在传统DVS制导算法中，在实船与GVS之间生成一个DVS作为实船的直接追踪目标，其位置固定在实船和GVS的连线上，DVS的位置受到约束，不适应多目标避障/避碰的制导。为了进一步满足避障/避碰制导的要求，本发明对DVS制导算法进行了改进，即DVS的生成不再受GVS和实船位置的限制，将其视为按照公式(7)所示运动学关系独立航行的目标。在路径跟踪模式下，DVS艏向角时刻ψ_d选取为GVS相对于DVS的真方位角ψ_dp，即式(8)：

其中，g代表GVS，d代表DVS，(x,y)为位置坐标。

为了保证路径跟踪模式下DVS对GVS位置的指数收敛，将路径跟踪DVS速度定义为式(9)，令u_d＝u_dp

其中，k_d为用于调整收敛速度快慢的设定参数，其值越大，u_dp越大，DVS对GVS收敛越快，由此可能造成实船执行器输入越大，所以应根据执行器能力折中选取。l_dbset为实船到DVS距离的上限设定，其值的选取也由船舶的执行器能力确定，l_dg为动态虚拟小船到导引虚拟小船的距离，u_g为导引虚拟小船的前进速度，ψ_g为导引虚拟小船的艏向角，ψ_d为动态虚拟小船的艏向角，l_db为实船到动态虚拟小船的距离。从式(9)可以看出，DVS路径跟踪速度和实船到DVS的距离l_db成线性关系。当l_db＝l_dbset时，速度u_dp＝0，这说明在路径跟踪模式下，DVS不会超出以实船为中心l_dbset为半径的圆范围内。该设定保证了在路径跟踪模式下，不会出现因实船距离DVS位置过远而产生的执行器输入过大，该设计考虑了执行器的饱和特性。

由以上论述可知，改进DVS制导算法中的DVS是独立的，可根据其相对于障碍目标的位置实时调整DVS姿态命令以实现避碰/避障引导。

S3：在采样时间点进行采样，判断动态虚拟小船是否进入障碍物探测环，如进入障碍物探测环，进入障碍物排序过程，执行S4；如未进入障碍物探测环，保持路径跟踪制导模式，执行S6。

S4：进入障碍物排序过程：对避障物的优先级进行排序，得到障碍物的优先级序列，选取最高优先级障碍物作为当前避障目标，判断当前避障目标是否满足避障操纵条件，若当前避障目标满足避障操纵条件，则进入对当前障碍物的避障制导模式，即执行S5；若当前避障目标不满足避障操纵条件，选取次优先级的障碍物作为当前避障目标；若所有障碍物都不满足避障操纵条件，则进入路径跟踪模式，执行S6。

S5：进入避障制导模式：在每一采样时间点，判断当前障碍物的避障操纵条件是否满足，如满足避障操纵条件，则启动对当前障碍物的避障制导模式，获取动态虚拟小船的导引变量，引导实船；如不满足避障操纵条件，则从障碍物优先级序列中去除当前障碍物，并执行S4。

S6：根据动态虚拟小船的导引变量和控制策略，实时调整实船的执行器输入，控制实船跟踪动态虚拟小船。

在上述过程中，首先对DVS是否进入障碍物探测环做一个判断。如果进入，则需由路径跟踪制导模式改为避障制导模式，在避障制导模式中，根据障碍物的种类和个数对障碍物进行避障优先级的排序，依次完成避障任务。

为便于避障优先级的排序，本发明将避障任务分成如图5～图7所示的三种避障态势(图5中的H表示静态障碍物，图6中的I表示动态障碍物)：分别为多静态目标避障态势、多动态目标避障态势和混合多目标避障态势。所谓多目标避障态势是指DVS处于多个障碍物的探测环内。因为控制算法会保证实船快速收敛到DVS上，所以假设DVS拥有障碍物的探测能力是合理的。本发明通过分配障碍物的优先级逐一对多个障碍物进行避障，当对优先级最高的障碍物进行避障时，不考虑其他障碍物。因为本制导策略可以保证DVS快速收敛到障碍物的安全极限环(如图8所示具有半径R_o的实线圆)上，而障碍物之间(诸如多艘船舶之间，船舶与岛礁之间)也存在安全距离，即障碍物的安全极限环之间不会相交，所以当DVS对某个障碍物进行避障时，不会受到其他障碍物的干扰。图8中所示的障碍物探测环的半径R_m和安全极限环的半径R_o根据障碍物的大小确定，障碍物越大，其值越大。实际上，障碍物探测环和安全极限环是实船探测能力的一种体现。本发明规定多静态避障根据图8中所示DVS距离安全极限环的距离e的大小确定静态障碍物的优先级，e最小的障碍物拥有最高的优先级，依次类推；对于多动态避障，则根据评价函数的最大化原则决定优先级，评价函数的公式如下：

其中，F为评价函数，R_m为障碍物探测环的半径，R_o为安全极限环的半径，e为动态虚拟小船到该动态障碍物的安全极限环的距离，

为动态虚拟小船在路径跟踪模式下其与障碍物距离的导数，u_do表示动态虚拟小船的避障速度，为一个常数，其值要大于所有动态障碍物的速度，以保证避障的有效性，同时满足实船执行器性能的要求；

为权重参数，其值越大，则表示在评价函数中DVS与障碍物的距离越重要，DVS对障碍物的相对速度越不重要，反之亦然。对于同时存在静态和动态障碍物的情况，该制导策略规定静态障碍物相比动态具有更高的优先级，在对全部静态障碍物的优先级进行排序后，再对动态障碍物的优先级进行排序。

在确定障碍物优先级顺序之后，则根据避障操纵条件是否满足决定是否启动避障操纵模式。如图8所示，对于静态障碍物，其避障操纵条件为DVS的艏向在切线l₁和l₂之间；对于动态障碍物，其避障操纵条件如下式所示：

其中，σ为动态虚拟小船到动态障碍物的距离，R_o为动态障碍物的极限安全环的半径，R_m为动态障碍物的探测环的半径，

为路径跟踪模式下动态虚拟小船到动态障碍物距离的导数。即，当动态虚拟小船进入动态障碍物的安全极限环内时，说明马上需要避障，否则会发生碰撞；当动态虚拟小船进入探测环内并且路径跟踪模式下动态虚拟小船到动态障碍物距离的导数小于0时，说明有碰撞的危险，需要采取措施避免碰撞。

如果较高优先级的障碍物不满足避障操纵条件，则算法转而寻找下一优先级的避障操纵条件；如果所有障碍物都不满足避障操纵条件，则算法保持路径跟踪模式不变；如果当前优先级障碍物满足避障操纵条件，则启动避障操纵模式。启动避障操纵模式后的每一采样时间点，制导算法不再对障碍物优先级进行排序，只判断当前障碍物的避障操纵条件是否依旧满足，若不满足，则退出避障操纵模式，判断DVS与障碍物的相对位置，若DVS处于障碍物探测环内，则重新开始优先级排序，否则启动路径跟踪模式。

在避障操纵模式下，首先DVS速度u_d会调整为目标避障速度u_do。对静态障碍目标的避障DVS艏向角选取为下式：

其中，ψ_do为动态虚拟小船在避障制导模式中的艏向角，φ为静态障碍物相对动态虚拟小船的真方位角，Δ为设定的前向距离，其值越小，DVS对极限环的收敛速度越快，由此可能造成实船转向幅度较大，所以应当根据实船转向能力灵活选取。λ＝±1决定环绕静态障碍物的方向，+1为顺时针环绕静态障碍物的安全极限环，-1为逆时针环绕静态障碍物的安全极限环。

对动态障碍目标的避障DVS艏向角选取为：

其中，φ为动态障碍物相对动态虚拟小船的真方位角，Δ为设定的前向距离，k为补偿参数，其定义同公式(6)，V₀＝u_ccos(π-φ+θ)，u_c为当前动态障碍物的速度，

λ＝±1，+1为顺时针环绕动态障碍物的安全极限环，-1为逆时针环绕动态障碍物的安全极限环。当动态障碍物为单船时，λ的选取应满足《国际海上避碰规则》(COLREGs)的要求，当动态障碍物为其它，例如漂浮物等，按照以下原则选取：

其中，ψ_dp为导引虚拟小船相对于动态虚拟小船的真方位角，

为动态障碍物相对动态虚拟小船的真方位角。即当导引虚拟小船相对于动态虚拟小船的真方位角小于等于动态障碍物相对动态虚拟小船的真方位角时，则λ＝+1，顺时针环绕动态障碍物；当导引虚拟小船相对于动态虚拟小船的真方位角小于等于动态障碍物相对动态虚拟小船的真方位角时，则λ＝-1，逆时针环绕动态障碍物。式(14)的障碍物环绕方向选取原则保证了对障碍物的最小环绕长度，减小了航程浪费。

为了保证DVS制导变量(u_d,ψ_d)在避障操纵和路径跟踪两种模式切换时以及不同障碍物避障之间的切换时的光滑性，引入如式(15)所示的时间过渡函数：

其中，α(t)为时间过渡函数，t_c为起始切换时间点，t_s为手动设定的过渡时间，其值的选取应在不影响避障安全性能的前提下尽可能地保证过渡平缓。以ψ_d为例，其切换过程如式(16)所示：

ψ_d(t)＝(1-α)ψ_{d_start}+αψ_{d_end} (16)

其中，ψ_d将在t_s时间内完成从切换起始艏向角ψ_{d_start}到目标模式艏向角ψ_{d_end}的过渡，其过渡过程是光滑的。对于u_d而言，因为目标避障速度u_do是人为设定的，且对于所用障碍物都不变，所以u_d的过渡仅有u_dp→u_do,u_do→u_dp两种情况。

S7：测量实船位置，判断是否到达终点，如果是，结束船舶航行，如果否，则执行S2。

综上所述，本发明将制导过程分为路径跟踪和避障操纵制导两种模式，以改进DVS制导算法为基本构架，由GVS动态规划出由直线和曲线组成的光滑参考路径，路径跟踪模式和避障操纵模式分别对应不同的DVS的引导变量。对于多个或混合的障碍物的避障制导，首先将根据优先级排序及避障操纵条件评判确定当前的避障目标，启动避障操纵模式，过渡函数保证DVS引导变量在不同模式切换之间的光滑性。本发明还可以对多种控制策略具有适用性，便于同现有先进控制算法结合，控制算法的作用在于保证实船对DVS的收敛，保证制导策略的有效性。

为了验证本发明所提出制导算法的有效性，这部分将以大连海事大学教学实习船“育鲲”(如图10)为被控对象，利用matlab开展计算机仿真实验。表1给出了“育鲲”轮的主要尺度参数。这里采用三自由度欠驱动数学模型(17)，其相关水动力系数基于2013年9月“育鲲”轮开展的系列实船操纵性试验，利用先进***辨识算法获得。

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

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

其中，P为螺距，δ为舵角，作为控制***的输入。

该实例中，计划航线由4个航路点W₁(200,0)，W₂(200,1000)，W₃(1200,1500)，W₄(1200,2500)确定。在计划航线附近设置多静态障碍物、多动态障碍物以及动静混合障碍物，模拟船舶在穿越多岛礁水域和航运繁忙水域的情况。船舶初始状态为[x,y,ψ,u,v,r]_t＝0＝[0m,0m,90deg,0m/s,0m/s,0deg/s]，制导算法参数设置为：u_g＝10Kn，u_do＝12Kn，l_dbset＝200m，k_d＝0.05，Δ＝20m，

t_s＝2s为了更加贴近真实环境，仿真中考虑了风、浪、流的海洋环境干扰。控制算法鲁棒神经阻尼控制律，这一应用体现了本发明与先进控制算法的良好结合。

仿真实验所使用环境干扰为：风速(蒲福风6级)V_wind＝15.25m/s，风向ψ_wind＝50deg；海浪干扰由风干扰模型耦合产生，即为在蒲福风7级情况下充分成长生成的海浪，图11给出了试验海浪干扰的三维视图；海流V_current＝0.5m/s，流向β_current＝280deg。图12～图15给出了在上述实验条件下，4个时间点采用本发明的制导策略得到的实船轨迹图。可以看出该制导和控制策略保证了实船在避障时有效地收敛到障碍物的安全极限环上，避障结束后快速回归到路径跟踪模式上，单船避障满足COLREGs的要求，多船避障按照优先级顺序执行避障任务。图16给出了制导策略得到的引导变量在整个实验过程中随时间的变化，可以看出制导策略中过渡函数的引入保证了模式切换之间引导变量光滑的过渡。图17给出了实验过程中的控制输入—螺距P和舵角δ，控制输入满足执行器有界的要求，抖振较小，可以直接作为执行器的输入，贴近工程实际。

通过上述仿真实验，并同已有研究进行对比，现将本发明带来的有益效果总结为以下3点：

1)本发明提出了一种适用于存在多静态目标、多动态目标和混合多目标障碍物的复杂航行态势的改进DVS制导技术，适用范围更广。所提出的制导算法创造性地将船舶路径跟踪制导同混合避障制导结合。本策略下的混合避障制导机制具有多静态障碍物避障、多动态障碍物避障和混合多目标障碍物避障的能力，在结束避障任务后，船舶可以迅速回归到路径跟踪任务上，其适用范围更广。

2)基于极限环的避障策略可以确保船舶快速地收敛到障碍物的安全极限环上，安全性能较高。该算法中，通过合理设计避障目标的安全极限环和探测环(由船载设备的测量能力决定，如雷达、摄像机等)可以灵活调整船舶避障操纵的安全域度和避障操纵后恢复航线的收敛速度。实际工程中，避障目标的安全极限环和探测环是实船探测能力的一种体现，实船进入障碍物探测环的过程等效于障碍物进入实船探测范围的过程，故本制导算法便于工程实现。

3)本发明提出的制导策略继承了DVS制导策略的优点，能够在直线段、曲线段航线以及多种船舶航行态势下提供合理有效的制导机制，引导船舶完成智能航行任务；该策略具有控制理论的通用性，为先进控制算法和工程实践的结合搭建了桥梁；该策略考虑了船舶执行装置的限制，符合“绿色、节能”的主题。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (5)

1.一种考虑混合多目标避障的水面无人艇路径跟踪制导方法，其特征在于包括以下步骤：

S1：设定航路点信息W₁,W₂,…,W_n，分别建立导引虚拟小船和动态虚拟小船的运动数学模型；其中，所述导引虚拟小船和动态虚拟小船的运动数学模型的表达式为：

其中，d代表动态虚拟小船，g代表导引虚拟小船，(x,y)为位置坐标，

分别为距离的一阶导数，ψ为艏向角，u为前进速度，r为转艏角速度；

S2：导引虚拟小船根据制导算法规划出参考路径，并始终沿参考路径行驶，启动路径跟踪制导模式，动态虚拟小船的艏向角时刻等于导引虚拟小船相对于动态虚拟小船的真方位角，并向实船发出制导命令，以引导实船进行路径跟踪，实船不断获取动态虚拟小船的导引变量；其中，所述动态虚拟小船的导引变量包括艏向角ψ_d和前进速度u_d，艏向角ψ_d的计算公式为：

其中，d代表动态虚拟小船，g代表导引虚拟小船，(x,y)为位置坐标；

前进速度u_d的计算公式为：

其中，k_d为用于调整收敛速度快慢的设定参数，l_dg为动态虚拟小船到导引虚拟小船的距离，u_g为导引虚拟小船的前进速度，ψ_g为导引虚拟小船的艏向角，ψ_d为动态虚拟小船的艏向角，l_dbset为实船到动态虚拟小船的距离的上限设定，l_db为实船到动态虚拟小船的距离；

S3：在采样时间点进行采样，判断动态虚拟小船是否进入障碍物探测环，如进入障碍物探测环，进入障碍物排序过程，执行S4；如未进入障碍物探测环，保持路径跟踪制导模式，执行S6；

S4：进入障碍物排序过程：对避障物的优先级进行排序，得到障碍物的优先级序列，选取最高优先级障碍物作为当前避障目标，判断当前避障目标是否满足避障操纵条件，若当前避障目标满足避障操纵条件，则进入对当前障碍物的避障制导模式，即执行S5；若当前避障目标不满足避障操纵条件，选取次优先级的障碍物作为当前避障目标；若所有障碍物都不满足避障操纵条件，则进入路径跟踪模式，执行S6；

S5：进入避障制导模式：在每一采样时间点，判断当前障碍物的避障操纵条件是否满足，如满足避障操纵条件，则启动对当前障碍物的避障制导模式，获取动态虚拟小船的导引变量，引导实船；如不满足避障操纵条件，则从障碍物优先级序列中去除当前障碍物，并执行S4；其中，S5所述的避障制导模式中，动态虚拟小船的速度u_d调整为避障速度，艏向角按照下列方法选取：

S51：如当前避障目标为静态障碍物，则

其中，ψ_do为动态虚拟小船在避障制导模式中的艏向角，φ为静态障碍物相对动态虚拟小船的真方位角，e为动态虚拟小船到该静态障碍物的安全极限环的距离，Δ为设定的前向距离，λ＝±1决定动态虚拟小船避障时环绕静态障碍物的方向，+1为顺时针方向环绕静态障碍物的安全极限环，-1为逆时针方向环绕静态障碍物的安全极限环；

S52：如当前避障目标为动态障碍物，则

其中，φ为动态障碍物相对动态虚拟小船的真方位角，Δ为设定的前向距离，e为动态虚拟小船到该动态障碍物的安全极限环的距离，λ＝±1，+1为顺时针环绕动态障碍物的安全极限环，-1为逆时针环绕动态障碍物的安全极限环；补偿参数k用于补偿会遇船舶运动对艏向角造成的影响，其定义为：

其中，

b＝-2eV₀ ²，c＝-(Δ²+e²)V₀ ²且u_do≥u_c≥V₀；

；

u_c为当前动态障碍物的速度；u_do表示动态虚拟小船的避障速度；

S6：根据动态虚拟小船的导引变量和控制策略，实时调整实船的执行器输入，控制实船跟踪动态虚拟小船；

S7：测量实船位置，判断是否到达终点，如果是，结束船舶航行，如果否，则执行S2。

2.根据权利要求1所述的考虑混合多目标避障的水面无人艇路径跟踪制导方法，其特征在于，S4中，所述的障碍物优先级序列的生成方法为：

S41：将障碍物分为静态障碍物和动态障碍物，静态障碍物的优先级高于动态障碍物的优先级；

S42：静态障碍物的优先级与动态虚拟小船到该静态障碍物安全极限环的距离e大小成反比，e最小的障碍物拥有最高的优先级；

S43：动态障碍物的优先级由下列公式确定

其中，F为评价函数，R_m为障碍物探测环的半径，R_o为安全极限环的半径，e为动态虚拟小船到该动态障碍物的安全极限环的距离，

为动态虚拟小船在路径跟踪模式下其与障碍物距离的导数，u_do表示动态虚拟小船的避障速度，为一个常数，其值要大于所有动态障碍物的速度，

为权重参数。

3.根据权利要求1所述的考虑混合多目标避障的水面无人艇路径跟踪制导方法，其特征在于，S4中，所述避障操纵条件分为对静态障碍物的避障操纵条件和对动态障碍物的避障操纵条件，所述对静态障碍物的避障操纵条件为：动态虚拟小船的艏向在动态虚拟小船到静态障碍物的安全极限环的两条切线之间；所述对动态障碍物的避障操纵条件为下列公式：

其中，σ为动态虚拟小船到动态障碍物的距离，R_o为动态障碍物的极限安全环的半径，R_m为动态障碍物的探测环的半径，

为路径跟踪模式下动态虚拟小船到动态障碍物距离的导数。

4.根据权利要求1所述的考虑混合多目标避障的水面无人艇路径跟踪制导方法，其特征在于，路径跟踪制导模式和避障制导模式切换时以及不同障碍物的避障制导模式切换时，采用如下时间过渡函数：

其中，α(t)为时间过渡函数，t_c为起始切换时间点，t_s为手动设定的过渡时间。

5.根据权利要求1所述的考虑混合多目标避障的水面无人艇路径跟踪制导方法，其特征在于，S3中，所述障碍物探测环的半径R_m根据障碍物的大小确定，障碍物越大，其值越大。

Images