CN112362067B - 一种用于内河智能船舶的自主航线规划方法 - Google Patents

Abstract

本发明公开了一种用于内河智能船舶的自主航线规划方法，包括以下步骤：步骤1：构建基于航线的栅格地图；步骤2：在栅格地图中确定可航行航道；步骤3：基于A星算法，在可航行航道上生成最短航线；步骤4：对最短航线进行长直线形处理和平滑处理，得到适合智能船舶航行的最佳航线。本发明适用于在内河河道内，对智能船舶规划自主航线，不依赖于电子航道图和电子海图，能够自己生成栅格地图，为航线规划算法提供搜索空间，确定确定可航行航道，基于A星算法规划出来一条在可航行航道上的最短航线，而且优化了A星算法规划出来的航线，使得航线是一段段的长直线而且更加平滑，能够符合船舶的航行。

Description

一种用于内河智能船舶的自主航线规划方法

技术领域

本发明涉及船舶路径规划领域，具体涉及一种用于内河智能船舶的自主航线规划方法。

背景技术

随着人工智能产业的兴起，智能船舶作为其中的一部分，也备受瞩目。无人驾驶的智能船舶在航行之前，需要智能船舶能够自主规划出一条从起点到终点的最短航线。因此，自主航线规划是智能船舶无人驾驶的一个重要组成部分。

现有技术方案中有以电子航道图或者电子海图为搜索空间，利用蚁群算法或者A星算法在电子航道图或者电子海图中为智能船舶规划航线。

现有技术方案在规划航线时，依赖于已有的电子航道图或者电子海图。在没有这些地图信息的内河区域将无法规划航线，而且现有技术方案规划的航线并不适合船舶的航行，因为船舶航行需要的是一段段长直线构成的以及平滑的小角度转弯的航线。

发明内容

本发明要解决的技术问题是提供一种不依赖于电子航道图或者电子海图，能够自己生成航线的规划空间，并且在此规划空间中优化A星算法使之生成一条长直线形和平滑的适合船舶航行的航线。

为了解决上述技术问题，本发明提供了一种用于内河智能船舶的自主航线规划方法，包括以下步骤：

步骤1：构建基于航线的栅格地图；

步骤2：在栅格地图中确定可航行航道；

步骤3：基于A星算法，在可航行航道上生成最短航线；

步骤4：对最短航线进行长直线形处理和平滑处理，得到适合智能船舶航行的最佳航线。

本发明一个较佳实施例中，进一步包括所述栅格地图包括内河上用于标示路线的航标挺、不可航向区域、可航行区域、障碍物缓冲区、上行区域、下行区域以及上下行边间缓冲区。

本发明一个较佳实施例中，进一步包括步骤1中栅格地图的生成方法具体包括：

步骤1-1：采集两列航标挺的GPS坐标，将坐标转换到墨卡托坐标系下；

步骤1-2：设定按上行或者下行的顺序分别存放两列航标艇的墨卡托坐标到两个容器中记为mark1_list和mark2_list，mark1_list和mark2_list中最大和最小的x坐标记为max_x和min_x，最大和最小的y坐标记为max_y和min_y，障碍物缓冲区的距离记为inflation_radius，上下行边界的缓冲区距离记为inflation_radius_min；

步骤1-3：设定栅格地图记为map，栅格地图的分辨率记为map_resolution，宽和高记为map_width和map_height，大小为map_width*map_height，在map中-100代表障碍物，-3代表障碍物缓冲区，-2代表上下行边界缓冲区，-1代表初始值，0代表被访问过，1代表可航行，11代表靠近mark1_list的可航行区域，22代表靠近mark2_list的可航行区域，11和22区分了上下行区域。

本发明一个较佳实施例中，进一步包括步骤2中的可航行航道的确定方法具体包括：

步骤2-1：遍历mark1_list和mark2_list找出max_x，min_x，max_y和min_y；

步骤2-2：计算栅格地图的宽高，map_width＝(max_x–min_x)/map_resolution+1，map_height＝(max_y–min_y)/map_resolution+1，然后用map_width和map_height初始化map；

