改进人工势场法的路径规划研究

林 洁，张志安

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

0 引言

路径规划是目前移动机器人研究的一个重要方向，以实现机器人的自主避障，确定最优路线。常见的全局路径规划方法有遗传算法[1-5]、A*算法[6-7]、RRT算法[8]、蚁群算法[9-11]和人工势场法等。

人工势场法(artificial potential field，APF)对于硬件平台要求不高，计算量较小且规划的轨迹较为平滑被广泛使用[12]。但人工势场法极易陷入局部极小点，导致目标不可达。对此，文献[13]提出一种用偏转角度来构建附加牵引力的方法以解决局部极小点问题；文献[14]提出分段混合算法改进势场函数；文献[15]提出改进斥力场函数解决局部极小值问题，但对路径是否是最短以及平滑性没有具体分析。本文针对人工势场法容易造成陷入局部极小点的问题，提出“沿边走”的详细策略进行路径的求解。

1 算法基本原理

1.1 APF算法基本原理

人工势场法是由Khatib提出的一种虚拟力法，目标点对机器人产生引力，障碍物对机器人产生斥力，机器人在这2种力的作用下运动。总势场函数为

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

(1)

引力场函数为

(2)

Ka为引力增益系数；qgoal为目标点的坐标；ρ(q,qgoal)为机器人距离目标点的距离。斥力场函数为

(3)

Kr为斥力增益系数；ρ(q,qobs)为机器人与最近障碍物的距离；ρd为距离阈值。当当前点和障碍物的距离小于ρd时，机器人才会受到斥力。

机器人沿着总势场的下降最快的方向运动，所受合力为

F(q)=-▽U(q)=-∇Uatt(q)-∇Urep(q)

(4)

1.2 SA算法基本原理

模拟退火算法(simulated annealing,SA)是一种基于概率的算法，其出发点是固体物质的退火过程与一般的组合优化问题具有相似性。Metropolis接受准则以一定概率接受恶化解，可以使算法跳出局部最优[16]。

模拟退火算法首先需要1个初始解和给定温度T，然后在其邻域不断搜索新的解，在该温度下在不断得到的新解的基础上再迭代，直至在该温度下趋于稳定，然后温度T下降，再重复上述过程直到解趋于稳定，得到近似最优解。

2 “沿边走”的APF-SA算法

本文利用“沿边走”(wall-following)解决传统人工势场法中，在同时存在非凸多边形和凸多边形的复杂环境中陷入局部极小点难以逃出的问题，利用模拟退火算法解决路径长度过长和路径平滑度差的问题。

本文的“沿边走”的APF-SA算法流程如图1所示。

图1 “沿边走”的APF-SA算法流程

2.1 “沿边走”的APF算法

传统的人工势场法在障碍物附近容易震荡甚至停止，通过设置虚拟目标点可以跳出局部极小点。“沿边走”的APF算法可以分为5个部分。

2.1.1 “沿边走”行为的激活条件

为了保障算法的收敛速度，在陷入局部极小点之前的路径都是基于梯度下降的传统APF算法，在陷入局部极小点之后再激活“沿边走”的行为。当梯度下降过程中，连续多次出现“往回走”或者“原地不动”的现象，则视作陷入局部极小点。本文“沿边走”的激活条件为

Δρ=ρ(qdest-qcur)-ρ(qdest-qlast)≤0

(5)

Δρ为规划的路径中每2步距离终点的差值；qdest为终点坐标；qcur为当前这一步的坐标；qlast为上一步的坐标；ρ(qdest-qcur)为当前这一步和终点距离；ρ(qdest-qlast)为上一步和终点的距离。

2.1.2 “沿边走”搜索范围的判定依据

当陷入局部极小点时，qlimit=(xlimit,ylimit)，在以局部极小点为中心的正方形区域(xlimit±k,ylimit±k)内搜索障碍物边界点，对于凸多边形或者非凸边形的障碍物，都存在2个可行的运动方向，此时优先选择离终点更近的方向作为“沿边走”的初始移动方向，便于减少路径长度。

