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

王 维，裴 东，2*，冯 璋

(1．西北师范大学物理与电子工程学院，兰州730030; 2．甘肃省智能信息技术与应用工程研究中心，兰州730030)

(*通信作者电子邮箱peidong@nwnu．edu．cn)

0 引言

自主移动机器人能够通过一些传感器实现自主移动和完成某种任务，能够替代人类进行一些作业［1－3］。路径规划是机器人完成导航以及其他任务的前提。机器人路径规划是指按照一定的指标为机器人搜索出一条从起点到目标点的无碰撞路径，是自主移动机器人研究的基础［4］。移动机器人路径规划研究非常广泛，有许多方法来实现，如栅格法［5］、人工势场法、蚁群算法、模糊逻辑和神经网络算法［6－7］等。栅格法是对机器人所处环境的一种表示方法，以栅格为单位记录环境信息，“0”表示无障碍物(地图上表现为白色)，“1”表示有障碍物(地图上表现为黑色);栅格划分越小精确度越高，但占用存储空间越大，搜索时间指数级增长。人工势场法是机器人路径规划算法中一种简单有效的方法，但存在诸多问题，当机器人远离目标点时引力过大，存在碰撞障碍物的危险，在障碍物之间行走时存在振荡问题，易陷入局部最优值从而使目标不可达。蚁群算法收敛速度慢，容易陷入局部最优值。神经网络算法的网络模型参数对规划器要求高［8］，而且其中的参数只能进行反复的试验才能得到最优路径［9］。模糊逻辑方法规划的机器人运动路径存在着“对称无法确定”的现象［10］，尽管文献［10］和文献［11］提出了改进的方法，但最终得到的路径都不是最优路径。

Dijkstra算法采用遍历搜索方式，缺点是当节点较多时，节点网络变得非常庞大，算法效率非常低下［12］。在Dijkstra算法的基础上，Nilsson在1980年提出了 A*算法，它是一种基于启发式搜索的全局路径规划［13］，缺点是计算效率低、路径非最优［14］。文献［15］通过引入父节点的影响提高了A*算法的实时性，但是在对估价函数进行加权时未考虑权重的变化。

对此，本文提出对估价函数进行指数衰减的方式加权，进一步改进A*算法，并对路径轨迹进行5次多项式平滑处理，提高了算法的实时性，处理后路径变短变光滑。

1 算法描述

Dijkstra算法和A*算法是运用最广泛的两种路径规划算法，A*算法是在Dijkstra算法的基础上提出的，先介绍一下Dijkstra算法和A*算法的原理。

1．1 Dijkstra算法

Dijkstra算法采用贪心算法来解决最短路径问题，是一种局部最优选择，在每一次寻找下一个节点时，总是选取到源节点最近的节点作为子节点，直到遍历整个网络。因此，其准确性是以计算速度为代价的，对于较大网络，该算法遍历整个网络将耗时非常长。

Dijkstra算法只考虑了起始点到某节点的实际路径代价，没有考虑到目标点的影响，属于盲目的遍历式搜索。

1．2 A*算法

A*算法是在Dijkstra算法基础上加入了目标点到当前节点的估计代价，根据地图上从起始点经过当前节点到达目标点的代价决定搜索的方向，已知条件是全局地图信息已知，效率较Dijkstra算法大大提高。

A*算法将当前节点的启发函数定义为:

其中:f(n)为当前节点的估价函数;g(n)是起始点到当前节点的实际路径代价;h(n)是当前节点到目标点的最小估计代价;h(n)应当满足不大于当前节点到目标点的实际路径代价的条件。当估价函数中h(n)=0时，即为Dijkstra算法。h(n)有曼哈顿距离(式(2))、切比雪夫距离(式(3))及欧几里德距离(式(4))等表示形式:

欧氏距离应用较为广泛，因此，本文采用该距离。A*算法广泛运用于解决机器人路径规划问题，适用于环境信息已知的全局规划。

全局路径规划可分为3步:地图构建;根据算法规则选取地图点;连接这些点形成路径。A*算法在搜索下一个节点时，有相邻的4个或8个节点两种选择。不同方法所规划的路径不同，耗时、路径长度也不同。

1．3 算法流程

图1给出A*算法的算法流程，得到初始规划路径。传统A*算法存在搜索节点多、耗费时间长、路径节点多及转折角度大的缺点，机器人追踪此路径轨迹耗时长且不便于控制。

2 算法改进

2．1 指数加权

A*算法按照传统的估价函数进行路径搜索时，会不断往返搜索，搜索节点过多。实际上，当前节点到目标点的估计路径代价h(n)的值不大于实际路径代价值，因此，应该加大启发函数中h(n)的权重。对此文献［16］进行了改进，如式(5)，改进后实时性较传统A*算法有所提高。文献［15］又加入了父节点对当前节点扩展的影响，如式(6)，大幅减少了往返搜索次数，改进后实时性得到进一步提高。

