CN111186549A - 一种具有船舶避碰功能的航向自动舵控制系统 - Google Patents

Abstract

本发明提供一种具有船舶避碰功能的航向自动舵控制系统，其特征在于，包括：通过船载设备获取船舶的当前状态信息，计算最近会遇距离DCPA和到达最近会遇点时间TCPA的动态信息，结合本船和他船的航向及速度信息建立碰撞风险模型用于评估碰撞风险度的碰撞风险评估模块，用于船舶避碰操作的航向变化量且基于国际海上避碰规则COLREGS的避碰算法模块以及船舶自动驾驶航向控制模块。本发明所提供的航向自动舵设计可以在航向控制的基础上，实现两船间的自动避碰，减轻了船舶驾驶员的操舵负担。本发明将最近会遇距离(DCPA)和达到最近会遇点时间(TCPA)与船舶的速度、航向等信息相结合，动态评估船舶的碰撞风险，提高了船舶对碰撞风险的评估准确度。

Description

一种具有船舶避碰功能的航向自动舵控制系统

技术领域

本发明涉及自动舵控制方法技术领域，具体而言，尤其涉及一种具有船舶避碰功能的航向自动舵控制方法。

背景技术

中国专利CN 110164185A公开了一种基于AIS的船舶避碰系统。该发明中的船舶避碰系统主要依靠船载AIS(Automatic Identification System)设备采集的船舶位置、环境信息，通过计算到达最近会遇点的时间和最近会遇点的信息判断船舶之间是否会发生碰撞。然后给船舶驾驶人员一个预警信息，由驾驶员操纵船舶实现避障。现有的AIS船舶避碰系统因为用于评估碰撞风险度的信息过于单一，实际航行中可能会出现对他船的误判，从而给出一个错误的预警信息。并且因为该系统只能给出船舶发生碰撞的预警信息，具体的船舶避障行为仍需由驾驶员进行操纵。随着船舶自动驾驶技术的发展，这种方式存在着明显的局限性。

发明内容

根据上述提出的技术问题，而提供一种具有船舶避碰功能的航向自动舵控制方法。本发明主要利用一种具有船舶避碰功能的航向自动舵控制系统，其特征在于，包括：通过船载设备获取船舶的当前状态信息，计算最近会遇距离DCPA和到达最近会遇点时间TCPA的动态信息，结合本船和他船的航向及速度信息建立碰撞风险模型用于评估碰撞风险度的碰撞风险评估模块，用于船舶避碰操作的航向变化量且基于国际海上避碰规则COLREGS的避碰算法模块以及船舶自动驾驶航向控制模块。

进一步地，所述船舶风险评估模块通过船载传感器获取本船和他船的位置信息，则所述他船相对所述本船的坐标和两船之间的距离为：

ΔX_(t)＝X_T(t)-X_(t)

ΔY_(t)＝Y_T(t)-Y_(t)；

其中，(X_T(t),Y_T(t))为他船当前位置坐标，(X_(t),Y_(t))为本船当前位置坐标；根据所述他船相对所述本船的坐标，则所述他船的真方位α_(t)为：

式中，ΔX_(t)表示两船之间的北向位置误差；ΔY_(t)表示两船之间的东向位置误差；Δα表示需要调节的航向变化量；

设他船和本船的速度分别为V_T和V，则他船相对本船速度在X轴，Y轴上的分量V_X(t)、V_Y(t)分别为：

V_X(t)＝V_T cosψ_T-V cosψ

V_Y(t)＝V_T sinψ_T-V sinψ；

则两船相对速度V_R(t)大小为：

通过相对速度在X轴，Y轴上的分量，计算相对速度方位ψ_R(t)为：

则最近会遇点距离DCPA和到达最近会遇点时间TCPA表示为：

DCPA＝R_(t)sin(ψ_R(t)-α_(t)-π)

TCPA＝R_(t)cos(ψ_R(t)-α_(t)-π)/V_R(t)；

四元数船舶领域由下述表达式给出：

QSD_p＝{(x,y)|f_p(x,y；Q)≤1,Q＝{R_fore,R_aft,R_starb,R_port},p≥1}；

