改进人工势场法在未知环境中的路径规划研究

陈冠星，张志安，黄学功，华 洪，施振稳

(南京理工大学机械工程学院，江苏 南京 210094)

0 引言

机器人的路径规划，是指机器人根据给定地图环境信息或者传感器探测信息寻找一条从起点到终点的无碰撞路径[1]。路径规划在无人机[2]、无人车[3]等方面都有广泛应用。

移动机器人的路径规划包括全局路径规划和局部路径规划[4]。局部路径规划可以应用在未知环境中，局部路径规划算法包括人工势场法[5]、动态窗口法[6]等。在复杂未知环境中，人工势场法因其实时性高、计算量小的优点得到广泛应用[7-8]。但人工势场法本身也存在缺陷：路径震荡和局部极小点。

对于路径震荡问题，文献[9]采用过滤震荡点法解决规划路径震荡的问题，但是其需要提前规划数个周期的路径，计算量较大。本文提出采用角度过滤器的方法改善了规划路径震荡的问题，在计算引力时采用不同增益系数的方法进一步改善了规划路径震荡的问题，最终得到了较为光滑的路径。对于局部极小点问题，文献[10]针对U型陷阱问题添加了模糊控制的思想，提出了判断U型障碍物的依据，但是当U型障碍物范围较大时，这种方法会极大增加机器人的运动路径；文献[11]采用改进人工协调场的方法来改进局部极小点问题，但是没有在更复杂的环境中进行实验；文献[12]针对传统人工势场法的缺点，使用势场强度代替力矢量进行路径规划，利用遗传信赖域算法搜索机器人周围势场强度最小点，并引入“填平势场”解决局部极值点问题。在此，本文采用添加虚拟目标点的方法引导机器人走出陷阱区域，且对多种陷阱区域进行了仿真验证。

1 人工势场法

1.1 传统人工势场法

1986年Khatb首次提出了人工势场法，其基本原理是将机器人所处环境模拟为一个人造势场，机器人所处的环境中，人工势场的势场函数定义为

U(q)=Uatt(q)+Urep(q)

(1)

机器人所受的合力为

F(q)=Fatt(q)+Frep(q)

(2)

U(q)为总的势场函数；Uatt(q)为目标点产生的引力势场函数；Urep(q)为障碍物产生的斥力势场函数；q为机器人在绝对坐标系中的位置；F(q)为机器人所受合力；Fatt(q)为目标点对机器人的引力；Frep(q)为障碍物对机器人的斥力。

人工势场受力模型如图1所示，目标点对机器人产生引力Fatt，障碍物对机器人产生斥力Frep。目标点所产生的引力会引导机器人朝着目标点运动，而障碍物产生的斥力避免了机器人与障碍物的碰撞，在引力和斥力的合力F作用下，机器人能够无碰撞地到达目标点。

图1 人工势场受力模型

目标点产生的引力势场为全局势场，且势场与机器人和目标点距离的平方成比例关系，目标点产生的势场函数和引力一般定义为

(3)

Ka为引力增益系数；qtar为目标点在绝对坐标系中的位置；ρ(q,qtar)为机器人距离目标点的距离。根据引力函数，引力的方向为机器人指向目标点，且距离目标点越远，引力越大。

障碍物产生的斥力势场为局部势场，只有在障碍物影响范围内具有势场，斥力势场函数定义为

(4)

Kr为斥力增益系数；ρ(q,qobs)为机器人与障碍物的最近距离；ρd为障碍物的影响范围。由函数可知，在障碍物的影响范围内才具有势场，机器人才会受到斥力。机器人所受的斥力函数为

Frep(q)=-Urep(q)=

(5)

1.2 传统人工势场法的缺点

机器人利用人工势场法规划路径时，在某些情况下，规划出来的路径呈现震荡状，如同“锯齿波”。例如，在机器人所受引力方向和斥力方向之间的夹角大于一定角度时，极易出现路径震荡。还有一种情况，在机器人经过狭窄通道时，规划路径也极其容易出现震荡状。如图2所示，人工势场法规划路径分别在“震荡段1”(图2中下方震荡段)和“震荡段2”(图2中上方震荡段)处出现震荡，可以看出震荡呈“锯齿波”状。

