CN113671968A - 一种基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法 - Google Patents
一种基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法 Download PDFInfo
- Publication number
- CN113671968A CN113671968A CN202110982087.7A CN202110982087A CN113671968A CN 113671968 A CN113671968 A CN 113671968A CN 202110982087 A CN202110982087 A CN 202110982087A CN 113671968 A CN113671968 A CN 113671968A
- Authority
- CN
- China
- Prior art keywords
- collision avoidance
- ship
- speed
- obstacle
- collision
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 118
- 230000007246 mechanism Effects 0.000 title claims abstract description 23
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 29
- 230000002265 prevention Effects 0.000 claims abstract description 20
- 230000008859 change Effects 0.000 claims abstract description 14
- 238000011156 evaluation Methods 0.000 claims abstract description 10
- 230000004888 barrier function Effects 0.000 claims description 40
- 230000006870 function Effects 0.000 claims description 20
- 230000008569 process Effects 0.000 claims description 13
- 238000013459 approach Methods 0.000 claims description 9
- 239000000126 substance Substances 0.000 claims description 9
- LFULEKSKNZEWOE-UHFFFAOYSA-N propanil Chemical compound CCC(=O)NC1=CC=C(Cl)C(Cl)=C1 LFULEKSKNZEWOE-UHFFFAOYSA-N 0.000 claims description 4
- 241000283283 Orcinus orca Species 0.000 claims description 3
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000009471 action Effects 0.000 claims description 3
- 239000002245 particle Substances 0.000 claims description 3
- 230000008092 positive effect Effects 0.000 claims description 3
- 230000001007 puffing effect Effects 0.000 claims description 3
- QZXCCPZJCKEPSA-UHFFFAOYSA-N chlorfenac Chemical compound OC(=O)CC1=C(Cl)C=CC(Cl)=C1Cl QZXCCPZJCKEPSA-UHFFFAOYSA-N 0.000 claims 1
- 238000013461 design Methods 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 6
- 230000007547 defect Effects 0.000 description 3
- 238000013528 artificial neural network Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000010355 oscillation Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 235000001968 nicotinic acid Nutrition 0.000 description 1
- 230000002787 reinforcement Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，包括：构建无人艇运动学模型，并结合复杂海面环境构建运动约束条件；根据海上会遇规则遵守判断、碰撞可能性判断、意识并采取避碰手段判断等参数建立评价函数，并结合权重影响避碰责任划分；采用动态最优互惠避碰算法选择理想机动速度代价函数，在期望速度的基础上加入当前速度和当前角度代价，减少速度变化带来的抖振及损耗。本发明设计了一种面向复杂海面情况下的海上会遇规则判断体系，并将其应用于避碰权重选择。并进一步构建了新型最优速度代价函数，加入当前速度与当前角度两个评价标准，以减少算法带来的抖振和不必要的损耗，从而实现无人水面艇的高效、快速避障。
Description
技术领域
本发明涉及无人艇技术领域，具体而言，尤其涉及一种基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法。
背景技术
近年来，无人艇(Unmanned Surface Vehicles)以安全性高、成本低、可控性强等优势，被广泛应用于气候变化、环境监测、水文测绘等科学领域；此外在商业和军事等领域对水面无人船舶领域也有较大的需求。然而，由于USV的诸多问题，例如复杂的海洋环境对控制稳定性的挑战，未知环境下的实时避障，部分海域存在的通信中断及信号屏蔽、干扰等，这些使得高性能、高速度的USV仍处于实践阶段，人们大多只能对USV进行半自动控制而不是完全自主控制。由此可见，为了最大限度达到自主控制目标，减少人为干预，亟需对无人水面船舶做进一步的研究。
无人艇自主航行的核心是航行控制，它分为制导、导航与控制三部分。目前控制方面已有许多先进的控制系统设计方案，而对规划领域的研究相对较少，路径规划可以划分为全局路径规划和局部路径规划，而避碰属于其中的局部路径规划范畴。目前应用于无人艇领域的避碰算法主要有人工势场法、动态窗口法、向量场直方图法、速度障碍法、蚁群算法等仿生学算法、神经网络等智能算法。具体说来，人工势场法因其原理简单、实时性好而被广泛应用，然而，由于该算法本身的缺陷，极易陷入局部极小，并且当斥力引力相同时将导致USV停滞不前；动态窗口法符合模型的动力学约束且不易陷入局部最优，该方法的缺点为运算量大，实时性较差；向量场直方图法通过计算运动速度与方向，在一定区域通过设立向量场描述空间状态，达到避障的目的，缺点为易陷入局部最优，且只能改变速度大小而无法改变速度方向；蚁群算法等相关算法作为基于群体智能的方法，不仅可以作为全局路径规划方法，亦可作用于局部路径规划；目前使用较多的还有新兴的人工智能算法诸如神经网络算法、深度强化学习算法等，但它们都要受到泛化性限制且需要提前进行训练的限制；速度障碍法作为一种传统的几何方法因其运算速度快、不易陷入局部最优、适应性强等特点被广泛应用于各种避障环境，本专利同样运用改进的速度障碍法对无人艇实施避障规划。
速度障碍法最早由Fiorini由1998年提出，并成功应用于各个领域，近年来，针对速度障碍法(Velocity Obstacle)的一些不足，陆续提出了线性速度障碍法(LinearVelocity Obstacle)、非线性速度障碍法(Non-linear Velocity Obstacle)、概率速度障碍法(Probabilistic Velocity Obstacle)、互惠速度障碍法(Reciprocal VelocityObstacles)、基于椭圆的速度障碍法(Ellipse-based Velocity Obstacles)、联合障碍避碰(Combine Collision Avoid)、最优互惠避碰(Optimal Reciprocal CollisionAvoidance)、不确定非线性速度障碍法(Uncertainty Non-Linear Velocity Obstacles)等算法，速度障碍法大体分为线性与非线性两种算法，其中非线性算法需要对轨迹进行预测，其避障准度与精度与轨迹预测准确程度有直接关系，但是在实际中很难通过获取其他障碍的精确信息进行轨迹预测，故本专利采用的是线性算法中基于改进的最优互惠避碰算法。
目前在无人艇避障领域已有很多方法，然而这些方法大多并没有考虑海上避碰规则，或只是简单将其应用于避碰。此外，当障碍物不遵守避碰规则时很容易造成碰撞或大幅度转角。虽然目前已有少部分避碰方法考虑了复杂情况下遵循海上避碰规则的问题，但往往只单独作为一个模块进行判断。
发明内容
根据上述提出最优互惠避碰算法避碰责任划分的问题和最优速度选择代价函数单一的技术问题，提供一种基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法。本发明设计了一种面向复杂海面情况下的海上会遇规则判断体系，并将其应用于避碰权重选择。在此基础上，进一步构建了新型最优速度代价函数，加入当前速度与当前角度两个评价标准，以减少算法带来的抖振和不必要的损耗，从而实现无人水面艇的高效、快速避障。
本发明采用的技术手段如下：
一种基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，包括如下步骤：
S1、构建无人艇运动学模型，并结合复杂海面环境构建运动约束条件；
S2、根据海上会遇规则遵守判断、碰撞可能性判断、意识并采取避碰手段判断一系列参数建立评价函数，并结合权重影响避碰责任划分；
S3、采用动态最优互惠避碰算法选择理想机动速度代价函数，在期望速度的基础上加入当前速度和当前角度代价，减少速度变化带来的抖振及损耗。
进一步地，所述步骤S1的具体实现过程如下：
S11、构建无人船运动学模型，如下：
其中，(x,y,ψ)表示无人船在地球坐标系的位置和航向，(u,v,r)分别表示在船体坐标系下无人船的前向、横向速度和航向角速度；
S12、将无人船膨化为以船的中心为圆心的圆，半径与船长相关形成避障过程中不能与障碍物接触的安全范围D，保证无人船安全行驶，对于障碍物同样做膨化处理，安全范围定义如下：
其中，pT表示障碍物位置；p(t)表示己船在t时刻位置；dO为一固定常数，具体参数根据船长变化而变化。
进一步地，所述步骤S2的具体实现过程如下：
S21、引入速度障碍法：
设受到控制的己船OS的位置和速度为PO(t)和vO，目标船只TS的位置和速度设为PT(t)和vT，己船OS相对于目标船只TS的相对速度设为vOT＝vOT-vT；
将目标无人船进行膨化处理，设障碍物为半径为RT的圆，同时将己船OS的半径RO加入到目标船中，将己船OS作为质点，通过相对速度vOT将目标船视为静止，从己船OS的位置PO(t)出发沿相对速度vOT方向与目标船只的膨化范围D(P,R)相交时无人船在时间τ内一定会碰撞，此时定义障碍速度空间如下：
D(P,R)＝{Q|||Q-P||＜R} (3)
其中，Q表示障碍物位置；P表示己船位置；R表示位置常数，大小由己船与障碍物半径共同决定；
其中，
表示避免碰撞速度集合；v表示己船速度；t表示有限时间；D表示速度集合区域；PT表示障碍物位置；PO表示己船位置；RT表示障碍物半径；RO表示己船半径；
定义X与Y的Minkowski和如下：
对于任意速度VT,若vT∈VT，
则A和B在当前速度下，至少在τ时间内是无碰撞的；因此，定义避碰速度集合
如下：
其中，VB表示目标船速度集合；
S22、基于速度障碍法，引入互惠速度障碍法：
基于速度障碍法再次更新vO，具体更新机制如下：
其中，
表示互惠速度障碍法避免碰撞速度集合；
表示己船更新速度；VOO|T(vT)表示原避免碰撞速度集合；
S23、基于引入的互惠速度障碍法，进一步引入最优互惠速度障碍法：
在互惠速度障碍法的基础上再次优化
因此进一步引入优化速度
用于从可行解中选出最优解，具体的ORCA定义区域定义如下：
其中，u是使点vOT-vT到速度障碍区
边缘最近的向量，n是该点的法向量，
表示每人承担一半的避碰责任；
是一个被直线分隔的半平面，该直线通过点
方向垂直于n；
是一个被直线分隔的半平面，该直线通过点
方向垂直于n。
S24、基于引入的最优互惠速度障碍法，进一步引入通常互惠速度障碍法，具体公式如下：
其中，ηO|T表示常数，ηO|T|(ηO|T∈[0,1])与避碰责任相关联，σ为接近无穷小的数，用以确保避碰需要的时间；使点vOT-vT到速度障碍区
边缘最近的向量，n是该点的法向量；
S25、基于引入的通常互惠速度障碍法，进一步引入动态最优互惠速度障碍法，使用动态参数对避碰责任进行评价和细化，即：
其中，
表示动态速度障碍法避免碰撞速度集合；β表示常数，β∈[0,1]，主要由三部分组成，分别为目标船只TS是否意识到避碰威胁并采取手段、避碰可能性、是否遵守海上会遇规则。
进一步地，所述步骤S3的具体实现过程如下：
S31、基于速度变化因子、避碰可能性因子以及海事规则因子，得到最佳避碰速度；
S32、基于最佳避碰速度，采用代价函数获取vnew，具体的代价函数Cv表示如下：
Cv(k)＝(1-β)(||vd(k)-vO(k)||)+β(||vO(k-1)-vO(k)||) (12)
其中，vd(k)表示期望速度；vO(k)表示当前速度；vO(k-1)表示上一时刻速度；代价函数的第一项在保证船向目标方向行驶的同时兼顾了避障时的安全性，第二项则避免船改变方向是带来的抖振。
进一步地，所述步骤S31的具体实现过程如下：
S311、速度变化因子：
判断航向角ψ是否发生变化，再引入一个新的变量接近加速度aT，通过对最近三个时刻的位置差判断目标船是否采取积极手段，具体公式如下：
aT(k)＝Δp(k)-Δp(k-1) (13)
Δp(k)＝|pO(k)-pT(k)| (14)
其中，Δp(k)为k时刻己船与目标船距离，当(aT(k)＜aT(k-1))∩(|Δψ|＞5)时说明目标船只TS有避障能力且采取积极行动避障，同理可判断障碍物是否有避障能力且是否采取手段；
S312、避碰可能性因子：
基于接近船舶的估计最近位置CPA，得到避碰可能性结果，具体的表达式如下：
Δpx＝pTx-pOx (18)
Δvx＝vOx-vTx (19)
其中，DCPA为最小接近距离，TCPA为最小接近时间，pTx、pOx、vOx、vTx分别为己船与障碍物的位置与速度；因此，根据船长和CPA可将避碰可能性分为高中低三个级别，分别用H、M、L表示。
S313、海事规则因子：
根据目标船的相对位置θT和海事规则判断是否遵守海上避碰规则，根据海上避碰规则将相遇问题分为追越、对遇和交叉三种，通常右舷为避碰责任方，避碰责任方应承担全部的避碰责任，交叉时从目标船后方绕行，相对位置θT、无人船航向角
目标船航向角
障碍物相对方位角αT的具体关系如下：
若目标船处于应承担避碰责任范围并进行右舷避碰动作或非责任范围并直行，则判定符合避碰规则，反之则判定未遵守规则。
较现有技术相比，本发明具有以下优点：
1、本发明提供的基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，根据原最优互惠避碰算法并未考虑海事规则判断责任的问题，创新性的引入了动态权重判断责任机制，并在该机制中充分考虑了是否意识到冲突并采取积极手段、己船避碰可能性、目标是否遵守海上避碰规则等因素。
2、本发明提供的基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，在最优速度评价函数加入当前速度与当前角度两个评价标准，进一步减少了算法带来的抖振和不必要的损耗，从而使无人船安全且快速完成避障。
3、本发明提供的基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，根据海上会遇规则遵守判断、碰撞可能性判断、意识并采取避碰手段判断等因素设立评价函数，进而通过权重影响避碰责任划分，以使得无人艇在更为合理地遵循海上会遇规则的同时兼顾碰撞风险。
4、本发明提供的基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，针对动态最优互惠避碰算法(DORCA)在选择理想机动速度代价函数时存在速度变化过快的问题，在期望速度的基础上引入当前速度和当前角度代价，以减少速度变化带来的抖振及损耗。
5、本发明提供的基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，其局部路径规划机制的应用使用约束符合无人船的实际运动特性。
基于上述理由本发明可在无人艇等领域广泛推广。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明方法流程图。
图2为本发明实施例提供的速度障碍法图例。
图3为本发明实施例提供的速度障碍法图例。
图4为本发明实施例提供的海上会遇规则示意图。
图5为本发明实施例提供的各航向角关系示意图。
图6为本发明实施例提供的权重判断示意图。
图7为本发明实施例提供的DORCA左舷会遇图。
图8为本发明实施例提供的TS直行碰撞风险对比图。
图9为本发明实施例提供的TS直行速度变化对比图。
图10为本发明实施例提供的DWA左舷会遇图。
图11为本发明实施例提供的DORCA右舷会遇图。
图12为本发明实施例提供的DORCA对遇图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
如图1所示，本发明提供了一种基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，包括如下步骤：
S1、构建无人艇运动学模型，并结合复杂海面环境构建运动约束条件；
S2、根据海上会遇规则遵守判断、碰撞可能性判断、意识并采取避碰手段判断一系列参数建立评价函数，并结合权重影响避碰责任划分；
S3、采用动态最优互惠避碰算法选择理想机动速度代价函数，在期望速度的基础上加入当前速度和当前角度代价，减少速度变化带来的抖振及损耗。
具体实施时，作为本发明优选的实施方式，所述步骤S1的具体实现过程如下：
S11、构建无人船运动学模型，如下：
其中，(x,y,ψ)表示无人船在地球坐标系的位置和航向，(u,v,r)分别表示在船体坐标系下无人船的前向、横向速度和航向角速度；
注1：对于欠驱动无人船，存在前向偏航动力的限制，其横向速度v有界，故在动力学约束情况下，只需约束横向速度和航向角速度即可，二者限制范围将于仿真中给出。
S12、将无人船膨化为以船的中心为圆心的圆，半径与船长相关形成避障过程中不能与障碍物接触的安全范围D，保证无人船安全行驶，对于障碍物同样做膨化处理，安全范围定义如下：
其中，pT表示障碍物位置；p(t)表示己船在t时刻位置；dO为一固定常数，具体参数根据船长变化而变化。
具体实施时，作为本发明优选的实施方式，所述步骤S2的具体实现过程如下：
S21、引入速度障碍法：
如图2所示，为常规速度障碍法，设受到控制的己船OS的位置和速度为PO(t)和vO，目标船只TS的位置和速度设为PT(t)和vT，己船OS相对于目标船只TS的相对速度设为vOT＝vOT-vT；
将目标无人船进行膨化处理，设障碍物为半径为RT的圆，同时将己船OS的半径RO加入到目标船中，将己船OS作为质点，通过相对速度vOT将目标船视为静止，从己船OS的位置PO(t)出发沿相对速度vOT方向与目标船只的膨化范围D(P,R)相交时无人船在时间τ内一定会碰撞，此时定义障碍速度空间如下：
D(P,R)＝{Q|||Q-P||＜R} (3)
其中，Q表示障碍物位置；P表示己船位置；R表示位置常数，大小由己船与障碍物半径共同决定；
其中，
表示避免碰撞速度集合；v表示己船速度；t表示有限时间；D表示速度集合区域；PT表示障碍物位置；PO表示己船位置；RT表示障碍物半径；RO表示己船半径；
定义X与Y的Minkowski和如下：
对于任意速度VT,若vT∈VT，
则A和B在当前速度下，至少在τ时间内是无碰撞的；因此，定义避碰速度集合
如下：
其中，VB表示障碍物速度集合；
S22、基于速度障碍法，引入互惠速度障碍法：
传统速度障碍法要求vO应处于
之外，由于速度障碍法提供的是可行解而非最优解，因而当两个具备同样算法的无人船行进时易在速度选择时产生振荡。为了解决这个问题引入了互惠速度障碍法，基于速度障碍法再次更新vO，具体更新机制如下：
其中，
表示互惠速度障碍法避免碰撞速度集合；
表示己船更新速度；VOO|T(vT)表示原避免碰撞速度集合；
S23、基于引入的互惠速度障碍法，进一步引入最优互惠速度障碍法：
通过改变区域更新的vO扩大了碰撞区域，从而消除了振荡，然而最优互惠避碰算法需要在互惠速度障碍法的基础上再次优化
因此进一步引入优化速度
用于从可行解中选出最优解，如图3所示，具体的ORCA定义区域定义如下：
其中，u是使点vOT-vT到速度障碍区
边缘最近的向量，n是该点的法向量，
表示每人承担一半的避碰责任；
是一个被直线分隔的半平面，该直线通过点
方向垂直于n；
是一个被直线分隔的半平面，该直线通过点
方向垂直于n。
S24、基于引入的最优互惠速度障碍法，进一步引入通常互惠速度障碍法，值得提出的是互惠速度障碍法的有效性是基于双方都有避碰能力的基础之上，并且更多应用于多无人船避碰及协同问题中。基于此Zhao等人提出改进的GORCA算法，将其与海上避碰规则结合判断权重，具体公式如下：
其中，其中，ηO|T表示常数，ηO|T|(ηO|T∈[0,1])与避碰责任相关联，σ为接近无穷小的数，用以确保避碰需要的时间；使点vOT-vT到速度障碍区
边缘最近的向量，n是该点的法向量；
S25、基于引入的通常互惠速度障碍法，进一步引入动态最优互惠速度障碍法，使用动态参数对避碰责任进行评价和细化，即：
其中，
表示动态速度障碍法避免碰撞速度集合；β表示常数，β∈[0,1]，主要由三部组成，分别为目标船只TS是否意识到避碰威胁并采取手段、避碰可能性、是否遵守海上会遇规则。其中是否意识到冲突并采取手段规则根据两方面判断。
具体实施时，作为本发明优选的实施方式，所述步骤S3的具体实现过程如下：
S31、基于速度变化因子、避碰可能性因子以及海事规则因子，得到最佳避碰速度；
S32、基于最佳避碰速度，采用代价函数获取vnew，具体的代价函数Cv表示如下：
Cv(k)＝(1-β)(||vd(k)-vO(k)||)+β(||vO(k-1)-vO(k)||) (12)
其中，vd(k)表示期望速度；vO(k)表示当前速度；vO(k-1)表示上一时刻速度；代价函数的第一项在保证船向目标方向行驶的同时兼顾了避障时的安全性，第二项则避免船改变方向是带来的抖振。
具体实施时，作为本发明优选的实施方式，所述步骤S31的具体实现过程如下：
S311、速度变化因子：
判断航向角ψ是否发生变化，再引入一个新的变量接近加速度aT，通过对最近三个时刻的位置差判断目标船是否采取积极手段，具体公式如下：
aT(k)＝Δp(k)-Δp(k-1) (13)
Δp(k)＝|pO(k)-pT(k)| (14)
其中，Δp(k)为k时刻己船与目标船距离，当(aT(k)＜aT(k-1))∩(|Δψ|＞5)时说明目标船只TS有避障能力且采取积极行动避障，同理可判断障碍物是否有避障能力且是否采取手段；
S312、避碰可能性因子：
避碰可能性由目前广泛使用的方法基于接近点(CPA)来决定，CPA是接近船舶的估计最近位置，通常采用距离(DCPA)和时间(TCPA)两个指标进行判断，当这些指标小于一定的阈值时就会给出碰撞警告，这种方法目前已经嵌入到机载雷达系统故可实现性极强。
基于接近船舶的估计最近位置CPA，得到避碰可能性结果，具体的表达式如下：
Δpx＝pTx-pOx (18)
Δvx＝vOx-vTx (19)
其中，DCPA为最小接近距离，TCPA为最小接近时间，pTx、pOx、vOx、vTx分别为己船与障碍物的位置与速度；因此，根据船长和CPA可将避碰可能性分为高中低三个级别，分别用H、M、L表示。
S313、海事规则因子：
根据目标船的相对位置θT和海事规则判断是否遵守海上避碰规则，根据海上避碰规则将相遇问题分为追越、对遇和交叉三种，通常右舷为避碰责任方，避碰责任方应承担全部的避碰责任，交叉时从目标船后方绕行，具体规则如图4所示，相对位置θT、无人船航向角
目标船航向角
障碍物相对方位角αT的具体关系如图5所示，表达式如下：
若目标船处于应承担避碰责任范围并进行右舷避碰动作或非责任范围并直行，则判定符合避碰规则，反之则判定未遵守规则。
结合式(12-23)是否意识到避碰威胁并采取手段、避碰可能性、是否遵守海上会遇规则三个评价标准对目前海域情况进行判定，判定结果所对应的α将给出相应参数以进一步影响避碰承担责任。具体判定逻辑及影响因素判断如图6所示。可以看出，通过对参数的改变达到了在遵守海事规则的基础上最大程度保证安全性的避碰要求。
为了检验DORCA方法的有效性使用MATLAB对此方法进行仿真验证了方法的有效性，同时与其他方法相对比突入了方法的优越性，具体仿真如图7-12所示。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
Claims (5)
1.一种基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，其特征在于，包括如下步骤：
S1、构建无人艇运动学模型，并结合复杂海面环境构建运动约束条件；
S2、根据海上会遇规则遵守判断、碰撞可能性判断、意识并采取避碰手段判断一系列参数建立评价函数，并结合权重影响避碰责任划分；
S3、采用动态最优互惠避碰算法选择理想机动速度代价函数，在期望速度的基础上加入当前速度和当前角度代价，减少速度变化带来的抖振及损耗。
2.根据权利要求1所述的基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，其特征在于，所述步骤S1的具体实现过程如下：
S11、构建无人船运动学模型，如下：
其中，(x,y,ψ)表示无人船在地球坐标系的位置和航向，(u,v,r)分别表示在船体坐标系下无人船的前向、横向速度和航向角速度；
S12、将无人船膨化为以船的中心为圆心的圆，半径与船长相关形成避障过程中不能与障碍物接触的安全范围D，保证无人船安全行驶，对于障碍物同样做膨化处理，安全范围定义如下：
其中，pT表示障碍物位置；p(t)表示己船在t时刻位置；dO为一固定常数，具体参数根据船长变化而变化。
3.根据权利要求1所述的基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，其特征在于，所述步骤S2的具体实现过程如下：
S21、引入速度障碍法：
设受到控制的己船OS的位置和速度为PO(t)和vO，目标船只TS的位置和速度设为PT(t)和vT，己船OS相对于目标船只TS的相对速度设为vOT＝vOT-vT；
将目标无人船进行膨化处理，设障碍物为半径为RT的圆，同时将己船OS的半径RO加入到目标船中，将己船OS作为质点，通过相对速度vOT将目标船视为静止，从己船OS的位置PO(t)出发沿相对速度vOT方向与目标船只的膨化范围D(P,R)相交时无人船在时间τ内一定会碰撞，此时定义障碍速度空间如下：
D(P,R)＝{Q|||Q-P||＜R} (3)
其中，Q表示障碍物位置；P表示己船位置；R表示位置常数，大小由己船与障碍物半径共同决定；
其中，
表示避免碰撞速度集合；v表示己船速度；t表示有限时间；D表示速度集合区域；PT表示障碍物位置；PO表示己船位置；RT表示障碍物半径；RO表示己船半径；
定义X与Y的Minkowski和如下：
对于任意速度VT,若vT∈VT，
则A和B在当前速度下，至少在τ时间内是无碰撞的；因此，定义避碰速度集合
如下：
其中，VB表示目标船速度集合；
S22、基于速度障碍法，引入互惠速度障碍法：
基于速度障碍法再次更新vO，具体更新机制如下：
其中，
表示互惠速度障碍法避免碰撞速度集合；
表示己船更新速度；VOO|T(vT)表示原避免碰撞速度集合；
S23、基于引入的互惠速度障碍法，进一步引入最优互惠速度障碍法：
在互惠速度障碍法的基础上再次优化
因此进一步引入优化速度
用于从可行解中选出最优解，具体的ORCA定义区域定义如下：
其中，u是使点vOT-vT到速度障碍区
边缘最近的向量，n是该点的法向量，
表示每人承担一半的避碰责任；
是一个被直线分隔的半平面，该直线通过点
方向垂直于n；
是一个被直线分隔的半平面，该直线通过点
方向垂直于n；
S24、基于引入的最优互惠速度障碍法，进一步引入通常互惠速度障碍法，具体公式如下：
其中，ηO|T表示常数，ηO|T|(ηO|T∈[0,1])与避碰责任相关联，σ为接近无穷小的数，用以确保避碰需要的时间；使点vOT-vT到速度障碍区
边缘最近的向量，n是该点的法向量；
S25、基于引入的通常互惠速度障碍法，进一步引入动态最优互惠速度障碍法，使用动态参数对避碰责任进行评价和细化，即：
其中，
表示动态速度障碍法避免碰撞速度集合；β表示常数，β∈[0,1]，主要由三部分组成，分别为目标船只TS是否意识到避碰威胁并采取手段、避碰可能性、是否遵守海上会遇规则。
4.根据权利要求1所述的基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，其特征在于，所述步骤S3的具体实现过程如下：
S31、基于速度变化因子、避碰可能性因子以及海事规则因子，得到最佳避碰速度；
S32、基于最佳避碰速度，采用代价函数获取vnew，具体的代价函数Cv表示如下：
Cv(k)＝(1-β)(||vd(k)-vO(k)||)+β(||vO(k-1)-vO(k)||) (12)
其中，vd(k)表示期望速度；vO(k)表示当前速度；vO(k-1)表示上一时刻速度；代价函数的第一项在保证船向目标方向行驶的同时兼顾了避障时的安全性，第二项则避免船改变方向是带来的抖振。
5.根据权利要求1所述的基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，其特征在于，所述步骤S31的具体实现过程如下：
S311、速度变化因子：
判断航向角ψ是否发生变化，再引入一个新的变量接近加速度aT，通过对最近三个时刻的位置差判断目标船是否采取积极手段，具体公式如下：
aT(k)＝Δp(k)-Δp(k-1) (13)
Δp(k)＝|pO(k)-pT(k)| (14)
其中，Δp(k)为k时刻己船与目标船距离，当(aT(k)＜aT(k-1))∩(|Δψ|＞5)时说明目标船只TS有避障能力且采取积极行动避障，同理可判断障碍物是否有避障能力且是否采取手段；
S312、避碰可能性因子：
基于接近船舶的估计最近位置CPA，得到避碰可能性结果，具体的表达式如下：
Δpx＝pTx-pOx (18)
Δvx＝vOx-vTx (19)
其中，DCPA为最小接近距离，TCPA为最小接近时间，pTx、pOx、vOx、vTx分别为己船与障碍物的位置与速度；因此，根据船长和CPA可将避碰可能性分为高中低三个级别，分别用H、M、L表示；
S313、海事规则因子：
根据目标船的相对位置θT和海事规则判断是否遵守海上避碰规则，根据海上避碰规则将相遇问题分为追越、对遇和交叉三种，通常右舷为避碰责任方，避碰责任方应承担全部的避碰责任，交叉时从目标船后方绕行，相对位置θT、无人船航向角
目标船航向角
障碍物相对方位角αT的具体关系如下：
若目标船处于应承担避碰责任范围并进行右舷避碰动作或非责任范围并直行，则判定符合避碰规则，反之则判定未遵守规则。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110982087.7A CN113671968B (zh) | 2021-08-25 | 2021-08-25 | 一种无人水面艇实时避碰方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110982087.7A CN113671968B (zh) | 2021-08-25 | 2021-08-25 | 一种无人水面艇实时避碰方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113671968A true CN113671968A (zh) | 2021-11-19 |
| CN113671968B CN113671968B (zh) | 2023-12-12 |
Family
ID=78546206
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110982087.7A Active CN113671968B (zh) | 2021-08-25 | 2021-08-25 | 一种无人水面艇实时避碰方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113671968B (zh) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114326720A (zh) * | 2021-12-20 | 2022-04-12 | 上海大学 | 一种无人艇实时避障方法及系统 |
| CN115019561A (zh) * | 2022-08-09 | 2022-09-06 | 武汉理工大学 | 互见情况下船舶拖曳系统的外部碰撞风险预警系统 |
| CN116483091A (zh) * | 2023-05-10 | 2023-07-25 | 北鲲睿航科技(上海)有限公司 | 一种船舶自主避障方法及系统 |
| CN116564136A (zh) * | 2023-05-20 | 2023-08-08 | 广西综合交通大数据研究院 | 基于ais的船舶碰撞预测方法 |
| CN116610125A (zh) * | 2023-05-26 | 2023-08-18 | 北鲲睿航科技(上海)有限公司 | 一种用于智能船舶主动防撞系统的避碰方法及系统 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20110114078A (ko) * | 2010-04-12 | 2011-10-19 | 현대중공업 주식회사 | 선박 충돌회피 시스템을 위한 이동 장애물 시뮬레이터. |
| CN108664020A (zh) * | 2018-04-11 | 2018-10-16 | 上海大学 | 一种基于速度障碍法和动态窗口法的无人艇动态避障算法 |
| CN110174896A (zh) * | 2019-05-31 | 2019-08-27 | 中国船舶重工集团公司第七0七研究所 | 一种无人艇多目标自主避碰方法 |
| CN110197598A (zh) * | 2019-05-31 | 2019-09-03 | 中国船舶重工集团公司第七0七研究所 | 一种融合海事规则的动态目标避碰方法 |
| CN110427044A (zh) * | 2019-07-17 | 2019-11-08 | 河北科技大学 | 基于改进速度障碍法的无人机冲突探测与冲突解脱方法 |
| CN111399506A (zh) * | 2020-03-13 | 2020-07-10 | 大连海事大学 | 一种基于动力学约束的全局-局部混合无人船路径规划方法 |
| KR102269676B1 (ko) * | 2020-10-14 | 2021-06-25 | 제모스 주식회사 | 선박의 자동 충돌 예방 방법 및 장치 |
-
2021
- 2021-08-25 CN CN202110982087.7A patent/CN113671968B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20110114078A (ko) * | 2010-04-12 | 2011-10-19 | 현대중공업 주식회사 | 선박 충돌회피 시스템을 위한 이동 장애물 시뮬레이터. |
| CN108664020A (zh) * | 2018-04-11 | 2018-10-16 | 上海大学 | 一种基于速度障碍法和动态窗口法的无人艇动态避障算法 |
| CN110174896A (zh) * | 2019-05-31 | 2019-08-27 | 中国船舶重工集团公司第七0七研究所 | 一种无人艇多目标自主避碰方法 |
| CN110197598A (zh) * | 2019-05-31 | 2019-09-03 | 中国船舶重工集团公司第七0七研究所 | 一种融合海事规则的动态目标避碰方法 |
| CN110427044A (zh) * | 2019-07-17 | 2019-11-08 | 河北科技大学 | 基于改进速度障碍法的无人机冲突探测与冲突解脱方法 |
| CN111399506A (zh) * | 2020-03-13 | 2020-07-10 | 大连海事大学 | 一种基于动力学约束的全局-局部混合无人船路径规划方法 |
| KR102269676B1 (ko) * | 2020-10-14 | 2021-06-25 | 제모스 주식회사 | 선박의 자동 충돌 예방 방법 및 장치 |
Non-Patent Citations (4)
| Title |
|---|
| YUN LI等: "Real-time collision avoidance planning for unmanned surface vessels based on field theory", ISA TRANSACTIONS, pages 233 - 242 * |
| 张洋洋;瞿栋;柯俊;李小毛;: "基于速度障碍法和动态窗口法的无人水面艇动态避障", 上海大学学报(自然科学版), no. 01, pages 1 - 16 * |
| 朱凯歌;史国友;汪琪;刘娇;: "基于船舶领域的碰撞危险度评估模型", 上海海事大学学报, no. 02, pages 1 - 6 * |
| 罗贤程;尹建川;李宏宾;: "基于动态分阶势场法的船舶自动避碰系统", 上海海事大学学报, no. 01, pages 1 - 7 * |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114326720A (zh) * | 2021-12-20 | 2022-04-12 | 上海大学 | 一种无人艇实时避障方法及系统 |
| CN114326720B (zh) * | 2021-12-20 | 2023-08-22 | 上海大学 | 一种无人艇实时避障方法及系统 |
| CN115019561A (zh) * | 2022-08-09 | 2022-09-06 | 武汉理工大学 | 互见情况下船舶拖曳系统的外部碰撞风险预警系统 |
| CN116483091A (zh) * | 2023-05-10 | 2023-07-25 | 北鲲睿航科技(上海)有限公司 | 一种船舶自主避障方法及系统 |
| CN116483091B (zh) * | 2023-05-10 | 2024-04-19 | 北鲲睿航科技(上海)有限公司 | 一种船舶自主避障方法及系统 |
| CN116564136A (zh) * | 2023-05-20 | 2023-08-08 | 广西综合交通大数据研究院 | 基于ais的船舶碰撞预测方法 |
| CN116564136B (zh) * | 2023-05-20 | 2024-03-19 | 广西综合交通大数据研究院 | 基于ais的船舶碰撞预测方法 |
| CN116610125A (zh) * | 2023-05-26 | 2023-08-18 | 北鲲睿航科技(上海)有限公司 | 一种用于智能船舶主动防撞系统的避碰方法及系统 |
| CN116610125B (zh) * | 2023-05-26 | 2024-01-30 | 北鲲睿航科技(上海)有限公司 | 一种用于智能船舶主动防撞系统的避碰方法及系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113671968B (zh) | 2023-12-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113671968A (zh) | 一种基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法 | |
| CN109597417B (zh) | 一种基于避碰准则的多usv群体协同避碰规划方法 | |
| CN109753068B (zh) | 一种考虑通信情况的多usv群体协同避碰规划方法 | |
| CN112327885B (zh) | 一种无人船自适应全局-局部混合路径规划的方法 | |
| CN110083159B (zh) | 基于sbg和动态窗口约束的无人船自主动态避碰方法 | |
| CN110444046B (zh) | 一种受限水域非冲突会遇船舶集群态势分析方法 | |
| CN110837255B (zh) | 一种适用于高速水面无人艇的自主危险规避方法 | |
| CN112462786B (zh) | 一种基于模糊控制策略双窗口算法的无人艇避碰方法 | |
| Fossen et al. | Extended kalman filter design and motion prediction of ships using live automatic identification system (ais) data | |
| CN113032896B (zh) | 一种基于船舶驾驶员偏好的避碰辅助决策方法 | |
| CN114895673A (zh) | 一种规则约束下基于深度强化学习的船舶避碰决策方法 | |
| CN114355899A (zh) | 一种基于动态导航船域与速度障碍的无人船避碰方法 | |
| CN111881580A (zh) | 一种无人船规避障碍物的运动规划方法 | |
| Wenming et al. | A dynamic collision avoidance solution scheme of unmanned surface vessels based on proactive velocity obstacle and set-based guidance | |
| Zhao et al. | Decision-making for the autonomous navigation of USVs based on deep reinforcement learning under IALA maritime buoyage system | |
| CN113538973B (zh) | 一种基于改进粒子群算法的船舶自动避碰方法 | |
| Liang et al. | Economic MPC-based planning for marine vehicles: Tuning safety and energy efficiency | |
| Chen et al. | Decision support based on artificial fish swarm for ship collision avoidance from AIS data | |
| CN110414042B (zh) | 一种冲突会遇局面下船舶集群态势分析方法 | |
| CN110796142B (zh) | 一种基于船舶集群态势的船间相对领域获取方法 | |
| Chen et al. | Pattern knowledge discovery of ship collision avoidance based on AIS data analysis | |
| Yuan et al. | EMPMR berthing scheme: A novel event-triggered motion planning and motion replanning scheme for unmanned surface vessels | |
| Jose et al. | Navigating the Ocean with DRL: Path following for marine vessels | |
| CN116414123A (zh) | 基于改进快速行进法的水面无人艇路径规划方法 | |
| CN110175406A (zh) | 基于船联网条件下的多船跟随行为模拟方法 |
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 |