基于改进人工势场的无人机动态避障路径规划

丛玉华，赵宗豪，邢长达，王志胜

(1.南京理工大学紫金学院，南京 210023；2.南京航空航天大学自动化学院，南京 211106；3.南京航空航天大学深圳研究院， 广东 深圳 518063)

1 引言

无人机作为一种新兴的飞行器，相较于其他飞行器有着成本低、机动性强、速度快以及体积小等优点。随着人工智能、微电子、控制理论、数据处理以及传感器的发展，无人机逐步克服了之前技术上的一些阻碍[1-3]，在一些领域中开始投入实际应用，如军用无人机、农业无人机、高空检修无人机、道路勘探无人机、航拍无人机等，都体现了无人机在军事和民用领域极大的应用潜能。然而，要使无人机能够更好完成任务，需要好的路径规划算法适应变化的任务以及环境，使无人机能够快速、自主地完成指定任务。关于无人机路径规划的技术已有许多相关研究以及理论成果，大部分无人机路径规划主要涉及两类，全局规划和局部规划。全局规划可以分为两种类型：基于搜索的方法以及基于采样的方法。基于搜索的方法包括：基于图搜索的方法，如迪杰斯特拉算法(Dijkstra)，A*算法，混合A*算法等。基于采样的方法包括：概率路图法，RRT*算法，Informed RRT*算法等。局部规划中经典的方法包括：人工势场法(APF)，动态窗口法(DWA)[4-9]。其中，人工势场法是一种经典的路径规划方法[10-13]。是由Khatib提出的一种虚拟力场法，考虑了无人机的运动学问题，路径规划实时性强，可动态规避障碍物，较好实现无人机路径规划。但易出现狭窄通道内轨迹振荡问题以及目标点不可达问题，本文将针对这2个问题对人工势场法提出改进，并进行仿真验证。

2 基于势场的路径规划方法

2.1 人工势场法

使用人工势场法引导无人机的过程，可将无人机视为一个在负梯度向量场中移动的“正电荷粒子”[10]。负梯度可以直观地看作是作用在带正电荷粒子无人机上的力，它被吸引到带“负电荷”的目标上。与物理上的电荷不同的是，目标点的负电荷产生的梯度为负梯度，对无人机产生引力，使得无人机趋向目标点。障碍物具有“正电荷”，产生正梯度的场，对无人机产生斥力，迫使无人机远离障碍物。引力和斥力的结合可以引导无人机在避开障碍物的同时从起始位置到达目标位置[14-16]。

1) 引力场分析。与物理上的引力场或电场随距离增加而衰减的特点不同，引力场Uatt应当随着无人机与目标点的距离而单调增加。通过测量无人机到目标点的距离，构建一个场函数，即：

U(x)=ζd(p，pgoal)

(1)

其中：p表示无人机当前位置，坐标为(x，y)；pgoal表示目标点位置，坐标为(xgoal，ygoal)，ζ是一个用于控制引力场对无人机作用强度的参数。d(p，pgoal)表示目标点与无人机当前位置之间的距离，通过该式得出引力场梯度为

(2)

梯度向量从目标点开始，以ζ量级向外远离目标，目标点处未定义梯度向量。从目标点以外的任何一点出发，通过负梯度，可以找到一条通往目标的路径。

在通过数学模型实现该方法时，由于引力梯度在原点处存在不连续，梯度下降可能存在“抖振”问题。因此建立一个连续可微的势能函数，当无人机接近目标点时，引力梯度的强度就会减小。势能函数是一个随着无人机与目标点的距离二次增长的函数，以消除分母导致的梯度不连续问题，即：

(3)

通过平方后构建的引力场负梯度函数变为

(4)

从数学模型上不难看出，引力场是一个基于p的矢量，由目标点起始指向远离目标点的方向，引力场的强度与无人机到目标点的距离间隔成比例关系。无人机离目标距离越远，引力场作用的强度越强，无人机会快速接近目标；当无人机接近目标时，引力场效应变弱。引力场数学模型如图1所示，图1中Z轴表示势场强度，平面上X和Y轴表示产生势场的区域，引力场为式(5)，其中参数，(xgoal，ygoal)=(-5，5)，ζ=0.125。

