改进人工势场法的无人机避障路径规划*

涂 柯，侯宏录，苏 炜

(西安工业大学 光电工程学院，西安 710021)

随着智能技术的发展，无人机被广泛应用于军事、农业、物流和其他任务中[1]。无人机作为代表，在众多领域发挥着重要作用，但在实际应用中值得关注的是安全问题，而避障路径规划是解决安全问题的重要手段[2-4]。无人机自主避障能力对无人机任务完成产生重要影响。

文献[5-7]提出的人工势场法，满足无人机实时避障路径规划要求，可规划出较平滑的路径躲避障碍物，且算法所需时间短、满足实时性。但该方法应用在无人机工作场景中，进行无人机的在线路径规划躲避障碍物时，存在以下问题：目标附近存在障碍物时，无人机所受引力小于斥力，在目标附近震荡，无法到达目标位置[8]；目标引力与障碍物斥力大小相等，方向相反，无人机所受合力为零，陷入局部极小值陷阱[9]；由于障碍物的影响，使得无人机在躲避障碍物时偏离原直线路径的避障路线，路径长度大幅度增加，导致路径过长。文献[10]在无人机陷入局部极值陷阱时，引入惩罚势函数代替传统斥力势函数，并采用模拟退火法来搜索路径，有效避免了局部最小点和震荡点，但引入太多信息导致计算量及路径长度的增加；文献[11]针对路径震荡问题，设置探测窗口判断是否激活衰减函数，消除狭窄通道处轨迹振荡现象，针对目标不可达问题，设置无人机与目标点距离因子，调节斥力场系数，有效解决目标附近存在障碍物而致使无法到达情况，但该法存在部分较陡峭的弯角路径；文献[12]针对局部极小值陷阱问题，在斥力势场函数中加入航迹点和目标点之间的距离，同时引入了协调力帮助无人机逃离局部极小值陷阱；文献[13]针对局部极小值问题，设计避障预防检测策略，对无人机飞行方向进行实时校正，并通过对无人机在局部极小值点附近的飞行方向和步长调整，实现无人机自主避障功能，但验证方案简单且只在二维环境中验证；文献[14]引入斥力偏转模型，解决局部极小值问题，并引入斥力增益系数，优化航向过大问题；文献[15]用正六边形引导方法来改善局部极小值问题，并在排斥势场函数中添加距离校正因子，有效改善局部极小值问题，但仍然存在路径震荡及转弯角度过大情况；文献[16]将机器人与目标点之间的距离引入斥力场函数中，改变排斥力方向，避免机器人陷入局部极小点而引起的停滞不前问题，但仍存在路径震荡及路径长度增加问题。目前大多避障路径规划都是基于二维空间中，而无人机工作环境为三维工作环境，且复杂程度、不确定程度更高，对避障路径要求更高。

本文针对传统人工势场法避障路径规划时会出现路径过长、目标不可达以及易局部极小值陷阱问题做出改进，并将其推广至三维空间中。

1 人工势场法分析

人工势场法将无人机飞行环境虚拟为力场环境，不受障碍物数量影响，且算法所需时间短、满足实时性，其原理是视目标位置为负电荷，对无人机产生引力;视障碍物为正电荷，产生斥力；各障碍物斥力及目标引力之和为无人机所受合力[17-19]，图1为无人机受力示意图。

图1 无人机受力示意图

人工势场法势场函数为

(1)

Urep(Xp)=

(2)

式中：Xp=(x,y,z)为无人机当前位置，Xob=(xob,yob,zob)为障碍物位置坐标；Xgoal=(xgoal,ygoal,zgoal)为目标位置；katt、krep为引力、斥力势场系数；Uatt(Xp)、Urep(Xp)为引力、斥力势场函数；ρ(Xp,Xgoal)=‖Xp-Xgoal‖为无人机与目标点相对距离；ρ(Xp,Xob)=‖Xp-Xob‖为无人机与障碍物相对距离；ρ0为障碍物最大作用范围，超过该范围时不产生影响。

对势场求取负梯度可得

Fatt(Xp)=-(Uatt(Xp))

(3)

Frep(Xp)=

(4)

引力与斥力的叠加为无人机所受合力，则合力为

(5)

2 改进人工势场法

2.1 求解目标不可达

