基于分层栅格地图的移动机器人路径规划

于晓天高秀花张 俊郑冰环费雯凯(北京航天微系统研究所北京100094)

基于分层栅格地图的移动机器人路径规划

于晓天，高秀花，张 俊，郑冰环，费雯凯
(北京航天微系统研究所，北京100094)

针对传统A⁃Star算法与模糊控制算法单独应用于移动机器人路径规划时各自的局限性，提出一种基于分层栅格地图并将两种算法融合的移动机器人路径规划新方法。融合后的新算法先利用A⁃Star算法在高层栅格地图中整体规划出一条概括性路径，再利用模糊控制算法以概括性路径中的点为导航点，在底层栅格地图中进行局部规划，从而得出最终的路径。仿真结果表明，与传统的A⁃Star算法与模糊控制算法相比较，新算法所规划路径距离较短且平滑可行，具有较高的品质。

路径规划；A⁃Star算法；模糊控制算法；分层栅格地图；融合算法

0 引言

近年来，移动机器人已经被应用到越来越多的领域当中，各式各样的移动机器人呈井喷状发展。尽管移动机器人的种类、数目繁多，但其在执行任务时都需要解决一个问题：如何在有障碍物的环境中从给定起始点无碰地到达目标点。因而，路径规划一直是移动机器人研究方向上的一个热点与难点。

从20世纪70年代开始，大量的学者投入到移动机器人路径规划的研究中，并且提出了很多优秀的路径规划算法［1⁃5］，如Dijkstra算法、遗传算法、A⁃Star算法、人工势场法、蚁群算法、模糊控制算法等。然而，单一地使用一种路径规划算法进行路径规划往往有较大的局限性。因此，本文针对传统的A⁃Star算法规划路径距离最短(前提为路径存在)、但路径折线多且边沿可靠性差，而模糊控制算法规划路径相对较平滑、但路径距离较长且边沿可靠性不足的特点，基于分层栅格地图，将两种算法进行融合、优势互补，弥补了单一算法的一些缺点与不足，获得了更为理想的路径规划效果。

1 分层栅格地图的构建

本文所提的融合算法是在分层栅格地图的基础上进行路径规划的，因而构建分层栅格地图是整个路径规划的首要工作。设定全局地图为一正方形的迷宫地图，根据融合算法的需要，全局地图可分为高层栅格地图和底层栅格地图两层。

首先，将正方形迷宫地图均匀划分为40行和40列，总共1600个栅格。每个栅格用深黑色或者白色标记，其中深黑色栅格表示有障碍物，白色栅格表示无障碍物，这样便构成了底层栅格地图，如图1所示。然后，在底层栅格地图的基础上，将N×N个底层栅格合并组成一个高层栅格，从而完成高层栅格地图的构建。在底层栅格个数一定的情况下，增大N值，则会降低高层栅格的个数。本文中N值取4，所构建的高层栅格地图如图2所示。

2 基于高层栅格地图的A-Star算法

2.1 A⁃Star算法基本原理

A⁃Star算法是一种启发式搜索算法(Heuristic Search Algorithm)，通过不断搜索逼近目的地的路径来获得移动机器人的移动路径。而在启发式搜索过程中，最重要的是启发式函数，其表达式为：

其中，f(x)为从初始节点到目标节点的最小消耗，g(x)为初始节点到节点x的最优路径代价，h(x)为节点x到目标节点的估计消耗。在进行路径规划时，从起始点开始，在栅格地图范围内，算法分别对当前点左上、上、右上、右、右下、下、左下、左8个方向上的非障碍物栅格进行扩展，每次选取拓展点中f(x)值最小的点为下一路径点，依次循环，直到找到目标点。

2.2 改进的A⁃Star算法

传统A⁃Star算法在范围较小的环境地图中，可以快捷、高效地找出一条最短路径。但随着环境范围扩大，栅格数量增多，将会导致计算量迅速上升。考虑到算法本身的效率以及下一步算法融合的要求，结合高层栅格地图的特点，本文在传统A⁃Star算法的基础上，对原有算法做了相应的改进［6］，下文将详细介绍。