步骤2-3：新建一个空的容器记为open_list，从mark1_list中取出第一个数据放入open_list中；

步骤2-4：判断open_list是否为空，如果是则进入步骤2-6，如果不是，取出open_list中的第一个数据记为current_node，并从open_list中删除current_node，判断current_node是否被访问过，如果被访问过则重新开始步骤2-4步，如果没被访问过，设置current_node为被访问过，然后计算出current_node在墨卡托坐标系中的上下左右四个邻居记为up，down，left和right，以及在栅格地图中的上下左右四个邻居记为map_up，map_down，map_left和map_right；

步骤2-5：对上邻居做处理，判断map_up有没有超出栅格地图，如果超出则放弃该map_up，如果没超出，判断map_up在map中的数值是否为-1，如果不是则放弃该map_up，如果是，将该数值设置为0，然后判断up是否在可航行航道内，如果不是则放弃该map_up，如果是，将map_up在map中的数值设置为1，并将up放入open_list中，然后计算出up距离mark1_list的距离distance1和距离mark2_list的距离distance2，如果distance1或者distance2小于inflation_radius则将map_up在map中的数值设置为-3，否则如果distance1和distance2的差值的绝对值小于inflation_radius_min则将map_up在map中的数值设置为-2，否则如果distance1小于distance2则将map_up在map中的数值设置为11，否则将map_up在map中的数值设置为22.对剩余的下左右三个邻居做同样的处理，然后返回到第4步；

步骤2-6：将map_width，map_height，map_resolution，min_x，min_y和map保存到一个文本文件，完成在栅格地图中确定可航行航道。

本发明一个较佳实施例中，进一步包括步骤3中给定起点和终点，将起点和终点转换到栅格地图当中，使用A星算法在栅格地图中得到一条最短航线，设定经A星算法得到的最短航线的航线的节点存储在容器path中，经过长直线形处理的存储在容器path_filtered中，最后优化过的存储在容器path_filtered_smoothed中。

本发明一个较佳实施例中，进一步包括步骤4中的长直线形处理具体包括：

步骤4-1-1：将path中的最后一个节点放入path_filtered中，然后记upper为path中节点的索引，upper的初始值为path最后一个节点的索引值，记0为path第一个节点的索引值；

步骤4-1-2：对path在[0,upper)索引区间内逐个取节点，当前取到的索引记为i，如果i大于或等于upper则进入步骤4-1-5，如果i小于upper，取path中索引为i的节点，记为path[i]；

步骤4-1-3：计算path[i]与path_filtered中最后一个节点的连线上的点，判断这些点在栅格地图map中的数值是否等于终点在栅格地图map中的数值，统计不相等的次数，如果次数小于等于阈值3，将path[i]放入path_filtered中，并设置upper为i，进入步骤4-1-4，如果次数大于阈值3，判断i是否等于upper–1，如果不是则进入步骤4-1-4，如果是，将path[i]加入path_filtered中，并设置upper为i，进入步骤4-1-4；

步骤4-1-4：判断path的第一个节点是否等于path_filtered的最后一个节点，如果是则进入步骤4-1-5，如果不是则返回步骤4-1-2；

步骤4-1-5：将path_filtered中的节点的顺序颠倒，完成长直线形处理。

本发明一个较佳实施例中，进一步包括步骤4中的平滑处理具体包括：

步骤4-2-1：将path_filtered赋值给path_filtered_smoothed；

步骤4-2-2：对于path_filtered_smoothed中的第二个节点到最后第二个节点，判断每个节点的前后两条航线之间的夹角是否大于5度，如果不是则保留该节点，如果是，记i为当前节点的索引，p_f_s[i]为当前节点，p_f_s[i-1]为上一个节点，p_f_s[i+1]为下一个节点，temp等于p_f_s[i]，根据公式temp＝temp+0.8*(p_f_s[i]-temp)+0.2*(p_f_s[i-1]–2*temp+p_f_s[i+1])来更新temp；

