改进人工势场法在无人机路径规划中的应用研究

巫 茜，曾 青，罗金彪，邝 茜，黄 浩

(重庆理工大学 计算机科学与工程学院， 重庆 400054)

随着控制论、材料学、计算机与通信等相关学科技术的不断进步，无人机的性能得以大幅提升，出现了大量形态各异但制造成本不断降低的无人机，这使得无人机开始大规模普及，在军事和民用领域发挥了巨大作用[1-3]。自主与智能控制是无人机未来的发展趋势[4-5]，其中路径规划是提高无人机自主飞行水平和保障飞行安全的一项重要技术，已经成为了无人机领域的研究热点。

路径规划指的是在一定约束条件下，为无人机规划从起点到目标点的最优或可行航迹[6]。针对路径规划问题，国内外学者开展了许多工作，提出了快速扩展随机树法、人工势场法、A*算法、蚁群算法、粒子群算法、遗传算法等众多方法[7-8]。在现有方法中，人工势场法具有实现简单、实时性高、生成的路径安全平滑等优点，因此在实时在线路径规划问题上得到了广泛应用。但该方法也存在目标不可达、易陷入局部极小值、局部震荡等不足[9]。面对上述不足，学者们进行了深入研究，并提出了众多改进方法。Sun等[10]通过改进斥力势场函数和动态步长调节策略来解决目标不可达和局部震荡问题，但仅在二维环境下进行简单仿真，且没有考虑局部极小值问题，不能满足无人机在复杂三维环境下的飞行要求。马娅婕等[11]将三维空间拆分为XOY和XOZ2个平面，对斥力势场和引力势场都进行了改进，以规避位于预设航迹上的障碍物并消除局部极小值带来的影响，但计算量较大，实时性较低。付泽民等[12]和梁献霞等[13]通过引入虚拟障碍物，解决了局部极小值问题，但文献[12]中需要根据局部极小值附近的障碍物位置信息计算出虚拟障碍物的位置，存在一定局限性，且作者没有给出选取障碍物的范围以及计算方法，文献[13]在局部极小值点处需要依次扫描60个扇区，当环境存在多个局部极小值且总是在排序靠后的扇形中找到虚拟障碍物时，该方法会带来计算量过大的问题。唐嘉宁等[14]借鉴电势场提出了模拟等势线的概念，专门用于解决连续型障碍物带来的易陷入局部极小值问题，但存在转向角过大、路径长度过长等问题。谌海云等[15]采用“沿目标方向90°移动”策略以跳出局部极小值，与文献[14]类似，也会造成路径中存在大角度转向的问题，需要进一步平滑路径后才能供无人机使用。

针对上述算法的缺点，本文在充分利用三维空间的基础上，提出了一种改进人工势场法，该算法主要用于规划旋翼无人机和微小型固定翼无人机在低空复杂三维环境下的路径。改进方法是：① 通过在斥力势场函数中引入无人机与目标点的相对距离来解决目标不可达问题；② 提出三维目标分段法解决局部极小值和局部震荡问题。在Matlab仿真软件中进行了仿真实验，仿真结果表明，提出的改进方法计算量小，能使无人机快速且连续摆脱多个局部极小值，在三维复杂环境下顺利到达目标点。

1 传统人工势场法基本原理

人工势场法的基本原理是构建一个虚拟势场，由目标点对无人机持续产生引力作用，障碍物在一定范围内对无人机产生斥力作用，根据平行四边形法则产生二者的合力，无人机最终将在合力作用下到达目标点。

人工势场的构建依赖引力势场函数和斥力势场函数，分别如式(1)和式(2)所示。

(1)

(2)

式中：X、Xg、Xo分别代表无人机、目标点和障碍物所在位置，α为引力增益系数，β为斥力增益系数，ρ(X，Xg)为无人机与目标点之间的欧氏距离，ρ(X，Xo)为无人机与障碍物之间的欧氏距离，ρo为障碍物对无人机产生斥力作用的最远距离。

对式(1)和式(2)求负梯度，可以得到相应的引力函数和斥力函数，分别如式(3)和式(4)所示。

Fatt(X)=-▽Uatt(X)=αρ(X,Xg)

