CN110006429A - 一种基于深度优化的无人船航迹规划方法 - Google Patents

Abstract

本发明属于无人驾驶船舶航行控制技术领域，尤其涉及一种基于深度优化的无人船航迹规划方法。包括如下步骤：读取栅格化的地图信息、设置起止点，将获得的起点及沿该起点八个方向的可达邻居节点加入openlist列表中；寻找openlist列表中F值最小的点，设为当前节点；判断当前节点是否为终点，若是结束寻路，若不是，利用当前节点，基于跳点搜索策略寻找下一跳点；判断跳点是否在openlist列表中，继续寻找F值最小的点；输出最优路径。该方法基于跳点搜索优化预处理算法大幅提升栅格法航迹规划算法的执行效率，且优化路径转折点，最终输出一条平滑的航行轨迹。

Description

一种基于深度优化的无人船航迹规划方法

技术领域

本发明属于无人驾驶船舶航行控制技术领域，尤其涉及一种基于深度优化的无人船航迹规划方法。

背景技术

船舶航迹规划是指船舶能够根据航行环境在保证安全的前提下(避开各种动态或静态的碍航物)自主规划出一条最优路径(即路径最短)。

其中，基于栅格法的路径寻优是目前最常用且最有效的一种航迹规划方法。栅格法是指将背景地图网格化，然后基于网格进行路径规划。

但目前应用的栅格法航迹规划算法仍存在较多的缺陷，还有深度优化的空间。首先，由于该算法遍历点过多，会导致执行速度降低，执行效率下降。其次，由于该算法是基于网格寻点，所以规划出的路径是由几条折线组成，不平滑，在实际运行当中，会增加船舶在转折点处的控制难度，不利于其循迹航行。

发明内容

(一)要解决的技术问题

针对现有存在的技术问题，本发明提供一种基于深度优化的无人船航迹规划方法，该方法基于跳点搜索优化预处理算法大幅提升栅格法航迹规划算法的执行效率，再通过二阶贝塞尔曲线优化路径转折点，最终输出一条平滑的航行轨迹。

(二)技术方案

一种基于深度优化的无人船航迹规划方法，包括如下步骤：

A1、基于A星算法获得栅格化地图的起点和终点，初始化openlist列表和closedlist列表；

A2、遍历openlist列表，寻找openlist列表中F值最小的点，设为当前节点；

判断当前节点是否为终点，若当前节点是终点，则寻路结束，执行步骤A6，若不是，在openlist列表中删除当前节点，将该当前节点加入closedlist列表继续执行步骤A3；

A3、利用当前节点，基于跳点搜索策略寻找下一跳点，具体如下：

判断当前节点的方向，所述当前节点的方向通过当前节点的父节点到当前节点的方向确定，若当前节点的方向为直线方向，则依据以下三种情况寻找跳点：

(1)如果当前节点左后方不可走且左方可走，即左方是强迫邻居，则沿当前节点左前方和左方寻找不在closedlist列表中的跳点；

(2)如果当前节点当前方向可走，则沿当前节点当前方向寻找不在closedlist列表中的跳点；

(3)如果当前节点右后方不可走且右方可走，即右方是强迫邻居，则沿当前节点右前方和右方寻找不在closedlist列表中的跳点；

若当前节点当前方向为对角线方向，则依据以下三种情况寻找跳点：

(1)如果当前节点当前方向的水平分量可走，则沿当前节点当前方向的水平分量寻找不在closedlist列表中的跳点；

(2)如果当前节点当前方向可走，则沿当前节点当前方向寻找不在closedlist的跳点；

(3)如果当前节点当前方向的垂直分量可走，则沿当前节点当前方向的垂直分量寻找不在closedlist的跳点；

A4、判断跳点是否在openlist列表中，若是，执行步骤A5，若不是，将跳点加入openlist列表中后执行步骤A5；

A5、更新跳点的G值和跳点的父节点，若该跳点的G值小于当前节点的G值，则该跳点的父节点为当前节点，将该跳点从openlist列表中删除并加入closedlist列表，同时更新该跳点的F值和该跳点的G值，否则，返回步骤A2，继续利用A星算法寻找openlist列表中F值最小的点，作为新的当前节点，重复步骤A2～A5；

A6、将最终规划出的路径输出，所述路径是根据closedlist列表中当前节点集合输出的折线路径；

A7、对所述折线路径进行优化，得到最终路径。

进一步地，所述步骤A1包括如下步骤：

A11、读取栅格化的地图信息、设置起点和终点；

A12、将步骤A11获得的起点及沿该起点八个方向的可达邻居节点加入openlist列表中，将步骤A11获得的起点和终点加入closedlist列表中。

进一步地，所述步骤A11包括如下步骤：

A111、根据船舶的感知模块获取船舶当前的位置坐标作为原始起点，根据当前航行任务获取目标点的位置坐标作为终点；

A112、读取栅格化的地图环境，将地图环境中的障碍物坐标存储到障碍物列表中，并将地图环境分为可行区域和不可行区域；

A113、初始化openlist列表和closedlist列表，所述openlist列表用于存储待判断航路点，所述closedlist列表用于存储已判断航路点；

A114、判断原始起点和终点是否在同一连通区：若不在，说明原始起点和终点之间无可行路线，返回步骤A111，重新设置获得起点，若在，则将原始起点作为起点，继续执行步骤A12。

进一步地，在所述步骤A114中，采用宽度优先搜索算法判断原始起点和终点是否在同一连通区，具体地：将原始起点和与其相连通的所有可行区域的网格进行统一编号，将终点和与其相连通的所有可行区域的网格进行统一编号，若两者的可行区域有交叉，则他们的编号将一致，说明有可行路线；若两者的可行区域无交叉，则他们的编号将不同，说明无可行路线。

进一步地，所述强迫邻居的定义如下：如果节点a是节点b的邻居，并且节点a的邻居中有障碍物，并且从节点b的父节点到节点b再到节点a的路径长度比其他任何从节点b的父节点到节点a且不经过节点b的路径短，则节点a为节点b的强迫邻居，节点b为节点a的跳点。

进一步地，所述跳点的定义如下：

如果节点y是起点或终点，则节点y是跳点；

如果节点y有邻居且是强迫邻居，则节点y是跳点；

如果从节点y的父节点到节点y是对角线移动，并且节点y经过水平或垂直方向移动可以到达跳点，则节点y是跳点。

进一步地，如果直线方向以及对角线方向都可以移动，则首先在直线方向搜索跳点，再在对角线方向搜索跳点。

进一步地，所述步骤A7中采用二阶贝塞尔曲线对折线路径进行优化。

进一步地，所述步骤A2中，F值表示路径的总代价，F＝G+H；

其中，G表示原始起点到当前节点的路径耗费，H表示当前节点到终点的不考虑障碍物的直线距离的路径耗费。

(三)有益效果

本发明提出的一种基于深度优化的无人船航迹规划方法，通过跳点搜索优化预处理算法，大幅优化算法路径寻找的速度。跳点搜索优化预处理算法为节点剪枝技术，不同于A星算法中直接获取当前节点所有非关闭的可达邻居节点来进行拓展的策略，跳点搜索根据当前节点的方向、并基于跳点的策略(遵循两个定义，三个规则)来扩展后继节点，因此大幅度减少了节点扩展的数量，加快了寻路的速度。

与传统的跳点搜索算法不同，在跳点搜索优化预处理算法中加入了节点剪枝优化以及不可达两点提前判断等方法，可以将传统A星算法的执行效率提高多个数量级，可以节省绝大多数运行内存，加快运行速度，提高运行效率，并且可以应用在复杂多变的航行环境的动态路线规划当中。

最后，用二阶贝塞尔曲线对A星输出的路径进行优化，可以平滑转折点处的路径，更有利于船舶的循迹航行。通过该方法对栅格法航迹规划算法进行深度优化，在大幅度提升算法执行效率的基础上，规划出可以避开障碍物的最短且平滑的航行轨迹。同时，该算法规划出的航行路线安全性高、可靠性高且能保证航行的最大经济化。

另外，通过对该算法进行适当处理，可以应用在任何情景当中的路径规划，如无人车/机的航迹规划等等且都能达到跟船舶航迹规划同样的效果。

附图说明

图1为本发明方法的流程图；

图2为本发明方法的应用流程图；

图3为仿真实验中A星算法仿真结果图；

图4为仿真实验中传统跳点搜索算法的结果图；

图5为仿真实验中基于深度优化的A星算法的结果图。

具体实施方式

为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。

本发明为了更好的阐述，以A星算法为例介绍本发明的应用，提供一种基于深度优化的无人船航迹规划方法，如图1所示，具体包括如下步骤：

A1、基于A星算法获得栅格化地图的起点和终点，初始化openlist列表和closedlist列表。具体如下：

A11、读取栅格化的地图信息、设置起点和终点；

A12、将步骤A11获得的起点及沿该起点八个方向的可达邻居节点加入openlist列表中，将步骤A11获得的起点和终点加入closedlist列表中。

进一步地，所述步骤A11包括如下步骤：

A111、根据船舶的感知模块获取船舶当前的位置坐标作为原始起点，根据当前航行任务获取目标点的位置坐标作为终点；

A112、读取栅格化的地图环境，将地图环境中的障碍物坐标存储到障碍物列表中，并将地图环境分为可行区域和不可行区域；

A113、初始化openlist列表和closedlist列表，所述openlist列表用于存储待判断航路点，所述closedlist列表用于存储已判断航路点；

其中，openlist列表内的节点有待进一步探索扩展，closedlist列表内的节点后续不再进行扩展；

A114、判断原始起点和终点是否在同一连通区：若不在，说明原始起点和终点之间无可行路线，返回步骤A111，重新设置获得起点，若在，则将原始起点作为起点，继续执行步骤A12。

传统寻路算法中，如果起止点间无可行路线，寻路算法仍然会花费时间去寻找起止点间的路径，而且失败情况下寻路花费的时间远大于成功情况下寻路花费的时间，因为失败情况下需要遍历所有的路径，才能确定两点不可达。因此为了避免这种情况，在每次寻路之前，判断起点和终点是否可达：如果起点和终点在同一连通区域，则起点和终点可达，否则不可达。只有起点和终点可达，才需要去寻路。

优选地，本发明中采用宽度优先搜索算法判断原始起点和终点是否在同一连通区，具体地：将原始起点和与其相连通的所有可行区域的网格进行统一编号，将终点和与其相连通的所有可行区域的网格进行统一编号，若两者的可行区域有交叉，则他们的编号将一致，说明有可行路线；若两者的可行区域无交叉，则他们的编号将不同，说明无可行路线。

A2、遍历openlist列表，寻找openlist列表中F值最小的点，设为当前节点。其中：F＝G+H，F表示路径的总代价，G表示原始起点到当前节点的路径耗费，H表示当前节点到终点的不考虑障碍物的直线距离的路径耗费。

进一步地，判断当前节点是否为终点，若当前节点是终点，则寻路结束，执行步骤A6，若不是，在openlist列表中删除当前节点，将该当前节点加入closedlist列表继续执行步骤A3。

A3、利用当前节点，基于跳点搜索策略寻找下一跳点，具体如下：

判断当前节点的方向，所述当前节点的方向通过当前节点的父节点到当前节点的方向确定。其中，父节点为当前节点的上一个节点，例如：以起点开始搜索第一个符合要求的当前节点，起点就是当前节点的父节点，再以当前节点搜索下一个符合要求的当前节点，那么上一个当前节点就是下一个当前节点的父节点。

若当前节点的方向为直线方向，则依据以下三种情况寻找跳点：

(1)如果当前节点左后方不可走且左方可走，即左方是强迫邻居，则沿当前节点左前方和左方寻找不在closedlist列表中的跳点；

(2)如果当前节点当前方向可走，则沿当前节点当前方向寻找不在closedlist列表中的跳点；

(3)如果当前节点右后方不可走且右方可走，即右方是强迫邻居，则沿当前节点右前方和右方寻找不在closedlist列表中的跳点；

若当前节点当前方向为对角线方向，则依据以下三种情况寻找跳点：

(1)如果当前节点当前方向的水平分量可走(如当前节点当前为东北方向，则水平分量为东)，则沿当前节点当前方向的水平分量寻找不在closedlist列表中的跳点；

(2)如果当前节点当前方向可走，则沿当前节点当前方向寻找不在closedlist的跳点；

(3)如果当前节点当前方向的垂直分量可走(如当前节点当前为东北方向，则垂直分量为北)，则沿当前节点当前方向的垂直分量寻找不在closedlist的跳点。

在上述寻找跳点的过程中，遵循“两个定义、三个规则”：

定义一，强迫邻居：如果节点a是节点b的邻居，并且节点a的邻居中有障碍物(不可行区域)，并且从节点b的父节点(该节点的上一个节点)到节点b再到节点a的路径长度比其他任何从节点b的父节点到节a且不经过节点b的路径短，则节点a为节点b的强迫邻居，节点b为节点a的跳点。

定义二，跳点：如果节点y是起点或终点，则节点y是跳点；

如果节点y有邻居且是强迫邻居，则节点y是跳点；

如果从节点y的父节点到节点y是对角线移动，并且节点y经过水平或垂直方向移动可以到达跳点，则节点y是跳点。

规则一：在搜索跳点的过程中，如果直线方向以及对角线方向都可以移动，则首先在直线方向搜索跳点，再在对角线方向搜索跳点。

规则二：如果从节点a的父节点到节点a是直线移动，节点b是节点a的邻居，若有从节点a的父节点到节点b的路径不经过节点a且路径长度小于或等于从节点a的父节点经过节点a到节点b的路径，则走到节点a后下一个点不会走到节点b；

如果从节点a的父节点到节点a是对角线移动，节点b是节点a的邻居，若有从节点b的父节点到节点a的路径不经过节点a且路径长度小于从节点a的父节点经过节点a到节点b的路径，则走到节点a后下一个点不会走到节点b。

规则三：只有跳点才会加入openlist列表，因为跳点会改变行走方向，而非跳点不会改变行走方向，最后寻找出来的路径点也只会是跳点集合的子集。

在传统A星算法中，会沿着当前节点的八个方向中寻找当前节点不在closedlist中的可达邻居节点，而本发明中的跳点搜索基于搜索规则只探索有效节点，大幅度削减冗余节点，提高了算法执行效率，且与传统跳点搜索算法不同，通过对搜索规则优化(参考定义二)，进一步对中间跳点进行剪枝，从而避免冗余的节点拓展运算，将算法的执行效率提升一个数量级。

A4、判断跳点是否在openlist列表中，若是，执行步骤A5，若不是，将跳点加入openlist列表中后执行步骤A5；

A5、更新跳点的G值和跳点的父节点，若该跳点的G值小于当前节点的G值，则该跳点的父节点为当前节点，将该跳点从openlist列表中删除并加入closedlist列表，同时更新该跳点的F值和该跳点的G值，否则，返回步骤A2，继续利用A星算法寻找openlist列表中F值最小的点，作为新的当前节点，重复步骤A2～A5；

A6、将最终规划出的路径输出，所述路径是根据closedlist列表中当前节点集合输出的折线路径；

A7、对步骤A6输出的折线路径进行二阶贝塞尔曲线优化，使转折点处的路径趋于平滑。这样，在实际运行当中，会降低船舶在转折点处的控制难度，有利于其循迹航行。

