基于方向和步长约束的安全A*算法

陈 娇，向建平，刘 卿

（西南交通大学 交通运输与物流学院，四川 成都 611756）

1 引言

目前，移动机器人已经广泛应用在制造业、医疗、物流、居家等多个领域，移动机器人的研究成为热点问题，而路径规划是移动机器人技术中的重点和难点问题。路径规划是指为移动机器人规划一条从起点到终点无障碍，并付出最小代价的最优或者次优路径。路径规划的核心问题是路径规划算法，目前路径规划算法主要分为传统路径规划算法和智能路径规划算法。传统路径规划算法主要有人工势场法[1]、A*[2]、快速扩展随机树[3]等，智能路径规划算法主要有遗传算法[4]、蚁群算法[5]、神经网络算法[6]等。在路径规划的算法中，A*算法因其简单、适用于大部分环境等优点，在移动机器人全局路径规划中得到广泛应用。但使用A*算法规划的路径也存在非最短路、平滑度低等缺点，针对A*算法的缺点，国内外学者从多方面进行了改进。

程传奇等提出了一种融合改进A*算法和动态窗口法的全局动态路径规划方法，针对传统A*算法，设计了一种关键点选取策略，再结合动态窗口法进行实时路径规划，使路径更加平滑[7]，但该算法未对路径长度实现优化，仍然存在冗余路段。Pei Cao等提出了Any-Angle A*算法，该算法基于可视图，在起点到目标点的连线附近进行搜索，减少了搜索节点，缩短了计算时间[8]，但它只适用于可视图，目前的适用性小，并且尚未考虑到机器人的体积进行防撞处理。文献[9]结合跳点搜索算法对A*算法进行改进，对寻路过程中的关键节点进行预处理，使用跳点代替搜索过程中大量的不必要节点，提高了路径规划的效率，但未解决传统A*算法规划的路径折点多、平滑度低等问题。张红梅等根据节点与障碍物的最小距离安全威胁代价，在估价函数中加入安全威胁代价，并对规划路径进行平滑优化处理，改进后的A*算法在简单环境中规划的路径更短，安全性和平滑程度提高[10]，但当路径规划的环境模型增大、复杂度加大时，改进算法出现规划的路径更长、转弯次数较多的问题。

上述改进算法对传统A*算法从不同的侧重进行了改进，但未能完全解决传统A*算法所规划路径折点多、路径冗余、路径平滑度低的问题，并且没有考虑到机器人和障碍物的实际大小，不适用于移动机器人在复杂场景中进行实际应用。针对上述问题，本文从搜索方向和安全性两个方面对传统的A*算法进行改进，利用改进A*算法规划的路径实现了长度、安全性、平滑度等多方面的综合优化，最后通过实验仿真证明了改进A*算法的可行性和有效性。

2 问题描述

2.1 环境建模

A*算法属于遍历性启发式算法，在进行路径规划之前，需要根据所得环境对路径搜索区域进行环境建模。考虑到移动机器人在行进过程中的安全性，本文将环境中简单障碍物膨胀成为边长为1的单位矩形，大型的复杂障碍物区域由多个规则的单位矩形构成。结合障碍物膨胀方法和栅格法，随机建立移动机器人路径规划区域的栅格地图，如图1所示。

图1中，栅格地图分为空闲区域和占据区域，白色栅格区域为空闲区域，移动机器人在行进过程中在保证不与障碍物发生碰撞的前提下，可以在空闲区域的任意移动位置和方向通行或停留；黑色区域为障碍物区域，移动机器人在行进过程中不能与障碍物发生碰撞或穿越障碍物，且最好与障碍物保持一定的安全距离。

图1 路径规划栅格地图

2.2 传统A＊搜索方向

传统A*算法主要有四个搜索方向和八个搜索方向，如图2所示，图中圆点表示移动机器人当前位置，实线箭头表示机器人在移动过程中的搜索方向。四方向A*算法的搜索方向为上、下、左、右四个，移动机器人的最小转角为，单次移动步长为1；八方向A*算法的搜索方向在上、下、左、右四个方向的基础上加入了左上、左下、右上、右下四个，移动机器人的最小转角为，单次移动步长为1或。

图2 传统A＊算法的搜索方向

