CN109597417A - 一种基于避碰准则的多usv群体协同避碰规划方法 - Google Patents

Abstract

本发明属于USV控制技术领域，具体涉及一种基于避碰准则的多USV群体协同避碰规划方法。该方法包括：步骤1、制定合理的USV避碰规则；步骤2、多USV系统建模，计算运动参数和碰撞危险度；步骤3、构建USV避碰规划仿真软件平台，添加雷达探测模块和遗传算法，设计典型的仿真案例验证算法的有效性。本发明使多个USV从起点出发躲避环境中所有静态障碍物到达终点，在整个航行过程中USV之间不发生碰撞且在相遇时遵守避碰准则采取避碰策略，同时避免出现大角度转向、紧急加减速的情况。本发明致力找到严格遵守避碰准则的多USV避碰规划方法，并且解决航行过程的大角度转向、大范围加减速的不良航行问题。实现路径最短、符合经济性、平滑性、安全性的最优避碰。

Description

一种基于避碰准则的多USV群体协同避碰规划方法

技术领域

本发明属于USV控制技术领域，具体涉及一种基于避碰准则的多USV群体协同避碰规划方法。

背景技术

随着无人水面艇USV技术的发展，单一USV的执行任务的能力、航行的鲁棒性、安全性等性能都有很大提升。然而USV经常被用来在严峻复杂的海洋环境中执行各种特殊的、不适合有人平台完成的军事活动、海事监管、海洋环境监测等任务，对于这样复杂、动态性高的任务，单一USV则难以胜任。针对这样的情况，多USV群体协同作业能最大限度地发挥整体优势，有效弥补单一USV难以完成的任务领域。另一方面，USV航行环境不仅包含海岛、礁石等静态障碍物，还受海浪、海流等海况和运动的船舶等动态障碍物的影响，在复杂动态的海洋环境里多USV能安全航行作业的重要前提就是实现避碰功能。目前，国际上并没有官方针对USV制定避碰规则，只是参考《国际海上避碰规则公约》提出各种避碰方法，国内外对考虑避碰准则的多USV避碰规划技术的研究较少，具有代表性如文献“刘佳男.基于进化遗传算法的无人艇避碰系统研究[D].大连：大连海事大学硕士学位论文,2015.”，提出的 USV避碰方法计算分布静动态障碍物的航行环境的USV运动参数及碰撞可能性，将其和经济性、安全性作为适度函数因子建立迭代过程得出避碰路径。文献“茅云生、宋利飞、向祖权等.水面无人艇多船障碍智能避碰[J].大连海事大学学报,2015,41(4):8-13.”提出基于避碰紧迫度、安全性预测以及调整范围的模糊规划方法，建立障碍船的偏心膨胀运动模型对追越、対遇情况进行避碰规划。文献“尚明栋、朱志宇、周涛等.水面无人艇动态避碰策略研究[J].舰船科学技术,2017,39(9):69-73.”提出基于USV操纵运动模型的避碰方法，根据海事规则将会遇分为対遇、交叉和追越三种，在MMG模型基础上对航速和航向进行规划实现避碰。

现有技术的缺陷：现有的考虑避碰准则的多USV避碰方法对动态目标会遇局势划分简单粗略，不足以详细描述各种会遇情况，并且针对各种会遇局势没有给出明确可行的避碰策略，文献“茅云生、宋利飞、向祖权等.水面无人艇多船障碍智能避碰[J].大连海事大学学报,2015,41 (4):8-13.”和文献“尚明栋、朱志宇、周涛等.水面无人艇动态避碰策略研究[J].舰船科学技术,2017,39(9):69-73.”只将会遇分为対遇、追越、交叉三种，每种会遇局势包含角度过大使得制定的避碰策略不能保证该会遇的角度范围内所有角度相遇都能实现最优避碰。文献“茅云生、宋利飞、向祖权等.水面无人艇多船障碍智能避碰[J].大连海事大学学报,2015,41(4):8-13.”将会遇划分很详细，然而制定的各种避碰措施未考虑实际航行的操作性，设计的仿真案例非常简单，并不足以验证划分的会遇局势与制定的避碰策略的可行性与合理性。

发明内容

本发明的目的在于提供一种基于避碰准则的多USV群体协同避碰规划方法。

一种基于避碰准则的多USV群体协同避碰规划方法，该方法包括以下步骤：

