Lambda*路径规划算法

摘要

本发明公开了一种Lambda*路径规划算法，针对现有A*算法中open表中所含节点多、耗时多的问题进行改进。其算法步骤包括：采用可视图构建路径规划的环境、创建open表和closed表、创建节点的数据结构、搜索路径、加入Smooth过程对路径进行平滑处理。本发明主要应用于机器人二维、三维空间的快速路径规划。

权利要求

1.一种Lambda*路径规划算法，其特征在于，包括以下步骤:

(1)采用可视图构建路径规划的环境：采用障碍物包络体包络障碍物，由路径规划的起点pstart、终点pgoal、以及障碍物的顶点，构成节点集合P；

(2)创建两张表：open表和closed表，其中closed表中记录已扩展过的节点，open表仅保存当前扩展节点的后续节点，不保存已扩展节点的其他后续节点；但是，在当前扩展节点的后续节点中，有部分可能是已扩展节点的后续节点，曾经进入过open表；

(3)创建节点的数据结构：α(p)表示可视图中从pstart到节点p的最短路径长度，β(p)表示从p到终点pgoal的实际最短路径长度；每个节点p数据结构包含父节点pre、节点的g值gval和f值fval，p.gval表示从起点pstart到节点p的现有最短路径的长度，p.gval≥α(p)；估价函数值为p.fval＝p.gval+hval(p)，启发函数hval(p)为p到终点pgoal的最短距离的估计值，节点的估价函数值p.fval代表经过该节点p，从起点pstart到终点pgoal的最短路径的一个估计值；若hval(p)≤β(p)，即启发函数值小于或等于节点p到终点pgoal的实际最短路径长度，则可以找到从起点pstart到终点pgoal的最短路径；

(4)搜索路径：扩展当前节点pclosed时先清空open表，仅将pclosed的不在closed表中的后续节点加入open表，丢掉当前扩展节点的父节点、兄弟节点；由于这些丢掉的节点可能与pclosed可视，被重新加入到open表中，由此在路径pstart,p1,p2...px,p上，p作为px的后续节点加入closed表，但p与从pstart出发到px所经过的路径点可视；

(5)加入Smooth过程对路径进行平滑处理：从前往后检查路径上的节点pi与后面节点pj是否可视，其中，1≤i≤z-1,i+1＜j≤z，其中z为路径上的节点数；若可视，则将它们之间的节点都删除，并调整平滑后各节点的pre,gval,fval。

2.根据权利要求1所述的一种Lambda*路径规划算法，其特征在于，节点之间可视是指，对于空间障碍物，节点之间的直线段不通过空间障碍物的内部，也不通过空间障碍物的公共面，但允许通过空间障碍物的公共线；对于平面障碍物，指节点之间的线段不通过平面障碍物的内部，也不通过平面障碍物的公共线，但允许通过平面障碍物的公共点，可视与否通过节点间的连线段与障碍物包络体各面的位置关系来判断，节点之间可视时lineofsight(p,p')为真，其中，函数lineofsight(p,p')用于判断p和p'是否可视，具体是判断p和p'与障碍物Oj各顶点连线的位置关系； 是与节点p可视的节点集合。

3.根据权利要求1或2中任一权利要求所述的一种Lambda*路径规划算法，其特征在于，选取节点p到终点pgoal的欧氏距离作为启发函数hval(p)。

说明书

Lambda*路径规划算法

【技术领域】

[0001] 本发明涉及智能控制领域，特别涉及移动机器人的路径规划技术领域。【背景技术】

[0002] 随着工业机器人应用要求的进一步提高，已有的二维路径规划技术已经满足不了工业机器人在许多领域的需求，人们迫切需要一套成熟的、能应用于三维空间的快速路径规划技术。

[0003] 基于自由空间构造的规划方法是一类重要的路径规划方法。这类方法先通过几何方法将具有障碍物的连续环境表示成图，然后利用图搜索方法搜索一条从起始点到目标点的无碰最短路径。

