CN105022270B - 基于速度矢量坐标系的船自动避碰方法 - Google Patents

Abstract

本发明提供一种基于速度矢量坐标系的船自动避碰方法：包括:(1)、以某一船舶作为质点，获取该船舶周围的障碍物信息；(2)、通过碰撞预测模型判断船舶与周围障碍物是否有碰撞危险，若存在危险，则进入步骤(3)，若不存在危险，则返回步骤(1)；(3)、在速度矢量坐标系下，解算该船舶能够移出碰撞区域的相对速度,根据相对速度求解出船舶的绝对速度；(4)、利用船舶的绝对速度在自动舵控制器下调整船舶的航向，直到船舶的当前速度为所述绝对速度；(5)、反复循环步骤(1)‑步骤(4)，以实现基于速度矢量坐标系的船自动避碰.本发明不仅能判断出船舶间是否有碰撞风险还能提供避碰方法。

Description

基于速度矢量坐标系的船自动避碰方法

技术领域

本发明涉及航海技术领域，具体涉及一种基于速度矢量坐标系的船自动避碰方法。

背景技术

船舶智能避碰技术研究一直以来是智能航海研究领域的热点和前沿课题，是实现船舶自动化的关键技术之一。由于涉及因素较多，尽管人们对船舶避碰的研究已经付出了很大的努力，但是船舶智能避碰技术研究进展相对还是比较缓慢。例如关于多船会遇避碰决策问题还没有得到很好解决，特别是较为复杂的多船会遇避碰决策问题。

船舶的局部避碰是指船舶在水面航行过程中，遇到如岛屿、船只、浮标等障碍物时，能够考虑到其操纵运动特性，自主地识别避让障碍物。目前，国内外专家学者解决船舶局部避碰问题多采用局部路径规划、轨迹规划方法，并且将多种智能算法注入其中，使得规划时间降低，规划精度提高。目前已有的船舶局部路径规划研究为船舶避碰提供了一定的参考，但是这些方法实施的前提多是将船舶视为质点，忽略其操纵运动特性，并且不考虑外界条件对船舶航行的影响。然而，在复杂的海洋环境中，船舶在躲避障碍物的同时，还会受到风浪流的影响，所以这种理想化的方法对于船舶在实际海面上航行避碰的借鉴意义是有限的，船舶避碰不仅要考虑如何进行固定会遇情况下的路径规划，还需要考虑动态情况下的路径规划。

发明内容

本发明的目的在于解决上述现有技术存在的缺陷，提供一种不仅能判断出船舶间是否有碰撞风险，同时也能提供避碰方法的于速度矢量坐标系的船自动避碰方法。

一种基于速度矢量坐标系的船自动避碰方法，包括以下步骤：

(1)、以某一船舶作为质点，获取该船舶周围的障碍物信息；

(2)、通过碰撞预测模型判断船舶与周围障碍物是否有碰撞危险，若存在危险，则进入步骤(3)，若不存在危险，则返回步骤(1)；所述通过碰撞预测模型判断船舶与周围障碍物是否有碰撞危险的方法为：

当本船舶处于单目标障碍物情形时：

在全局坐标系XOY中，船舶S位于点(X_S,Y_S)，速度为V_S；O视为运动障碍物，位于点(X_O,Y_O)，速度为V_O，首先将船舶S模型化为一个质点，然后将障碍物O根据船舶S的大小进行膨化，其膨化后的半径扩展为R_O，并且使得膨化后的障碍物边界为安全区域，称膨化障碍物O为船舶S的一个位置障碍PO， l_MO和l_NO是船舶S与PO两侧切线方向的射线，D_SO为l_SO方向上测量到的S与 O之间的距离，定义S和O的相对速度为V_SO＝V_S-V_O，则通过相对速度可以把 O当作静止障碍物，S的速度则看作为V_SO，如果V_SO保持不变，l_SO为其方向上的射线，则船舶将和障碍物O发生碰撞的条件为：

