专利名称：一种随机路线图的快速路径规划方法及增强方法
技术领域：
本发明涉及运动规划领域，特别涉及一种随机路线图的快速路径规划方法及增强 方法。
背景技术：
在运动规划领域中路径规划是一重要分支，一般的路径规划方法处理复杂环境的 能力明显不足，极易受环境空间维数、实际环境的复杂度及需要处理的信息量等的影响。如 需要处理复杂环境下的运动规划，例如无人飞机导航中最优路线的实现，常需要超级计算 机的支持，对计算机运算速度有较高要求。近年来，一些优秀运动规划方法相继被提出。时至今日，各种方法的完备性都有待 讨论，但其可操作性和实用性是毋庸置疑的，即都能够在一定条件下实现运动规划。然而这 些方法大都有如下特点第一，最优式运动规划算法的计算时间随问题规模的变大而爆炸 式增长，即产生所谓的维数灾难；第二，一般的规划问题都是NP-hard (non-deterministic polynomial-hard，非确定性多项式困难问题)，这意味着不存在已知的多项式时间算法来 求取最优解。基于上述原因，一般规划问题往往先采用启发式算法构建模型，再在此基础上 采用最优式算法来获取问题的次优解。因此启发式的路图方法是当今路径规划运用较多的 方法。Visibility graph方法，Voronoi diagram方法以及随机路线图方法(Probabilistic Roadmap Method，简称PRM)是几种常用的路图方法，这些方法的相同点都是在一定的规划 空间生成路径组成的网络图。但Visibility graph方法和Voronoi diagram方法局限于 低维空间的路径规划，相对而言，PRM方法是一种使用较为普遍的路图方法，它跟以前路图 方法的不同之处在于，路图在规划空间中不是以确定的方式来构造，而是基于概率论，使用 某种随机概率的方法来构造的。随机路图方法的一个巨大优点在于，其复杂度主要依赖于 寻找路径的难度，跟整个规划环境的复杂度和规划空间的维数基本无关，实际操作性较强， 适合工程上应用。一般PRM方法操作流程首先在规划空间(比如地图)中随机生成足够数目自由节 点；其次采取某种策略将所生成的随机自由节点连接成边进而形成一个图，该图即是生成 的路图，这个阶段称为“学习阶段”;最后，在所生成的路图上进行运动规划，比如路径规划 或最优路径的查询，该阶段称为“查询阶段”。如图2所示为各区域分布示意图，在一个规划空间中，包括几个部分困难区域、 正常区域(C区域)以及障碍物区域，其中困难区域又可分为空旷区域(将该区域记为A区 域)以及障碍物之间的夹缝即狭窄通道区域(将该区域记为B区域)。由于一般PRM方法是 完全等概率地均勻生成自由节点，容易使得A区域中生成的随机自由节点数目不够，造成 路图的不完备性；以及使得B区域自由节点分布不理想，造成某些可行路径的遗漏。总之， 采用一般PRM方法，规划空间中的困难区域内自由节点数目分布不够，不能很好地搜寻到 通过该困难区域的可行路径，易遗漏某些可行路径导致寻径失败等问题。

发明内容
为解决上述问题，本发明提供一种随机路线图的快速路径规划方法及增强方法， 能够增加困难区域内自由节点的个数，减少遗漏可行路径的概率。本发明一种随机路线图的快速路径规划方法，包括如下步骤 步骤Si，在规划空间区域内随机生成自由节点；
步骤S2，采用增强方法对困难区域进行处理；
步骤S3，按照预定策略生成路步骤S4，在上述路图中根据运动规划要求查询最优路径。另外本发明还提供一种随机路线图的增强方法，步骤S2中采用增强方法对困难 区域中的狭窄通道区域进行处理的过程包括如下步骤
步骤S201，对于该狭窄通道区域的任意选取的一点，在该点的邻域内随机增加一个自 由节点；
步骤S202，按照以下步骤循环
判断增加的自由节点是否仍属于该狭窄通道区域，若是，则在该增加的自由节点的邻 域内再随机增加一个新自由节点；
步骤S203，当所述狭窄区域某一点的邻域内自由节点的个数达到一个预先设定的阈值 时，停止增加自由节点。从以上的方案可以看出，本发明由于采用增强方法对困难区域进行了处理，较好 地解决了困难区域内自由节点数目分布不够的问题，提高了路图的连通性，减少了遗漏某 些可行路径的概率。