图1 引力场数学模型示意图Fig.1 Distribution map of gravitational field

(5)

2) 斥力场分析。斥力场Urep迫使无人机远离障碍物。斥力的强度大小与无人机与障碍物之间的距离大小有关。与目标点产生的负梯度对无人机产生的引力场的特点相反，障碍物产生正梯度的斥力场，无人机离障碍物越近，斥力越强。因此，斥力通常以无人机到最近障碍物的距离D(x，y)为依据，斥力场Urep可表示为

(6)

从而得出斥力场梯度函数，

(7)

以上数学模型中的ρ为障碍物影响尺度因子，通过这个因子控制无人机忽略距离较远的障碍物产生的斥力场。η是一个用于控制斥力场对无人机作用强度的参数。设(xobs，yobs)为障碍物的坐标。则得式(8)：

(8)

式(8)中，

(9)

设置3个障碍物的参数η均为η1=η2=η3=20，3个障碍物的坐标分别是(xobs1，yobs1)=(-2.05，1)，(xobs2，yobs2)=(4.05，-2)，(xobs3，yobs3)=(2，3)，对应的ρ分别为ρ1=3，ρ2=3，ρ3=1。斥力场数学模型如图2所示，图2中Z轴表示势场强度，平面上X轴和Y轴表示产生势场的区域。

图2 斥力场分布示意图Fig.2 Distribution map of repulsion field

3) 引力和斥力合场分析。通过上文获得引力场与斥力场的数学模型后，将2个势场叠加得到总的势场函数。为了更易于理解，将势场中的梯度向量类比于力向量，令Fatt=Uatt，令Frep=Urep，则无人机受到的合力为

Ft=Fatt+Frep

(10)

Fatt的参数为：ζ=0.125，(xgoal，ygoal)=(-5，-5)；Frep的参数为：参数η为η1=η2=η3=20；障碍物的坐标分别是(xobs1，yobs1)=(-2.05，1)，(xobs2，yobs2)=(4.05，-2)，(xobs3，yobs3)=(2，3)；对应的ρ分别为ρ1=3，ρ2=3，ρ3=1。图3为引力场和斥力场合场Ft的分布图。

对无人机所受的合力正交分解后，通过X轴的分力与Y轴的分力求出无人机的航向角，由式(11)可得航向角θ，式中n为区域内障碍物的数量。可见航向角由目标点的引力、障碍物的斥力合力决定，通过合理调整无人机的航向角，从而达到避开障碍物、抵达目标点的目的。

(11)

4) 人工势场法路径规划步骤。梯度下降法是求解优化问题的一种常用方法。从初始状态开始，朝与梯度相反方向移动，其算法伪代码如下：

Algorithm APF

1:x(0)=xstart

2：y(0)=ystart

3：θ=θstart

4：i=0

5： while ▽U(x(i))≠0 and ▽U(y(i))≠0 do

6：θ=arctan(▽U(x(i))/▽U(y(i)))

7：x(i+1)=x(i)+γ*cos(θ)

8：y(i+1)=y(i)+γ*sin(θ)

9：i++

10： end while

进入下一个状态，并重复这个过程，直到梯度为零，即抵达目标点。具体步骤为：

1) 确定引力场和斥力场的强度系数ζ和η，障碍物的影响尺度ρ，无人机运动的步长γ(设无人机匀速运动)，无人机起始位置(x，y)，无人机初始航向角θ。

2) 计算航向角以及下一步无人机的坐标及航向角，公式如下：

(12)

3) 无人机移动至(xk+1，yk+1)，设置k=k+1为当前位置。

4) 判断无人机是否达到目标点，若抵达目标点则路径规划结束，若未到达目标点则返回步骤2)继续执行。

2.2 人工势场法仿真实验与分析

2.2.1人工势场法仿真验证