步骤1：根据《国际海上避碰规则》制定USV避碰规则，进而划分多USV会遇局势并针对各种会遇局势设计避碰策略；

步骤2：多USV系统建模，计算运动参数和碰撞危险度，判断是否需要采取避碰措施及采取避碰策略的种类和时间；

步骤3：构建USV避碰规划仿真软件平台，添加雷达探测模块和遗传算法。

步骤1所述的根据《国际海上避碰规则》制定USV避碰规则，主要参考《国际海上避碰规则》第7、第8、第13-15规则，并做出相应修改，规则如下：

规则7：每个USV都应该根据所处海洋环境判断是否存在碰撞危险来决定是否采取避碰措施。如果不确定是否存在碰撞危险，则认定存在碰撞危险来采取相应避碰措施；

规则8：每个USV根据具体情况尽可能早地采取积极的避碰措施；

规则13：任何USV追越其他USV时均应该给被追越USV让路；

规则14：当两个USV构成相反或者近似相反的航向上的対遇局势时，形成的対遇碰撞危险应采取各自向右转向从对方左船舷通过的避碰措施；

规则15：当两个USV构成一定角度的交叉相遇局面时，形成的交叉相遇碰撞危险应采取本USV右船舷侧的其他USV让路的避碰措施，如果条件允许应避免横穿它船前方。

步骤2所述的多USV系统建模过程为：船体随动坐标系o-xy取USV重心为原点o，取船体上与o在同一水平面指向船艏的方向为o-x轴正方向，取船体上与o和o-x轴在同一水平面指向右舷的方向为o-y轴正方向；雷达随动坐标系o₀-x₀y₀取雷达重心为原点o₀，取雷达上与o₀在同一水平面指向船艏的方向为o₀-x₀轴正方向，取雷达上与o₀和o₀-x₀轴在同一水平面指向右舷的方向为o₀-y₀轴正方向；

设为大地惯性坐标系坐标、是船体随动坐标系坐标、是船体随动坐标系坐标、为雷达重心安装偏差，则坐标系间相互转换关系为：

式中，

设(V_x,V_y)为USV在大地惯性坐标系中O-X轴、O-Y轴速度分量、x(t)和y(t)为t时刻O-X和O-Y轴方向的位移量、x(t₀)和y(t₀)为初始t₀时刻位置，(v_x,v_y)为USV在船体随动坐标系o-x轴、o-y轴的速度分量，则有：

如果假设USV的初始位置x(t₀)＝0和y(t₀)＝0、α为艏向角加速度，则有：

合并得USV运动模型：

当两USV相遇时，取本USV雷达探测到障碍物后的2个连续时刻的探测到的障碍物信息，其中第i时刻雷达探测范围内探测出有障碍物的扫描点集合为： {(X_i1，Y_i1)，(X_i2，Y_i2)，…，(X_in，Y_in)}。其中，探测出的扫描点集合的坐标为大地环境坐标系下的坐标值。由障碍物的扫描点集合可估算出障碍物的中心位置：

其中，为第i时刻障碍物中心估计位置横坐标，为第i时刻障碍物中心估计位置纵坐标，X_ij为第i时刻雷达扫描出的第j障碍物扫描点的横坐标，Y_ij为第i时刻雷达扫描出的第j障碍物扫描点的横坐标；

相邻时刻障碍物估算位置之间的距离：

若则判定障碍物为静态障碍物；若则判定障碍物为动态障碍物；综合考虑USV航行情况；当障碍物为动态USV时，可以根据两次障碍物中心估算位置的变化和2个时刻的时间间隔估算出动态目标的运动速度和航向：

其中，Δt为两个连续时刻的时间间隔，且角度β_n表示为：

另一USV速度在横纵方向上的分量为：

同理已知本USV的速度和艏向β可计算出横方向上的分量和纵方向上的分量那么两USV的相对速度为：

USV之间的相对速度大小：

USV之间的最近会遇距离DCPA：

其中，DCPA的符号由USV之间的相对位置决定，当另一USV位于本USV的船艏方向时，DCPA为正；当另一USV位于本USV的船尾方向时，DCPA为负；

到达最近会遇地点的时间TCPA：

其中，TCPA的符号由USV之间的相对位置决定，当USV未到达最近相遇地点时，TCPA为正；当USV超过最近相遇地点时，TCPA为负；