步骤4-2-3：计算p_f_s[i-1]与temp的连线上的点，temp与p_f_s[i+1]的连线上的点，判断这些点在栅格地图map中的数值是否等于终点在栅格地图map中的数值，统计不相等的次数，如果次数小于等于阈值6，将temp赋值给p_f_s[i]来更新path_filtered_smoothed，如果次数大于阈值6，则保留该节点。

本发明的有益效果：

本发明适用于在内河河道内，对智能船舶规划自主航线，不依赖于电子航道图和电子海图，能够自己生成栅格地图，为航线规划算法提供搜索空间，确定确定可航行航道，基于A星算法规划出来一条在可航行航道上的最短航线，而且优化了A星算法规划出来的航线，使得航线是一段段的长直线而且更加平滑，能够符合船舶的航行。

附图说明

图1是本发明的用于内河智能船舶的自主航线规划方法的步骤图；

图2是本发明的栅格地图及在栅格地图上确定可航行航道的流程图；

图3是本发明的长直线形处理流程图；

图4是本发明的平滑处理流程图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

参照图1所示，本发明的用于内河智能船舶的自主航线规划方法，包括以下步骤：

步骤1：构建基于航线的栅格地图；

步骤2：在栅格地图中确定可航行航道；

步骤3：基于A星算法，在可航行航道上生成最短航线；

步骤4：对最短航线进行长直线形处理和平滑处理，得到适合智能船舶航行的最佳航线。

本发明适用于在内河河道内，对智能船舶规划自主航线，不依赖于电子航道图和电子海图，能够自己生成栅格地图，为航线规划算法提供搜索空间，确定确定可航行航道，基于A星算法规划出来一条在可航行航道上的最短航线，而且优化了A星算法规划出来的航线，使得航线是一段段的长直线而且更加平滑，能够符合船舶的航行。

实施例：

内河智能船舶航行时，必须在海事部门限定的航道内航行，而可航行航道是通过在内河两边放置一个个航标艇来提示船舶，在这些航标艇之内可以航行，之外不可以航行，否则会有危险。由于内河航道具有两边，因此航标艇也分为两列，内河航道的一边对应着一列航标艇，在这两列航标艇之间的区域就是可航行航道，之外的区域为不可航行航道。根据船舶是顺流航行还是逆流航行，可航行航道还区分为下行区域和上行区域，本实施例中的栅格地图以此区分为不可航向区域、可航行区域，将可航行区域内分为上行区域和下行区域，并在栅格地图中标注了航标挺、障碍物缓冲区以及上下行边间缓冲区，来保证可航行区域的安全。

参照图2所示，步骤1中栅格地图的生成方法具体包括：

步骤1-1：采集两列航标挺的GPS坐标，将坐标转换到墨卡托坐标系下；

步骤1-2：设定按上行或者下行的顺序分别存放两列航标艇的墨卡托坐标到两个容器中记为mark1_list和mark2_list，mark1_list和mark2_list中最大和最小的x坐标记为max_x和min_x，最大和最小的y坐标记为max_y和min_y，障碍物缓冲区的距离记为inflation_radius，上下行边界的缓冲区距离记为inflation_radius_min；

步骤1-3：设定栅格地图记为map，栅格地图的分辨率记为map_resolution，宽和高记为map_width和map_height，则栅格地图的大小为map_width*map_height，在map中-100代表障碍物，-3代表障碍物缓冲区(包括航标艇的缓冲区)，-2代表上下行边界缓冲区，-1代表初始值，0代表被访问过，1代表可航行，11代表靠近mark1_list的可航行区域，22代表靠近mark2_list的可航行区域，11和22区分了上行区域和下行区域。

通过上述步骤1-1～步骤1-3，将栅格地图的位置坐标进行标定，确定航标挺的位置、确定栅格地图的大小及分辨率，在栅格地图中确定障碍物缓冲区、上行区域、下行区域以及上下行边界缓冲区，接下来就是要通过这些区域计算得到可航行航道和不可航行航道的位置。

步骤2中的可航行航道的确定方法具体包括：

步骤2-1：遍历mark1_list和mark2_list找出max_x，min_x，max_y和min_y；

步骤2-2：计算栅格地图中可航行航道的宽高，map_width＝(max_x–min_x)/map_resolution+1，map_height＝(max_y–min_y)/map_resolution+1，然后用map_width和map_height初始化map；

步骤2-3：新建一个空的容器记为open_list，从mark1_list中取出第一个数据放入open_list中；

步骤2-4：判断open_list是否为空，如果是则进入步骤2-6，如果不是，取出open_list中的第一个数据记为current_node，将数据定义为current_node，并从open_list中删除current_node，判断current_node是否被访问过，如果被访问过则重新开始步骤2-4，如果没被访问过，设置current_node为被访问过，然后计算出current_node在墨卡托坐标系中的上下左右四个邻居记为up，down，left和right，以及在栅格地图中的上下左右四个邻居记为map_up，map_down，map_left和map_right；

步骤2-5：对上邻居做处理，判断map_up有没有超出栅格地图，如果超出则放弃该map_up，如果没超出，判断map_up在map中的数值是否为-1，如果不是则放弃该map_up，如果是，将该数值设置为0，然后判断up是否在可航行航道内，如果不是则放弃该map_up，如果是，将map_up在map中的数值设置为1，并将up放入open_list中，然后计算出up距离mark1_list的距离distance1和距离mark2_list的距离distance2，如果distance1或者distance2小于inflation_radius则将map_up在map中的数值设置为-3，否则如果distance1和distance2的差值的绝对值小于inflation_radius_min则将map_up在map中的数值设置为-2，否则如果distance1小于distance2则将map_up在map中的数值设置为11，否则将map_up在map中的数值设置为22.对剩余的下左右三个邻居做同样的处理，然后返回到第4步；

步骤2-6：将map_width，map_height，map_resolution，min_x，min_y和map保存到一个文本文件，完成在栅格地图中确定可航行航道。

通过上述步骤2，能过在栅格地图中生成用于标示路线的航标挺、不可航向区域、可航行区域、障碍物缓冲区、上行区域、下行区域以及上下行边间缓冲区。

步骤3中给定起点和终点，将起点和终点转换到栅格地图当中，使用A星算法在栅格地图中得到一条最短航线，所述A星算法即为A*搜寻算法，是一种成熟的算法技术，是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法，能够基于栅格地图中生产的可航行航线，计算得到在此航线上任意两点之间的最短航线。

但是对于智能船舶来讲，最短航线并不意味着是最优航线，智能船舶在航行时，需要尽量保持平稳，因此在最短航线的基础上，应该对最短航线进行优化处理，将最短航线进行长直线形处理和平滑处理，使得航线是一段段的长直线而且更加平滑，能够符合船舶的航行。

具体地，设定经A星算法得到的最短航线的航线的节点存储在容器path中，经过长直线形处理的存储在容器path_filtered中，最后优化过的存储在容器path_filtered_smoothed中。

参照图3所示，步骤4中的长直线形处理具体包括：

步骤4-1-1：将path中的最后一个节点放入path_filtered中，然后记upper为path中节点的索引，upper的初始值为path最后一个节点的索引值，记0为path第一个节点的索引值；

步骤4-1-2：对path在[0,upper)索引区间内逐个取节点，当前取到的索引记为i，如果i大于或等于upper则进入步骤4-1-5，如果i小于upper，取path中索引为i的节点，记为path[i]；

步骤4-1-3：计算path[i]与path_filtered中最后一个节点的连线上的点，判断这些点在栅格地图map中的数值是否等于终点在栅格地图map中的数值，统计不相等的次数，如果次数小于等于阈值3，将path[i]放入path_filtered中，并设置upper为i，进入步骤4-1-4，如果次数大于阈值3，判断i是否等于upper–1，如果不是则进入步骤4-1-4，如果是，将path[i]加入path_filtered中，并设置upper为i，进入步骤4-1-4；