其中，(x,y)表示障碍物相对本船的位置，R_fore、R_aft表示四元船舶领域的前、后半径，R_starb、R_port表示四元船舶领域的右、左半径，f(·)表示边界函数，即：

其中，p表示船舶领域参数，决定船舶领域的形状，1≤p；当p＝1时，则船舶域的边界是直线；当p＞1时，则船舶域的边界是曲线；参数Q表示船舶四元数领域参数，决定船舶领域的大小，Q表示为：

其中，L表示船舶的长度，A_D和D_T分别表示船舶的进距和回转的直径，K_AD和K_DT分别表示进距和回转直径与船长的比值，V表示本船的速度则：

基于四元数船舶领域，船舶碰撞危险度模型被定义为：

其中，CR表示碰撞危险度；r₀表示调节四元船舶领域半径的参数，决定四元船舶领域的四个半径，当碰撞危险度CR随着r₀的增加而增加；通过船舶碰撞危险度模型，计算船舶碰撞危险度，当危险高于阈值时，船舶通过避障算法计算船舶的新安全航向，当风险低于阈值时，船舶保持既定的航向。

更进一步地，所述基于COLREGS的避碰算法模块根据本船的动态信息，当所述DCPA小于最小安全会遇距离，则需要改变航线，则通过二分法在区间[Δψ_min,Δψ_max]中得出船舶航向变化量为Δψ，其中，Δψ_min＝-180°,Δψ_max＝180°。

当所述DCPA大于最小安全会遇距离，则本船保持当前的航向继续航行，即Δψ＝0。

更进一步地，所述基于MPC的船舶航向控制模块的船舶运动模型为：

其中，x＝[ψ r]^T，

ψ表示船舶航向，δ表示输入舵角，r表示舵角变化率，K、T表示船舶模型参数。

通过给定的采样周期，可将该连续空间状态模型转化为离散时间状态空间模型：

x(k+1)＝Ax(k)+Bδ(k)

y(k)＝Cx(k)；

其中，A∈R^2×2,B∈R^2×1,C∈R^2×1分别表示

离散后结果，k表示采样时刻；在k时刻，通过预测模型和当前的状态信息，系统未来的状态信息可通过下式计算得出：

x(k+1|k)＝Ax(k)+Bδ(k)

x(k+2|k)＝Ax(k+1|k)+Bδ(k+1)

＝A²x(k)+ABδ(k)+Bδ(k+1)

其中，N_p表示预测时域，N_c表示控制时域；

当前采样时刻k处已知的状态信息x(k)和舵角控制输入u(k)输入到确定的预测模型x(k+1)＝Ax(k)+Bδ(k)中依次计算获取系统中未来预测时域中的状态量为：

y(k+1|k)＝CAx(k)+CBδ(k)

y(k+2|k)＝CAx(k+1|k)+CBδ(k+1)

＝CA²x(k)+CABδ(k)+CBδ(k+1)

定义输出向量Y，控制输入向量U如下：

将预测输出转换为紧凑形式为：

Y＝Fx(k)+ΦU；

其中

考虑能量消耗的优化控制问题，将船舶的航向控制问题转化为考虑能量消耗的优化控制问题，则目标代价函数为：

J＝(R_s-Y)^TQ_W(R_s-Y)+U^TR_WU；

其中，(R_s-Y)^TQ_W(R_s-Y)表示对控制目标偏离的调节，U^TR_WU表示对能量消耗的调节，Q_W和R_W均表示权重矩阵；

其中，r(k)＝ψ+Δψ，表示船舶的新安全航向。

当考虑舵角输入的限制时：

δ_min≤δ(k+j)≤δ_max j＝0,1,…N_c-1,；

其中，δ_min和δ_max分别表示舵角的左、右最大幅度。

较现有技术相比，本发明具有以下优点：

1)本发明所提供的航向自动舵设计可以在航向控制的基础上，实现两船间的自动避碰，减轻了船舶驾驶员的操舵负担。

2)本发明将最近会遇距离(DCPA)和达到最近会遇点时间(TCPA)与船舶的速度、航向等信息相结合，动态评估船舶的碰撞风险，提高了船舶对碰撞风险的评估准确度。

