一种改进的人工势场法路径规划算法

何世琼，陈 雨

（四川大学电子信息学院，成都 610065）

0 引言

机械臂的路径规划是机械臂运动到指定位置执行任务的前提，它的任务是找到一条从起始位置到目标位置的无碰撞运动轨迹。常见的路径规划算法有快速搜索随机树（RRT）、C空间搜索和人工势场法等。其中，人工势场法因为模型简单、计算量小且实时性高等优点而被广泛使用。但是，作为一种典型的局部路径规划算法，传统人工势场法存在易陷入局部最小解的问题。

针对传统人工势场法的局部最小解问题，研究者从多个方面进行了改进。文献［5-6］通过改进势场函数模型来优化传统人工势场法；文献［7］将人工势场法与RRT算法相结合，避免了人工势场法陷入局部极小点的问题；文献［8］将强化学习与人工势场法结合，用于串联机械臂的避障和路径规划；文献［9］提出一种利用人工势场法和蚁群算法相结合的路径规划方法。上述方法通过不同途径解决了人工势场法容易陷入局部最小解的问题，但是都不同程度地带来了路径不平滑，路径离最优较远等问题。

本文针对传统人工势场法的缺陷，提出了一种改进的人工势场路径规划算法，在解决局部最小解问题的同时，仍能得到较短的平滑路径。算法先判断路径规划是否陷入局部最小解，如果是，则启动模拟退火策略进行随机搜索，直到跳出局部最小点，得到起始位置到目标位置的有效路径。

1 人工势场法基本原理

人工势场法是一种局部路径规划方法，它的基本思想是将机器人的运动空间看作一个虚拟的势场，在这个势场中，目标点产生引力引导机器人向其运动，障碍物产生斥力避免机器人与之碰撞，机器人在一个点所受的合力等于该点的引力和斥力之和，合力控制机器人向目标点运动。

人工势场包括引力势场和斥力势场，假设势场函数用表示，在点时的势场合力用U表示，则有：

其中，()表示点的引力场函数，()表示斥力场函数。引力场函数通常定义为公式（2）：

式中：表示引力增益，(,)表示当前点到目标点之间的距离。

斥力场函数通常定义为公式（3）：

式中：表示斥力增益，()表示点距离最近障碍物的距离，表示障碍物斥力作用的距离阈值，超过这个阈值的障碍物对机器人不产生斥力。

在引力场和斥力场中，引力和斥力通常用两种势场的梯度来表示。假设点的势场函数的值U为该点的能量，那么该点的梯度∇U就能看作该点的力向量，引力场和斥力场中的虚拟力就可以表示为如下两式：

结合势场函数和势场力的公式不难看出，势场中某点处梯度的方向就是势函数增长最快的方向，因此采用梯度下降法将人工势场法应用于机器人的路径规划。所谓梯度下降法，就是让机器人从起点沿着梯度的反方向运动，直到势场的梯度为0。

人工势场法对控制和传感误差有一定的鲁棒性，但是这种方法也有明显的缺点，当某点的引力和斥力的大小刚好相等、方向相反时，机器人会陷入局部最优解，进而无法达到目标点。

2 基于模拟退火的改进人工势场法

2.1 模拟退火法求全局最优

模拟退火法的基本思想是从一个较高的初始温度出发，伴随温度的不断下降，结合一定的概率突跳特性在解空间中随机寻找目标函数的全局最优解，即概率性地跳出局部最优解，并最终趋于全局最优。以图1为例，从A点出发，模拟退火算法在搜索到局部最优解B点以后，随机向右开始移动，以一定的概率冲过B和C之间的顶峰跳出局部最优解。

图1 模拟退火法跳出局部最优示意图

模拟退火算法更新解的机制依据Metropolis准则，Metropolis准则通常表示为：

其中，表示新解的接受概率，表示当前的温度，()表示新解对应的系统能量，()表示当前解对应的系统能量。从公式（6）中可以看出，当新解更优时直接更新，当新解的能量高于当前解时，是否更新则与当前的温度和能量有关。如图1所示，初始状态在A点，随着迭代次数的增加更新到点局部最优解，此时B点的能量低于A点。到达B点后，根据当前状态、温度和能量计算更新概率并跳出B点，达到全局最优的C点后稳定下来。

模拟退火法求全局最优的算法流程如图2所示，在任意温度水平下，随机扰动产生新解，并计算目标函数值的变化，决定是否接受新解。由于算法初始温度比较高，所以使能量增大的新解在初始时也可能被接受，因而能跳出局部极小解，然后通过缓慢地降低温度，最终可能收敛到全局最优解。

图2 模拟退火法求全局最优流程图

2.2 改进人工势场法

改进人工势场法将模拟退火法引入人工势场法中，当路径规划出现局部最小解时，通过退火策略的随机搜索跳出局部最小解，算法流程图如图3所示，详细的实现过程如下：

图3 改进人工势场法算法流程

（1）设置机械臂末端的起点、终点以及地图中障碍物的位置，设机械臂末端的当前点为，局部最小解的点为。

（2）判断末端是否陷入局部最小解，如果陷入局部最小解，则启用模拟退火策略，同时设置初始温度=。

（3）在当前点附近产生一个随机点，根据势场函数公式计算当前点和随机点的势能，分别用U和U表示，设置Δ=U-U。

（5）判断末端是否逃离局部最小解，如果U≤U，则认为机械臂末端逃离了局部最小解，否则，逐渐减小温度，直到末端跳出局部最小解，通常()=(-1)，0.85≤≤1，表示步长。

