基于预添加虚拟力的改进人工势场算法

张佳尚,陈志华

(南京理工大学 瞬态物理国家重点实验室， 南京 210094)

1 引言

路径规划是保证移动机器人自主导航能够稳定运行的关键技术之一，吸引了众多学者对路径规划算法进行研究。人工势场法(artificial potential field,APF)是由Khatib提出的一种用于机器人路径规划的算法[1]。人工势场法具有直观，对运算量要求不高，可以跟机器人的控制相结合[2]的特点，因此得到了广泛的应用。但传统人工势场法并不是完备的，在实际应用中无法避开陷入局部极小点与目标点不可达问题。解决传统人工势场法的易陷入局部极小点问题的改进方法主要分成2个方向：一种是构造合适的势场函数以减小或避免局部极小点的出现；另一种是将人工势场法与其他的算法结合[3]，从而改善传统人工势场法的缺陷。现有的研究成果中运用第一种方法的有：Liu等[4]提出的通过优化斥力势场函数，调整斥力分量在坐标轴上的方向，来解决传统人工势场法的局部极小值缺陷。Qi等[5]通过引入动态调整系数，对传统人工势场法中的引力和斥力函数进行改进，使避障安全系数更高，路径更平滑。针对目标不可达问题，在引力函数中构建新的引力势场，能够高效地解决无人机的路径规划避障问题。Zhang等[6]提出了一种在斥力函数中加入一个绕行力来改进斥力函数的方法，同样可以有效地解决局部极小点问题。运用第2种方法的有：郭枭鹏[7]提出融合人工势场法与快速扩展随机树算法(rapidly-exploring random tree,RRT)的特性，通过RRT算法与人工势场法优势互补同时解决各自的缺陷，通过多个算法的对比实验，证明了APF-RRT算法具有良好的适用性。本文的改进方向是基于第2种改进方法，通过将预添加虚拟力的算法与传统人工势场算法结合改良传统人工势场法的缺陷。通过设置的五组仿真实验结果与传统人工势场法的仿真结果对比，分别验证了在环境中存在一个与多个障碍物的情况下基于预添加虚拟力的改进人工势场算法的可行性与有效性。证明了改良算法能够解决传统人工势场算法易陷入局部极小值与目标点不可达的问题。相较于其他的第二种改良方向的方法，例如通过设置虚拟目标点、人工势场法结合蚁群算法[8]、人工势场法结合粒子群算法[9-10]、人工势场法结合快速扩展随机树算法[11]等改良方向，本文提出的改良方法具有计算量小，规划速度快，实时性好，规划路径更短等优点。

2 人工势场法

2.1 基本原理

人工势场法是一种通过将障碍物和目标点抽象为斥力与引力虚拟势场的算法，让目标点处于谷底，距离目标点越远的位置上引力势场越高，同时为了避免碰撞，在障碍物四周添加排斥势场[12-13]。图1为利用传统人工势场法得到的势场中的轨迹，其中蓝色箭头为势场线，箭头越长代表该位置的引力或斥力越大，红线为规划的轨迹。机器人距离目标点越远，受到目标点的引力越强，机器人在障碍物影响范围内离障碍物越近受到的排斥力越强，当机器人不在障碍物影响范围内时不会受到排斥力。人工势场法会根据机器人在势场中的受力情况，沿着所受到的引力与斥力合力的方向自主规划出一条能够到达目标点的最优路径。

2.2 人工势场数学模型

人工势场法模型分为引力场模型与斥力场模型。

引力场与引力模型：

(1)

(2)

其中：Uatt为引力势场强度；Fatt为机器人所受的引力；katt为引力势场增益系数；dg为机器人到目标点的直线距离；grad(·)为求·的梯度；m为常数，通常取值为2；eg为机器人到目标点的单位矢量。

排斥势场与斥力模型：

(3)

(4)

(5)

