基于可视图与A*算法的路径规划

黎 萍，朱军燕，彭 芳，杨 亮



基于可视图与A*算法的路径规划

黎 萍1，朱军燕2，彭 芳1，杨 亮1

(1. 电子科技大学中山学院，广东 中山 528402；2. 中山出入境检验检疫局技术中心，广东 中山 528403)

结合可视图的骨架构造方法和A*图搜索方法，采用矩形包络障碍物，在障碍物顶点外延生成路径点。在此基础上，提出一种新的路径规划算法Lambda*，与A*算法类似，搜索过程需要2张表，但CLOSED表保存从起始节点开始的路径节点，OPEN表保存CLOSED表中扩展节点的后续节点，可减少在OPEN表中保存的节点数量，减少计算量和耗时，并通过增加SMOOTH过程以提高路径的平滑度。将算法应用于二维空间环境进行机器人路径规划仿真实验，结果表明，与A*算法相比，Lambda*算法能够以增加较少路径长度为前提，大幅降低路径规划的耗时。

路径规划；可视图；A*算法；Lambda*算法；路径平滑

1 概述

路径规划作为很多领域的关键技术已经成为当前的研究热点之一[1]，它是一个带约束的复杂优化问题，其任务是在具有障碍物的环境内按照一定的评价标准寻找一条从起始状态到达目标状态的无碰路径。

基于自由空间构造的规划方法是一类重要的机器人路径规划方法，它们的工作原理基本上都是先通过几何方法构造自由空间的骨架图，然后利用图搜索算法搜索一条可行的最短路径[2]。常用的适合低维状态空间的骨架图有可视图[3]、Voronoi图[4]、栅格图分解[5]以及切线图[6]等，这些骨架图不适用于高姿态空间，但具有较好的完备性[1]。常用的图搜索方法包括深度优先和广度优先、Dijkstra算法[7]、A*算法[7]和D*算法[8]。

经典启发式搜索式算法——A*算法是20世纪60年代提出的，在相关领域得到了广泛的应用。但该算法只能用在格子环境中，在一定程度上限制了其进一步的推广，算法复杂度和发现路径的能力受栅格细化程度的影响，且环境信息存储量大。文献[9]提出了一种A*算法的变种—— Theta*算法，突破了格子的限制，允许以任意角度改变路径的方向，但依然采用栅格地图描述环境。

本文结合可视图的骨架构造方法和A*图搜索方法，采用矩形包络障碍物的碰撞检测方法，为简化路径优化，在障碍物顶点外延生成路径点。在此基础上，提出一种新的路径规划算法Lambda*，并将算法应用于二维环境空间进行机器人路径优化。

2 算法描述

2.1 碰撞检测

为了解决机器人与障碍物之间的碰撞检测问题，本文采用一种矩形包络碰撞检测方法。障碍物一般具有不规则几何形状[10]，采用矩形对二维空间的障碍物包络进行简化，如图1所示。这种建模方法虽然在一定程度上扩大了障碍域，但使得障碍域大大简化，提高了规划的效率，使得避碰路径更具安全性[11]。

图1 障碍物的矩形包络几何模型

在简化障碍物的基础上，障碍物与机器人之间的碰撞检测就转化为机器人路径(表现为线)与障碍物(表现为矩形)的位置关系问题，当线与矩形的任意一条边线段相交则发生碰撞，反之不发生碰撞。在检测碰撞时，仅需分别计算路径点之间连线与障碍物矩形4条边之间的位置关系，就可以判断机器人是否与障碍物发生碰撞。

2.2 路径点的生成

可视图法视移动机器人为一点，将机器人、目标点和障碍物的各顶点进行组合连接，并保证这些直线均不与障碍物相交，这就形成了一张图，称为可视图[1-3]。由于任意两直线的顶点都是可见的，从起点沿着这些直线到达目标点的所有路径均是运动物体的无碰路径。搜索最优路径的问题就转化为从起点经过这些可视直线到目标点的最短距离问题。

在可视图中，机器人以所有障碍物的顶点为中间路径点。本文采用矩形代替任意形状的多边形包络障碍物，为保证路径的安全性，在矩形包络的对角线上外延一定的距离产生路径点，如图2所示，若将外延距离设为0，则退化为一般可视图。

图2 路径点

2.3 Lambda*算法

Lambda*算法与A*算法类似，在搜索过程中需设置 2张表：OPEN表和CLOSED表。在A*算法中，OPEN表保存所有已生成而未考察的节点，CLOSED表中记录已访问过的节点；而在Lambda*算法中，CLOSED表保存获取的从起始节点开始的路径节点，OPEN表保存CLOSED表中扩展节点的后续节点。假设是起点、终点和所有插入的路径点的集合，从几何路径点构造双向链表的节点单元，节点的数据结构为：

