基于改进A*势场法的机器人动态路径规划研究

2021-01-24王晓燕吕金豆

制造业自动化 2021年1期

王晓燕，吕金豆

（西安建筑科技大学，西安 710000）

0 引言

路径规划问题一直是机器人研究领域的一个重点问题，其目的是在含有障碍物的环境中，寻找一条从起始位置到达目标位置的无碰撞路径[1]。在一个含有动态障碍物的环境中对机器人进行路径规划是当下的一类研究热点[2]。

而障碍已知的环境中，障碍又分为静态障碍和动态障碍，动态障碍一般使用的算法如：滚动路径规划法[3]、人工势场法、蚁群算法[4]、遗传算法[5]、D*算法[6]等。人工势场法（Artificial potential field method，APF）由Khatib于 1986 年提出的[7]，它的基本原理是：将移动机器人在一定环境中的运动虚拟为一种在抽象的人造势力场中的运动，目标点和机器人间产生引力，力的大小与两者距离成正比；障碍物和机器人间产生斥力，力的大小与两者距离成反比。通过求合力来对移动机器人的运动进行控制。

在寻路问题上，作为一种较为完善的方法，人工势场法以其局部实时性、平滑安全性等特点，常被用于含动态障碍物的避障算法中，但其明显缺少全局搜索能力，较易出现局部最优解、目标点不可达等停滞问题[8]。而A*算法[9]以其全局最优性、完备性和高效性时常用于全局最优路径规划中，是一种典型的静态路径全局规划算法，但其无法做到实时避障，不适于含有动态障碍物的环境。

本文的主要思路是在含动态障碍物的模拟环境中，对传统人工势场法改进并与A*算法相结合，规划出一条移动机器人由起始点到目标点的无碰撞路径。当未进入动态障碍物影响范围ρ0时，移动机器人按照A*算法所规划的路径行使；进入影响范围ρ0时，采用改进人工势场法进行实时动态避障。以此来弥补传统人工势场法的静态陷阱和震荡问题，并有效减小路径长度。

1 基本算法原理描述

1.1 人工势场法

传统人工势场法是在建模后的环境中假设有虚拟的引力场和斥力场存在，二者的力的作用平衡使得移动机器人能够躲避障碍。对人工势场法进行如下定义：

机器人R的当前方位为X=（x，y），目标位置G为Xg=（xg，yg），式(1)为机器人与目标点之间的引力场：

由该引力场所生成的引力：

其中Katt是引力增益系数，|X-Xg|是机器人与目标点的直线距离。机器人与障碍物之间的斥力场如下式：

由斥力场所生成的斥力：

其中Krep是斥力增益系数，Xobs是障碍物的位置，|X-Xobs|是机器人与障碍物的直线距离，ρ0是障碍物的影响距离。

综合上式，分别可得移动机器人在运动空间中的合势场及合力：

1.2 A＊算法

A*算法是一种经典的启发式算法，能够在静态环境中高效地求出从起始点到目标点的最优路径[10]。将其应用于移动机器人全局路径规划，其原理可简述为：将移动机器人的运动环境分割为若干相同的节点，创建两张表OPEN LIST与CLOSED LIST，CLOSED表用来记录已访问过的节点，OPEN表用来保存所有已生成而未检查的节点。

首先进行初始化，将起始点START放入OPEN表中，CLOSE表清空；

算法开始，从OPEN表内取第一个节点n，若n是目标解则继续寻找或终止算法，若不是，则按一定规则展开与n点相关的子节点，并将非障碍物、地图边缘的子节点以及n放入CLOSE表，其他放入OPEN表；

计算每一个子节点的估计值f（n），将OPEN表按照f（n）由小到大排序；

重复上述过程直至目标点。保存这些f(n)最小的节点，将它们沿目标点到起始点连接，便得到A*算法计算出的全局最优路径。

估价函数为：

式中，f（n）表示起始节点经由节点n到目标节点的估计代价值；g（n）表示在状态空间中从起始节点到节点n的实际代价值；h（n）表示节点n到目标节点的最小路径的估计代价值。

2 改进A＊势场算法

2.1 改进依据

本文采用A*算法进行移动机器人的全局路径规划，改进势场法实现局部在线规划。利用MATLAB R2014a软件对两种基本算法进行仿真，通过大量的仿真与分析，验证了在静态环境中，A*算法较人工势场法全局路径规划在轨迹震荡和路径长度问题上的表现更加优越。

选取如图1所示位置随机的10个障碍物，用以两种算法仿真普遍结果的比对。从图中可以看出，当运动环境相同时，采用A*算法进行全局路径规划能够高效减少轨迹转折次数，消除震荡，并解决了人工势场法目标点不可达的问题。