搜寻到的2个方向的障碍物边界点集合分别为n1和n2，由于n1和n2中的点是连续的，故将n1和n2中离局部极小点最远的点和终点的距离ρ1和ρ2，作为“沿边走”初始条件的判定依据。若ρ1≤ρ2，则选择n1作为初始移动方向的参照集合，即n=n1；反之则选择n2。

沿初始方向的集合“沿边走”一定距离后，此时的坐标qcur=(xcur,ycur)，上一次“沿边走”行为的起点为qcur_last，qcur_last的初始值为qlimit。为了保证不“往回走”造成循环，以及增加算法的收敛速度，接下来的每一步的搜寻方向将以qcur为起点的正方形区域内搜索。

为了尽可能保证“沿边走”的路径连续平滑，将n按照离qcur_last的距离升序排序。优先选择从上一次的搜索范围内障碍物边界点中，距离qcur_last最远的边界点qobs和qcur_last作比较。若qcur_last和qobs的x方向或者y方向坐标相同，则从尾部开始在上一次获得的边界点集合near中，查找x方向或者y方向不同的点qobs=(xobs,yobs)，由该点和上一次“沿边走”行为的起点qcur_last=(xcur_last,ycur_last)的相对方位决定此时的搜索方向，此时的搜索范围为

(6)

搜索范围如图2所示。qcur_last为初始的局部极小点，初始在以qcur_last为中心的正方形范围内搜索，存在如图2中箭头所示的2个参照移动方向，往x方向移动是距终点qdest更近的选择，因此，参照本次搜寻范围x方向移动，到达qcur。由于上一次获得的沿x方向的边界点中离qcur_last最远的点qobs在qcur_last的右下方，因此，下一次的搜索范围是qcur的右下方。接着qcur=qcur_last，重复上述过程。

图2 搜索范围

为了保证路径的平滑性，规划的路径将按照n中的边界点为参照依次按照每步的增量移动。

2.1.3 防止判定搜索范围失效

由于上述算法是提取的障碍物边界作为可移动的方向，所以规划的路径在障碍物拐角的地方容易出现“穿过”障碍物或者和障碍物边线重合的现象。当障碍物边界出现连续的横坐标或者纵坐标不变化，并且规划的路径正好和边界重合时，通过障碍物边界点和“沿边走”的起点的相对方位，来判定搜索范围会失效，并且和障碍物几乎重叠的情况是需要尽可能避免的。

当n中出现一半以上连续x方向坐标不变的情况，记录下最多的连续不变的障碍物边界点的横坐标xobs，路径的下一个点xcur_next往往不是障碍物的区域故再移动s，如式(7)所示，同时纵坐标不变。

(7)

下一次搜索范围以xcur_next为起点，此时xcur_next和障碍物边界会存在明显的方位关系，可以利用式(6)判断范围，如图3所示。图3中，qobs和qcur重叠，根据式(6)无法找到相对位置关系，qcur=qcur_next后获得qcur_next的搜索范围，在图3中为第2个实线框。

图3 防止搜索范围失效

2.1.4 更正搜索范围

搜索范围内得到的near中的点较少时，此时的搜索范围出现了误判，如图4所示。其中，qcur为此时的坐标点，qcur_last是上一次搜索范围的起点，通过“沿边走”qcur_last移动到了qcur。此时搜索范围如图4中黑色虚线框所示，通过qcur_last和qcur的相对位置关系对搜索范围进行更正，如图4中qcur在qcur_last右下方，qcur的搜索范围更正为以qcur为起点的右下方搜索范围，如图4中实线框所示。

图4 更正搜索范围

2.1.5 “沿边走”行为的退出条件

“沿边走”的APF算法流程如图5所示。