以船舶避碰系统中碰撞危险度计算方法为参考，综合DCPA和TCPA加权方法、人工神经网络法和模糊集方法，结合USV的实际航行情况与航行环境，制定适应于USV避碰规划的碰撞危险度计算方法，并且将其分类为空间和时间两方面碰撞危险度；影响空间碰撞危险度大小的因素主要包括USV间的最近会遇距离DCPA、最近会遇地点相对本USV的距离和方向、另一USV所在方向和距离以及USV间的最低安全会遇距离；

其中，DCPA为USV间的最近会遇距离，u为DCPA的隶属函数值，d₁为本USV最晚进行避碰时到另一USV的距离，d₂为恰好构成碰撞紧迫局面时本USV与另一USV的临界距离。当d₂＜|DCPA|时，本USV航行安全；当|DCPA|＜d₁时，本USV与另一USV存在碰撞危险，需要采取避碰措施来消除；当d₁＜|DCPA|＜d₂时，不确定是否存在碰撞危险，需要计算碰撞危险度来判定；

影响时间碰撞危险度大小的因素主要包括两个USV的航行速度、本USV与另一USV的距离、两个USV的长度；

其中，

综合空间碰撞危险度和时间碰撞危险度得出USV系统的碰撞危险度：

其中，的含义为：若u_D＝0或u_T＝0，则u＝0；若u_D≠0且u_T≠0，则u＝max(u_D,u_T)。

步骤3所述添加雷达探测模块和遗传算法，遗传算法的选择操作采取轮盘赌选择法，交叉操作采取离散交叉法，变异操作采取高斯变异法。

一种基于避碰准则的多USV群体协同避碰规划方法选择遗传算法为核心规划算法，速度变化量和艏向变化量为个体变量，利用浮点数编码方式对其编码，经迭代后得到每一时刻最优的速度改变量和艏向变化量，由其组成一条USV的最优路径，模拟搜索到障碍物与其他 USV的状态，那么第j个USV避碰规划的评价函数取为：

式中，f_j为评价函数值，其值衡为正，f_j值越大被选择成为子代的可能性越大，ω_i+1为 USV下一时刻采取的第i个体艏向角，d为当前位置到终点的距离，评价函数采取奖惩机制增大区分度，当下一时刻的艏向角在障碍物边界上下限角范围内或者另一USV边界上下限角范围内，继续航行会接近障碍物或另一USV，此时对这样个体的评价函数值缩小100倍使其被选择的概率减小；当下一时刻的艏向角不在障碍物边界上下限角范围内和另一USV边界上下限角范围内，继续航行能避开障碍物，此时对这样个体的评价函数值扩大100倍使其被选择的概率增大，其中到USV终点的距离d会对路径的长度进行约束，其他的平滑性指标、安全性指标、可行性指标在种群初始化阶段和程序判定阶段已充分考虑；

那么同理其他的USV该时刻的评价函数值也能求出为f₁、f₂…f_j…f_n，那么再取整体的评价函数为：其中每个USV的评价函数f_j保证每个USV此时刻输出速度改变量和艏向变化量的为最优，而整体的评价函数F保证整体所有USV输出的速度改变量和艏向变化量为最优。

本发明的有益效果是：

本发明使多个USV从起点出发躲避环境中所有静态障碍物到达终点，在整个航行过程中 USV之间不发生碰撞且在相遇时遵守避碰准则采取避碰策略，同时避免出现大角度转向、紧急加减速的情况。本发明致力找到严格遵守避碰准则的多USV避碰规划方法，并且解决航行过程的大角度转向、大范围加减速的不良航行问题。本发明的关键点：避碰准则的制定和会遇局面的准确划分；各种会遇局面避碰策略的合理设计；碰撞危险度计算方法的设计。

附图说明

图1是USV间会遇局势图。

图2是USV系统坐标系。

图3是USV与障碍物运动参数图。

图4是雷达搜索图。

图5是多USV任务无通信式避碰规划仿真效果图。

图6是多USV任务无通信式避碰规划速度变化趋势图。

图7是多USV任务无通信式避碰规划艏向变化趋势图。

图8是多USV任务无通信式避碰规划三艇相对距离变化趋势图。

图9(a)为多USV任务无通信式避碰规划大角度交叉相遇避碰前示意图。