图1为本发明方法流程图； 图2为各区域分布示意图3为一般PRM方法生成的路图； 图4为本发明生成的路图； 图5为一般PRM方法生成的最优路径； 图6为本发明生成的最优路径。
具体实施例方式本发明提供一种随机路线图的快速路径规划方法及增强方法，能够解决一般PRM 方法完全等概率地均勻生成自由节点，导致困难区域内分布的自由节点数目不够，从而遗 漏某些可行路径的问题。下面结合附图详细描述本发明的技术方案。作为一个实施例，假设在某一个规划 空间中，包括了 A区域(空旷区域)、B区域(狭窄通道区域)、C区域(正常区域)以及障碍物 区域，现在需要在规划空间中的某两点之间生成一条可行路径，则可以采用如下的做法
步骤Si，在规划空间区域内随机生成自由节点； 步骤S2，采用增强方法对困难区域进行处理； 步骤S3，按照预定策略生成路图；步骤S4，在上述路图中根据运动规划要求查询最优路径。其中，步骤Sl采用与一般PRM方法相同的做法随机等概率地均勻生成自由节点， 但是采用这种做法生成自由节点，在遇到某些规划空间的困难区域时容易造成自由节点分 布数目不够或不理想的问题，所以需对这些困难区域进行处理。本发明采用启发式增强方 法，从软件角度对该困难区域进行相关处理，具体过程如下
对困难区域中的狭窄通道区域(B区域)，B区域的困难主要是由于地形造成的，从而导 致B区域随机自由节点分布数目不够。这时可以在步骤S2中采用增强方法对该区域进行 处理，处理的过程包括如下步骤
步骤S201，对于B区域的任意选取的一点q，在该点q的邻域内随机添加一个自由节占.
^ \\\
步骤S202，按照以下步骤循环
判断增加的自由节点是否仍属于B区域，若是，则在该增加的自由节点的邻域内再随 机增加一个新自由节点；
例如，记第一次增加的自由节点为pl，若Pl仍属于B区域，则在pi的邻域内再随机增 加新自由节点P2……
步骤S203，当B区域某一点的邻域内自由节点的个数达到一个预先设定的阈值，则停 止增加自由节点。在步骤S201中，当q的邻域内增加的自由节点(记为pl)不属于B区域时，假设点 Pl属于正常区域(C区域)，而且Pl也一定不在障碍物上，则可将Pl与点q连接成一可行边。 然后回到步骤S201，即继续在q的邻域中增加新的自由节点p2，并且继续对p2进行判断是 否仍属于B区域；若p2仍属于B区域，则进入步骤S202，在p2的邻域内增加一个新的自由 节点P3，继续判断操作；若p3仍属于B区域，则在p3的邻域内随机增加一个新的自由节点 P4……若pn仍属于B区域，则在pn的邻域内随机增加一个新的自由节点ρ (η+1)……采用 了上述的增强方法对B区域进行处理后，该困难区域分布的自由节点数目得到一定程度的 增加。作为本发明的进一步改进，在上述的步骤S202循环增加自由节点的过程中(假设 已经循环了一定的次数)，若其中增加的某一点(设为ρη，η表示当前为第η次增加的自由节 点)不属于B区域，则将该增加的点pn与之前增加的那一点ρ (n-1)连接成一可行边，并返 回到之前增加的点P (n-1)的邻域中继续添加新的自由节点。这样做的好处是可以生成若 干连接困难区域和非困难区域的可行边(路径)，进一步提高路图的连通性。另外，困难区域还包括空旷区域(又称之为A区域)，A区域是由于比较空旷，所以 导致自由节点数目不够，为此在步骤S2中也需采用增强方法对该空旷区域进行相关处理， 处理的过程如下
对于该空旷区域的任意选取的一点q，在该点q的邻域内随机添加自由节点，直到空旷 区域内某一点的邻域内自由节点的个数达到一个预先设定的阈值。采用如上的增强方法对A区域进行处理后，提高了 A区域自由节点的分布密度，解 决了 A区域分布自由节点数目不够的问题，保证了空旷区域中自由节点个数的最小值，使 得生成的路图得以优化，路图的完备性得到改善。在A区域和B区域中计算某一点的邻域内自由节点的个数的具体过程如下定义
6函数n(q)用于记录点q的邻域内自由节点的个数，当在q的邻域内每增加一个自由节点 时，对函数n(q)的值进行一次更新，即在原η(q)的值上增加1。事实上在执行上述任一步 骤之前，可以首先将函数n(q)的值与预先设定的阈值进行一下比较，当n(q)小于预先设定 的阈值时才进行添加自由节点的操作，当比较发现n(q)大于或等于阈值时马上停止添加 节点操作，需要说明的是，这里点q具有任意性。由于上述的点q的任意性，故当出现如下的情况困难区域中随机选取的点q的邻 域内自由节点的个数已经达到阈值，但仍没有找到一条连通困难区域和非困难区域的较优 的可行路径。这种情况下，如果此时至少还存在另一点P的邻域内自由节点的个数仍小于 该阈值，则需在困难区域中另取一点P，采用增强方法对点P进行相关处理，如此循环…… 直到找到一条较优的可行路径，或者至少有一条可行路径可以连接困难区域与非困难区 域。阈值概念的提出是为了使某个区域内自由节点的个数达到一定的数值，以保证该 区域自由节点的个数不会太少而无法生成符合要求的路图。这个阈值的数值是根据具体环 境情况而由用户设定的，其中A区域和B区域由于环境情况的不同，具体数值也可能不同。经过对上述的A区域以及B区域的增强方法处理后，整个规划区域内的自由节点 个数都已经达到一定数目，这时候就可进入步骤S3，按照预定策略生成路图这个过程了，这 个过程可以是将处理后的整个区域中每个自由节点与其邻域内的各点相互连接成可行边 进而生成路图，当然还可以是按照其他的策略，例如将该点与其周围最接近的几点(这个数 目为预先设定值)分别相连接。图3为一般PRM方法所生成的路图，可见在空旷区域(A区域)以及狭窄通道区域 (B区域)由于自由节点数目不够，采用上述的某种策略生成路图时无法连接困难区域与非 困难区域中的自由节点生成可行边，这必然导致某些可行路径的遗漏。而本发明所生成的 路图则较好解决了这个问题，见图4，可知在A区域以及B区域中都相应增加了一定数目的 自由节点，并在此基础上采用某种策略生成了较完整的路图，这样就可以相应减少遗漏某 些可行路径的概率。另外在图3和图4中，图中还标出一正常区域(C区域)作为困难区域 的比较，本发明并未对该C区域进行任何处理。生成路图之后进入步骤S4，即在生成的路图中根据运动规划要求查询相应路径， 本发明中查询最优路径，对比图5和图6，可知在一般PRM方法中(图5)由于在困难区域与 非困难区域之间没有连通的可行路径，在生成最优路径时跳过了这些有可能是可行路径的 路径，从而导致了某些可行路径的遗漏，造成最后生成的最优路径明显不符合要求的情况； 而本发明(图6)则由于解决了困难区域分布的自由节点数目不够的问题，生成了若干连接 困难区域和非困难区域的可行路径，提高了路图的连通性，较好地找到了更符合要求的最 优路径，减少了遗漏通过困难区域的可行路径的概率。从以上方案可以看出，本发明继承了一般PRM方法的优点——实现的复杂度与整 个规划环境的复杂度及规划控件的维数基本无关，在此基础上，还弥补了一般PRM方法完 全等概率均勻分布自由节点导致路图生成完备性较差的缺点，采用增强方法对困难区域进 行相关处理，增加了困难区域内自由节点的数目，具有较好的完备性，不易遗漏通过规划空 间中困难区域的可行路径。以上所述的本发明实施方式，并不构成对本发明保护范围的限定。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明的权利要求保护范 围之内。
权利要求
一种随机路线图的快速路径规划方法，其特征在于，包括以下步骤步骤S1，在规划空间区域内随机生成自由节点；步骤S2，采用增强方法对困难区域进行处理；步骤S3，按照预定策略生成路图；步骤S4，在所述路图中根据运动规划要求查询最优路径。
2.根据权利要求1所述的一种随机路线图的快速路径规划方法，其特征在于，所述困 难区域包括狭窄通道区域，所述采用增强方法对该狭窄通道区域进行处理的过程包括如下 步骤步骤S201，对于该狭窄通道区域的任意选取的一点，在该点的邻域内随机增加一个自 由节点；步骤S202，按照以下步骤循环判断增加的自由节点是否仍属于该狭窄通道区域，若是，则在该增加的自由节点的邻 域内再随机增加一个新自由节点；步骤S203，当所述狭窄通道区域某一点的邻域内自由节点的个数达到一个预先设定的 阈值时，停止增加自由节点。
3.根据权利要求1所述的一种随机路线图的快速路径规划方法，其特征在于，所述困 难区域还包括空旷区域，所述采用增强方法对该空旷区域进行处理的过程如下对于该空旷区域的任意选取的一点，在该点的邻域内随机增加自由节点，直到该空旷 区域某一点的邻域内自由节点的个数达到所述预先设定的阈值。
4.根据权利要求2或3所述的一种随机路线图的快速路径规划方法，其特征在于，计算 所述某一点的邻域内自由节点的个数的具体过程如下定义函数n(q)用于记录点q的邻域内自由节点的个数，当点q的邻域内每增加一个自 由节点时对所述函数n(q)的值进行一次更新。
5.根据权利要求2所述的一种随机路线图的快速路径规划方法，其特征在于，所述步 骤S201之后还包括步骤若增加的自由节点不属于该狭窄通道区域，则连接该增加的自由 节点与所述任意选取的一点成一可行边，并返回步骤S201。
6.根据权利要求5所述的一种随机路线图的快速路径规划方法，其特征在于，按照步 骤S202循环了一定次数后，若增加的自由节点不属于该狭窄通道区域，则将该增加的自由 节点与之前增加的自由节点连接成一可行边，并在之前增加的自由节点的邻域内进行增加 自由节点的操作。
7.根据权利要求1所述的一种随机路线图的快速路径规划方法，其特征在于，所述步 骤S3中按照预定策略生成路图的具体步骤包括将所述处理后的规划空间区域中每个自由节点与其邻域内的点相互连接成可行边以 生成路图；或将所述处理后的规划空间区域中每个自由节点与其周围最接近的预定数目的点相互 连接成可行边以生成路图。
8.一种随机路线图的增强方法，其特征在于，包括权利要求2-6所述的一种随机路线 图的快速路径规划方法，所述采用增强方法对困难区域中狭窄通道区域进行处理的过程包 括如下步骤步骤S201，对于该狭窄通道区域的任意选取的一点，在该点的邻域内随机增加一个自 由节点；步骤S202，按照以下步骤循环判断增加的自由节点是否仍属于该狭窄通道区域，若是，则在该增加的自由节点的邻 域内再随机增加一个新自由节点；步骤S203，当所述狭窄通道区域某一点的邻域内自由节点的个数达到一个预先设定的 阈值时，停止增加自由节点。
全文摘要
本发明涉及运动规划领域，提供一种随机路线图的快速路径规划方法及增强方法，本发明的规划方法包括以下步骤在规划空间区域内随机生成自由节点；采用增强方法对困难区域进行处理；按照预定策略生成路图；根据运动规划要求查询最优路径。本发明继承了一般PRM方法的优点，实现发明方法的复杂度与整个规划环境的复杂度及规划控件的维数基本无关，还解决了一般PRM方法完全等概率均匀分布自由节点导致困难区域自由节点数目分布不够的问题，采用增强方法对困难区域进行相关处理，增加了困难区域内自由节点的数目，具有较好的完备性，减少了遗漏某些通过困难区域的可行路径的概率。
文档编号G01C21/34GK101963510SQ201010519248
公开日2011年2月2日 申请日期2010年10月26日 优先权日2010年10月26日
发明者曾相宗 申请人:广东威创视讯科技股份有限公司