classdef Node

point //几何路径点

value //节点的值

Prev //该节点的前驱节点

end

end

其中，表示从起点到节点的路径长度；start表示起始节点；goal表示终点；所有节点创建时仅有point特性，其value特性值设为0。Lambda*算法的估价函数为：(P)=(P)+(P)，与A*算法估价函数的选取一致，(P)表示从起始节点start到节点P的路径长度；(P)为启发函数，Lambda*算法的(P)采用从节点P到终点goal之间的直线距离，节点P距离goal越近，(P)越小，以此引导搜索的方向，使得机器人逐步靠近目标。Lambda*算法如图3所示。

图3 Lambda*算法流程

首先创建2张空的双向链表OPEN和CLOSED，将起始节点start插入CLOSED链表，若终点不在CLOSED链表中，则清空OPEN链表，从CLOSED中选取节点进行扩展，将扩展节点的后续节点加入OPEN链表。计算OPEN表中所有节点的估价值，搜索具有最小估价值的节点，将其插入CLOSED链表，并修改插入节点的value值；若终点在OPEN表中，则直接将其插入CLOSED，省去计算估价值和比较估价值的步骤，节省路径规划时间。扩展OPEN链表时，遍历中几何点构造的所有节点，若节点P不在CLOSED表中且与CLOSED表的扩展节点close可视(之间没有障碍物)，则将其作为一个后续节点。

Lambda*算法与A*算法的主要区别是在Lambda*算法中，OPEN表仅保存当前扩展节点的后续节点，不保存已扩展节点的其他后续节点，这大大减少了OPEN表保存的节点数量，减少了计算量和时耗。

CLOSED中保存的是一条从起点到终点的无碰路径，受启发函数的限制，获取的路径不保证为最短路径，如图4所示。对图4(a)采用SMOOTH过程后，能进一步优化路 径，获得如图4(b)所示的最优路径，增加路径的光滑度，从而提高达到目标点的效率。因此，在获得这样一条路径后，Lambda*算法通过SMOOTH过程对CLOSED链表表示的路径进行平滑[12]，SMOOTH过程的流程如图5所示。

图4 路径平滑前后效果对比

图5 SMOOTH过程流程

3 仿真实验及结果分析

为了测试Lambda*的性能，将Lambda*和A*算法[13]应用于有障碍物的2D环境中进行路径规划实验，在100×100的二维环境下，起点为(0,0)，终点为(100,100)，随机生成障碍物，障碍物比例分别设为0%、5%、10%、20%、30%、40%，2种算法均在Matlab 2012a上实现，实验用计算机CPU型号为Intel(R)Core(TM)2 Duo T6500，主频均为2.1 GHz，RAM为1.86 GB。本文并未对算法的实现进行优化，因此，实验结果可能还能进一步改进。

统计每种情况下用2种算法分别进行100次路径规划的平均路径长度、耗时、总节点数、扩展节点数和OPEN中容纳的节点总数，结果如表1所示。随机障碍物比例越大，总节点数增多，路径规划也越来越复杂，所得路径长度和耗时、扩展节点数以及OPEN表中的节点数都呈增加趋势，若改变环境大小，则相同比例下障碍物数量不同，总节点数也随之改变，本文未对2D环境不同环境大小时的路径规划进行实验，但随着节点数增多，路径规划越复杂。在有障碍物的2D环境下，障碍物比例为0%~5%的情况下，路径规划较简单，总节点比较少，A*和Lambda*所获得路径质量相当，90%以上的情况2种算法所获路径相同。随着障碍物比例增大，Lambda*算法所获路径质量略低于A*算法，同时Lambda*算法耗时大幅减少。

表1 2D有障碍物环境下的实验结果

此外，还统计了采用2种方法所获路径的方向变化次数，采用Lambda*算法获得的路径方向平均变化2次，而A*算法获得的路径方向平均变化3.143次。实验结果表明，障碍物越多，路径越长，算法耗时越多。在相同情况下，Lambda*需扩展的节点数和OPEN表中节点总数都显著小于A*的对应值，路径长度略有增长，所获路径质量稍逊于A*算法所得但耗时大幅减少。

4 结束语

本文结合可视图和A*算法，提出了一种新的路径规划算法Lambda*，采用矩形包络二维空间的障碍物，为保证路径的安全性，在障碍物顶点外延生成路径点。Lambda*算法采用链表的数据结构来描述路径，在路径搜索时，CLOSED表保存获取的从起始节点开始的路径节点，OPEN表保存CLOSED表中扩展节点的后续节点，对获取的路径进行平滑处理，并进一步优化，提高路径的平滑度。