3)在实际情况中，船舶舵角幅度会受到限制，而模型预测控制可以通过对控制输入的限制，很好地处理舵角限制问题。同时，模型预测控制的预测能力还可以减少船舶大惯性对航向控制的影响。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明整体系统结构示意图。

图2为本发明本船的航向与舵角示意图。

图3为本发明两船的实际位置与相对位置的运动轨迹图。

图中：ψ表示本船当前航向；ψ_T表示他船当前航向；α表示他船相对本船的真实方位；R表示两船之间的距离；V表示本船速度；VT表示他船速度。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

如图1-3所示，本发明提供了一种具有船舶避碰功能的航向自动舵控制系统，包括：通过船载设备获取船舶的当前状态信息，计算最近会遇距离DCPA和到达最近会遇点时间TCPA的动态信息，结合本船和他船的航向及速度信息建立碰撞风险模型用于评估碰撞风险度的碰撞风险评估模块，用于船舶避碰操作的航向变化量且基于国际海上避碰规则COLREGS的避碰算法模块以及船舶自动驾驶航向控制模块。

作为本申请的一种优选的实施方式，在本申请中，所述船舶风险评估模块通过船载传感器获取本船和他船的位置信息，则所述他船相对所述本船的坐标和两船之间的距离为：

ΔX_(t)＝X_T(t)-X_(t)

ΔY_(t)＝Y_T(t)-Y_(t)；

其中，(X_T(t),Y_T(t))为他船当前位置坐标，(X_(t),Y_(t))为本船当前位置坐标；根据所述他船相对所述本船的坐标，则所述他船的真方位α_(t)为：

式中，ΔX_(t)表示两船之间的北向位置误差；ΔY_(t)表示两船之间的东向位置误差；Δα表示需要调节的航向变化量。

作为优选的，设他船和本船的速度分别为V_T和V，则他船相对本船速度在X轴，Y轴上的分量V_X(t)、V_Y(t)分别为：

V_X(t)＝V_Tcosψ_T-Vcosψ

V_Y(t)＝V_Tsinψ_T-Vsinψ；

则两船相对速度V_R(t)大小为：

通过相对速度在X轴，Y轴上的分量，计算相对速度方位ψ_R(t)为：

则最近会遇点距离DCPA和到达最近会遇点时间TCPA表示为：

DCPA＝R_(t)sin(ψ_R(t)-α_(t)-π)

TCPA＝R_(t)cos(ψ_R(t)-α_(t)-π)/V_R(t)；

四元数船舶领域由下述表达式给出：

QSD_p＝{(x,y)|f_p(x,y；Q)≤1,Q＝{R_fore,R_aft,R_starb,R_port},p≥1}；

其中，(x,y)表示障碍物相对本船的位置，R_fore、R_aft表示四元船舶领域的前、后半径，R_starb、R_port表示四元船舶领域的右、左半径，f(·)表示边界函数，即：

其中，p表示船舶领域参数，决定船舶领域的形状，1≤p；当p＝1时，则船舶域的边界是直线；当p＞1时，则船舶域的边界是曲线；参数Q表示船舶四元数领域参数，决定船舶领域的大小，Q表示为：

其中，L表示船舶的长度，A_D和D_T分别表示船舶的进距和回转的直径，K_AD和K_DT分别表示进距和回转直径与船长的比值，V表示本船的速度则：

基于四元数船舶领域，船舶碰撞危险度模型被定义为：

其中，CR表示碰撞危险度；r₀表示调节四元船舶领域半径的参数，决定四元船舶领域的四个半径，当碰撞危险度CR随着r₀的增加而增加；通过船舶碰撞危险度模型，计算船舶碰撞危险度，当危险高于阈值时，船舶通过避障算法计算船舶的新安全航向，当风险低于阈值时，船舶保持既定的航向。

在本申请中，作为一种优选的实施方式，在本申请中基于COLREGS的避碰算法模块根据本船的动态信息，当所述DCPA小于最小安全会遇距离，则需要改变航线，则通过二分法在区间[Δψ_min,Δψ_max]中得出船舶航向变化量为Δψ，其中，Δψ_min＝-180°,Δψ_max＝180°。

当所述DCPA大于最小安全会遇距离，则本船保持当前的航向继续航行，即Δψ＝0。

