基于改进人工势场法的多无人车编队路径规划

尹凤仪，刘 芳，吴向阳

（北京特种机械研究所，北京 100143）

1 引 言

随着无人车的高速发展，多无人车的应用在军事、农业、工业等方面越来越广泛，其中多无人车的编队和路径规划也成为其中的重点。首先，领航跟随法[1]作为常用的多无人机编队控制方法，其实现方式较为简单，能够解决无人车之间的跟随控制问题，具有很高的实际应用价值，也是本文使用的编队控制方法。

多无人车的路径规划也是国内外的研究热点，其规划方向主要分为两类：基于全局地图信息的全局路径规划和基于局部地图信息的局部路径规划[2]。常见的路径规划算法有A*算法、RRT算法、人工势场法、群体智能算法及上述算法的改进融合[3-8]。其中，人工势场法[9]是一种简单且实用性强的路径规划算法，其原理是将路环境空间作为虚拟力场空间，将目标点作为引力点，障碍点作为斥力点，由目标引力点和障碍斥力点的共同作用形成力场，规划无人车行进路线。人工势场法也存在显著的缺陷[10]：当障碍物与目标点位置较近或者障碍物在无人车和目标点中间时，会出现目标不可达的问题；当出现引力与斥力合力为零的情况时，无人车会停摆不前或振荡，无法逃出局部极小值的问题。针对上述问题，许多研究人员提出很多改进方法。文献[11]改写了函数形式，选取适当的增益参数解决目标不可达问题；文献[12]提出在局部极小点设定虚拟最小区域以及改变斥力角度的方案；文献[13]应用模糊控制，在相应时刻改变无人车的合力大小及方向，避免出现局部极小值振荡问题；文献[14]重新定义引力公式，提高机器人的避障和动态跟踪灵活性；文献[15]将蚁群算法引入人工势场法，降低局部极小值的可能性，但算法过于复杂；文献[16]在斥力场函数中加距离调节因子，使得在目标点附近斥力为零；文献[17]提出采用虚拟水流法解决局部极小值陷阱；文献[18]采用“沿边规划”解决局部极小值问题；文献[19]通过设置中间目标点的方法，给机器人一个虚拟逃逸力逃出极小值情况；文献[20]通过使用序列凸优化方法，在保证轮式机器人最优化规划同时大大缩短计算时间；文献[21]研究出基于模型预测控制的阿克曼转向结构车辆的运动控制方法，证明了方法的有效性和鲁棒性。

本文针对多无人车的编队特性，将无人车分为领航车和跟随车分别进行人工势场法的改进。针对目标不可达问题，通过动态改变障碍物的影响范围解决目标不可达问题；针对局部极小值问题，采用动态法向力设置虚拟力的方式逃脱振荡情况。通过设置合适的对比试验，针对两种问题比较传统算法和改进算法的实际规划效果，在MATLAB 仿真环境中验证规划算法的有效性。

2 传统人工势场法

人工势场法主要思想是：将无人车的工作环境模拟为人工势场，目标点对无人车产生一个引力场，每个障碍物对无人车都产生一个斥力场，将引力场和所有斥力场合成一个势能场。无人车在该势场力的作用下进行路径规划。

如图1 所示，在无人车二维空间运行环境中存在一个目标点和一个障碍物，其中无人车位置q、目标点位置qa、障碍物位置qr坐标分别为［x,y］、［xa,ya］、［xr,yr］。在虚拟势能场中，当无人车与障碍物的距离越小时，障碍物产生对无人车的斥力越大；而当无人车和目标的距离越大时，目标产生对无人车的引力越大，因此引力场与斥力场势能设计如下，引力场势能函数Ua表示为

图1 虚拟人工势场法Fig. 1 Virtual artificial potential field method

式中，d(q,qa)表示无人车和目标点的欧式距离，e˙ (q,qa)是一个从无人车指向目标点方向的单位矢量。ka为正实数，表示引力系数。

则引力场产生的引力ka可表示为

斥力场势能函数Ur表示为

式中，d（q,qr）表示无人车和障碍物的距离，e˙ (q,qr)是一个从障碍物指向无人车方向的单位矢量，ρ0为正实数，表示避障半径，即当无人车处在避障半径为ρ0的区域内才需要进行避障。k r为正实数，表示斥力系数。

则斥力场产生的斥力kr可表示为

由图1 所示，假设运动过程中有n个障碍物，有无人车所受到的合力为引力与斥力之和，因此无人车所受合力为

3 改进人工势场法

3.1 目标不可达问题

当目标点附近存在障碍物，小车在行驶到目标点附近时，受到引力越来越小，而障碍物的斥力越来越大，将会是小车处于停滞状态，从而无法达到目标点，如图2 所示。通过改变目标点附近原障碍物斥力场的影响范围，使目标点不再受到障碍物斥力作用，从而使无人车可以到达目标点。并且在远离目标点位置保持斥力作用，避免撞到障碍物，因此引入无人车到目标点的距离影响因子d n(q,qa)，对斥力场函数改进

