基于改进AG算法的机器人动态路径规划方法

王楠 张军 解鹏

摘要：为了充分发挥Agoraphilic（AG）算法的优越性，使其可以在动态环境中有效地进行路径规划，对传统AG算法进行了研究和改进，在计算自由空间力时增加了机器人和动态障碍物之间的相对速度分量，该分量可分解为2个方向的分力，一个分力使机器人向背离障碍物的方向运动，另一个分力使机器人向垂直于障碍物的方向运动，充当机器人绕行的动力。利用Matlab进行了仿真实验，将改进的AG算法和几种其他动态路径规划方法进行了对比。改进后的AG算法使机器人能够迅速躲避动态障碍物，有效地进行动态避障。研究方法不仅可以解决动态环境中机器人躲避动态障碍物并到达目标点的问题，而且与其他动态路径规划算法相比，具有路径长度更短、耗时更少、路径更平滑等优点。

关键词：计算机仿真； 移动机器人； 路径规划； 动态环境； 改进AG算法； 动态避障

中图分类号：TP399文献标志码：Adoi： 10.7535/hbgykj.2018yx03005

随着科技的迅猛发展，移动机器人越来越普遍地出现在人们的生活中，在深海作业、消防作业、航空航天、国防部门、娱乐场所甚至医院、银行等地方都能看到它们的身影。移动机器人众多研究领域中最基本同时也是最重要的一个领域就是路径规划问题。路径规划要完成的任务是找到一条从起点到终点的最优路径，同时需要避开环境中的障碍物，这是机器人智能化的体现[1-2]。由于机器人的工作环境通常是动态不确定的，所以机器人动态路径规划也成为当前研究的热点。

目前根据对各领域常用路径规划算法的研究，将动态环境下机器人路径规划方法分为传统路径规划方法和智能仿生路径规划方法，传统算法主要有人工势场法[3-4]、模糊逻辑法、可视图法等，人工势场法的优点是结构简单、计算量小、实时性好， 在实时避障和平滑的轨迹控制方面应用广泛，但是机器人易出现合力为零、陷入局部极小点的问题；模糊逻辑法的优点是鲁棒性较强，并且在环境中存在不确定性时便于与其他方法融合，但是需要人工经验的支撑，而且在计算过程中模糊表和推理的规则可能出现迅速膨胀，影响算法性能；可视图法的优点是方便简易，缺点是若环境中起点和目标点改变则可视图便不再适用，因此灵活度低。智能仿生算法也叫生物启发式算法，是根据生物在复杂的动态环境中寻找食物或迁徙中得到的启发，主要有蚁群算法[5-6]、遗传算法、粒子群算法等，蚁群算法的优点是采用分布式并行运算机制，具有很强的鲁棒性和适应性，且很容易与其他算法相结合，但是易出现停滞现象，或者陷入局部最优解；遗传算法的优点是在全局搜索上显示了明显优势，计算量小，缺点是在局部搜索上局限性较大，劣势较明显；粒子群算法的优点是能快速收敛，算法简单、鲁棒性强，但是易于陷入局部最优，并且若算法中的参数设置不合适可能丧失粒子的多样性。综上所述，传统方法和智能算法仍存在改进的空间。

由IBRAHIM等[7]提出的Agoraphilic（AG）算法是一种基于虚拟立场技术（VFF）的反应性的移动机器人路径规划算法，文献＼[8＼]中论述了它的优点是只使用一个单一的由周边自由空间产生的吸引力控制机器人运动，所以避免了局部最优解的问题；随后MCFETRIDGE等[9]论证了通过该算法进行导航可以产生更平滑的路径，实现了更高的平移速度且避免路径振荡；HONG等[10]也提到AG算法的目标是为机器人寻找一个开放的可前行空间而不是单纯避障，从而进一步确保了机器人运行的安全。但是动态环境中动态障碍物的运动速度和方向具有不确定性，机器人在避障的过程中可能会与障碍物发生碰撞，上述使用该算法的文献并没有考虑这个问题，这使得该算法的应用领域一直局限于静态环境中，直至2015年BILBEISI等[11]进一步论证了AG算法是一种反应性移动机器人路径规划机制，计算复杂性低，为充分发挥其优越性本文对基本AG算法进行改进使其更适合于实时动态导航，通过增加相对速度分量最终使其能够完成在动态环境中的路径规划，仿真实验证明了改进算法具有可行性，并通过对比实验证明了该方法较传统动态人工势场法和智能动态蚁群算法均具有路径长度更短、更平滑、可视性更强等优势。