使上述条件成立的相对速度V_SO的集合，定义为速度空间中的相对碰撞区 RCC：

即射线l_MO和l_NO之间的区域，对于本船的任一相对速度V_SO，如果V_SO∈ RCC，则船舶将与障碍物O发生碰撞；

把RCC平移V_o后得到的区域称为绝对碰撞区域ACC：其中表示闵可夫斯基矢量和运算，则V_S的末端点位于ACC等价于V_SO∈RCC，所以当V_S的末端点位于ACC时，船舶将与障碍物O发生碰撞，ACC表示S与 O会发生碰撞的速度V_SO的集合，称为S对O的速度障碍VO：

VO(V_O)＝{V_S|(V_S-V_O)∈RCC}

在多目标标障碍物情形时，设障碍物O₁的速度为V_O1，相应的障碍速度为 OV₁；障碍物O₂的速度为V_O2，相应的速度障碍为OV₂，则V_S位于ACC₁和ACC₂之中，即V_S∈OV₁且V_S∈OV₂，如果在接下来的时刻V_S保持不变，则S将与 O₁、O₂发生碰撞；

(3)、在速度矢量坐标系下，解算该船舶能够移出碰撞区域的相对速度，根据相对速度求解出船舶的绝对速度；设k时刻，本船S向障碍物O两侧切线方向发射射线l_NO和l_MO，l_NO和l_MO与x轴的夹角分别为θ₁和θ₂；本船S和障碍物O的速度分别为和两者相对速度为 与x轴的夹角θ_a，避让障碍物所需要的相对速度的方向为θ_rr，本船S避让障碍物O所需要的相对速度的方向θ_rr应满足：

0＜θ_rr＜θ₁或θ₂＜θ_rr＜2π

取其边界值为本船k时刻避让障碍物所需要的相对速度的方向θ_rr，则：

根据速度三角形求解本船k时刻避让障碍物所需要的绝对速度方向；

(4)、利用船舶的绝对速度在自动舵控制器下调整船舶的航向，直到船舶的当前速度为所述绝对速度；

(5)、反复循环步骤(1)-步骤(4)，以实现基于速度矢量坐标系的船自动避碰。

进一步地，如上所述的基于速度矢量坐标系的船自动避碰方法，其特征在于，所述步骤(3)包括：

船舶针对区域内的所有障碍物分别作切线，其切线l₁,l₂...l_2n与x轴的夹角分

别为θ₁,θ₂…θ_2n，n＝1，2…船舶每隔一段较短的时间都要重新计算

θ_a,θ₁,θ₂……θ_2n；

设本船k时刻避让障碍物所需要的相对速度θ_rr，取边界值为本船k时刻避让障碍物所需要的相对速度θ_rr，则船舶避让障碍物所需要的相对速度θ_rr应满足：

判断出所需的θ_rr后，根据速度三角形求解本船k时刻避让障碍物所需要的绝对速度方向。

本发明的有益效果是：本发明提供的基于速度矢量坐标系的船自动避碰方法不仅能判断出船舶间是否有碰撞风险，同时也能提供避碰方法，本发明将速度障碍方法应用于船舶避碰领域，深入分析多目标船下船舶避碰的约束条件及操纵要求，构建以速度障碍为核心的基于速度矢量坐标系的多目标船情形下的船舶自动避碰数学模型，并提出本船转向避碰的操纵方案，通过仿真结果表明，该方法能够实现多目标船情形下船舶的自动避碰，本发明建立的基于速度矢量坐标系的多目标船自动避碰问题模型及求解的转向避碰操纵方案是可行的。

附图说明

图1为本发明基于速度障碍的单目标碰撞预测示意图；

图2为本发明基于速度障碍的多目标碰撞预测示意图；

图3为本发明避碰流程图；

图4为本发明速度矢量坐标系下单目标避碰示意；

图5为本发明速度矢量坐标系下多目标避碰示意；

图6为本发明离散化PID转向自动舵的控制流程；

图7(a)为多动态目标且有障碍较为密集情景下船舶避碰的初始场景图；