其中：kUrep为第k个障碍物的排斥势场强度；krep为排斥势场增益系数；kdo为机器人到第k个障碍物的直线距离；dd为障碍物的影响范围；keo为机器人到第k个障碍物的单位矢量；kFrep为第k个障碍物对机器人的排斥力；Frep为机器人在势场中受到的所以排斥力的合力。

合势场与合力模型：

Utol=Uatt+Urep

(6)

(7)

其中：Utol为合势场强度；Ftol为机器人所受的合力。

3 改进方法

传统人工势场法常面临的问题有：

1) 易陷入局部极小点[14]。即当势场中出现除目标点外其他合力为零的点，使机器人在到达该点后无法再向目标点移动的现象。

2) 当目标点在障碍物影响范围内时可能存在目标点不可达的问题[15]。即当机器人接近目标点时，其所受到的引力会逐渐减小，若此时目标点恰好在障碍物的影响范围内，其所受到的排斥力会因机器人靠近目标点的同时靠近障碍物而不断增大且与引力方向相反，就会使机器人停在某一点(局部极小点)或发生抖动，导致无法到达目标点的现象。目标点不可达问题可以看作是陷入局部极小点问题的特例，不同于其他陷入局部极小点问题的是此时的目标点不再是全局极小点，使得机器人到达不了目标点而陷入局部极小点。

传统人工势场法存在着缺陷，在应用中会面临很多问题导致机器人无法安全到达目标点，要想在实际中运用必须进行改进，在本文中提出了将传统人工势场算法结合预添加虚拟力的方法改良传统人工势场法的缺陷，具体改进方法见下文。

3.1 解决局部极小点问题的方法

针对易陷入局部极小值点的问题，提出的解决方法如下：

但丁韪良也是京师同文馆的教习，其译文表述必须借助中国译员的协助，方能将浸淫传统理学、只识文言文的清朝大员纳入目标读者群的序列。这就给了本土文化的主导意识形态控制译者声音的可乘之机。毕竟以培养中国译员为目标的京师同文馆本身即清政府的官办机构。

第1步：当利用传统人工势场算法达到设置的最大迭代次数但规划的轨迹未达到目标点时判断目标点处的合势场是否为零，若为零则进行第2步。若不为零，则可得知系统陷入目标点不可达问题，直接结束此进程，进入解决目标点不可达问题的子程序。

第2步：通过给原人工势场预添加几个指定方向的虚拟力(具体方法在下文给出)，得到机器人在势场中受到的新合力，根据新合力继续规划路径，使机器人能够逃离局部极小点，到达下一个极小点(可能为目标点)且不碰撞障碍物，得到几条可行的路径。

第3步：若出现几个预选方向到达的下一个极小点仍是局部极小点而存在其他预选方向可到达的下一个极小点是目标点则将再次陷入局部极小点的几条路径重新执行第2步。在舍弃不合理的预添加虚拟力的方向后规划出可以到达目标点的几条轨迹。然后分别计算每条预添加虚拟力方向的预规划路径的路程，选取其中路程最短的方向确定为最优虚拟力方向，若出现两条路程相同的路径选取预添加虚拟力方向与用传统人工势场法规划出的陷入局部极小点的路径间夹角最小的一条路径，若为对称路径选择改进算法先得到的一条路径，然后结束算法进程。

预添加虚拟力方法中最重要问题的是如何选择合适的时机去掉预添加虚拟力。在本文中选择当达到设定的最大迭代次数或新规划的轨迹点与目标点的连线之间没有障碍物时，则去掉预添加虚拟力，此方法可有效避免与障碍物发生碰撞并减少冗余路径，得到最优路径。若想使预添加虚拟力的方向更接近最优，可以通过更加细化等分预添加虚拟力的角度的方法，并且等分的角度越小预添加虚拟力的方向就越精确，但是如果细化等分的角度过小，将会延长规划多个方向路径与计算路径长度的时间，从而在一定程度上影响改进人工势场法避障的实时性。

3.2 解决目标点不可达问题的方法

