CN112306067A - 一种全局路径规划方法及系统 - Google Patents

Abstract

本发明涉及一种全局路径规划方法及系统，所述方法公开了初始化采样区域、重复次数和搜索到路径次数；将采样地图设置为采样区域，重复次数用M表示，搜索到路径次数用L表示，L初始值为0；在所述采样地图上设置起点和终点，在所述采样地图上随机撒n个点；删除所述采样地图上障碍物上的点，所述采样区域上的点构成第一点集；在所述第一点集中搜索从所述起点到所述终点的路径。对已经搜索到的路径进行路径膨胀，将膨胀后的路径区域作为采样区域，通过在采样区域上构成的第一点集中搜索从起点到终点的路径，并且通过更新采样区域进行路径优化，加快了计算速度，提高了路径的优化效率。

Description

一种全局路径规划方法及系统

技术领域

本发明涉及路径规划技术领域，特别是涉及一种全局路径规划方法及系统。

背景技术

目前已有很多全局路径规划算法被广泛运用到移动机器人导航中，如A*算法、遗传算法(GA)、快速随机搜索树算法(RRT)等。在室内复杂环境尤其在障碍物数量较多并且位置分散的环境下，大部分的全局路径规划算法存在搜索效率过低、参数难以设置或出现局部最优等问题。快速随机搜索树算法(RRT和RRT*)在多障碍物复杂环境中存在搜索效率较低、占用内存过大、路径不平滑等问题。

发明内容

基于此，本发明的目的是提供一种全局路径规划方法及系统，提高了路径的优化效率。

为实现上述目的，本发明提供了如下方案：

一种全局路径规划方法，所述方法包括：

初始化采样区域、重复次数和搜索到路径次数；将采样地图设置为采样区域，重复次数用M表示，搜索到路径次数用L表示，L＝0；

在所述采样地图上设置起点和终点，

在所述采样地图上随机撒n个点；

删除所述采样地图上障碍物上的点，所述采样区域上的点构成第一点集；

在所述第一点集中搜索从所述起点到所述终点的路径；

若不能搜索到所述路径，返回步骤“在所述采样地图上随机撒n个点”；

若搜索到所述路径，L值加1，将所述路径记录为路径L；

若L＝1，则对所述路径进行膨胀，所述采样区域更新为所述路径的膨胀区域，返回步骤“在所述采样地图上随机撒n个点”；

若L>1，判断路径L与路径L-1是否相同；

若所述路径L与所述路径L-1不相同，则对所述路径进行膨胀，所述采样区域更新为所述路径的膨胀区域，返回步骤“在所述采样地图上随机撒n个点”；

若所述路径L与所述路径L-1相同，则M值加1；

若M小于或等于设置值，对所述路径进行膨胀，所述采样区域更新为所述路径的膨胀区域，返回步骤“在所述采样地图上随机撒n个点”；

若M大于所述设置值，则将路径L作为输出路径。

可选地，所述在所述第一点集中搜索从所述起点到所述终点的路径，具体包括：

根据所述第一点集确定无向邻接矩阵；

通过迪杰斯特拉算法，在所述无向邻接矩阵中搜索从所述起点到所述终点的路径。

可选地，所述对所述路径进行膨胀，具体包括：

向所述路径两侧膨胀设定距离，膨胀方向为垂直于所述起点到所述终点的直线。

可选地，所述设定距离的计算公式为：

其中，D_factor表示设定距离，E表示所述采样地图的长和宽中的最大值，

表示所述采样区域的拓展因子。

可选地，当L>1时，若连续N次不能搜索到所述起点到所述终点的路径，则减小所述采样区域的拓展因子，若搜索到所述路径，则所述采样区域的拓展因子

更新为最大值

可选地，所述方法还包括：采用B样条曲线平滑方法平滑所述输出路径。

可选地，所述方法还包括：

将所述输出路径上起点作为起始端点，依次将所述输出路径上点作为检测节点；

判断所述起始端点到所述检测节点之间是否与障碍物相交；

若不相交，则将所述起始端点到所述检测节点之间的点删除；

若相交，则将所述检测节点作为起始端点，返回步骤“判断所述起始端点到所述检测节点之间是否与障碍物相交”。

可选地，在所述采样地图上随机撒n个点之前，具体包括：

将所述采样地图栅格化；

将述采样地图上所占栅格数小于设定阈值的障碍物删除。

本发明还公开了一种全局路径规划系统，所述系统包括：

