基于人工势场法的局部路径规划改进

张 军 许靖宜 于士坤

（1.安徽理工大学人工智能学院；2.安徽理工大学机械工程学院 安徽淮南 232001）

路径规划技术是机器人移动机器人领域的关键技术之一，其含义为在包含障碍物的二维或三维环境中，按照一定规则和标准，规划出一条从起始点到目标点的满足机器人自身约束要求的无碰撞路径[1]。如图1 所示，为移动机器人在未知环境下完成路径规划，到达预设地点的示意图，其中曲线为移动机器人运动路径。路径规划技术广泛应用于多种领域，如移动机器人的自主避障行动，巡航导弹在避开雷达搜索情况下的最优飞行路线，工厂车间中物流机器人的自主行动等[2]。在移动机器人路径规划技术中，除了确保机器人能够抵达目标点外，还应考虑两个目标：一是要令所规划出的路径长度短，转折点少，以减少成本开销，提高工作效率；二 是要充分考虑机器人移动路线与障碍物的相对位置关系以及路径所经区域的障碍物分布情况，规避行驶风险，确保安全性[3]。

图1 移动机器人路径规划示意图

在移动机器人进行路径规划时，将机器人在地图环境中的运动抽象成一种人造引力场中的运动，构建人工势场模型[4]。势场包含两个部分，一部分是由目标点产生的引力场，一部分是由障碍物产生的斥力场，其中，引力场发挥全局作用，机器人在引力的作用下进行趋向目标点的运动，引力的大小与机器人和目标点之间的距离成正比；斥力场在一定范围内发挥作用，在障碍物的影响范围内，机器人在斥力作用下进行远离障碍物的运动，斥力与机器人和障碍物之间的距离成正比，所受斥力为多个障碍物斥力的矢量叠加。机器人在地图环境中所受的合力即为引力和所有斥力的矢量和，始终保持在合力的方向上进行移动，直至合力为零或抵达目标点[5,6]。

本次研究通过对当前技术手段所存在的传统人工势场法路径规划存在的问题进行分析，针对性地提出对传统人工势场法的改进方法，并用MATLAB 进行仿真实验，通过实验验证改进后算法的可行性与准确性。

一、传统人工势场法路径规划存在的问题

人工势场法是通过利用地图环境信息建立势场模型，在机器人路径规划中具有比较成熟的应用并取得了理想的效果。易先军[7]等人提出了一种模糊改进人工势场法，针对人工势场法存在的目标不可达等问题，建立斥力势场梯度等级解决目标不可达问题，同时引入基于模糊算法推算的逃逸力，避免局部最优；为克服局部极小值问题，Hongqiang Sang[8]等人提出了一种多目标人工势场算法，将地图环境划分为多个子目标势场分段实现路径规划，该算法在不同环境中均能有效发挥作用；Ulises Orozco-Rosas[9]等人提出了一种膜进化人工势场法，将膜计算与遗传算法和人工势场法结合，优化参数生成安全可行的路径。但由于势场函数定义方式的局限性，使得算法会在某些场景下因为机器人、目标点和障碍物之间的相对位置不同而存在目标不可达、局部极小值、U型障碍物导致的局部死区等问题[10]，导致机器人无法到达目标点，下面就具体问题进行说明。

（一）障碍物影响范围内的目标不可达问题。当目标点的周围存在一个及以上障碍物且目标点在障碍物的辐射范围内时，当机器人向目标点和障碍物所在区域移动时，机器人所受引力逐渐减小而斥力逐渐加大，从而可能会出现斥力大于引力的情况，导致机器人无法到达目标点。如图1所示，其中Fatt为目标点产生的引力，Freq为障碍物产生的斥力，Fres为二者合力。

图2 障碍物影响范围内的目标不可达问题

（二）局部极小值问题。局部极小值问题[11]是传统人工势场法中导致路径规划失败的主要问题。在人工势场法中，当引力和斥力位于同一直线上，且绝对值相同、方向相反时机器人所受合力为零，但此时机器人所处位置并非全局势场最低点[12]，从而导致局部极小值问题。

如图3所示，障碍物的数量为两个及以上，机器人、目标点及障碍物不在同一直线上，障碍物产生的斥力的合力与目标点产生的引力大小相同、方向相反。

图3 局部极小值问题

（三）U型障碍物导致的局部死区问题。如图4所示，当移动机机器人的运动方向上存在U型障碍物时，随着机器人与障碍物距离的逐步减小，由障碍物所产生的斥力会越来越大，当机器人进入U型区域内时，来自各个方向上的斥力的合力逐渐抵消目标点的引力作用，当斥力的合力与引力值大小相等时，机器人运动在末端震荡后陷入停滞，即陷入U型死区[13]。

图4 移动机器人进入U型死区导致路径规划失败

二、针对传统人工势场法进行改进