图7(b)为多动态目标船且有障碍较为密集情景下本船逐渐避开目标船场景图；

图8(a)为动静障碍物结合场景下的初始场景图；

图8 (b)为动静障碍物结合场景下本船逐渐避开静态和动态障碍物。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本文将速度障碍方法应用于船舶避碰领域，深入分析多目标船下船舶避碰的约束条件及操纵要求，构建以速度障碍为核心的基于速度矢量坐标系的多目标船情形下的船舶自动避碰数学模型，提出避碰的操纵方法，并进行仿真试验；

基于速度障碍的碰撞预测模型

单避让目标情形

如图1所示，全局坐标系XOY中，船舶S位于点(X_S,Y_S)，绝对速度为 V_S；O视为运动障碍物，位于点(X_O,Y_O)，速度为V_O。为了简化，将船舶及障碍物(此处以目标船为例)看作为圆形进行分析。首先将船舶S模型化为一个质点，然后将障碍物O根据船舶S的大小进行“膨化”，其半径扩展为R_O，所述R_O一般取本船的长度，若本船较小，一般取300m，并且使得“膨化”后的障碍物边界为安全区域。称“膨化”障碍物O为船舶S的一个位置障碍(PositionObstacle，PO)，l_MO和l_NO是船舶S与PO两侧切线方向的射线，D_SO为l_SO方向上测量到的S与O之间的距离。

定义S与O的相对速度：

V_SO＝V_S-V_O

则通过相对速度可以把O当作静止障碍物，S的速度则看作为V_SO，如果 V_SO保持不变，l_SO为其方向上的射线，则船舶将和障碍物O发生碰撞的条件为：

使上述条件成立的相对速度V_SO的集合，定义为速度空间中的相对碰撞区(Relative Collision Cone,RCC)：

即图1中射线l_MO和l_NO之间的区域，对于本船的任一相对速度V_SO，如果V_SO∈RCC，则船舶将与障碍物O发生碰撞。

把RCC平移V_o后得到的区域称为绝对碰撞区域(Absolute Collision Cone，ACC):

其中表示闵可夫斯基矢量和运算。

可以看出，则V_S的末端点位于ACC等价于V_SO∈RCC，所以当V_S的末端点位于ACC时，船舶将与障碍物O发生碰撞，ACC表示S与O会发生碰撞的速度V_SO的集合，称为S对O的速度障碍(Velocity Obstacle，VO):

VO(V_O)＝{V_S|(V_S-V_O)∈RCC}

多目标避让情形

图2为船舶S遇到多运动障碍物(其他船舶)的一个例子，其中障碍物O₁的速度为V_O1，相应的障碍速度为OV₁；障碍物O₂的速度为V_O2，相应的速度障碍为OV₂。可以看出V_S位于ACC₁和ACC₂之中(即V_S∈OV₁且V_S∈OV₂)，如果在接下来的时刻V_S保持不变，则S将与O₁、O₂发生碰撞。

速度矢量坐标系的船舶自动避碰方案

船舶运动数学模型

船舶运动规律与传统机器人或车辆有较大区别，难以像移动机器人那样急停急转,其紧急刹车性能和快速转艏性能严重弱于移动机器人,控制时滞性严重，避碰过程中必须要考虑船舶的运动特性。

船舶避碰主要考虑船舶的水面运动，所以需要建立本船前进、横移、转首三个自由度上的运动方程。基于分离模型(MMG)的思想可以分别计算力和力矩，推导得到船舶三自由度动力学方程为^[9]：

式中:x、y、ψ为大地坐标系下的横向坐标、纵向坐标、船舶航向；u、v、r 分别为纵向、横向和垂向的速度及绕Oz轴的角速度；m为船舶质量；m_x、m_y、 m_z分别为纵向、横向和垂向船舶的附加质量；X、Y、N为船船舶在纵向、横向上的受力及绕Oz轴转动的力矩；下标H、P、R、wind、wave与current的分别代表船体、桨、舵、风、浪、流，表示力或力矩的种类；I_zz为绕Oz轴的转动惯量；J_zz为绕Oz轴的附加转动惯量。