（6）判断机械臂末端是否达到目标点，达到目标点后结束，否则重新设置初始温度进行搜索。

3 仿真实验

3.1 二维路径规划仿真实验

二维路径仿真实验中，二维避障环境设置为10×10的正方形，环境中的障碍物是半径为0.5的圆，机械臂末端的起点为（0，0），目标点的位置为（10，10），两种算法的迭代次数均为1000次，实验次数为50次。传统APF算法的参数设置如表1所示，改进APF算法中的其他参数设置见表2。其中表示引力增益系数，表示斥力增益系数，表示退火策略的初始温度，T表示退火策略的最低温度，表示退火策略的移动步长，表示路径规划时的搜索步长，表示障碍影响距离，当障碍和机械臂的距离大于这个距离时，斥力为0，即不再受该障碍的影响。

表1 传统APF算法参数设置

表2 改进APF算法参数设置

图4为传统APF路径规划的仿真结果，实心圆点表示障碍物，网格表示自由运动区域，（0，0）点表示起点，（10，10）点表示终点，密集的折线表示可能陷入局部最小解时模拟退火策略的随机搜索，点状虚线表示从起点到终点的有效路径。

图4 传统APF和改进APF算法二维路径规划仿真结果

其中，图4（a）～（f）分别表示在少量障碍物、较多障碍物和大量障碍物的场景下，传统APF和改进APF在同一地图中成功规划的仿真效果。图4（a）、（b）、（c）表示在三种障碍物场景下，传统APF都可以成功避障并完成从起点到目标点的路径规划，并且没有陷入局部最小解。从图4（a）可以看出，在少量障碍物环境中，传统APF可以避开障碍物搜索到有效路径达到目标点；从图4（b）可以看出，在障碍物较多的环境中，传统APF也可以成功避障达到目标点；在图4（c）中，传统APF避开了大量障碍物得到有效路径。图4（d）、（e）、（f）显示了在三种不同的障碍物场景下，改进APF也能成功避开障碍搜索到有效路径，从图4（d）和（f）中可以看出，机器人在经过第一个障碍物和障碍物较多的位置时，启动了退火策略产生了随机扰动，最终成功规划出了从起点到终点的有效路径；图4（e）中改进的APF规划出了一条不同于传统APF的路径。

表3列出了传统APF算法和改进APF算法的时间性能数据，实验数据取算法成功的情况下的平均值。从表3可以看出，相较于传统APF算法，改进APF算法由于增加了模拟退火策略，需要更长的路径搜索时间。此外，随着环境中障碍物数量的增加，APF算法需要更多的迭代次数搜索路径，所以运行时间更长。但是在路径长度方面，改进APF算法优于传统APF算法。

表3 传统APF（Tra_APF）和改进APF（Imp_APF）二维仿真结果

图5给出了不同数量的障碍物环境下传统APF算法和改进APF算法的部分路径规划仿真结果。图5（a）、（b）、（c）中，由于传统APF算法的缺陷，机器人都在障碍物附近陷入了局部最小解，无法继续搜索路径并运动到目标点，路径规划失败。图5（d）、（e）、（f）分别表示了在少量障碍物、较多障碍物和大量障碍物场景下APF算法对陷入局部最小解问题的优化，可以看出，当机器人陷入局部最小解时，触发了算法中的模拟退火策略，在局部最小值附近产生了随机搜索，最终跳出了局部最小解，成功规划出一条有效路径。

图5 改进APF算法对传统APF的局部最小解问题的优化仿真结果

3.2 三维路径规划仿真实验

在三维仿真实验中，路径规划环境为10×10×10的立方体空间，机械臂末端的起点设置为［0，0，0］，目标点设置为［10，10，10］，两种算法的参数设置与二维仿真实验相同。

图6为三维仿真实验结果，球体表示使被包围球包络的障碍物，实线表示从起点到终点的路径。图6（a）为传统APF路径规划结果，可以看出当两个障碍物距离较远时传统APF算法可以成功规划出一条从起点到终点的有效路径；图6（b）表示了传统APF陷入了局部最小解时机器人无法继续向目标点移动，从而导致路径规划失败；图6（c）表示了改进APF算法成功跳出局部最小解，并运动到了目标位置。

图6 传统APF和改进APF三维路径规划仿真结果

表4列出了在三维仿真环境中两种APF算法的性能数据仿真结果。在运行时间方面，改进APF算法的运行时间相比传统APF算法平均延长了0.2 s。在路径长度方面，两种算法得到的路径随着环境中障碍物数量的增加而增大，并且改进APF算法得到的路径长度要小于传统APF算法。

表4 传统APF（Tra_APF）和改进APF（Imp_APF）三维仿真结果

二维和三维仿真实验结果都验证了改进APF算法的有效性。在机器人没有陷入局部最小解的情况下，改进APF算法的运行时间比传统APF算法的时间平均延长了0.2s，规划出的路径长度比传统APF算法更短；当路径规划陷入了局部最小解时，传统APF算法路径规划失败，改进APF算法可以成功跳出局部最小解并运动到目标位置，并且得到更短的路径。

4 结语

针对传统人工势场法的缺陷，本文提出了改进算法并通过实验验证了其有效性。主要研究结论有如下两点：

（1）基于模拟退火的改进人工势场路径规划方法能有效地解决传统人工势场法的缺陷，使路径规划成功逃离局部最小解，得到一条从起始位置到目标位置的有效路径。

（2）相比传统人工势场法，改进人工势场法获得的路径更短，但运行时间平均增加了0.2s。