在无障碍物的环境中，算法的搜索方向即为移动机器人的可移动方向；在有障碍物的环境中，移动机器人的搜索方向不变，移动方向为无障碍物的方向，如图3所示。

图3 传统A＊算法在障碍物环境中的可移动方向

观察图3可知，传统A*算法在障碍物环境中的可移动方向十分有限，在路径规划过程中有可能错过最佳路径的方向。利用传统八搜索方向A*算法在图1所示的栅格环境中进行路径规划的结果如图4所示。由图4可以看出，传统A*算法所规划路径存在多个与障碍物距离过近的危险节点，移动机器人运行过程中可能与环境中的物品发生碰撞，安全性低。由图4还可以看出，该路线曲率不连续，平滑度低，不符合移动机器人的运动规则。

图4 传统八方向A＊算法路径图

3 改进的A＊算法

3.1 搜索方向和步长限制

利用传统八搜索方向A*算法进行移动机器人的路径规划时存在许多不足，一方面移动机器人在路径规划的过程中搜索方向有限，使移动机器人可能错过最佳的行进方向，从而偏离最优路径；另一方面传统A*算法规划路径的最小转角为转弯半径较大，路径平滑度低，不利于移动机器人的实际运行。本文基于传统A*算法存在的不足，对传统A*算法的搜索方向和步长进行改进。

针对算法的搜索方向，在当前节点寻找下一拓展节点时，在传统A*算法原有的八个搜索方向的基础上，在两个相邻方向中间增加一个新的搜索方向，算法的最小搜索角度从缩小到，算法的搜索方向增加到16个。随着算法搜索方向的增加，算法的搜索节点、机器人可移动方向及机器人的单次移动步长也进行相应的改进。改进算法的搜索节点与算法的搜索方向同步增加为16个，各节点之间的间隔从1缩小到0.5；改进算法的机器人单次移动步长在传统A*算法1、 2的基础上，增加，即机器人单次移动步长可选范围变为以图1所示的栅格地图为例，将(-1,-1)与(1,1)之间的栅格区域局部放大，假设点(0,0)为当前节点，改进A*算法的搜索方向和搜索节点如图5所示。

图5 改进的A＊算法搜索方向和搜索节点

由图5可以看出，在搜索方向上，改进A*算法在传统A*算法的已有八个搜索方向（实线箭头表示）的基础上，新增了8个搜索方向（虚线箭头表示），搜索节点也相应增加至16个，新增的八个搜索方向上的单次移动步长为

3.2 路径安全性改进

在使用A*算法进行路径规划时大多把移动机器人看作是质点，未考虑机器人和障碍物的自身体积，导致移动机器人在行进中与障碍物的距离过近，容易与障碍物发生碰撞或擦划等安全性问题。针对传统A*算法所规划路径存在的上述问题，本文提出一种路径安全性检测算法对传统A*算法进行安全性改进，使移动机器人在路径规划过程中，能够寻找路径最短但不与障碍物发生碰撞的安全路径。

首先结合实际，针对圆形、矩形、其他多边形等外形较规则移动机器人，获取移动机器人前、后、左、右共四个方向最外径到重心的距离l{li,lb,ll,lr}，以图6（a）、图6（b）所示的常见矩形轮式和圆形轮式机器人为例，移动机器人最外径到重心的距离求解示意图如图6（c）、图6（d）所示。

图6 移动机器人俯视图和外径到重心距离

获取移动机器人的距离属性l{li,lb,ll,lr}后，再结合改进A*算法进行路径规划，对传统A*算法安全性改进的具体步骤如下：

（1）获取移动机器人前、后、左、右共四个方向最外围到重心的距离l{li,lb,ll,lr}；

（2）计算待选子节点与周围障碍物之间的最短距离，计算公式如下：

式中：xi(xi,yi)为待选节点，oj(xoj,yoj)为xi周围的障碍物；d(xi)为xi与oj之间的距离；X为包含所有xi的集合；O为包含所有oj的集合。

假设现有路径穿过图3所示的障碍物环境，待选子节点xi与周围三个障碍物o1、o2、o3之间的直线距离示意图如图7（a）所示。

（3）计算待选子节点与当前节点的中点与周围障碍物之间的最短距离，计算公式如下：

