基于动态引力场的机器人路径规划研究

李靖 杨帆 韩艳芬 陈虹

摘  要： 人工势场法对机器人进行路径规划时，存在障碍物使目标不可达、死锁和局部极小点等问题，容易导致路径规划失败，此外发现即使路径规划成功，由于斥力的存在，必定使得规划路径不是最短、最优的。针对上述问题提出一种动态引力场路径规划算法，该算法完全去掉斥力场，将引力场的大小及方向动态化，虚拟目标点动态化，不断交替寻找最优或次优方向进行避障，动态地修正路径，最后对路径进行平滑优化，规划出最短、最优的路径。该方法解决了人工势场法固有的缺陷，同时优化了规划路径的长度，仿真实验结果说明该算法具有实用性和有效性。

关键词： 机器人路径规划; 动态引力场; 动态路径修正; 路径长度优化; 路径平滑优化; 路径优化对比

中图分类号： TN99?34; TP242                     文献标识码： A                     文章编号： 1004?373X（2020）11?0041?06

Research on robotic path planning based on dynamic gravitational field

LI Jing1， YANG Fan1， HAN Yanfen2， CHEN Hong2

（1. School of Electronic & Information Engineering， Hebei University of Technology， Tianjin 300401， China;

2. Tianjin Light Industry Vocational Technical College， Tianjin 300350， China）

Abstract： Some problems may occur when the artificial potential field method is applied to robot path planning， for example， target is unreachable due to obstacles， deadlock and local minimum point may occur， which can easily lead to failure of path planning. In addition， it is found that even if the path planning is successful， the planned path is definitely not the shortest and optimal due to the existence of repulsive force. In view of the above， a path planning algorithm based on dynamic gravitational field is proposed. In the algorithm， the repulsive force field is completely removed， both the size and direction of gravitational field and the virtual target point are changed dynamically to continuously search the optimal or suboptimal directions alternately for the obstacle avoidance， so that the path is corrected dynamically. In the end， the path is subjected to evenness optimization and the shortest and optimal path is planned. With this method， the inherent defects of the artificial potential field method are removed， and the length of the planning path is optimized. The simulation results show that the algorithm is practical and effective.

Keywords： robotic path planning; dynamic gravitational field; dynamic path correction; path length optimization; path evenness optimization; path optimization contrast

0  引  言

随着机器人技术飞速发展，路径规划或障碍物规避问题成为机器人调度策略等控制领域研究的重点和热点问题之一，是机器人顺利完成各项作业任务的前提条件。机器人路径规划技术是根据规划空间中障碍物信息，在满足一定约束条件和评价指标的条件下，线下或线上为机器人规划出从出发点到目标点合理避开障碍的满意路径。

机器人路径规划根据环境信息掌握情况[1]，可以分为全局规划和局部规划搜索算法。全局规划算法主要应用于障碍物地图信息已知的情况，启发式算法运用居多，如遺传算法[2]、粒子群算法[3]等。局部规划搜索算法主要是根据实时环境信息确定的环境地图，进行障碍物避障的路径规划方法，主要有人工势场法[4?6]、模糊逻辑算法[7]、A*算法[8]等。此外，还有深度学习路径规划算法[9?10]，深度学习通过模拟大脑神经元网络进行机器学习分析，对路径规划领域带来了飞跃式发展。但其需要大数据集的支撑，模型复杂化，训练、验证、测试迭代导致时间复杂度大，且需要高要求的硬件支撑，对小样本数据集无法进行无偏差的估计。因此，对于多机器人协同合作布置传感器的一次性工作场景下，人工势场法体现出了建模简单、搜索路径快捷且计算复杂度低的路径规划算法的优点，该算法借助虚拟力思想能搜索出比较平滑且安全的路径。但人工势场法会出现局部最小值及临近目标点震荡徘徊的现象，由于斥力的存在，其规划的路径一定不是最短、最优化路径，且引力系数、斥力系数要根据不同的情况进行不同的设置，很难统一确定，在复杂的环境下会有局限性。

本文借助人工势场法中虚拟力的思想，提出了动态引力场算法。借助引力场作用，将起点与目标点动态化，形成动态引力场，去掉斥力场作用，不但解决了传统人工势场法无法解决的四种情况，而且解决了由于斥力的存在，路径不是最短的问题，可以得到最短、最优的路径，且计算复杂度低，计算时间进一步减小。

1  传统的人工势场法路径规划算法