1基本AG算法

Agoraphilic（AG）算法是一种基于虚拟力场技术（VFF）[12]的反应性移动机器人路径规划算法。该算法是势场法的一种形式，它有别于传统的经典人工势场法，使用自由空间力（FSF）作为吸引力控制机器人向环境中的自由空间运动。

AG算法的步骤概括如下：首先初始化参数，机器人在路径规划中不断利用传感器检测周围环境信息[13]，算法中使用直方图来存储本地环境地图，该直方图中包含着某些确定的值，表示某个对象例如障碍物等在特定位置出现的可能性，然后计算直方图中每个扇区的期望和力的值，每个扇区的力正比于距离值的平方，最終根据机器人所受的合力及方向角的值确定机器人下一步要走的位置，更新机器人的位置。

AG算法中最重要的阶段为计算机器人所受的合力，现设λk为自由空间直方图（FSH）中第k个扇区的期望，dk为扇区k中的距离值，ρ0为障碍物影响距离，即障碍物在该范围内会对机器人产生影响，ρ（XR，Xi）为障碍物到机器人的距离，dobs为机器人到障碍物的距离，Δk为扇区k方向的单位矢量，k为当前的扇区，K为FSH中扇区的总数，使用下述公式计算每个单独扇区的力：

FK=（λkdk）2×Δk，k=1，2，…，K， （1）

dk=dobs（ρ（XR，Xi）≤ρ0），dmax（ρ（XR，Xi）>ρ0）。 （2）

将各个扇区的力汇总，作用在机器人上的合力为

FFs=∑Kk=1FK。 （3）

用FFsx表示合力在x轴方向分量，FFsy表示合力在y方向分量，机器人当前的前进角度为

θFs=tan-1FFsyFFsx。 （4）

由于上述基本AG算法只考虑了障碍物是静态的情况，当环境中出现动态障碍物时，动态障碍物的运动速度和运动方向具有不确定性[14-15]，当动态障碍物的运动轨迹与机器人运动轨迹出现交点时可能会发生碰撞的情况，上述算法并不适用，所以在AG算法的基础上进行改进，以使其能够用于动态环境中的机器人路径规划。

2改进的AG算法

2.1基本原理

在基本AG算法的自由空间力中加入机器人和障碍物的相对速度分量，该相对速度分量的增加可以起到使机器人远离障碍物运动的作用，从而使机器人能够迅速躲避动态障碍物，有效地进行动态避障。图1为增加速度分量后机器人受到的总的合力。

用VR表示机器人的速度，Vi表示动态障碍物的速度，qi为障碍物所在位置，q为机器人当前位置，VRi表示机器人与障碍物的相对速度在两者连线上的分量：

VRi=（VR-Vi）TΔRi。 （5）

动态障碍物与机器人之间会产生与速度及位置有关的势场：

U（qi，Vi）=ηVVRi， （6）

F=-ΔU。 （7）

根据式（6）和式（7）可以推出：

ΔVVRi=ΔRi， （8）

ΔqVRi=VRiΔRi-（VR-Vi）‖qi-q‖。 （9）

用VRi⊥ΔRi⊥表示与VRi垂直方向的速度：

VRi⊥ΔRi⊥=VR-Vi-VRi， （10）

FV1=ηV1ΔiR， （11）

FV2=ηV2×VRi⊥ΔRi⊥ρ（XR，Xi）。（12）

式中：ηV为速度增益系数，表明速度分量影响大小，则增加的相对速度分量可看成由FV1和FV2组成，其中FV1的方向为障碍物和机器人连线上由障碍物指向机器人的方向，该分力使机器人向背离障碍物的方向运动，FV2的方向为与机器人和障碍物连线垂直的方向，该分力使机器人向垂直于障碍物的方向运动，充当机器人绕行的动力。