如果目标附近存在障碍物，无人机向目标飞行，则会造成无人机接近目标时，其自身所受引力小、斥力大情况，合力方向背离目标位置，无人机在合力驱使下远离目标行驶，无人机会驶出一定的距离，所受引力增大，斥力减小，又逐渐接近目标位置，反复在目标位置附近徘徊震荡但却不能到达目标位置。

为解决该问题，将无人机与目标位置相对距离引入到斥力势场中[20-21]，斥力势场函数如下：

Urep(Xp)=

(6)

式中:t为调节因数，可变化的正常数。求取改进斥力势场负梯度，可求得斥力为

Frep(Xp)=-Urep(Xp)

(7)

式中：

(8)

(9)

改进后斥力矢量分量Frep1(Xp)方向由障碍物远离方向指向无人机；改进后斥力矢量分量Frep2(Xp)方向由无人机指向目标位置方向，与引力同向。无人机驶向目标位置时，无人机与目标位置之间距离逐渐减小。当n∈(0,1)时，ρ(Xp,Xgoal)→0,斥力分量Frep1(Xp)→0,斥力分量Frep2(Xp)→∞，在引力Fatt(Xp)和斥力分量Frep2(Xp)作用下，即使目标位置周围存在障碍物，无人机仍然能朝目标位置飞行，并且方向背离障碍物，在人工势场总和力的作用下无人机可以安全的到达目标位置，完成飞行任务。

2.2 基于调控力的局部极小值抑制算法

分析无人机受力情况可知，当斥力与引力大小相等方向相反时，会出现局部极小。本文利用调控力以解决无人机局部陷入极小值引起的受力平衡状态。

若在所有位置引入调控力，则会导致避障路径的增加，与最优路径要求不符；故只需在陷入局部极小时引入调控力，使无人机逃离局部极小陷阱。因此需检测到无人机陷入局部极小值点，引入调控力，使无人机逃离陷阱。

定义夹角θF，为各障碍物产生的合斥力与引力的夹角，极小值检测条件为

(10)

式(10)成立时，判定无人机陷入极小值陷阱，设定的ε≥0值越小，判定越灵敏，误判率越低。经过多次在Matlab中仿真实验，本文将ε设定为π/90时，可实现算法最优实现。若式(10)成立，将引入调控力进行逃离极小值陷阱。

在无人机陷入局部极小值陷阱时，引入调控力

(11)

(12)

μ为调控系数，调控系数定义为

(13)

在调控力的作用下，无人机将打破局部极小状态，逃离局部极小值陷阱后，取消调控力，在引力和斥力作用下，无人机会继续朝目标位置飞行。

2.3 基于检测因子的路径优化策略

在无人机的人工势场法避障路径规划中，若无障碍物，无人机沿直线路径前进，快速安全到达目标点。但由于存在障碍物的影响，使得无人机在躲避障碍物时难免规划出偏离原直线路径的避障路线，路径长度大幅度增加，从而导致路径过长。无人机受无人机自身及载重影响，续航能力有限，因此路径的长度必须小于或等于无人机可飞行的最大距离。

针对路径过长的问题，躲避障碍物时，避免无效斥力,检测因子如图2所示。

图2 检测因子示意图

在障碍物躲避过程计算障碍物斥力时，首先引入检测因子Si，Si定义如下：

Si=|ρi(X,X0)sinθi|,

(14)

式中：ρi(X,X0)为无人机与第i个障碍物之间距离；sinθi为无人机与第i个障碍物之间连线和无人机与目标位置连线夹角正弦值。

当Si大于无人机行进安全距离R，无人机依当前行进路线前进不会与该障碍物发生碰撞，判定该障碍物为无效障碍物，不考虑该障碍物所产生斥力，当Si小于无人机行进安全距离R，无人机依当前行进路线前进会与该障碍物发生碰撞，判定该障碍物为有效障碍物，计算斥力时需考虑该障碍物所产生斥力。则引入距离因子后的斥力公式为

(15)

当障碍物不构成当前路径威胁时，忽略该障碍物影响，继续沿原路径前进，引入Si后避障路径相较于传统人工势场法更加平滑且路径较短。

3 仿真验证与分析

为验证改进人工势场法的可行性，分别设置造成目标不可达、局部极小值、影响路径长度的标志性障碍物，并对传统算法和改进算法做对比，并进行模拟仿真实验。主要参数设定见表1。

表1 改进算法验证基本参数