针对目标点在障碍物附近时可能存在目标不可达的问题，提出的解决办法如下：

若通过上面3.1节中的第一步判断出目标点处的合势场不为零，则可判断为遇到了目标点不可达问题。改进算法选择将在目标点处给目标点施加排斥势场的障碍物在势场中先去除掉，然后在原轨迹的基础上规划出一条能够到达目标点的预规划路径。再重新将障碍物放回原势场中，若预规划路径与障碍物无碰撞，则可将预规划路径作为最优路径。若预规划路径与障碍物发生碰撞，则将预规划路径与障碍物影响范围重合的部分路径进行重新规划，此时运用预添加虚拟力的方法规划出重合部分的新路径，将新路径与原规划路径结合作为最终的规划路径。

4 仿真实验

以下运用Matlab 2020b软件进行仿真，利用上述改进方法对各种不同的障碍物环境下改进算法的有效性进行验证。在仿真实验中人工势场各参数设置为：katt=0.001，m=2，krep=800，dd=15 cm。仿真结果图中X坐标与Y坐标单位为厘米。

4.1 单障碍物陷入局部极小点的场景

为验证在环境中存在一个障碍物的情况下机器人陷入局部极小点时改进算法的优化效果，设计了两组仿真实验。

第1组仿真实验设置机器人的起始点位置坐标为(10，10)，目标点位置坐标为(360，360).仿真环境中存在一个圆形障碍物，其坐标为(250，250)，半径为30 cm，障碍物影响范围为15 cm。

图2为运用传统人工势场法进行路径规划得到的二维势场中的轨迹图，图2中蓝色圆点为起始点，绿色圆点为目标点，红线为规划的轨迹，大小不一的箭头为势场线。从仿真结果可以看出运用传统方法规划的轨迹陷入了局部极小点，未能完成到达目标点的任务。

图2 传统APF解决第一组实验时势场中的轨迹图

图3为运用预添加虚拟力改进算法后得到的轨迹图，图中黑色圆形为障碍物所在的位置，图4为运用预添加虚拟力改进算法进行路径规划得到的二维势场中的轨迹图。从图3可以看出，运用改进算法规划的轨迹能够与障碍物保持足够的距离，并沿着障碍物的排斥势场边缘规划出平滑的路径。结合图3与图4可知当运用传统人工势场法进行轨迹规划陷入局部极小值时，利用预添加虚拟力的改进算法可以有效地使机器人逃离局部极小点，同时在保证不与障碍物发生碰撞的情况下降低冗余路程，规划出一条能够到达目标点的轨迹，证明了提出的基于预添加虚拟力的改良方法在解决传统人工势场法易陷入局部极小值问题的可行性。

第2组仿真实验设置机器人的起始点位置坐标为 (80，200)，目标点位置坐标为(370，200)。仿真环境中存在一个由3个矩形障碍物拼接而成的C型障碍物，障碍物影响范围为15 cm。

图3 改进算法解决第一组实验时的轨迹图

图4 改进算法解决第一组实验时势场中的轨迹图

图5为运用传统人工势场法进行路径规划得到的二维势场中的轨迹图，从仿真结果可以看出传统势场法规划的轨迹由于目标点与起始点的连线与机器人受到的障碍物的排斥力处于同一水平线，没有垂直方向的分力，所以在引力与斥力平衡后陷入了局部极小点，使得机器人未能到达目标点。图6为运用改进算法得到的轨迹图，图7为运用改进算法进行路径规划时二维势场中的轨迹图。从仿真结果可以看出在机器人陷入局部极小点后，通过添加的虚拟力能够摆脱局部极小点，在与障碍物保持一定距离避免碰撞的同时，沿着障碍物的轮廓走出障碍物凹陷部分，最终在撤去虚拟力后，在人工势场力的作用下顺利到达目标点。仿真结果表明在复杂的障碍物环境下改进算法依旧可以克服传统人工势场法易陷入局部极小点的缺陷。

图6 改进算法解决第二组实验的轨迹图