针对障碍物影响范围内的目标不可达问题，提出通过考虑目标点因素改进斥力函数的改进方法；针对局部极小值问题，提出引入粒子群算法优化势场参数克服局部最优的改进方法；针对U型障碍物导致的局部死区问题，提出引入切向力改进力的组成结构的改进方法。

（一）通过考虑目标点因素改进斥力函数。针对障碍物影响范围内的目标不可达问题，提出通过考虑目标点因素改进斥力函数的改进方法。机器人所受斥力是关于机器人与障碍物之间距离倒数的二次函数，当机器人进入障碍物影响范围时，随着机器人逐渐接近障碍物其斥力会迅速变大。机器人所受引力是关于机器人与目标点之间距离的二次函数，随着机器人逐渐接近目标点其引力会迅速减小。因此当目标点位于障碍物影响范围内时，在机器人逐渐接近目标点的过程中，会发生引力急剧减小而斥力急剧变大的情况，从而导致机器人无法到达目标点。为解决上述问题，提出改进的斥力函数，即在斥力函数中加入目标点因素，如式（1）所示：

Urep(q)表示斥力势能函数，ρo代表每个障碍物的影响半径；

ρ(q,qgoal)表示机器人和目标点之间的距离；n为斥力调节因子，其取值为正实数。

斥力Frep(q)为斥力场的负梯度，则

Fr1和Fr2为改进后斥力的两个分量，Fr1方向为从障碍物点指向机器人所在位置，Fr2方向为从机器人所在位置指向目标点。改进前斥力的斥力变化只与机器人和障碍物之间的距离有关，而改进之后的斥力还受机器人和目标点之间的距离影响。Fr1的作用为使机器人远离障碍物，Fr2的作用为使机器人向目标点移动。

根据斥力调节因子n的取值不同，改进斥力也会表现出不同的特性，下面分情况展开讨论：

（1）n= 0，Fr2= 0，此时

（2）0 ＜n＜1，改进后的斥力势场函数为非连续函数，在目标点位置处不可微，当机器人与障碍物之间的距离条件满足ρ(q,qobs)＜ρo且ρ(q,qobs)≠0时，改进后斥力的两个分量为：

（3）n= 1，当机器人与障碍物之间的距离条件满足ρ(q,qobs)＜ρo且ρ(q,qobs)≠0时，改进后斥力的两个分量为：

（4）n＞1，斥力势场函数处处可微。

改进后的斥力函数的优势在于额外提供一个从机器人指向目标点的力，使机器人能够克服障碍物的驱离作用，从而到达目标点。

（二）引入粒子群算法优化势场参数克服局部最优。针对局部极小值问题，提出引入粒子群算法优化势场参数克服局部最优的改进方法。人当机器人遇到合力为零的局部极小值问题时，可以通过调整参数，使合力始终大于零，从而确保机器人总是朝指向目标点位置的方向运动[14]。在改进斥力函数后，机器人所受合力为：

考虑力的方向，使机器人所受合力大于零，有

其中，ρrg=ρ(q,qgoal)，表示机器人和目标点之间的距离；ρro=ρ(q,qobs)，表示机器人和障碍物之间的距离。

考虑引力增益系数ξ和斥力增益系数η为正实数，式（10）可以改写为：

其中m=ρro-ρrg。当机器人、障碍物和目标点在同一直线上且目标点位于机器人和障碍物之间时，m表示障碍物和目标点之间的距离，且满足。

令

根据前文所述，得

其中，

则式(2-11)可以改写为

设式（16）右侧的上界为

（1）n＜2,此时

（2）n= 2，此时

（3）n＞2，此时

通过引入粒子群算法对相关参数进行优化，使算法在动态调整参数的过程中逐渐改变力的结构，从而摆脱局部困局。在人工势场法运行过程中，将参数ξ和η实时反馈给粒子群算法，粒子群算法基于式（18）、（19）、（20）的约束条件对这两个参数进行优化，直至合力不为零，即摆脱局部极小值。

（三）引入切向力。针对U型障碍物导致的局部死区问题，提出引入切向力的改进方法。针对人工势场法在局部受困从而导致路径规划失败的问题，许多研究者通过添加辅助力的方式进行解决[15]。通过引入逃逸力打破局部力的平衡，使机器人逃离受困点。本文提出引入切向力辅助机器人避开障碍的策略。

1.导航线的基本定义。如图5，定义线段Lot为起始点Po到目标点Pt的连接线，命名为导航线；定义Dp为机器人与障碍物切点Pp到导航线Lot的距离：

其中，Sotp表示由起始点Po、目标点Pt和障碍物切点Pp组成的三角形。

定义σ为路径效率因子：

2.判断当前障碍物的影响。提出一种新的障碍物规避策略，该策略主要依赖当前位置到障碍物的切点实现。借助切点的避障策略的基本原理为首先画出导航线Lot如果Lot没有与障碍物相交，那么Lot就是目标路径，否则，找到当前位置到当前障碍物的切点(将障碍物拟化为圆形)，将切点设置为当前目标，并进行后续运算。