1)算法在搜索路径过程中，加入了每个高层栅格被障碍物占有的概率，并将其定义为栅格内障碍物的面积与栅格面积的比值，用Pi表示。栅格内无障碍物时Pi＝0，全部被障碍物填充时Pi＝1，其他情况Pi取值0～1之间。同时，设定一个阈值Pt，高于这个阈值的栅格视为被障碍物占据，算法将其自动剔除，而低于阈值的栅格保留下来用于路径的拓展。本文中，Pt设定为0.7。另外，对启发式函数中各个参数的计算方法也做出了相应改进，具体如下：

在式(2)、式(3)中，g(xpar)表示从初始节点移动到当前节点x的父节点xpar时所经过的实际距离；g(x)中的dn(xpar，x)表示当前节点x与其父节点xpar之间的欧几里得距离；h(x)中的dn(x，xgoal)表示当前节点x与目标节点xgoal之间的曼哈顿距离；而cm(x)表示在高层栅格地图中，x节点被障碍物占据的综合评判概率系数，其表达式如下：

由式(4)、式(5)可以看出，cm(x)由两部分组成。一部分为x节点本身被障碍物占据的概率pi(x)，另一部分为x点周围r个节点被障碍物占据的算术平均值cp(x)。其中，ω1、ω2为权值系数，本文将其设定为0.5。

最终，改进后的A⁃Star算法可利用式(1)～式(5)完成对高层栅格地图中每个扩展节点的f(x)值的计算，然后按照A⁃Star算法本身的搜索规则，在高层栅格地图中规划出一条概括性的路径，具体算法流程［7］如图3所示。

3 基于底层栅格地图的模糊控制算法

3.1 算法基本原理

模糊控制算法是一种模拟人的大脑对一些模糊事物进行识别和判决的人工智能算法。在路径规划中，模糊逻辑法模拟驾驶员的驾驶思想，将模糊控制本身具有的鲁棒性与基于生理学的感知⁃动作行为结合起来，显示出了很大的优越性［8］。

本文设定3个互成锐角的虚拟测量传感器作为测距系统，用于测量3个方向上各自与机器人距离最近的障碍物的距离，并根据实际应用情况限定了最大测量距离。如图4所示。记机器人与目标点之间的连线为Ln，图4中D为位于Ln方向上的障碍物距离机器人的最短距离，DL、DR分别为与Ln呈±α夹角的两方向上障碍物距离机器人的最短距离。在底层栅格地图中，测量系统探测的障碍物即地图中的黑色小栅格。算法开始运行时，机器人从起点出发，确定所要到达的目标点之后，首先通过3个测量传感器获取D、DL、DR的值，记R＝DL－DR，然后以D和R为两个输入量输入到模糊控制器中，再经过模糊化、模糊规则解算、去模糊等一系列处理，最后输出机器人行走步长的比例因子Sd以及转动角度的比例因子Sθ(逆时针为正方向)，进而更新机器人的位置和姿态，依次循环，直至到达目标点。本文中，α取30°且左偏为正。

由于模糊逻辑存在着对称无法确定现象，因此当机器人面临当前环境左右对称时，无法确定行进方向或在两个障碍物之间震荡，陷入死锁。本文采用预防死锁机制［9］进行处理。从起始点开始，每次机器人到达新位置之后，先往目标方向进行探测，如果在此方向小范围内行动受阻，则认为机器人进入危险区域，启动解锁算法。之后，机器人立于原地分别转向两边探测出口，然后选择转角比较小的一侧，按照左正右负的规则标记机器人进入解锁状态。当处于解锁状态下，机器人到达新位置时仍然受阻，则加深此种状态，否则减轻，直至解除危险标示。机器人解锁时具体的探测过程如图5所示。

图5中，设定机器人位于A点，AB1为机器人按输出转角转动后的前进方向。经探测，AB1方向上障碍物距离小于固定步长，则机器人向左(假定)偏转角度δ，方向为AB2，继续探测，AB2方向上障碍物距离仍小于固定步长，于是再向左偏转角度δ探测，依次下去，直到找到安全的前进方向ABi，然后机器人前进固定步长到C点，完成本次解锁，本文中δ取15°。

3.2 模糊控制器设计