为了使问题简化暂不考虑环境中的扰动力，不失一般性，对(2)式进行变换，获得如下形式的分别以三个自由度位置坐标为因变量的二阶微分方程组：

船体各个方向上的附加质量可用相应的经验公式计算，可以表达为船体外形尺度的函数，因此对于某个固定的船，附加质量是常数。船体所受到的流体力用贵岛模型来计算。

基于速度矢量的船舶避碰

在以下分析中，所有船舶以及障碍物都简化为圆形。通过相对速度的概念把物体的相对运动转化为本船以相对速度相对于静止障碍物的避障问题。由碰撞预测模型可知，避障过程就是使相对速度V_SO移出碰撞区的过程。由于障碍物速度是不可控的，只有改变本船速度来控制合速度。其核心问题就是在速度矢量坐标系(随船坐标系)求解能使相对速度V_SO移出碰撞区的V_S的期望速度。即，船舶避碰的策略就是求解能使相对速度V_SO移出碰撞区的V_S的期望速度方向θr。

避碰的输入量包括目标船的位置信息和速度、其他静态障碍物的位置和本船的位置信息与速度。目标船的相关信息通过AIS来获取，其他障碍物，如礁石或浮标，本文通过搜索矢量电子海图获取本船周边的部分环境信息。通过船上现有设备实现了船舶避碰环境的构建。避碰流程如图3所示。

单避让目标情形

对于船舶与单运动障碍物O，分别计算出PO、RCC及VO，则避碰条件为：且(即V_S的末端点位于ACC之外)，如图4所示。

设k时刻，本船S向障碍物O两侧切线方向发射射线l_NO和l_MO，l_NO和l_MO与x轴的夹角分别为θ₁和θ₂；本船S和障碍物O的速度分别为和两者相对速度为 与x轴的夹角θ_a，避让障碍物所需要的相对速度的方向为θ_rr。则由图4，本船S避让障碍物O所需要的相对速度的方向θ_rr应满足：

0＜θ_rr＜θ₁或θ₂＜θ_rr＜2π

取其边界值为本船k时刻避让障碍物所需要的相对速度的方向θ_rr，则：

判断出所需的θrr后，对本船S、障碍物O的速度进行x,y方向分解，分别得到V_SX、V_SY、V_OX和V_OY，则：

根据速度三角形求解本船k时刻避让障碍物所需要的真实速度方向。

多避让目标情形

在一定水域内随机分布着多个障碍物时，这些障碍物碰撞危险区域可能互相重叠，或者是一个面积较大、覆盖区域较广的障碍物无法直接进行膨化处理，那么可以将该障碍物视作多个小障碍物重叠紧密相连，如图5所示。

船舶针对区域内的所有障碍物分别作切线，其切线l₁,l₂...l_2n与x轴的夹角分别为θ₁,θ₂…θ_2n，n＝1，2…船舶每隔一段较短的时间都要重新计算θ_a,θ₁,θ₂……θ_2n；如果有：

|θ₂-θ₁|∪|θ₃-θ₂|∪…∪|θ_2n-θ_2n-1|

＝|θ_2n-θ₁|

那么就称多个障碍物紧密重叠相连。

对于船舶与多运动障碍物{O₁、O₂...O_n}，分别计算出O_i(i＝1,2,3...n)对应的PO_i、ACC_i及VO_i，则船舶与多个运动障碍物的避碰条件为：且(即V_R的末端点位于MACC之外)，其中：

设本船k时刻避让障碍物所需要的相对速度θ_rr，取边界值为本船k时刻避让障碍物所需要的相对速度θ_rr，则由图5，船舶避让障碍物所需要的相对速度θ_rr应满足：

判断出所需的θ_rr后，根据速度三角形求解本船k时刻避让障碍物所需要的绝对速度方向θ_r。

