CN105606113A

CN105606113A - 快速规划最优路径方法及装置

Info

Publication number: CN105606113A
Application number: CN201610059840.4A
Authority: CN
Inventors: 潘晨劲; 赵江宜
Original assignee: Foochow Hua Ying Heavy Industry Machinery Co Ltd
Current assignee: Foochow Hua Ying Heavy Industry Machinery Co Ltd
Priority date: 2016-01-28
Filing date: 2016-01-28
Publication date: 2016-05-25
Anticipated expiration: 2036-01-28
Also published as: CN105606113B

Abstract

一种快速规划最优路径方法及装置，其中方法包括如下步骤，将地图信息分为若干单元格，确定行驶路径的起点和终点；以单个单元格作为顶点，计算顶点的距离值；从终点开始通过相邻顶点搜索，当前距离为终点距离值减一；寻找距离值为当前距离的顶点，如果其中一个顶点为起点，则找到了最佳路径；区别于现有技术，上述技术方案通过计算顶点距离，寻找最佳路径，通过更新优先级队列的方法使得在环境变动的时候优先在最佳路径附近的顶点优先寻找重新规划路径，提高搜索效率。还通过计算本地距离值的方法优化环境变动后的距离计算，提高方法的可靠性，还通过增加启发式函数的算法节约计算步骤。

Description

快速规划最优路径方法及装置

技术领域

本发明涉及智能路径规划领域，尤其涉及一种快速规划最优路径的方法及装置。

背景技术

无人车辆(公交车、卡车、汽车)或者其它机器人经常需要在陌生或者信息不完全的地域活动。在这类环境中，无人车往往根据任务被要求从某个起点坐标移动至某个终点坐标。整个区域内的障碍物、区域内不同的部分是否可以穿过的信息可能已知(比如以地图的形式)，但是这个地图有可能不准确，或者环境可能变化。

比如，当无人车在一个停车场行驶时，虽然停车场的地图/空间结构可能事先获取，但因为各种车辆的动态停放，停车场空间的拓扑结构可能改变。如果无人车一开始搜索了一条通往特定停车位的路径，在这条路径上行驶的过程中，有可能碰到原先以为可以通行的地方被新停的车堵住了。这时候，无人车就要再次搜寻一条到达目标点的路径。

又比如，在城市道路网络上行驶的无人车，有的时候会遇到一些突发状况导致原来通畅的道路变得拥堵，甚至不可通行。这种情况下，原先地图上的距离(这里的距离指地图上两点间预期行驶的时间)和可否通行信息就发生了变化，原来的计划的路径很可能不再是最佳选择。

因此，无人车需要不断地适应环境(更准确地说是它们抽象的环境模型，一些会介绍)的变化，根据变化不断地重现计划自己的行为。

重现规划一个简单的解决办法就是重新从头运行一次最佳路径搜索算法，以当前的位置为新的起点，目标点不变，独立地(不依赖已经行驶的路程或者之前搜索时生成的额外信息)重新规划一条路径。但这样做很可能极其没有效率，特别当1.搜索区域很大；2.变化发生得很频繁的时候。有的时候这样做甚至可能完全不行，比如当完成一次搜寻所需要的时间大于变化发生的平均间隔-这种情况下重新搜索永远都不会完成。

因此，采用一种增量搜索法(incrementalsearch)在动态或者未知的环境中就很有必要。

发明内容

为此，需要提供一种快速路径规划方法，解决频繁变动条件下路径自动规划的问题。

为实现上述目的，发明人提供了一种快速规划最优路径方法，包括如下步骤，

将地图信息分为若干单元格，确定行驶路径的起点和终点；

以单个单元格作为顶点，计算顶点的距离值；

从终点开始通过相邻顶点搜索，当前距离为终点距离值减一；

寻找距离值为当前距离的顶点，如果其中一个顶点为起点，则找到了最佳路径；

如果未找到距离值为当前距离的顶点，则当前距离减一，重复上一步；