图1 A*算法与人工势场法全局静态路径

表1 A＊算法与人工势场法仿真结果对比

由表1可知在相同环境进行全局路径规划时，人工势场法路线较长，累计转折角远大于A*算法，并且出现了震荡、转折次数多、规划路径不平滑等现象。A*算法路线转折次数少，累计转折角度远小于人工势场法且路径较平滑。因此本文选用A*算法对已知环境进行全局路径规划，人工势场法作为局部规划算法。

2.2 引入速度势场模型

2.2.1 速度斥力场

针对动态环境，本文对传统人工势场法的斥力势场进行了改进，即引入相对速度斥力势场。对局部动态避碰问题做了如下简化：已知机器人和障碍物的运动状态、两者的相对位置矢量以及相对速度矢量；视机器人与障碍物为质点；移动机器人与障碍物都具有全方位的运动能力。

记机器人刚好进入障碍物影响范围的时刻为t，图2为t时刻移动机器人与障碍物的运动状态，此时生成虚拟目标点G，以速度沿A*算法路径移动，并与移动机器人速度重合，障碍物以速度作直线运动驶向机器人。

图2 t时刻移动机器人与障碍物运动状态

设定速度势场为：

2.2.2 速度引力场

机器人驶离动态障碍物后需回到原A*路径上，因此位于A*路径上的虚拟目标点G是动态的，由t时刻产生，直到移动机器人速度与其再次重合时消失。图3为期间任意时刻t'移动机器人与障碍物的运动图示。建立虚拟目标点G的引力势场模型如式(9)所示，该模型由相对位置和相对速度两部分势场函数构成：

图3 t'时刻移动机器人与障碍物运动状态

结合上述公式，可求得引入速度势场后机器人所受的合力：

当移动机器人进入障碍物影响范围ρ0，且两者运动方向相靠近时，机器人所受总合力由综合引力Fatt、斥力Frep及速度斥力Frv构成；当移动机器人进入障碍物影响范围，但两者的运动方向相背离时，机器人所受总合力由综合引力Fatt及斥力Frep构成；当移动机器人在影响范围之外，总合力为0。

2.3 改进算法具体步骤

图4所示为算法流程图，改进算法具体步骤为：

步骤1：初始化参数。选择起始位置xStart、yStart，目标位置xTarget、yTarget，创建列表OPEN、CLOSED，将起始节点设为第一个节点。

步骤2：初始化参数。设置人工势场法步长l，循环迭代次数J，增益系数Katt、Krep，速度斥力常量λrv，速度势场增益系数Kattp和Kattv等其他参数的初始化。

步骤3：A*算法路径规划。将起始节点放入关闭列表CLOSED，更新后续节点放入打开列表OPEN，对后续节点做最小f（n）检查并选取。遍历列表，通过最后一个节点（如果它是目标节点）开始，然后识别它的交节点，直到它到达起始节点，生成静态最优路径。

步骤4：改进势场法动态避障。进入ρ0后，势场法开始运行，虚拟目标点沿步骤3）生成路径以运动。移动机器人依据式(8)进行避碰，依据式(9)回到步骤3）生成路径，使得保存机器人走过的每个坐标，生成路径。

步骤5：输出最优路径。将步骤4中求出的动态路径与步骤3）求出的静态路径相结合，可得出本文全局动态路径规划最优路径。

图4 算法流程

3 实验研究与仿真分析

将本文的改进A*及势场算法应用于机器人动态路径规划问题求解，为验证本文算法的可行性和有效性，进行大量仿真实验，并通过与传统人工势场法进行比对，验证本文算法的优越性。算法运行环为：Windows10 64bit，仿真软件MATLAB R2014a。

为了验证改进算法动态避障的可行性，选取静态障碍物位置同图1。设改进势场法初始参数引力场常量Katt=2，速度引力常量Kattv=2，斥力场常量Krep=1，速度斥力常量λrv=1，斥力影响范围ρ0=1.5，步长l=0.1，步长设置不同，每一次循环迭代机器人移动的距离不同。选取四个不同循环迭代次数下，移动机器人的实时路径，如图5所示。其中实心方块为动态障碍物，沿y轴负方向作速度为0.1/s的匀速直线运动，空心方框为静态障碍物，粗实线为A*算法路径，细实线为改进势场法路径。

图5(a)表示改进算法开始，机器人刚好进入移动障碍物影响范围，移动机器人开始局部路径规划；图5(b)、图5(c)分别表示第10步与第18步，移动机器人基于改进势场法进行动态避障；图5(d)表示机器人在第25步完成避碰要求，开始追踪原路径。