一种基于不确定速度障碍的智能船舶避碰路径规划方法

1.本发明属于智能船舶的路径规划领域，涉及一种基于不确定速度障碍的智能船舶避碰路径规划方法。

背景技术：

2.智能船舶的无碰撞局部路径规划具有实时性要求。现有技术中采用的基于吸引力和排斥力的势场方法用于局部路径规划，会存在陷入局部最小值的风险。当目标点附近有障碍物时，现有技术中提出了向量场直方图算法，然而，向量场直方图方法在复杂环境中难以达到鲁棒性。现有技术中还提出了速度空间方法，如动态窗口方法、速度障碍物及其变体，用于在线处理避碰问题。这些方法适用于真实和复杂的环境，并且可以通过在动态约束范围内选择可行的速度来避免速度空间中可能发生的碰撞。
3.然而，上述大部分现有技术的工作只关注路径的效率和可行性，并没有结合国际海上避碰规则(colregs)考虑路径规划。一些研究者提出了一些方法来改进上述工作，使船舶操纵服从colregs，例如模糊逻辑、区间规划和基于局部正态分布的轨迹算法。然而，这些方法难以应用于需要同时满足多个规则的多船相遇场景。为此，现有技术中还提出了一种改进的人工势场方法来处理涉及多艘船的情况，通过在相应的虚拟力中添加适当的功能和安全要求来实现colregs约束的行为。现有技术中还提出了基于模型预测控制的船舶避碰方法通过约束可选控制行为来满足colregs的要求。然而，在复杂的海洋环境中，决策系统所使用的获取信息往往是不确定和嘈杂的。这给导航系统设计带来了进一步的挑战。
4.虽然colregs制定了一般情况下如何机动以避免船舶碰撞的规则，但它们没有提供关于何时启动程序以及如何在操作层面机动的定量标准。同时，在多船相遇场景中，本船与多目标船的碰撞风险是不同的。在大多数研究中，实时船舶碰撞风险通常通过计算最近接近点(cpa)来监测，例如到最近接近点的距离(dcpa)，和/或到最近接近点的时间(tcpa)。还有些方法通过设置船舶安全领域，并通过船舶之间的冲突来定义碰撞风险。还有通过考虑到碰撞风险的不确定性，提出了一种漏斗库方法，它使机器人能够在具有复杂几何约束的环境中执行实时规划。模糊逻辑方法也广泛用于船舶之间的风险评估，通过考虑不同的影响参数作为风险的输入隶属函数来扩展基于规则的方法。但之前的方法中，大多数研究都没有明确有效地考虑本船与目标船之间的动态碰撞风险变化问题。

技术实现要素：

