CN109597417B - 一种基于避碰准则的多usv群体协同避碰规划方法 - Google Patents
一种基于避碰准则的多usv群体协同避碰规划方法 Download PDFInfo
- Publication number
- CN109597417B CN109597417B CN201910033309.3A CN201910033309A CN109597417B CN 109597417 B CN109597417 B CN 109597417B CN 201910033309 A CN201910033309 A CN 201910033309A CN 109597417 B CN109597417 B CN 109597417B
- Authority
- CN
- China
- Prior art keywords
- usv
- usvs
- collision avoidance
- obstacle
- angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 238000002121 ultrasonic speckle velocimetry Methods 0.000 claims abstract description 195
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 21
- 230000003068 static effect Effects 0.000 claims abstract description 13
- 230000002068 genetic effect Effects 0.000 claims abstract description 12
- 238000001514 detection method Methods 0.000 claims abstract description 8
- LFULEKSKNZEWOE-UHFFFAOYSA-N propanil Chemical compound CCC(=O)NC1=CC=C(Cl)C(Cl)=C1 LFULEKSKNZEWOE-UHFFFAOYSA-N 0.000 claims description 26
- 230000004888 barrier function Effects 0.000 claims description 23
- 238000011156 evaluation Methods 0.000 claims description 23
- 230000008859 change Effects 0.000 claims description 22
- 230000002265 prevention Effects 0.000 claims description 14
- QZXCCPZJCKEPSA-UHFFFAOYSA-N chlorfenac Chemical compound OC(=O)CC1=C(Cl)C=CC(Cl)=C1Cl QZXCCPZJCKEPSA-UHFFFAOYSA-N 0.000 claims description 11
- 230000005484 gravity Effects 0.000 claims description 8
- 238000013459 approach Methods 0.000 claims description 3
- 230000007246 mechanism Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 abstract description 15
- 238000004088 simulation Methods 0.000 abstract description 11
- 230000001133 acceleration Effects 0.000 abstract description 8
- 238000012795 verification Methods 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 14
- 238000004364 calculation method Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 5
- 238000011160 research Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 238000002474 experimental method Methods 0.000 description 4
- 230000001105 regulatory effect Effects 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 238000013528 artificial neural network Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000007667 floating Methods 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000035772 mutation Effects 0.000 description 2
- 238000010187 selection method Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
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避碰规划技术的研究较少,具有代表性如文献“刘佳男.基于进化遗传算法的无人艇避碰系统研究[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轴正方向;雷达随动坐标系o0-x0y0取雷达重心为原点o0,取雷达上与o0在同一水平面指向船艏的方向为o0-x0轴正方向,取雷达上与o0和o0-x0轴在同一水平面指向右舷的方向为o0-y0轴正方向;
设(Vx,Vy)为USV在大地惯性坐标系中O-X轴、O-Y轴速度分量、x(t)和y(t)为t时刻O-X和O-Y轴方向的位移量、x(t0)和y(t0)为初始t0时刻位置,(vx,vy)为USV在船体随动坐标系o-x轴、o-y轴的速度分量,则有:
如果假设USV的初始位置x(t0)=0和y(t0)=0、α为艏向角加速度,则有:
合并得USV运动模型:
当两USV相遇时,取本USV雷达探测到障碍物后的2个连续时刻的探测到的障碍物信息,其中第i时刻雷达探测范围内探测出有障碍物的扫描点集合为:{(Xi1,Yi1),(Xi2,Yi2),…,(Xin,Yin)}。其中,探测出的扫描点集合的坐标为大地环境坐标系下的坐标值。由障碍物的扫描点集合可估算出障碍物的中心位置:
相邻时刻障碍物估算位置之间的距离:
另一USV速度在横纵方向上的分量为:
同理已知本USV的速度vm和艏向βm可计算出横方向上的分量vxm和纵方向上的分量vym,那么两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的隶属函数值,d1为本USV最晚进行避碰时到另一USV的距离,d2为恰好构成碰撞紧迫局面时本USV与另一USV的临界距离。当d2<|DCPA|时,本USV航行安全;当|DCPA|<d1时,本USV与另一USV存在碰撞危险,需要采取避碰措施来消除;当d1<|DCPA|<d2时,不确定是否存在碰撞危险,需要计算碰撞危险度来判定;
影响时间碰撞危险度大小的因素主要包括两个USV的航行速度、本USV与另一USV的距离、两个USV的长度;
综合空间碰撞危险度和时间碰撞危险度得出USV系统的碰撞危险度:
步骤3所述添加雷达探测模块和遗传算法,遗传算法的选择操作采取轮盘赌选择法,交叉操作采取离散交叉法,变异操作采取高斯变异法。
一种基于避碰准则的多USV群体协同避碰规划方法选择遗传算法为核心规划算法,速度变化量和艏向变化量为个体变量,利用浮点数编码方式对其编码,经迭代后得到每一时刻最优的速度改变量和艏向变化量,由其组成一条USV的最优路径,模拟搜索到障碍物与其他USV的状态,那么第j个USV避碰规划的评价函数取为:
式中,fj为评价函数值,其值衡为正,fj值越大被选择成为子代的可能性越大,ωi+1为USV下一时刻采取的第i个体艏向角,d为当前位置到终点的距离,评价函数采取奖惩机制增大区分度,当下一时刻的艏向角在障碍物边界上下限角范围内或者另一USV边界上下限角范围内,继续航行会接近障碍物或另一USV,此时对这样个体的评价函数值缩小100倍使其被选择的概率减小;当下一时刻的艏向角不在障碍物边界上下限角范围内和另一USV边界上下限角范围内,继续航行能避开障碍物,此时对这样个体的评价函数值扩大100倍使其被选择的概率增大,其中到USV终点的距离d会对路径的长度进行约束,其他的平滑性指标、安全性指标、可行性指标在种群初始化阶段和程序判定阶段已充分考虑;
那么同理其他的USV该时刻的评价函数值也能求出为f1、f2…fj…fn,那么再取整体的评价函数为:其中每个USV的评价函数fj保证每个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中o0-x0y0为本USV雷达随动坐标系,圆表示雷达检测的范围。其中A为障碍物,o1为另一USV的雷达坐标系原点,d 0为雷达坐标系的原点到障碍物的最短距离,d1表示本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轴正方向。雷达随动坐标系o0-x0y0取雷达重心为原点o0,取雷达上与o0在同一水平面指向船艏的方向为o0-x0轴正方向,取雷达上与o0和o0-x0轴在同一水平面指向右舷的方向为o0-y0轴正方向。
设(Vx,Vy)为USV在大地惯性坐标系中O-X轴、O-Y轴速度分量、x(t)和y(t)为t时刻O-X和O-Y轴方向的位移量、x(t0)和y(t0)为初始t0时刻位置,(vx,vy)为USV在船体随动坐标系o-x轴、o-y轴的速度分量,则有:
如果假设USV的初始位置x(t0)=0和y(t0)=0、α为艏向角加速度,则有:
合并得USV运动模型:
如图3,当两USV相遇时,取本USV雷达探测到障碍物后的2个连续时刻的探测到的障碍物信息,其中第i时刻雷达探测范围内探测出有障碍物的扫描点集合为:{(Xi1,Yi1),(Xi2,Yi2),…,(Xin,Yin)}。其中,探测出的扫描点集合的坐标为大地环境坐标系下的坐标值。由障碍物的扫描点集合可估算出障碍物的中心位置:
相邻时刻障碍物估算位置之间的距离:
这里设置一个阈值,如果则判定障碍物为静态障碍物;如果则判定障碍物为动态障碍物。综合考虑USV航行情况,这里设置阈值为0.1海里。当障碍物为动态USV时,可以根据两次障碍物中心估算位置的变化和2个时刻的时间间隔估算出动态目标的运动速度和航向:
其中,Δt为两个连续时刻的时间间隔。
另一USV速度在横纵方向上的分量为:
同理已知本USV的速度和艏向vm、βm可计算出横纵方向上的分量vxm、vym,那么两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的隶属函数值,d1为本USV最晚进行避碰时到另一USV的距离,d2为恰好构成碰撞紧迫局面时本USV与另一USV的临界距离。当d2<|DCPA|时,本USV航行安全;当|DCPA|<d1时,本USV与另一USV存在碰撞危险,需要采取避碰措施来消除;当d1<|DCPA|<d2时,不确定是否存在碰撞危险,需要计算碰撞危险度来判定。
则d1=1.5ρ(θT)、d2=2d1。
影响时间碰撞危险度大小的因素主要包括两个USV的航行速度、本USV与另一USV的距离、两个USV的长度。
综合空间碰撞危险度和时间碰撞危险度得出USV系统的碰撞危险度:
若uD=0或uT=0,则u=0;
若uD≠0且uT≠0,则u=max(uD,uT)。
步骤三中,选择遗传算法为核心规划算法,设置种群容量为50,速度变化量和艏向变化量为个体变量,利用浮点数编码方式对其编码。经迭代后得到每一时刻最优的速度改变量和艏向变化量,由其组成一条USV的最优路径。
如图4的USV雷达搜索图,其模拟搜索到障碍物与其他USV的状态,那么第j个USV避碰规划的评价函数取为:
式中,fj为评价函数值,其值衡为正,fj值越大被选择成为子代的可能性越大。ωi+1为USV下一时刻采取的第i个体艏向角,d为当前位置到终点的距离。评价函数采取奖惩机制增大区分度,当下一时刻的艏向角在障碍物边界上下限角范围内或者另一USV边界上下限角范围内,继续航行会接近障碍物或另一USV,此时对这样个体的评价函数值缩小100倍使其被选择的概率减小;当下一时刻的艏向角不在障碍物边界上下限角范围内和另一USV边界上下限角范围内,继续航行能避开障碍物,此时对这样个体的评价函数值扩大100倍使其被选择的概率增大。其中到USV终点的距离d会对路径的长度进行约束,其他的平滑性指标、安全性指标、可行性指标在种群初始化阶段和程序判定阶段已充分考虑。
那么同理其他的USV该时刻的评价函数值也能求出为f1、f2…fj…fn,那么再取整体的评价函数为:其中每个USV的评价函数fj保证每个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 (1)
1.一种基于避碰准则的多USV群体协同避碰规划方法,其特征在于,该方法包括以下步骤:
步骤1:多USV系统建系;
步骤1.1:取空间内的任意一点O为原点,取空间中与O在同一水平面的正北方向为O-X轴正方向,取空间中与O和O-X轴在同一水平面指向正东方向为O-Y轴正方向,建立大地惯性坐标系;
步骤1.2:对于多USV系统中的每一个USV,取USV重心为原点o,取船体上与o在同一水平面指向船艏的方向为o-x轴正方向,取船体上与o和o-x轴在同一水平面指向右舷-的方向为o-y轴正方向,建立船体随动坐标系o-xy;
步骤1.3:对于多USV系统中的每一个USV,取雷达重心为原点o0,取雷达上与o0在同一水平面指向船艏的方向为o0-x0轴正方向,取雷达上与o0和o0-x0轴在同一水平面指向右舷的方向为o0-y0轴正方向,建立雷达随动坐标系o0-x0y0;
步骤2:对于多USV系统中的每一个USV,取本USV雷达探测到障碍物后的2个连续时刻的探测到的障碍物信息,判断障碍物属于动态障碍物还是静态障碍物;若动态障碍物为多USV系统中其他USV,则计算该USV与本USV的碰撞危险度,判断是否需要采取避碰措施及采取避碰策略的种类和时间;
步骤2.1:取本USV雷达探测到障碍物后的2个连续时刻的探测到的障碍物信息,其中第i时刻雷达探测范围内探测出的障碍物的扫描点集合为:{(Xi1,Yi1),(Xi2,Yi2),...,(Xin,Yin)},探测出的扫描点集合的坐标为大地惯性坐标系下的坐标值;由障碍物的扫描点集合估算障碍物的中心位置:
其中,Δt为两个连续时刻的时间间隔;
步骤2.4:根据已知的本USV的速度vm和艏向βm,计算作为动态障碍物的其他USV与本USV之间的相对速度Vr;
步骤2.5:计算本USV与作为动态障碍物的其他USV的最近会遇距离DCPA和到达最近会遇地点的时间TCPA;
步骤2.6:计算本USV与作为动态障碍物的其他USV的空间碰撞危险度uD;当d2<|DCPA|时,本USV航行安全,不需要执行避碰;当|DCPA|<d1时,本USV与作为动态障碍物的其他USV存在碰撞危险,需要采取避碰措施来消除,执行步骤2.9;当d1≤|DCPA|≤d2时,执行步骤2.7;
其中,d1=1.5ρ(θT);d2=2d1;
步骤2.7:计算本USV与作为动态障碍物的其他USV的时间碰撞危险度uT;
步骤2.8:计算本USV与作为动态障碍物的其他USV的总碰撞危险度u;
若uD=0或uT=0,则u=0;
若uD≠0且uT≠0,则u=max(uD,uT);
步骤2.9:判断本USV与作为动态障碍物的其他USV的会遇局势,包括对遇、右船舷小角度交叉相遇、右船舷大角度交叉相遇、左船舷小角度交叉相遇、左船舷大角度交叉相遇、它船右船舷追越、它船左船舷追越;
(1)对遇;
当其他USV从本USV左船舷5°到右船舷5°的扇形方向驶来时,断定为对遇,各自向右转弯从对方左船舷驶过;
(2)右船舷小角度交叉相遇;
当其他USV从本USV右船舷5°~67.5°的扇形方向驶来时,断定为右船舷小角度交叉相遇,交叉相遇的双方需各自向右转向进行避让;
(3)右船舷大角度交叉相遇;
当其他USV从本USV右船舷67.5°~112.5°的扇形方向驶来时,断定为右船舷大角度交叉相遇,这种情况下本USV或其他USV采取左转避让;
(4)左船舷小角度交叉相遇;
当其他USV从本USV左船舷5°~67.5°的扇形方向驶来时,断定为左船舷小角度交叉相遇;这种情况下,只有在其他USV与本USV形成紧迫局面而其他USV未采取避碰措施时才右转避让;
(5)左船舷大角度交叉相遇;
当其他USV从本USV左船舷67.5°~112.5°的扇形方向驶来时,断定为左船舷大角度交叉相遇;在这种情况下,只有在其他USV与本USV形成紧迫局面而其他USV未采取避碰措施时才右转避让;
(6)它船右船舷追越;
当其他USV从本USV右船舷112.5°~180°的扇形方向驶来存在追赶趋势时,并且驶来的USV能看到本USV的尾,断定为它船右船舷追越;在这种情况下,本USV为被追越船,驶来的USV为追越船,在两个USV没有形成紧迫局面前保持原来航行状态,存在紧迫局面时根据两个USV各自的方位来采取不同的转向避让;
(7)它船左船舷追越;
当其他USV从本USV左船舷112.5°~180°的扇形方向驶来存在追赶趋势时,并且驶来的USV能看到本USV的尾,断定为它船左船舷追越;在这种情况下,本USV为被追越船,驶来的USV为追越船,在两个USV没有形成紧迫局面前保持原来航行状态,存在紧迫局面时根据两个USV各自的方位来采取不同的转向避让;
步骤3:选择遗传算法为核心规划算法,设置种群容量,速度变化量和艏向变化量为个体变量,利用浮点数编码方式对其编码;经迭代后得到每一时刻最优的速度改变量和艏向变化量,由其组成一条USV的最优路径;
第j个USV避碰规划的评价函数取为:
其中,fi为评价函数值,其值衡为正,fi值越大被选择成为子代的可能性越大;ωi+1为本USV下一时刻采取的第i个体艏向角;d为本USV当前位置到终点的距离;d0为本USV的雷达随动坐标系的原点到静态障碍物的最短距离;μ为本USV的静态障碍物的边界下限角,即本USV的雷达随动坐标系原点与雷达检测到的静态障碍物下边界点的夹角;ν为本USV的静态障碍物的边界上限角,即本USV的雷达随动坐标系原点与雷达检测到的障碍物上边界点的夹角;δ为作为动态障碍物的其他USV的边界下限角,即本USV的雷达坐标系原点与雷达检测到的作为动态障碍物的其他USV的下边界点的夹角;γ为作为动态障碍物的其他USV的边界上限角,即本USV的雷达坐标系原点与雷达检测到的作为动态障碍物的其他USV的上边界点的夹角;
评价函数采取奖惩机制增大区分度,当下一时刻的艏向角在障碍物边界上下限角范围内或者作为动态障碍物的其他USV的边界上下限角范围内,继续航行会接近障碍物或其他USV,此时对这样个体的评价函数值缩小100倍使其被选择的概率减小;当下一时刻的艏向角不在障碍物边界上下限角范围内和作为动态障碍物的其他USV的边界上下限角范围内,继续航行能避开障碍物,此时对这样个体的评价函数值扩大100倍使其被选择的概率增大;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910033309.3A CN109597417B (zh) | 2019-01-14 | 2019-01-14 | 一种基于避碰准则的多usv群体协同避碰规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910033309.3A CN109597417B (zh) | 2019-01-14 | 2019-01-14 | 一种基于避碰准则的多usv群体协同避碰规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109597417A CN109597417A (zh) | 2019-04-09 |
CN109597417B true CN109597417B (zh) | 2022-04-05 |
Family
ID=65966257
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910033309.3A Active CN109597417B (zh) | 2019-01-14 | 2019-01-14 | 一种基于避碰准则的多usv群体协同避碰规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109597417B (zh) |
Families Citing this family (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109782760B (zh) * | 2019-01-14 | 2022-06-21 | 哈尔滨工程大学 | 一种考虑无通信的多usv群体协同避碰规划方法 |
CN110262483A (zh) * | 2019-06-10 | 2019-09-20 | 华东师范大学 | 一种无人船航向控制方法及无人船 |
CN110544395B (zh) * | 2019-08-28 | 2020-11-13 | 中船重工鹏力(南京)大气海洋信息系统有限公司 | 一种vts系统中船舶追越态势的智能精算预警方法 |
CN110716570B (zh) * | 2019-10-30 | 2022-09-06 | 青岛科技大学 | 一种开阔繁忙水域基于船舶集群态势的相对领域获取方法 |
CN110906934B (zh) * | 2019-11-29 | 2022-04-15 | 华中科技大学 | 一种基于碰撞危险系数的无人艇避障方法和系统 |
CN111736601B (zh) * | 2020-06-15 | 2023-04-07 | 大连海事大学 | 一种近距离会遇时船舶避碰动态辅助方法 |
CN111798702B (zh) * | 2020-07-07 | 2022-05-31 | 中国船舶工业系统工程研究院 | 无人艇路径跟踪控制方法、系统、存储介质及终端 |
CN112650246B (zh) * | 2020-12-23 | 2022-12-09 | 武汉理工大学 | 一种船舶自主导航方法及装置 |
CN113110460B (zh) * | 2021-04-20 | 2022-09-02 | 哈尔滨工程大学 | 一种动态环境下获取水面无人艇艏向可行区间的方法 |
RU2768207C1 (ru) * | 2021-06-10 | 2022-03-23 | Акционерное общество "Концерн "Центральный научно-исследовательский институт "Электроприбор" | Способ обхода автономным необитаемым подводным аппаратом неподвижного подводного препятствия |
CN114897443A (zh) * | 2022-07-07 | 2022-08-12 | 武汉理工大学 | 一种船舶碰撞风险评估方法、装置、设备和存储介质 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104050329A (zh) * | 2014-06-25 | 2014-09-17 | 哈尔滨工程大学 | 一种检测船舶碰撞危险度的方法 |
CN105512769A (zh) * | 2015-12-16 | 2016-04-20 | 上海交通大学 | 基于遗传规划的无人机航迹规划系统及方法 |
CN105589464A (zh) * | 2016-03-28 | 2016-05-18 | 哈尔滨工程大学 | 一种基于速度障碍法的uuv动态避障方法 |
CN105807769A (zh) * | 2016-03-09 | 2016-07-27 | 哈尔滨工程大学 | 无人水下航行器ivfh避碰方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8830793B2 (en) * | 2012-06-28 | 2014-09-09 | Massachusetts Institute Of Technology | System and method for collision avoidance in underwater vehicles |
-
2019
- 2019-01-14 CN CN201910033309.3A patent/CN109597417B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104050329A (zh) * | 2014-06-25 | 2014-09-17 | 哈尔滨工程大学 | 一种检测船舶碰撞危险度的方法 |
CN105512769A (zh) * | 2015-12-16 | 2016-04-20 | 上海交通大学 | 基于遗传规划的无人机航迹规划系统及方法 |
CN105807769A (zh) * | 2016-03-09 | 2016-07-27 | 哈尔滨工程大学 | 无人水下航行器ivfh避碰方法 |
CN105589464A (zh) * | 2016-03-28 | 2016-05-18 | 哈尔滨工程大学 | 一种基于速度障碍法的uuv动态避障方法 |
Non-Patent Citations (3)
Title |
---|
Cooperative collision avoidance for unmanned surface vehicles based on improved genetic algorithm;Hongjian Wang,etc;《Ocean Engineering》;20210215;第1-25页 * |
基于进化遗传算法的无人艇避碰系统研究;刘佳男;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》;20160215;参见摘要,第9-18页第2.2-2.4节,第19页,第21页3.2.3节 * |
目标船运动参数及dCPA、tCPA决策模型;刘德新,等;《大连海事大学学报》;20040315;第22-25页 * |
Also Published As
Publication number | Publication date |
---|---|
CN109597417A (zh) | 2019-04-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109597417B (zh) | 一种基于避碰准则的多usv群体协同避碰规划方法 | |
CN109753068B (zh) | 一种考虑通信情况的多usv群体协同避碰规划方法 | |
CN110196598B (zh) | 一种无人艇的动态避碰方法 | |
CN110262492A (zh) | 一种无人艇的实时避碰和目标跟踪方法 | |
CN110083159B (zh) | 基于sbg和动态窗口约束的无人船自主动态避碰方法 | |
CN108803313A (zh) | 一种基于海流预测模型的路径规划方法 | |
Wang et al. | Ship route planning based on double-cycling genetic algorithm considering ship maneuverability constraint | |
Wang et al. | Cooperative collision avoidance for unmanned surface vehicles based on improved genetic algorithm | |
Li et al. | Toward time-optimal trajectory planning for autonomous ship maneuvering in close-range encounters | |
CN108459602B (zh) | 多障碍复杂环境下欠驱动无人艇的自主靠泊方法 | |
CN111861155B (zh) | 船舶碰撞风险检测方法、系统、计算机设备和存储介质 | |
CN112578793B (zh) | 一种快速运动水面艇的避障方法 | |
CN113032896B (zh) | 一种基于船舶驾驶员偏好的避碰辅助决策方法 | |
CN113538973B (zh) | 一种基于改进粒子群算法的船舶自动避碰方法 | |
CN110444046B (zh) | 一种受限水域非冲突会遇船舶集群态势分析方法 | |
CN109916419A (zh) | 一种面向对象的混合遗传算法无人艇实时路径规划方法 | |
CN109782760B (zh) | 一种考虑无通信的多usv群体协同避碰规划方法 | |
CN114326702B (zh) | 一种基于模型预测控制的内河无人船避碰方法 | |
Li et al. | Distributed multi-objective algorithm for preventing multi-ship collisions at sea | |
CN111984012A (zh) | 一种无人船自主避障及航线规划的方法 | |
CN116360434A (zh) | 基于改进的csac-apf算法的船舶路径规划方法 | |
CN110414042B (zh) | 一种冲突会遇局面下船舶集群态势分析方法 | |
Wu et al. | Multi-vessels collision avoidance strategy for autonomous surface vehicles based on genetic algorithm in congested port environment | |
CN115407780A (zh) | 一种基于三阶段避障策略的无人船局部路径规划方法 | |
Yu et al. | Dynamic adaptive autonomous navigation decision-making method in traffic separation scheme waters: A case study for Chengshanjiao waters |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |