CN114674336A - 基于线段的路径规划方法 - Google Patents

Abstract

本发明提供一种基于线段的路径规划方法，该方法包括以下步骤：步骤1.接收预先绘制好的线段集合，对所有线段集合中的坐标进行初始化节点并进行关联。步骤2.根据路径规划的起点，查找当前车辆所在的路线节点，并将该节点加入open集合中。步骤3.循环取出open中代价值最低的一个节点，从open集合中删除该节点，加入到close集合中，并比较节点信息中的坐标是否和终点坐标相等，相等则将final_path节点指向当前节点，跳出循环，执行步骤5，不相等则执行步骤4。本发明的优点因为是基于已绘制好的线段集合，不必考虑空间复杂性，所需计算量较小，算法时间可以得到控制。

Description

基于线段的路径规划方法

技术领域

本发明涉及一种基于线段的路径规划方法。

背景技术

AStar算法是在Dijkstra算法基础上发展出来的，是在静态路径中用于求解最优路径有效的直接搜索算法，比Dijkstra算法多了一个启发式的搜索函数，也就是通过一个代价函数来确定搜索方向，即从起点开始向周围扩张，通过代价函数，计算得到周围每个节点的代价值，选出最小代价节点作为下一个扩展点，重复这个过程直到到达目标点。AStar算法具有以下优点：1)Astart算法的整体搜索规模比Dijkstra算法小不少，其搜索具有一定的指向性，会促使整体搜索方向朝向目标点，使得搜索效率较高，很适合在开放空间中的路径规划；2)Astart算法拥有与Dijkstra算法相同的最终确定唯一最优解，具有很好的收敛性，是一个优良的搜索算法。同时AStar算法具有以下缺点：1)不论选择冯诺依曼邻域还是摩尔邻域，其计算规模仍然较大，对于较复杂路径较大空间的搜索，其路径计算往往不能达到很强的实时性；2)Astart算法整体需要求解出确切路径后才能知晓真正最短路径的方向，而任何过程中的计算都存在试错的过程，所以其适用于整体路径规划完毕后的执行，而不适合边执行边规划的任务作业，当然对于实时性要求很强的任务作业，可以采用分段的方式进行Astart算法，不过这样的分段策略和衔接策略就需要更详细的讨论和调优。

然而传统基于已绘制的线段集合进行路径规划的算法是使用穷举法，从当前线段往相交线段查找，因为并没有引入代价值的概念，所以传统算法会规划出所有可能性的路径，然后再从所有可行线段中找出合适的线段，这无疑是增加算法的计算量和时间，算法效率低下，而且在复杂场景不能实时计算。

发明内容

本发明目的是：提供一种基于线段的路径规划方法，使得搜索效率较高，具有很好的收敛性，所需计算量较小，算法时间可以得到控制。

本发明的技术方案是：1.一种基于线段的路径规划方法，其特征在于其包括如下步骤:

步骤S1：接收预先绘制好的线段集合，对所有线段集合中的坐标进行初始化路线节点并进行关联，返回起点的交点节点；

步骤S2：接收步骤1返回的起点交点节点，并将该节点加入open集合中；

步骤S3：循环取出open集合中代价值最低的一个节点，从open集合中删除该节点，加入到close集合中，并比较节点信息中的坐标是否和终点坐标相等，相等则将final_path节点指向当前节点，跳出循环，执行步骤S5，不相等则执行步骤S4；

步骤S4：根据关联过后的路线节点集合，当前所在路线节点信息，查找所在路线的相邻交点节点集合，进行代价值计算，将交点节点加入open集合中，继续执行步骤S3；

步骤S5：取出final_path节点中的路径信息，反馈给调用程序，路径规划成功。

在上述技术方案的基础上，进一步包括如下附属技术方案：

优选地，在步骤S1中，所述初始化路线节点并进行关联包括如下步骤：

步骤S1-1：接收线段集合，起点坐标信息，初始化回传节点集合；

步骤S1-2：对线段集合进行遍历，判断起点坐标在哪条路线上,并初始化路线节点lineNode；

步骤S1-3：逐一查找当前路线和其他路线的交点坐标，并将交点坐标初始化为交点节点Node，保存两条路线的ID放入节点的父路线索引Parent_index集合中，将节点与路线ID关联，其中判定两条线段是否相交的条件为：起点或终点坐标相等；两线端首尾相连；根据外积结果判定是否交于线上；

步骤S1-4：满足其中一个条件则认为两线段相交，并获取交点坐标，计算交点坐标与起点距离，计算方法位两点间距离公式，公式为：

将距离值作为距离起点代价值保存，生成当前路线节点，并对当前路线中所包含的交点节点集合按照距离起点距离从大到小排序；

步骤S1-5，循环遍历后得到当前线段和其他所有线段的交点集合，将所有交点集合放入到lineNode路线节点中，如此循环得到每条线段与其他线段的交点节点集合及所有路线节点集合，最终返回起点的交点节点信息。优选地，在步骤S3中所述循环取出open中的路线信息的步骤包括：

步骤S3-1：循环取出open集合中的交点节点信息，取出代价值最低的一个节点，代价值计算方法为：

Cost＝F+angle_change＝G+H+angle_change

其中Cost为总的代价值，F＝G+H,G为从起点到当前节点的距离累加值，H为当前节点到终点坐标的直线距离值，angle_change为当前路线到相邻路线的角度变换值；

步骤S3-2：从open集合中取出代价值最低的节点，open集合中已按Cost代价值从小到大排序，使用直接取出集合中第一个节点即可，取出节点，从open集合中删除，再加入到close集合中。

在步骤S4中查找相邻的交点节点加入open集合的方法的步骤包括：

步骤S4-1：根据当前Node节点中的父路线索引Parent_index，在路线节点集合中查找当前节点最近的其他交点节点，将所有节点放入集合中返回；

步骤S4-2：根据得到的最近的其他交点节点集合，按照步骤S3-1进行代价值计算，并查找当前交点节点是否在close集合和open集合中，如果在close集合中则不进行操作，直接查找下一交点节点；如果当前交点节点在open集合中，则在对比已有交点节点和当前交点节点的代价值，如果当前交点节点代价值比已有交点节点代价值更低，则对已有的交点节点代价值重新计算，并重新指定父节点，否则不进行计算；如果当前交点节点不在open集合中，则将节点放入到open集合中。

优选地，在步骤S5中取出final_path节点中的路径信息方法包括：

步骤S5-1：循环判断当前节点是否有父节点，有则将当前坐标保存，并将父节点指向当前节点；当当前节点无父节点则认为已取出所有节点，路径规划完成，返回坐标信息，程序退出。

本发明的优点：

(1)其搜索具有一定的指向性，会促使整体搜索方向朝向目标点，使得搜索效率较高。

(2)基于AStar算法思想，具有最终确定唯一最优解，具有很好的收敛性，是一个优良的搜索算法。

(3)因为是基于已绘制好的线段集合，不必考虑空间复杂性，所需计算量较小(只需遍历所有路线交点作为相邻点搜索)，算法时间可以得到控制。

附图说明

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

图1是本发明的流程图；

图2是本发明中对线段合集进行初始化并进行节点关联的流程图；

图3是本发明中基于线段节点合集使用AStar算法思想的流程图；

图4是本发明中路线节点和交点节点的示意图。

具体实施方式

实施例：为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，以下实施例结合附图对本发明基于线段的路径规划方法作进一步说明。

如图1所示，基于线段的路径规划方法包含以下步骤：

步骤S1，接收预先绘制好的线段集合，对所有线段集合中的坐标进行初始化节点并进行关联。其中需要预先定义路线节点(lineNode),lineNode中包含交点节点(Node)，Node节点中包含父路线索引(Parent_index)。

其中初始化并对节点进行关联的步骤包括如下：

步骤S1-1，接收线段集合，起点坐标信息，初始化回传节点集合。

步骤S1-2，对线段集合进行遍历，判断起点坐标在哪条路线上,并初始化路线节点(lineNode)。

步骤S1-3，逐一查找当前路线和其他路线的交点坐标，并将交点坐标初始化为交点节点(Node)，保存两条路线的标识号(ID)放入节点的父路线索引Parent_index集合中，将节点与路线的标识号(ID)关联。

其中判定两条线段是否相交的条件为：

a)起点或终点坐标相等

b)两线端首尾相连

c)根据外积结果判定是否交于线上

步骤S1-4，满足其中一个条件则认为两线段相交，并获取交点坐标，计算交点坐标与起点距离。

将距离值作为距离起点代价值保存，生成当前路线节点，并对当前路线中所包含的节点集合按照距离起点距离从大到小排序。

步骤S1-5，循环遍历后得到当前线段和其他所有线段的交点集合。将所有交点集合放入到lineNode路线节点中，如此循环可得到每条线段与其他线段的交点节点集合及所有路线节点集合。最终返回起点的交点节点信息。

步骤S2.接收步骤S1返回的起点的交点节点，并将该节点加入open集合中。并初始化Node节点final_path。

步骤S3.循环取出open中代价值最低的一个节点，从open集合中删除该节点，加入到close集合中，并比较节点信息中的坐标是否和终点坐标相等，相等则将final_path节点指向当前节点，跳出循环，执行S5，不相等则执行S4，open集合和close集合为C++中的map数据类型，键值为交点节点的代价值，值为交点节点数据类型,map数据类型默认以从小到大排序。当open集合不为空时，说明还有未扩展的节点，open集合中代价值最小的节点，等价于最接近终点的节点，循环取出open集合中代价值最低的节点放入close集合中就是从起点向终点靠近的过程。

其中循环取出open集合中的路线信息的步骤包括如下：

步骤S3-1，循环取出open集合中的交点节点信息，取出代价值最低的一个交点节点，代价值计算方法为：

Cost＝F+angle_change＝G+H+angle_change

其中Cost为总的代价值，F＝G+H,G为从起点到当前节点的距离累加值，H为当前节点到终点坐标的直线距离值。angle_change为当前路线到相邻路线的角度变换值。

步骤S3-2，从open集合中取出代价值最低的节点，open集合中已按cost代价值从小到大排序，使用直接取出集合中第一个节点即可，取出节点，从open集合中删除，加入到close集合中。

S4.根据关联过后的路线节点集合，当前所在路线节点信息，查找所在路线的相邻路线节点集合，进行代价值计算，将相邻路线节点加入open集合中。

其中查找相邻的交点节点加入open集合的方法的步骤包括：

步骤S4-1，根据当前Node节点中的父路线索引Parent_index，在路线节点集合中查找当前节点最近的其他交点节点，将所有交点节点放入集合中返回。

步骤S4-2，根据得到的最近的其他交点节点集合，按照步骤S3-1进行代价值计算，并查找当前节点是否在close集合和open集合中，如果在close集合中则不进行操作，直接查找下一节点。如果当前节点在open集合中，则在对比已有节点和当前节点的代价值，如果当前节点代价值比已有节点代价值更低，则对已有的节点代价值重新计算，并重新指定父节点，否则不进行计算。如果当前节点不在open集合中，则将节点放入到open集合中。

S5.当open集合为空，未找到与终点坐标相等的节点则路径规划失败。否则将final_path节点指向当前节点，并取出final_path节点中的路径信息，反馈给调用程序，路径规划成功。

如图2所示，对线段合集进行初始化并进行关联的步骤如下：

步骤S1-1，作为参数传入接收线段集合，起点坐标信息，回传线段节点集合。

步骤S1-2，对线段集合进行遍历，判断起点是否在当前正在遍历的线段上，具体判断是使用点到线段距离公式计算出起点到线段的距离，当距离值小于一点阈值则认为起点在当前线段上。具体计算公式不作详述。

初始化路线节点lineNode方式为，查找当前路线ID是否出现在回传线段节点集合中，如果在，说明当前线段被保存过(某次查找当前线段和其他线段交点时，这条线段遇到过，遇到过就保存线段对象，防止重复计算交点，导致算法冗余)，直接取出集合中的对象使用即可。

如果不在回传线段节点集合中，即第一次遇到该线段，生成路线节点对象，保存线段ID,将线段起点坐标保存(为后续计算其他交点节点与起点距离代价值作准备)。

计算起点坐标在整个地图范围内的下标值(作唯一性判断使用)。

具体计算方式如下：

int index＝(p.y-(gs.Min_y))*(X_Width)+(p.x-(gs.Min_x))；

p.x,p.y表示某点的坐标，gs.Min_y表示线段集合中的最小坐标y,gs.Min_x表示线段集合中的最小坐标x,X_Width为整个地图宽度，使用gs.Max_x-gs.Min_x得出。

步骤S1-3，逐一查找当前路线和其他路线的交点坐标，并将交点坐标初始化为交点节点Node，保存两条路线的ID放入节点的父路线索引Parent_index集合中，将节点与路线ID关联。

其中判断两条线段是否相交的条件为：

a)起点或终点坐标相等

b)两线端首尾相连

c)根据外积结果判定是否交于线上

步骤S1-4，满足其中一个条件则认为两线段相交，并获取交点坐标，计算交点坐标与起点距离，计算方法位两点间距离公式，公式为：

将距离值作为距离起点代价值保存，生成当前路线节点，并对当前路线中所包含的节点集合按照距离起点距离从大到小排序。

步骤S1-5，循环遍历后得到当前线段和其他所有线段的交点集合。将所有交点集合放入到lineNode路线节点中，如此循环可得到每条线段与其他线段的交点节点集合及所有路线节点集合。最终返回起点的交点节点信息。

如图3所示，根据所有路线节点集合和起点的交点节点查找相邻的交点节点加入open集合的方法的步骤包括：

步骤S4-1，根据当前Node节点中的父路线索引Parent_index，在路线节点集合中查找当前节点最近的其他节点，将所有节点放入集合中返回。

步骤S4-2，根据得到的最近的其他节点集合，按照步骤S3-1进行代价值计算，并查找当前节点是否在close集合和open集合中。

如果在close集合中则不进行操作，直接查找下一节点。

如果当前节点在open集合中，则在对比已有节点和当前节点的代价值，如果当前节点代价值比已有节点代价值更低，则对已有的节点代价值重新计算，并重新指定父节点，否则不进行计算。

如果当前节点不在open集合中，则将节点放入到open集合中。

只要线段合集未出现断开的情况，按照上述方法，终点节点最终会被加入到open集合中，当取出相对于的节点与终点坐标对比，则跳出循环。

接着按照步骤S5中取出final_path节点中的路径信息，其中方法包括：

步骤S5-1，循环判断当前节点是否有父节点，有则将当前坐标保存，并将父节点指向当前节点。当当前节点无父节点则认为已取出所有节点，路径规划完成，返回坐标信息，程序退出。

图3所示的基本思路是经典的AStar路径规划算法，不过AStar路径规划算法依赖的地图是栅格地图或者拓扑地图。

本发明的核心特征在于对有线线段集合进行初始化并对各个线段的相交点作为节点保存，将节点按照距离起点的距离值排序，路线节点中保存相交节点，相交节点中保存相交的两条路线ID，以此对节点进行关联。

如图4所示，AB线段是线段节点，点C是交点节点。线段节点中有交点节点A，B，C三点。C交点节点中由AB线段和DE线段相交得到，其中保存AB,DE的线段ID。

后续使用经典的AStar算法思路，将线段弱化成节点，查找相邻的交点节点，最终规划出可行的以节点相连的路径。

栅格地图(Grid Map)则是把环境划分成一系列栅格，在数学视角下是由边联结起来的结点的集合，一个基于图块拼接的地图可以看成是一个栅格图，每个图块(tile)是一个结点，图块之间的连接关系如短线。

拓扑地图(Topological Map)是指地图学中一种统计地图,一种保持点与线相对位置关系正确而不一定保持图形形状与面积、距离、方向正确的抽象地图。包括有有向图和无向图(字面意思)。

关于如何在栅格地图或者托盘地图中进行AStar路径规划方式不作累述。

由此本发明的优点：

(1)其搜索具有一定的指向性，会促使整体搜索方向朝向目标点，使得搜索效率较高。

(2)基于AStar算法思想，具有最终确定唯一最优解，具有很好的收敛性，是一个优良的搜索算法。

(3)因为是基于已绘制好的线段集合，不必考虑空间复杂性，所需计算量较小(只需遍历所有路线交点作为相邻点搜索)，算法时间可以得到控制。

