基于改进人工势场法的智能汽车轨迹规划算法研究
时间：2023-06-06 08:45:26 来源：雅意学习网 本文已影响 人 
赵奉奎，葛 振，董锋威，张 涌
(南京林业大学 汽车与交通工程学院，江苏 南京 210037)
避障路径规划技术是自动驾驶汽车关键技术之一，在环境感知层获得行驶环境的信息后，轨迹规划模块可按照不同的行为决策，为自动驾驶汽车规划出一条从起点到目标位置的最优路径。根据行驶场景的不同，路径规划技术主要分为全局路径规划和局部路径规划。全局路径规划主要是根据预先存储的地图进行路径规划，如在现有电子地图中设定一条连接两城市的最优路径。汽车在道路上行驶时，行驶环境复杂，需要利用视觉或雷达实时感知周围环境[1]，进行局部路径规划。路径规划算法不仅能及时准确的规划出避障路径，而且能规划出易于汽车跟踪的路径，以及具有较高的乘坐舒适性[2]。
国内外学者对局部路径规划开展了广泛的研究。王富奎[3]将避障路径规划问题转换多约束优化问题，设定了参考速度、加速度、车辆动力学约束、车道约束等若干约束目标，据此建立优化模型高维图，采用图优化法进行求解；
雷艳敏等[4]将最短路径、安全性能和平滑度综合考虑，得到了一种用于路径规划的全新的遗传算法；
田欣[5]基于遗传算法提出了一种新的自适应调整方式，提高了算法的效率，引入了模拟退火算法来克服局部最优解的缺陷。此外，仍有基于传统路径规划算法展开的研究，如基于采样的RRT算法[6]和基于图搜索的A*算法[7]，以及针对结构化道路提出的路径规划算法[8]。但当前多数算法较为复杂，求解难度较大，算法效率低，不利于实时路径规划。
人工势场法是一种常用的路径规划的算法，具有算法结构简单，实时性好，规划出的路径较为平滑等优点。当前对于人工势场法的改进主要集中在解决其固有的两个缺陷——局部最优解和目标不可达[9-12],也有将其与其他算法相结合，段建民等[13]提出一种改进人工势场法与遗传算法结合的方法来寻求全局最优解，并引入填平势场跳出局部极小点。同时也出现许多针对自动驾驶汽车的改进方案，谭宝成等[14]优化了经典人工势场法中圆形虚拟势场的作用域，能够实现结构化道路中较小角度的避障；
王树凤等[15]将障碍物模型改进为椭圆模型，引入了基于前车工况的斥力系数以及局部虚拟目标点；
曹昊天等[16]在人工势场法的基础上，增加了引导车辆避障的引导势场，使避障过程更为安全。当前基于人工势场法的路径规划算法存在很多问题，如算法未考虑车辆在不同速度下的势场影响范围，未考虑车辆运动学约束和车辆实际行驶的环境，以及未考虑对动态障碍物的处理等[17]。
因此提出了一种改进型的人工势场法，在进行路径规划时加入车辆运动学约束，使规划的路径满足当前速度下车辆前轮最大转角约束。研究了超车场景路径规划，设计了基于安全车距的不规则障碍物模型，在设定的道路约束下，实现该算法对动态障碍物的避障路径规划处理。
1.1 经典人工势场法
目标点产生的引力势场记为Uatt，假设车辆为一质点，引力势能的大小与目标点和车辆所在位置间的距离有关，距离越远，车辆受引力势能的影响就越强，距离越近，车辆受引力势能的影响就越小，目标点处的引力势能为0。将车辆的运动空间抽象成二维平面，在整个运动空间内的引力势能可表示为：
(1)
式中：Katt为引力势能比例增益因子；
X为车辆当前位置的坐标；
Xg为目标点位置坐标；
m为引力势场因子，一般取值为2；
ρ为车辆当前位置距目标点的距离。
引力势场对车辆产生的引力记为Fatt，那么Fatt为引力势能的负梯度，即沿引力势能下降最快的方向，其可表示为：
Fatt(X)=-Katt·ρ(X,Xg)
(2)
与引力势场相似，车辆在行驶过程中还会受到由障碍物产生的斥力势场的作用，其对车辆的影响也与车辆与障碍物的距离有关。当车辆与障碍物越近时，障碍物产生的斥力势场对车辆的影响越大，超过一定距离时，斥力势场便不再对车辆产生影响。与引力势能函数类似，构建函数为：
(3)
式中：Krep为斥力势能比例增益因子；
Xo为障碍物位置坐标；
ρo为车辆受障碍物影响的最大距离。
与引力的求法同理，斥力势场对车辆产生的斥力记为Frep，那么Frep为斥力势能的负梯度，即沿斥力势能下降最快的方向，可表示为：
(4)
由于运动空间中每个位置处的引力势能大小和斥力势能大小为标量，根据叠加定理，运动空间中的总势能大小如式(5):
U(X)=Uatt(X)+Urep(X)
(5)
从而，车辆所受到的合力F(X)为：
F(X)=Fatt(X)+Frep(X)
(6)
1.2 线性二自由度车辆运动学模型
由于车辆运动学系统极为复杂，为了便于分析，将四轮车辆简化为两轮的单车模型，需做如下假设：
1)汽车的纵向行驶速度u恒定不变。
2)忽略转向系统的影响，以前轮转角作为输入，后轮转角始终为0。
3)忽略悬架系统的影响，认为车辆仅做平行于地面的平面运动，即忽略汽车的垂向运动、俯仰运动以及侧倾运动。
4)不考虑载荷的左右转移，即前轴和后轴的左右轮分别等效为一个轮。
5)忽略横纵向空气动力学。
6)侧向加速度小于0.4g，以保证侧偏刚度为常数。
于是汽车被简化成仅有侧向和横摆两自由度的单车模型，如图1。根据余志生[18]的《汽车理论》，当汽车运动时，汽车质心加速度ay沿Oy轴方向上的分量和线型二自由度模型的运动微分方程分别为：
(7)
(8)
式中：k1，k2分别为前轮侧偏刚度、后轮侧偏刚度；
β为质心侧偏角；
v1为车辆质心速度；
u，v分别为车辆质心速度在车辆坐标系中沿x轴和y轴的分量；
a，b分别为质心至前轴的距离和质心至后轴的距离；
δ为前轮转角；
ωr为车辆横摆角速度；
a1，a2分别为前轮侧偏角和后轮侧偏角；
m为车辆的质量；
IZ为车辆的转动惯量。
(9)
式(9)便是车辆前轮转角和速度的运动学约束，现假设K=0，车辆为中性转向，轴距L为3 m，代入式(9)，以前轮最大转角为因变量，速度为自变量，作出关系图，如图2。由图2可知，前轮最大转角与车速呈反比关系。因此，车辆在某一速度下，只需前轮转角的值位于图2的曲线下方，即可满足车辆的运动学约束。
图1 二自由度车辆运动学模型Fig. 1 Two-degree-of-freedom vehicle kinematics model
图2 前轮角度随速度的变化曲线Fig. 2 Variation curve of front wheel angle changing with speed
2.1 引入道路约束
道路约束是指由右车道中心线产生的引力势场，如图3。在该势场内的车辆会受到指向右车道中心线的引力，以保持沿右车道行驶，同时防止越过道路边界。因此，距右车道中心线越远，引力势能的值越大。
为满足上述要求，采用基于二次函数的势场分布模型。如图3(a)，当车辆位于右车道中心线的左侧时，记车辆质心所在位置为点A，dLM为点A距右车道中心线的距离，ULroad(dLM) 为右车道中心线在其左侧dLM处产生的引力势能，简称左引力势能，取：
(10)
式中：KLroad为左侧道路约束系数，用于左引力势能的缩放。同理，如图3(b)，右引力势能为：
(11)
式中：KRroad为右侧道路约束系数，用于右引力势能缩放；
dRM为点A距右车道中心线的距离。
图3 道路约束示意Fig. 3 Schematic diagram of road restraint
为保证道路的左右边界具有相同的约束作用，道路左右边界处的引力势能的大小应相等，即：
(12)
化简可得：
9×KLroad=KRroad
(13)
于是可得到道路约束引力势场的总分布图，如图4。
图4 道路约束引力势场分布Fig. 4 Distribution of road constrained gravitational potential field
2.2 引入二自由度运动学约束
在经典人工势场法中，自车遇到障碍物进行避障时，人工势场法会规划出现近乎直角的路径，如图5，显然车辆很难跟踪该路径，因此需要在路径规划时引入车辆模型约束。车辆按照某一速度行驶时，可根据图2得到此时的最大前轮转角，算法所规划的路径点如图6，任意相邻的3个路径点角度差Δθi不能大于当前速度最大前轮转角[19]，如式(14):
Δθ≤δmax
(14)
图5 经典人工势场法路径规划结果Fig. 5 Path planning results of the classical artificial potential field method
图6 相邻路径点示意Fig. 6 Schematic diagram of adjacent path points
2.3 改进障碍物模型
在车辆实际行驶过程中，遇到的障碍物多为静止或运动的汽车，其外形可近似为矩形。在人工势场法中，当车辆前方存在障碍车辆需要进行超车路径规划时，由于障碍车辆后方的势场影响范围在沿车辆横向方向(如图7，Y方向)各点大小相等，不利于引导自车进行变道，据此，有学者提出了Yukawa车辆模型。Yukawa模型将车身后部按照距离衰减的斥力势场用一个楔形势场来表示，其分布规律按照某一特定函数来约束，如图8。
图7 车辆-障碍物坐标Fig. 7 Vehicle-obstacle coordinates
图8 Yukawa车辆障碍物模型Fig. 8 Yukawa vehicle obstacle model
为了简化Yukawa模型，采用椭圆形的改进斥力势场模型，从而便于引导后方车辆进行超车或避障时的变道。势场分布图如图9。该模型由车辆矩形、斥力势能峰值区和斥力势场影响范围3个同心图形组成。车辆矩形描述了车辆的二维空间尺寸。
经典人工势场法中的单个障碍物是由若干“作用点”及其势能作用范围组成的，由式(3)可知，“作用点”中心处的斥力势能为峰值(趋近于正无穷)。将前述的“作用点”转化为单个车辆的及其对应的安全范围，即斥力势能峰值不再是存在于“作用点”的中心处，而是“作用点”周围的一整个区域内，即斥力势能峰值区。由图9可知，该斥力势能峰值区由前部半椭圆区、中部矩形区和后部半椭圆区共3部分组成。后半椭圆区是为了引导车辆超车时第1次变道而设定的，相比矩形和圆形的势场范围，其曲率较小且沿道路方向的变化较为缓慢，极易引导车辆进行变道。此外，车辆与前方障碍车辆具有最小的跟车距离，即安全车距。因此，取后半椭圆长轴长AR为：
AR=KERDmin
(15)
式中：KER为后半椭圆的长轴缩放系数，取值为1；
Dmin为安全车距，根据游峰等[20]提出的基于纵向安全距离的超车预警模型(图10)，可确定不同相对速度下的安全距离Dmin。
前部半椭圆区用于在一定程度上引导后方车辆超车后进行并道，同时为了保持一定安全车距。由此确定该半椭圆的长轴长AF为：
AF=KEFDmin
(16)
式中：KEF为前半椭圆的长轴缩放系数。因实际驾驶时超车车辆的速度较快，故超车后可再为抵达安全车距的位置时提前返回右向车道，因此可以取KEF为0.5。
图9 改进障碍物斥力作用域模型Fig. 9 Improved obstacle repulsion scope model
图10 恒速条件下自车与障碍车辆的碰撞区域Fig. 10 The collision area between the own vehicle and the obstacle vehicle under constant speed conditions
由于在结构化道路上，同一车道仅允许1辆车行驶，因此可以取中部矩形区的宽度为车道宽度，长度为车长，两个半椭圆的短轴长与中部矩形区的宽度相等，即半椭圆的短轴长为车道宽度。
斥力势能峰值区外部即为障碍物产生的斥力势场的作用范围，大于此范围，斥力势能为0。考虑驾驶环境为双向车道和实际超车环境，确定斥力势场作用范围边界与斥力势能峰值区边界之间的距离为2 m。
3.1 仿真效果分析
车辆在实际避障和超车时应考虑诸多因素，如障碍物(车流)密度、与障碍物的安全距离以及对向是否有车辆等。为了简化模型，笔者设定驾驶环境为双向车道，且仅存在自车和1辆行驶中的障碍车辆，在该环境下模拟超车路径规划。车辆和道路尺寸参数来自表1。设定自车车速为匀速25 m/s，障碍车辆车速为匀速15 m/s，相对速度大小为10 m/s，根据图2可得到该车速下前轮最大转角约为π/200 rad(约为0.9°)。根据图10，确定纵向安全车距Dmin为23 m。设定目标点为车辆前方Dmin处，而车辆在实际运行过程中，自车和障碍车辆的位置是不断变化的，因此，在设定好目标点后，自车每到达一个位置都要更新目标点的位置，以保持车辆在避障后仍然与周围车辆保持安全车距，然后重新计算到达新的目标点的路径。
表1 仿真参数Table 1 Simulation parameter table
在MATLAB中脚本文件中编写仿真程序，运行结果如图11。从图11中可以看出，改进的人工势场法能较好的完成针对运动障碍物的避障路径规划，其所规划的路径更加接近车辆实际行驶的路径，规划出的路径较为光滑，说明改进的障碍物模型能较好的引导车辆做出变道行为。
图11 仿真结果的6个时间节点Fig. 11 Six time nodes of the simulation results
3.2 实验对比分析
通过对比基于Yukawa模型和改进模型的仿真规划路径来验证所提出的模型的优越性，对比效果如图12。由于改进模型的后部采用椭圆，使得规划出的路径更接近于左侧车道中心位置，超车车辆与被超车车辆之间的横向距离相对较大，增加了超车的安全性；
改进模型的前部也采用椭圆，从图12中可以看出在超车即将完成，改进模型使得超车车辆回到右侧车道的时间比较晚，两车之间的纵向距离明显较大，能更有效避免后车突然加速导致的交通事故，并且规划出的路径更为平滑。
图12 不同模型规划路径对比Fig. 12 Comparison of planning paths of different models
为了验证引进二自由度运动学约束的有效性，在保证其它条件不变的情形下，对没有引进二自由度运动学约束进行仿真，规划出的路径如图13，前轮转角如图14(a)，尽管能实现超车的任务，但是可以看出在超车任务后半程，车头会剧烈晃动，经过计算最大晃动角度可达到1.14 rad(约为65.39°)，严重影响了车辆行驶的稳定性和乘坐舒适性。图14(b)为引入二自由度运动学约束时各路径点处的车辆前轮转角分布图，车辆前轮转角被控制在π/200内，转角变化较小，最大晃动角度只有0.016 rad(约为0.9°),车辆行驶较为稳定。经过对比和分析，发现引进二自由度运动学约束极大得增加了车辆行驶的稳定性，验证了引进二自由度运动学约束的有效性。
图13 无运动学约束规划路径Fig. 13 Planning path without kinematic constraints
图14 前轮转角分布Fig. 14 Front wheel angle distribution
为了验证引进道路约束得有效性，笔者在保证其它条件不变的情形下，删除道路约束对仿真的影响，规划出的路径如图15。对比图12和图15可以看出当超车车辆在左侧车道时，在无道路约束的条件下车辆与道路左侧边界的距离非常近，容易造成车辆脱离道路的风险。通过对比和分析，说明引进道路约束是有效的。
图15 无道路约束规划路径Fig. 15 Planning path without road constraints
对经典人工势场法进行了改进，引入道路约束和运动学约束，使规划的路径更加符合车辆实际行驶的特性；
依据超车安全距离模型改进了障碍物模型，调整了障碍物模型的结构以及作用范围，使其便于引导后方车辆进行变道；
建立了针对移动障碍物中特殊情况——超车避障的路径规划仿真模型。仿真结果表明：采用改进的人工势场法具有良好的避障路径规划效果，能引导车辆做出满足车辆运动学约束的变道行为，且规划出的避障路径更为光滑。
——《势能》文化纵横(2022年3期)2022-09-07“动能和势能”知识巩固中学生数理化·八年级物理人教版(2022年6期)2022-06-05基于Frenet和改进人工势场的在轨规避路径自主规划北京航空航天大学学报(2021年4期)2021-11-24“动能和势能”随堂练中学生数理化·八年级物理人教版(2021年6期)2021-11-22玩转角的平分线中学生数理化·七年级数学人教版(2021年9期)2021-11-20基于改进人工势场法的维修分队机动路线规划方法*指挥控制与仿真(2021年3期)2021-06-15融合前车轨迹预测的改进人工势场轨迹规划研究汽车工程(2021年12期)2021-03-08侧围外板转角深拉伸起皱缺陷研究模具制造(2019年10期)2020-01-06基于势场搜索的无人车动态避障路径规划算法研究北京汽车(2019年4期)2019-09-17动能势能巧辨析中学生数理化·八年级物理人教版(2019年6期)2019-06-25