最终增加的相对速度分量为

FV=FV1+FV2。 （13）

计算出作用在机器人上的合力如式（14）所示：

F′Fs=FFs+FV，VRi>0，FFs，VRi≤0。 （14）

当VRi>0时表示障碍物朝向机器人运动，此时应该采取避障措施，当VRi≤0时表示障碍物远离机器人运动，此时不用采取避障措施。机器人当前的前进角度为

θ′Fs=tan-1F′FsyF′Fsx。 （15）

2.2算法描述

综上所述，加入相对速度分量后具体的改进AG算法流程图如图2所示。

改进AG算法的实现步骤如下。

1）参数初始化，包括设置机器人的起点和目标点的位置坐标、静态障碍物的位置坐标、直方图初始距离值等。

2）机器人根据传感器获取周围环境信息和动态障碍物的速度，同时计算到障碍物的距离，生成图3所示自由空间直方图（FSH）。在直方图的每个扇区中包含着机器人在特定方向的距离信息，提供了一个距离指示，该机器人在发生碰撞之前可在给定的方向移动。

生成自由空间直方图之前的步骤是用最大的距离值在直方图内初始化每个扇区，该距离值可以从下列2个值中选取，第1个为环境中有源区域的尺寸，以使机器人最初假定它被定位在开放区域，该有源区域的最大距离是机器人活动窗口的半径raw；第2个距离值是当目标在机器人视觉范围内（FOV）出现时机器人当前位置到目标的距离dgoal，该值的设置是为了使机器人在接近障碍物的情况下仍然可以到达目标。每个扇区的最后初始化值是通过式（16）给定的，

dmax=min（raw，dgoal），目标点在FOV内；raw，其他。 （16）

3） 计算直方图中每个扇区的期望。如果没有期望的影响，机器人将试图找到最大的自由空间并且直接朝其前进而不管目标在哪，因此期望的增加是为了使机器人能够朝向目标运动。一个扇区的期望不仅是可用的自由空间区域的大小，同时也包括如何使扇形角更接近目标[16]。所以扇区中最大的期望将会在具有可用的最大自由空间并且朝向目标的扇区中产生，基本AG算法期望采用固定值，本文中使用式（17）来计算期望：

λ=ηλ×1ρ（XR，Xg）， （17）

式中：ηλ为期望增益系数，表明期望的影响大小；ρ（XR，Xg）为机器人到目标的距离，通过式（17）计算的期望会使得机器人朝向目标点运动，且机器人离目标的距离越近该期望值越大，机器人朝向目标运动的动力也越大，从而使机器人最终到达目标点。

4）计算每个扇区的力，初始的力正比于每个扇区中的距离值，加入相对速度分量后通过式（14）和式（15）计算机器人所受合力及当前方向角。

5）根据所受的合力及方向角的值确定机器人下一步要走的位置，更新机器人的坐标。

6）判断机器人是否到达目标点作为终止算法的条件。

3对比实验及结果分析

使用Matlab进行仿真实验，采用Windows操作系统，CPU为1.70 GHz、内存为4 GB。

3.1基本AG算法与改进AG算法对比实验

首先將基本AG算法和改进AG算法在具有静态和动态障碍物的环境中进行对比实验，地图的尺寸为10×10，静态障碍物的分布相同，机器人起始点为（0，0），目标点为（10，10），机器人活动窗口半径r=5，障碍物影响范围大小ρ0=2.5，期望增益系数ηλ=500，机器人的速率均为2，动态障碍物T1的速率为1.18，动态障碍物T2的速率为1。

使用基本AG算法进行仿真实验时机器人路径如图4所示，其中小圆圈代表静态障碍物，障碍物T1会和机器人发生碰撞，交点位置为（2.875，1.625）。

采取改进的AG算法进行路径规划如图5所示，可以看出改进后的算法能够有效避开动态障碍物T1。

3.2改进AG算法与动态蚁群、动态人工势场法对比实验

