改进的A*算法移动机器人路径规划*

柴红杰李建军姚 明

(江苏大学汽车与交通工程学院，江苏镇江 212001)

当今社会移动机器人在各行各业都扮演着重要角色，但是路径规划仍然是移动机器人应用中面临的一个重要难题。它的目的是在有障碍物的环境中为移动机器人从起始位置到目标位置规划出一条最优或次优安全无碰撞可行路径并且得到的路径要满足一定的约束条件[1-2]。机器人路径规划的常用方法主要有蚁群算法[3]，粒子群优化算法[4]，栅格法[5]等。其中，由于栅格法具有结构简单，易于实现，对传感器容错性强等优点，被广泛应用于机器人路径规划中。基于栅格地图的A*算法适用于环境信息已知的一类路径规划方法。已有多种改进的A*算法被提出，顾辰[6]在对机器人进行路径规划过程中，把扩展结点进行优先分级，避免穿过障碍物顶点，与障碍物有一定的安全距离，但路径依然存在较多转折点和安全问题；辛煜等[7]通过A*算法搜索离散的8 个邻域扩展到无限个，增加了搜索方向，提高了路径平滑性的性能指标，但计算量增大，致使搜索效率明显降低；陈诺男[8]等提出更改障碍搜索矩阵的尺寸来获得不同的安全间距，以保证不同机器人在不同地图环境下的安全性，但未考虑机器人本体尺寸不利于实际环境规划；李冲等[9]提出了一种单边矩形扩展A*算法，采用受迫扩展规则，单条公共边取代2 条相邻冗余边，简化了终止条件，但在复杂环境搜索时效率明显降低。卜新苹等[10]提出改进的三阶Bezier 曲线方法，虽然能实现路径平滑，但是算法计算较繁琐，实现效率低。

综上所述，如果考虑到机器人的本体尺寸，基本的A*算法在搜索路径时很有可能撞到障碍物，对机器人和障碍物来说是不安全的，而且不能得到次优或最优路径。因此提出了一种在障碍物边缘放置虚拟障碍进行缓冲和改进启发搜索函数的算法，并在静态和动态障碍物环境下进行仿真，最后利用动态切点法对路径进行二次平滑处理，提高了算法的实时性和机器人的稳定性。

1 A*算法的机器人路径规划

1.1 环境场景的建立

环境模型的建立是对机器人进行控制的基础，为实现路径规划算法，移动机器人看作二维平面移动的质点[11]，将障碍物信息以及机器人运动轨迹记录在栅格上，黑色块表示障碍物单元栅格，白色块表示可通行区域单元栅格。机器人运动空间为，将栅格化的环境信息映射到直角坐标系XOY中，且Xmax和Ymax分别为X、Y轴上的最大值。假设机器人的移动步长为σ，对X、Y轴分别以σ进行划分如图1 所示。

图1 栅格化的移动机器人环境

图1 中，行列的栅格数分别为m＝采用“栅格-存储”的映射法[12]将格子信息存储在第m行、第n列，记为C(m，n)。定义从原点开始第一个栅格C(1，1)的序号i为1，C(2，1)的序号为2，…，在m×n的栅格中，坐标(x，y) 与栅格序号的映射关系为:

式中:mod，int 分别为求余、取整运算。

1.2 机器人路径规划的A*算法

A*算法是典型的启发式搜索算法，其搜索区域为四方向或八方向。具体搜索如图2 所示。

图2 格栅场地及机器人运动方向

代价函数的核心表达式为:

式中:f(n)表示从起始节点Nstart到目标节点Ggoal总的代价消耗。g(n)表示从起始节点到当前节点Current 的实际消耗，h(n)为当前节点Current 到目标节点Ggoal的估计代价消耗值，f(n)的大小取决于h(n)的大小，h(n)越接近实际距离，消耗的总代价越小。因此分析A*的关键所在就是h(n)。假设真实路径代价值用Ch(n，goal)表示，扩展搜索范围用EX 表示，h(n)与Ch(n，goal)的不同对A*搜索算法的影响如表1 所示。

