Lambda*路径规划算法
【专利摘要】本发明公开了一种Lambda*路径规划算法，针对现有A*算法中open表中所含节点多、耗时多的问题进行改进。其算法步骤包括：采用可视图构建路径规划的环境、创建open表和closed表、创建节点的数据结构、搜索路径、加入Smooth过程对路径进行平滑处理。本发明主要应用于机器人二维、三维空间的快速路径规划。
【专利说明】Lambda*路径规划算法
【【技术领域】】
[0001]本发明涉及智能控制领域，特别涉及移动机器人的路径规划【技术领域】。
【【背景技术】】
[0002]随着工业机器人应用要求的进一步提高，已有的二维路径规划技术已经满足不了工业机器人在许多领域的需求，人们迫切需要一套成熟的、能应用于三维空间的快速路径规划技术。
[0003]基于自由空间构造的规划方法是一类重要的路径规划方法。这类方法先通过几何方法将具有障碍物的连续环境表示成图，然后利用图搜索方法搜索一条从起始点到目标点的无碰最短路径。
[0004]机器人研究领域的学者常常将连续的环境用栅格图(Grid graphs)、可视图(Visibility graphs)、导航网格(Navigation meshs)、概率路线图(Probabilistic roadmap, PRMs)和快速探索随机树(Rapidly exploring random trees)等表示。其中，可视图中的节点包括起点Pstart和终点PgMl，以及空间中障碍物的顶点。当且仅当两个节点之间的连线不与空间中的障碍物相交时，将两个节点用直线连接起来就构成了可视图。由于没有栅格限制，允许在任意方向进行路径搜索，在可视图中搜索获得的路径较在栅格图中获取的路径短。
[0005]常用的图搜索方法包括非启发式算法和启发式算法。启发式搜索方法大致可分为局部择优搜索方法和最好优先搜索方法。局部择优搜索方法在搜索的过程中选取了 “最佳节点”后，舍弃其他的兄弟节点、父亲节点。由于舍弃了其他的节点，可能也把最好的节点都舍弃了，求解的“最佳节点”只是在该阶段的最佳，并不一定是全局的最佳。而最好优先搜索方法在搜索时，没有舍弃节点(除非该节点是死节点)，在每一步的估价中都把当前扩展节点的后续节点和以前的节点的估价值比较得到一个“最佳的节点”。A*算法就是一种最好优先搜索方法，A*算法利用启发式信息，对当前未扩展节点根据设定的估价函数值选取离目标最近的节点进行扩展，从而缩小搜索空间；选取合适的启发函数，则能使得A*算法具有可采纳性。然而A*算法拓展的节点多，采用A*算法在三维空间进行路径规划不能保证最优，且耗时较多。
【
【发明内容】
】
[0006]本发明在分析A*的算法的基础上对其进行改进。针对A*算法中open表中所含节点多、耗时多的问题，提出一种能应用于机器人二维、三维空间的快速路径规划算法Lambda*路径规划算法。
[0007]本发明的技术方案如下:
[0008]一种Lambda*路径规划算法，其特征在于，包括以下步骤:
[0009](I)采用可视图构建路径规划的环境:采用障碍物包络体包络障碍物，由路径规划的起点Pstart、终点PgMl、以及障碍物的顶点，构成节点集合P ；[0010](2)创建两张表:open表和closed表,其中closed表中记录已扩展过的节点，open表仅保存当前扩展节点的后续节点，不保存已扩展节点的其他后续节点；但是，在当前扩展节点的后续节点中，有部分可能是已扩展节点的后续节点，曾经进入过open列表；
[0011](3)创建节点的数据结构:a (p)表示可视图中从Pstart到节点p的最短路径长度，P (P)表示从P到终点Pgral的最短路径长度；每个节点P数据结构包含父节点pre、节点的g值gval和f值fval, p.gval表示从起点Pstart到节点P的现有最短路径的长度，p.gval≥a (p);估价函数值为p.fval=p.gval+hval (p),启发函数hval (p)为p到终点Pgoal的最短距离的估计值，节点的估价函数值P.fval代表经过该节点P，从起点Pstart到终点PgMl的最短路径的一个估计值；若hval (P) < P (P)，即启发函数值小于节点P到终点Pgoal的实际最短路径长度，则可以找到最短路径；
[0012](4)搜索路径:扩展当前节点Peltjsed时先清空open表,仅将Peltjsed的不在closed中的后续节点加入open,丢掉当前扩展节点的父节点、兄弟节点；由于这些丢掉的节点可能与Pdtjsed可视，被重新加入到open中，由此在路径Pstart, Pi, P2, ? ? ? Px, P上,P作为px的后续节点加入closed,但p与px的前辈可视；
[0013](5)加入Smooth过程对路径进行平滑处理:从前往后检查路径上的节点Pi(l≤i≤Z-1)与后面节点Pj(i + l〈j ( z)是否可视；若可视，则将它们之间的节点都删除，并调整平滑后各节点的pre, gval, fval。
[0014]所述的一种Lambda*路径规划算法，其特征在于，节点之间可视是指，对于空间障碍物，节点之间的直线段不通过空间障碍物的内部，也不通过空间障碍物的公共面，但允许通过空间障碍物的公共线；对于平面障碍物，指节点之间的线段不通过平面障碍物的内部，也不通过平面障碍物的公共线，但允许通过平面障碍物的公共点，可视与否通过节点间的连线段与障碍物包络体各面的位置关系来判断，节点之间可视时lineofsight (p，p’)为真'succ(p) C Z5是与节点P可视的节点集合。
[0015]所述的一种Lambda*路径规划算法，其特征在于，估价函数值为p.fval=p.gval+hval(p)。
[0016]所述的一种Lambda*路径规划算法，其特征在于，选取节点p到终点pgMl的欧氏距离作为启发函数hval (p)。
[0017]本发明的有益效果在于，由于open仅保存当前扩展节点的后续节点，不保存已扩展节点的其他后续节点，大大减少了计算量，能在较少的时间内得到较优的路径。本算法以较小的路径长度增加为代价，大幅度地减少了路径规划的耗时。
【【专利附图】
【附图说明】】
[0018]图1为A*算法伪代码
[0019]图2为本发明实施例1的路径规划算法伪代码
[0020]图3为本发明实施例1采用的路径平滑算法伪代码
[0021]图4为本发明实施例2的路径规划算法流程图
【【具体实施方式】】
[0022]为了使本发明实施例的目的、技术方案、优点更加清晰、明确，下面结合实施方式和附图，对本发明做进一步详细说明。
[0023]实施例1:本发明实施于平面路径规划时的算法
[0024]图2所示是本发明实施于平面路径规划时的算法伪代码。如图2所示，本发明实施例I的路径规划算法包括:
[0025]①根据障碍物的位置及形状生成包络体，生成路径中间点集合V，本发明实施例1中采用矩形包络，路径中间点为障碍物包络体的顶点；
[0026]②构建中间的数据结构，仓Il建open和closed表；
[0027]③将pstart 插入 closed 表；
[0028]④当PgMl不在closed表中，则扩展节点，转向⑤；否则平滑路径(算法伪代码如图3所示)，并输出路径信息；
[0029]⑤将closed中fva l最小的节点作为Pdtjsed进行扩展，在节点集合V中查找不在closed中、且Iineofsight (P, Pclosed)为真的点P，作为Pclosed的后续节点插入open表,更新节点数据，包括更新节点的父节点、节点gval与fval;
[0030]⑥若open表为空，则输出“没能找到路径”;否则，从open表中选出fval最小的节点插入closed;转入④。
[0031]实施例1中，lineofsight (p, Pclosed)判断P和Peltjsed是否可视，具体是判断P和Pdosed与障碍物O」各顶点连线的位置关系，lineofsight (P, Pclosed)遇以下情况时为假:
[0032]?存在障碍物Oj, P和Peltjsed连线与Oj的四个顶点中任意两顶点q” q2连线互相跨越，即P在q” q2的一边,Pcltjsed在q” q2的另一边,q!在P和Pclosed连线一侧，q2在P和Pclosed连线的另一侧；
[0033]?存在障碍物0」，P在Oj的四个顶点中任意两顶点qi，q2连线上，且在q。q2线段内；
[0034]?存在障碍物Oj，Peltjsed在Oj的四个顶点中任意两顶点qi，q2连线上，且在q。q2线段内；
[0035]本发明实施例1所采用的路径平滑算法伪代码如图3所示。从前往后检查路径上的节点Pi(l≤i≤Z-1)与后面节点Pj(i+l〈j ( z)是否可视。若可视，则将它们之间的节点都删除，并调整平滑后各节点的pre，gval, fval。虽然增加了 SmoothO过程，时耗也相应增加，但由于Smooth O仅对路径上的节点进行平滑处理,处理的节点较open表中的节点数量少，因此相同应用环境下，Lambda*算法耗时比A*算法少。
[0036]在实施例1中，分别采用本发明算法和A*算法进行路径规划。设定2D环境大小为100*100，起点为(0，0)，终点为(100，100);随机生成障碍物，障碍物比例分别设为0%，5%，10%, 20%, 30%, 40%O两种算法均在Matlab2012a上实现，实验用计算机CPU型号为Intel (R)Core (TM) 2Duo T6500，主频均为2.lGHz，RAM为1.86Gbyte。统计每种情况下用两种算法分别进行100次路径规划的平均路径长度、耗时、总节点数、扩展节点数和open中容纳的节点总数，结果如下表1所示。
[0037]表1
[
【权利要求】
1.一种Lambda*路径规划算法，其特征在于，包括以下步骤: (1)采用可视图构建路径规划的环境:采用障碍物包络体包络障碍物，由路径规划的起点Pstart^tS PgM1、以及障碍物的顶点，构成节点集合P; (2)创建两张表:open表和closed表,其中closed表中记录已扩展过的节点,open表仅保存当前扩展节点的后续节点，不保存已扩展节点的其他后续节点；但是，在当前扩展节点的后续节点中，有部分可能是已扩展节点的后续节点，曾经进入过open列表； (3)创建节点的数据结构:a(p)表示可视图中Wpstart到节点P的最短路径长度，& (P)表示从P到终点Pgral的最短路径长度；每个节点P数据结构包含父节点pre、节点的g值gval和f值fval, p.gval表示从起点Pstart到节点P的现有最短路径的长度，p.gval≤a (p);估价函数值为p.fval=p.gval+hval (p),启发函数hval (p)为p到终点Pgoal的最短距离的估计值，节点的估价函数值P.fval代表经过该节点P，从起点Pstart到终点PgMl的最短路径的一个估计值；若hval (P) < P (P)，即启发函数值小于节点P到终点Pgoal的实际最短路径长度，则可以找到最短路径； (4)搜索路径:扩展当前节点Pdtjsejd时先清空open表,仅将Pdtjsejd的不在closed中的后续节点加入open,丢掉当前扩展节点的父节点、兄弟节点；由于这些丢掉的节点可能与Pci。—可视，被重新加入到open中，由此在路径Pstart, P1, P2，…px, p上，p作为px的后续节点加入closed,但p与px的前辈可视； (5)加入Smooth过程对路径进行平滑处理:从前往后检查路径上的节点Pi(l≤i≤Z-1)与后面节点Pj(i+l〈j ( z)是否可视；若可视，则将它们之间的节点都删除，并调整平滑后各节点的pre, gval, fval。
2.根据权利要求1所述的一种Lambda*路径规划算法，其特征在于，节点之间可视是指，对于空间障碍物，节点之间的直线段不通过空间障碍物的内部，也不通过空间障碍物的公共面，但允许通过空间障碍物的公共线；对于平面障碍物，指节点之间的线段不通过平面障碍物的内部，也不通过平面障碍物的公共线，但允许通过平面障碍物的公共点，可视与否通过节点间的连线段与障碍物包络体各面的位置关系来判断，节点之间可视时lineofsight (p, p’)为真;siicc、p、(= P是与节点p可视的节点集合。
3.根据权利要求1或2所述的一种Lambda*路径规划算法，其特征在于，估价函数值为p.fval=p.gval+hval(p)。
4.根据权利要求3所述的一种Lambda*路径规划算法，其特征在于，选取节点p到终点Pgoal的欧氏距离作为启发函数hval (p)。
【文档编号】G05D1/02GK103529843SQ201310488139
【公开日】2014年1月22日 申请日期:2013年10月17日 优先权日:2013年10月17日
【发明者】黎萍, 周文辉, 朱军燕, 彭芳, 杨亮, 李云松, 付根平, 朱春媚, 刘金华 申请人:电子科技大学中山学院