[0004] 机器人研究领域的学者常常将连续的环境用栅格图(Grid graphs)、可视图(Visibility graphs)、导航网格(Navigation meshs)、概率路线图(Probabilistic road map,PRMs)和快速探索随机树(Rapidly exploring random trees)等表示。其中，可视图中的节点包括起点pstart和终点pgoal，以及空间中障碍物的顶点。当且仅当两个节点之间的连线不与空间中的障碍物相交时，将两个节点用直线连接起来就构成了可视图。由于没有栅格限制，允许在任意方向进行路径搜索，在可视图中搜索获得的路径较在栅格图中获取的路径短。

[0005] 常用的图搜索方法包括非启发式算法和启发式算法。启发式搜索方法大致可分为局部择优搜索方法和最好优先搜索方法。局部择优搜索方法在搜索的过程中选取了“最佳节点”后，舍弃其他的兄弟节点、父亲节点。由于舍弃了其他的节点，可能也把最好的节点都舍弃了，求解的“最佳节点”只是在该阶段的最佳，并不一定是全局的最佳。而最好优先搜索方法在搜索时，没有舍弃节点(除非该节点是死节点)，在每一步的估价中都把当前扩展节点的后续节点和以前的节点的估价值比较得到一个“最佳的节点”。A*算法就是一种最好优先搜索方法，A*算法利用启发式信息，对当前未扩展节点根据设定的估价函数值选取离目标最近的节点进行扩展，从而缩小搜索空间；选取合适的启发函数，则能使得A*算法具有可采纳性。然而A*算法拓展的节点多，采用A*算法在三维空间进行路径规划不能保证最优，且耗时较多。【发明内容】

[0006] 本发明在分析A*的算法的基础上对其进行改进。针对A*算法中open表中所含节点多、耗时多的问题，提出一种能应用于机器人二维、三维空间的快速路径规划算法Lambda*路径规划算法。

[0007] 本发明的技术方案如下:

[0008] 一种Lambda*路径规划算法，其特征在于，包括以下步骤:

[0009] (1)采用可视图构建路径规划的环境：采用障碍物包络体包络障碍物，由路径规划的起点pstart、终点pgoal、以及障碍物的顶点，构成节点集合P；

[0010] (2)创建两张表：open表和closed表，其中closed表中记录已扩展过的节点，open表仅保存当前扩展节点的后续节点，不保存已扩展节点的其他后续节点；但是，在当前扩展节点的后续节点中，有部分可能是已扩展节点的后续节点，曾经进入过open列表；

[0011] (3)创建节点的数据结构：α(p)表示可视图中从pstart到节点p的最短路径长度，β(p)表示从p到终点pgoal的实际最短路径长度；每个节点p数据结构包含父节点pre、节点的g值gval和f值fval，p.gval表示从起点pstart到节点p的现有最短路径的长度，p.gval≥α(p)；估价函数值为p.fval＝p.gval+hval(p)，启发函数hval(p)为p到终点pgoal的最短距离的估计值，节点的估价函数值p.fval代表经过该节点p，从起点pstart到终点pgoal的最短路径的一个估计值；若hval(p)≤β(p)，即启发函数值小于或等于节点p到终点pgoal的实际最短路径长度，则可以找到从起点pstart到终点pgoal的最短路径；

[0012] (4)搜索路径：扩展当前节点pclosed时先清空open表，仅将pclosed的不在closed中的后续节点加入open，丢掉当前扩展节点的父节点、兄弟节点；由于这些丢掉的节点可能与pclosed可视，被重新加入到open中，由此在路径pstart,p1,p2...px,p上，p作为px的后续节点加入closed，但p与从pstart出发到px所经过的路径点可视；

[0013] (5)加入Smooth过程路径进行平滑处理：从前往后检查路径上的节点pi与后面节点pj是否可视，其中，1≤i≤z-1,i+1＜j≤z，其中z为路径上的节点数；若可视，则将它们之间的节点都删除，并调整平滑后各节点的pre,gval,fval。

[0014] 所述的一种Lambda*路径规划算法，其特征在于，节点之间可视是指，对于空间障碍物，节点之间的直线段不通过空间障碍物的内部，也不通过空间障碍物的公共面，但允许通过空间障碍物的公共线；对于平面障碍物，指节点之间的线段不通过平面障碍物的内部，也不通过平面障碍物的公共线，但允许通过平面障碍物的公共点，可视与否通过节点间的连线段与障碍物包络体各面的位置关系来判断，节点之间可视时lineofsight(p,p')为真，其中，函数lineofsight(p,p')用于判断p和p'是否可视，具体是判断p和p'与障碍物Oj各顶点连线的位置关系； 是与节点p可视的节点集合。