表1 h(n)对A*算法的影响

全局路径规划可分为3 个部分，首先是机器人环境模型的建立，其次调用A*算法进行路径搜索，最后路径输出，如图3 所示。

图3 路径规划流程图

2 改进的A*路径规划算法

2.1 虚拟障碍物的设置

A*算法把机器人运动的整个空间分成2 部分:自由空间和障碍物空间。没有考虑到机器人的实体尺寸、转弯半径、定位与距离误差等因素的影响。针对上述问题，提出了把整个运动区域进行安全等级划分，在障碍物周围放置虚拟障碍物形成缓冲区。

为保障机器人能安全快速的通过障碍物区域到达目标点，可把整个运动区域进行安全等级划分。机器人在路径规划时选择安全等级较高的优先进行规划，远离障碍物，减少干扰，提高安全性。机器人当前位置的代价值与距离最近障碍物距离D成反比。规定代价的范围为[0 255]，代价值255 用MCV(Max Cost Value)表示。最近障碍物距离D 规定0 至BR(Buffer radius)，BR 一般取机器人半径的2 倍。缓冲区代价等级R计算公式用如式(3)所示，R值越小安全等级越高。

式中:Rd(Robot radius)表示机器人半径。

经过放置虚拟障碍物处理的地图如图4 所示。

图4 虚拟障碍物地图处理

图4 中粗黑色部分代表障碍物所占据的空间，浅黑色(灰色)部分为虚拟放置障碍物膨胀部分，白色为自由无障碍区。其中R值越小代表障碍等级越低，在机器人路径规划中经优先规划R较小的区域。

2.2 启发函数的改进

分析表1 可知，A*算法的搜索性能受h(n)的影响，当的大小越接近真实代价消耗的总代价越小，h(n)搜索效率越高。实际中很难估计h(n)大小，一般有曼哈顿距离(Manhattan)、切比雪夫距离和欧氏距离3 种方法。欧氏距离相对实际距离偏短，Manhattan 距离相对于实际距离偏大。所以可以取Manhattan 距离和欧氏距离的中间值作为h(n)的估计值。中间值h(n)的估计值如图5(a)、5(b)、5(c)所示。

图5 中，线段d1和d2分别代表当前节点N点到目标节点G点的横向距离与纵向距离，d1和d2之和为Manhattan 距离。斜线段为欧氏距离，虚线段为Manhattan 距离和欧氏距离的中间值，即改进的启发函数h*(n)。改进后的h*(n)的推导如下所示:

图5 改进启发函数示意图

(1)当d1＝d2时；

(2)当d1＞d2时；

(3)同理当d1＜d2时；

综上h*(n)表达式如(7)所示。

式中:d1与d2的计算公式如(8)所示:其中d1与d2是在二维平面内的距离。N(xn，yn)为当前节点坐标，G(xg，yg)为目标点坐标。

改进后的算法流程图如图6 所示。

图6 改进后A*算法

3 仿真结果和分析

3.1 静态障碍物环境下

静态障碍物环境也就是机器人所处的空间障碍物是固定不变的，不会突然出现移动的障碍物等情况。实验环境:Intel(r)Core(TM)I5-6500 CPU。编译工具MATLAB 2017a。分别对A*算法和改进A*算法进行仿真，地图尺寸为30 m ×30 m，设起点为Start，目标点为Goal 进行仿真计算，结果如图7 所示。

图7(a)为A*算法没有进行虚拟障碍物放置处理和未进行h*(n)改进的路径规划，可以很明显地看出机器人贴近障碍物附近运行且转弯幅度过大，生成的路径不平滑且可靠性低[13]，这会导致机器人的碰撞几率增大，燃料成本增加。

改进A*启发算法转折角度明显减小，机器人远离障碍物有效地降低了机器人与障碍物的碰撞几率，提高了路径的可靠性，如图7(b)所示。

图7 基本A*算和改进A*算仿真结果

