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

2019-05-05陆皖麟雷景森

兵器装备工程学报 2019年4期

陆皖麟，雷景森，邵炎

(66133部队，北京 100043)

路径规划技术是移动机器人研究的热点问题，对它的研究为控制机器人自主行驶奠定基础[1]。路径规划的定义可以表示为：设定起始点、目标点后，依据规划算法搜寻从起点至目标点同时可以避障的一条最佳路线。依照机器人的环境情况将运动路径列为两类：一类是基于地图信息已知的全局路径规划，此类路径规划环境中的信息包括障碍物和可行驶区域全部已知；另外一类是根据相机、惯性导航、里程计等获取实时信息的局部路径规划，局部路径规划是处在障碍物信息未知的环境[2]。常用的全局路径规划算法有[3]：A*算法、Dijkstra算法、粒子群算法、遗传算法等。A*算法最早由Nilsson提出来的启发式算法，它的核心是对目标点进行不断搜寻，从而取得机器人的避障路径。通过对状态空间中搜索点进行评价，取得最佳节点，然后依据此位置节点继续进行搜寻，一直到找到目标点为止。该算法具有编程方法相对简单，参数设置较少，搜索路径效率高[4]的特点。

1 A*算法原理

A*算法是获取最短路径比较有效的搜索方法，也是典型的启发式算法。A*算法一般用于静态全局规划，其核心表达式为

f(n)=g(n)+h(n)

(1)

其中：f(n)表示从起点开始经过当前节点到目标节点的路径长度；g(n)表示起点至当前节点n的路径长度，此时g(n)已经是机器人从起始点到当前点的最短距离；h(n)是从状态节点n到目标状态的估计代价函数距离。由于g(n)是实际距离，为了在静态地图可以搜寻到最优解，选择代价h(n)类型非常重要，f(n)的值取决于h(n)的大小。下面描述算法搜索节点过程，如图1所示，绿色小格表示起始点位置，红色小格表示目标点位置，蓝色小格周围的8个点表示可以设定为当前状态空间的子节点候选点，设小格的一条边长度为10，式(1)简写为F=G+H，G表示起始点至当前状态的曼哈顿距离，H是当前状态节点至目标节点估计代价函数值，这里代价函数H的计算使用的是欧式距离，起始点周围8个格子F、G、H各数值见图，由图可知2号小格F最小，将其作为接下来要处理的点，然后检查2号周围的小格的G、H值，黑色方格是障碍物，在计算G、H值时忽略其不可通行，确定2号的待选小格是1号、3号。由于1号、3号是父节点(起始点)的8个相邻节点、3号节点周围没有可选新的相邻节点，故从起始点选择待处理节点是1号而不是2号、3号。按照此思路，4号节点是1号节点的子节点。

图1 A*算法搜索节点

在已构建的地图中，设起始点坐标节点为S(Sx,Sy),当前节点C(Cx,Cy)，目标节点的坐标T(Tx,Ty)，若使用相对简单的欧式距离估价函数值可表示为

(2)

2 A*算法流程

下面将以伪代码的形式描述A*算法流程，首先设置各项参数，start：机器人路径规划的出发点；goal：机器人路径规划的目标点位置；g(n)：n节点到start点的实际移动距离；h(n)：n节点到达目标点的理论移动距离；openlist：搜寻节点的过程中保存未检测的节点列表；closelist：已经被检测的节点保存至该列表；comaFrom：保存子节点和父节点，最终生成的路径在此列表中。使用伪代码为：

把起点加入到openlist

{

搜寻openlist中f(n)值最小的节点，它表示当前节点

加入至closelist

对当前节点相邻的所有候选节点

if(是障碍物点‖在closelist中)

{

继续搜寻下一个相邻候选节点

}

if(openlist不存在此节点)

{

将此候选节点加入openlist，更新父节点，计算它的f(n)、g(n)、h(n)值

}

if(候选节点已经在openlist内)

{

if(使用g(n)值作为评判标准，来检测新的路径，判断是否有更低的g(n))

{

将它的父节点换为当前节点，计算当前节点的f(n)、h(n)的值

}}

使用comaFrom保存父节点

}while(目标节点在openlist里面，表示路径规划成功)

若openlist列表是空值，表示没有可寻找的路径。

最后把comaFrom保存的所有父节点从起点开始以此连接起来，该路线就是A*算法所求的路径。

传统A*算法路径规划程序结构如图2所示。

3 A*算法改进

尽管传统A*算法已经成为移动机器人导航中广泛使用的路径规划算法，但是算法规划出来的路径存在拐点多，转折次数多的问题，在实际环境中不利于机器人的行驶[5]。因此在研究A*算法的基础上提出一种适宜机器人行驶改进的A*算法。

图2 传统A*算法路径规划程序框图