(3)

(4)

最终求得的合力如式(5)所示。

Ftotal(X)=Fatt(X)+Frep(X)

(5)

图1直观地展示了无人机在人工势场法中的受力情况。障碍物1和障碍物2分别产生斥力Frep1和Frep2，障碍物对无人机的总斥力为Frep，而目标点对无人机的引力为Fatt，最终的合力为Ftotal。

图1 人工势场法中无人机受力分析示意图

2 传统人工势场法缺陷分析

2.1 目标不可达问题

物体在目标点附近因斥力大于引力，无法到达目标点的情况被称为目标不可达问题[16]。如图2所示，当目标点与障碍物比较接近时，随着无人机越来越靠近目标点，也越来越靠近障碍物，因此ρ(X，Xo)和ρ(X，Xg)递减。由式(3)和式(4)可知，斥力Frep逐渐增大，而引力Fatt却逐渐减小；越靠近目标点，斥力越大，以至于斥力大于引力，导致无人机无法到达目标点。

图2 目标不可达问题示意图

2.2 局部极小值与局部震荡问题

传统人工势场法的另一个主要缺陷是存在局部极小值和局部震荡问题[17-18]。原因在于人工势场法中可能存在局部极小值点，当规划过程中遇到这样的点时，无人机受到的合力接近或等于零，造成规划终止；而在遇到以U型障碍为代表的非凸障碍物时，斥力作用时而大于引力作用，时而小于引力作用，或者斥力作用一直占主要地位，但其方向频繁大幅改变，从而导致规划的路径上产生局部震荡现象。

典型的局部极小值问题如图3所示。在图3中，无人机、目标点以及障碍物在同一直线上，该直线上必有一处引力和斥力大小相等而方向相反的点，此时合力为零，形成局部极小值点，无人机在该点处将停滞不前。

图3 典型局部极小值问题示意图

图4为典型的局部震荡现象示意图，两障碍物组成狭窄通道，导致无人机在通过该通道时的前进方向频繁大幅改变，因机动性能有限，无人机很有可能无法按该路径飞行。

图4 典型局部震荡现象示意图

3 传统人工势场法改进策略

3.1 改进斥力势场函数

为解决目标不可达问题，可将无人机与目标点之间的相对距离引入斥力势场函数[13,15]。改进后的斥力势场函数如式(6)所示，式中n可取任意大于零的常数。

(6)

对式(6)所示的斥力势场函数使用复合函数链式求导法则，可得斥力函数，如式(7)—(9)所示。

(7)

(8)

(9)

式(7)中的F1和F2分别如式(8)和式(9)所示，F1和F2中的偏微分都是单位向量，均指向无人机。

改进后的斥力将受无人机与目标点之间距离的影响，确保无人机在接近目标点时的斥力作用小于引力作用，使无人机能够到达目标点。

3.2 三维目标分段法

考虑到无人机在复杂环境下可能遇到多个局部极小值点或局部震荡问题，无法直接到达目标点，提出一种将路径分段的方法，通过在合适的位置设定子目标点，以暂时取代目标点对无人机产生引力作用，保证无人机能到达子目标点，进而到达最终的目标点。

首先需要检测无人机是否陷入局部极小值点或产生了局部震荡现象。由于人工势场法在每一轮迭代后，按照固定步长，在合力指向的方向下得到下一个路径点，因此可以通过判断第i(i≥ 2)次迭代时的位置和第i+1次迭代时的位置距离是否为零，第i-1次和第i+1次迭代时的位置距离是否小于移动步长来检查无人机是否陷入局部极值或震荡。当条件符合时，采用三维目标分段法。

借鉴二维平面下通过圆来寻找虚拟目标点的过程[19-20]，三维目标分段法中，需要先通过向量运算求出一个圆，然后取圆上一点作为子目标点。设空间中的局部极小值点或开始震荡的点为(x1，y1，z1)，目标点为(x2，y2，z2)，连接上述两点，可以得到一条线段，将其命名为L，设线段L上与局部极小值点一端接近的三等分点为(x3，y3，z3)，可以由式(10)求得该三等分点的位置。

(10)