人工势场法应用在随机生成障碍物地图中进行路径规划，无人机起点坐标为(5，5)，终点坐标为(450，450)。障碍物半径为25，单个障碍物面积为1 962.5，地图面积为500×500。表1为具体实验参数。

表1 实验参数Table 1 Experimental parameters

图4为无人机路径规划结果，图5为航向角变化曲线，曲线中两个峰值处分别对应轨迹图中无人机的两次大角度转向，最后的航向角振动对应轨迹图中轨迹振荡部分。

图4 路径规划结果示意图Fig.4 Result of path planning

图5 航向角变化曲线Fig.5 Change curve of heading angle

从仿真效果中可以看出：人工势场法可以较好实现路径规划的基本要求，但是无人机在避开障碍物时需要做大角度的航向角变化，并且在一些狭窄的障碍物间隙中运动轨迹发生了振荡。

2.2.2人工势场法存在的问题

1) 运动轨迹振荡。人工势场法轨迹振荡问题，如图6和图7所示，图6为无人机穿越狭窄通道时，轨迹出现不断在2个障碍物方向上切换的现象，对应的图7中航向角则表现为航向角在两个方向上振荡。

图6 运动轨迹振荡现象示意图Fig.6 Oscillation of motion trajectory

图7 振荡区域航向角曲线Fig.7 Heading angle curve of oscillation area

图6和图7所体现的无人机运动轨迹振荡问题虽然对于路径规划本身是否成功影响不大，但在实际情况中会增大对无人机硬件以及能源的消耗。

2) 目标点不可达。设定无人机的起点坐标(10，10)，终点坐标(450，495)，即当终点坐标位于边缘障碍物附近时。路径规划结果如图8所示，航向角变化如图9所示。

图8 目标点不可达现象示意图Fig.8 Inaccessibility of target point

图9 航向角变化曲线Fig.9 Change curve of heading angle

由图8规划路径可见，当目标点距离边界障碍物距离很近的时候，由于目标点和边界障碍物对无人机产生的引力和斥力近似抵消，导致无人机在原地徘徊，无法抵达目标点，路径规划任务失败。对应的图9中航向角即为盘旋的走向。

3 人工势场算法改进

根据以上提出的2种问题，通过增加衰减函数和距离因子来调整斥力场系数的方法来解决。

3.1 激活衰减函数防止轨迹振荡

当无人机通过狭窄通道时，通过衰减函数来调整斥力系数，从而防止轨迹振荡产生。具体步骤如下：

1) 狭窄通道的判断。通过探测与障碍物间距离的方法来判断狭窄通道，该方法以无人机为圆心，在无人机移动方向的一定扇区范围设立半径为R的扇形探测窗口，测量进入探测范围的障碍物与无人机之间的距离。即测量无人机移动方向两侧±(β°,α°)扇区内是否在对称方向上同时存在障碍物进入测量半径R内，若存在则通过激光雷达等传感器获取障碍物与无人机之间的距离。同时测量无人机正前方是否有障碍物进入测量半径R内，若不存在则认为当前无人机周围的障碍物分布情况存在可以通过的狭窄通道。图10为探测窗口示意图。

图10中虚线范围为探测窗口，黑色质点表示无人机，两侧灰色区域为障碍物。

图10 探测窗口示意图Fig.10 Schematic diagram of detection window

2) 斥力场系数衰减。若存在狭窄通道则使用衰减函数减小该区域的斥力场系数，使无人机能够通过狭窄通道并消除轨迹振荡。同时设置最小的斥力场系数阈值，斥力系数的阈值取决于安全距离以防止撞击障碍物，一般与无人机自身尺寸和飞行时速有关。根据该方法，斥力场方程变为

(13)

式(13)中：ρ1为障碍物与无人机之间的距离阈值；DLR(x，y)为无人机探测窗口范围内左右两侧探测角度为[β°，α°]范围内探测到的障碍物与无人机之间的最短距离，当两侧范围内最短距离都小于阈值ρ1时，激活衰减函数f(η)；f(η)为设定的关于斥力场系数的衰减函数，根据设置的函数形式不同，该函数使斥力场系数以线性或者指数衰减，同时设定该系数存在一个最小值，以确保存在最小斥力防止无人机撞向障碍物。线性衰减函数为式(14)，指数衰减函数为式(15)。两种衰减函数如图11，指数衰减函数较快，更有利于迅速调整斥力场系数，因此选择式(15)。