初始化模块，用于初始化采样区域、重复次数和搜索到路径次数；将采样地图设置为采样区域，重复次数用M表示，搜索到路径次数用L表示，L＝0；

起点和终点设置模块，用于在所述采样地图上设置起点和终点；

随机撒点模块，用于在所述采样地图上随机撒n个点；

点删除模块，用于删除所述采样地图上障碍物上的点，所述采样区域上的点构成第一点集；

路径搜索模块，用于在所述第一点集中搜索从所述起点到所述终点的路径；

返回模块，用于所述路径搜索模块不能搜索到所述路径时，返回随机撒点模块；

路径记录模块，用于所述路径搜索模块搜索到所述路径，L值加1，将所述路径记录为路径L；

第一判断模块，用于判断L是否为1；

第二判断模块，用于所述第一判断模块输出为否时，判断所述路径L与所述路径L-1是否相同；

第三判断模块，用于所述第二判断模块输出为是时，则M值加1，判断M是否小于或等于设置值；

路径输出模块，用于所述第三判断模块输出为否时，将路径L作为输出路径；

路径膨胀模块，用于所述第一判断模块输出为是时或第二判断模块输出为否时或所述第三判断模块输出为是时，对所述路径进行膨胀，所述采样区域更新为所述路径的膨胀区域，返回随机撒点模块。

可选地，所述路径搜索模块包括：

无向邻接矩阵确定单元，用于根据所述第一点集确定无向邻接矩阵；

路径搜索单元，用于通过迪杰斯特拉算法，在所述无向邻接矩阵中搜索从所述起点到所述终点的路径。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明公开的全局路径规划方法及系统，对已经搜索到的路径进行路径膨胀，将膨胀后的路径区域作为采样区域，通过在采样区域上构成的第一点集中搜索从起点到终点的路径，并且通过更新采样区域进行路径优化，加快了计算速度，提高了路径的优化效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明一种全局路径规划方法流程示意图；

图2为RRT*算法在地图上搜索到的路径；

图3为RRT*smart算法在地图上搜索到的路径；

图4为本发明一种全局路径规划方法在地图上搜索到的路径；

图5为采用本发明一种全局路径规划方法获得的路径的曲线曲率变化图；

图6为本发明一种全局路径规划系统结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种全局路径规划方法及系统，提高了路径的优化效率。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明一种全局路径规划方法流程示意图，如图1所示，一种全局路径规划方法包括以下步骤：

步骤101：初始化采样区域、重复次数和搜索到路径次数；将采样地图设置为采样区域，重复次数用M表示，搜索到路径次数用L表示，L＝0，即L的初始值为0。

步骤102：在所述采样地图上设置起点和终点。

步骤103：在所述采样地图上随机撒n个点。

步骤104：删除所述采样地图上障碍物上的点，所述采样区域上的点构成第一点集。

步骤102之前，具体包括：

将所述采样地图栅格化；将所述采样地图上所占栅格数小于设定阈值的障碍物删除。

步骤105：在所述第一点集中搜索从所述起点到所述终点的路径。

步骤106：判断搜索到所述路径。

若不能搜索到所述路径，返回步骤103。

若搜索到所述路径，执行步骤107。

步骤107：L值加1，将所述路径记录为路径L。例如，当第1次搜到到路径时，所述路径记录为路径1；当第2次搜到到路径时，所述路径记录为路径2；当第3次搜到到路径时，所述路径记录为路径3。

步骤108：判断L是否等于1。

若L＝1，则执行步骤113并返回步骤103；

步骤113：对所述路径进行膨胀，所述采样区域更新为所述路径的膨胀区域。

若L不等于0，执行步骤109。

步骤109：判断路径L与路径L-1是否相同；

若所述路径L与所述路径L-1不相同，则执行步骤113并返回步骤103；

若所述路径L与所述路径L-1相同，则执行步骤110。

步骤110：M值加1；

步骤111：判断M是否小于或等于设置值。

若M小于或等于设置值，则执行步骤113并返回步骤103；

若M大于所述设置值，则执行步骤112。

步骤112：将路径L作为输出路径。

步骤105，具体包括：

根据所述第一点集确定无向邻接矩阵。

通过迪杰斯特拉算法，在所述无向邻接矩阵中搜索从所述起点到所述终点的路径。

步骤113，具体包括：

向所述路径两侧膨胀设定距离，膨胀方向为垂直于所述起点到所述终点的直线。

所述设定距离的计算公式为：

其中，D_factor表示设定距离，E表示所述采样地图的长和宽中的最大值，

表示所述采样区域的拓展因子。设定

的默认值为4，

按照设定步长减小到1，则膨胀区域就覆盖整个地图，具备完整型。

当L不等于1(L>1)时，所述方法还包括：

若连续N次不能搜索到所述起点到所述终点的路径，则减小所述采样区域的拓展因子，若搜索到所述路径，则所述采样区域的拓展因子

更新为最大值

最小值为1。

所述方法还包括：采用B样条曲线平滑方法平滑所述输出路径。

所述方法还包括：将所述输出路径上的起点作为起始端点，依次将所述输出路径上的点作为检测节点。

判断所述起始端点到所述检测节点之间是否与障碍物相交；

若不相交，则将所述起始端点到所述检测节点之间的点删除；

若相交，则将所述检测节点作为起始端点，返回步骤“判断所述起始端点到所述检测节点之间是否与障碍物相交”，直到检测节点为终点结束。

本发明一种全局路径规划方法的具体实施方法为：

Step1：已知初始栅格地图MAP，栅格地图中障碍物的大小由障碍物所占栅格数表示，选取合适的设定阈值m，将所占栅格数大于设定阈值的障碍物保留，所占栅格数小于设定阈值的障碍物删去，从而对栅格地图MAP中的障碍物进行分割，得到简化全局栅格地图(Simplify global grid map，SMG-MAP)。

Step2：在Step1中的简化全局栅格地图SMG-MAP上随机撒n个点得到点集Z_rand，然后删除SMG-MAP中障碍物上的点(随机撒点是在整个地图上撒点，有可能会撒在障碍物上，路径不可能通过障碍物，所以障碍物上的点需要删除)，同时再添加起点Z_init和终点Z_goal得到新的点集Z_free，Z_free为在随机撒n的个构成的点集Z_rand的基础上删除障碍物上的点后再加上已知的起点和终点得到的新的点集，然后将Z_free中的所有点构建成无向邻接矩阵T，最后通过Dijkstra(迪杰斯特拉)算法在无向邻接矩阵T中搜索出一条由Z_init到Z_goal的路径点集Z_p。

如果没有搜索到路径就重复上述操作，即重复Step2的操作(在Step1中的简化全局栅格地图SMG-MAP上随机撒n个点……)直到搜索到路径为止，最终路径即为初始引导路径R。

Step3：将Step2中的初始路径R膨胀成初始引导区域(采样区域)，即在初始路径R所在线段两侧沿着垂直于路径起点和终点连线方向平移一定距离得到初始采样区域(采样区域)，确定该区域为SMG-RRT*算法的初始采样区域A，路径膨胀平移的距离公式如下：

其中，D_factor表示设定距离，E表示所述采样地图的长和宽中的最大值，

表示所述采样区域的拓展因子，E和

均大于0，路径膨胀时，在初始路径R所在线段两侧沿着垂直于路径起点和终点连线方向分别平移D_factor。

在初始采样区域A中通过Step2重新引导路径(重新搜索路径)，当搜索一定次数N后，通过减小

来更新采样区域，一旦算法在采样区域中初次搜索到路径时，

取定值

从而得到新的膨胀采样区域A_new和新的引导路径R_new。

重新通过Step3中的膨胀路径方法膨胀路径R_new确定采样区域，SMG-RRT*算法仅在新采样区域内采样使路径快速优化，当路径优化后，再重新以优化路径为中心膨胀确定采样区域，如此循环迭代，直至得到的优化的路径满足N次数搜索中未改变并且总搜索次数不小于设定的迭代次数时，算法结束，输出路径，其中设定值N与地图大小有关，地图越大，N值越大。

本发明采用引入节点选择技术，在SMG-RRT*算法添加新的节点时，当新的随机节点满足路径膨胀的距离公式时，才会被选择加入到随机树中，否则拒绝该节点加入随机树中。即满足关系式C_new≤C_path-C_g，C_path表示当前搜索到的最优路径的总代价，C_g表示新节点到目标点的欧式几何距离(m)，C_new表示新节点R_new的当前代价。

RRT算法随机采样的特性导致产生的路径点较曲折，有许多冗余和不必要的节点在路径优化时需要删除。

对得到的输出路径进行优化：首先以起始点为线段的起始端点，依次将路径上的后续点作为检测节点进行检测，将检测节点作为线段终止端点，检查从起始端点到终止端点之间的直线段是否和障碍物相交，如果不相交，则这两个节点之间的所有路径节点均可删除；如果直线段和障碍物相交，则以该检测节点作为直线段的起始端点，再次执行上述操作，直至到达目标点(终点)。然后基于最小转弯半径约束的路径修剪，当相邻路径形成的夹角α小于最小转弯半径所要求的最小路径夹角α_min时，在这两段路径中插入一段路径，从而使得路径夹角变得平缓，得到平缓处理后的路径，以确保使用B样条曲线平滑路径时，路径曲率不超过最大曲率。