式中：p(xp,yp)为当前节点，xi(xi,yi)为待选节点，m是 p和xi的中点，oj(xoj,yoj)为xi周围的障碍物；l(xi)为m与oj之间的距离；X为包含所有xi的集合；O为包含所有oj的集合。

假设现有路径穿过图3所示的障碍物环境，待选子节点xi与当前节点p的中点m与周围三个障碍物o1、o2、o3之间的直线距离示意图如图7（b）所示。

（4）计算 d(xi)、l(xi)中的最短距离 dmin，比较 dmin与l{li,lb,ll,lr}。 l≤dmin时，执行操作（5）；dmin

式中：f(xi)为xi的估价函数；g(xi)为xi到起点S的实际代价；h(xi)为xi到目标点d的估计代价；ds为移动机器人的安全半径。

图7 路径上节点与障碍物直线距离示意图

（5）判断是否已经计算所有待选子节点的估价函数。如果已经计算完成，执行操作（6）；如果还未计算完成，返回执行操作（2）。

（6）选择估价函数最小的待选子节点为拓展节点。

利用改进A*算法在图1所示的栅格环境中进行路径规划的结果如图8所示，由图8可以看出，利用改进A*算法规划的路径所有节点与障碍物之间都保留了一定的安全距离，无危险节点，说明移动机器人在实际的运行过程中不会与环境中的物品发生碰撞。由图8还可以看出，相比传统A*算法规划的路径，该路线转弯的角度更小，路径也更为平滑，更符合移动机器人的运动特性。

图8 改进A＊算法路径图

4 仿真对比实验

为了验证改进A*算法的有效性，采用改进算法与传统八方向A*算法在不同规模地图和不同复杂程度地图中进行了多组仿真对比研究，实验环境为：64位WIN10操作系统，运行内存为8GB，编译环境为：Python3.7。

采用本文改进的A*算法与传统的A*算法分别在规模为30*30、50*50、100*100且存在多种不规则形状障碍物、圆形障碍物以及混合障碍物的环境地图中进行移动机器人路径规划仿真，仿真结果如图9—图11所示。

图9 30＊30不规则障碍物环境中路径规划仿真对比

图10 50＊50圆形障碍物环境中路径规划仿真对比

对比图9、图10、图11中传统A*算法规划的路径图与改进A*算法规划的路径图可以发现，传统A*算法规划的路径在进行仿真的三个地图中都存在与障碍物接触的危险节点；改进A*算法所规划路径与环境中的障碍物都保持着一定的安全距离，不存在与障碍物接触的危险节点。传统A*算法规划的路径转弯角度较大，路径平滑度较低；改进A*算法规划的路径较传统A*算法规划的路径转弯角度更小，路径更为平滑。对传统A*算法和改进A*算法在规模为30*30、50*50、100*100的环境地图中仿真的路径长度、危险节点数、最大转弯角度、累积转折角度四个方面进行数据对比，结果见表1。

图11 100＊100混合障碍物环境中路径规划仿真对比

表1 传统A*算法与改进A*算法的仿真结果数据对比

对比表1中的实验数据可以发现，在不同规模和不同障碍物的仿真环境中，相比传统A*算法，改进A*算法实现了多方面的优化。在路径长度方面，改进A*算法规划的路径长度均小于传统A*算法规划的路径长度，且地图规模越大、环境越复杂时，改进A*算法的路径长度优势越明显。在危险节点数方面，传统A*算法规划路径的危险节点随着地图规模的增加而增加，但是改进A*算法在任意规模的地图中规划的路径都不存在危险节点。在最大转弯角度与累积转弯角度方面，传统A*算法规划的路径最大转弯角度是改进A*算法的两倍，且传统A*算法所规划路径的累积转弯角度始终大于改进A*算法的累积转弯角度。综合以上实验结果说明，改进A*算法较传统A*算法在路径长度、路径安全性、路径平滑度三方面都实现了一定程度的优化和改进。

5 结论

本文针对传统A*算法规划的安全系数低、平滑度低等问题，提出了一种基于搜索方向与安全性改进的A*算法。通过仿真实验研究，对比了传统A*算法与其他文献中的路径规划算法，验证了本文改进A*算法实现了路径长度、安全性以及平滑性的优化。未来将研究在多种实际应用场景中多移动机器人的协调与路径规划算法，进一步推动移动机器人的算法研究和实际应用。