图11 线性衰减和指数衰减曲线Fig.11 Linear and exponential decay curves

f(η)=η/DLR(x，y)

(14)

f(η)=η/eDLR(x，y)

(15)

3.2 加入距离因子防止目标不可达

随着无人机与目标之间距离减小，距离因子变小，使得斥力场系数变小，从而使障碍物产生的斥力场减小，最终无人机可以靠近并达到目标点。B.H.Krogh.A曾提出在斥力场计算公式的基础上加入无人机与目标点相对距离的平方项[17]，如式(16) 所示：

(xgoal-x)2+(ygoal-y)2

(16)

这种方法解决了目标不可达的问题，但由于直接采用距离作为系数，导致斥力场被放大，且无人机与目标点距离越大，斥力场越强。为此采用式(17) 的方式确保斥力场不会被放大并且在无人机与目标点距离较远时以较为缓和的趋势衰减。

(17)

式(17)中：dist(p，pgoal)作为影响斥力系数的依据如式(18)所示，当无人机与目标距离很近时则减小斥力值。式(18)中m为指数可调节衰减速度。

dist(p，pgoal)=((xgoal-x)2+(ygoal-y)2)m

(18)

3.3 改进后的人工势场算法

引入距离因子和衰减函数后，人工势场算法改为

(19)

改进后人工势场算法的步骤为，当无人机前方不存在狭窄通道时，按照传统人工势场法执行。当前方出现狭窄通道时执行如下中的功能函数使斥力场系数开始衰减。算法伪代码如下：

Algorithm Window

1： DIST_R=DETECT_POL(β，α)

2： DIST_L=DETECT_POL(-β，-α)

3：η=η0

4： dist_head=DETECT_POL (0)

5： for each dist_r in DIST_R

6： for each dist_l in DIST_L

7： if dist_r< threshold and dist_l < threshold and dist_head =Randη>ηmin

8：η=f(η)

9： elseη=η0

3.4 仿真验证

仿真地图障碍物半径为25，单个障碍物面积为1 962.5，地图面积为500×500。仿真前设置扇形探测窗口的大小、引力和斥力系数、衰减函数、距离因子等，表2为具体的实验参数。

表2 实验参数

1) 轨迹振荡。无人机起点坐标为(5，5)，终点坐标为(450，470)，路径规划和航向角变化仿真结果如图12和图13所示。图12为改进前后路径规划对比，图13为改进前后无人机航向角变化曲线对比。可看出路径在穿越狭窄区时航迹变得平滑，航向角振荡消除。

图12 改进前后路径规划示意图Fig.12 Comparison of path planning before and after improvement

图13 改进前后航向角变化曲线Fig.13 Change curve of heading angle before and after improvement

2) 目标不可达。无人机的起点坐标(10，10)，终点坐标(450，495)。仿真结果如图14所示，可以看出改进后目标不可达问题已解决。

图14 改进前后目标点抵达效果示意图Fig.14 Target point arrival effect before and after improvement

通过仿真分析可见改进后算法很好地解决了在狭窄通道内轨迹振荡问题和障碍物附近目标不可达问题。

4 结论

1) 通过设置探测窗口判断是否存在狭窄通道，如存在则激活衰减函数，进而调整斥力场系数，消除了狭窄通道处轨迹振荡现象。

2) 通过设置无人机与目标点距离因子来调整斥力场系数，当靠近目标点时，减小斥力系数从而减小斥力，解决了目标与障碍物距离很近时目标点不可达问题。

3) 通过理论分析和仿真验证，表明改进后方法可以更有效地进行路径规划，但人工势场法为局部方法易陷入局部最优，后续将在此基础上研究解决局部最优问题，并进行三维空间上路径规划的研究。