3.1 相对距离求解目标不可达仿真验证

验证加入目标与无人机距离改进斥力函数解决目标不可达问题，并与传统算法进行对比。在目标位置附近设置障碍物，且离目标位置非常相近，障碍物位置见表2，目标位置为(10,10,10)，距离目标最近的障碍物为5号障碍物，该障碍物位置为(10,9.5,10)，其与目标位置之间距离为0.5 m，与目标点距离小于障碍物影响距离，会导致无人机飞行至目标点附近时，障碍物产生斥力大于目标点所产生引力造成目标不可达现象。

表2 验证目标不可达问题障碍物参数

由图3可看出，传统算法下无人机飞行至目标点附近时受障碍物5影响，在目标附近震荡，但无法到达目标位置；而在图4改进算法下，无人机可顺利抵达目标位置。表明改进算法可以有效解决目标位置无法到达问题。

图3 改进前无法抵达目标点

图4 改进后抵达目标点

3.2 调控力极值抑制算法仿真验证

针对局部极小值验证，本文引入调控力逃离局部极小值陷阱问题做出验证，将加入调控力算法与传统人工势场算法进行对比。无人机起始位置为(0，0，0)，目标位置为(10，10，10)。障碍物位置见表3。1号障碍物位置为(4，4，4)，无人机行进到该障碍物附近时，目标点引力与障碍物1所产生斥力大小相等方向相反，无人机此时合力近乎为零，无人机陷入局部极小值陷阱。

表3 验证局部极小值问题障碍物参数

由仿真结果看出，图5传统人工势场法算法下，在行进到(3.5，3.5，3.5)位置附近时，陷入局部极小值陷阱中，无法逃离，无人机陷入停滞不前状态导致无人机避障失败。

图5 改进前陷入极小值陷阱无法逃离

而在图6，在行进到(3.5，3.5，3.5)位置，无人机陷入局部极小值陷阱后，由于调控力的作用，无人机能逃离极小值陷阱，到达目标位置。在引入调控力后，无人机能够检测到局部极小值陷阱，并可逃离该陷阱，仿真结果表明改进后算法可解决局部极小值陷阱问题。

图6 改进后可逃离极小值陷阱

3.3 检测因子路径优化策略仿真验证

针对路径过长问题，验证加入检测因子后优化路径长度方法的有效性，将传统法与引入距离因子后的人工势场法躲避障碍物时避障路径规划情况做对比。无障碍物时，无人机沿直线向目标位置飞行，当Si>R时，所产生斥力为无效斥力，反之，为有效斥力。障碍物参数见表4。障碍物1、2、3、4相应的距离因子Si均大于R,障碍物5距离因子Si小于R，则在改进后的算法中，无人机应只受障碍物5的影响。

表4 验证路径过长问题障碍物参数

图7和图8为两种算法对比结果，图7为传统法路径，可以看出，在遇到障碍物时，传统法由于在障碍物斥力影响下，路径发生变化，“绕路”躲避障碍物；图8为改进法路径，1～4号障碍物检测因子都大于R,无人机可安全通过，改进法将其视为无效障碍物，不考虑其影响，而障碍物5检测因子Si小于R，视为威胁障碍物，考虑障碍斥力影响躲避障碍物，仿真结果表明改进算法可明显减小路径长度，可降低优化避障路径。经分析多次仿真实验数据可得，改进后算法可减少路径长度9%～15%。

图7 改进前路径

图8 改进后路径

4 结 论

针对人工势场法在无人机避障路径规划时的路径过长问题、目标不可达问题以及局部极小值陷阱问题做出改进。针对目标不可达问题，引入无人机与目标之间相对距离到斥力势场函数中，减小目标附近障碍物斥力解决目标不可达问题；针对极小值陷阱问题，在无人机陷入局部极小值陷阱后，引入调控力帮助无人机逃离，以该方法解决局部极小值陷阱问题；针对路径长度问题，将检测因子引入到斥力函数中，检测障碍物斥力是否为无效斥力，排除无效斥力影响，优化无人机避障路径减小路径长度，减小路径长度9%～15%。通过理论分析与传统人工势场法仿真对比，得出结论，改进算法有效解决了上述传统人工势场法存在的目标不可达、局部极小值陷阱、路径长度长问题，提升了无人机工作安全性的同时提高了适应性，使其在无人机避障路径规划中具有应用价值。