不断更新一个优先级队列，所述优先级队列根据到最佳路径的远近对优先级队列中的顶点进行排序，当环境变动时重新计算顶点的距离值，根据优先级队列的顶点排序重新返回步骤“寻找距离值为当前距离的顶点”，直到重新规划出最佳路径。

进一步地，还包括步骤，计算顶点的本地到达值，具体为：

读取第一顶点所有相邻顶点的距离值g；

分别计算所有相邻顶点的g+1的值，取其中最小值为第一顶点的本地到达值；

将本地到达值与距离值不同的顶点添加到优先级队列当中。

进一步地，所述优先级队列通过顶点的本地到达值及启发式函数值确定，所述启发式函数为顶点到起点的横、纵距离中的最大值。

一种快速规划最优路径装置，包括地图确定模块、距离值模块、最佳路径寻找模块、优先级队列模块；

所述地图确定模块用于将地图信息分为若干单元格，确定行驶路径的起点和终点；

所述距离值模块用于以单个单元格作为顶点，计算顶点的距离值；

所述最佳路径寻找模块用于从终点开始通过相邻顶点搜索，当前距离为终点距离值减一；

还用于寻找距离值为当前距离的顶点，如果其中一个顶点为起点，则找到了最佳路径；如果未找到距离值为当前距离的顶点，则当前距离减一，重复上一步；

所述优先级队列模块用于不断更新一个优先级队列，所述优先级队列根据到最佳路径的远近对优先级队列中的顶点进行排序，当环境变动时重新计算顶点的距离值，根据优先级队列的顶点排序重新返回步骤“寻找距离值为当前距离的顶点”，直到重新规划出最佳路径。

进一步地，还包括本地到达值模块、添加模块，所述本地到达值模块用于计算顶点的本地到达值，具体用于，读取第一顶点所有相邻顶点的距离值g；分别计算所有相邻顶点的g+1的值，取其中最小值为第一顶点的本地到达值；

所述添加模块将本地到达值与距离值不同的顶点添加到优先级队列当中。

进一步地，所述优先级队列还用于通过顶点的本地到达值及启发式函数值确定优先级队列，所述启发式函数为顶点到起点的横、纵距离中的最大值。

区别于现有技术，上述技术方案通过计算顶点距离，寻找最佳路径，通过更新优先级队列的方法使得在环境变动的时候优先在最佳路径附近的顶点优先寻找重新规划路径，提高搜索效率。还通过计算本地距离值的方法优化环境变动后的距离计算，提高方法的可靠性，还通过增加启发式函数的算法节约计算步骤。

附图说明

图1为本发明具体实施方式所述的起始点重点路径规划示意图；

图2为本发明具体实施方式所述的环境变动路径规划示意图；

图3为本发明具体实施方式所述的快速规划最优路径方法流程图；

图4为本发明具体实施方式所述的路径示意图；

图5为本发明具体实施方式所述的本地到达值示意图；

图6为本发明具体实施方式所述的根据最初环境计算最佳路径示意图；

图7为本发明具体实施方式所述的环境变动后计算最佳路径示意图；

图8为本发明具体实施方式所述的快速规划最优路径装置模块图。

附图标记说明：

800、地图确定模块；

802、距离值模块；

804、最佳路径寻找模块；

806、优先级队列模块；

808、本地到达值模块；

810、添加模块。

具体实施方式

为详细说明技术方案的技术内容、构造特征、所实现目的及效果，以下结合具体实施例并配合附图详予说明。

1、模型假设

在介绍本篇的增量搜寻法之前，首先需要定义一下本方法对于物理世界的抽象模型。和大部分的路径规划/导航的算法相似，物理世界可以用一个图模拟，图由顶点和弧组成。顶点代表地图上的不同地点，如果两个顶点之间有弧相连，则地图上两点间可通行。弧还可以包括距离信息(比如两个地点之间的预计行驶时间)。

无人车在进行路径规划的时候，总是假定区域内未知的(不在视野范围内的或者从未到达过的)的部分为可通行的。