采用B样条曲线对平缓处理后的路径进行连续曲率平滑处理，设现有控制顶点为P₀，P₁，P₂，…，P_n，则k阶(k-1次)B样条曲线的数学表达式为：

P_i表示控制顶点，控制顶点为路径上的点，N_i,k(u)表示K阶B样条曲线的基函数，P(u)表示B样条曲线平滑处理后的路径曲线，最后得到的路径曲线的曲率变化如图5所示。

RRT*算法解决了RRT算法的路径非最优解，确保每一个节点的路径总是当前最优，但RRT*算法是渐进最优的，其随着节点的不断增加，内存消耗和修剪随机树T的计算量会呈指数增加，同时算法的搜索效率和节点的使用率也不断降低。

分别通过RRT*算法、RRT*Smart算法和本发明一种全局路径规划方法(SMG-RRT*算法)在相同的地图上进行路径搜索，获得的搜索结果如图2-4所示，图2为RRT*算法在地图上搜索到的路径，图3为RRT*smart算法在地图上搜索到的路径，图4为本发明一种全局路径规划方法在地图上搜索到的路径；各算法仿真数据如表1所示。

表1图2-4中各算法仿真数据

由上述图表可知，本发明方法提高了RRT*算法的收敛速度和采样效率，并且通过路径的后处理，使得路径曲率连续。

本发明一种全局路径规划方法通过基于栅格地图简化的初始引导路径搜索方法，给RRT*算法选定初始引导路径并将初始引导路径周围的区域作为采样区域，在整个地图上撒点，将采样区域之外的点删除，仅取采样区域上的点，提高了算法在全地图随机撒点的搜索效率，加快了收敛速度。同时引入节点选择技术，当新的随机节点满足路径膨胀的距离公式时，才会被选择加入到随机树中，否则拒绝该节点加入随机树中。通过上述的采样区域优化和节点选择方法，可保证SMG-RRT*算法在提高算法搜索效率的同时，减小占用内存。最后使用基于最小转弯半径的路径修剪和基于B样条曲线优化算法搜索到的路径，使路径的曲率连续。

本发明还公开了一种全局路径规划系统，如图6所示，所述系统包括：

初始化模块201，用于初始化采样区域、重复次数和搜索到路径次数；将采样地图设置为采样区域，重复次数用M表示，搜索到路径次数用L表示，L＝0，即L初始值为0；

起点和终点设置模块202，用于在所述采样地图上设置起点和终点；

随机撒点模块203，用于在所述采样地图上随机撒n个点；

点删除模块204，用于删除所述采样地图上障碍物上的点，所述采样区域上的点构成第一点集；

路径搜索模块205，用于在所述第一点集中搜索从所述起点到所述终点的路径；

返回模块206，用于所述路径搜索模块不能搜索到所述路径时，返回随机撒点模块203；

路径记录模块207，用于所述路径搜索模块搜索到所述路径，L值加1，将所述路径记录为路径L；

第一判断模块208，用于判断L是否为1；

第二判断模块209，用于所述第一判断模块输出为否时，判断所述路径L与所述路径L-1是否相同；

第三判断模块210，用于所述第二判断模块输出为是时，则M值加1，判断M是否小于或等于设置值；

路径输出模块211，用于所述第三判断模块输出为否时，将路径L作为输出路径；

路径膨胀模块212，用于所述第一判断模块输出为是时或第二判断模块输出为否时或所述第三判断模块输出为是时，对所述路径进行膨胀，所述采样区域更新为所述路径的膨胀区域，返回随机撒点模块203。

所述路径搜索模块205还包括：

无向邻接矩阵确定单元，用于根据所述第一点集确定无向邻接矩阵；

路径搜索单元，用于通过迪杰斯特拉算法，在所述无向邻接矩阵中搜索从所述起点到所述终点的路径。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种全局路径规划方法，其特征在于，所述方法包括：

初始化采样区域、重复次数和搜索到路径次数：将采样地图设置为采样区域，重复次数用M表示，搜索到路径次数用L表示，L＝0；

在所述采样地图上设置起点和终点，

在所述采样地图上随机撒n个点；

删除所述采样地图上障碍物上的点，所述采样区域上的点构成第一点集；