图9(b)为多USV任务无通信式避碰规划大角度交叉相遇避碰后示意图。

图10(a)多USV任务无通信式避碰规划対遇避碰前示意图。

图10(b)多USV任务无通信式避碰规划対遇避碰后示意图。

图11(a)多USV任务无通信式避碰规划追越避碰前示意图。

图11(b)多USV任务无通信式避碰规划追越避碰后示意图。

图12是多USV任务无通信式避碰规划遗传算法迭代优化图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明的目的是提供充分考虑避碰准则的多USV群体协同避碰规划方法，以使USV从起点出发驶向终点的过程中避开环境中所有静动态障碍物，USV间相遇时能严格遵守避碰准则且不出现大角度转向、大范围加减速的情况下实现路径最短、符合经济性、平滑性、安全性的最优避碰。

本发明为解决上述技术问题采取的技术方案是：

多USV群体协同避碰规划方法中采用几何环境模型建模描述USV航行环境、USV实时位置等，环境地图赋值法表示存在障碍物和其他USV。

图4中o₀-x₀y₀为本USV雷达随动坐标系，圆表示雷达检测的范围。其中A为障碍物，o₁为另一USV的雷达坐标系原点，d₀为雷达坐标系的原点到障碍物的最短距离，d₁表示本USV到另一USV的最短距离，d表示USV当前位置到对应终点的距离。μ为障碍物边界下限角(雷达坐标系原点与雷达检测到的障碍物下边界点的夹角)，ν为障碍物边界上限角 (雷达坐标系原点与雷达检测到的障碍物上边界点的夹角)。δ为另一USV边界下限角(雷达坐标系原点与雷达检测到的另一USV下边界点的夹角)，γ为另一USV边界上限角(雷达坐标系原点与雷达检测到的另一USV上边界点的夹角)。

多USV群体协同避碰规划方法中采用几何环境模型建模来描述USV航行环境、USV实时位置等，环境地图赋值法表示存在障碍物和其他USV。

所述多USV群体协同避碰规划方法的实现过程为：

步骤一、参照《国际海上避碰规则》制定合理的USV避碰规则，进而科学的划分多USV 会遇局势并针对各种会遇局势设计避碰策略。

步骤二、多USV系统建模，计算运动参数和碰撞危险度，判断是否需要采取避碰措施及采取避碰策略的种类和时间。

步骤三、利用QT软件构建USV避碰规划仿真软件平台，添加雷达探测模块和遗传算法，设计典型的仿真案例验证算法的有效性。

在《国际海上避碰规则》中，主要参考第7、第8、第13-15规则，并做出相应修改，如下：

规则7：每个USV都应该根据所处海洋环境判断是否存在碰撞危险来决定是否采取避碰措施。如果不确定是否存在碰撞危险，则认定存在碰撞危险来采取相应避碰措施。

规则8：每个USV根据具体情况尽可能早地采取积极的避碰措施。

规则13：任何USV追越其他USV时均应该给被追越USV让路。

规则14：当两个USV构成相反或者近似相反的航向上的対遇局势时，形成的対遇碰撞危险应采取各自向右转向从对方左船舷通过的避碰措施。

规则15：当两个USV构成一定角度的交叉相遇局面时，形成的交叉相遇碰撞危险应采取本USV右船舷侧的其他USV让路的避碰措施，如果条件允许应避免横穿它船前方。

根据USV的航行情况以及另一USV驶来的方位，将会遇局势划分为対遇(G)、右船舷小角度交叉相遇(A)、右船舷大角度交叉相遇(B)、左船舷小角度交叉相遇(F)、左船舷大角度交叉相遇(E)、它船右船舷追越(C)、它船左船舷追越(D)，如图1所示：

A：右船舷小角度交叉相遇；

当其他USV从本USV左船舷5°～112.5°或右船舷5°～112.5°的两个扇形方向驶来时，判断为会发生交叉相遇，将交叉相遇情况再进行细分为左右船舷小角度交叉相遇和左右船舷小角度交叉相遇。如图1中A区域，当其他USV从本USV右船舷5°～67.5°的扇形方向驶来时，断定为右船舷小角度交叉相遇，交叉相遇的双方需各自向右转向进行避让。

B:右船舷大角度交叉相遇；