式中:h(p)是当前节点的父节点到目标点的距离，a为权重。

图1 A*算法流程Fig．1 Flow chart of A*algorithm

本文考虑到当前节点到目标点的估计代价占实际代价的比重与当前节点的位置有关，当前节点到目标点的估计路径代价为最小路径代价，小于实际路径代价。当节点离目标点较远时，估计值远小于实际值，估计值权重应该大一些;当节点逐渐靠近目标点时，估计值逐渐逼近实际值，估计值权重随之降低;当节点到达目标点时，估计值等于实际值。对此，本文在文献［15］的基础上提出指数衰减的方式进行加权，如式(7):当h(n)较大时，权重大，这样使节点迅速向目标点行进;当h(n)较小时，权重变小;目标点附近时，权重接近1，可保证目标点可达。

仿真实验如图2所示，图(a)为Dijkstra算法搜索结果，有3110个节点;图(b)为传统A*算法搜索结果，有2 362个节点;图(c)为文献［15］改进算法搜索结果，有1026个节点;图(d)为本文改进算法搜索结果，只有158个节点，路径搜索节点数明显降低，实时性提高效果明显。

2．2 五次多项式平滑处理

Dijkstra算法和A*算法搜索的初始路径存在许多冗余点，文献［17］通过消除两连通节点之间的节点对路径进行了平滑处理，存在转折点。文献［18］提出了三次Ferguson样条法，优点是易于实现，缺点是加速度不平滑。文献［19］提出了“圆弧－直线－圆弧”的方法，缺点是圆弧段加速度不平滑，并且在一些拐点处需要转一圈，增加了路径长度。

图2 几种算法节点搜索结果对比Fig．2 Comparison of node searching results of several algorithms

本文对路径中的冗余点进行剔除，伪代码如下:

BEGIN

path1(end+1，:)← path(1，:);

FOR t←1 to size(path，1)

IF直线lt穿过障碍物 ＆＆直线lt－1不穿过障碍物

(lt表示连接 path1(end，:)、path1(t，:)两点的直线)

THEN

path1(end+1， :) ← path(t－1，:);

END IF

t←t+1;

END FOR

END

经过对路径冗余点处理，只保存起点、拐点以及终点，再采用五次多项式进行平滑处理［20］。在满足初始时刻和T时刻加速度为零的前提下，其加速度变化率也是连续平滑的，即二次函数型，如式(8):

则速度v(t)=s'(t)=5At4+4Bt3+3Ct2+2Dt+E，加速度a(t)=s″(t)=20At3+12Bt2+6Ct+2D，加速度变化率为a'(t)=s(t)=60At2+24Bt+6C，写成矩阵形式如式(9):

满足约束条件:s(T)=AT5+BT4+CT3+DT2+ET+F;s(0)=常数;v(0)=v(T)=常数;a(0)=a(T)=0时，解出系数向量(A，B，C，D，E，F)。在给定T时即可画出s(t)的轨迹。

各算法规划的最终路径如图3所示，通过对比可以看出本文算法转折角明显减少，路径更短、更光滑。

图3 算法规划路径结果对比Fig．3 Comparison of path results of algorithm

表1为Dijkstra算法、传统A*算法、文献［15］算法和本文算法实验数据比较。

表1 几种算法实验结果比较Tab．1 Experimental results comparison of several algorithms

可以看出，本文改进算法在实时性和路径长度方面都有很大改善。本文算法较传统Dijkstra算法运行时间降低95%、路径长度降低19．5%;较传统A*算法运行时间降低94%、路径长度降低19．5%;较文献［15］算法运行时间降低85%、路径长度降低23．9%。

为了验证本文算法的可靠性，在简单室内环境一和随机地图环境二下对算法进行验证，如图4所示。

图4 本文算法验证结果Fig．4 Verification results of the proposed algorithm

2．3 实验结果

本仿真实验采用微软 Core i5 6500处理器，主频3．2 GHz，内存4 GB。在Matlab2014b环境下，为了验证算法的可行性，除文献［15］采用的环境外，再通过环境一、环境二对改进算法进行验证，得出本文改进A*算法的访问节点数、运行时间以及路径长度数据，如表2所示。

表2 本文算法测试实验结果Tab．2 Test results of the proposed algorithm

3 结语

在环境信息已知的栅格环境中，本文在已有的改进基础上，对A*算法提出了进一步改进，在不同环境下，本文算法在耗费时间上大大减少，路径更短且得到了平滑处理，便于机器人不间断地跟踪路径轨迹到达目标点。对本文改进算法的仿真结果与Dijkstra算法、传统A*算法、文献［15］改进的A*算法进行了比较，结果证明了本文算法的正确性、合理性、很好的实时性以及复杂环境下的可靠性。