常见的路径搜寻算法，如A*利用启发(heuristics)来有效减少搜索空间。比起广度优先算法(breadth-firstsearch)，A*在启发的帮助下能有效地回避搜索许多相关度比较低的部分。比如当目标在无人车正前方时，广度优先搜索很可能会以无人车当前位置为中心，环形地扩展搜索，而A*则很可能将搜索集中于无人车前方的区域内。很显然广度优先搜索搜索无人车后方的空间做无用功的可能性很大。本增量搜索法也借鉴了A*利用启发的优点。

但A*在完成一次搜索后，在下一次搜索的时候没有办法利用上一次搜索时得出的关于顶点和弧的信息，即便在下一次由于环境变化而触发的搜索中，大部分这些信息依然有效(图1和图2)。

所以，如果在一次搜索的时候能够计算一种信息，这种信息具备以下两个特点：

1.利用这种信息，可以快速地得出最佳路线。

2.在环境发生某种局部的变化后，需要更新这种信息的顶点也是限于局部的。并且这些顶点能够快速地被找到。

结合以上两点在极大地减少每次重新规划时所需要搜寻的空间(第二点)的同时能得出最佳路线。

2、总体思路

在本方法中，请看图3所示，为一种快速规划最优路径方法流程图这种信息就是每个顶点距离起始点的距离，或者是距离目标点的距离。这种距离的计算方法可以用曼哈顿距离(即两点间的距离严格地为垂直或者水平的路径长度之和)，或者其它距离计算法，如图4中的蓝/红/黄线所示，任何一个单元格/顶点到八个相邻的单元的距离都为一。

如图1和图2所述的实施例中所示，如果从起始点到终点之间的一部分顶点的距离值已知，则求最佳路径便简化为如下方法：

步骤S300将地图信息分为若干单元格，确定行驶路径的起点和终点；

步骤S302以单个单元格作为顶点，计算顶点的距离值；

步骤S304从终点开始通过相邻顶点搜索，当前距离为终点距离值减一；

其中最佳路径可以是起点与终点间贪婪算法的距离值的直接返回。

在每一次搜索中，本方法计算并且赋予每个顶点这个距离信息。新的情况出现后(图1、图2)，大部分的顶点距离值并没有变。如何快速找到距离值改变了的顶点是方法效率的关键。为了快速找到改变了的顶点，还包括步骤S306，不断更新一个优先级队列，所述优先级队列根据到最佳路径的远近对优先级队列中的顶点进行排序，当环境变动时重新计算顶点的距离值，根据优先级队列的顶点排序重新返回步骤“寻找距离值为当前距离的顶点”，直到重新规划出最佳路径。通过上述方法设计，使得在环境变动的时候优先在最佳路径附近的顶点优先寻找重新规划路径，提高搜索效率。

在一些优选的实施例中，本方法在计算每个顶点的距离值的同时，还计算每个顶点的本地到达值值。一个顶点A的本地到达值值的计算为：

1.读取顶点A所有相邻顶点的距离值g。

2.计算每个相邻的顶点到达顶点A的距离d，这个距离一般为一。

3.分别计算所有相邻顶点的(g+d)值，顶点A的本地到达值值为其中最小的值。

一个顶点A的本地到达值其实反映了到达A的最短的路径的距离值。在一次搜索完成后，环境出现新的变化前，本地到达值值要等于距离值。但一旦环境发生了变化(比如一些原来可以通行的顶点变得不可通行了)，则这些发生变化的顶点周围的顶点的本地到达值值很可能不再等于距离值。

比如，图5中所示的实施例中，每个单元格(顶点)内的数字为本地到达值。当单元D1由可通行(左边)变为不可通行时，D1邻近的两个单元C1和E1需要重新检查本地到达值值和距离值是否依然相等。检查的结果就是E1的本地到达值值改变了，不再是图上的5，而是6(因为E1原来可以经由D1这条更短的路径被堵住了)。