图7 改进算法解决第二组实验时势场中的轨迹图

4.2 目标点不可达的场景

为验证运用传统人工势场法进行路径规划陷入目标点不可达问题时改进算法的优化效果，设计了一组仿真实验。障碍物的环境情况为起始点与目标点之间有障碍物相隔，但目标点在障碍物影响范围内。仿真时设置的机器人的起始点位置坐标为(10，10)，目标点位置坐标为(337，350).仿真环境中存在一个圆形障碍物，其坐标为(300，330)，半径为40 cm，障碍物影响范围为20 cm。

图8为利用传统人工势场法进行路径规划得到的仿真结果。从仿真图可知轨迹陷入了目标点不可达问题，出现此问题的原因是目标点在障碍物影响范围内，机器人所受到的斥力会因靠近障碍物而增大，并与其受到的引力相平衡，使得规划的轨迹未能顺利到达目标点。图9为利用改进算法时得到的势场中的轨迹图，从仿真结果可以看出由于引入了虚拟力的原因，机器人克服了障碍物的排斥力，逃离了局部极小点(合力为零点)，成功到达了目标点。证明了改进算法在解决传统人工势场法易陷入目标点不可达问题时的有效性。

图8 传统APF解决问题时势场中的轨迹图

图9 改进算法解决问题时势场中的轨迹图

4.3 多障碍物陷入局部极小点的场景

为验证当环境中存在多障碍物情况下改进算法的性能，设计了两组多障碍物场景的仿真实验。

第一组仿真实验设置机器人的起始点位置坐标为(10，10)，目标点位置坐标为(360，360).仿真环境中存在3个障碍物，其坐标分别为(210，210)、(280，320)、(310，230)，半径依次为30 cm、35 cm和30 cm，障碍物影响范围都为15 cm。

图10为利用传统人工势场法进行路径规划得到的势场中的轨迹图，从图中可以看出利用传统人工势场法进行路径规划时会陷入局部极小点，未能完成路径规划的任务。图11为利用改进算法进行路径规划得到的势场中的轨迹图，从仿真结果可知引入预添加虚拟力的人工势场法能够有效地避免陷入局部极小点，在不与障碍物发生碰撞的情况下，规划出一条最优路径，证明了此改进方法在多障碍物环境下应用的可行性。

图10 利用传统APF时势场中的轨迹图

图11 利用改进算法时势场中的轨迹图

为验证改进算法在更加复杂的障碍物环境下的优化效果，设计了第二组仿真实验。实验设置机器人的起始点位置坐标为(10，10)，目标点位置坐标为(460，460)。仿真环境中存在15个不同形状的障碍物，障碍物的影响范围都为15 cm。

图12为利用传统人工势场法进行路径规划后得到的轨迹图，从图中可以看出利用传统人工势场法进行路径规划时会因仿真轨迹图轨迹末端左上角的圆形障碍物与右下角的方形障碍物的排斥合力作用与来自目标点的引力平衡，而陷入局部极小点，未能规划出到达目标点的路径。图13为利用改进算法进行路径规划得到的轨迹图，从仿真结果可以看出原人工势场作用力在结合了东南方向的虚拟力后，可以逃离局部极小点，到达目标点。证明了改良算法多在障碍物的复杂工况下依旧能够改良传统人工势场法的缺陷，规划出合理的轨迹。

图12 利用传统APF时的轨迹图

图13 利用改进算法时的轨迹图

5 结论

在本文中提出了将预添加虚拟力的方法与传统人工势场法结合的想法，对传统人工势场法进行改进。通过设计的5组Matlab仿真实验，验证了在环境中存在一个障碍物与多个障碍物的复杂环境情况下，改进算法能够逃离局部极小点与目标点不可达问题，同时降低冗余路程，并在避免与障碍物发生碰撞的前提下规划出一条到达目标点的合理轨迹，证明了改进算法可以有效地改良传统人工势场法的缺陷，具有一定的可行性。