根据3.1节中算法的具体要求，设置D、R两个变量为模糊控制器的输入，两个比例因子Sd、Sθ为输出。将D均匀量化到 ［0，10］区间内，论域为 ［1，3，5，7，9］。将左右探测器的距离差值R均匀量化到 ［－10，10］区间内，R的论域为［－10，－7，－4，－1，0，1，4，7，10］。将输出转角比例因子Sθ均匀量化到 ［－2，2］区间内，论域为 ［－1.5，－0.75，0，0.75，1.5］。将输出步长比例因子Sd均匀量化到 ［0，2］区间内，论域为［0，0.3，0.6，1，1.4，1.5，1.8］。定义各变量的模糊语言为D＝｛VS，S，M，B，VB｝，R＝｛RVB，RB，CE，LB，LVB｝，Sθ＝ ｛TRVB，TRN，Z，TLB，TLVB｝，Sd＝ ｛VS，S，M，B，VB｝，各字母含义为(V：Very，S：Small，M：Middle，B：Big，R：Right，Z：Zero，L：Left，T：Turn)。当位于机器人前进方向左(右)侧的障碍物离机器人近些时，R为正(负)，机器人向左(右)转为正(负)。取各个语言变量的隶属度函数形状为对称的三角形且模糊分割完全是对称的，D、R、Sθ及Sd的隶属度函数形如图6所示。

与此同时，根据人类驾驶汽车的相关经验设计了相应的模糊规则库，以供推理使用。设定模糊控制器的推理方法采用Mamdani推理法，清晰化方法采用加权平均法，至此，模糊控制器设计完成。最终，每当有一组D与R的值输入，就可以通过模糊控制器处理并求得一组清晰值Sd、Sθ，再经尺度变换即可输出实际的控制量。

4 算法融合

基于分层栅格地图进行路径规划时，整体思想为：运用改进的A⁃Star算法在高层栅格地图中规划出一条概括性路径，然后运用模糊控制算法，以概括性路径中的节点为局部导航目标点，从起始点开始，在相邻两个导航目标点之间的地图中进行局部路径规划，逐次到达每个导航目标点，最终形成一条从起始点到最终目标点，平滑、可行且距离较短的路径。

然而，由于在高层栅格地图中规划的概括性路径是一种基于概率性质的规划，而且改进的A⁃Star算法是沿着栅格边缘进行路径规划的，因此概括性路径给出的路径点有可能是某个障碍物栅格的顶点，如图7所示。A点为概括性路径中的一个节点，B点为概括性路径中与A点相邻的下一个节点。此时若在底层栅格地图中调用模糊控制算法到达A点后，B点即为下一个导航目标点，但从图中显然可以看出，到达A点后，机器人已经进入了死区，这将导致路径规划失败。为了避免此种情况的发生，在基于底层栅格地图的路径规划中，对原有模糊控制算法做出以下3点改进：

1)基于高层栅格地图进行路径规划得出概括性路径后，在底层栅格地图中对概括性路径中的节点进行筛查，对包含在障碍物中的节点进行标记，即置Flag＝1。

2)每次给出机器人下一步的转角θ及步长d后，机器人并不立即前进，而是按照给出的转角θ先进行旋转，然后在新的方向上测量与机器人距离最短的障碍物的距离dm。若dm＞d，则机器人按照步长d正常前进，否则，仿照算法预防死锁机制中的探测过程，按照一定的方向(如向右)连续偏转角度δ，直到找到安全的前进方向，继续行进，实现二次转动避障。

3)记机器人与局部目标点之间的距离为drg。当机器人到达一个新的位置后，若drg＜d且Flag＝1，则按照步骤2中的二次转动避障方法寻找出安全前进方向并前进步长d。此时，认定机器人已经到达该局部目标点，然后获取下一局部目标点进行下一轮局部路径规划。

基于双层栅格地图的融合算法整体流程如图8所示。

5 算法仿真

在Matlab r2014a的开发环境中，利用前文构建的40×40的正方形迷宫地图，分别对A⁃Star算法、模糊控制算法和本文所提出的融合算法进行仿真。仿真过程中，所有地图的左上角为机器人的起始点，右下角为机器人的目标点，算法中的其他参数如前文中所述，仿真结果如图9所示。

仿真结果表明，融合算法在高层栅格地图中进行路径规划时，栅格的增大以及基于栅格障碍物占有率这一参数对A⁃Star算法启发式函数的改进，使得所规划路径较传统A⁃Star算法在距离上有所增大，但这样却避开了障碍物密集的区域，避免碰到局部 “陷阱”的情况，优化了路径规划的时间。而且从所规划路径的长度、平滑性和边缘可靠性3方面进行综合评判，融合算法明显优于单独的A⁃Star算法和模糊控制算法。