如图1中B区域，当其他USV从本USV右船舷67.5°～112.5°的扇形方向驶来时，断定为右船舷大角度交叉相遇，当其他USV从本USV右船舷90°的方向驶来时为垂直交叉相遇。在此区域中相互右转会产生大幅度转弯，难以实现且不符合经济性，所以这种情况本USV或其他USV采取左转避让。

C、D：它船右船舷追越和它船左船舷追越；

如图1中C区域和D区域，当其他USV从本USV右船舷112.5°～180°或左船舷 112.5°～180°的两个扇形方向驶来存在追赶趋势时，并且驶来的无人航行器智能看到本USV 的尾，断定为它船右船舷追越和它船左船舷追越。在这种情况下，本USV为被追越船，驶来的USV为追越船，在两个USV没有形成紧迫局面前保持原来航行状态，存在紧迫局面时根据两个USV各自的方位来采取不同的转向避让。

E、F：左船舷大角度交叉相遇和左船舷小角度交叉相遇；

如图1的E和F区域，当其他USV从本USV左船舷5°～67.5°或左船舷67.5°～112.5°的两个扇形方向驶来时，断定为左船舷小角度交叉相遇和左船舷大角度交叉相遇。在这种情况下，只有在其他USV与本USV形成紧迫局面而其他USV未采取避碰措施时才右转避让。

G：对遇；

如图1的G区域，当其他USV从本USV左船舷5°到右船舷5°的扇形方向驶来时，断定为对遇，各自向右转弯从对方左船舷驶过。

多USV系统建模过程为：

如图2，大地惯性坐标系O-XY取空间内的任意一点O为原点，取空间中与O在同一水平面的正北方向为O-X轴正方向，取空间中与O和O-X轴在同一水平面指向正东方向为 O-Y轴正方向。船体随动坐标系o-xy取USV重心为原点o，取船体上与o在同一水平面指向船艏的方向为o-x轴正方向，取船体上与o和o-x轴在同一水平面指向右舷-的方向为 o-y轴正方向。雷达随动坐标系o₀-x₀y₀取雷达重心为原点o₀，取雷达上与o₀在同一水平面指向船艏的方向为o₀-x₀轴正方向，取雷达上与o₀和o₀-x₀轴在同一水平面指向右舷的方向为o₀-y₀轴正方向。

设为大地惯性坐标系坐标、是船体随动坐标系坐标、是船体随动坐标系坐标、为雷达重心安装偏差，则坐标系间相互转换关系为：

式中，

设(V_x,V_y)为USV在大地惯性坐标系中O-X轴、O-Y轴速度分量、x(t)和y(t)为t时刻O-X和O-Y轴方向的位移量、x(t₀)和y(t₀)为初始t₀时刻位置，(v_x,v_y)为USV在船体随动坐标系o-x轴、o-y轴的速度分量，则有：

如果假设USV的初始位置x(t₀)＝0和y(t₀)＝0、α为艏向角加速度，则有：

合并得USV运动模型：

如图3，当两USV相遇时，取本USV雷达探测到障碍物后的2个连续时刻的探测到的障碍物信息，其中第i时刻雷达探测范围内探测出有障碍物的扫描点集合为： {(X_i1，Y_i1)，(X_i2，Y_i2)，…，(X_in，Y_in)}。其中，探测出的扫描点集合的坐标为大地环境坐标系下的坐标值。由障碍物的扫描点集合可估算出障碍物的中心位置：

其中，为第i时刻障碍物中心估计位置横坐标，为第i时刻障碍物中心估计位置纵坐标，X_ij为第i时刻雷达扫描出的第j障碍物扫描点的横坐标，Y_ij为第i时刻雷达扫描出的第j障碍物扫描点的横坐标。

相邻时刻障碍物估算位置之间的距离：

这里设置一个阈值，如果则判定障碍物为静态障碍物；如果则判定障碍物为动态障碍物。综合考虑USV航行情况，这里设置阈值为0.1海里。当障碍物为动态USV时，可以根据两次障碍物中心估算位置的变化和2个时刻的时间间隔估算出动态目标的运动速度和航向：

其中，Δt为两个连续时刻的时间间隔。

另一USV速度在横纵方向上的分量为：

同理已知本USV的速度和艏向β可计算出横纵方向上的分量那么两USV 的相对速度为：

USV之间的相对速度大小：

USV之间的最近会遇距离DCPA：