前面已经求出了相对速度的方向θ_rr，求出相对速度的方向后，才能解算真实的方向

其中，s是相对速度方向的正切值，在每一秒固定，假如相对速度方向固定，反过来要求本船的方向，就要求解期望速度方向θ_r，所以变形上式，可得：

令

继续整理推导上式，全部化成cosθ_r的二次方程，求解关于它的二次方程，在求反余弦函数。

V²(1-n)＝[m(Vn-v)+u]²

V²(1-n)＝m²(Vn-v)²+u²+2mu(Vn-v)

V²(1-n)＝m²(V²n²+v²-2Vnv)+u²+2mu(Vn-v)

0＝m²V²n²+m²V²-2m²Vnv+u²+2muVn-2mun-V²+V²n

m²V²n²+n(-2m²Vv+2muV+V²)+m²V²+u²-2muv-V²＝0

求根公式：

仿真实验

船舶自动舵控制器

由速度障碍计算得到的期望速度方向θ_r后，采用离散化的PID算法对本船转向避碰进行自动操舵，控制流程如图6所示。

图6中，离散化PID算法构成为：

其中，k表示计算时间；e_θ为由速度障碍计算得到的期望速度方向θ_r与实测本船当前速度方向θ的偏差，即：

e_θ＝e_r-θ

经整定后，本文取KP＝2.1，KI＝0.01，KD＝0.9。

场景设置及仿真结果分析

仅有动态多目标船的场景

图7(a)图7(b)所示为有多个动态目标，且有障碍较为密集情景下的船舶避碰。设置前方三艘，后方两艘来船，均朝向本船航行。选取本船的参数：m＝55000t、 l＝280m、b＝35.5m、d＝9.45m，初速度10m/s，方向0°。其中m为质量，l 为船长，b为船舶型宽，d为船舶型深。对本船以(1)～(3)式建立其运动模型。选取目标船1#、2#、3#、4#、5#，其初始运动参数分别为：初速度5m/s，方向 180°；初速度6m/s，方向180°；初速度7m/s，方向180°；初速度7m/s，方向0°；初速度7m/s，方向0°。目标船匀速直线运动。试验中，膨化范围取为 300m。

图7(a)为初始场景，图7(b)为避让过程。观察到，本船较好地避开了多条来船，并对前方较为密集的障碍物采取了大角度转弯避碰的策略，较好地避开了障碍物，保证了安全。

同时有静态障碍物和动态多目标船的场景

图8(a)图8(b)所示，场景中既有礁石作为静态障碍物，又有来船作为动态障碍物。选取本船的参数：m＝55000t、l＝280m、b＝35.5m、d＝9.45m，初速度10m/s，方向0°。其中m为质量，l为船长，b为船舶型宽，d为船舶型深。对本船以(1)～(3)式建立其运动模型。选取目标船运动速度为5m/s，方向 180°。目标船匀速直线运动，并设置礁石两处。试验中，膨化范围取为300m。

图8(a)为初始场景，图8(b)为一段时间后的避让结果。结果表明船舶对同时有静态和动态障碍物的避碰效果也较理想，在障碍物不密集的时候，采取从两障碍物之间穿越的策略，在保证安全的同时，航行的路径较短达到了预期目标。

本发明将速度障碍方法应用于船舶避碰领域，深入分析多目标船下船舶避碰的约束条件及操纵要求，构建以速度障碍为核心的基于速度矢量坐标系的多目标船情形下的船舶自动避碰数学模型，并提出本船转向避碰的操纵方案。仿真结果表明，该方法能够实现多目标船情形下船舶的自动避碰，本文所建立的基于速度矢量坐标系的多目标船自动避碰问题模型及求解的转向避碰操纵方案是可行的。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (2)

1.一种基于速度矢量坐标系的船自动避碰方法，其特征在于，包括以下步骤：

(1)、以某一船舶作为质点，获取该船舶周围的障碍物信息；