以三等分点(x3，y3，z3)为圆心，k倍步长为半径r，在空间中可得无数个圆。选择与线段L垂直的圆作为产生子目标点的圆，这样做可以确定唯一一个圆，大大减少了搜索空间，而且能确保产生的子目标点位于局部极小值点和目标点之间，符合航迹长度尽量短的原则。将与线段L垂直的圆所在的平面命名为A，将平面A的法向量命名为n，n可由目标点和式(10)所求得的三等分点得到，设n=(n1，n2，n3)；取平面A中的任意向量，将其命名为m，设m=(m1，m2，m3)，由于n是法向量，因此m与n正交。根据向量正交相乘结果为零的原则，可以由n求得m，设n的一个坐标点为零，其余2个坐标点互换位置，符号相反即可，例如m=(-n2，n1，0)，但n12与n22之和不能为零，否则由于z坐标为零，m将是一个零向量。

(11)

在式(11)中输入一个角度值θ，可以求得圆上一点，拟将该点作为子目标点，但还需要判断该点与障碍物之间的关系。求该点与障碍物之间的距离d，若该距离大于障碍物碰撞半径，则该点成为子目标点，否则增大圆的半径，另求一个点作为备选的子目标点，直到找到符合条件的子目标点。图5展示了三维目标分段法中的子目标点与其他物体的位置关系。

图5 三维目标分段法中的子目标点示意图

4 路径规划仿真及结果分析

4.1 改进斥力势场函数路径规划仿真

为了验证改进斥力势场函数的有效性，使用Matlab仿真软件进行仿真。设定2组仿真实验，其中无人机的起点均为(0，0，0),目标点分别为(2，2，2.5)、(2.8，2.8，2.6),靠近目标点的障碍物分别在(2，2，2)和(3，3，3)处，另设一个障碍物分别在(1.5，1.2，2.7)和(1.5，1.2，1.7)处，由此构成目标点与障碍物距离较近的实验条件。借鉴文献[16]的规划任务参数，本文的参数设置如表1所示。

表1 人工势场法参数表

仿真结果如图6所示，红色路径表示传统人工势场法的规划结果，蓝色路径表示改进斥力势场函数后的规划结果。可以看出，传统人工势场法的路径在目标点附近反复徘徊，无法到达；而改进人工势场中，当无人机靠近目标点时，目标点附近的障碍物对无人机的斥力减小，引力可以一直大于斥力，最终使无人机顺利到达目标点。

图6 斥力势场改进前后的仿真结果示意图

4.2 三维目标分段法路径规划仿真

为验证三维目标分段法的有效性，使用Matlab进行2项仿真实验，每项实验均设2组。

首先验证对于典型局部极小值问题的改进效果。第1组实验将无人机的起点设置为(0，0，0)，目标点设置为(6，6，7)，障碍物1个，其位置为 (6，6，4)；第2组实验将无人机的起点设置为(6，0，3)，目标点设置为(6，6，3)，障碍物1个，其位置为(6，4.5，3)。算法的通用参数如表1所示，三维目标分段法参数k初始值为2，θ取0～360之间的任意实数。第1组实验的仿真结果如图7和图8所示；第2组实验的仿真结果如图9和图10所示。图7和图9中，由于起点、障碍物、目标点3点共线，导致传统人工势场法陷入局部极值，并在该极值点处达到受力平衡，使规划提前结束。而在使用三维目标分段法后的结果如图8和图10所示，产生子目标点使受力平衡被打破，令无人机能够摆脱局部极小值点的束缚，在到达子目标后，接着到达目标点。由此可知，三维目标分段法可以有效解决三维空间中的局部极小值问题。

图7 传统人工势场法规划结果Ⅰ

图8 三维目标分段法规划结果Ⅰ

图9 传统人工势场法规划结果Ⅱ

图10 三维目标分段法规划结果Ⅱ

