一种水面无人艇路径规划方法

1.本发明属于水上交通路径规划技术领域，更具体地，涉及一种水面无人艇路径规划方法。


背景技术：

2.目前，随着机器人技术的迅猛发展，水面无人艇越来越受到大家的广泛关注，路径规划作为水面无人艇的关键技术之一越来越受到重视。水面无人艇的路径规划是在环境建模的基础上，给定起始点和目标点，成功规划出一条自起始点到目标点的自由空间的无碰撞可行路径。
3.传统的路径规划方法，如a*、dijkstra、人工势场法、遗传算法等，不适合在复杂空间进行路径规划。而快速探索随机树(rrt)采用随机采样的方式，能够避免因环境复杂而陷入局部最优，有效的解决高维空间和复杂约束的路径规划问题。但是，rrt在有限时间内很少能够达到最佳状态，而且收敛速度很慢。


技术实现要素：

4.本发明通过提供一种水面无人艇路径规划方法，解决现有技术中的路径规划方法不适用于复杂空间、收敛速度较慢的问题。
5.本发明提供一种水面无人艇路径规划方法，包括以下步骤：
6.步骤1、采用双向rrt*smart算法，对水面无人艇进行全局路径规划，得到优化全局路径；
7.步骤2、采用colregs-rrt*smart算法，对水面无人艇进行局部路径规划，得到渐进最优的局部路径；
8.步骤3、使用所述渐进最优的局部路径替换所述优化全局路径中需要考虑动态障碍物区域的部分，逐步融合后得到最终的规划路径。
9.优选的，所述步骤1包括以下子步骤：
10.步骤11、获取航行水域的静态环境信息，采用栅格法，根据所述静态环境信息建立环境模型；
11.步骤12、在所述环境模型中选取全局规划的起始节点和全局规划的目标节点，采用双向快速拓展随机树算法，分别以所述全局规划的起始节点与所述全局规划的目标节点为根节点构造两棵随机树，得到一条连接所述全局规划的起始节点与所述全局规划的目标节点的初始全局路径；通过互连直接可见的节点对所述初始全局路径进行迭代优化，得到优化全局路径。
12.优选的，所述步骤2包括以下子步骤：
13.步骤21、根据实际航行信息和国际海上避碰规则确定当前的避碰场景，基于当前的避碰场景构建符合国际海上避碰规则的采样空间；
14.步骤22、在所述采样空间中选取局部避碰的起始节点和局部避碰的目标节点，采
用rrt*smart算法进行局部路径规划，得到渐进最优的局部路径。
15.优选的，所述步骤11中，利用s-57电子海图得到所述静态环境信息，所述静态环境信息包括可航行水域信息和水深信息；采用栅格法建立环境模型时，根据所述可航行水域信息划分空白栅格和障碍栅格，根据所述水深信息将水深数值归一化并赋值到所述空白栅格。
16.优选的，所述步骤12中，所述通过互连直接可见的节点对所述初始全局路径进行迭代优化的具体实现方式为：迭代过程从全局规划的目标节点开始，并向全局规划的起始节点方向移动，每次迭代后得到一个新的优化路径；优化路径中的节点称为信标，优化路径基于信标形成用于智能采样的偏置点；将当前迭代得到的优化路径的成本与前一次迭代得到的优化路径的成本进行比较，若当前迭代得到的优化路径的成本较小，则生成新的信标，基于新的信标形成新的偏置点；重复上述迭代过程，直至所述全局规划的起始节点与所述全局规划的目标节点通过迭代过程连接起来，形成完整的路径，得到所述优化全局路径。
17.优选的，迭代过程中，在两个节点之间的可见性检测中采用插值方法进行无碰撞检查；基于所述插值方法，将节点两两连接在一起，直至所有的节点连成一条完整不间断的路径，并确保优化路径基于信标形成用于智能采样的偏置点位于空白栅格中。
18.优选的，智能采样时，在以信标为中心的圆中生成第一数量的采样点。
19.优选的，所述步骤21中，基于国际海上避碰规则确定局部路径规划可能面临的海事场景，所述局部路径规划可能面临的海事场景包括：“追越”、“对遇”和“交叉相遇”；所述实际航行信息包括船舶之间的相对方位信息和cpa限制信息，所述cpa限制信息包括最小会遇距离d
cpa
和最小会遇时间t
cpa
；结合所述实际航行信息和所述局部路径规划可能面临的海事场景确定当前的避碰场景；
20.将所述采样空间表示为半圆空间，所述半圆空间的内圈半径由所述最小会遇距离d
cpa
确定，所述半圆空间的外圈半径由所述最小会遇时间t
cpa
确定；若当前的避碰场景为“追越”，则采样空间限制在外圈半径内、内圈半径外的整个圆环中；若当前的避碰场景为“对遇”，则采样空间限制在外圈半径内、内圈半径外的一半圆环中；若当前的避碰场景为“交叉相遇”，则采样空间限制在外圈半径内、内圈半径外的一半圆环中。
21.优选的，所述最小会遇距离d
cpa
和所述最小会遇时间t
cpa
的计算方式为：
22.d
cpa
＝dsinθ
23.t
cpa
＝dcosθ/vr[0024][0025][0026][0027]
其中，d为两船转向后的距离，θ为转向后两船相对运动线与目标船方位线的夹角，vr为转向后两船的相对运动速度，a为转向后两船相对运动线与本船船首向的夹角，q为本船舶舷角，c
0t
为目标船舶转向角度，v0为本船速度，v
t
为目标船速度。
[0028]
优选的，所述步骤22中，所述采用rrt*smart算法进行局部路径规划的具体实现方式为：在所述采样空间中选取局部避碰的起始节点和局部避碰的目标节点，采用rrt*smart算法得到一条连接所述局部避碰的起始节点和所述局部避碰的目标节点的初始局部路径，路径中彼此可见的节点直接相连；迭代过程从随机节点x
rand
开始，并朝着终点依次进行碰撞检测，以便与每个节点的连续父节点直接连接，直到无法通过碰撞检测；到此过程结束时，不再存在可直接连接的节点；通过迭代过程依次对连续父节点进行检测；在两个节点之间的可见性检查中采样插值方法进行无碰撞检查。
[0029]
本发明中提供的一个或多个技术方案，至少具有如下技术效果或优点：
[0030]
本发明所采用的rrt*smart算法考虑了colregs海上避碰规则，对于其规划过程中的碰撞检测要求更为严格，且对于不同场景存在不同的检测方式，方法更具有准确性与完善性。本发明所采用的rrt*smart算法对碰撞检测要求更为严格，但是不需要有关障碍物的明确信息，计算内容上更为简洁，对于未知障碍物的检测与有一定的可行性。本发明所采用的rrt*smart算法在传统rrt*smart算法上增加了双向采样的方式，计算速度更快，算法效率更高。本发明能够使水面无人艇成功避开障碍物并快速到达目标点。
附图说明
[0031]
图1为本发明静态环境信息建模示意图；
[0032]
图2为本发明根据colregs定义的三种会遇局面；其中，图2中的(a)表示追越，图2中的(b)表示对遇，图2中的(c)表示交叉相遇；
[0033]
图3为本发明考虑到不同的会遇类型下三个符合colregs的采样空间；其中，图3中的(a)表示交叉相遇下符合colregs的采样空间，图3中的(b)表示对遇下符合colregs的采样空间，图3中的(c)表示追越下符合colregs的采样空间；
[0034]
图4为本发明寻找最优路径的结果示意图。
具体实施方式
[0035]
为了更好的理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。
[0036]
本实施例提供了一种水面无人艇路径规划方法，包括以下步骤：
[0037]
步骤1、采用双向rrt*smart算法，对水面无人艇进行全局路径规划，得到优化全局路径。
[0038]
其中，所述步骤1包括以下子步骤：
[0039]
步骤11、获取航行水域的静态环境信息，采用栅格法，根据所述静态环境信息建立环境模型。
[0040]
利用s-57电子海图得到所述静态环境信息，所述静态环境信息包括可航行水域信息和水深信息；采用栅格法建立环境模型时，根据所述可航行水域信息划分空白栅格和障碍栅格，根据所述水深信息将水深数值归一化并赋值到所述空白栅格。
[0041]
步骤12、在所述环境模型中选取全局规划的起始节点和全局规划的目标节点，采用双向快速拓展随机树算法，分别以所述全局规划的起始节点与所述全局规划的目标节点为根节点构造两棵随机树，得到一条连接所述全局规划的起始节点与所述全局规划的目标
节点的初始全局路径；通过互连直接可见的节点对所述初始全局路径进行迭代优化，得到优化全局路径。
[0042]
所述通过互连直接可见的节点对所述初始全局路径进行迭代优化的具体实现方式为：迭代过程从全局规划的目标节点开始，并向全局规划的起始节点方向移动，每次迭代后得到一个新的优化路径；优化路径中的节点称为信标，优化路径基于信标形成用于智能采样的偏置点；将当前迭代得到的优化路径的成本与前一次迭代得到的优化路径的成本进行比较，若当前迭代得到的优化路径的成本较小，则生成新的信标，基于新的信标形成新的偏置点；重复上述迭代过程，直至所述全局规划的起始节点与所述全局规划的目标节点通过迭代过程连接起来，形成完整的路径，得到所述优化全局路径。
[0043]
迭代过程中，在两个节点之间的可见性检测中采用插值方法进行无碰撞检查；基于所述插值方法，将节点两两连接在一起，直至所有的节点连成一条完整不间断的路径，并确保优化路径基于信标形成用于智能采样的偏置点位于空白栅格中。
[0044]
智能采样时，在以信标为中心的圆中生成第一数量的采样点。
[0045]
步骤2、采用colregs-rrt*smart算法，对水面无人艇进行局部路径规划，得到渐进最优的局部路径。
[0046]
其中，所述步骤2包括以下子步骤：
[0047]
步骤21、根据实际航行信息和国际海上避碰规则确定当前的避碰场景，基于当前的避碰场景构建符合国际海上避碰规则的采样空间。
[0048]
基于国际海上避碰规则确定局部路径规划可能面临的海事场景，所述局部路径规划可能面临的海事场景包括：“追越”、“对遇”和“交叉相遇”；所述实际航行信息包括船舶之间的相对方位信息和cpa限制信息，所述cpa限制信息包括最小会遇距离d
cpa
和最小会遇时间t
cpa
；结合所述实际航行信息和所述局部路径规划可能面临的海事场景确定当前的避碰场景。
[0049]
将所述采样空间表示为半圆空间，所述半圆空间的内圈半径由所述最小会遇距离d
cpa
确定，所述半圆空间的外圈半径由所述最小会遇时间t
cpa
确定；若当前的避碰场景为“追越”，则采样空间限制在外圈半径内、内圈半径外的整个圆环中；若当前的避碰场景为“对遇”，则采样空间限制在外圈半径内、内圈半径外的一半圆环中；若当前的避碰场景为“交叉相遇”，则采样空间限制在外圈半径内、内圈半径外的一半圆环中。
[0050]
步骤22、在所述采样空间中选取局部避碰的起始节点和局部避碰的目标节点，采用rrt*smart算法进行局部路径规划，得到渐进最优的局部路径。
[0051]
所述采用rrt*smart算法进行局部路径规划的具体实现方式为：在所述采样空间中选取局部避碰的起始节点和局部避碰的目标节点，采用rrt*smart算法得到一条连接所述局部避碰的起始节点和所述局部避碰的目标节点的初始局部路径，路径中彼此可见的节点直接相连；迭代过程从随机节点x
rand
开始，并朝着终点依次进行碰撞检测，以便与每个节点的连续父节点直接连接，直到无法通过碰撞检测；到此过程结束时，不再存在可直接连接的节点；通过迭代过程依次对连续父节点进行检测；在两个节点之间的可见性检查中采样插值方法进行无碰撞检查。
[0052]
步骤3、使用所述渐进最优的局部路径替换所述优化全局路径中需要考虑动态障碍物区域的部分，逐步融合后得到最终的规划路径。
[0053]
下面对本发明做进一步的说明。
[0054]
水面无人艇的路径规划算法通常是根据对当前所处环境的动态和静态信息获取情况，将水面无人艇路径规划问题分为已知静态信息的全局路径规划和动态未知信息的局部规划。
[0055]
本发明针对水面无人艇的全局路径主要采用双向rrt*smart算法来规划，对于动态障碍物的避障问题采用colregs-rrt*smart。最后通过两种算法的混合使用，实现水面无人艇在各种复杂环境下可以按照避碰规则从起点到目标点完成相关路径规划的任务。本发明主要包括以下五个部分：
[0056]
s1：建立环境模型。
[0057]
所述s1主要针对静态环境信息建模，包括：
[0058]
水面无人艇通过获取静态环境信息进行路径规划，静态环境信息包括航行障碍、位置、水深以及形状等参数。对于水面无人艇，电子海图可以提供详细而准确的全局环境信息，本发明使用s-57电子海图作为无人艇路径规划的环境模型数据来源。
[0059]
栅格法建立环境模型相对简单且易于维护，进行路径规划和控制也比较简单。通过解析电子海图提取的海洋环境信息是由复杂几何图形组成的，使得多数路径搜索算法不能直接被利用，因此需要对海洋环境信息进行网格划分，转换成几何关系简单的环境模型，以提高路径搜索算法的效率。
[0060]
根据海电子图信息，可以确定空白栅格和障碍栅格，再根据海图中提供的信息，并将水深数值归一化，赋值到空白栅格，并用不同深浅的颜色可视化。结合图1可以看到，水深数值越小颜色越深。
[0061]
全局路径规划可以根据不同类型的水面无人艇吃水确定选择不同的空白栅格，进而确定路径搜索的范围，不仅能保证水面无人艇航行安全，而且可以加快搜索速度。
[0062]
s2：获得优化全局路径。
[0063]
双向快速拓展随机树算法分别以起始点s与目标点g为根节点，构造两棵快速扩展随机树。所述两颗快速扩展随机树分别向对方扩展，直到两树相遇，完成连接生成第一个路径。找到第一个路径后，它会通过互连直接可见的节点来优化此路径。该优化路径产生用于智能采样的偏置点。随着算法的进展，该过程会继续，并且路径快速优化，每当找到较短的路径时，偏置就会向新路径移动。
[0064]
所述s2的全局路径规划，具体为：
[0065]
s21：双向快速拓展随机树算法是将两棵搜索树分别以水面无人艇全局规划航线上的起始点s和目标点g为根节点初始化。
[0066]
s22：假设以起始点s为根节点的树为t1，以目标点g为根节点的树为t2。在水面无人艇航行区域随机生成一个点pi，在t1现有的节点中确定距离pi最近的节点pc。以pc为父节点，向pi方向延伸一个步长l，得到新节点pn，检查新节点pn是否落在障碍物上或pcpn路径是否与障碍物相交。如果是，则返回到生成随机点的步骤；如果不是，则新节点通过了障碍物检测，将pn放入t1中。
[0067]
s23：同理设pj是t2现有节点中距离pw最近的点，以pj为父节点向pw延伸步长l的距离，得到新节点p
x
，检查它是否落在障碍物上，或pjp
x
路径是否穿过障碍物。如果是，则返回到生成随机点的步骤；如果不是，则新节点通过了障碍物检测，将p
x
放入t2中。完成这一过程
称为一次搜索，每次搜索完成后需要判断两树的新节点是否满足相遇条件，若不满足则需要继续搜索。若满足，则利用两树中除根节点外每一节点有且仅有一个父节点这一特性，从两树相遇的两个节点开始回溯操作，即可得到一条连接起始点s与目标点g的水面无人艇全局路径。
[0068]
s24：rrt*smart算法在找到第一个路径后，它会通过互连直接可见的节点来优化此路径。该优化路径产生用于智能采样的偏置点。随着算法的进展，该过程会继续，并且路径快速优化，每当找到较短的路径时，偏置就会向新路径移动。
[0069]
一旦rrt*给出初始路径，路径中的节点：z节点(即z
node
)和z节点的父节点(即z
parent-to-parent
)相互可见的是直接连接的。迭代过程从目标节点(即z
goal
)开始，并向起始节点(即z
init
)方向移动，以检查与每个节点的连续父节点的直接连接，直到每个节点可以与其父节点成功连接。此过程结束时，不再存在可直接连接的节点。基于三角不等式的概念优化路径。根据三角不等式，两边之和一定大于第三边，即优化后的一条边比优化之前的两条边路径之和更短，因此总是给出较短的路径。
[0070]
因此，与rrt*找到的原始路径相比，此路径中存在的节点数量减少。这些节点被称为信标z
beacons
，构成智能采样的基础。每次发现成本较低的新路径时，都会使用路径优化技术对其进行优化，以提供相对较好的路径。将此优化路径的成本与先前优化路径的成本进行比较。如果优化后的成本较高，则选择此路径中已存在的节点作为信标z
beacons
，用于智能采样。如果优化后的成本较小，则会生成新的信标z
beacon
，从而形成新的偏置点。这些新形成的信标更接近顶点。此过程将继续，直到完成所需的迭代。
[0071]
在两个节点之间的每次可见性检查中，都需要进行无碰撞检查。为此，在所提出的方法中使用了插值技术，其工作原理是构造线上的每个点(通过将所有节点连接在一起)，同时确保新添加的点位于自由配置空间中。这种可见性检查方法不需要其他碰撞检查方法中要求的关于障碍物的明确信息。因此，提出的无碰撞检查插值方法与障碍物的形状无关，且计算成本较低。
[0072]
一旦找到初始路径，智能采样将开始，在以z
beacons
为中心的半径r
beacons
圆中直接生成一定数量的样本(即采样点)。采样偏向于这些信标，因为它们提供了有关障碍物顶点(或圆形障碍物的外围)位置的有用线索。因此，这些信标需要被最大节点包围，以优化这些转弯处的路径。与rrt*相比，这一特性迫使所提出的rrt*smart算法以更少的迭代次数达到最优解。
[0073]
s3：根据实际航行环境和国际海上避碰规则确定避碰区域，并确定局部避碰的起始点和目标点。
[0074]
所述s3用于确定局部路径规划面临的场景，主要包括：
[0075]
所述国际海上避碰规则(international regulations for preventing collisions at sea，colregs)，是为确保船舶航行安全，预防和减少船舶碰撞，规定在公海和连接于公海的一切通航水域共同遵守的海上交通规则。
[0076]
所述规则13条：追越：一船正从他船正横后大于22.5度的某一方向赶上他船时，即该船对其所追越的船所处位置，在夜间只能看见被追越船的尾灯而不能看见它的任一舷灯时，应认为是在追越中；参见图2中的(a)。
[0077]
所述规则14条：对遇:当一船看见他船在正前方或接近正前方：即在夜间，能看见
他船的前后桅灯成一直线或接近一直线，和(或)两盏舷灯；在日间，看到他船的上述相应形态时，则应认为存在这样的局面；参见图2中的(b)。
[0078]
所述规则15条：交叉相遇:当两艘机动船交叉相遇致有构成碰撞危险时，有他船在本船右舷的船舶应给他船让路，如当时环境许可，还应避免横越他船的前方。参见图2中的(c)。
[0079]
船对船碰撞风险可以通过两个基本指标来衡量，即d
cpa
(最小会遇距离)和t
cpa
(最小会遇时间)。d
cpa
是根据当前的航行状态计算出的两艘船舶会遇的最近距离，而t
cpa
则是到达该会遇位置的时间。
[0080]
所述最小会遇距离d
cpa
和所述最小会遇时间t
cpa
的计算方式为：
[0081]dcpa
＝d sinθ
[0082]
t
cpa
＝d cosθ/vr[0083][0084][0085][0086]
其中，d为两船转向后的距离，θ为转向后两船相对运动线与目标船方位线的夹角，vr为转向后两船的相对运动速度，a为转向后两船相对运动线与本船船首向的夹角，q为本船舶舷角，c
0t
为目标船舶转向角度，v0为本船速度，v
t
为目标船速度。
[0087]
由上述公式可以计算得到d
cpa
和t
cpa
。由公式可见，船舶转向避碰后最大的最近会遇距离max d
cpa
＝d，即θ＝90
°
时。从安全角度考虑，d
cpa
越大安全性越高。
[0088]
为了提高rrt*smart的采样性能，构建了符合colregs的采样空间。首先，必须确定当前的避碰场景，这可以通过使用最小允许cpa限制以及船舶之间的相对方位来实现。
[0089]
规则13-15中的单船相遇可以用两个圆表示，如图3所示，内圈半径由dcpa确定，外圈半径由tcpa确定。其中，图3中的(a)表示交叉相遇下符合colregs的采样空间(即子集)，图3中的(b)表示对遇下符合colregs的子集，图3中的(c)表示追越下符合colregs的子集。在内圈内航行违反cpa限制，外圈表示需要执行操作的区域(给定时间跨度)。对于上述三种情况，采样空间都可以表示为半圆空间，追越情况可以自由的表示为圆环。通过将采样限制在此空间，可以确保所有点相对于colregs有效，从而使采样空间限制在规定的空间内加快采样效率，提升搜索速度。
[0090]
s4：获得渐进最优的路径。
[0091]
所述s4为colregs结合rrt*smart算法，具体为：
[0092]
根据s3所述避碰场景，一旦rrt*给出初始路径，路径中彼此可见的节点直接相连。迭代过程从随机节点x
rand
开始，并朝着终点xs依次进行碰撞检测，以便与每个节点的连续父节点直接连接，直到无法通过碰撞检测。到此过程结束时，不再存在可直接连接的节点。因此，路径基于三角不等式的概念进行优化。根据三角不等式，两边之和一定大于第三边，即优化后的一条边比优化之前的两条边路径之和更短，通过迭代过程依次对连续父节点进行
检测。
[0093]
在两个节点之间的每次检查中，都需要进行无冲突检查。利用插值而不需要其他碰撞检查方法所需的关于障碍物的明确信息。这种用于无碰撞检查的插值方法与障碍物的形状无关并且在计算上更简洁。与rrt*找到的原始路径相比，此路径中存在的节点数量更少，利用rrt*smart算法对rrt*算法路径进行优化。
[0094]
s5：使用所述渐进最优的局部路径替换所述优化全局路径中需要考虑动态障碍物区域的部分，逐步融合后得到最终的规划路径。
[0095]
由于全局规划不考虑动态障碍物的预先规划，而实际情况会存在动态障碍物，因此局部规划是在全局规划的基础上进行优化修正，对全局路径中需要考虑动态障碍物区域的部分进行替换。采用上述方法，本发明寻找最优路径的结果示意图如图4所示。
[0096]
最后所应说明的是，以上具体实施方式仅用以说明本发明的技术方案而非限制，尽管参照实例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。