在本实施方式中，所述基于MPC的船舶航向控制模块的船舶运动模型为：

其中，x＝[ψ r]^T，

ψ表示船舶航向，δ表示输入舵角，r表示舵角变化率，K、T表示船舶模型参数。

通过给定的采样周期，可将该连续空间状态模型转化为离散时间状态空间模型：

x(k+1)＝Ax(k)+Bδ(k)

y(k)＝Cx(k)；

其中，A∈R^2×2,B∈R^2×1,C∈R^2×1分别表示

离散后结果，k表示采样时刻；在k时刻，通过预测模型和当前的状态信息，系统未来的状态信息可通过下式计算得出：

x(k+1|k)＝Ax(k)+Bδ(k)

x(k+2|k)＝Ax(k+1|k)+Bδ(k+1)

＝A²x(k)+ABδ(k)+Bδ(k+1)

其中，N_p表示预测时域，N_c表示控制时域；

当前采样时刻k处已知的状态信息x(k)和舵角控制输入u(k)输入到确定的预测模型x(k+1)＝Ax(k)+Bδ(k)中依次计算获取系统中未来预测时域中的状态量为：

y(k+1|k)＝CAx(k)+CBδ(k)

y(k+2°k)＝CAx(k+1|k)+CBδ(k+1)

＝CA²x(k)+CABδ(k)+CBδ(k+1)

定义输出向量Y，控制输入向量U如下：

将预测输出转换为紧凑形式为：

Y＝Fx(k)+ΦU；

其中

考虑能量消耗的优化控制问题，将船舶的航向控制问题转化为考虑能量消耗的优化控制问题，则目标代价函数为：

J＝(R_s-Y)^TQ_W(R_s-Y)+U^TR_WU；

其中，(R_s-Y)^TQ_W(R_s-Y)表示对控制目标偏离的调节，U^TR_WU表示对能量消耗的调节，Q_W和R_W均表示权重矩阵；

其中，r(k)＝ψ+Δψ，表示船舶的新安全航向。

当考虑舵角输入的限制时：

δ_min≤δ(k+j)≤δ_max j＝0,1,…N_c-1,；

其中，δ_min和δ_max分别表示舵角的左、右最大幅度。

图2为在计算机MATLAB的仿真环境中，采用本发明的航向控制器的本船与他船交叉相遇时的避碰情形。本船和他船的初始状态信息如表1中所示。可明显地看出，若不采取操作，两条船舶将会发生碰撞。

图2是在应用了本发明的航向控制器后本船的航向及舵角变化信息，可看出本船在400s的时候航向发生了改变，此改变是为了躲避他船做出的避碰行动。在图3中清晰的展示出了两船的运动轨迹图。本发明的航向控制器提供了从碰撞风险评估到避碰行动规划再到船舶运动控制的一套技术方案。

表1

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

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

Claims (3)

1.一种具有船舶避碰功能的航向自动舵控制系统，其特征在于，包括：

通过船载设备获取船舶的当前状态信息，计算最近会遇距离DCPA和到达最近会遇点时间TCPA的动态信息，结合本船和他船的航向及速度信息建立碰撞风险模型用于评估碰撞风险度的碰撞风险评估模块，用于船舶避碰操作的航向变化量且基于国际海上避碰规则COLREGS的避碰算法模块以及船舶自动驾驶航向控制模块；

所述船舶风险评估模块通过船载传感器获取本船和他船的位置信息，则所述他船相对所述本船的坐标和两船之间的距离为：

ΔX_(t)＝X_T(t)-X_(t)

ΔY_(t)＝Y_T(t)-Y_(t)；

其中，(X_T(t),Y_T(t))为他船当前位置坐标，(X_(t),Y_(t))为本船当前位置坐标；根据所述他船相对所述本船的坐标，则所述他船的真方位α_(t)为：

式中，ΔX_(t)表示两船之间的北向位置误差；ΔY_(t)表示两船之间的东向位置误差；Δα表示需要调节的航向变化量；

设他船和本船的速度分别为V_T和V，则他船相对本船速度在X轴，Y轴上的分量V_X(t)、V_Y(t)分别为：

V_X(t)＝V_Tcosψ_T-Vcosψ