由此可见，本算法结合了启发式经验算法和增量搜索算法的效率，在不断变化的图中内确定两个顶点间的最短路径。增量搜索会趋向于重新计算距离值已改变或之前没有计算的顶点。启发式搜索则往往趋向于重新计算那些会在最短路径上的点。因此，综合两种策略，算法只需要重新计算极少数顶点值。

3、具体实现

考虑在不明地形进行导航任务的机器人，其中机器人观察其中的八个相邻单元是否能通过，然后用不同的花费在其中进行移动。机器人会从起始区域移动到目标区域。假定通行性未知的是可通过的前提下，算法总是计算当前单元格到目标单元格的最短路径。然后，它遵循这一路径，要么直到机器人到达目标单元格，要么知道观察到新的不可行单元格为止。在到达目标单元格的情况下，机器人会成功地停止。而在观察到新的不可行单元格的情况下，算法会重新计算从当前单元格到目标单元格的最短路径。

3.1定义变量

S表示图各个顶点组成的有限集合。表示顶点s∈S在图中的后续顶点组成的集合。与之相似地，表示顶点s∈S在图中的前置顶点组成的集合。0<c(s，s′)≤∞表示从点s移到s′∈Succ(s)点的成本。

通过已知的图拓扑和当前各边成本，算法会计算，从s_起点∈S到s_终点∈S的最短路径。

算法保存了对距离函数g^*的两种估计：一个g值g(s)和一个所谓的右侧值rhs(s)。一个顶点的rhs是基于其前任的g值求得，并总是满足下列关系：

而作为最终比较重要的距离值g的计算则是由算法通过rhs值间接获得。

倘若单元格内的g值和rhs不一致，那我们把这种单元格称为本地不一致。

3.2优先级队列

在具体的实施例中，还包括步骤S306维护一个优先级队列。在具体的实施例中，队列总是包含本地不一致的顶点。算法可能需要改变本地不一致顶点的g值，以使得顶点变为本地一致。优先级队列内的优先级k(s)是具有两个分量的矢量：k(s)＝[k₁(s)；k₂(s)]

其中，k₁(s)＝min(g(s),rhs(s))+h(s,s_终点)，而k₂(s)＝min(g(s),rhs(s))。其中h是下文所述的启发式经验函数，其值等于两点间横纵坐标取较大值。

优先级按字母顺序进行排序。也就是说，首先比较向量的第一项，较小的量排在线面。如果不能排出顺序，算法会再比较向量的第二项。

伪代码使用以下函数能来管理优先级队列：U最前的优先级顶点()返回优先队列U内具有最前优先级的顶点。U.最前的优先级值()返回优先队列U内最前的优先级值。(如果U为空，则U.最前的优先级值返回[∞,∞]。)U取出最前()从队列中删除并返回优先队列U内具有最前优先级的顶点。U.插入(s,k)将优先级为k的顶点s插入到优先级队列U。U更新(s,k)将优先级队列U中的顶点s优先级更新为k。

启发式经验函数用于在环境变化之后使得路径优先向起点方向进行计算，摒弃其他方向，通过设计上述优先级队列，能够使得环境改变时更快的计算最有可能的最佳路径，提升了重新规划路径的速度。

表1帮助函数算法

3.3主算法