为解决A*算法在实际路径规划中不能求最短路径，Anthony Stentz提出基于插值方法的D*方法[6]，使路径变得平滑，但采用插值的方法使计算量增大，实时性变差。遗传算法、神经网络算法等智能算法也用于路径规划，但对于静态地图而言智能算法花费时间较多。为优化A*算法的规划路径，减少路径的长度，针对A*算法的3个方面加以改进：

1) 设定openlist访问时间阈值

传统A*算法在当前节点扩展子节点时，每次都要把所有候选节点扩展完毕，这种方法在复杂环境下可能存在一定的无效搜索，将会耗费一定的时间，因此，在传统A*算法的基础上，每次检测openlist表中最先插入的节点在经过一定时间阈值N后，判断是否得到扩展，若未扩展，则将此节点作为最高级优先扩展，在一定程度可以避免陷入局部最小值，解决无法规划出全局路径问题。

2) Floyd算法优化

Floyd算法借鉴动态规划的思路搜寻最短路径，是用来解决两点之间的最优距离问题的算法[7]。使用Floyd算法和A*算法结合的方法以减少路径长度，符合实际应用需求。Floyd算法原理如图3所示。

图3 Floyd算法原理

L(A,D)为A、D两点间的距离，如图3，A、D存在障碍物，可设L(A,D)=+∞，R(A,D)=A->D

B点是A点和D之间已规划出的节点：

若

L(A,B)+L(B,D)

(3)

则

L(A,D)=L(A,B)+L(B,D)

(4)

R(A,D)=A->B->D

(5)

在B、D线段上插入点C：

若

L(A,C)+L(C,D)

(6)

则

L(A,D)=L(A,C)+L(C,D)

(7)

R(A,D)=A->C->D

(8)

去除C点，A到D的优化路径表示为一段优化后的弧形路径R(A,D)。采取Floyd算法优化A*算法规划的路径能够删除多余的点，更加适合移动机器人行驶。

3) 路径光滑处理

A*算法规划的路径存在许多尖锐的拐点和折点，这种拐点不符合移动机器人的平稳运动。为了满足移动机器人在规划的路径上能够平稳移动，需要对规划的路径进行平滑处理[8]，用平滑的曲线来代替尖锐点。其原理框图如图4。

图4 平滑处理原理框图

(9)

过直线BC方程为

(10)

由图可知

(11)

设圆弧半径

|p1r|=|p2r|=R

(12)

由式可得

|p1B|=|p2B|=R·cot(δ/2)

(13)

设

(14)

由A，B点坐标和式得p1和p2点坐标:

p1·x=ε1·x1+(1-ε1)·x2

p1·y=ε1·y1+(1-ε1)·y2

p2·x=(1-ε2)·x2+ε2·x3

p2·y=(1-ε2)·y2+ε2·y3

(15)

过p1点和点p2分别做直线AB和BC的垂线，其交点为。则圆弧方程为：

(x-ry)2+(y-ry)2=R2

(16)

那么轨迹A->P1->P2->C的方程为:

(17)

通过以上方法可以在拐点、折点处，采取Floyd算法去除多余的点，缩短机器人行驶路径，然后再平滑算法处理路径，从而减少拐点的弯折程度，有利于机器人转向行驶以及避障操作[9]。A*算法改进后的程序流程框图如图5所示。

图5 改进A*算法路径规划程序流程框图

4 仿真实验与分析

为验证改进后的A*算法效果，仿真使用MATLAB2016b进行编程。在MATLAB中采用GUI设计A*算法图形化验证界面，如图6所示，在guide里面设置两个按钮，一个命名为A*算法路径规划，另外一个命名为生成障碍物，它们会自动生成.m文件，使用GUI可编辑文本设置起点坐标、目标点坐标，分别在A*算法路径规划和生成障碍物按钮生成的.m文件导入相关的代码，修改对应Function函数的参数，生成图7的GUI界面，点击生成障碍物按钮，会出现长和宽为32×32，且周围有一定障碍物的地图，地图上一个栅格单位长为1。

图6 仿真界面GUI设计

图7 A*算法验证界面

构建仿真环境地图，地图中障碍物随机设置，同时在GUI编辑文本中随机设置起点坐标为(3,3)，目标点随机设置为(29,22)。仿真功能就是实现从起点(3,3)至(29,22)寻找一条最优路径。实验一采用传统A*算法进行路径规划，路径仿真结果见图8(a)；实验二仅设定openlist访问时间阈值，进行路径规划，路径仿真结果见图8(b)；实验三在实验二基础上增加Floyd和平滑处理算法，路径仿真结果见图8(c)。

图8 仿真路径规划效果

分析对比实验结果可知改进后的算法路径总长度缩短了，拐点和折点数量有所降低，提高了路径平滑度，更加适合移动机器人的运动。统计实验数据如表1：改进后规划时间略有增加，这是因为增加了Floyd算法和平滑算法。但规划时间相差不是很大，在可以接受范围内，这是由于对算法做了改进，在开启列表(openlist)增加了一个阈值N，提高了寻找子节点的效率。