采用动态蚁群算法进行路径规划时主要参数为蚁群数量m，启发因子α和期望启发因子β，这些参数的设置对结果起到重要的影响。本文中参数的优化配置过程如下：首先分析蚁群数量对结果的影响，设置α=1，β= 5，分别选择蚂蚁数目m为10，30，50，80，100，150进行仿真实验。结果表明：当蚂蚁数量为80时，最优路径长度与收敛迭代次数均为最小值；启发因子α对算法的影响为α越高，蚂蚁越倾向于选择其他蚂蚁所经过的路径，期望启发因子β越高蚂蚁更倾向选择更近的路径点。仿真时，当信息启发因子α过小时收敛速度慢，α过大时算法会出现过早收敛，蚂蚁很难找到最优路径，因此取最优组合为α=1，β=7。

综上所述，设置动态蚁群算法的参数：蚁群数目m=80，启发因子α=1，期望启发因子β=7，迭代次数N=100进行仿真实验。

采用动态人工势场法进行路径规划时，取障碍物影响距离ρ0=2.5，机器人起始点为（0，0），目标点为（10，10）。

进行对比实验时环境中存在固定参数和随机设置的变量，固定的参数为地图尺寸、机器人初始位置、目标位置、机器人初始速率，表1中列出了这些参数的值。环境中的变量为静态障碍物的个数及位置，动态障碍物的个数、初始位置、速率、运动方向，在实验过程中这些变量随机取值。

参数名称地图尺寸机器人

初始位置目标点机器人初始速率/

（栅格边长·s-1）参数值10×10（0，0）（10，10）2

在该环境下分别用改进AG算法、动态蚁群算法、动态人工势场法进行20组随机实验，得到不同的机器人路径长度和运行时间，得到的统计结果如表2所示。

表3对两组随机实验中变量的取值进行了说明。图6、图7从左至右分别为两组不同的随机实验环境里采用改进AG算法、动态蚁群算法、动态人工势场法得到的路径规划。

表4列出了两组随机实验的运行结果。

随机实验算法名称路径长度/栅格边长运行时间/s改进AG算法15.557.78实验1动态蚁群算法16.588.29动态人工势场法16.918.46改进AG算法15.397.70实验2动态蚁群算法16.498.25动态人工势场法17.198.60

通过上述对比实验和统计表格可知：相比于传统AG算法，改进AG算法可以进行动态避障且最终到达目标点；而且相比于其他动态路径规划算法，20组对比实验中改进AG算法得到的实验结果优于动态蚁群算法的概率为90%，优于动态人工势场法的概率为95%，即采取改进AG算法进行动态路径规划得到的路径长度更短、耗时更少，同时从实验路径图中可以看出，改进AG算法得到的路径更加平滑，可视性更高。

4结语

介绍了解决机器人路径规划的一种基于势场的AG算法，该方法通过自由空间力来控制机器人使其运动。为了解决动态环境中机器人路径规划的问题，本文对基本的AG算法进行了改进，在计算机器人所受的最终合力时增加了相对速度分量，该分量的增加能够使机器人远离障碍物运动，实现动态避障。仿真结果表明了该方法的可行性，可以使机器人找到一条从起点到终点并且避开静态和动态障碍物的路径，并且通过对比实验表明了该方法具有优势，即较其他动态路径规划方法路径长度更短、耗时更少。

本文的研究仍然存在不足之处。首先对于AG算法的改进和创新仍有许多可提升和努力的空间；其次本文研究的是单个机器人的运动，在更为复杂的环境中如果需要多机器人共同完成困难的任务，路径规划的难度将大大增加，所以多机器人系统的路径规划问题仍需进一步研究；最后随着机器人应用领域的不断扩大，其应用领域逐渐从低维空间扩展到高维空间，因此高维空间机器人路径规划仍是目前研究的热点和难点，例如将机器人放入三维空间中。未来机器人路径规划研究可朝这些方向继续努力。

参考文献/References：

[1]DELMERICO J， MUEGGLER E， NITSCH J， et al. Active autonomous aerial exploration for ground robot path planning[J]. IEEE Robotics and Automation Letters， 2017， 2（2）： 664-671.

[2]张启飞，郭太良.基于多階段决策的机器人全局路径规划算法[J].计算机工程， 2016，42（10）：297-302.