图8、图9 为基本算法与改进算法运行时间和路径规划长度对比图。

图8 规划时间对比图

图9 路径长度对比图

由图9 可以看出在20 次仿真实验中，基本A*算法路径相对较短，但是最小转折角度太过于小，很容易使机器人不能转弯，也会导致电机磨损加重降低寿命。改进A*算法路径长度比基本算法路径长9.8%(路径长度变长是由于规划路径远离障碍物和机器人转弯半径增大的结果)，运行时间缩短16.3%，最小转折角度也得到了很大的改善。累计转折角度减少10%～20%。仿真结果证明改进A*启发算法符合机器人最优路径规划需求，能有效地解决实体机器人通过狭窄区域或通道的路径规划问题，提高了机器人的稳定性。

表2 实验结果平均值

3.2 动态障碍物环境下

在动态环境下，除了存在静止的障碍物外，当突然有动态障碍物出现在移动机器人的工作环境中时，机器人就可能与其发生碰撞，这就需要机器人避开障碍物进行规划。图10 是在复杂动态环境中进行规划的结果。起始点Start，目标点Goal，黑色、浅色方框分别为静态和动态障碍物。由仿真结果可知改进算法可以有效地避开静态和动态障碍物，提高机器人控制的可靠性。

图10 动态路径规划结果

3.3 路径平滑

改进的A*算法规划的路径虽然得到明显提高但频繁转向会使机器人发生抖动，而平滑的路径更便于移动机器人的控制。采用切线交点画圆法进行路径平滑。定义了Smooth 函数如图11 所示:(1)取中间某一节点为中间节点，连接中间节点前后两接点，若不与障碍物相交则去除中间节点，否则保持原路径[14]，原理如图12(a)所示；(2)若遇到转折点则做切线交点画圆法，如12(b)所示。

机器人起始位置N1(x1，y1)，终点位置Nn(xn，yn)。从起始位置依次对转折点Ni(xi，yi)(i＝1，2，…，n-1)进行平滑处理，具体步骤如下:

(1)取中间节点连接前后相邻节点。若不与障碍物相交则去除中间节点，否则保持原路径。

(2)比较Ni-1Ni和NiNi+1。选择较短的边作为切点，切线与∠Ni-1NiNi+1角平分线交于Oi。

图11 Smooth 流程

图12 路径平滑示意图

(3)判断圆弧是否与长边Ni-1Ni有交点A。若有，转至步骤(4)，若无转至(5)。

(4)用圆弧PA代替边Ni-1NiNi+1，并转至步骤(6)。

(5)无交点就继续在较短的边取下一临近节点，同理(2)。

(6)判断是否遍历所有节点。若是，返回(1)，否则结束。

路径平滑后的结果如图13、图14 所示。

图13 静态路径平滑结果

由图13 可以看出平滑后路径去除了冗余的节点，转折点得到圆弧过渡，减少了机器人的转弯频率。可以看出未改进算法路径会与障碍物边缘发生摩擦，如图14 中的②、③所示。平滑后的路径可以实时避开突然出现的障碍物，更接近真实环境，如图14 中的①所示，因此改进A*算法提高了机器人的稳定性、安全性和实时跟踪效率，同时减少了电机磨损程度和燃料成本。

图14 动态路径平滑结果

4 结论

(1)针对移动机器人路径规划问题，从路径的安全性、机器人的稳定性出发，提出一种在障碍物边缘放置虚拟障碍物形成缓冲区和改进搜索启发函数的算法。

(2)仿真结果表明，改进A*算法相比基本A*算法规划的路径更加远离障碍物且累计转折角度变小，并能有效避开动态障碍物，规划时间相对缩短，证明了改进算法的有效性。

(3)最后对静态障碍物环境和动态障碍物环境规划后的路径进行二次平滑处理。结果表明，平滑后的路径更符合实体机器人通过狭窄的区域或通道，提高了机器人的稳定性、安全性、控制性和实时跟踪效率。