1.1  人工势场法原理

1986年，Khatib将人工势场算法引入到了机器人避障路径规划领域。人工势场法[11]进行路径规划的基本思想是将机器人、障碍物、目标点简化为一点，机器人在周围环境中的运动抽象为在虚拟力场中的运动，即目标点对机器人产生引力场[Uatt]，障碍物对机器人产生斥力场[Urep]，机器人在斥力场和引力场的共同作用下向目标运动。

设机器人在空间中的位置为[X]，目标点位置为[Xg]，障碍物位置为[Xo]，引力势函数和斥力势函数可分别表示如下：

[Uatt（X）=12?k?X-Xg2] （1）

[Urep（X）=12?η?1ρ（X，Xo）-1ρo，    ρ（X，Xg）≤ρo0，    ρ（X，Xg）>ρo] （2）

式中：[k]和[η] 分別为引力和斥力增益系数;[ρ（X，Xg）]和[ρ（X，Xo）]分别为机器人到目标点与到障碍物的距离;[ρo] 为障碍物的影响半径。障碍物对机器人的斥力和目标点对机器人的引力分别对应斥力场和引力场的负梯度，得到相应引力函数和斥力函数：

[Fatt（X）=-?Uatt（X）=k?ρ（X，Xg）] （3）

[Frep（X）=-?Urep（X）     =η?1ρ（X，Xo）-1ρo1ρ2（X，Xo），   ρ（X，Xo）≤ρo0，    ρ（X，Xo）>ρo] （4）

则机器人受到的合力为：

[Ftotal=Fatt+Frep] （5）

机器人在合力的作用下，朝目标点运动，其受力如图1所示。

1.2  传统人工势场法的缺陷

1.2.1  目标不可达到

传统人工势场法在机器人还未到达目标点时，提前陷入局部势场极小点而无法到达目标，形成目标不可到达的情况。主要体现在以下四种情况：

1） 搜索目标点正前方有障碍物的情况，即引力和斥力形成的合力运动方向被障碍物阻断，使得机器人无法正常沿着合力的方向继续前进，从而引起不可到达目标点的情况，如图2a）所示。

2） 引力与斥力合力为0的情况，即引力和斥力大小相等方向相反，合力抵消为0，机器人无法找到运动方向，处于静止状态，从而使得目标不可达到，如图2b）所示。

3） 临近目标点斥力大于引力搜索失败的情况，即目标点附近有大（多）障碍物时，可能引起障碍物产生的斥力大于引力，从而使得合力的方向偏向斥力，从而偏离目标点，甚至出现倒退现象，使得目标不可到达，如图2c）所示。

4） 遇到陷阱会出现死锁的情况，即合力的方向被障碍物阻断，无法跳出陷阱，在陷阱内停止或死锁，从而使得目标不可到达，如图2d）所示。

1.2.2  易陷入徘徊抖动状态

在多个局部极小点附近徘徊抖动，当机器人处在多个局部极小点周围时，若[ρ（X（n+m），X（n））≤ε]，则表明机器人在第[n]步到第[n+m+1]步中的[m]个位置上没有实质性的位移，运动路径出现了周期性徘徊，其中，[m=2，3，…]，[ε]为无穷小量，或者因为某种因素，合力方向突变，也容易导致徘徊抖动状态。当机器人在运动过程中出现徘徊抖动现象时，尽管其最终可能到达目标点，但将严重影响路径规划质量，可行性极差，如图3所示。

1.2.3  规划路径非最短

为了解决传统人工势场法的固有缺陷，改进的人工势场法主要通过构建合理的势场函数，消除或减少局部极小点，从根本上解决局部最小值问题。在路径搜索算法方面，主要是针对已经陷入局部极小点的情况，研究使用何种策略跳出局部极小点，使机器人重新回到正确的搜索状态。但无论是修改势场函数模型，还是在局部最小点处加入适当的跳出策略，斥力场依旧存在其中，只要斥力场存在，必定会使得机器人远离障碍物边缘行走，且斥力越大，偏离最大引力场的方向越多，路径越长，如图4所示，机器人并没有贴合障碍物边缘行走，从而出现使得路径不是最短的情况，便有了继续缩短路径长度的空间。

2  基于动态引力场的路径规划算法

2.1  算法原理

2.1.1  环境地图栅格化[11]

根据机器人运动的有限场地大小，将工作环境进行栅格划分，标定起点和目标点位置，如图5所示。