其中，DCPA的符号由USV之间的相对位置决定，当另一USV位于本USV的船艏方向时，DCPA为正；当另一USV位于本USV的船尾方向时，DCPA为负。

到达最近会遇地点的时间TCPA：

其中，TCPA的符号由USV之间的相对位置决定，当USV未到达最近相遇地点时，TCPA为正；当USV超过最近相遇地点时，TCPA为负。

以船舶避碰系统中碰撞危险度计算方法为参考，综合DCPA和TCPA加权方法、人工神经网络法和模糊集方法，结合USV的实际航行情况与航行环境，制定适应于USV避碰规划的碰撞危险度计算方法，并且将其分类为空间和时间两方面碰撞危险度。影响空间碰撞危险度大小的因素主要包括USV间的最近会遇距离DCPA、最近会遇地点相对本USV的距离和方向、另一USV所在方向和距离以及USV间的最低安全会遇距离。

其中，DCPA为USV间的最近会遇距离，u为DCPA的隶属函数值，d₁为本USV最晚进行避碰时到另一USV的距离，d₂为恰好构成碰撞紧迫局面时本USV与另一USV的临界距离。当d₂＜|DCPA|时，本USV航行安全；当|DCPA|＜d₁时，本USV与另一USV存在碰撞危险，需要采取避碰措施来消除；当d₁＜|DCPA|＜d₂时，不确定是否存在碰撞危险，需要计算碰撞危险度来判定。

则d₁＝1.5ρ(θ_T)、d₂＝2d₁。

影响时间碰撞危险度大小的因素主要包括两个USV的航行速度、本USV与另一USV的距离、两个USV的长度。

其中，

综合空间碰撞危险度和时间碰撞危险度得出USV系统的碰撞危险度：

其中，的含义为：

若u_D＝0或u_T＝0，则u＝0；

若u_D≠0且u_T≠0，则u＝max(u_D,u_T)。

步骤三中，选择遗传算法为核心规划算法，设置种群容量为50，速度变化量和艏向变化量为个体变量，利用浮点数编码方式对其编码。经迭代后得到每一时刻最优的速度改变量和艏向变化量，由其组成一条USV的最优路径。

如图4的USV雷达搜索图，其模拟搜索到障碍物与其他USV的状态，那么第j个USV避碰规划的评价函数取为：

式中，f_j为评价函数值，其值衡为正，f_j值越大被选择成为子代的可能性越大。ω_i+1为 USV下一时刻采取的第i个体艏向角，d为当前位置到终点的距离。评价函数采取奖惩机制增大区分度，当下一时刻的艏向角在障碍物边界上下限角范围内或者另一USV边界上下限角范围内，继续航行会接近障碍物或另一USV，此时对这样个体的评价函数值缩小100倍使其被选择的概率减小；当下一时刻的艏向角不在障碍物边界上下限角范围内和另一USV边界上下限角范围内，继续航行能避开障碍物，此时对这样个体的评价函数值扩大100倍使其被选择的概率增大。其中到USV终点的距离d会对路径的长度进行约束，其他的平滑性指标、安全性指标、可行性指标在种群初始化阶段和程序判定阶段已充分考虑。

那么同理其他的USV该时刻的评价函数值也能求出为f₁、f₂…f_j…f_n，那么再取整体的评价函数为：其中每个USV的评价函数f_j保证每个USV此时刻输出速度改变量和艏向变化量的为最优，而整体的评价函数F保证整体所有USV输出的速度改变量和艏向变化量为最优，以解决单一USV算法应用到多USV出现的某一USV最优而其他USV次优的问题。

遗传算法的选择操作采取最为传统的轮盘赌选择法，交叉操作采取最为适合的离散交叉法，变异操作最为适合的高斯变异法。

下面利用QT构建的多USV群体协同避碰规划仿真平台设计典型的仿真案例来验证多 USV群体协同避碰规划方法和避碰算法的有效性和可行性。首先任意设置多USV相应的起点、终点位置，为了更好的实验效果这里选择3-5个USV；再设置航行环境中障碍物的形状、大小、位置，任意选择障碍物的数量形状大小位置参数；最后选择多USV实验的控制模式开始实验。