(2)、通过碰撞预测模型判断船舶与周围障碍物是否有碰撞危险，若存在危险，则进入步骤(3)，若不存在危险，则返回步骤(1)；所述通过碰撞预测模型判断船舶与周围障碍物是否有碰撞危险的方法为：

当本船舶处于单目标障碍物情形时：

在全局坐标系XOY中，船舶S位于点(X_S,Y_S)，速度为V_S；O视为运动障碍物，位于点(X_O,Y_O)，速度为V_O，首先将船舶S模型化为一个质点，然后将障碍物O根据船舶S的大小进行膨化，其膨化后的半径扩展为R_O，并且使得膨化后的障碍物边界为安全区域，称膨化障碍物O为船舶S的一个位置障碍PO，l_MO和l_NO是船舶S与PO两侧切线方向的射线，D_SO为l_SO方向上测量到的S与O之间的距离，定义S和O的相对速度为V_SO＝V_S-V_O，则通过相对速度可以把O当作静止障碍物，S的速度则看作为V_SO，如果V_SO保持不变，l_SO为其方向上的射线，则船舶将和障碍物O发生碰撞的条件为：

使上述条件成立的相对速度V_SO的集合，定义为速度空间中的相对碰撞区RCC：

即射线l_MO和l_NO之间的区域，对于本船的任一相对速度V_SO，如果V_SO∈RCC，则船舶将与障碍物O发生碰撞；

把RCC平移V_o后得到的区域称为绝对碰撞区域ACC：其中表示闵可夫斯基矢量和运算，则V_S的末端点位于ACC等价于V_SO∈RCC，所以当V_S的末端点位于ACC时，船舶将与障碍物O发生碰撞，ACC表示S与O会发生碰撞的速度V_SO的集合，称为S对O的速度障碍VO：

VO(V_O)＝{V_S|(V_S-V_O)∈RCC}

在多目标标障碍物情形时，设障碍物O₁的速度为V_O1，相应的障碍速度为OV₁；障碍物O₂的速度为V_O2，相应的速度障碍为OV₂，则V_S位于ACC₁和ACC₂之中，即V_S∈OV₁且V_S∈OV₂，如果在接下来的时刻V_S保持不变，则S将与O₁、O₂发生碰撞；

(3)、在速度矢量坐标系下，解算该船舶能够移出碰撞区域的相对速度，根据相对速度求解出船舶的绝对速度；设k时刻，本船S向障碍物O两侧切线方向发射射线l_NO和l_MO，l_NO和l_MO与x轴的夹角分别为θ₁和θ₂；本船S和障碍物O的速度分别为和两者相对速度为 与x轴的夹角θ_a，避让障碍物所需要的相对速度的方向为θ_rr，本船S避让障碍物O所需要的相对速度的方向θ_rr应满足：

0＜θ_rr＜θ₁或θ₂＜θ_rr＜2π

取其边界值为本船k时刻避让障碍物所需要的相对速度的方向θ_rr，则：

根据速度三角形求解本船k时刻避让障碍物所需要的绝对速度方向；

(4)、利用船舶的绝对速度在自动舵控制器下调整船舶的航向，直到船舶的当前速度为所述绝对速度；

(5)、反复循环步骤(1)-步骤(4)，以实现基于速度矢量坐标系的船自动避碰。

2.根据权利要求1所述的基于速度矢量坐标系的船自动避碰方法，其特征在于，所述步骤(3)包括：

船舶针对区域内的所有障碍物分别作切线，其切线l₁,l₂…l_2n与x轴的夹角分别为θ₁,θ₂…θ_2n，n＝1，2…船舶每隔一段较短的时间都要重新计算θ_a,θ₁,θ₂……θ_2n；

设本船k时刻避让障碍物所需要的相对速度θ_rr，取边界值为本船k时刻避让障碍物所需要的相对速度θ_rr，则船舶避让障碍物所需要的相对速度θ_rr应满足：

判断出所需的θ_rr后，根据速度三角形求解本船k时刻避让障碍物所需要的绝对速度方向。