5.为了克服现有智能船舶研究中没有很好的统一运动规划中规则和风险的问题，本发明提出了一种基于不确定速度障碍的智能船舶避碰路径规划方法。本发明同时考虑碰撞风险、规则要求和传感器的不确定性，首先提出了基于输入模糊隶属函数的动态风险评估步骤，用于监控复杂场景的风险。然后使用风险值来激活避碰操作。然后，提出了符合colregs规则的基于不确定vo的动态路径规划步骤。此外，使用具有鲁棒性的漏斗思想系统地扩展相对锥角，以处理被测目标船中的概率不确定性。本发明方法不仅能处理智能船舶
运动规划的风险和不确定性，还能在满足《国际海上避碰规则》的基础下进行路径规划与避障。
6.本发明的技术方案如下：
7.本发明提供了一种基于不确定速度障碍的智能船舶避碰路径规划方法，其包括如下步骤：
8.s1.多船相遇场景的碰撞风险评估；
9.选择最近会遇时间t
cpa
、最近会遇距离d
cpa
、相对距离、相对方位和船速比作为本船与目标船碰撞风险最相关的参数，通过各参数的模糊隶属函数计算本船与任一目标船i之间的碰撞风险指数crii，将最大crii值与安全cri阈值进行比较，若高于阈值，则执行s2，否则持续进行碰撞风险评估；
10.s2.进行规则感知动态无碰撞路径规划，包括s21-s23；
11.s21.使用vo进行无碰撞路径规划
12.令下标o表示本船，i＝1，2，...，n表示n艘目标船；令po＝[xo，yo]
t
，pi＝[xi，yi]
t
表示本船和目标船在二维海洋表面上的位置，示本船和目标船在二维海洋表面上的位置，分别表示本船和目标船在二维海洋表面上的速度矢量，
[0013]
定义vo
o|i
是导致本船o与目标船i发生碰撞的速度集，则本船避免与目标船i发生碰撞的条件是
[0014][0015]
d(pi，ro+ri)是一个以pi为中心，半径为ro+ri的开圆盘，ro和ri分别是本船和目标船的半径；对于所有目标船i＝1，2，...，n，多船遭遇场景中设置的速度障碍物为本船必须以不在此设定范围内的速度航行以保证安全；
[0016]
s22.具有不确定vo的鲁棒无碰撞路径规划
[0017]
考虑目标船的速度和位置存在不确定性，设目标船的半径、速度和位置存在服从高斯分布的不确定性，其中是对应的平均值；为对应的方差或协方差矩阵；δ
·
表示对应的标准值；diag表示对角矩阵；s21中的确定性vo
o|i
扩展到不确定vo(uvo)为：
[0018][0019]
其中：
[0020][0021][0022][0023]
对于所有目标船i＝1，2，...，n，多船相遇场景中设置的不确定速度障碍物为本船以不在该联合集合内的速度航行保证航行安全；
[0024]
s23.规则感知下的鲁棒无碰撞路径规划
[0025]
在符合colregs规则约束下，规划路径，避免与其他船舶发生碰撞。
[0026]
与现有技术相比，本发明的有益效果包括：
[0027]
本发明提出的基于不确定速度障碍物的规则感知动态路径规划方法，解决了多船相遇场景的决策过程中的优先级问题。本发明实时计算船只的碰撞风险，以激活防撞策略。此外，考虑到交叉、超车和迎面的相遇场景，使用鲁棒管道的思想来处理目标船信息中的不确定性。在存在不确定性的情况下，在与多艘船的各种相遇场景中也实现了符合colregs的安全路径规划，该方法能使智能船舶在复杂海洋环境中做出安全决策并避免碰撞。在遵循colregs的各种复杂场景下，路径规划的碰撞风险和安全性得到了保证，这意味着在智能船舶中的实际应用潜力巨大。
附图说明
[0028]
图1是确定性速度障碍原理图；
[0029]
图1中，1-目标船1的位置，2-目标船1的速度，3-目标船1与本船的半径之和，4-目标船1的确定速度障碍，5-目标船2的位置，6-目标船2的速度，7-目标船2的确定速度障碍，8-本船位置，9-本船的速度，10-目标船1的平移速度，11-目标船2的平移速度。
[0030]
图2是本发明的不确定性速度障碍原理图。
[0031]
图2中，1-目标船1的位置，2-目标船1的位置不确定性，3-目标船1的半径不确定性方差，4-目标船1的不确定半径与本船半径之和，5-目标船1的速度不确定性，6-目标船1与本船的不确定速度障碍，7-目标船2的位置，8-目标船2的位置不确定性，9-目标船2的半径不确定性方差，10-目标船2的不确定半径与本船半径之和，11-目标船2的速度不确定性，12-目标船2与本船的不确定速度障碍，13-目标船1的平移速度，14-目标船2的平移速度，15-目标船2与本船的不确定速度障碍的锥点，16-目标船1与本船的不确定速度障碍的锥点，17-不确定速度障碍相应的左转速度可达集，18-不确定速度障碍相应的右转速度可达集，19-不确定速度障碍相应的远离目标的速度可达集。
[0032]
图3是colregs下的usv路径规划和超越场景下的不确定vo算法。图中左侧是目标船的不确定路径，右侧是本船的规划路径
[0033]
图4是坐标系下usv路径规划的相对距离图。(a)在不考虑不确定性的情况下，本船与目标船之间的距离。(b)考虑到目标船的不确定性，使用不确定性速度障碍方法计算自身船和目标船之间的距离。
[0034]
图5 colregs和不确定速度障碍算法下多目标船舶相遇场景1。三艘目标船船在半径、速度和位置上都存在不确定性。从上到下，左到右依次为：(a)四艘船的初始阶段；(b)超车情况下的操纵；(c)超车和对遇情况下的操纵；(d)本船和目标船各自的总体操纵路径。
[0035]
图6场景1中的实时碰撞风险指数。
[0036]
图7场景1中colregs下usv路径规划的相对距离。(a)在不考虑不确定性的情况下，本船与目标船之间的距离。(b)考虑到目标船的不确定性，使用不确定vo方法计算本船和目标船之间的距离。
[0037]
图8 colregs和不确定速度障碍算法下多目标船舶相遇情况场景2。从上到下，左到右依次为：(a)四艘船的初始阶段；(b)超越情况下的操纵；(c)超越、交叉和对遇情况下的操纵；(d)本船和目标船各自的总体操纵路径。
[0038]
图9场景2中的实时碰撞风险指数。
[0039]
图10场景2中colregs下usv路径规划的相对距离。(a)在不考虑不确定性的情况下，本船与目标船之间的距离。(b)考虑到目标船的不确定性，使用不确定vo方法计算本船和目标船之间的距离。
[0040]
图11根据ais数据模拟的本船在长江武汉段的避障轨迹。从左到右依次为(a)：船舶的初始阶段；(b)右舷的两次机动；(c)超越情况下的最后一次操纵。
具体实施方式
[0041]
下面结合具体实施方式对本发明做进一步阐述和说明。所述实施例仅是本公开内容的示范且不圈定限制范围。本发明中各个实施方式的技术特征在没有相互冲突的前提下，均可进行相应组合。
[0042]
本实施例提出了一种基于不确定速度障碍物的智能船舶规则感知动态路径规划方法。特别是，我们首先提出了一种基于输入模糊隶属函数的动态风险评估方法，用于监控复杂场景的风险。然后使用风险值来激活避碰操作。然后，提出了一种符合colregs规则的基于不确定vo的动态路径规划方法。考虑到不同的遭遇场景，传统vo中可达速度集的不同子集旨在满足colregs的不同要求。此外，使用具有鲁棒性的漏斗思想系统地扩展相对锥角，以处理被测目标船中的概率不确定性。其具体包括如下步骤：
[0043]
s1多船相遇场景的碰撞风险评估
[0044]
需要实时评估本船与目标船的动态碰撞风险，以确定如何操纵船舶。五个参数，即最近会遇时间t
cpa
、最近会遇距离d
cpa
、相对距离、相对方位和船速比，被认为与确定碰撞风险最相关。然后，提出了模糊隶属函数来计算本船与所有遇到的目标船之间的碰撞风险指数(cri)。将最大cri值与安全cri阈值进行比较，以作为调节感知动态运动规划方法中的激活条件。
[0045]
令下标o表示本船，i＝1，2，...，n表示n艘目标船。令po＝[xo，yo]
t
，pi＝[xi，yi]
t
和分别表示本船和目标船在二维海洋表面上的位置和速度矢量。然后，本船与目标船之间的cpas计算如下：
[0046][0047][0048]
其中||
·
||是欧几里得范数。然后计算的风险隶属函数为：
[0049][0050]
其中，t0是到最近接近点的最短安全时间，t1是到最近接近点的足够安全时间。船舶之间的对船舶碰撞风险有很大影响。根据(3)，越大，他船对本船的危险性越小。参数t0和t1可根据本船大小进行设置。
[0051]
的风险隶属函数计算如下：
[0052][0053]
其中d0是到最近接近点的最小安全距离，d1是到最近接近点的足够安全距离。船舶之间的，对船舶碰撞的风险有很大的影响。根据(4)式，越大，本船对本船的危险性越小。参数d0和d1也是根据本船的实际大小来设置的。
[0054]
在某些场景下，使用cpa方法可以有效地评估船舶碰撞风险。然而，在考虑colregs的复杂多船相遇场景中，仅使用cpa值进行碰撞前检查是不可靠的。例如，对于船舶之前的会遇情况，本船和目标船相互靠近。位于本船中心线右侧的目标船比左侧的目标船更危险，在计算碰撞风险时需要考虑相对方位角。一般来说，交叉遭遇的情况比正面遭遇的情况更加危险。因此，在计算碰撞风险时，除了常规的tcpa和dcpa外，还考虑了相对距离、相对方位和船速比。
[0055]
具体而言，相对距离的风险隶属函数计算为：
[0056][0057]
其中si是本船与目标船i之间的相对距离。根据式(5)，两船的相对位置越近，两船相撞的风险就越高。s0和s1的值根据实际遇到的情况设置而不同。
[0058]
不同方向的目标船对本船造成不同程度的危险。根据避碰经验，当来船位于19
°
左右时，本船的碰撞风险最大。当来船位于199
°
左右时，碰撞风险最低。因此，建立目标船相对方位的隶属函数如下：
[0059][0060]
其中b0是目标船舶参数方位的阈值。如果b0＝19
°
，当bi＝19
°
时，相对方位风险这是最危险的相对方位。一般来说，右舷的风险大于左舷的风险，前侧的风险大于后侧的风险。
[0061]
同理，建立船速比隶属函数如下：
[0062][0063]
式中，船速比定义为k0一般设为1。当ki＝1时，两船速度相同，是安全与危险的分界线。
[0064]
然后，对于特定的目标船i，我们定义碰撞风险向量为
[0065][0066]
为了平衡五个因素中每一个的影响，定义权重向量
[0067]wt
＝[w
tcpa
，w
dcpa
，ws，wb，wk]
t
，
ꢀꢀ
(9)
[0068]
其中然后，目标船i的cri值计算为：
[0069]
crii＝w
tri
ꢀꢀ
(10)
[0070]
表示cri值的范围都在[0-1]之间。在多船相遇场景中，整体碰撞风险由最危险的目标船决定，即
[0071]
cri＝max{crii}，i＝1，...，n
ꢀꢀ
(11)
[0072]
然后将该cri值与安全阈值进行比较，以激活船舶防撞机动。
[0073]
s2.规则感知动态无碰撞路径规划：
[0074]
在s1中，对船舶航向的碰撞风险进行了评估。当cri值高于安全阈值时，需要激活避免碰撞操作。本步骤提出了一种基于vo的多船相遇场景中的无碰撞路径规划方法。vo方法基于多艘运动船舶的速度在速度空间中生成一个锥形障碍物用于动态避碰。此外，考虑到测量信息中普遍存在的不确定性，扩展了传统vo中的碰撞锥。提出了一种基于鲁棒管道思想的不确定vo路径规划方法。在一定的置信度(pr)下，统一考虑概率影响，实现符合colregs的鲁棒无碰撞路径规划。s2具体包括s2.1-s2.3。
[0075]
s2.1使用vo进行无碰撞路径规划
[0076]
vo的基本思想是通过在速度空间中定义一个锥形障碍物，将一般的动态避障问题转化为静态速度可达性问题。速度空间中的多船相遇场景如附图1所示。具体而言，观察对遇(目标船1)和从右侧交叉(目标船2)的相遇情况。本船和目标船都被定义为圆盘。圆盘的直径被设置为船的长度。
[0077]
图1中，目标船1的确定速度障碍是导致本船与目标船1发生碰撞的速度集，因此，本船避免与目标船1发生碰撞的条件是本船的速度目标船1的确定速度障碍。对于每艘目标船，虚线圆的半径是本船与目标船半径之和。实线形成了本船和目标船之间相对速度的碰撞圆锥。虚线通过考虑目标船速度平移来形成绝对速度的碰撞圆锥体。因此，当本船的速度在虚线圆锥体内部时，或者相当于当相对速度在实线圆锥体内部时，就会导致碰撞。
[0078]
更为具体的，在图1中，vo
o｜i
，i＝1，2是导致本船o与目标船i发生碰撞的速度集。因此，本船避免与目标船i发生碰撞的条件是对于每艘目标船，虚线圆圈的半径是本船和目标船半径之和。实线形成本船与目标船之间相对速度的碰撞锥。虚线通过考虑船舶速度平移锥体形成绝对速度的碰撞锥体。因此，当速度vo在虚线锥内时，或者等效地，当相对速度v
o-vi在实线锥内时，它会导致碰撞。
[0079]
在数学上，设λ(p，v)＝{p+tv|t≥0}是一条从p开始向v方向的射线。将minkowski和定义为定义为表示本船和目标船的半径的minkowski和。定义-o＝{-o|o∈o}作为反射。然后，
[0080][0081]
是多船相遇场景中本船速度障碍解的集合。直观地说，(12)意味着如果本船以vo∈vo
o|i
的速度航行，那么将来会发生碰撞。
[0082]
由于本船和目标船都被简化为圆盘，式(12)可以简化为
[0083][0084]
它起点为po，而d(pi，ro+ri)是一个以pi为中心，半径为ro+ri的开圆盘；ro和ri分别是本船和目标船的半径。对于所有目标船i＝1，2，...，n，多船遭遇场景中设置的速度障碍物为本船必须以不在此设定范围内的速度航行以保证安全。
[0085]
s2.2具有不确定vo的鲁棒无碰撞路径规划
[0086]
由式(13)可知，为了计算速度障碍物，需要知道目标船的速度和位置。然而，在复杂的海洋环境中，这些测量信息通常是不确定的和具有概率性的。一方面，激光雷达、摄像头等车载传感器测量到的信息可能存在噪声；另一方面，目标船的运动可能会受到风、浪和水流等环境干扰的影响。目标船的速度和位置的不确定性可能导致算法不可行，从而在多船相遇场景中引发严重的安全问题。因此，我们接下来提出了一种基于不确定vo的鲁棒无碰撞路径规划方法
[0087]
假设目标船的半径、速度和位置存在服从高斯分布的不确定性。具体来说，其中是对应的平均值；为对应的方差或协方差矩阵，δ
·
表示对应的标准值；diag表示对角矩阵。然后，将(13)中的确定性vo扩展到
[0088][0089]
其中：
[0090][0091][0092][0093]
由于这些随机不确定性有无限的情况，因此不可能也没有必要考虑这些极端条件。因此，本发明只考虑具有一定概率pr的碰撞避免。
[0094]
如图2所示，目标船1的位置不确定性、目标船1的不确定半径与本船半径之和、以及目标船1的速度不确定性，共同组成目标船1与本船的不确定速度障碍；本船与目标船2亦然。当本船和目标船处于国际海上避碰规则下的对遇、追越、或者交叉相遇的态势时，colreg要求规定本船负有责任来进行操作避开障碍物，避免碰撞。因此，比如，假设本船与目标船1、目标船2同时处于对遇的态势局面，规则规定本船与目标船双方有各自向右转来避开来往船舶的责任，本发明此时可以采取不确定速度障碍相应的右转速度可达集中的其中一个速度来采取避碰措施。
[0095]
具体的，如图2所示，对于每艘目标船，虚线最小小圆圈被认为是目标船的位置不确定性，稍大的虚线圆圈被认为是速度不确定性。较大的虚线圆圈是d
cone
的minkowski和，其中d
cone
表示碰撞锥的扩展圆。具体来说，表示碰撞锥的扩展圆。具体来说，其中，其中，其中，因此，d
cone
可以表示为：
[0096][0097]
是一个以为中心，半径为||δpi||的开环半径。
[0098]
考虑到本船与目标船之间的不确定性，实线形成碰撞锥。碰撞锥表示为c(po，d
cone
)。其中po是锥点，d
cone
是扩展的不确定半径。p
cone
是po的线性变换。圆锥c(p
cone
，d
cone
)的虚线是将不确定性碰撞圆锥c(po，d
cone
)的边缘垂直向外移动足够大的量，包括通过协方差
矩阵对圆盘进行的线性变换，而p
cone
是uvo
o|i
的锥点。这通过假设鲁棒性仅针对无限支持随机不确定性的某个概率pr来满足。我们考虑平均值周围的2个标准差。然后可以将不确定的vo设计为对95％的测量不确定性具有鲁棒性。
[0099]
不确定的vo可以有效地处理现有的不确定性。当速度vo在虚线锥内时，即vo∈uvo
o|i
，或者等效地，当相对速度v
o-vi在实线锥内时，它会导致碰撞。对于所有目标船i＝1，2，...，n，多船相遇场景中设置的不确定速度障碍物为在这种情况下，即使存在不确定性，本船以不在该联合集合内的速度航行也能保证航行安全。
[0100]
s2.3规则感知下的鲁棒无碰撞路径规划
[0101]
有必要考虑智能船舶和传统船舶在水上共享的环境。这就要求智能船舶遵守colregs，能够安全规划路径，避免与其他智能船舶或传统船舶发生碰撞。本文重点介绍colregs中考虑的三种主要情况，即超车、交叉和迎头，colregs对场景会遇态势的定义有不同的看法。对于传统船舶的迎面相遇场景，角度一般定义为船前左右5-6度。本文考虑到智能船舶的感知距离和精度限制，将角度扩大到本船前方左右15度，更符合实际情况。在第前一节的基础上，我们将不确定的vo算法和colregs相结合，提出了基于规则感知的动态无碰撞路径规划方法。
[0102]
目标船的每个速度可达集根据colregs分为三个子集，即三部分子集的划分是通过与不确定速度障碍锥c(p
cone
，d
cone
)相切的角平分线获得的，子集垂直于圆锥体。
[0103]
当本船和目标船处于对遇(迎头)态势时，colregs要求两艘船右转以避开障碍物。因此，usv需要遵循中的一个速度，即，
[0104][0105]
然后，所有目标船的相应子集在多船相遇场景中合并为如附图2所示。
[0106]
这些合并的子集计算为：
[0107][0108]
colregs的规则8还建议，如果有足够的可用空间，则应优先考虑改变航向而不是速度改变，并且必须在充足的时间内进行机动。为了更好地遵守规则8，当可达速度集的速度大小或方向可以同时改变时，我们通常选择改变速度方向。令αi，βi分别定义为c(po，d
cone
)和c(p
cone
，d
cone
)锥角的一半，则
[0109][0110]
然后，让αi＝βi来计算新的vo。当多艘目标船的碰撞风险值同时超过设定的阈值时，需要选择最大的角度βi来进行避让。选择最大角度的另一个好处是目标船可以更好地
感知本船的运动意图，从而使目标船也可以提前进行相应的机动。
[0111]
需要注意的是，当本船在各种遭遇情况下作为直航船时，本船没有colregs约束。然后目标船需要采取相应的机动以避免与本船发生碰撞。但是，在真实的遭遇场景中，目标船可能会不采取任何措施，或者采取的措施不符合colregs的要求。即使发生这种情况，本船仍然可以采用可达速度集中的速度来安全地避免与目标船发生碰撞。
[0112]
本发明给出了仿真结果来证明所提出的动态无碰撞路径规划方法的有效性。考虑了海洋环境中的三种情况，即追越、超车和对遇。本发明方法主要用于模拟本船和目标船的规划决策过程，主要考虑了目标船位置、速度和半径的不确定性。
[0113]
本船在路径规划过程中实时评估每艘目标船的碰撞风险(即前述的s1)。当其中一艘目标船达到安全阈值时，本船开始根据不确定vo算法(即前述的s2)执行路径规划和避碰过程。当多艘目标船的风险指数同时超过设定阈值时，需要首先考虑碰撞风险指数最高的船。在计算cri时，需要注意的是，五个风险因素的权重是不同的，其中，我们设置w
t
＝[0.32，0.36，0.14，0.10，0.08]
t
，那么，根据影响因素的隶属函数，就可以建立目标船对本船的碰撞风险评估矩阵r。我们考虑平均值周围的2个标准差。然后，不确定vo可以被设计为对pr＝95％的测量不确定性下具有鲁棒性。在我们的模拟中，我们将船只的长度设置为4米。
[0114]
情形1：单个目标船
[0115]
如附图3所示，该图模拟显示了在colregs下追越单艘目标船的情况。己方船初始位置在[0，-25]m，目标船在[0，0]m，己方船和目标船分别以1米/秒和0.5米/秒的速度直线向北。其中，目标船(左侧轨迹)的位置和速度用二元高斯分布处理，以表示目标船的随机不确定性。本船(右侧轨迹)的避碰决策基于本发明签署的扩展的不确定vo。本发明不确定参数的高斯分布选取两个标准差，以保证智能船95％置信度的安全运动规划和避碰(pr＝95％)。因为目标船是不确定的，所以每个模拟的轨迹结果是随机的，并且图3展示出了超越场景的模拟之一。
[0116]
其中，cri是基于模糊集合理论计算的，并设置了一定的阈值，本发明中取阈值为0.6。当超过设定的阈值时，本船开始做出规划决策，并根据colregs的规则8改变速度方向。图4(a)展示出了通过使用确定性速度障碍方法获得的本船和目标船之间的距离，而不考虑目标船的不确定性。直线是设定的安全线，需要注意的是，不确定速度障碍设定的安全线距离通常大于确定速度障碍。这是因为它需要考虑目标船半径和位置的不确定性。图4(b)示出了通过使用本发明考虑了目标船的不确定性的获得的本船和目标船之间的距离。仿真结果验证了该算法的有效性。
[0117]
情形2：多目标船
[0118]
本次模拟展示了在colregs下，本船与多目标船的相遇情况。考虑到三艘目标船的半径、位置和速度的不确定性，扩展了不确定vo碰撞锥的大小。使用扩展的碰撞锥，即在95％的置信度(pr＝95％)下，保证了路径规划的可靠性。
[0119]
场景1:超车，对遇。图5示出了本船和目标船的仿真过程，以验证使用扩展的不确定vo碰撞锥的碰撞避免的有效性。图中灰虚线表示避碰过程中展开的vo圆锥的左右切线。目标船1、2和3(从下到上第2，3，4个轨迹)和自己的船(最下面的轨迹)使用不确定vo方法来决定机动。在图5(a)中，本船位于[0，-50]m，以1m/s的速度直线向北航行；其中两艘目
标船位于[0，-25]m和[0，-15]m，均以0.5m/s的速度直线向北航行，第三艘目标船位于[5，45]m的位置，并以[0.05，-0.5]m/s的速度航行。在初始阶段，对于目标船1和目标船2而言，本船处于超越的相遇场景，与目标船3处于对遇态势。
[0120]
图5(b)示出了本船识别超越情况并应用colregs约束。目标船1碰撞风险超过设定阈值，colregs约束鼓励超车规避机动右转。本船和目标船之间的实时碰撞风险如图6所示。
[0121]
在图5(c)中，在本船已经移动了一段时间之后，目标船3已经逐渐接近，本船和目标船1已经完成了碰撞回避过程，并且仍然处于与目标船2的超越情况和与目标船3的对遇情况。此时，目标船2和3的碰撞风险值同时超过设定的阈值。
[0122]
然后，根据避碰规则选择βi的最大角度。
[0123]
图5(d)示出了成功避免与目标船只的碰撞。当各目标船的碰撞风险值低于设定的阈值时，本船以初速度航行。图7(a)示出了在不考虑目标船的不确定性的情况下，通过使用确定性vo方法的本船和目标船之间的距离，其中直线是设定的安全线。图7(b)示出了通过使用考虑了三艘目标船的不确定性的不确定vo方法获得的本船和目标船之间的距离。
[0124]
场景2:超车、对遇、交叉相遇。在这种情况下，自己的船首先超过目标船1(从下往上第二个轨迹)。当在它周围机动时，另外两艘目标船(最上面和最右边轨迹)开始接近自己的船，同时形成对遇和交叉态势，如图8所示。
[0125]
图8(a)示出了船只的初始状态，并且本船形成了三个碰撞锥。在图8(b)中，目标船1的cri大于安全cri，并且本船识别出超越情况。然后，自己的船向右舷移动，这符合航海规则。本船和目标船之间的实时碰撞风险如图9所示。
[0126]
在图8(c)中，在避开目标船1的过程中，本船检测其他目标船。然后，自己的船意识到它同时处于超车、对遇和交叉相遇的情况。目标船1和目标船3的cri超过设定的安全cri。colregs约束迫使自己的船向右舷转弯以避开穿过的目标船3。
[0127]
图8(d)示出了所有目标船只都安全通过。注意，当本船成功避开目标船3时，本船仍处于与目标船1的追越状态。此时，目标船1足够远，cri相对较小。本船不认为这种情况是超越。最终，自己的船以初始速度开始转弯机动。图10(a)和图10(b)分别示出了通过使用传统vo或不确定vo方法的本船和目标船之间的距离。
[0128]
3.3用ais数据验证
[0129]
根据长江武汉段的真实自动识别系统(ais)数据，进一步验证了本发明的有效性和应用潜力。
[0130]
图11(a)示出了开始时的模拟。初始方向和长度分别代表每艘船的速度和方向。在图11(b)中，在该过程中，本船处于对遇情况，并且检测到与目标船1碰撞的风险。根据colregs的说法，自己的船有责任右转以避免碰撞。在避让目标船1的过程中，目标船2也可能与本船发生碰撞，与本船处于超车情况。这导致了复杂的多船相遇场景。然后，根据所本发明提出的方法，本船计算与目标船2的转向角。计算角度小于避开目标船1的角度，速度方向不变，如图中轨迹所示。完全避开目标船1后，本船以避开目标船2的速度航行。
[0131]
本船在避让目标船2的过程中，探测到目标船3，也处于与目标船3的超越情况。然后，自己的船选择一个更大的角度来避免碰撞。最后的轨迹表示本船与目标船3之间的避碰路线，避碰过程完成后返回到初始速度。图11(c)示出了本船成功避开所有目标船。
[0132]
4结论
[0133]
本发明提出了一种基于不确定速度障碍物的规则感知动态路径规划方法。所提出的方法解决了多船相遇场景的决策过程中的优先级问题。本发明实时计算船只的碰撞风险，以激活碰撞规避策略。此外，考虑到交叉、超越和对遇的相遇场景，使用鲁棒管道的思想来处理目标船信息中的不确定性。在存在不确定性的情况下，在与多艘船的各种相遇场景中也实现了符合colregs的情形。仿真实验和ais数据验证表明，该方法能使智能船舶在复杂海洋环境中做出安全决策并避免碰撞。
[0134]
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