当然上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明主要技术方案的精神实质所做的等效变换或修饰，都应涵盖在本发明的保护范围之内。

Claims (5)

1.一种基于线段的路径规划方法，其特征在于其包括如下步骤:

步骤S1：接收预先绘制好的线段集合，对所有线段集合中的坐标进行初始化路线节点并进行关联，返回起点的交点节点；

步骤S2：接收步骤S1返回起点的交点节点，并将该节点加入open集合中；

步骤S3：循环取出open集合中代价值最低的一个节点，从open集合中删除该节点，加入到close集合中，并比较节点信息中的坐标是否和终点坐标相等，相等则将final_path节点指向当前节点，跳出循环，执行步骤S5，不相等则执行步骤S4；

步骤S4：根据关联过后的路线节点集合，当前所在路线节点信息，查找所在路线的相邻交点节点集合，进行代价值计算，将交点节点加入open集合中，继续执行步骤S3；

步骤S5：取出final_path节点中的路径信息，反馈给调用程序，路径规划成功。

2.根据权利要求1所述的路径规划方法，其特征在于，在步骤S1中，所述初始化路线节点并进行关联包括如下步骤：

步骤S1-1：接收线段集合，起点坐标信息，初始化回传节点集合；

步骤S1-2：对线段集合进行遍历，判断起点坐标在哪条路线上,并初始化路线节点lineNode；

步骤S1-3：逐一查找当前路线和其他路线的交点坐标，并将交点坐标初始化为交点节点Node，保存两条路线的ID放入节点的父路线索引Parent_index集合中，将节点与路线ID关联，其中判定两条线段是否相交的条件为：起点或终点坐标相等；两线端首尾相连；根据外积结果判定是否交于线上；

步骤S1-4：满足其中一个条件则认为两线段相交，并获取交点坐标，计算交点坐标与起点距离，计算方法位两点间距离公式，公式为：

将距离值作为距离起点代价值保存，生成当前路线节点，并对当前路线中所包含的交点节点集合按照距离起点距离从大到小排序；

步骤S1-5，循环遍历后得到当前线段和其他所有线段的交点集合，将所有交点集合放入到lineNode路线节点中，如此循环得到每条线段与其他线段的交点节点集合及所有路线节点集合，最终返回起点的交点节点信息。

3.根据权利要求1所述的路径规划方法，其特征在于，在步骤S3中所述循环取出open中的路线信息的步骤包括：

步骤S3-1：循环取出open集合中的交点节点信息，取出代价值最低的一个节点，代价值计算方法为：

Cost＝F+angle_change＝G+H+angle_change

其中Cost为总的代价值，F＝G+H,G为从起点到当前节点的距离累加值，H为当前节点到终点坐标的直线距离值，angle_change为当前路线到相邻路线的角度变换值；

步骤S3-2：从open集合中取出代价值最低的节点，open集合中已按Cost代价值从小到大排序，使用直接取出集合中第一个节点即可，取出节点，从open集合中删除，再加入到close集合中。

4.根据权利要求1所述的路径规划方法，其特征在于，在步骤S4中查找相邻的交点节点加入open集合的方法的步骤包括：

步骤S4-1：根据当前Node节点中的父路线索引Parent_index，在路线节点集合中查找当前节点最近的其他交点节点，将所有节点放入集合中返回；

步骤S4-2：根据得到的最近的其他交点节点集合，按照步骤S3-1进行代价值计算，并查找当前交点节点是否在close集合和open集合中，如果在close集合中则不进行操作，直接查找下一交点节点；如果当前交点节点在open集合中，则在对比已有交点节点和当前交点节点的代价值，如果当前交点节点代价值比已有交点节点代价值更低，则对已有的交点节点代价值重新计算，并重新指定父节点，否则不进行计算；如果当前交点节点不在open集合中，则将节点放入到open集合中。

5.根据权利要求1所述的路径规划方法，其特征在于，在步骤S5中取出final_path节点中的路径信息方法包括：

步骤S5-1：循环判断当前节点是否有父节点，有则将当前坐标保存，并将父节点指向当前节点；当当前节点无父节点则认为已取出所有节点，路径规划完成，返回坐标信息，程序退出。

Images