3.借助地图信息反馈选择切点。因为点到圆的切点有两个，所以要进行切点选择。提出利用地图反馈信息来选择切点的方法，定义T为比较因子来辅助选择切点：

其中，Ep为路径效率系数，σp为路径效率因子(与p在同一侧的分支点到当前位置的路径规划效率)，w1和w2分别为Dp和Ep的权重。分别计算导航线两侧的平均效率，效率越低表示相关区域中的障碍物越多，找到更优路径的可能性越低。

4.在人工虚拟力场中引入切向力。为摆脱局部极小点，在传统势场的基础上加入切向力。在确定切点后，从当前点到切点的方向即为切向力的方向，此时合力为：

图6和图7分别表示切向力的作用位置和包含切向力的合力结构。其中，FA和Ftan分别表示传统势场合力和障碍物产生的切向力，wA和wT分别表示FA和Ftan所占的比重。当机器人位于障碍物作用范围之外时，只有切向力发挥作用，此时机器人只在切向力驱动下运动；当机器人进入障碍物的作用范围内时，机器人在障碍物所产生的斥力和切向力以及目标点产生的引力的共同作用下运动，直至离开障碍物作用范围。针对U型障碍物问题，可以将其视作圆形障碍物，通过结合切向力的作用避障。

图6 U型障碍物切向力的作用位置

图7 包含切向力的合力结构图

三、仿真实验与结果分析

为验证改进后算法的有效性，使用MATLAB软件并编写相关程序进行仿真验证。针对障碍物影响范围内的目标不可达问题、局部极小值问题以及U 型障碍物问题进行仿真，相关参数设置为斥力增益系数η=5，引力增益系数ξ=2，障碍物影响半径为ρo=2，步长为stepSize=0.1，斥力调节因子n=2，切向力相关参数w1=0.6，w2=0.4，wA=0.5，wB=0.5，迭代次数为Itemax=200。

在图8中，障碍物位于接近目标点的位置，目标点位于障碍物的影响范围内。当机器人在到达目标点和障碍物附近的范围时，由于障碍物斥力的干扰作用，机器人将在合力为零点处停止运动。使用改进后人工势场法时，由于在斥力公式中加入了目标点因素，使得机器人在干扰区域(即障碍物作用范围)内合力始终不为零且指向目标点，最终机器人成功到达目标点位置，结果如图9所示

图8 传统人工势场法路径规划失败

图9 引入斥力参数后路径规划成功

在图10中，目标点位于两个障碍物连线的垂线上，当机器人沿该垂线向目标点行进时，如图-所示，会在障碍物斥力的合力与目标点引力数值相等点处陷入局部极小值停止运动。使用改进后人工势场法，通过优化参数使机器人所受合力不为零且方向指向目标点，成功摆脱局部极小值点，抵达目标点，结果如图11所示。

图11 引入粒子群算法后路径规划成功

在图4中，地图环境中的障碍物为U形障碍物，机器人的前进方向存在U 型障碍物，当使用传统人工势场法时，机器人将进入位于障碍物内部的死区范围，导致局部震荡或运动终止。改进后通过使用引入切向力的方法，机器人在包含切向力的合力作用向下脱离局部极小值，成功抵达目标点，结果如图12所示。

图12 引入切向力后路径规划成功

通过实验仿真结果分析可知：传统人工势场法在目标不可达、局部极小值和U 型死区的情况下，移动机器人路径规划失败；而在对传统人工势场法使用了引入斥力参数、引入粒子群算法和引入切向力的算法改进，移动机器人则能够成功完成局部路径规划。

四、结论

通过分析传统人工势场法及其存在的问题，针对具体情况提出了相应的改进方法，并用MATLAB 软件进行仿真验证，仿真结果表明：改进后的算法能更好地实现局部路径规划。针对目标不可达情况，提出在斥力计算公式中引入斥力参数的改进方法；针对局部极小值问题，提出对相关参数进行优化并进行相应合力计算的改进方法；针对U型障碍物导致机器人陷入局部死区的情况，提出引入切向力的改进方法。通过MATLAB软件进行了仿真实验，所提出的方法均能够有效解决相应的路径规划问题，表明改进后的算法能够有效完成局部路径规划。

所提的改进人工势场法算法总体上能够在二维环境中实现预期目标，但仍存在一定局限性，后续应从以下两个方面继续开展研究工作：

（一）算法只在考虑二维环境因素，对环境信息的考虑比较单一，后续应综合考虑立体环境信息进行三维建图，在三维环境下测试算法的性能并进一步改进，增强算法对环境的适应性。

（二）算法只考虑了单目标的情况，在多目标条件下的规划效果有待检验，后续应考虑基于多目标情况进行试验和改进。