顺序进行排序。也就是说，首先比较向量的第一项，较小的量排在线 面。如果不能排出顺序，算法会再比较向量的第二项。
[0086] 伪代码使用以下函数能来管理优先级队列:U最前的优先级顶点〇返回优先队列U 内具有最前优先级的顶点。U.最前的优先级值（）返回优先队列U内最前的优先级值。（如果U 为空，则U.最前的优先级值返回[00，00 ]。）U取出最前（）从队列中删除并返回优先队列U内 具有最前优先级的顶点。U.插入(s，k)将优先级为k的顶点s插入到优先级队列IU更新（s， k)将优先级队列U中的顶点s优先级更新为k。
[0087] 启发式经验函数用于在环境变化之后使得路径优先向起点方向进行计算，摒弃其 他方向，通过设计上述优先级队列，能够使得环境改变时更快的计算最有可能的最佳路径， 提升了重新规划路径的速度。
[0088]
[0089] 表1帮助函数算法
[0090] 3.3主算法
[0091] 因为优先队列往往含有大量的顶点，为了避免对优先级队列进行昂贵的反复重排 序。本算法优先级向量的意义在保存距离的下限。我们需要设计一个非负并前后一致的启 发式经验函数h(s，s'）也就是说，对于所有顶点s，s'，s"eS满足h(S， S'Hh(S，S'）+h(S'， s"）而且无论目标顶点在何处，启发式经验函数对于所有顶点s，s ' es满足h(s，s ' H c*(s， s'）。其中c*(s，s'）表示从顶点seS到顶点s'eS的最短路径所用的总费用。这些条件可以 通过放松搜索问题来满足。
[0094]
[0095] 表3主程序算法
[0096] 当机器人从点s-动点一些顶点s'，并检测某条边费用发生改变后，在顶点优先级 的第一项具有至多下降h(s，s'）。（第二项不依赖与经验函数，因而保持不变）。因此，为了计 算距离下限，D*Lite需要从优先级队列内所有顶点优先级的第一项减去h(s，s'）。因此，新 优先级的第一项需要增加 h(s，s '）。
[0097] 如果机器人再次移动，然后检测成本的变化，那么就需要考虑启发式经验函数的 改变。我们通过变量1^来做到这一点。
[0098] 因此，计算新的优先级时，变量km被添加到其第一项，如算法1的行2.然后，机器人 移动不改变优先级队列中顶点的顺序，而且优先级队列也不需要进行重新排序。
[0099] 计算最短路径〇函数会从优先队列内删除了一个优先级最前的顶点，然后用计算 优先级(u)函数计算其(新的）优先级。倘若kcad<计算优先级(u)，那么算法会将u其重新插 入删除顶点，并将其优先级设置为计算优先级U)。倘若kca d>计算优先级(u)，那么算法实 际上会使得kQld =计算优先级(u)。这是因为kcdd的定义是计算优先级0函数返回值的下限。 [0100]最终最佳路径可以简单是贪婪算法按个点的g值直接返回。
[0101] 4 例子
[0102] 下面请看图6和图7，在其所示的具体实施例中，给出了本方法的具体应用。图6标 示出了机器人最初知道的环境地图。黑色的单元格不能通过。图的最上方部分也显示了在 可通过单元格里的启发式经验函数值。函数近似表示该单元格与开始单元格的距离:定义 为两者X，y坐标差值之中更大的那个。
[0103] 图6下方各个单元格的g-值和rhs值。而在本地不一致的单元格里（也就是在优先 队列里的单元格），也显示了他们的相应优先级。优先级排在最前的单元格具有粗体边框， 提示接下来将对其进行运算。
[0104] 标有"初始化"的图块显示了在第一次调用计算最短路径〇函数前，算法的状态。
[0105] 下面的图块则显示了计算最短路径（）函数内每次循环后(算法2第3行）显示的值。 如果单元格的g值大于其rhs值，计算最短路径（）函数会将g值设为rhs的值。否则，计算最短 路径（）函数将会将g值设置为无穷大。计算最短路径0函数然后重新计算可能会收到此赋 值影响的单元格的rhs值，再次检查这些单元格是否成为据不一致或不一致，而相应(如果 有需要)将其从优先队列里添加或除去。然后重复这个过程，知道确信函数已经找到了一个 最短路径。过程中不需要重新计算所有的g值。最后一个图块显示计算最短路径0函数最终 结果。
[0106] 通过直接用贪婪算法计算，从当前单元格B1至目标E3单元格的最短路径是通过单 元格C1和D2。
[0107] 该机器人然后移动到单元格C1，发现D2单元格不能通过。图7上方标示出机器人现 在感知的情况。该图还显示了可通过单元格里启发式函数新的值。标有"边缘费用改变了" 的图块显示第二次调用计算最短路径0函数前的值。下面的图块则显示了计算最短路径0 函数内每次循环后的各单元格的值。最后一个图块也是显示计算最短路径（）函数最终结 果。从当前单元格C1到目标单元格E3的最短路径是通过单元格D1和E2。那么机器人跟随该 路径到的目标单元格。这条路径若没有额外的不能通过单元格，就没有必要再次调用计算 最短路径0函数。
[0108] 下面请看图8,为一种快速规划最优路径装置模块徒，包括地图确定模块800、距离 值模块802、最佳路径寻找模块804、优先级队列模块806;
[0109] 所述地图确定模块800用于将地图信息分为若干单元格，确定行驶路径的起点和 餘占 . ，-、，
[0110] 所述距离值模块802用于以单个单元格作为顶点，计算顶点的距离值；
[0111]所述最佳路径寻找模块用于从终点开始通过相邻顶点搜索，当前距离为终点距离 值减一；
[0112]还用于寻找距离值为当前距离的顶点，如果其中一个顶点为起点，则找到了最佳 路径;如果未找到距离值为当前距离的顶点，则当前距离减一，重复上一步；
[0113] 所述优先级队列模块806用于不断更新一个优先级队列，所述优先级队列根据到 最佳路径的远近对优先级队列中的顶点进行排序，当环境变动时重新计算顶点的距离值， 根据优先级队列的顶点排序重新返回步骤"寻找距离值为当前距离的顶点"，直到重新规划 出最佳路径。通过上述模块设计，能够在已知的地图环境中快速计算出最佳路径，并通过设 计优先队列提高了环境变动后重新规划的能力，提高了现有路径搜索方式的效率。
[0114] 在一些进一步的实施例中，还包括本地到达值模块808、添加模块810,所述本地到 达值模块808用于计算顶点的本地到达值，具体用于，读取第一顶点所有相邻顶点的距离值 g;分别计算所有相邻顶点的g+Ι的值，取其中最小值为第一顶点的本地到达值；
[0115] 所述添加模块810将本地到达值与距离值不同的顶点添加到优先级队列当中。通 过上述方式能够更快地搜寻环境变化后距离变动的顶点，提高了本发明的最佳路径搜索效 率。
[0116] 在一些进一步的实施例中，所述优先级队列还用于通过顶点的本地到达值及启发 式函数值确定优先级队列，所述启发式函数为顶点到起点的横、纵距离中的最大值。通过设 计启发式函数，提高了本装置的搜索效率。
[0117] 区别于现有技术，上述装