步骤4-1-4：判断path的第一个节点是否等于path_filtered的最后一个节点，如果是则进入步骤4-1-5，如果不是则返回步骤4-1-2；

步骤4-1-5：将path_filtered中的节点的顺序颠倒，完成长直线形处理。

本实施中将阈值设置为3，即实现了将三次弯折连成一条直线，在其他实施例中，可以根据不同的需求程度及路线的复杂程度，设置不同的阈值。

参照图4所示，步骤4中的平滑处理具体包括：

步骤4-2-1：将path_filtered赋值给path_filtered_smoothed；

步骤4-2-2：对于path_filtered_smoothed中的第二个节点到最后第二个节点，判断每个节点的前后两条航线之间的夹角是否大于5度，如果不是则保留该节点，如果是，记i为当前节点的索引，p_f_s[i]为当前节点，p_f_s[i-1]为上一个节点，p_f_s[i+1]为下一个节点，temp等于p_f_s[i]，根据公式temp＝temp+0.8*(p_f_s[i]-temp)+0.2*(p_f_s[i-1]–2*temp+p_f_s[i+1])来更新temp；

步骤4-2-3：计算p_f_s[i-1]与temp的连线上的点，temp与p_f_s[i+1]的连线上的点，判断这些点在栅格地图map中的数值是否等于终点在栅格地图map中的数值，统计不相等的次数，如果次数小于等于阈值6，将temp赋值给p_f_s[i]来更新path_filtered_smoothed，如果次数大于阈值6，则保留该节点。

本实施例中，用于消除转弯夹角大于5度的转弯，将大于5度的转弯进行角度分解，进行多次转弯，保证每次转弯角度平滑。

以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (4)

1.一种用于内河智能船舶的自主航线规划方法，其特征在于，包括以下步骤：

步骤1：构建基于航线的栅格地图，所述栅格地图包括内河上用于标示路线的航标挺、不可航向区域、可航行区域、障碍物缓冲区、上行区域、下行区域以及上下行边间缓冲区，栅格地图的生成方法具体包括：

步骤1-1：采集两列航标挺的GPS坐标，将坐标转换到墨卡托坐标系下；

步骤1-2：设定按上行或者下行的顺序分别存放两列航标艇的墨卡托坐标到两个容器中记为mark1_list和mark2_list，mark1_list和mark2_list中最大和最小的x坐标记为max_x和min_x，最大和最小的y坐标记为max_y和min_y，障碍物缓冲区的距离记为inflation_radius，上下行边界的缓冲区距离记为inflation_radius_min；

步骤1-3：设定栅格地图记为map，栅格地图的分辨率记为map_resolution，宽和高记为map_width和map_height，大小为map_width*map_height，在map中-100代表障碍物，-3代表障碍物缓冲区，-2代表上下行边界缓冲区，-1代表初始值，0代表被访问过，1代表可航行，11代表靠近mark1_list的可航行区域，22代表靠近mark2_list的可航行区域，11和22区分了上下行区域

步骤2：在栅格地图中确定可航行航道；可航行航道的确定方法具体包括：

步骤2-1：遍历mark1_list和mark2_list找出max_x，min_x，max_y和min_y；

步骤2-2：计算栅格地图的宽高，map_width＝(max_x–min_x)/map_resolution+1，map_height＝(max_y–min_y)/map_resolution+1，然后用map_width和map_height初始化map；

步骤2-3：新建一个空的容器记为open_list，从mark1_list中取出第一个数据放入open_list中；

步骤2-4：判断open_list是否为空，如果是则进入步骤2-6，如果不是，取出open_list中的第一个数据记为current_node，并从open_list中删除current_node，判断current_node是否被访问过，如果被访问过则重新开始步骤2-4，如果没被访问过，设置current_node为被访问过，然后计算出current_node在墨卡托坐标系中的上下左右四个邻居记为up，down，left和right，以及在栅格地图中的上下左右四个邻居记为map_up，map_down，map_left和map_right；