V_Y(t)＝V_Tsinψ_T-Vsinψ；

则两船相对速度V_R(t)大小为：

通过相对速度在X轴，Y轴上的分量，计算相对速度方位ψ_R(t)为：

则最近会遇点距离DCPA和到达最近会遇点时间TCPA表示为：

DCPA＝R_(t)sin(ψ_R(t)-α_(t)-π)

TCPA＝R_(t)cos(ψ_R(t)-α_(t)-π)/V_R(t)；

四元数船舶领域由下述表达式给出：

QSD_p＝{(x,y)|f_p(x,y；Q)≤1,Q＝{R_fore,R_aft,R_starb,R_port},p≥1}；

其中，(x,y)表示障碍物相对本船的位置，R_fore、R_aft表示四元船舶领域的前、后半径，R_starb、R_port表示四元船舶领域的右、左半径，f(·)表示边界函数，即：

其中，p表示船舶领域参数，决定船舶领域的形状，1≤p；当p＝1时，则船舶域的边界是直线；当p＞1时，则船舶域的边界是曲线；参数Q表示船舶四元数领域参数，决定船舶领域的大小，Q表示为：

其中，L表示船舶的长度，A_D和D_T分别表示船舶的进距和回转的直径，K_AD和K_DT分别表示进距和回转直径与船长的比值，V表示本船的速度则：

基于四元数船舶领域，船舶碰撞危险度模型被定义为：

其中，CR表示碰撞危险度；r₀表示调节四元船舶领域半径的参数，决定四元船舶领域的四个半径，当碰撞危险度CR随着r₀的增加而增加；通过船舶碰撞危险度模型，计算船舶碰撞危险度，当危险高于阈值时，船舶通过避障算法计算船舶的新安全航向，当风险低于阈值时，船舶保持既定的航向。

2.根据权利要求1所述的一种具有船舶避碰功能的航向自动舵控制系统，其特征在于：

所述基于COLREGS的避碰算法模块根据本船的动态信息，当所述DCPA小于最小安全会遇距离，则需要改变航线，则通过二分法在区间[Δψ_min,Δψ_max]中得出船舶航向变化量为Δψ，其中，Δψ_min＝-180°,Δψ_max＝180°。

当所述DCPA大于最小安全会遇距离，则本船保持当前的航向继续航行，即Δψ＝0。

3.根据权利要求1所述的一种具有船舶避碰功能的航向自动舵控制系统，其特征在于：

所述基于MPC的船舶航向控制模块的船舶运动模型为：

其中，x＝[ψ r]^T，

ψ表示船舶航向，δ表示输入舵角，r表示舵角变化率，K、T表示船舶模型参数；

通过给定的采样周期，可将该连续空间状态模型转化为离散时间状态空间模型：

x(k+1)＝Ax(k)+Bδ(k)

y(k)＝Cx(k)；

其中，A∈R^2×2,B∈R^2×1,C∈R^2×1分别表示

离散后结果，k表示采样时刻；在k时刻，通过预测模型和当前的状态信息，系统未来的状态信息可通过下式计算得出：

其中，N_p表示预测时域，N_c表示控制时域；

当前采样时刻k处已知的状态信息x(k)和舵角控制输入u(k)输入到确定的预测模型x(k+1)＝Ax(k)+Bδ(k)中依次计算获取系统中未来预测时域中的状态量为：

定义输出向量Y，控制输入向量U如下：

将预测输出转换为紧凑形式为：

Y＝Fx(k)+ΦU；

其中

考虑能量消耗的优化控制问题，将船舶的航向控制问题转化为考虑能量消耗的优化控制问题，则目标代价函数为：

J＝(R_s-Y)^TQ_W(R_s-Y)+U^TR_WU；

其中，(R_s-Y)^TQ_W(R_s-Y)表示对控制目标偏离的调节，U^TR_WU表示对能量消耗的调节，Q_W和R_W均表示权重矩阵；

其中，r(k)＝ψ+Δψ，表示船舶的新安全航向。

当考虑舵角输入的限制时：

δ_min≤δ(k+j)≤δ_max j＝0,1,…N_c-1,；

其中，δ_min和δ_max分别表示舵角的左、右最大幅度。

Images