图2 规划路径震荡

此外，人工势场法因为算法本身的原因，机器人在环境中运动时会陷入局部极小点，导致机器人在局部极值点附近徘徊不前。因为人工势场中存在目标点产生的全局引力势场以及障碍物产生的局部斥力势场，机器人所受合力为势场函数的梯度，此梯度即为势能下降最快的方向，导致在局部极小点附近机器人所受合力一直指向局部极小点，因此，使机器人在局部极小点附近徘徊不前，在所受的力方面表现为机器人受到的引力与斥力共线反向。机器人陷入局部极小点状态如图3所示。

图3 机器人陷入局部极小点

当障碍物为长边界障碍物时，机器人更加容易陷入局部极小点。在机器人靠近局部极小点的过程中，机器人受到的引力方向和斥力方向之间的夹角大于一定角度时，人工势场法规划路径是震荡的，而且当机器人越靠近局部极小点，引力方向和斥力方向夹角越大，规划路径震荡程度越剧烈。机器人会陷入局部极小点，同时规划路径剧烈震荡，如图4所示。

图4 机器人陷入局部极小点

2 改进人工势场法

2.1 解决规划路径震荡

人工势场法规划路径是由机器人所受合力方向决定的。如图5所示，机器人的位置分别经过R1、R2和R3，合力分别为F1、F2和F3，合力方向不断变化，导致机器人频繁远离和靠近障碍物。合力方向变化剧烈，是由于机器人受到的引力在斥力方向上的分力Fa2与斥力Frep的合力(Fa2-Frep)方向不断变化，而合力在斥力垂直方向上的分力为Fa1，两者绝对值的比值为比重系数k=|(Frep-Fa2)|/|Fa1|。当机器人受到的引力方向与斥力方向之间的夹角越大时，k越大，规划路径越震荡。

图5 机器人受力示意

图2中“震荡段1”和“震荡段2”处的机器人受力曲线如图6所示(受力为虚拟力，无单位)。由图6可以发现，机器人所受引力基本上没有变化，机器人受到的斥力剧烈变化导致机器人受到的合力方向剧烈变化。并且根据图6可知，合力震荡的幅值越来越大，这是因为，机器人受到的斥力和引力之间的夹角越来越大，导致k值越来越大，机器人受到的合力在斥力垂直方向上的分力所占的比重越来越小。

图6 路径震荡时机器人受力曲线

为了减轻规划路径震荡程度，本文提出利用角度过滤器计算机器人下一周期运动方向。角度过滤器通过对传统人工势场法计算出来的机器人转动角度Θ乘以过滤系数f,得到最终的机器人转动角度Θ′，角度过滤器过滤系数定义为

(6)

根据角度过滤器原理，机器人下一周期的运动方向向量按如下步骤计算：

a.记录本周期机器人在绝对坐标系中的运动方向单位向量为V1，V1的方向角度(0～360°)为Θ1，通过传统人工势场算法计算得到下一周期的运动方向单位向量为V2，V2的方向角度为Θ2，计算可得V1和V2的夹角为Θ。为了方便描述，假设V1逆时针旋转Θ角后达到V2,则Θ2=Θ1+Θ。

c.定义V1和下一周期运动方向单位向量V3之间的夹角为Θ′,则Θ′=fΘ，Θ3=Θ1+Θ′。

加入角度过滤器后的路径规划效果如图7所示，与图2所示传统人工势场法的路径规划效果对比，可以看出路径已经平滑了很多。

图7 加入角度过滤器的效果

改进人工势场法规划路径的结果数据如表1所示，由表1可知，加入角度滤波器的路径规划相对于传统人工势场法来说，规划路径的总转动角度大幅减少，规划周期也有减少。

表1 路径规划结果

在添加角度过滤器的基础上，针对规划路径震荡的问题，提出在计算机器人受到的引力的分力Fa1和Fa2时采用不同增益系数k1和k2。根据前面定义的机器人引力函数和斥力函数，计算出机器人受到的引力Fa和斥力Frep。然后，如图8所示，以机器人为坐标原点Or，机器人所受斥力反方向为Y轴正方向，以Y轴正方向顺时针旋转90°方向为X轴正方向，建立斥力坐标系OrXY。