步骤2-5：对上邻居做处理，判断map_up有没有超出栅格地图，如果超出则放弃该map_up，如果没超出，判断map_up在map中的数值是否为-1，如果不是则放弃该map_up，如果是，将该数值设置为0，然后判断up是否在可航行航道内，如果不是则放弃该map_up，如果是，将map_up在map中的数值设置为1，并将up放入open_list中，然后计算出up距离mark1_list的距离distance1和距离mark2_list的距离distance2，如果distance1或者distance2小于inflation_radius则将map_up在map中的数值设置为-3，否则如果distance1和distance2的差值的绝对值小于inflation_radius_min则将map_up在map中的数值设置为-2，否则如果distance1小于distance2则将map_up在map中的数值设置为11，否则将map_up在map中的数值设置为22.对剩余的下左右三个邻居做同样的处理，然后返回到第4步；

步骤2-6：将map_width，map_height，map_resolution，min_x，min_y和map保存到一个文本文件，完成在栅格地图中确定可航行航道

步骤3：基于A星算法，在可航行航道上生成最短航线；

步骤4：对最短航线进行长直线形处理和平滑处理，得到适合智能船舶航行的最佳航线。

2.如权利要求1所述的用于内河智能船舶的自主航线规划方法，其特征在于，步骤3中给定起点和终点，将起点和终点转换到栅格地图当中，使用A星算法在栅格地图中得到一条最短航线，设定经A星算法得到的最短航线的航线的节点存储在容器path中，经过长直线形处理的存储在容器path_filtered中，最后优化过的存储在容器path_filtered_smoothed中。

3.如权利要求2所述的用于内河智能船舶的自主航线规划方法，其特征在于，步骤4中的长直线形处理具体包括：

步骤4-1-1：将path中的最后一个节点放入path_filtered中，然后记upper为path中节点的索引，upper的初始值为path最后一个节点的索引值，记0为path第一个节点的索引值；

步骤4-1-2：对path在[0,upper)索引区间内逐个取节点，当前取到的索引记为i，如果i大于或等于upper则进入步骤4-1-5，如果i小于upper，取path中索引为i的节点，记为path[i]；

步骤4-1-3：计算path[i]与path_filtered中最后一个节点的连线上的点，判断这些点在栅格地图map中的数值是否等于终点在栅格地图map中的数值，统计不相等的次数，如果次数小于等于阈值3，将path[i]放入path_filtered中，并设置upper为i，进入步骤4-1-4，如果次数大于阈值3，判断i是否等于upper–1，如果不是则进入步骤4-1-4，如果是，将path[i]加入path_filtered中，并设置upper为i，进入步骤4-1-4；

步骤4-1-4：判断path的第一个节点是否等于path_filtered的最后一个节点，如果是则进入步骤4-1-5，如果不是则返回步骤4-1-2；

步骤4-1-5：将path_filtered中的节点的顺序颠倒，完成长直线形处理。

4.如权利要求2所述的用于内河智能船舶的自主航线规划方法，其特征在于，步骤4中的平滑处理具体包括：

步骤4-2-1：将path_filtered赋值给path_filtered_smoothed；

步骤4-2-2：对于path_filtered_smoothed中的第二个节点到最后第二个节点，判断每个节点的前后两条航线之间的夹角是否大于5度，如果不是则保留该节点，如果是，记i为当前节点的索引，p_f_s[i]为当前节点，p_f_s[i-1]为上一个节点，p_f_s[i+1]为下一个节点，temp等于p_f_s[i]，根据公式temp＝temp+0.8*(p_f_s[i]-temp)+0.2*(p_f_s[i-1]–2*temp+p_f_s[i+1])来更新temp；

步骤4-2-3：计算p_f_s[i-1]与temp的连线上的点，temp与p_f_s[i+1]的连线上的点，判断这些点在栅格地图map中的数值是否等于终点在栅格地图map中的数值，统计不相等的次数，如果次数小于等于阈值6，将temp赋值给p_f_s[i]来更新path_filtered_smoothed，如果次数大于阈值6，则保留该节点。

Images