[0015] 所述的一种Lambda*路径规划算法，其特征在于，选取节点p到终点pgoal的欧氏距离作为启发函数hval(p)。

[0016] 本发明的有益效果在于，由于open仅保存当前扩展节点的后续节点，不保存已扩展节点的其他后续节点，大大减少了计算量，能在较少的时间内得到较优的路径。本算法以较小的路径长度增加为代价，大幅度地减少了路径规划的耗时。【附图说明】

[0017] 图1为A*算法伪代码

[0018] 图2为本发明实施例1的路径规划算法伪代码

[0019] 图3为本发明实施例1采用的路径平滑算法伪代码

[0020] 图4为本发明实施例2的路径规划算法流程图【具体实施方式】

[0021] 为了使本发明实施例的目的、技术方案、优点更加清晰、明确，下面结合实施方式和附图，对本发明做进一步详细说明。

[0022] 实施例1：本发明实施于平面路径规划时的算法

[0023] 图2所示是本发明实施于平面路径规划时的算法伪代码。如图2所示，本发明实施例1的路径规划算法包括：

[0024] ①根据障碍物的位置及形状生成包络体，生成路径中间点集合V，本发明实施例1中采用矩形包络，路径中间点为障碍物包络体的顶点；

[0025] ②构建中间的数据结构，创建open和closed表；

[0026] ③将pstart插入closed表；

[0027] ④当pgoal不在closed表中，则扩展节点，转向⑤；否则平滑路径(算法伪代码如图3所示)，并输出路径信息；

[0028] ⑤将closed中fval最小的节点作为pclosed进行扩展，在节点集合V中查找不在closed中、且lineofsight(p,pclosed)为真的点p，作为pclosed的后续节点插入open表，更新节点数据，包括更新节点的父节点、节点gval与fval；

[0029] ⑥若open表为空，则输出“没能找到路径”；否则，从open表中选出fval最小的节点插入closed；转入④。

[0030] 实施例1中，lineofsight(p,pclosed)判断p和pclosed是否可视，具体是判断p和pclosed与障碍物Oj各顶点连线的位置关系，lineofsight(p,pclosed)遇以下情况时为假：

[0031] 存在障碍物Oj，p和pclosed连线与Oj的四个顶点中任意两顶点q1,q2连线互相跨越，即p在q1,q2的一边，pclosed在q1,q2的另一边，q1在p和pclosed连线一侧，q2在p和pclosed连线的另一侧；

[0032] 存在障碍物Oj，p在Oj的四个顶点中任意两顶点q1,q2连线上，且在q1,q2线段内；

[0033] 存在障碍物Oj，pclosed在Oj的四个顶点中任意两顶点q1,q2连线上，且在q1,q2线段内；

[0034] 本发明实施例1所采用的路径平滑算法伪代码如图3所示。从前往后检查路径上的节点pi(1≤i≤z-1)与后面节点pj(i+1＜j≤z)是否可视。若可视，则将它们之间的节点都删除，并调整平滑后各节点的pre,gval,fval。虽然增加了Smooth()过程，时耗也相应增加，但由于Smooth()仅对路径上的节点进行平滑处理，处理的节点较open表中的节点数量少，因此相同应用环境下，Lambda*算法耗时比A*算法少。

[0035] 在实施例1中，分别采用本发明算法和A*算法进行路径规划。设定2D环境大小为100*100，起点为(0,0)，终点为(100,100)；随机生成障碍物，障碍物比例分别设为0％，5％，
10％，20％，30％，40％。两种算法均在Matlab2012a上实现，实验用计算机CPU型号为Intel(R)Core(TM)2Duo T6500，主频均为2.1GHz，RAM为1.86Gbyte。统计每种情况下用两种算法分别进行100次路径规划的平均路径长度、耗时、总节点数、扩展节点数和open中容纳的节点总数，结果如下表1所示。

[0036] 表1

[0037]