第2项仿真实验用于验证三维目标分段法对典型路径震荡问题的改进效果。第1组仿真实验中，将无人机的起点设置为(1，3.5，-0.2)，目标点设置为(5，3.8，0.2)，2个障碍物的位置分别为(3，3，0)和(3，4.2，0)；第2组仿真实验中，将无人机的起点设置为(2，3.5，0)，目标点设置为(5，3.5，0)，6个障碍物的位置分别为(3，3，0)，(3.8，2.7，0)，(4.6，3.2，0)，(3.8，4.1，0)，(4.6，4.5，0)，(3，4.2，0)。算法的参数和第1项仿真实验一致。

仿真结果如图11—14所示。图11中的路径在方框处产生了剧烈震荡，因此该路径对无人机而言不可用，而图13中传统人工势场法无法规划出完整路径。图12和图14中，改进后的算法在判定产生局部震荡后，使用三维目标分段法产生子目标点，在子目标点的引导下，局部震荡现象消失，规划出的路径安全且平滑，可供无人机使用。因此，三维目标分段法可以有效解决三维空间中的局部震荡问题。

图11 传统人工势场法路径局部震荡Ⅰ

图12 三维目标分段法解决局部震荡问题Ⅰ

4.3 复杂环境路径规划仿真

4.1节和4.2节的仿真实验分别验证了在斥力势场函数中引入相对距离因子和三维目标分段法用于改进传统人工势场法的有效性，但实验环境相对比较简单，为了进一步验证本文提出改进方法的可行性，在Matlab仿真软件中进行复杂三维环境下的路径规划仿真。

图13 传统人工势场法路径局部震荡Ⅱ

图14 三维目标分段法解决局部震荡问题Ⅱ

本节同样设2组仿真实验，第1组实验中，无人机起点坐标设为(5，5，2)，目标点坐标设为(5，5，12)，障碍物共设6个，其坐标分别设为(5，5，3.5)，(5，5，11.5)，(5.7，4.5，6.3)，(5.7，5.4，6.3)，(4.5，5，9)，(5.8，5.8，5)。第2组实验中，无人机起点坐标设为(0，0，0)，目标点坐标设为(9，9，9)，障碍物共设12个，从而组成含有多个局部极小值点、路径震荡以及目标不可达问题的三维空间，其坐标分别设为(1，1，1)，(1.2，1.8，2)，(1.8，1，2)，(2.7，3.6，3.2)，(3.3，4，3.7)，(4，4，4)，(4.3，3.2，3.6)，(5，5，4)，(7，7，5)，(7，7，7)，(9，9，9.5)，(9，9，8)。算法使用的参数和4.2节一致。

实验结果如图15—17所示，图15由(a)和(b)2个部分组成，皆为复杂环境Ⅰ中三维视角下的路径规划结果；而图16和图17分别为复杂环境Ⅱ中XOZ平面和YOZ平面下的规划结果，为了便于描述，将目标点记为Goal，按子目标点出现的先后顺序，分别记为SubG1、SubG2、SubG3。从起点出发前往Goal的过程中，无人机陷入局部极小值点或局部震荡，此时三维目标分段法生成子目标点SubG1; 在前往SubG1的途中，无人机再次被困，此时以SubG1作为目标点生成子目标点SubG2；当无人机到达SubG2之后，将 Goal恢复为目标点，因此无人机没有必要再前往SubG1；再次前往Goal的途中，无人机又一次陷入局部极小值点或局部震荡，此时生成子目标点SubG3，在无人机到达SubG3之后，恢复Goal作为目标点的地位；随后，无人机最终到达离障碍物距离很近的Goal，规划结束。

图15 复杂环境Ⅰ下三维目标分段法规划结果

通过上述实验可以看出，本文提出的改进方法，尤其是三维目标分段法，简洁易行，通过向量运算可以快速找到子目标点，具备在复杂环境中为无人机寻找可行路径的能力。

5 结论

分析了传统人工势场法的原理及其存在的主要缺陷，提出了解决方案。通过引入无人机与目标点之间的相对距离因子改造斥力势场函数，解决目标不可达问题；将虚拟目标点法拓展至三维空间，提出了三维目标分段法，解决了传统人工势场法的局部极小值和局部震荡问题。在Matlab仿真平台上开展了一系列仿真实验，证实了该方法的可行性和有效性，表明本文提出的改进方法可以适用于复杂三维环境下的无人机路径规划。