基于改进人工势场算法的自主车辆局部路径规划方法研究*

王翔昌 吴训成 张伟伟 王鸿超

（上海工程技术大学 上海 201620）

1 引言

近年来，智能辅助驾驶随着汽车技术的发展而逐渐普及，自主车辆局部路径规划和动态避障作为智能驾驶的重要组成部分一直是专家学者的研究热点。自主车辆的局部路径规划通过车载传感器进行道路环境信息感知，通过系统决策进行车辆的行为规划，完成从目标起点至目标终点路径规划［1］，并通过车辆控制器完成估计的跟踪。程虹霞等通过对超声波传感器、红外传感器等进行传感器融合系统建立，研究多传感器信息融合技术在无人驾驶车辆以及机器人中自主避障行为中的应用［2］。张瑜等结合无人驾驶避障系统的应用需求，将多传感器数据融合技术应用于无人驾驶避障导航控制系统，改进了基于模糊神经网络的人车路径规划避障导航算法［3］。宋建辉等提出了一种基于BP（Back Propagation）神经网络的无人车避障技术。建立了以BP神经网络作为环境信息识别模型的无人车辆避障方法［4］。遗传算法作为一种通过模拟自然过程搜索最优解的方法常用于局部路径规划［5］。近年来随着计算机图形处理器（GPU）计算能力的增强，基于卷积神经网络［6］的路径规划算法也逐渐被重视起来。相比于两外两种算法，人工势场算法不需要进行全区域搜索，路径规划速度快，执行效率高，有较高的精确度和实时性，满足车载计算平台的需求。张建英等通过在附加条件上增加控制力来建立相同直线方向上的重力和排斥力动作线，以执行机器人的局部路径规划［7］。张琦等学者将障碍速度和加速度矢量引入了足球机器人避障控制［8］。在文献［9］中，作者从车辆的避障的实际情况考虑，针对传统人工势场法的不足，通过引入速度矢量，结合模糊控制方法，实时调节斥力场系数，进行无人车辆局部路径规划方法研究。谭宝成等［10］根据传统人工势场路径规划中存在的避障较大太大问题提出了随机改变目标点的方法，并在此基础上避免安全避障中的圆形虚拟力场范围。LaValle等在RRT 的基础上提出了连接型RRT算法［11］，该算法通过扩展随机树的进行双向搜索，提高传统RRT 的搜索效率。Sangwoo Moon 等基于虚拟人工势场算法实现了在未知环境的动态路径规划，结合了车辆的位置和方位信息［12］。Carvalho等基于模型预测控制MPC 进行车辆的局部路径规划［13］，模型预测控制算法能够协调系统约束和系统的非线性问题。Rui Oliveira等提出了一种基于“晶格”的路径规划及其优化方法［14］，该方法能狗为自动驾驶卡车提供实时轨迹规划。Chu 等人基于最优控制方法提出了一种局部路径规划算法［15］。该算法在满足车辆横向、纵向等约束下进行局部路径规划。

为了规划自主车辆的最优路径，实现对自主车辆的准确平滑控制，本文将对自主车辆的轨迹规划跟踪控制分为环境感知、系统决策、路径规划和路径跟踪四个部分，不同部分依赖相应的车载设备进行处理，如图1所示。

图1 路径规划与跟踪流程

环境感知部分由车载传感器收集和处理道路以及障碍物的信息；系统决策部分处理收集到的信息，根据驾驶行为决策对障碍进行分析判断，完成基于目标点、障碍点和目标点模型建立，确定相对位置。路径规划部分采用改进的人工势场算法，根据所建立的障碍点、目标点模型和车辆行驶状态进行避障路径规划。在完成道路感知、行为决策以及路径规划后，路径跟踪执行系统发出的指令进行车辆转向以及加速制动控制，实现自主车辆的轨迹跟踪。

2 改进的人工势场路径规划算法

2.1 传统的人工势场算法原理

人工势场算法的基本思想是将空间中被控对象的运动虚拟为虚拟力场中粒子的运动。目标点吸引被控对象，障碍物区域对被控对象产生斥力作用，被控物体在二者的合力作用下移动。

图2 传统人工势场算法原理

在二维空间中将被控对象假定为质点并设定其坐标为X=(x，y) ，目标点坐标为Xg=(xg，yg) 。根据传统人工势场算法的二力合成原理进行被控对象的引力势场Uatt和斥力势场Urep。

式中，k为引力场系数；X-Xg是被控对象到目标点的距离，用ρg表示，ρg可以通过式（2）进行计算。

据此可以得到被控对象的引力如式（3）所示：

式中，Vg是被控对象指向目标点的单位向量。

若在障碍物存在的情况下，假设其欧式空间中的坐标位置为Xob=(xob，yob)，改障碍点对被控对象的斥力势场函数可由式（4）计算：

式中η和ρo为斥力势场的作用系数和距离范围；ρob是被控对象与障碍点之间的距离。

根据斥力势场函数，当ρob≤ρo，控制对象的斥力通过式（5）给出：

式中，Vob为被控对象指向障碍点的方向向量。

综上所述，可以计算存在n 个障碍点时，被控对象收到斥力合力表示为式（6）：

通过对传统的人工势场算法进行分析，我们可以发现，传统人工势场算法仅是对作用在被控对象上的力进行矢量合成，存在一些不足。

1）目标无法接近的情况

当目标点附近有障碍物时，被控对象在目标点受到的斥力势场超过目标点的引力势场，导致被控对象无法接近目标点。

2）目标停止的情况

当被控对象在同一点所受到的障碍物斥力和目标点引力大小相等，方向相反时，被控对象处于动态平衡的状态，出现被控目标停止的情况。