[0038] 表1所示结果显示，随机障碍物比例越大，总节点数增多，路径规划也越来越复杂，所得路径长度和耗时、扩展节点数以及open表中的节点数都呈增加趋势。

[0039] 将两种算法在各种障碍物比例情况下所获得路径长度和耗时进行比较，结果如下表2所示。

[0040] 表2

[0041]

[0042] 总体而言，在相同情况下，Lambda*需扩展的节点数和open表中节点总数都显著小于A*的对应值，耗时较少。随着障碍物比例增大，Lambda*算法所获路径质量略低于A*算法，Lambda*算法所获路径的长度平均为A*算法所获路径长度的1.0030倍，即路径长度增加了0.3％，同时，Lambda*算法耗时相对于A*算法减少了48.76％。

[0043] 实施例2，本发明实施于3D环境下的路径规划时的算法

[0044] 图4所示是本发明实施于3D环境下的路径规划时的算法流程图。如图4所示，本发明实施例2的路径规划算法包括：

[0045] ①根据障碍物的位置及形状生成包络体，生成路径中间点集合V，本发明实施例2中采用长方体包络，路径中间点为障碍物包络体的顶点；

[0046] ②构建中间的数据结构，创建open和closed表；

[0047] ③将pstart插入closed表；

[0048] ④当pgoal不在closed表中，则扩展节点，转向⑤；否则平滑路径(算法伪代码如图3所示)，并输出路径信息。

[0049] ⑤将closed中fval最小的节点作为pclosed进行扩展，在节点集合V中查找不在closed中，且lineofsight(p,pclosed)为真的点p作为pclosed的后续节点插入open表。更新节点数据，包括更新节点的父节点、节点gval与fval。

[0050] ⑥若open表为空则输出“没能找到路径”；否则，从open表中选出fval最小的节点插入closed；转入④。

[0051] 实施例2采用的路径平滑算法流程与实施例1中的路径平滑算法相同，不再赘述。

[0052] 实施例2中，lineofsight(p,pclosed)判断p和pclosed是否可视，具体是判断p和pclosed连线与长方体障碍物包络Oj 6个矩形表面位置的关系。仅当存在一个矩形表面，p和pclosed的连线与该面有一个公共点，且公共点在p和pclosed的线段上时，lineofsight(p,pclosed)为假。

[0053] 实施例2中，分别采用本发明算法和A*算法进行路径规划。设定3D环境大小为100*100*100，起点为(0,0,0)，终点为(100,100,100)，随机生成障碍物，障碍物比例分别设为
0％，5％，10％，20％，30％，40％。两种算法均在Matlab 2012a上实现，计算机CPU型号为Intel(R)Core(TM)2Duo T6500，主频均为2.1GHz，RAM为1.86Gbyte。统计每种情况下用两种算法分别进行100次路径规划的平均路径长度，耗时，总节点数，扩展节点数和open中容纳的节点总数，结果如下表3所示。

[0054] 表3

[0055]

[0056] 表3所示结果显示，随机障碍物比例越大，总节点数增多，路径规划也越来越复杂，所得路径长度和耗时、扩展节点数以及open表中的节点数都呈增加趋势。将两种算法在各种障碍物比例情况下所获得路径长度和耗时进行比较，结果如下表4所示。

[0057] 表4

[0058]

[0059] 总体而言，在相同情况下，Lambda*需扩展的节点数和open表中节点总数都显著小于A*的对应值，耗时较少。在实施例2中，Lambda*算法所获路径的长度平均为A*算法所获路径长度的1.0025倍，即路径长度增加了0.25％，同时，Lambda*算法耗时相对于A*算法减少了30.11％。

[0060] 以上所述具体实施例对本发明的目的、技术方案和有益效果进行了进一步的说明。所应理解的是，以上所述并不用于限定本发明的保护范围，凡在本发明的精神和原则内所做的任何修改、等同替换、改进等均应包含在本发明的保护范围之内。

[0061] 本发明的有益效果在于，通过减少open表中保持的节点数，减少了计算量。这虽然会在一定程度上影响了路径质量，但能在较少的时间里得到较优的路径，可以满足机器人二维、三维空间的快速路径规划的应用需求。