因为优先队列往往含有大量的顶点，为了避免对优先级队列进行昂贵的反复重排序。本算法优先级向量的意义在保存距离的下限。我们需要设计一个非负并前后一致的启发式经验函数h(s,s')也就是说，对于所有顶点s,s',s”∈S满足h(s,s')≤h(s,s')+h(s',s”)而且无论目标顶点在何处，启发式经验函数对于所有顶点s，s'∈S满足h(s,s')≤c*(s,s')。其中c*(s,s')表示从顶点s∈S到顶点s'∈S的最短路径所用的总费用。这些条件可以通过放松搜索问题来满足。

表2最短路径算法

表3主程序算法

当机器人从点s一动点一些顶点s'，并检测某条边费用发生改变后，在顶点优先级的第一项具有至多下降h(s,s')。(第二项不依赖与经验函数，因而保持不变)。因此，为了计算距离下限，D*Lite需要从优先级队列内所有顶点优先级的第一项减去h(s,s')。因此，新优先级的第一项需要增加h(s,s')。

如果机器人再次移动，然后检测成本的变化，那么就需要考虑启发式经验函数的改变。我们通过变量k_m来做到这一点。

因此，计算新的优先级时，变量k_m被添加到其第一项，如算法1的行2.然后，机器人移动不改变优先级队列中顶点的顺序，而且优先级队列也不需要进行重新排序。

计算最短路径()函数会从优先队列内删除了一个优先级最前的顶点，然后用计算优先级(u)函数计算其(新的)优先级。倘若k_old＜计算优先级(u)，那么算法会将u其重新插入删除顶点，并将其优先级设置为计算优先级(u)。倘若k_old＞计算优先级(u)，那么算法实际上会使得k_old＝计算优先级(u)。这是因为k_old的定义是计算优先级()函数返回值的下限。

最终最佳路径可以简单是贪婪算法按个点的g值直接返回。

4例子

下面请看图6和图7，在其所示的具体实施例中，给出了本方法的具体应用。图6标示出了机器人最初知道的环境地图。黑色的单元格不能通过。图的最上方部分也显示了在可通过单元格里的启发式经验函数值。函数近似表示该单元格与开始单元格的距离：定义为两者x,y坐标差值之中更大的那个。

图6下方各个单元格的g-值和rhs值。而在本地不一致的单元格里(也就是在优先队列里的单元格)，也显示了他们的相应优先级。优先级排在最前的单元格具有粗体边框，提示接下来将对其进行运算。

标有“初始化”的图块显示了在第一次调用计算最短路径()函数前，算法的状态。

下面的图块则显示了计算最短路径()函数内每次循环后(算法2第3行)显示的值。如果单元格的g值大于其rhs值，计算最短路径()函数会将g值设为rhs的值。否则，计算最短路径()函数将会将g值设置为无穷大。计算最短路径()函数然后重新计算可能会收到此赋值影响的单元格的rhs值，再次检查这些单元格是否成为据不一致或不一致，而相应(如果有需要)将其从优先队列里添加或除去。然后重复这个过程，知道确信函数已经找到了一个最短路径。过程中不需要重新计算所有的g值。最后一个图块显示计算最短路径()函数最终结果。

通过直接用贪婪算法计算，从当前单元格B1至目标E3单元格的最短路径是通过单元格C1和D2。

该机器人然后移动到单元格C1，发现D2单元格不能通过。图7上方标示出机器人现在感知的情况。该图还显示了可通过单元格里启发式函数新的值。标有“边缘费用改变了”的图块显示第二次调用计算最短路径()函数前的值。下面的图块则显示了计算最短路径()函数内每次循环后的各单元格的值。最后一个图块也是显示计算最短路径()函数最终结果。从当前单元格C1到目标单元格E3的最短路径是通过单元格D1和E2。那么机器人跟随该路径到的目标单元格。这条路径若没有额外的不能通过单元格，就没有必要再次调用计算最短路径()函数。

下面请看图8，为一种快速规划最优路径装置模块徒，包括地图确定模块800、距离值模块802、最佳路径寻找模块804、优先级队列模块806；

所述地图确定模块800用于将地图信息分为若干单元格，确定行驶路径的起点和终点；

所述距离值模块802用于以单个单元格作为顶点，计算顶点的距离值；

所述优先级队列模块806用于不断更新一个优先级队列，所述优先级队列根据到最佳路径的远近对优先级队列中的顶点进行排序，当环境变动时重新计算顶点的距离值，根据优先级队列的顶点排序重新返回步骤“寻找距离值为当前距离的顶点”，直到重新规划出最佳路径。通过上述模块设计，能够在已知的地图环境中快速计算出最佳路径，并通过设计优先队列提高了环境变动后重新规划的能力，提高了现有路径搜索方式的效率。

在一些进一步的实施例中，还包括本地到达值模块808、添加模块810，所述本地到达值模块808用于计算顶点的本地到达值，具体用于，读取第一顶点所有相邻顶点的距离值g；分别计算所有相邻顶点的g+1的值，取其中最小值为第一顶点的本地到达值；

所述添加模块810将本地到达值与距离值不同的顶点添加到优先级队列当中。通过上述方式能够更快地搜寻环境变化后距离变动的顶点，提高了本发明的最佳路径搜索效率。

在一些进一步的实施例中，所述优先级队列还用于通过顶点的本地到达值及启发式函数值确定优先级队列，所述启发式函数为顶点到起点的横、纵距离中的最大值。通过设计启发式函数，提高了本装置的搜索效率。

区别于现有技术，上述装置设计通过计算顶点距离，寻找最佳路径，通过更新优先级队列的方法使得在环境变动的时候优先在最佳路径附近的顶点优先寻找重新规划路径，提高搜索效率。还通过计算本地距离值的方法优化环境变动后的距离计算，提高方法的可靠性，还通过增加启发式函数的算法节约计算步骤。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括……”或“包含……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的要素。此外，在本文中，“大于”、“小于”、“超过”等理解为不包括本数；“以上”、“以下”、“以内”等理解为包括本数。

本领域内的技术人员应明白，上述各实施例可提供为方法、装置、或计算机程序产品。这些实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。上述各实施例涉及的方法中的全部或部分步骤可以通过程序来指令相关的硬件来完成，所述的程序可以存储于计算机设备可读取的存储介质中，用于执行上述各实施例方法所述的全部或部分步骤。所述计算机设备，包括但不限于：个人计算机、服务器、通用计算机、专用计算机、网络设备、嵌入式设备、可编程设备、智能移动终端、智能家居设备、穿戴式智能设备、车载智能设备等；所述的存储介质，包括但不限于：RAM、ROM、磁碟、磁带、光盘、闪存、U盘、移动硬盘、存储卡、记忆棒、网络服务器存储、网络云存储等。

上述各实施例是参照根据实施例所述的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到计算机设备的处理器以产生一个机器，使得通过计算机设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机设备以特定方式工作的计算机设备可读存储器中，使得存储在该计算机设备可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机设备上，使得在计算机设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

尽管已经对上述各实施例进行了描述，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改，所以以上所述仅为本发明的实施例，并非因此限制本发明的专利保护范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围之内。

Claims

1.一种快速规划最优路径方法，其特征在于，包括如下步骤，

将地图信息分为若干单元格，确定行驶路径的起点和终点；

以单个单元格作为顶点，计算顶点的距离值；

2.根据权利要求1所述的快速规划最优路径方法，其特征在于，还包括步骤，计算顶点的本地到达值，具体为：

读取第一顶点所有相邻顶点的距离值g；

将本地到达值与距离值不同的顶点添加到优先级队列当中。

3.根据权利要求2所述的快速规划最优路径方法，其特征在于，所述优先级队列通过顶点的本地到达值及启发式函数值确定，所述启发式函数为顶点到起点的横、纵距离中的最大值。

4.一种快速规划最优路径装置，其特征在于，包括地图确定模块、距离值模块、最佳路径寻找模块、优先级队列模块；

5.根据权利要求4所述的快速规划最优路径装置，其特征在于，还包括本地到达值模块、添加模块，所述本地到达值模块用于计算顶点的本地到达值，具体用于，读取第一顶点所有相邻顶点的距离值g；分别计算所有相邻顶点的g+1的值，取其中最小值为第一顶点的本地到达值；

6.根据权利要求5所述的快速规划最优路径方法，其特征在于，所述优先级队列还用于通过顶点的本地到达值及启发式函数值确定优先级队列，所述启发式函数为顶点到起点的横、纵距离中的最大值。