基于目标方向约束的路径规划算法
【技术领域】
[0001] 本发明属于室内导引技术领域，具体涉及室内（如交通枢纽、大型商场等）路径导 引技术的基于目标方向约束的路径规划算法。
【背景技术】
[0002] 目前，基于室内导引的最短路径问题已经有很多成熟的算法，常见的路径规划算 法有Di jkstra算法、Floyd算法、启发式搜索算法等。
[0003] Di jkstra算法是最经典的最短路径搜索算法，亦是一种相对耗时的算法。它按权 值递增顺序求最短路径，具有简洁清晰的特点，算法搜索所得的结果也比较准确。从另一方 面看，Dijkstra对于输入节点数繁多的稀疏图，无论是计算指定两点还是空间任意两点都 是计算整个稀疏图的最短路径，具有效率低、占用空间大的缺点。
[0004] Floyd算法又称弗洛伊德算法，是一种用于寻找给定加权路径拓扑网络中顶点间 最短路算法，其基本原理是动态规划，它先把路径网络转化为权值矩阵，而后在权值矩阵中 求任意两点的最短路径，它相较于Di jkstra算法有了很大的改进，稠密图效果最佳，起始 点和终点的变化对算法影响不大，简单有效，效率高于Dijkstra算法，但也存在时间复杂 度高，不适合计算大量数据的缺点。
[0005] 以Di jkstra算法、Floyd算法为代表的最短路径算法属于盲目搜索算法，虽然能 够求得最短路径，但是计算量却非常大，适用于节点数较少的图结构，而对于节点数量非常 庞大的图结构却不适用。
[0006] 启发式搜索算法是基于具体领域的知识的搜索算法，在搜索的过程中，算法不仅 考虑节点的当前代价，而且考虑到了扩展该节点所需要的估计代价，使搜索过程向着最有 希望的方向前进，进而加速整个计算过程。启发性信息主要反映在估价函数上，在搜索过程 中估价函数的任务就是估计待搜索节点处于最佳路径上的可能性，从而优先搜索可能性比 较大的节点，从而达到提高搜索速度的目的。
[0007] 基于启发式搜索的最短路径算法有局部择优搜索算法、最好优先算法，以及常见 的Y算法，局部择优搜索算法是最简单的启发式搜索算法，在搜索的过程中，当某个节点被 扩展以后，"最优"的那个节点将被进一步扩展，舍弃掉了这个子节点的父节点和其他的扩 展子节点。如果搜索过程一直继续下去，由于舍弃许多的扩展子节点，就有可能把真正的最 佳节点都舍弃了，所以在一段过程中的最佳节点并不是全局的最佳节点，所以这种算法搜 索到的路径不一定是真正的最佳路径。
[0008] Y算法是启发式搜索算法中重要的方法之一。它是一种最好优先搜索算法，在搜 索过程中，没有舍弃掉节点，在每一步的估价中都把当前的节点和以前的节点的估价值比 较得到一个"最佳节点"。这样可以有效地防止"最佳节点"的丢失，提高了算法搜索结果的 准确性。在比较简单的地图上它的速度非常快，能很快找到最短路径。在比较复杂的地图 中，由于在每一步扩展子节点时，把当前节点的所有可扩展的子节点都保留下来，随着搜索 的进行，需要保留的节点数量越来越多，保留节点的数量过于庞大，导致搜索效率不高，占 用内存资源较大，所以在一些实时性要求高的搜索中并不适用。
【发明内容】
[0009] 本发明的目的在于克服现有技术的不足，提供在路径搜索过程中，只保留当前节 点与目标节点在同一方向上的可扩展节点，减少了当前节点的扩展节点的状态空间中的节 点数量，降低了算法的搜索规模，提高了算法的搜索效率的基于目标方向约束的路径规划 算法。
[0010] 本发明的目的是通过以下技术方案来实现的：基于目标方向约束的路径规划算 法，其原理为：在路径搜索过程中，只保留当前节点与目标节点在同一方向上的可扩展节 点，并将这些可扩展节点加入可扩展节点的状态空间，对状态空间中的每一个可扩展节点 进行评估，得到估价函数值最小的可扩展节点作为下一个当前节点，重复路径搜索，直到状 态空间中估价函数值最小的可扩展节点为目标节点，得到最优路径。
[0011] 进一步地，所述的步骤Sl中状态空间的确定方法为：设当前节点为S，当前节点S 的可扩展节点为N个，连线当前节点S与目标节点D，分别作该N个可扩展节点在连线方向 上的投影，选取投影恰好落在连线上方向上的节点加入状态空间。
[0012] 进一步地，所述的状态空间的确定的具体操作方法为：设当前节点S的坐标为 (X1, Y1, Z1)，其可扩展节点X的坐标为（x2, y2, Z2)，目标节点D的坐标为（x3, y3, Z3)，分别连 线 SX、SDU
[0013]
[0014]
[0015]
[0016]
[0017] 判断cosXSD值的大小：
[0018] (1)如果cosXSD <0,则可扩展节点X在连线SD上的投影在有向线段SD的反向 延长线上，则舍弃该可扩展节点X ;
[0019] (2)如果cosXSD彡0,则可扩展节点X在连线SD上的投影恰好在有向线段SD上， 保留符合该条件的可扩展节点X，并将所有的可扩展节点X形成当前节点S的可扩展节点的 状态空间。
[0020] 进一步地，所述的步骤S2的具体实现方法为：针对每个可扩展节点的评估，采用 该可扩展节点的启发式估价函数进行计算，f'（X)函数定义式为：
[0021] ?' (x) = g(x)+hr (X)
[0022] 式中，f'（X)是f (X)的估价函数，其中f (X)是从当前节点S到达目标节点D的实 际代价值，g(x)是从当前节点S到可扩展节点X的实际代价值；h'（X)是启发函数，是h(x) 的估计函数，其中h(x)是从可扩展节点X到目标节点D的实际的最小代价值，h'（X)小于 可扩展节点X到目标节点D的实际的最小代价；
[0023] 利用上述估价函数f'（X)来衡量状态空间中所有可扩展节点的重要程度，可扩展 节点的估价函数的值越小，该可扩展节点对寻路来讲就越重要，因此最终即选取估价函数 值最小的可扩展节点，将其作为下一步待扩展的当前节点。
[0024] 具体地，所述的路径规划算法的具体操作方法包括以下步骤：
[0025] 步骤1、生成空的open、close表，将起始节点放入open表中；
[0026] 步骤2、判断open表是否为空，如果open表为空，则表示没找到路径，失败退出，否 贝IJ，执行步骤3;
[0027] 步骤3、从open表中找出头节点作为当前节点，并将其从open表中移除，存入 close表中；
[0028] 步骤4、判断该头结点是否是目标节点，如果是，则头节点是终点，判断其是否存在 父节点；若存在父节点，则在close表中找到该节点的父节点，遍历close表直到起始节点， 找到最优路径，算法结束；若不存在父节点，则算法结束；若该头结点不是目标节点则执行 步骤5 ;
[0029] 步骤5、判断该头节点是否为可扩展节点，若是则进行步骤6,若不是则返回步骤 2 ；
[0030] 步骤6、将该头节点进行扩展，找到其可扩展节点，连线该头节点与目标节点，作可 扩展节点在连线方向上的投影，选择投影在连线上的可扩展节点构成集合V;
[0031] 步骤7、遍历集合V中的可扩展节点，如果可扩展节点既不在open表中，又不在 close表中，将该可扩展节点加入open表中，并计算该可扩展节点的估价函数，将扩展出这 些可扩展节点的头节点定义为其可扩展节点的父节点；
[0032] 步骤8、如果可扩展节点在open表中，则该可扩展节点在open表中原本已经存在 一个估价函数和一个父节点，比较该可扩展节点的估价函数和open表中原有的估价函数 大小，如果可扩展节点的估价函数小于open表中原有的估价函数，就更新该可扩展节点在 open表中的估价函数和父节点，若不小于则不操作；如果可扩展节点在close表中，则不做 处理，继续判断其它可扩展节点；
[0033] 步骤9、按照估价函数值递增的顺序，对open表中的所有节点进行排序，返回步骤 3〇
[0034] 进一步地，所述的步骤3中open表中的第一个头节点为起始节点，以后每一个头 节点即为open表中估价函数值最小的节点。
[0035] 本发明的有益效果是：与现有的算法相比，本发明在路径搜索过程中，不保留当前 节点的所有可扩展节点，只保留当前节点与目标节点在同一方向上的可扩展节点，减少了 当前节点的扩展节点的状态空间中的节点数量，降低了算法的搜索规模，减少了内存资源 的占用，提高了算法的搜索效率，适用于各种场景的路径搜索，特别适用于实时性要求高的 场景的路径搜索。
【附图说明】
[0036] 图1为本发明的本发明的路径搜索示意图；
[0037] 图2为本发明的具体实施例的算法流程图。
【具体实施方式】
[0038] 下面结合附图进一步说明本发明的技术方案。
[0039] 基于目标方向约束的路径规划算法，基于目标方向约束的路径规划算法，其算法 原理为：在路径搜索过程中，只保留当前节点（起始节点为第一个当前节点）与目标节点在 同一方向上的可扩展节点，并将这些可扩展节点加入可扩展节点的状态空间，对状态空间 中的每一个可扩展节点进行评估，得到估价函数值最小的可扩展节点作为下一个当前节点 (作为当前节点的节点已经不在状态空间中了），重复路径搜索，直到状态空间中估价函数 值最小的可扩展节点为目标节点，得到最优路径。
[0040] 根据几何学可知，两点之间直线距离最短，所以在道路网络拓扑中对给定两点进 行路径规划时，从起始节点到目标节点的连线方向，基本上代表了最短路径的大致走向。也 就是说，最终的最短路径基本是在两节点连线的两侧，而且通常在其附近，所以对算法搜索 的范围在连线两侧进行目标方向约束，即对每一个节点扩展的时候，连接当前节点与目标 节点的连线，对当前节点的可扩展节点在连线方向上的作投影，若可扩展节点的投影恰好 落在连线上，保留该可扩展节点：设当前节点为S，当前节点S的可扩展节点为N个，连线当 前节点S与目标节点D，分别作该N个可扩展节点在连线方向上的投影，选取投影恰好落在 连线上方向上的节点加入状态空间。如图1所示，比如在当前节点S时，它的可扩展的节点 有六个1、2、3、6、7、8。连线当前节点S与目标节点D，然后分别作该六个节点在连线方向上 的投影，则节点6、7、8的投影恰好落在连线上，而节点1、2、3的投影落在了连线的反向延长 线上，选取投影恰好落在连线上的节点。具体操作方法为：设当前节点S的坐标为（Xl，yi， Z1)，其可扩展节点X的坐标为（x2, y2, z2)