2.2 改进的人工势场算法

针对以上问题，本文通过引入调和函数来计算障碍物斥力与目标点引力，从而解决目标遥不可及或者无法靠近的情况。

调和函数是二阶连续可导且满足拉普拉斯方程，即f:U→R，其中U满足式（7）中的方程且为Rn的开子集：

式（7）可化简为

式中的Δ 表示拉普拉斯算子。

因此，可将障碍物势场函数换用调和函数来表达，因其满足极大值存在于障碍物的边界上，所以能够使其免陷入极值。

改进后的斥力势场可表示为式（9）：

式中，m是一个大于零的实数。

修正的斥力表达式为可由式（10）和式（11）给出：

当空间中存在n 个有效障碍物时，被控对象的合力可表示为式（12）：

据此开支，通过引入调和函数，能够有效的在车辆路径规划中的运用人工势场法。

2.3 基于障碍点的被控目标建模

本文基于改进人工势场算法进行被控目标建模，首先进行障碍物椭圆势场的建立，完成目标点基于障碍点的目标建模；然后基于自然三次样条曲线和全局优化在满足车道保持约束、最短路径原则、最优曲率原则和路径航向约束的前提下进行路径规划。

本文通过建立椭圆势场区域来调整障碍车辆的势场作用区域，能够在考虑障碍车辆车身形状的情况下区别于传统圆形势场。椭圆势场的示意图如图3 所示，以障碍车辆的几何中心为椭圆势场的圆心，长短轴分别为a和b。障碍势场Utruck可用式（13）来表示：

图3 椭圆势场示意图

式中ktruck为障碍车辆势场刚度，表示第i个障碍的几何中心坐标。

本节通过考虑车身形状在传统圆形势场建立“椭圆势场”调整障碍车辆势场的作用区域，利用椭圆势场进行路径规划能够使规划的路径过渡更自然，曲率变化曲线更平缓。

2.4 基于自然三次样条曲线和全局优化的路径规划

本节在前节改进人工势场算法的基础上，通过引入车道保持约束、最短路径原则、最优曲率原则和路径航向约束等约束，使得生成的路径尽可能地满足驾驶平顺性、曲率最小等要求。

1）车道保持约束

车道保持约束作为路径规划中首要被考虑的约束指标，是避免规划的路径与车道发生过度偏离的关键约束。本文通过假设测试车辆行驶在与车道中心线的距离为Ld道路下，车道保持约束可用式（14）来描述：

2）最短路径原则

规划路径应尽可能再保证车辆安全的情况下通过减少路径长度来提高卡车运输效率，减少运输成本。本文通过计算相邻控制节点的路径长度变化量来计算最短路径：

3）最优曲率原则

大曲率意味着车辆在行驶过程中会有较大程度的转向角度变化，这对于车辆行驶是及其危险的因素，因此在路径规划时要满足最优曲率原则，且曲率变化在趋于平缓，过渡自然。本文中将路径简化为由若干分段式自然三次样条曲线连接各控制节点组成的，则有：

于是其路径曲率的二次式即为

4）路径航向约束

路径航向约束是指车辆的路径规划满足于车道中心线航向角度相同，避免车辆偏离车道，本文通过控制相邻节点间的航向角来实现路径航向约束。相邻控制节点间的航向角可用式（19）来表示，即：

从本文选择的四个约束原则可以看出，路径规划应满足这四个约束来确保所规划路径的合理与安全。因此，可将该问题转为为一个多目标优化问题。考虑安全因素，在上述四个多目标优化问题，本文的优化目标是使计划路径形状尽可能流畅自然，同时也确保计划路径保持在一定的距离尽可能中心线的道路。在这个范围内，路径长度尽可能短，并且路径朝向尽可能与道路的中心线一致。换句话说，它希望在相应的计划节点，路径的曲率变化的总合是最小的，同时，累积的和剩下的三个约束条件的变化（车道保持、最短路径和路径标题）也计划节点的最小，因此有：

上式中I为控制变量，它包含以上四个约束表达式中的参数。

3 仿真分析

本节利用Prescan 软件搭建测试环境，同时结合Matlab/Simulink 软件对基于人工势场的路径规划算法编程控制进行了改进，完成本文提出的基于改进人工势场算法路径规划方法性能验证。

单向车道宽度为3.5m 的双车道道路被选中作为车辆测试道路，在Prescan 中进行相关道路信息的设置并将相应的环境感知传感器添加到自主车辆控制和动态模型中。

图4 不同速度下的实际运动路径

设定智能车恒速行驶36km/h、54km/h 和72km/h，通过车载感知传感器进行障碍物的检测。在检测遇到障碍物后，通过局部路径规划系统进行路径规划和车道变换，完成车辆避障，得到路径如下仿真结果。

通过添加椭圆人工势场进行障碍点的势场建立改进，并通过引入四个约束基于自然样条曲线进行路径规划和全局约束，所规划的自主车辆的移动路径与目标路径基本一致，且车辆轨迹的曲率比那花十分平顺，未出现大曲率路径的现象。表明所提的基于改进人工势场的局部路径规划能够满足平滑变道与平缓驾驶的要求。

4 结语

本文提出了一种基于改进人工势场算法的局部路径规划算法，引入了椭圆势场调整障碍车辆势场的作用，建立了对称点障碍模型的方法保证了局部目标点在对称轴上。仿真结果表明，本文提出的路径规划方法能有效地规划一条曲率平滑的路径，且满足自主车辆行驶控制的需求，车辆行驶安全平稳，解决了传统人工势场算法存在的无法接近问题，具有较好的鲁棒性。