根据人工势场法原理中引力势场确定每个栅格的势场值，即：

[Uatt（X）=12?k?X-Xg2]

[Fatt（X）=-?Uatt（X）=k?ρ（X，Xg）]

2.1.2  搜索区域

机器人在进行路径搜索时有8个方位的搜索，即N，NE，E，SE，S，SW，W，NW，如图6所示。

当搜索区域无障碍物时，将机器人搜索路径转化成相遇问题，假设起点与目标点均在做相向运动。首先，机器人在起点位置搜索相邻8个方位，寻找目标最大引力势场方向，并朝着此最大引力完全栅格移动，即朝着最大引力场方向进行移动，无需修改[θ]（偏移角）值。随后，目标点根据新起点位置重新计算新起点最大引力势场方向，并朝着最大引力完全栅格移动，使起点与目标点逐一进行相向运动，最大引力势场根据起点和目标点位置的变化，不断地被修正，形成动态引力势场，当起点与目标点相遇（或距离小于一个步长，认为相遇）时，所记录下来的运动路径集便是规划出的路径。

当搜索区域有障碍物时，无论是起点还是目标点依旧搜索最大引力场栅格方向，若搜索到的最大引力场栅格有障碍物，将向次优引力场方向搜索，如该方向栅格无障碍物，便向次优引力势场栅格移动，如图6所示。此时最优引力势场修正了[θ]角，起点与目标点逐一搜索路径，进行相向运动，直到相遇（或距离小于一个步长，认为相遇）形成的运动路径集便是规划路径。

2.1.3  路径平滑

为了进一步优化路径长度，从起点节点开始，与其后不在同一直线上的节点依次相连，直到连接到某个节点时出现新生成的路径上存在障碍物时停止，将起点节点与该节点的前一个节点相连作为新的路径。将该节点作为起始节点继续与其后不在同一直线上的节点相连。依此类推，直到连接到终点节点，如图7所示，虚线为动态引力场规划路径，灰色路径为最终平滑后的最短路径。

2.2  算法流程及分析

基于动态引力场的路径规划算法：首先要栅格化工作环境地图，标定起点及目标点位置，通过引力势场让起点与目标点交替行驶一个步长，若最大引力场方向无障碍物，则沿着引力场方向运动，若最大引力场方向有障碍物，按照8个搜索方位，依次搜索次优方向，则对最大引力运动角修正[θ]角度，朝著次优引力场方向进行运动，直到起点与目标点相遇（或者起点与目标点小于一个步长，认为相遇），标定轨迹形成路径集，并对轨迹进行路径平滑的优化处理，从而得到最短、最优路径，如图8所示。

3  算法仿真及结果分析

为了验证算法的可行性，采用Matlab 2017软件进行仿真分析。实验的硬件平台是Intel[?] CoreTM i5?4210U CPU @1.7 GHz 2.14 GHz处理器和4 GB RAM计算机。

3.1  动态引力场算法仿真

为了检验动态引力场的机器人路径规划算法的性能，对人工势场法的固有缺陷情况进行了对比仿真实验。

3.1.1  搜索目标点正前方有障碍物

人工势场法中，当搜索到目标点正前方有障碍物，合力运动方向被障碍物阻挡，或者引力与斥力合力为0，无法到达目标点。而动态引力场法起点与目标点交替搜索，引力场在不断被修正，形成动态引力场，且搜索遇到障碍物，将向次优方向移动，能够沿着障碍物的边沿，绕开障碍物做相向运动，相遇时形成搜索路径。仿真如图9所示，图中将（0，0）设定为起点，图中用正方形标注;（10，10）设定为目标点，图中用三角形标注，根据具体场景分别设定障碍物，图中用圆圈标注。仿真实验结果表明，传统的人工势场法因合力的方向被障碍物阻档，从而无法继续规划路径，如图9a）所示，而本文算法可以绕开障碍物，继续进行路径规划，如图9b）所示。

3.1.2  易陷入徘徊抖动状态

人工势场法中，临近目标点斥力大于引力，或存在多个障碍物产生的斥力场，机器人无法选择正确的运动方向搜索失败。动态引力场法只有引力的存在，只要起点与目标点不相遇，引力便不会是0。如图10所示，可知临近目标点时，传统算法由于斥力和引力的共同作用导致路径产生振荡且最终没有到达目标点，利用动态引力场和动态寻找次优方向很好地解决了遇到障碍物和临近目标点振荡以及到达不了目标点的情况。