图2 目标不可达示意图Fig. 2 Target unreachable diagram

3.2 局部极小值问题

针对局部极小值问题，本文进行实时局部路径规划设计，当无人车、障碍物和目标点处于同一直线时（图3），随着引力的减少和斥力的增加，会出现合力平衡的临界状态，此时无人车处于停滞振荡状态。在实际情况中，一般会出现接近三点共线状态，此时需要规划方案以避免陷入极小值陷阱。

图3 局部极小值示意图Fig. 3 Local minima diagram

判断形成局部极小值的条件为，斥力和引力的方向夹角θ∈ （ π -β,π+β）。其中，β为引力和斥力的夹角阈值。为了避免这种情况，施加动态法向力帮助无人机逃脱局部极小值。由于多无人车的特性，分别对领航车和跟随车施加不同类型的法向力。对于领航车，法向力方向为领航车前一步运动方向的垂直方向；对于跟随车，法向力方向为领航车与跟随车相对矢量距离的垂直方向。

为防止无人车受力突变导致不平滑曲线，对法向力进行定义

式中，k⊥为法向力系数，法向力大小随着无人车接近障碍物而增大，防止其陷入局部极小值。当无人车受到的引力与斥力的夹角偏离θ的范围时，取消法向力作用。

3.3 算法流程

由上述可得，改进人工势场法的算法流程如图4 所示。

图4 改进势场法算法流程Fig. 4 Improved artificial potential field method algorithm flowchart

4 仿真结果分析

4.1 多无人车编队

使用MATLAB 进行仿真实验，对于多无人车的编队设置，实验选择三角形型的领航-跟随编队，其通信拓扑结构如图5 所示。

图5 领航-跟随编队结构Fig. 5 Formation structure of leader-follower method

4.2 目标不可达问题

为了验证改进算法的有效性，使用原算法和改进算法进行仿真对比。首先针对目标不可达问题，对传统人工势场法进行仿真。可以发现，当目标点附近有障碍物时，存在目标不可达情况。这是由于障碍物的斥力场远大于目标点的引力场作用，导致车队出现停滞，无法抵达目标点。在第4.1 节的编队设置下，对目标不可达问题的仿真如图6 所示。

图6 目标不可达问题对比仿真Fig. 6 Comparative simulation of target unreachable problem

由图6 可知，传统人工势场法下无人车无法到达目标点，改进算法通过加入无人车到目标点的距离影响因子，增加目标点对于无人车的引力场影响，使其可以在误差允许情况下到达终点。

4.3 局部极小值问题

针对局部极小值问题，考虑目标、障碍物、无人车三点共线的情况时，无人车会出现停滞现象，无法继续前进，对传统人工势场法算法和改进势场法算法进行仿真，可以得到无人车局部极小值的逃离情况如图7 所示。

图7 局部极小值问题对比仿真Fig. 7 Comparative simulation of local minima problem

由图7 分析可得，传统算法由于局部极小值影响，导致路线出现多次停滞振荡，甚至跟随车2 出现无法前进和恢复队形的情况。改进后的算法通过调节动态法向力，让无人车逃离局部极小值情况，减少路线的振荡，引导多无人车编队平滑顺利到达目标点。

4.4 改进算法整体规划

整体实验使用MATLAB 进行仿真对比，实验分别对传统势场法和改进势场法进行仿真对比，算法主要参数设定如表1 所示。

表1 路径规划参数设置Table 1 Path planning parameter settings

传统算法与改进算法的仿真对比如图 8所示。

由图8 可得，传统算法由于人工势场法的缺陷，会导致无人车陷入局部极小值而停滞振荡，多无人车编队无法恢复，并且由于目标点附近障碍物影响，无法到达目标点的情况。改进算法通过引入动态法向力，帮助逃脱极小值陷阱；同时通过引入距离影响因子，从而在多个障碍物的影响下规划出多无人车各车的平滑曲线，保证多无人车编队队形并顺利到达目标点。

图8 整体路径规划对比仿真Fig. 8 Overall path planning comparative simulation

5 结 论

多无人车编队路径规划通常采用人工势场法，但是传统人工势场法存在目标不可达和局部极小值问题，为解决其缺陷，本文主要做了两个方面的改进：首先，通过增加距离影响因子改变目标点附近障碍物的斥力场函数，使目标点对无人车的引力场作用增大，从而使无人车到达目标点，并且在和传统算法的对比仿真中，可以看出改进算法可以引导无人车到达目标点；其次，通过增加动态法向力作为虚拟逃逸力，使无人车逃离局部极小值情况，对领航车和跟随车采取不同的法向力方向。通过对比仿真可以看出，无人车逃离局部极小值点，并恢复队形到达目标点。MATLAB 的仿真实验充分证明了改进算法的有效性和可行性，但是关于更复杂路况下多无人车编队的避障规划能力仍有待进一步研究。