ZHANG Qifei， GUO Tailiang. Global path planning algorithm of robot based on multistage decision[J]. Computer Engineering， 2016，42（10）：297-302.

[3]温素芳，郭光耀.基于改进人工势场法的移动机器人路径规划[J].计算机工程与设计，2015，36（10）：2819-2822.

WEN Sufang， GUO Guangyao. Path planning of mobile robot based on improved artificial potential field approach[J].Computer Engineering and Design， 2015，36（10）：2819-2822.

[4]丁家如，杜昌平，赵耀，等. 基于改进人工势场法的无人机路径规划算法[J].计算机应用，2016， 36（1）：287-290.

DING Jiaru， DU Changping， ZHAO Yao， et al. Path planning algorithm for unmanned aerial vehicles based on improved artificial potential field[J]. Computer Applications， 2016，36（1）：287-290.

[5]屈鸿，黄利伟，柯星.动态环境下基于改进蚁群算法的机器人路径规划研究[J].电子科技大学学报，2015，3（2）：261-265.

QU Hong， HUANG Liwei， KE Xing. Research of improved ant colony based robot path planning under dynamic environment[J]. Journal of University of Electronic Science and Technology of China， 2015， 3（2）：261-265.

[6]张成，凌有铸，陈孟元.改进蚁群算法求解移动机器人路径规划[J].电子测量与仪器学报，2016，30（11）： 1759-1764.

ZHANG Cheng， LING Youzhu， CHEN Mengyuan. Path planning of mobile robot based on an improved ant colony algorithm[J]. Journal of Electronic Measurement and Instrumentation， 2016， 30（11）：1759-1764.

[7]IBRAHIM M Y， MCFETRIDGE L. The agoraphilic algorithm： A new optimistic approach for mobile robot navigation[C]//2001 IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Como：IEEE，2001：1334-1339.

[8]MCFETRIDGE L， IBRAHIM M. Behavior fusion via free-space force shaping[J]. IEEE International Conference on Industrial Technology， 2003， 2：818-823.

[9]MCFETRIDGE L， IBRAHIM M. A new methodology of mobile robot navigation： The agoraphilic algorithm[J]. Robotics and Computer-Integrated Manufacturing， 2009， 25（3）：545-551.

[10]HONG J， PARK K. A new mobile robot navigation using a turning point searching algorithm with the consideration of obstacle avoidance[J].The International Journal of Advanced Manufacturing Technology， 2011， 52：763-775.

[11]BILBEISI G， AL-MADI N， AWAD F. PSO-AG： A multi-robot path planning and obstacle avoidance algorithm[C]// 2015 IEEE Jordan Conference on Applied Electrical Engineering and Computing Technologies. Amman， IEEE， 2015： 1-6.

[12]RASHID M B， SUNDARESAN S， JAYASEKERA T， et al. VFF-Monte Carlo framework for phonon transport in nanostructures[C]//International Workshop on Computational Electronics. West Lafayette： IEEE， 2015：1-2.

[13]HWU T， WANG A，OROS N， et al. Adaptive robot path planning using a spiking neuron algorithm with axonal delays[J]. IEEE Transactions on Cognitive & Developmental Systems， 2016（99）：1-1.

[14]黃立新，耿以才.基于动态人工势场法移动机器人路径规划研究[J].计算机测量与控制，2017， 25（2）： 164-166.

HUANG Lixin， GENG Yicai. Robot path planning based on dynamic potential field method [J].Computer Measurement & Control， 2017， 25（2）： 164-166.

[15]JIANG M， CHEN Y， ZHENG W， et al. Mobile robot path planning based on dynamic movement primitives [C]//Control Conference （CCC）. Dalian： IEEE， 2016：980-985.

[16]黄海卫，孔令成，谭治英.室内服务机器人实时目标识别与定位系统设计[J].计算机工程与设计，2016，37（8）：2229-2232.

HUANG Haiwei， KONG Lingcheng， TAN Zhiying. Real-time object recognition and localization system for indoor service robot [J]. Computer Engineering and Design， 2016， 37（8）：2229-2232.第35卷第3期河北工业科技Vol.35，No.3