图8 机器人在斥力坐标系中的受力示意

把机器人受到的引力Fa在斥力坐标系中分解可得到Fa1和Fa2，Fa1就是引力在斥力方向上的分力，Fa2就是引力在斥力垂直方向上的分力，并设置引力在斥力坐标系中的增益系数k1和k2。按照传统人工势场法计算的引力进行分解，则k1=k2，机器人引力在斥力坐标系中表示为

(7)

(8)

如图9所示，在加入角度过滤器的基础上，计算引力时采用不同增益系数，与图7进行对比，可以看出在某些位置处算法规划路径更加平滑。

图9 进一步采用不同引力增益系数效果

在加入角度过滤器基础上，计算引力时采用不同增益系数的数据结果如表2所示。由表2可知，相比于只添加角度过滤器，进一步采用不同增益系数使得规划路径总转动角度进一步减少。

表2 采用不同增益系数的结果

2.2 解决局部极值点

人工势场法因其算法本身在路径规划过程中会出现局部极小点，会导致路径规划失败。在局部极小点处，机器人受到的引力方向和斥力方向成180°关系，尤其当环境未知，且环境中存在长边界型障碍物、U型障碍物等复杂障碍物陷阱时，则更容易陷入局部极小点而难以脱离。因此，避免局部极小点的关键就是保证引力和斥力的夹角小于180°，本文采用添加虚拟目标点来脱离局部极小点。

2.2.1 虚拟目标点确定方法

在人工势场法进行路径规划的过程中，可能因为多种类型的障碍物陷阱导致机器人陷入局部极小点，包括长边界型障碍物和U型障碍物等。如图10所示为长边界型障碍物导致机器人陷入局部极小点，在局部极小点附近，机器人受到的引力方向和斥力方向的夹角接近180°，而且合力在斥力方向的垂直方向的分力会一直吸引机器人往局部极小点处靠近，因为添加了解决震荡的措施，所以机器人一直在围绕局部极小点运动。图11所示为U型障碍物导致机器人陷入局部极小点，机器人陷入局部极小点的原因与上述原因相同。

图10 长边界型障碍物导致的局部极小点

图11 U型陷阱导致的局部极小点

人工势场法加入虚拟目标点的算法执行过程如图12所示。首先判断机器人是否到达目标点，如果机器人到达目标点，则完成路径规划，如果没有到达目标点，则判断机器人是否陷入陷阱区域。当机器人没有陷入陷阱区域时，按照原来目标点进行路径解算，当引力和斥力夹角大于170°时，本文默认机器人陷入陷阱区域，这时把陷入陷阱标志位flag设置为1。当机器人陷入陷阱区域，利用添加虚拟目标点的方法引导机器人走出陷阱区域。当机器人脱离了陷阱区域后，设置陷入陷阱标志位flag=0，继续按照原来目标点进行路径解算。

图12 加入虚拟目标点后算法执行流程

有些文献是根据机器人受力达到平衡时的特征来判定机器人陷入局部极小点的，如机器人绕局部极小点徘徊，但是这种判定会增加机器人的运动时间以及运动路径。所以在引力方向和斥力方向的夹角大于170°(机器人距离局部极小点还有一段距离)时,不管机器人运动前方是否存在局部极小点，本文都默认机器人陷入局部极小点。

放置虚拟目标点的位置按2步计算。首先，如图13所示，以机器人的全局坐标位置为原点Oq，以斥力方向为Y轴正方向，如果在机器人运动过程中机器人与障碍物的距离大于障碍物影响范围ρd，则以障碍物距离机器人最近点指向机器人的方向为Y轴正方向。以Y轴正方向顺时针旋转90°方向为X轴正方向，建立局部坐标系OqXY，这时可得虚拟目标点在此局部坐标系Y轴上的坐标为Ty=-ρd。接下来求取虚拟目标点在局部坐标系X轴上的坐标Tx，此时根据机器人运动方向在局部坐标系中的表示vq=[vq,xvq,y]来判定Tx。如果vq,x≤0,则

图13 添加虚拟目标点

