[发明专利]基于目标方向约束的路径规划算法

说明书

技术领域

本发明属于室内导引技术领域，具体涉及室内(如交通枢纽、大型商场等)路径导引技术的基于目标方向约束的路径规划算法。

背景技术

目前，基于室内导引的最短路径问题已经有很多成熟的算法，常见的路径规划算法有Dijkstra算法、Floyd算法、启发式搜索算法等。

Dijkstra算法是最经典的最短路径搜索算法，亦是一种相对耗时的算法。它按权值递增顺序求最短路径，具有简洁清晰的特点，算法搜索所得的结果也比较准确。从另一方面看，Dijkstra对于输入节点数繁多的稀疏图，无论是计算指定两点还是空间任意两点都是计算整个稀疏图的最短路径，具有效率低、占用空间大的缺点。

Floyd算法又称弗洛伊德算法，是一种用于寻找给定加权路径拓扑网络中顶点间最短路算法，其基本原理是动态规划，它先把路径网络转化为权值矩阵，而后在权值矩阵中求任意两点的最短路径，它相较于Dijkstra算法有了很大的改进，稠密图效果最佳，起始点和终点的变化对算法影响不大，简单有效，效率高于Dijkstra算法，但也存在时间复杂度高，不适合计算大量数据的缺点。

以Dijkstra算法、Floyd算法为代表的最短路径算法属于盲目搜索算法，虽然能够求得最短路径，但是计算量却非常大，适用于节点数较少的图结构，而对于节点数量非常庞大的图结构却不适用。

启发式搜索算法是基于具体领域的知识的搜索算法，在搜索的过程中，算法不仅考虑节点的当前代价，而且考虑到了扩展该节点所需要的估计代价，使搜索过程向着最有希望的方向前进，进而加速整个计算过程。启发性信息主要反映在估价函数上，在搜索过程中估价函数的任务就是估计待搜索节点处于最佳路径上的可能性，从而优先搜索可能性比较大的节点，从而达到提高搜索速度的目的。

基于启发式搜索的最短路径算法有局部择优搜索算法、最好优先算法，以及常见的A^*算法，局部择优搜索算法是最简单的启发式搜索算法，在搜索的过程中，当某个节点被扩展以后，“最优”的那个节点将被进一步扩展，舍弃掉了这个子节点的父节点和其他的扩展子节点。如果搜索过程一直继续下去，由于舍弃许多的扩展子节点，就有可能把真正的最佳节点都舍弃了，所以在一段过程中的最佳节点并不是全局的最佳节点，所以这种算法搜索到的路径不一定是真正的最佳路径。

A^*算法是启发式搜索算法中重要的方法之一。它是一种最好优先搜索算法，在搜索过程中，没有舍弃掉节点，在每一步的估价中都把当前的节点和以前的节点的估价值比较得到一个“最佳节点”。这样可以有效地防止“最佳节点”的丢失，提高了算法搜索结果的准确性。在比较简单的地图上它的速度非常快，能很快找到最短路径。在比较复杂的地图中，由于在每一步扩展子节点时，把当前节点的所有可扩展的子节点都保留下来，随着搜索的进行，需要保留的节点数量越来越多，保留节点的数量过于庞大，导致搜索效率不高，占用内存资源较大，所以在一些实时性要求高的搜索中并不适用。

发明内容

本发明的目的在于克服现有技术的不足，提供在路径搜索过程中，只保留当前节点与目标节点在同一方向上的可扩展节点，减少了当前节点的扩展节点的状态空间中的节点数量，降低了算法的搜索规模，提高了算法的搜索效率的基于目标方向约束的路径规划算法。

本发明的目的是通过以下技术方案来实现的：基于目标方向约束的路径规划算法，其原理为：在路径搜索过程中，只保留当前节点与目标节点在同一方向上的可扩展节点，并将这些可扩展节点加入可扩展节点的状态空间，对状态空间中的每一个可扩展节点进行评估，得到估价函数值最小的可扩展节点作为下一个当前节点，重复路径搜索，直到状态空间中估价函数值最小的可扩展节点为目标节点，得到最优路径。

进一步地，所述的步骤S1中状态空间的确定方法为：设当前节点为S，当前节点S的可扩展节点为N个，连线当前节点S与目标节点D，分别作该N个可扩展节点在连线方向上的投影，选取投影恰好落在连线上方向上的节点加入状态空间。

进一步地，所述的状态空间的确定的具体操作方法为：设当前节点S的坐标为(x₁，y₁，z₁)，其可扩展节点X的坐标为(x₂，y₂，z₂)，目标节点D的坐标为(x₃，y₃，z₃)，分别连线SX、SD，则