综上所述，本发明提出的基于深度优化的无人船航迹规划方法，通过跳点搜索优化预处理算法，大幅优化算法路径寻找的速度。跳点搜索优化预处理算法为节点剪枝技术，不同于A星算法中直接获取当前节点所有非关闭的可达邻居节点来进行拓展的策略，跳点搜索根据当前节点的方向、并基于跳点的策略(遵循两个定义，三个规则)来扩展后继节点，因此大幅度减少了节点扩展的数量，加快了寻路的速度。

与传统的跳点搜索算法不同，在跳点搜索优化预处理算法中加入了节点剪枝优化以及不可达两点提前判断等方法，可以将传统A星算法的执行效率提高多个数量级，可以节省绝大多数运行内存，加快运行速度，提高运行效率，并且可以应用在复杂多变的航行环境的动态路线规划当中。

基于栅格法的航迹规划算法有多种节点扩展的方法，除了本发明提到的A星算法，还有Dijkstra路径规划算法、最佳优先搜索(Best-FirstSearch)、深度优先搜索(DepthFirst Search)、宽度优先搜索算法(又称广度优先搜索Breadth First Search)等基于网格寻点的路径规划算法，通过本发明提供的相关方法，均可以对上述算法进行深度优化，具体步骤如图2：

在通过上述算法进行航迹规划时，首先应用本发明判断起止点间有无可行路线，然后应用相应算法原理进行路径搜索，其中在探索下一节点时候，将原算法中的节点探索方法更换为基于跳点搜索策略的节点探索，最后对输出的路径进行二阶贝塞尔曲线优化，使得路线平滑，即完成了相应算法的深度优化。

仿真实验：

如图3～图5所示，分别为A星算法仿真结果、传统跳点搜索结果和基于深度优化的A星算法仿真结果，其中：灰色网格代表障碍物，深灰色网格为路径起点和终点，浅灰色网格为算法搜索到的路径节点，黑色线段为算法规划出的最终路径。

如表1所示，为上述三种方法的仿真结果对比。

表1三种方法仿真结果对比

算法	A星算法	传统跳点搜索算法	深度优化的A星算法
				扩展节点数	98	10	4
仿真时间(ms)	2.8670	1.4983	0.3658
				路径长度	15.31	15.31	15.31

结合表1和图3～图5可知，基于深度优化的A星算法在路径探索过程中历经更少的跳点，极大地提高了搜索效率。

以上结合具体实施例描述了本发明的技术原理，这些描述只是为了解释本发明的原理，不能以任何方式解释为对本发明保护范围的限制。基于此处解释，本领域的技术人员不需要付出创造性的劳动即可联想到本发明的其它具体实施方式，这些方式都将落入本发明的保护范围之内。

Claims (9)

1.一种基于深度优化的无人船航迹规划方法，其特征在于，包括如下步骤：

A1、基于A星算法获得栅格化地图的起点和终点，初始化openlist列表和closedlist列表；

A2、遍历openlist列表，寻找openlist列表中F值最小的点，设为当前节点；

判断当前节点是否为终点，若当前节点是终点，则寻路结束，执行步骤A6，若不是，在openlist列表中删除当前节点，将该当前节点加入closedlist列表继续执行步骤A3；

A3、利用当前节点，基于跳点搜索策略寻找下一跳点，具体如下：

判断当前节点的方向，所述当前节点的方向通过当前节点的父节点到当前节点的方向确定，若当前节点的方向为直线方向，则依据以下三种情况寻找跳点：

(1)如果当前节点左后方不可走且左方可走，即左方是强迫邻居，则沿当前节点左前方和左方寻找不在closedlist列表中的跳点；

(2)如果当前节点当前方向可走，则沿当前节点当前方向寻找不在closedlist列表中的跳点；

(3)如果当前节点右后方不可走且右方可走，即右方是强迫邻居，则沿当前节点右前方和右方寻找不在closedlist列表中的跳点；

若当前节点当前方向为对角线方向，则依据以下三种情况寻找跳点：

(1)如果当前节点当前方向的水平分量可走，则沿当前节点当前方向的水平分量寻找不在closedlist列表中的跳点；

(2)如果当前节点当前方向可走，则沿当前节点当前方向寻找不在closedlist的跳点；