Tx=-2ρd,记虚拟目标点选择方向dir=-1，表示障碍物在机器人前进路径的左侧，否则，Tx=2ρd，记dir=1，表示障碍物在机器人前进路径的右侧。

2.2.2 虚拟目标点取消方法

当采用添加虚拟目标点的方法脱离陷阱区域时，要能够判断是否已经脱离陷阱区域。如果已经脱离陷阱区域，则使用原目标点进行路径解算，否则，继续添加虚拟目标点进行路径解算。假设机器人开始采用添加虚拟目标点的规划周期为n。则判断机器人是否脱离陷阱区域的步骤为：

a.添加虚拟目标点，进行路径解算，假设机器人现在处于第j个规划周期，计算机器人相对于第j-1个周期的转动角度βj，记机器人顺时针转动时，βj<0,当机器人逆时针转动时，βj>0。

b.计算从第n个规划周期到第j个规划周期机器人转动总角度βsum=∑jβ(j≥n)。同时计算机器人距离原目标点的距离ρ(qj,qtar)，当j≥n+m时，比较ρ(qj,qtar)和ρ(qj-m,qtar)的关系，这里m取3。如果ρ(qj,qtar)≥ρ(qj-m,qtar),令j=j+1，并跳到步骤a，否则跳到步骤c。

c.如果虚拟目标点选择方向dir=-1，表示障碍物在机器人前进路径的左侧，当βsum>45°,结束添加虚拟目标点。如果dir=1，表示障碍物在机器人前进路径的右侧，当βsum<-45°,结束添加虚拟目标点。如果没有达到结束添加虚拟目标点的条件，则令j=j+1，并跳到步骤a。

机器人在虚拟目标点的引导下顺利通过U型障碍物如图14所示。

图14 机器人顺利通过U型障碍物

3 仿真结果及分析

在未知复杂环境采用传统人工势场法进行路径规划如图15所示。由图15可以看出，机器人在局部极小点附近徘徊，导致机器人无法到达目标点。图16表示采用改进算法引导机器人走出局部极小点，最终到达目标点。对比图15和图16可知,添加虚拟目标点能够克服局部极小点的缺点。

图15 传统人工势场法陷入复杂陷阱

图16 机器人在虚拟目标点引导下走出障碍物

传统人工势场法和改进人工势场法规划路径导致的机器人转动角度的对比，如图17所示，由图17可以看出到达目标点后转动角度值相差巨大。传统人工势场法和改进算法的数据结果如表3所示，由表3可知，采用改进的人工势场法后，机器人总转动角度极大减少，规划周期也稍微减少。

图17 改进算法和传统算法结果对比

表3 改进算法解决震荡的结果

图18表示在另一复杂环境中，利用传统人工势场法进行路径规划，机器人陷于局部极小点，并且在靠近局部极小点的过程中，规划路径剧烈震荡。图19为利用改进算法进行路径规划，由于设定为未知环境，所以机器人会进入障碍物围绕区域，但通过改进算法可以引导机器人走出障碍物区域，并寻求一条路径到达目标点。

图18 传统人工势场法进行路径规划

图19 改进算法进行路径规划

机器人运动到图18中a点时，传统人工势场法和改进算法的数据结果对比如表4所示。因为传统人工势场法会导致规划路径剧烈震荡，所以机器人运动到a点为止的总转动角度非常大，改进的算法可以明显减轻路径震荡，机器人运动到a点的总转动角度极大减少，且规划周期也明显减少。

表4 算法改进前后a点的结果

4 结束语

本文就人工势场法存在的规划路径震荡以及局部极小点问题进行了分析，并对这2个问题进行了改进。

a.通过本文分析可以知道，虽然人工势场法理论上不会存在规划路径震荡的问题，但是在实际应用中要对人工势场法进行离散化处理，导致机器人受到的引力在斥力方向的分力与斥力大小关系不断转变，导致规划路径震荡。因此，本文采用添加角度过滤器对机器人转角进行了限制，且考虑了机器人避障安全性，并进一步采用不同增益系数来计算引力。结果表明，规划路径震荡问题得到极大改善。

b.针对未知复杂环境下人工势场法容易出现的局部极小点问题，本文提出采用虚拟目标点引导机器人走出复杂环境，最终到达目标点。