每次沿n走了一段距离后，判断此时坐标的势能e1是否小于局部极小点的势能e0，小于则退出开始传统APF的梯度下降规划行为，保证不会再次陷入已经逃脱的局部极小点；否则继续“沿边走”行为，直至满足退出条件。

2.2 模拟退火算法

采用“沿边走”的方法在逃离局部极小点时获得的路径长度较长，并且在障碍物的拐角处可能存在转弯角度过大的情况，因此引入模拟退火算法对其进行优化。

利用“沿边走”的APF算法规划得到的路径，作为模拟退火算法的初始解作为启发信息，对其进行路径长度和平滑度的优化。

通过2.1中阐述的“沿边走”的APF算法流程，初始解实际上是由梯度下降规划的路径和“沿边走”的路径组成的。在规划的过程中为了保证路径的平滑度，整个路径的步数较多，直接对从起点到终点的所有路径进行优化会导致模拟退火算法的收敛速度较慢。因此，本文利用每一次跳出局部极小点的点将初始解进行分段，每一段包含梯度下降的部分路径和“沿边走”的部分路径。对每一段分别用模拟退火算法进行优化。

2.2.1 碰撞检测

本文产生新解的方式是在初始解的路径点中任意选择2个点，将其直接连接作为新解。

在初始解的基础上产生新解的过程中需要进行碰撞检测，即将产生的新路径进行等分，若等分后的每一个点都在可行域，即不和障碍物相交，认为此次新解有效，接受此次新解。

2.2.2 适应度函数

本文采取的适应度函数由路径长度和路径平滑度2部分组成，即

F=∂×flen+β×fsmooth

(8)

α、β分别为路径长度flen和路径平滑度fsmooth权重系数。

(9)

ρ(qi,qi-1)为路径上每一个点与上一个点的欧氏距离。flen越小代表路径长度越小。

(10)

θi，i-1为路径中相邻两点的夹角；fsmooth为路径中相邻夹角差之和，fsmooth越小代表平滑度越好。

适应度F越小代表路径长度越短、平滑度越高。

2.2.3 删除震荡段

在陷入局部极小点时可能会出现停滞不前或者震荡的情况，造成冗余路径。路径的震荡段对于应用模拟退火算法进行优化会造成无效迭代，因此需要对震荡段进行删除。

由于需要避免收敛速度过慢造成的震荡段的误判，多次满足式(5)的条件才会激活“沿边走”行为。在初始路径的规划中，记录所有满足条件的局部极小点，当初始路径完成后遍历所有局部极小点，每个局部极小点后续路径中，首个不满足式(5)条件的路径点认为是此次震荡段的终点。将每个局部极小点的附近产生的震荡段删除，保证算法的收敛速度。

2.2.4 模拟退火算法流程

每一次跳出局部极小点的点将初始路径按照每一次跳出局部极小点的路径点进行分段。针对每一段路径的适应度F按下述步骤进行迭代优化，最后得到适应度最小的最优路径解。

每一段的模拟退火算法的步骤如下所述：

a.设置模拟退火的初始温度t0和终止温度tstop。对于每个t的迭代次数N，初始解是“沿边走”的APF规划出的路径集合Loldpath。

b.对k=0，1，…,N重复步骤c～步骤e。

c.随机选取Loldpath中除了起点(局部极小点)和终点(跳出局部极小点的点)以外的2个点qnew1和qnew2，将qnew1和qnew22个点直接连接产生新的解Lnewpath，如果qnew1和qnew2形成的路径能经过碰撞检测则接受这个新解，不能则重复步骤c直至能经过碰撞检测。

e.当满足终止条件，认为此时已经达到最优解，则输出该温度值下的最优解。本文采取的终止条件为连续n个Lnewpath的适应度标准差小于5，可以认定在该温度下已经达到了稳态，可以提前跳出步骤b～步骤e的内循环，如式(11)所示。

(11)

f.t=ε×t，0<ε<1,当达到该温度下的最优解时，温度t下降，若t>tstop继续步骤b，当t

