快速规划最优路径方法及装置的制造方法
【技术领域】
[0001] 本发明涉及智能路径规划领域，尤其涉及一种快速规划最优路径的方法及装置。
【背景技术】
[0002] 无人车辆(公交车、卡车、汽车）或者其它机器人经常需要在陌生或者信息不完全 的地域活动。在这类环境中，无人车往往根据任务被要求从某个起点坐标移动至某个终点 坐标。整个区域内的障碍物、区域内不同的部分是否可以穿过的信息可能已知（比如以地图 的形式），但是这个地图有可能不准确，或者环境可能变化。
[0003] 比如，当无人车在一个停车场行驶时，虽然停车场的地图/空间结构可能事先获 取，但因为各种车辆的动态停放，停车场空间的拓扑结构可能改变。如果无人车一开始搜索 了一条通往特定停车位的路径，在这条路径上行驶的过程中，有可能碰到原先以为可以通 行的地方被新停的车堵住了。这时候，无人车就要再次搜寻一条到达目标点的路径。
[0004] 又比如，在城市道路网络上行驶的无人车，有的时候会遇到一些突发状况导致原 来通畅的道路变得拥堵，甚至不可通行。这种情况下，原先地图上的距离(这里的距离指地 图上两点间预期行驶的时间）和可否通行信息就发生了变化，原来的计划的路径很可能不 再是最佳选择。
[0005] 因此，无人车需要不断地适应环境(更准确地说是它们抽象的环境模型，一些会介 绍）的变化，根据变化不断地重现计划自己的行为。
[0006] 重现规划一个简单的解决办法就是重新从头运行一次最佳路径搜索算法，以当前 的位置为新的起点，目标点不变，独立地(不依赖已经行驶的路程或者之前搜索时生成的额 外信息）重新规划一条路径。但这样做很可能极其没有效率，特别当1.搜索区域很大;2.变 化发生得很频繁的时候。有的时候这样做甚至可能完全不行，比如当完成一次搜寻所需要 的时间大于变化发生的平均间隔-这种情况下重新搜索永远都不会完成。
[0007] 因此，采用一种增量搜索法（incremental search)在动态或者未知的环境中就很 有必要。
【发明内容】
[0008] 为此，需要提供一种快速路径规划方法，解决频繁变动条件下路径自动规划的问 题。
[0009] 为实现上述目的，发明人提供了 一种快速规划最优路径方法，包括如下步骤，
[0010] 将地图信息分为若干单元格，确定行驶路径的起点和终点；
[0011] 以单个单元格作为顶点，计算顶点的距离值；
[0012] 从终点开始通过相邻顶点搜索，当前距离为终点距离值减一；
[0013] 寻找距离值为当前距离的顶点，如果其中一个顶点为起点，则找到了最佳路径；
[0014] 如果未找到距离值为当前距离的顶点，则当前距离减一，重复上一步；
[0015] 不断更新一个优先级队列，所述优先级队列根据到最佳路径的远近对优先级队列 中的顶点进行排序，当环境变动时重新计算顶点的距离值，根据优先级队列的顶点排序重 新返回步骤"寻找距离值为当前距离的顶点"，直到重新规划出最佳路径。
[0016]进一步地，还包括步骤，计算顶点的本地到达值，具体为：
[0017]读取第一顶点所有相邻顶点的距离值g;
[0018] 分别计算所有相邻顶点的g+1的值，取其中最小值为第一顶点的本地到达值；
[0019] 将本地到达值与距离值不同的顶点添加到优先级队列当中。
[0020] 进一步地，所述优先级队列通过顶点的本地到达值及启发式函数值确定，所述启 发式函数为顶点到起点的横、纵距离中的最大值。
[0021] -种快速规划最优路径装置，包括地图确定模块、距离值模块、最佳路径寻找模 块、优先级队列模块；
[0022] 所述地图确定模块用于将地图信息分为若干单元格，确定行驶路径的起点和终 占.
[0023] 所述距离值模块用于以单个单元格作为顶点，计算顶点的距离值；
[0024]所述最佳路径寻找模块用于从终点开始通过相邻顶点搜索，当前距离为终点距离 值减一；
[0025]还用于寻找距离值为当前距离的顶点，如果其中一个顶点为起点，则找到了最佳 路径;如果未找到距离值为当前距离的顶点，则当前距离减一，重复上一步；
[0026] 所述优先级队列模块用于不断更新一个优先级队列，所述优先级队列根据到最佳 路径的远近对优先级队列中的顶点进行排序，当环境变动时重新计算顶点的距离值，根据 优先级队列的顶点排序重新返回步骤"寻找距离值为当前距离的顶点"，直到重新规划出最 佳路径。
[0027] 进一步地，还包括本地到达值模块、添加模块，所述本地到达值模块用于计算顶点 的本地到达值，具体用于，读取第一顶点所有相邻顶点的距离值g;分别计算所有相邻顶点 的g+Ι的值，取其中最小值为第一顶点的本地到达值；
[0028] 所述添加模块将本地到达值与距离值不同的顶点添加到优先级队列当中。
[0029] 进一步地，所述优先级队列还用于通过顶点的本地到达值及启发式函数值确定优 先级队列，所述启发式函数为顶点到起点的横、纵距离中的最大值。
[0030] 区别于现有技术，上述技术方案通过计算顶点距离，寻找最佳路径，通过更新优先 级队列的方法使得在环境变动的时候优先在最佳路径附近的顶点优先寻找重新规划路径， 提高搜索效率。还通过计算本地距离值的方法优化环境变动后的距离计算，提高方法的可 靠性，还通过增加启发式函数的算法节约计算步骤。
【附图说明】
[0031 ]图1为本发明【具体实施方式】所述的起始点重点路径规划示意图；
[0032] 图2为本发明【具体实施方式】所述的环境变动路径规划示意图；
[0033] 图3为本发明【具体实施方式】所述的快速规划最优路径方法流程图；
[0034] 图4为本发明【具体实施方式】所述的路径示意图；
[0035] 图5为本发明【具体实施方式】所述的本地到达值示意图；
[0036] 图6为本发明【具体实施方式】所述的根据最初环境计算最佳路径示意图；
[0037]图7为本发明【具体实施方式】所述的环境变动后计算最佳路径示意图；
[0038]图8为本发明【具体实施方式】所述的快速规划最优路径装置模块图。
[0039] 附图标记说明：
[0040] 800、地图确定模块；
[0041 ] 802、距离值模块；
[0042] 804、最佳路径寻找模块；
[0043] 806、优先级队列模块；
[0044] 808、本地到达值模块；
[0045] 81〇、添加模块。
【具体实施方式】
[0046]为详细说明技术方案的技术内容、构造特征、所实现目的及效果，以下结合具体实 施例并配合附图详予说明。
[0047] 1、模型假设
[0048]在介绍本篇的增量搜寻法之前，首先需要定义一下本方法对于物理世界的抽象模 型。和大部分的路径规划/导航的算法相似，物理世界可以用一个图模