人工势场法中，遇到陷阱会出现死锁的情况，动态引力场法当陷入陷阱时，在动态引力场的指引下，合理选择引力场栅格，沿着障碍物边缘避开障碍物运动，从而逃出陷阱。图11是在图10障碍物的基础上增加（9.5，9.5）和（9，9）两个障碍物，传统算法会导致起点在（8.844，8.649 7）和（8.371 7，8.485 4）两点之间循环运动，如图11直线的加粗，红色字体标注部分，最终搜索目标不成功。动态引力场能很好地解决传统算法的死锁而导致搜寻不到目标点的情况，具有良好的规划效果。

3.2  路径优化算法仿真结果

通过模拟画出障碍物不同的形状信息，不同的位置信息的AB场景进行仿真，如图12所示。设定（20，0）坐标点为起点，（0，20）坐标点为终点，运用基于动态引力场的路径规划算法对模拟现场进行路径规划，随后并对路径进行平滑优化处理，得到了最短、最优的路径。图12中虚线为本文提出的动态引力场规划出来的路线，实线为本文路径优化后的规划路线。从图中可看出，基于动态引力场的机器人路径规划算法可以有效地规划出从起点到终点的最优路径。

路径优化前后路径长度与花费时间对比结果见表1。由表1可知，运用本文路径平滑优化后的路径长度有缩短，规划路径所花费时间略有减少。本文算法中的路径优化会根据具体场景的复杂情况不同，缩短的路径不同，对于障碍物垂直于起点和目标点连线的情景，以及复杂度大的场景，效果更为明显。

4  结  论

本文对传统人工势场法的原理、实现方法以及存在的问题进行了分析，在此基础上提出了基于动态引力场的机器人路径规划算法，有效地解决了传统人工势场法对于障碍物、局部极小点和死锁等目标不可达的问题。动态引力场法根据交替相向运动的起点与目标点，不断更新引力势场，继而不断地对路径进行修正，最后通过路径平滑进一步优化，形成最短且最优化路径。在Matlab 2017仿真平台上对改进前后的方法进行仿真，对搜索目标点正前方有障碍物、临近目标点振荡、陷入徘徊抖动状态、死锁缺陷情景进行了仿真对比，对路径优化前后效果进行仿真对比。结果表明，改进后的方法能够解决传统人工势场法的固有缺陷，通过对路径进行平滑优化，在时间和长度上对路径进行了进一步优化，很好地使机器人顺利避开障碍物并最终到达目标点。

参考文献

[1] 霍凤财，迟金，黄梓健，等.移动机器人路径规划算法综述[J].吉林大学学报（信息科学版），2018，36（6）：639?647.

[2] DAS P K， BEHERA H S， PANIGRAHI B K. Intelligent?based multi?robot path planning inspired by improved classical Q?learning and improved particle swarm optimization with perturbed velocity [J]. Engineering science and technology， 2016， 19（1）： 651?669.

[3] TIAN L， COLLINS C. An effective robot trajectory planning method using a genetic algorithm [J]. Mechatronics， 2004， 14（5）： 455?470.

[4] 张殿富，刘福.基于人工势场法的路径规划方法研究及展望[J].计算机工程与科学，2013，35（6）：88?95.

[5] 彭艳，国文青，刘梅，等.基于切点优化人工势场法的三维避障规划[J].系统仿真学报，2014，26（8）：1758?1762.

[6] 王伟，王华.基于约束人工势场法的弹载飞行器实时避障航迹规划[J].航空动力学报，2014，29（7）：1738?1743.

[7] 李擎，张超，韩彩卫，等.动态环境下基于模糊逻辑算法的移動机器人路径规划[J].中南大学学报（自然科学版），2013，44（z2）：104?108.

[8] ZHAO Junwei， ZHAO Jianjun. Path planning of multi?UAVs concealment attack based on new A* method [C]// 2014 6th International Conference on Intelligent Human?machine Systems and Cybernetics. Hangzhou， China： IEEE， 2014： 401?404.

[9] 吴宗胜.室外移动机器人的道路场景识别及路径规划研究[D].西安：西安理工大学，2017.

[10] 卜祥津.基于深度强化学习的未知环境下机器人路径规划的研究[D].哈尔滨：哈尔滨工业大学，2018.

[11] 朱爱斌，刘洋洋，何大勇，等.解决路径规划局部极小问题的势场栅格法[J].机械设计与研究，2017，33（5）：46?50.