3 仿真与分析

本文在MATLAB2018软件上仿真。为了避免路径和障碍物相切，先将地图进行图像膨胀再进行路径规划。地图大小为400×400(本文的长度均为无量纲数值)，起点坐标设置为[1,1]，终点坐标设置为[400,400]。

传统APF只利用梯度下降进行规划，在陷入局部极小点后难以跳出，在图6所示的环境中均不能跳出局部极小点。

图6 设置虚拟目标点的APF算法规划的路径

确定可能的子目标点的流程是当搜索到的子目标点的势能e1小于当前局部极小点e0时，接受该子目标点成为路径中的下一步，再在该目标点的基础上不断寻找下一个子目标点，直到确定跳出了该局部极小点。这种方法在局部极小点深度较低时行之有效，但是依赖于遍历区域和陷阱范围大小，若可行的子目标点在遍历区域以外会导致不能跳出。

在相同的地图下，在图6a所示的简单环境下，可以通过虚拟目标点跳出局部极小点，能够到达终点。但在图6b所示的复杂环境中，由于无法搜索到可行的虚拟目标点会导致规划失败。

根据2.1中“沿边走”的APF算法在如图6a所示的简单环境中能到达终点，经过模拟退火算法优化，路径的长度变短，平滑度变高。如图7和图8所示。

图7 “沿边走”的APF算法在简单环境下规划的路径

图8 “沿边走”的APF-SA算法在简单环境下规划的路径

模拟退火算法改进前后的距离目标点的距离和迭代次数的关系，如图9所示。

图9 改进前后算法在简单环境下到目标点的距离变化

图9中“沿边走”的APF算法有2段几乎停滞的路径是陷入局部极小点处，删除震荡段后再进行优化，在只存在凸多边形的障碍物的简单环境中对路径的长度及平滑度都有所提升。

在同时存在凸边形和非凸边形的障碍物的复杂环境中利用“沿边走”的APF进行规划，再用模拟退火算法进行长度和平滑度的优化，如图10和11所示。

图10 “沿边走”的APF在复杂环境下规划的路径

图11 “沿边走”的APF-SA算法在复杂环境下规划的路径

模拟退火算法改进前后的距离目标点的距离和步数的关系如图12所示。可以看出在复杂环境中多次陷入局部极小点，利用本文的“沿边走”算法可以顺利逃出，但花费路径过长，平滑度差，删除震荡段后再经过模拟退火算法优化路径长度和平滑度提升较大。

各算法在简单环境和复杂环境下的路径长度和平滑度如表1所示。为了保证对比的可靠性，表1中的“沿边走”的APF算法中的路径长度和平滑度的计算，是利用删除了陷入局部极小点后产生的震荡段后的路径。路径长度计算如式(9)所示，路径平滑度由拐点数量表示(忽略由于靠近障碍物引起的路径震荡段造成的拐点数量)。

图12 改进前后的算法在复杂环境下到目标的距离变化

表1 算法对比

“沿边走”的APF算法相较于设置虚拟目标点的APF算法，在简单环境中由路径长度和平滑度相差不大。但在复杂环境中，设置虚拟目标点的APF算法陷入局部极小点后无法跳出，无法到达终点。

改进后的算法在简单环境中路径长度相较于“沿边走”的APF算法提高17.59%，平滑度提升50%；在复杂环境中路径长度提升38.72%，平滑度提升50%。

4 结束语

本文利用“沿边走”算法，弥补了传统APF算法在设置虚拟目标点中，扩展方向具有盲目性且当陷阱深度较深时不易逃出的缺点，同时又针对“沿边走”策略导致的路径长度较长以及平滑度差的问题，采用模拟退火算法进行改进。最后在不同的仿真环境下对提出的“沿边走”的APF-SA算法进行验证，结果表明，在简单和复杂的环境中改进后的算法对规划后的路径长度和路径平滑度都有所改善，具有一定实用性。