技术特征：

1.一种基于不确定速度障碍的智能船舶避碰路径规划方法，其特征在于，包括如下步骤：s1.多船相遇场景的碰撞风险评估；选择最近会遇时间t
cpa
、最近会遇距离d
cpa
、相对距离、相对方位和船速比作为本船与目标船碰撞风险最相关的参数，通过各参数的模糊隶属函数计算本船与任一目标船i之间的碰撞风险指数cri
i
，将最大cri
i
值与安全cri阈值进行比较，若高于阈值，则执行s2，否则持续进行碰撞风险评估；s2.进行规则感知动态无碰撞路径规划，包括s21-s23；s21.使用vo进行无碰撞路径规划令下标o表示本船，i＝1，2，
…
，n表示n艘目标船；令p
o
＝[x
o
，y
o
]
t
，p
i
＝[x
i
，y
i
]
t
表示本船和目标船在二维海洋表面上的位置，和目标船在二维海洋表面上的位置，分别表示本船和目标船在二维海洋表面上的速度矢量，定义vo
o|i
是导致本船o与目标船i发生碰撞的速度集，则本船避免与目标船i发生碰撞的条件是的条件是d(p
i
，r
o
+r
i
)是一个以p
i
为中心，半径为r
o
+r
i
的开圆盘，r
o
和r
i
分别是本船和目标船的半径；对于所有目标船i＝1，2，
…
，n，多船遭遇场景中设置的速度障碍物为本船必须以不在此设定范围内的速度航行以保证安全；s22.具有不确定vo的鲁棒无碰撞路径规划考虑目标船的速度和位置存在不确定性，设目标船的半径、速度和位置存在服从高斯分布的不确定性，其中是对应的平均值；为对应的方差或协方差矩阵；δ
·
表示对应的标准值；diag表示对角矩阵；s21中的确定性vo
o|i
扩展到不确定vo(uvo)为：其中：其中：其中：对于所有目标船i＝1，2，
…
，n，多船相遇场景中设置的不确定速度障碍物为本船以不在该联合集合内的速度航行保证航行安全；s23.规则感知下的鲁棒无碰撞路径规划在符合colregs规则约束下，规划路径，避免与其他船舶发生碰撞。2.根据权利要求1所述的基于不确定速度障碍的智能船舶避碰路径规划方法，其特征在于，s1中，以分别表示本船与目标船i之间的t
cpa
、d
cpa
、相对距离、相对方位和船速比的模糊隶属函数，
定义本船与目标船i的撞风险向量为：定义权重向量为：其中w
s
、w
b
、w
k
为对应参数的权重，为对应参数的权重，本船与目标船i的cri值计算为：cri
i
＝w
t
r
i
；则最大cri
i
值为max{cri
i
}，i＝1，...，n。3.根据权利要求2所述的基于不确定速度障碍的智能船舶避碰路径规划方法，其特征在于，s1中，t
cpa
、d
cpa
计算如下：计算如下：其中||
·
||是欧几里得范数；t
cpa
风险隶属函数为其中，t0是到最近接近点的最短安全时间，t1是到最近接近点的足够安全时间；d
cpa
风险隶属函数为：其中d0是到最近接近点的最小安全距离，d1是到最近接近点的足够安全距离；相对距离的风险隶属函数计算为：其中s
i
是本船与目标船i之间的相对距离；相对方位的隶属函数如下：其中b0是目标船舶参数方位的阈值；船速比隶属函数如下：
式中，船速比定义为4.根据权利要求1所述的基于不确定速度障碍的智能船舶避碰路径规划方法，其特征在于，s22中：由于服从高斯分布的不确定性有无限的情况，不可能也没有必要考虑所有极端条件，因此只考虑具有一定概率p
r
的碰撞避免；以d
cone
表示碰撞锥的不确定扩展圆半径，表示碰撞锥的不确定扩展圆半径，其中，中，则d
cone
表示为：表示为：是一个以为中心，半径为||δp
i
||的开环半径；表示闵可夫斯基和；考虑到本船与目标船之间的不确定性，从本船出发做目标不确定性圆的两条射线形成碰撞锥，碰撞锥表示为c(p
o
，d
cone
)，其中p
o
是锥点，d
cone
是扩展的不确定半径；p
cone
是p
o
的线性变换，是uvo
o|i
的锥点，圆锥c(p
cine
，d
cone
)的碰撞锥是将不确定性碰撞圆锥c(p
o
，d
cone
)的边缘垂直向外移动足够大的量，包括通过协方差矩阵对圆盘进行的线性变换；当速度v
o
在虚线锥内时，即v
o
∈uvo
o|i
，或者等效地，当相对速度v
o-v
i
在实线锥内时，会导致碰撞；对于所有目标船i＝1，2，
…
，n，多船相遇场景中设置的不确定速度障碍物为在这种情况下，即使存在不确定性，本船以不在该联合集合内的速度航行也能保证航行安全。5.根据权利要求5所述的基于不确定速度障碍的智能船舶避碰路径规划方法，其特征在于，s23具体为：目标船的每个速度可达集根据colregs分为三个子集，即分别代表本船对于目标船左转、右转，以及远离目标船的速度集；三部分子集的划分是通过与不确定速度障碍锥c(p
cone
，d
cone
)相切的角平分线获得的，子集垂直于圆锥体；当本船和目标船处于迎头对遇态势时，colregs要求两艘船右转以避开障碍物；因此，本船需要遵循中的一个速度，即，然后，所有目标船的相应子集在多船相遇场景中合并为这些合并的子集计算为：
在多船相遇场景中，本船可以采取合并中的某一子集来对多船同时进行避让措施，更好符合规则要求。6.根据权利要求5述的基于不确定速度障碍的智能船舶避碰路径规划方法，其特征在于，当可达速度集的速度大小或方向可以同时改变时，选择改变速度方向；令α
i
，β
i
分别定义为c(p
o
，d
cone
)和c(p
cone
，d
cone
)锥角的一半，则)锥角的一半，则然后，让α
i
＝β
i
来计算新的v
o
；当多艘目标船的碰撞风险值同时超过设定的阈值时，需要选择最大的角度β
i
来进行避让。

技术总结

本发明提出了一种基于不确定速度障碍物的规则感知动态路径规划方法。所提出的方法解决了多船相遇场景的决策过程中的优先级问题。本发明实时计算船只的碰撞风险，以激活碰撞规避策略。此外，考虑到交叉、超越和对遇的相遇场景，使用鲁棒管道的思想来处理目标船信息中的不确定性。在存在不确定性的情况下，在与多艘船的各种相遇场景中也实现了符合COLREGs的情形。仿真实验和AIS数据验证表明，该方法能使智能船舶在复杂海洋环境中做出安全决策并避免碰撞。碰撞。碰撞。