在所述第一点集中搜索从所述起点到所述终点的路径；

若不能搜索到所述路径，返回步骤“在所述采样地图上随机撒n个点”；

若搜索到所述路径，L值加1，将所述路径记录为路径L；

若L＝1，则对所述路径进行膨胀，所述采样区域更新为所述路径的膨胀区域，返回步骤“在所述采样地图上随机撒n个点”；

若L>1，判断路径L与路径L-1是否相同；

若所述路径L与所述路径L-1不相同，则对所述路径进行膨胀，所述采样区域更新为所述路径的膨胀区域，返回步骤“在所述采样地图上随机撒n个点”；

若所述路径L与所述路径L-1相同，则M值加1；

若M小于或等于设置值，对所述路径进行膨胀，所述采样区域更新为所述路径的膨胀区域，返回步骤“在所述采样地图上随机撒n个点”；

若M大于所述设置值，则将路径L作为输出路径。

2.根据权利要求1所述的全局路径规划方法，其特征在于，所述在所述第一点集中搜索从所述起点到所述终点的路径，具体包括：

根据所述第一点集确定无向邻接矩阵；

通过迪杰斯特拉算法，在所述无向邻接矩阵中搜索从所述起点到所述终点的路径。

3.根据权利要求1所述的全局路径规划方法，其特征在于，所述对所述路径进行膨胀，具体包括：

向所述路径两侧膨胀设定距离，膨胀方向为垂直于所述起点到所述终点的直线。

4.根据权利要求3所述的全局路径规划方法，其特征在于，所述设定距离的计算公式为：

其中，D_factor表示设定距离，E表示所述采样地图的长和宽中的最大值，

表示所述采样区域的拓展因子。

5.根据权利要求4所述的全局路径规划方法，其特征在于，当L>1时，若连续N次不能搜索到所述起点到所述终点的路径，则减小所述采样区域的拓展因子，若搜索到所述路径，则所述采样区域的拓展因子

更新为最大值

6.根据权利要求1所述的全局路径规划方法，其特征在于，所述方法还包括：采用B样条曲线平滑方法平滑所述输出路径。

7.根据权利要求1所述的全局路径规划方法，其特征在于，所述方法还包括：

将所述输出路径上的起点作为起始端点，依次将所述输出路径上的点作为检测节点；

判断所述起始端点到所述检测节点之间是否与障碍物相交；

若不相交，则将所述起始端点到所述检测节点之间的点删除；

若相交，则将所述检测节点作为起始端点，返回步骤“判断所述起始端点到所述检测节点之间是否与障碍物相交”。

8.根据权利要求1所述的全局路径规划方法，其特征在于，在所述采样地图上随机撒n个点之前，具体包括：

将所述采样地图栅格化；

将所述采样地图上所占栅格数小于设定阈值的障碍物删除。

9.一种全局路径规划系统，其特征在于，所述系统包括：

初始化模块，用于初始化采样区域、重复次数和搜索到路径次数；将采样地图设置为采样区域，重复次数用M表示，搜索到路径次数用L表示，L＝0；

起点和终点设置模块，用于在所述采样地图上设置起点和终点；

随机撒点模块，用于在所述采样地图上随机撒n个点；

点删除模块，用于删除所述采样地图上障碍物上的点，所述采样区域上的点构成第一点集；

路径搜索模块，用于在所述第一点集中搜索从所述起点到所述终点的路径；

返回模块，用于所述路径搜索模块不能搜索到所述路径时，返回随机撒点模块；

路径记录模块，用于所述路径搜索模块搜索到所述路径，L值加1，将所述路径记录为路径L；

第一判断模块，用于判断L是否为1；

第二判断模块，用于所述第一判断模块输出为否时，判断所述路径L与所述路径L-1是否相同；

第三判断模块，用于所述第二判断模块输出为是时，则M值加1，判断M是否小于或等于设置值；

路径输出模块，用于所述第三判断模块输出为否时，将路径L作为输出路径；

路径膨胀模块，用于所述第一判断模块输出为是时或第二判断模块输出为否时或所述第三判断模块输出为是时，对所述路径进行膨胀，所述采样区域更新为所述路径的膨胀区域，返回随机撒点模块。

10.根据权利要求9所述的全局路径规划系统，其特征在于，所述路径搜索模块包括：

无向邻接矩阵确定单元，用于根据所述第一点集确定无向邻接矩阵；

路径搜索单元，用于通过迪杰斯特拉算法，在所述无向邻接矩阵中搜索从所述起点到所述终点的路径。

Images