如图5的多USV任务无通信式避碰规划实验仿真图，实验中USV从起点向终点出发，当USV离障碍物较远时沿直线航行，USV航行到障碍物附近时通过雷达探测到了障碍物信息，当USV与障碍物的距离小于2海里时启动避碰，从两侧绕开障碍物后继续向终点航行。在航行过程中USV与障碍物的距离始终大于最小安全距离，并且USV之间未发生碰撞，避碰效果良好。

如图6的多USV任务式避碰规划速度调节量变化趋势图和图7多USV任务无通信式避碰规划艏向调节量变化趋势图，速度调节量始终在-2节到+2节之间变化，艏向调节量始终在 -15°到+15°之间变化，并无大角度转弯和紧急加减速，效果图中轨迹曲线和速度艏向具有较好的平滑性。如图8多USV任务无通信式避碰规划三艇相对距离变化趋势图显示的航行过程中三个USV的相对距离变化趋势，USV之间并未发生相撞。

如图9(a)和图9(b)的多USV任务无通信式避碰规划仿真中截取出的大角度交叉相遇避碰过程，1号USV和2号USV艇若继续按原速度航向行驶，2号USV对于1号USV艇构成右船舷大角度交叉相遇，此时采取2号USV右转避碰的策略。图10(a)和图10(b) 的多USV任务无通信式避碰规划仿真中截取出的対遇避碰过程，1号USV和3号USv若继续按原速度航行行驶，则构成対遇局势，此时两USV则采取分别左转避碰的策略。如图11 (a)和图11(b)的多USV任务无通信式避碰规划仿真中截取出的大角度交叉相遇避碰过程，1号USV和3号USV艇若继续按原速度航向行驶，3号USV对于1号USV艇构成追越局势，此时采取3号USV降速让1号USV加速从其前方穿过的策略。右船舷大角度交叉相遇、対遇和追越这三种典型会遇局势避让成功证明了避让过程遵循针对USV制定的避碰规则。

如图12多USV任务无通信式避碰规划遗传算法迭代优化图显示的优化过程中种群个体评价函数值的变化趋势，每个USV评价函数值的变化趋势都大致相似，即算法的迭代次数越多，种群个体的评价函数值越大，为最大值优化。保持其他参数不变，改变迭代次数多次试验，测试出迭代次数500最合适。

Claims (5)

1.一种基于避碰准则的多USV群体协同避碰规划方法，其特征在于，该方法包括以下步骤：

步骤1：根据《国际海上避碰规则》制定USV避碰规则，进而划分多USV会遇局势并针对各种会遇局势设计避碰策略；

步骤2：多USV系统建模，计算运动参数和碰撞危险度，判断是否需要采取避碰措施及采取避碰策略的种类和时间；

步骤3：在多USV系统模型中，添加雷达探测模块和遗传方法。

2.根据权利要求1所述的一种基于避碰准则的多USV群体协同避碰规划方法，其特征在于：步骤1所述的根据《国际海上避碰规则》制定USV避碰规则，主要参考《国际海上避碰规则》第7、第8、第13-15规则，并做出相应修改。

3.根据权利要求2所述的一种基于避碰准则的多USV群体协同避碰规划方法，其特征在于，步骤2所述的多USV系统建模过程为：船体随动坐标系o-xy取USV重心为原点o，取船体上与o在同一水平面指向船艏的方向为o-x轴正方向，取船体上与o和o-x轴在同一水平面指向右舷的方向为o-y轴正方向；雷达随动坐标系o₀-x₀y₀取雷达重心为原点o₀，取雷达上与o₀在同一水平面指向船艏的方向为o₀-x₀轴正方向，取雷达上与o₀和o₀-x₀轴在同一水平面指向右舷的方向为o₀-y₀轴正方向；

设为大地惯性坐标系坐标、是船体随动坐标系坐标、是船体随动坐标系初始坐标、为雷达重心安装偏差，则坐标系间相互转换关系为：

式中，

设(V_x,V_y)为USV在大地惯性坐标系中O-X轴、O-Y轴速度分量、x(t)和y(t)为t时刻O-X和O-Y轴方向的位移量、x(t₀)和y(t₀)为初始t₀时刻位置，(v_x,v_y)为USV在船体随动坐标系o-x轴、o-y轴的速度分量，则有：

若USV的初始位置x(t₀)＝0和y(t₀)＝0、α为艏向角加速度，则有：

合并得USV运动模型：