6 结论

针对传统A⁃Star算法与模糊控制算法各自的特点，提出了一种基于分层栅格地图并将两种算法融合的移动机器人路径规划新方法。传统A⁃Star算法规划路径距离最短(前提为路径存在)，但路径都为折线且常沿障碍物边缘前进，不一定可行；而模糊控制算法规划路径相对较平滑，但路径距离较长，且边沿可靠性不足。融合后的新算法利用A⁃Star算法在高层栅格地图中整体规划出一条概括性路径，再利用模糊控制算法以概括性路径中的点为导航点，在底层栅格地图中进行局部规划，从而得出最终的路径。仿真结果表明，与传统的A⁃Star算法与模糊控制算法相比较，基于分层栅格地图，融合后的新算法规划路径距离较短且平滑可行，具有更高的品质。

[1] Dijkstra E W.A note on two problems in connection with graphs[J].Numerische Mathematik，1959，1(1):269⁃271. [2] Agirrebeitia J，Aviles R.A new APF strategy for path planning in environments with obstacles[J].Mechanism and machine theory，2005，40(6):645⁃658.

[3] Mohanta J C，Parhi D R，Patel S K.Path planning strategy for autonomous mobile robot navigation using Petri⁃GA optimization[J].Computers and Electrical Engi⁃neering，2011，37(6):1058⁃1070.

[4] 曾明如，徐小勇，刘亮，等.改进的势场蚁群算法的移动机器人路径规划[J].计算机应用工程，2015，51 (22):33⁃37. ZENG Ming⁃ru，XU Xiao⁃yong，LIU Liang，etal. Improved ant colony optimization with potential field heu⁃ristic for robot path planning[J].Computer Engineering and Applications，2015，51(22):33⁃37.

[5] Perez D A.Fuzzy logic based speed planning for autono⁃mous navigation under velocity field control[C].IEEE In⁃tetnational Conference on Mechatronics，2009:14⁃17.

[6] 秦玉鑫，王红旗，杜翠杰.基于双层A∗算法的移动机器人路径规划[J].制造业自动化，2014，36(12): 21⁃25. QIN Yu⁃xin，WANG Hong⁃qi，DU Cui⁃jie.A path planning approach to moving robot based on double layer A∗algorithm[J].Manufacturing Automation，2014，36 (12):21⁃25.

[7] 时也.基于A⁃Star算法与模糊控制融合的移动机器人路径规划[D].武汉科技大学，2012. SHI Ye.Mobile robot path planning based on fusion of A⁃Star algorithm and fuzzy control[D].Wuhan University of Science and Technology，2012.

[8] Xu W L，Tso S K.Sensor⁃based fuzzy reactive navigation of a mobile robot through local target swithching[J].IEEE Transactions on Systems，Man and Cybemetics，1999，29 (3):451⁃459.

[9] 李鹏，温素芳.基于模糊控制的路径规划算法的实现[J].杭州电子科技大学学报，2007，27(6):82⁃86. LI Peng，WEN Su⁃fang.Realization of the path planning algorithm based on fuzzy control[J].Journal of Hangzhou Dianzi Univercity，2007，27(6):82⁃86.

Path Planning of Mobile Robot Based on Hierarchical Grid Map

YU Xiao⁃tian，GAO Xiu⁃hua，ZHANG Jun，ZHENG Bing⁃huan，FEI Wen⁃kai
(Beijing Aerospace Institute of Microsystems，Beijing 100094)

According to the limitation of the traditional A⁃Star algorithm and the fuzzy control algorithm applied to the path planning of mobile robot，this paper presents a new method based on the hierarchical grid map and the fusion of these two kinds of algorithm.The fusion algorithm plans a general path in the top grid map by using the A⁃Star algorithm.Then based on the navigation point in the general path，the fusion algorithm concludes the final path in the bottom gird map by u⁃sing the fuzzy control algorithm.The simulation results show the path planned by fusion algorithm has more excellent char⁃acter than the path planned by the other two algorithms.

path planning；A⁃Star algorithm；fuzzy control algorithm；hierarchical map；fusion algorithm

TP24

A

1674⁃5558(2017)01⁃01291

10.3969／j.issn.1674⁃5558.2017.02.006

于晓天，男，硕士，研究方向为机电控制。

2016⁃07⁃07