(3)如果当前节点当前方向的垂直分量可走，则沿当前节点当前方向的垂直分量寻找不在closedlist的跳点；

A4、判断跳点是否在openlist列表中，若是，执行步骤A5，若不是，将跳点加入openlist列表中后执行步骤A5；

A5、更新跳点的G值和跳点的父节点，若该跳点的G值小于当前节点的G值，则该跳点的父节点为当前节点，将该跳点从openlist列表中删除并加入closedlist列表，同时更新该跳点的F值和该跳点的G值，否则，返回步骤A2，继续利用A星算法寻找openlist列表中F值最小的点，作为新的当前节点，重复步骤A2～A5；

A6、将最终规划出的路径输出，所述路径是根据closedlist列表中当前节点集合输出的折线路径；

A7、对所述折线路径进行优化，得到最终路径。

2.根据权利要求1所述的基于深度优化的无人船航迹规划方法，其特征在于，所述步骤A1包括如下步骤：

A11、读取栅格化的地图信息、设置起点和终点；

A12、将步骤A11获得的起点及沿该起点八个方向的可达邻居节点加入openlist列表中，将步骤A11获得的起点和终点加入closedlist列表中。

3.根据权利要求2所述的基于深度优化的无人船航迹规划方法，其特征在于，所述步骤A11包括如下步骤：

A111、根据船舶的感知模块获取船舶当前的位置坐标作为原始起点，根据当前航行任务获取目标点的位置坐标作为终点；

A112、读取栅格化的地图环境，将地图环境中的障碍物坐标存储到障碍物列表中，并将地图环境分为可行区域和不可行区域；

A113、初始化openlist列表和closedlist列表，所述openlist列表用于存储待判断航路点，所述closedlist列表用于存储已判断航路点；

A114、判断原始起点和终点是否在同一连通区：若不在，说明原始起点和终点之间无可行路线，返回步骤A111，重新设置获得起点，若在，则将原始起点作为起点，继续执行步骤A12。

4.根据权利要求3所述的基于深度优化的无人船航迹规划方法，其特征在于，在所述步骤A114中，采用宽度优先搜索算法判断原始起点和终点是否在同一连通区，具体地：将原始起点和与其相连通的所有可行区域的网格进行统一编号，将终点和与其相连通的所有可行区域的网格进行统一编号，若两者的可行区域有交叉，则他们的编号将一致，说明有可行路线；若两者的可行区域无交叉，则他们的编号将不同，说明无可行路线。

5.根据权利要求4所述的基于深度优化的无人船航迹规划方法，其特征在于，所述强迫邻居的定义如下：如果节点a是节点b的邻居，并且节点a的邻居中有障碍物，并且从节点b的父节点到节点b再到节点a的路径长度比其他任何从节点b的父节点到节点a且不经过节点b的路径短，则节点a为节点b的强迫邻居，节点b为节点a的跳点。

6.根据权利要求4所述的基于深度优化的无人船航迹规划方法，其特征在于，所述跳点的定义如下：

如果节点y是起点或终点，则节点y是跳点；

如果节点y有邻居且是强迫邻居，则节点y是跳点；

如果从节点y的父节点到节点y是对角线移动，并且节点y经过水平或垂直方向移动可以到达跳点，则节点y是跳点。

7.根据权利要求1所述的基于深度优化的无人船航迹规划方法，其特征在于，如果直线方向以及对角线方向都可以移动，则首先在直线方向搜索跳点，再在对角线方向搜索跳点。

8.根据权利要求1所述的基于深度优化的无人船航迹规划方法，其特征在于，所述步骤A7中采用二阶贝塞尔曲线对折线路径进行优化。

9.根据权利要求3所述的基于深度优化的无人船航迹规划方法，其特征在于，所述步骤A2中，F值表示路径的总代价，F＝G+H；

其中，G表示原始起点到当前节点的路径耗费，H表示当前节点到终点的不考虑障碍物的直线距离的路径耗费。