仿真实验表明，Lambda*算法能够在增加较少路径长度的前提下，大大降低路径规划的耗时。然而验证实验在二维空间进行，下一步研究需要将算法拓展到三维空间，且进一步探究如何能快速地获得高质量的路径。

[1] 朱大奇, 颜明重. 移动机器人路径规划技术综述[J]. 控制与决策, 2010, 25(7): 961-967.

[2] 成伟明, 唐振民, 赵春霞, 等. 移动机器人路径规划中的图方法应用综述[J]. 工程图学学报, 2008, 29(4): 12-20.

[3] Oommen B, Iyengar S, Rao N S V, et al. Robot Navigation in Unknown Terrains Using Learned Visibility Graphs[J]. IEEE Journal of Robotics and Automation, 1987, 3(6): 672-681.

[4] Choset H, Burdick J. Sensor Based Planning, Part I: The Generalized Voronoi Graph[C]//Proc. of IEEE International Conference on Robotics and Automation. [S. l.]: IEEE Press, 1995: 1643-1648.

[5] Parsons D, Canny J F. A Motion Planner for Multiple Mobile Robots[C]//Proc. of IEEE International Conference on Robotics and Automation. [S. l.]: IEEE Press, 1990: 8-13.

[6] Liu Yunhui, Arimoto S. Computation of the Tangent Graph of Polygonal Obstacles by Moving-line Processing[J]. IEEE Transactions on Robotics and Automation, 1994, 10(6): 823-830.

[7] Papadatos A. Research on Motion Planning of Autonomous Mobile Robot[D]. Monterey, USA: Naval Postgraduate School, 1996.

[8] Stentz A. The Focussed D* Algorithm for Real-time Re- planning[C]//Proc. of the 14th International Joint Conference on Artificial Intelligence. San Francisco, USA: ACM Press, 1995: 1652-1659.

[9] Daniel K, Nash A, Koenig S, et al. Theta*: Any-angle Path Planning on Grids[J]. Journal of Artificial Intelligence Research, 2010, 39(1): 533-579.

[10] 汪首坤, 邸 智, 王军政, 等. 基于A*改进算法的机械臂避障路径规划[J]. 北京理工大学学报, 2011, 31(11): 1302- 1306.

[11] 贾庆轩, 陈 钢, 孙汉旭, 等. 基于A*算法的空间机械臂避障路径规划[J]. 机械工程学报, 2010, 46(13): 109-115.

[12] Botea A, Müller M, Schaeffer J. Near Optimal Hierarchical Path-finding[J]. Journal of Game Development, 2004, 1(1): 7-28.

[13]Lozano P T, Wesley M A. An Algorithm for Planning Collision-free Paths Among Polyhedral Obstacles[J]. Communications of the ACM, 1979, 22(10): 560-570.

编辑 顾逸斐

Path Planning Based on Visibility Graph and A*Algorithm

LI Ping1, ZHU Jun-yan2, PENG Fang1, YANG Liang1

(1. Zhongshan Institute, University of Electronic Science and Technology of China, Zhongshan 528402, China; 2. Technical Center, Zhongshan Entry-Exit Inspection and Quarantine, Zhongshan 528403, China)

Inspired by skeleton construction method visibility graph and graph search methods A*, obstacles are enveloped by rectangles, path points are generated as obstacle vertices’ extension, and a new path planning algorithm Lambda* is presented, which needs two lists. But differently, CLOSED list in Lambda* is used to store the path nodes from starting node sequentially, and OPEN list is used to save subsequence nodes for the extending node in CLOSED list. What’s more, SMOOTH process is added to smooth the path saved in CLOSED. For validation verification, Lambda* is used in 2D simulation environment. The simulation results show that Lambda* algorithm’s time consuming is decreased substantially with little increase of path length compared with A* algorithm.

path planning; visibility graph; A* algorithm; Lambda* algorithm; path smoothing

1000-3428(2014)03-0193-03

A

TP24

国家科技型中小企业技术创新基金资助项目(12C26214405188)；广东省教育部产学研基金资助项目(2011B090400371)； 电子科技大学中山学院博士启动基金资助项目(410YKQ01)。

黎 萍(1981－)，女，讲师、博士，主研方向：人工智能；朱军燕，工程师、硕士；彭 芳，副教授、硕士；杨 亮， 讲师、硕士。

2013-08-29

2013-10-29 E-mail：lipingjxau@163.com

10.3969/j.issn.1000-3428.2014.03.040