当两USV相遇时，取本USV雷达探测到障碍物后的2个连续时刻的探测到的障碍物信息，其中第i时刻雷达探测范围内探测出有障碍物的扫描点集合为：{(X_i1，Y_i1)，(X_i2，Y_i2)，…，(X_in，Y_in)}，其中，探测出的扫描点集合的坐标为大地环境坐标系下的坐标值，由障碍物的扫描点集合估算出障碍物的中心位置：

其中，为第i时刻障碍物中心估计位置横坐标，为第i时刻障碍物中心估计位置纵坐标，X_ij为第i时刻雷达扫描出的第j障碍物扫描点的横坐标，Y_ij为第i时刻雷达扫描出的第j障碍物扫描点的纵坐标；

相邻时刻障碍物估算位置之间的距离：

若则判定障碍物为静态障碍物；若则判定障碍物为动态障碍物；综合考虑USV航行情况；当障碍物为动态USV时，根据两次障碍物中心估算位置的变化和2个时刻的时间间隔估算出动态目标的运动速度和航向：

其中，Δt为两个连续时刻的时间间隔，且角度β_n表示为：

另一USV速度在横纵方向上的分量为：

同理已知本USV的速度和艏向β计算出横方向上的分量和纵方向上的分量那么两USV的相对速度为：

USV之间的相对速度大小：

USV之间的最近会遇距离DCPA：

其中，DCPA的符号由USV之间的相对位置决定，当另一USV位于本USV的船艏方向时，DCPA为正；当另一USV位于本USV的船尾方向时，DCPA为负；

到达最近会遇地点的时间TCPA：

其中，TCPA的符号由USV之间的相对位置决定，当USV未到达最近相遇地点时，TCPA为正；当USV超过最近相遇地点时，TCPA为负；

以船舶避碰系统中碰撞危险度计算方法为参考，综合DCPA和TCPA加权方法、人工神经网络法和模糊集方法，结合USV的实际航行情况与航行环境，制定适应于USV避碰规划的碰撞危险度计算方法，并且将其分类为空间和时间两方面碰撞危险度；影响空间碰撞危险度大小的因素主要包括USV间的最近会遇距离DCPA、最近会遇地点相对本USV的距离和方向、另一USV所在方向和距离以及USV间的最低安全会遇距离；

其中，DCPA为USV间的最近会遇距离，u为DCPA的隶属函数值，d₁为本USV最晚进行避碰时到另一USV的距离，d₂为恰好构成碰撞紧迫局面时本USV与另一USV的临界距离；当d₂＜|DCPA|时，本USV航行安全；当|DCPA|＜d₁时，本USV与另一USV存在碰撞危险，需要采取避碰措施来消除；当d₁＜|DCPA|＜d₂时，不确定是否存在碰撞危险，需要计算碰撞危险度来判定；

影响时间碰撞危险度大小的因素主要包括两个USV的航行速度、本USV与另一USV的距离、两个USV的长度；

其中，

综合空间碰撞危险度和时间碰撞危险度得出USV系统的碰撞危险度：

其中，的含义为：若u_D＝0或u_T＝0，则u＝0；若u_D≠0且u_T≠0，则u＝max(u_D,uT)。

4.根据权利要求2所述的一种基于避碰准则的多USV群体协同避碰规划方法，其特征在于：步骤3所述添加雷达探测模块和遗传方法，遗传方法的选择操作采取轮盘赌选择法，交叉操作采取离散交叉法，变异操作采取高斯变异法。

5.根据权利要求4所述的一种基于避碰准则的多USV群体协同避碰规划方法，其特征在于：一种基于避碰准则的多USV群体协同避碰规划方法选择遗传方法为核心规划方法，速度变化量和艏向变化量为个体变量，利用浮点数编码方式对其编码，经迭代后得到每一时刻最优的速度改变量和艏向变化量，由其组成一条USV的最优路径，模拟搜索到障碍物与其他USV的状态，那么第j个USV避碰规划的评价函数取为：

式中，f_j为评价函数值，其值衡为正，ω_i+1为USV下一时刻采取的第i个体艏向角，d为当前位置到终点的距离；

那么n个USV该时刻的评价函数值也能求出为f₁、f₂…f_j…f_n，取整体的评价函数为：

其中，每个USV的评价函数f_j保证每个USV此时刻输出速度改变量和艏向变化量的为最优，而整体的评价函数F保证整体所有USV输出的速度改变量和艏向变化量为最优。