基于速度障碍的碰撞预测模型
[0048] 单避让目标情形
[0049] 如图1所示，全局坐标系X0Y中，船舶S位于点（Xs，Ys)，绝对速度为Vs;0视为运 动障碍物，位于点（H)，速度为V。。为了简化，将船舶及障碍物（此处以目标船为例） 看作为圆形进行分析。首先将船舶S模型化为一个质点，然后将障碍物0根据船舶S的大 小进行"膨化"，其半径扩展为R。，所述R。一般取本船的长度，若本船较小，一般取300m，并 且使得"膨化"后的障碍物边界为安全区域。称"膨化"障碍物〇为船舶S的一个位置障碍 (PositionObstacle，P0)，1M。和1N。是船舶S与P0两侧切线方向的射线，Ds。为1 s。方向上 测量到的S与0之间的距离。
[0050] 定义S和0的相对速度：
[0051] Vso=Vs-V〇
[0052] 则通过相对速度可以把0当作静止障碍物，S的速度则看作为Vs。。如果Vs。保持 不变，ls。为其方向上的射线，则船舶将和障碍物〇发生碰撞的条件为：
[0053]
[0054] 使上述条件成立的相对速度Vs。的集合，定义为速度空间中的相对碰撞区 (Relative Collision Cone, RCC)：
[0055]
[0056] 即图1中射线1M。和1N。之间的区域，对于本船的任一相对速度Vs。，如果VS(]GRCC， 则船舶将与障碍物〇发生碰撞。
[0057] 把RCC平移V。后得到的区域称为绝对碰撞区域（AbsoluteCollisionCone，ACC):
[0058] ACC=RCC@V〇
[0059] 其中纖表示闵可夫斯基矢量和运算。
[0060] 可以看出，Vs的末端点位于ACC等价于Vs。GRCC，所以当Vs的末端点位于ACC时， 船舶将与障碍物0发生碰撞。ACC表示S与0会发生碰撞的速度Vs。的集合，称为S对0的 速度障碍（VelocityObstacle，V0):
[0061] VO(V〇) = {Vs| (Vs-V〇)GRCC}
[0062] 多目标避让情形
[0063] 图2为船舶S遇到多运动障碍物（其他船舶）的一个例子，其中仏的速度为VC1， 相应的障碍速度为(^1;02的速度为，相应的速度障碍为0V2。可以看出Vs位于ACCJP ACC2之中（即VSG〇V卫VSG〇V2)，如果在接下来的时刻％保持不变，则S将与0 ^ 02发 生碰撞。
[0064] 速度矢量坐标系下的船舶自动避碰方案
[0065] 船舶运动数学模型
[0066] 船舶运动规律与传统机器人或车辆有较大区别，难以像移动机器人那样急停急 转，其紧急刹车性能和快速转艏性能严重弱于移动机器人，控制时滞性严重，避碰过程中 必须要考虑船舶的运动特性。
[0067] 船舶避碰主要考虑船舶的水面运动，所以需要建立本船前进、横移、转首三个自由 度上的运动方程。基于分离模型（MMG)的思想可以分别计算力和力矩，推导得到船舶三自 由度动力学方程为[9]:
[0068]
[0070] 式中：x、y、篆为大地坐标系下的横向坐标、纵向坐标、船舶航向；u、v、r分别为纵 向、横向和垂向的速度及绕〇z轴的角速度；m为船舶质量；mx、my、mz分别为纵向、横向和垂 向船舶的附加质量；X、Y、N为船船舶在纵向、横向上的受力及绕0z轴转动的力矩；下标H、 P、R、wind、wave与current的分别代表船体、桨、舵、风、浪、流，表示力或力矩的种类；IZZS 绕〇z轴的转动惯量Jzz为绕0z轴的附加转动惯量。
[0071] 为了使问题简化暂不考虑环境中的扰动力，不失一般性，对（2)式进行变换，获得 如下形式的分别以三个自由度位置坐标为因变量的二阶微分方程组：
[0072]
[0073] 船体各个方向上的附加质量可用相应的经验公式计算，可以表达为船体外形尺度 的函数，因此对于某个固定的船，附加质量是常数。船体所受到的流体力用贵岛模型来计 算。
[0074] 基于速度矢量的船舶避碰
[0075] 在以下分析中，所有船舶以及障碍物都简化为圆形。通过相对速度的概念把物体 的相对运动转化为本船以相对速度相对于静止障碍物的避障问题。由碰撞预测模型可知， 避障过程就是使相对速度Vs。移出碰撞区的过程。由于障碍物速度是不可控的，只有改变本 船速度来控制合速度。其核心问题就是在速度矢量坐标系（随船坐标系）求解能使相对速 度Vs。移出碰撞区的Vs的期望速度。即，船舶避碰的策略就是求解能使相对速度Vs。移出碰 撞区的期望速度方向0p
[0076] 避碰的输入量包括目标船的位置信息和速度、其他静态障碍物的位置和本船的位 置信息与速度。目标船的相关信息通过AIS来获取，其他障碍物，如礁石或浮标，本文通过 搜索矢量电子海图获取本船周边的部分环境信息。通过船上现有设备实现了船舶避碰环境 的构建。避碰流程如图3所示。
[0077] 单避让目标情形
[0078] 对于船舶与单运动障碍物0,分别计算出P0、RCC及V0,则避碰条件为： A =: 0且心贫剛（即Vs的末端点位于ACC之外），如图4所示。
[0079]设k时刻，本船S向障碍物0两侧切线方向发射射线1_和1M。，1N。和1M。与x 轴的夹角分别为^和Q2;本船S和障碍物0的速度分别为^^和^^两者相对速度为 & A，&与X轴的夹角ea。避让障碍物所需要的相对速度的方向为。则由图 4,本船S避让障碍物0所需要的相对速度的方向0 "应满足：
[0080] 0< 0 rr< 0 !或 0 2< 0 rr< 2JT
[0081] 取其边界值为本船k时刻避让障碍物所需要的相对速度0"，则：
[0082]
[0083]判断出所需的0"后，对本船S、障碍物0的速度进行x，y方向分解，分别得到v sx、 Vsy、vjnVoy，则：
[0084]
[0085] 根据速度三角形求解本船k时刻避让障碍物所需要的真实速度方向。
[0086] 多避让目标情形
[0087] 在一定水域内随机分布着多个障碍物时，这些障碍物碰撞危险区域可能互相重 叠，或者是一个面积较大、覆盖区域较广的障碍物无法直接进行膨化处理，那么可以将该障 碍物视作多个小障碍物重叠紧密相连，如图5所示。
[0088] 船舶针对区域内的所有障碍物分别作切线，其切线1:，1 2~12"与x轴的夹角分别 为01，02~02"，11=1，2~船舶每隔一段较短的时间都要重新计算0 3，01，0 2...... 0 2n〇 如果有：
[0089] | 02-0」U| 03-0 2|U…U| 02n-02nl
[0090] = | 0 2n- 0 工
[0091] 那么就称多个障碍物紧密重叠相连。
[0092] 对于船舶与多运动障碍物IA、02. . . 0n}，分别计算出(i= 1，2, 3. . .n)对应的 POpACC#V0 ^则船舶与多个运动障碍物的避碰条件为：= 0且匕gMFO(即 VR的末端点位于MACC之外），其中：
[0095] 设本船k时刻避让障碍物所需要的相对速度0"，取边界值为本船k时刻避让障 碍物所需要的相对速度9 "，则由图5,船舶避让障碍物所需要的相对速度0 "应满足：
[0096]
[0097] 判断出所需的0"后，根据速度三角形求解本船k时刻避让障碍物所需要的真实 速度方向L。
[0098] 前面已经求出了相对速度的方向0"，求出相对速度的方向后，才能解算真实的方 向
[0099]
[0100] 其中，s是相对速度方向的正切值，在每一秒固定，假如相对速度方向固定，反过来 要求本船的方向，就要求解期望速度方向L，所以变形上式，可得：
[0101] 4
[0102] 继续整理推导上式，全部化成cos0 ^的二次方程，求解关于它的二次方
[0103] 程，在求反余弦函数。
[0114] 仿真实验
[0115] 船舶自动舵控制器
[0116] 由速度障碍计算得到的期望速度方向0J^，采用离散化的