CN111338340A - 基于模型预测的无人驾驶汽车局部路径规划方法 - Google Patents

Abstract

本发明公开了一种基于模型预测的无人驾驶汽车局部路径规划方法，步骤1、建立无人驾驶汽车运动学模型；步骤2、在车辆路径规划过程中利用多圆实现车辆及障碍物所构成的驾驶环境的近似；步骤3、进行车道线及道路边沿曲线的拟合；步骤4、构建目标函数J，目标函数的第一项与第二项代表着预测值与主车道中线参考位置之间的偏差；第三项表示预测输出的航向角和主车道中线航向角之间的偏差，以保证车辆航向角和道路航向角相同、前轮转角不会发生突变；第四项保证车辆按照参考速度进行行驶；第五项为了躲避当前车道上存在的障碍物，避免发生碰撞。与现有技术相比，本发明保证了路径规划的快速高效，方便了主车与周围车辆及障碍物之间最短距离的计算。

Description

基于模型预测的无人驾驶汽车局部路径规划方法

技术领域

本发明涉及无人驾驶汽车路径规划领域，尤其是涉及一种带有典型道路场景决策的模型预测无人驾驶汽车局部路径规划方法。

背景技术

无人驾驶汽车的局部路径规划是在全局路径规划的基础上为车辆寻找一条符合车辆运动学和动力学原理、能够躲避动态和静态障碍物、并符合交通规则的实时路线。鉴于上述约束条件，以及外界环境实时变化带来的不确定性，无人驾驶汽车的局部路径规划成为了一个复杂的决策与控制问题。研究高效的适合无人驾驶汽车实时路径规划的算法一直是众多无人驾驶汽车科研者奋斗的目标。

早期应用于无人驾驶汽车路径规划的是一些较为传统的路径规划算法，如人工势场法和模糊逻辑算法。其中，人工势场法将机器人在环境中的运动视为机器人在虚拟人工受力场的运动。障碍物对机器人产生斥力，目标点对机器人产生引力，合力控制机器人的运动方向。而设计者通过建立引力场和斥力场函数就能实现路径的寻优。简单的结构和良好的实时性让人工势场法在实时避障和平滑的轨迹控制方面取得广泛的应用，但其容易陷入局部最优解，并且当目标点附近存在障碍物时，机器人无法到达目标点。模糊逻辑算法通过模拟驾驶员的驾驶经验，将感知和动作进行结合，根据实时的传感器信息查表得到规划信息，从而实现路径规划。模糊逻辑算法免去了复杂的数学建模过程，具有很好的稳定性和连续性。但模糊规则的总结困难，应变性差。隶属度函数、控制规则及在线调整方法都为其实际应用带来很大难题。

现在应用较为广泛的是基于图搜索的最短路径规划算法，如：D*算法和A*算法。图形学的方法提供了一种简单快捷的建模方式，搜索的效率和最优性较以往有了一个较大幅度的提升，但规划轨迹的质量在很大程度上取决于图形的分辨率。而如果单纯提高分辨率会造成搜索时间成倍的增加，从而不能满足操作系统实时性的要求。此外，图搜索的方法也不能有效的考虑车辆自身的动力学约束，规划出来的路径可能是车辆无法正常行驶的。基于随机采样技术的随机路径图法(PRM)和快速搜索随机树法(RRT)可以有效解决高维空间和复杂约束中的路径规划问题。然而高计算量的采样过程却成为它在实际的应用过程中的最大阻碍。基于轨迹优化的无人驾驶汽车路径规划方法在实际的应用过程中越来越受到大家的青睐。其核心思想是将无人驾驶汽车的路径规划过程看作一个优化问题。在考虑车辆自身的性能约束、乘客的舒适性约束和外界真实的驾驶环境约束的情况下，优化求解出一条当前情况下最合适的行驶路线。模型预测控制可以求解一系列的带有约束的有限时域优化问题，为处理车辆自身的约束和乘客的舒适性要求提供了便利。模型预测控制的预见性使得它在处理当前交通状况的同时，还有效地考虑了今后一段时间的交通情况的变化，符合人类驾驶员的预判思维，为车辆的及时准确快速判断提供了依据。另外，智能仿生学算法，如遗传算法、神经网络算法、粒子群算法、蚁群算法等在处理复杂动态环境的路径规划问题时也发挥了重要作用，配合其他算法往往可以取得不错的规划效果。

本发明提出的基于模型预测的无人驾驶汽车局部路径规划方法不仅具有明确的理论意义、而且具有很强的实际应用价值与现实意义。

发明内容

本发明目的是提出了一种基于模型预测的无人驾驶汽车局部路径规划方法，将模型预测应用到无人驾驶汽车局部路径规划过程中，在考虑周围环境的实时变化和车辆自身约束的情况下，实现了车道线的四次多项式拟合、曲率求解和凹凸性判别，完成了直道保持、弯道减速、换道超车、跟驰行驶和主车道避障的操作。

本发明的基于模型预测的无人驾驶汽车局部路径规划方法，该方法包括以下步骤：

步骤1、建立无人驾驶汽车运动学模型，表达式为：

其中，x,y表示车辆在坐标系中的位置，

表示车辆的航向角，v表示当前车辆的速度，δ表示前轮转向角，L表示车辆的轴距，T表示采样时间；

步骤2、在车辆路径规划过程中利用多圆实现车辆及障碍物所构成的驾驶环境的近似，具体包括以下过程：

2-1、确定车辆的长度和宽度。本发明实施例设置被控车辆由长l＝4.815m，宽 w＝1.875m的矩形来表示；

2-2、应用进一法确定所需圆的个数g，表达式为

将矩形的长进行g等分，形成g个小矩形；

2-3、做小矩形的外接圆，外接圆的半径r的表达式为：

2-4、设矩形左下角坐标为(0,0)，确定各个圆的圆心坐标(x₀,y)₀，表达式为：

步骤3、进行车道线及道路边沿曲线的拟合，具体处理为：通过高精度地图和车载传感器获得在预测时域内车道线和道路边沿在建立的坐标系下的准确位置，通过计算，得到主车即将行驶车道的中间坐标，即两条车道线或车道线和路沿的中间位置，作为主车道中线；然后将车道线、道路边沿和主车道中线用四次多项式

进行拟合，得到四次多项式每一项系数的具体数值；接下来，对主车道中线进行处理，包括以下步骤：

3-1、进行参考点的选取：

首先，设定当前道路环境下允许行驶的最高速度v_top，计算出一个周期T内车辆行驶的距离S，表达式如下：

s＝v_top×T (5)

然后，结合车辆当前的位置和航向角，根据公式(6)依次推断出参考点的位置和航向角：

其中，x_line|i表示第i个参考点在X轴方向的分量，同理，y_line|i表示第i个参考点在Y轴方向的分量，

表示车辆在第个参考点时车i的当前航向角，a,b,c,d,e表示四次多项式的系数；

3-2、计算在每一个参考点

处的曲率值，计算公式如下：

其中，k_line|i表示在第i个参考点处的曲率值，y′_line|i和y″_linei分别表示在第i个参考点处，y对x的一阶导数和二阶导数；

通过曲率大小和曲线凹凸性，初步判断车辆在参考点前轮打角的幅度和方向，为后续的优化求解提供初始值参考；

步骤4、构建目标函数J，表达式如下：

目标函数的第一项与第二项代表着预测值与主车道中线参考位置之间的偏差；第三项表示预测输出的航向角、主车道中线航向角之间的偏差，保证车辆航向角和道路航向角相同以及保证前轮转角不会发生突变；第四项保证车辆按照参考速度进行行驶；第五项的作用是为了躲避当前车道上存在的障碍物，避免发生碰撞；

约束条件表达如下：

d_other>0 (9a)

δ_min≤δ≤δ_max (9b)

v_min≤v≤v_max (9c)

其中，N表示车辆运动过程中的总步数，

表示第j步主车道中线参考点的位置和航向角，

表示第j步模型预测输出的路径规划预测值，v_ref表示当前道路环境下的车辆参考速度，v_p|j为第j步输出的车ω₁,ω₂,ω₃,ω₄,ω₅辆速度， P(d_j,v_p|j)表示避障函数，作为权重因子；

避障函数的计算公式如下：

约束条件公式(9a)中，d_other表示主车与周围车辆、障碍物以及道路边沿之间的距离，要求d_other＞0；

约束条件公式(9b)和(9c)为来自车辆自身的物理条件的约束，δ_min和δ_max分别表示车辆前轮转角的最小值和最大值，v_min和v_max分别表示车辆运行速度的最小值和最大值，保证路径规划出来的轨迹在实际的车辆运行过程中是可以实现的。

与现有技术相比，本发明保证了路径规划的快速高效，方便了主车与周围车辆及障碍物之间最短距离的计算。

附图说明

图1为本发明的基于模型预测的无人驾驶汽车局部路径规划方法整体流程图；

图2为车辆运动学模型示意图；

图3为车辆形状的多圆形近似模型示意图；

图4为圆弧换道多项式曲线拟合模型示意图；

图5为直道高速行驶多项式曲线拟合模型示意图；

图6为弯道减速多项式曲线拟合模型示意图；

图7为换道超车多项式曲线拟合模型示意图；

图8为规划避障多项式曲线拟合模型示意图；

图9为成功避障多项式曲线拟合模型示意图；

图10为减速跟驰多项式曲线拟合模型示意图；

图11为跟驰过程中满足换道超车条件、开始超车多项式曲线拟合模型示意图。

具体实施方式

为了更清楚地说明本发明的目的、技术方案及优点，以下从模型预测局部路径规划的基本框架、路径规划过程中典型的道路环境进行的处理、模型预测路径规划典型道路环境下的仿真验证等几个方面来对本发明作进一步解释说明。应当理解，此处所描述的具体设计方法仅仅用以解释本发明，并不用于限定本发明。

一、模型预测局部路径规划的基本框架

模型预测处理路径规划，是将其等效为一个优化问题。这种方法最大的优点是可以很方便的将车辆自身的约束和人们对路径的各种期望都考虑进去。为了减少计算的复杂性，提高运算速率，一般情况下，路径规划层采用较为简单的车辆运动学模型，而在控制层采用较为复杂的动力学模型，这样做的好处是兼顾了控制性能和计算速度。另外，假定环境信息，包括车道线、道路标识牌、障碍物位置及大小、周围车辆位置与速度等信息，通过车载传感器与高精度地图可以全部得到。

如图1所示，为本发明的基于模型预测的无人驾驶汽车局部路径规划方法整体流程图。该流程具体包括以下步骤：

步骤1、建立无人驾驶汽车运动学模型，表达式为：

其中，x,y表示车辆在坐标系中的位置，

表示车辆的航向角，v表示当前车辆的速度，δ表示前轮转向角，L表示车辆的轴距，T表示采样时间；

步骤2、在车辆路径规划过程中利用多圆实现车辆及障碍物的近似，具体包括以下过程：

2-1、确定车辆的长度和宽度；

2-2、应用进一法确定所需圆的个数g，表达式为

代入本发明实施例的参数设置得到

应用进一法之后g＝3，即所需近似圆的个数为3，将表示车辆近似外形的矩形的长进行g等分，形成g个小矩形；

2-3、做小矩形的外接圆，外接圆的半径r的表达式为：

将本发明实施例数据代入，得到所需近似圆的半径为r＝1.2341m；

2-4、设矩形左下角坐标为(0,0)，则可确定各个圆的圆心坐标(x₀,y)₀，表达式为：

代入本发明实施例数据，可得近似被控车辆的三个圆的圆心坐标分别为：(0.8025， 0.9375)、(2.4075，0.9375)和(4.0125，0.9375)；

利用本步骤，可将常见的驾驶环境，如路障、行人和非机动车，采用多个圆来近似；

步骤3、进行车道线及道路边沿曲线的拟合，具体处理为：通过高精度地图和车载传感器获得在预测时域内车道线和道路边沿在建立的坐标系下的准确位置。通过计算，得到主车即将行驶车道的中间坐标，即两条车道线或车道线和路沿的中间位置，作为主车道中线；然后将车道线、道路边沿和主车道中线用四次多项式

进行拟合，得到四次多项式每一项系数的具体数值；接下来，对主车道中线进行处理，包括以下步骤：

3-1、进行参考点的选取：

首先，设定当前道路环境下允许行驶的最高速度v_top，计算出一个周期T内车辆行驶的距离S，表达式如下：

s＝v_top×T (5)

然后，结合车辆当前的位置和航向角，根据公式(6)依次推断出参考点的位置和航向角：

其中，x_line|i表示第i个参考点在X轴方向的分量，同理，y_line|i表示第i个参考点在Y轴方向的分量，

表示车辆在第个参考点时车i的当前航向角，a,b,c,d,e表示四次多项式的系数；

3-2、计算在每一个参考点

处的曲率值，计算公式如下：

其中，k_line|i表示在第i个参考点处的曲率值，y′_line|i和y″_line|i分别表示在第i个参考点处，y对x的一阶导数和二阶导数；

通过曲率大小和曲线凹凸性，初步判断车辆在参考点前轮打角的幅度和方向，为后续的优化求解提供初始值参考，加快优化求解的速度；

步骤4、构建目标函数J，表达式如下：

subject to

d_other>0 (9a)

δ_min≤δ≤δ_max (9b)

v_min≤v≤v_max (9c)

其中，N表示车辆运动过程中的总步数，

表示第j步主车道中线参考点的位置和航向角，

表示第j步模型预测输出的路径规划预测值，v_ref表示当前道路环境下的车辆参考速度，v_p|j为第j步输出的车ω₁,ω₂,ω₃,ω₄,ω₅辆速度， P(d_j,v_p|j)表示避障函数，作为权重因子。目标函数的第一项与第二项代表着预测值与主车道中线参考位置之间的偏差；第三项表示预测输出的航向角和主车道中线航向角之间的偏差，其可以保证车辆航向角和道路航向角相同，并且由于航向角和前轮转角之间存在直接联系，所以可以保证前轮转角不会发生突变；第四项保证车辆按照参考速度进行行驶；第五项的作用是为了躲避当前车道上存在的障碍物，避免发生碰撞。

约束条件式(9a)中，d_other表示主车与周围车辆、障碍物以及道路边沿之间的距离，要求d_other＞0，进一步保证了主车与周围物体之间不会发生碰撞，保证了行车的安全。关于主车与周围车辆(障碍物)的计算，利用将周围车辆(障碍物)等效为一个或多个圆，依次计算主车的每个圆心和周围车辆每个圆心之间的距离进行比较，选择出最短的距离，即为主车和周围车辆(障碍物)之间的距离。对于主车和道路边沿之间的距离，将道路边沿的四次曲线的预测范围内部分按照车道中线采样的方法离散成一个个的点，按照计算主车与周围车辆(障碍物)距离同样的方法，得到主车与道路边沿之间的距离。约束条件公式(9b)和(9c)为来自车辆自身的物理条件的约束，δ_min和δ_max分别表示车辆前轮转角的最小值和最大值，v_min和v_max分别表示车辆运行速度的最小值和最大值，保证路径规划出来的轨迹在实际的车辆运行过程中是可以实现的。

如图3所示，为车辆形状的多圆形近似模型示意图。为了有效地解决这个问题，本发明用多个圆形来近似车辆。被控车辆和周围的车辆建模为圆形或者矩形，方便计算主车和周围车辆之间的最短距离，但与实际的车辆形状偏差较大，会造成路径规划的解非最优，甚至出现没有可行解的情况。而矩形可以更为精确地描述车辆的形状，但计算主车和周围车辆的最短距离过于复杂。

如图4至11所示，为各种具体情形的多项式曲线拟合模型，其中粗线为对道路边沿的拟合，带“①②”标注的细线为车道线的拟合和绿色线段分别为对道路边沿、车道线和主车道中线的拟合。虚线的多圆代表主车，实线的多圆表示周围车辆。“.”表示模型预测路径规划生成的参考轨迹点。

对道路环境中遇到的各种场景进行有效的处理，是保证模型预测路径规划高效有序运行的关键。利用本发明的基于模型预测的无人驾驶汽车局部路径规划方法进行路径规划过程中典型的道路环境的处理，下面对道路环境中常见的场景进行针对性处理。

1、针对“弯道减速”场景的处理：

为了保证车辆在弯道行驶时的安全平稳，将预测时域之内各个参考点的曲率大小考虑到当前参考速度的设定之中，如公式(11)所示。

其中，v_ref表示车辆当前时刻的参考速度，v_max表示设定的车辆最高行驶速度；

表示在预测时域内所有参考点曲率的，ω_k表示可调节权重因子；

2、针对“换道超车与跟驰”场景的处理：

主车在行进过程中，如果与前方车辆的间距满足开始换道的距离L₁，则主车开始考虑相邻车道的车流状况(也就是说如果前方没有车辆或障碍物的时候，主车是不用考虑相邻车道车辆的运行状况)，观察邻道前方车辆与主车纵向距离L₂是否满足有价值的换道，所谓有价值的换道是指车辆换到相邻车道以后可以保证更加通畅的行驶，称之为有价值的换道。如果满足则主车道中线切换到相邻车道准备换道超车，如果不满足则主车道中线继续保持在当前车道，并开始减速，直到和前方车辆速度一样，跟车行驶。在跟车行驶的过程中判断相邻车道的车流状况，在满足最小安全换道距离L₀的前提下，如果可以进行有价值的换道，则开始换道超车。

1)最小安全换道距离，即主车与前方车辆之间允许换道超车的最短距离。本发明采用双圆弧换道路径法估算最小安全换道距离L₀。如图5所示的直道高速行驶多项式曲线拟合模型示意图中：O为圆心，S为主车质心到车道线的距离，

为圆弧的曲率半径，v_Ax为主车纵向速度，a_Ay为主车侧向加速度，为了在换道过程中保证乘客的舒适性和车辆行驶的平稳性，通常取a_Ay≤0.2g。由几何关系可以计算到达临界碰撞点的时刻，即主车由A₀点到A₁点行驶的时间

则最小安全换道距离

v_AB为主车和前方车辆的相对速度。而为了预留一定的安全余量，并且充分考虑本车速度，可取最小安全换道距离

其中ξ为安全系数，通常取ξ＝1.0～1.3；t_h为车间时距，通常取t_h＝2.0s～3.0s。另外，最小安全换道距离同样适用于主车和相邻车道后车之间。

2)开始换道的距离。为了计算的方便简单，可以取开始换道距离L₁为3到5倍的L₀。

3)有价值的换道。假设主车正前方车辆行驶速度为v_B，与主车之间的纵向距离为L_AB；主车侧前方车辆行驶速度为v_C，与主车之间的纵向距离为L_AC；当满足

并在最小安全换道距离的前提下，称之为有价值的换道。

3、针对“主车道避障”场景的处理：

当主车行驶车道出现障碍物时，根据d_other(d_other包括三种距离即主车与周围车辆距离、主车与障碍物距离和主车与道路边沿距离，统一表示为d_other)，判断出障碍物边缘与车道线(车道边沿)之间的最短距离D，如果最短距离D大于车辆自身的宽度w，则进行本车道的避障处理，即此时主车道中线保持在当前车道不变，如果不满足则观察相邻车道是否满足换道条件，如果满足则将主车道中线切换到相邻车道进行换道处理，如果两者都不满足，则减速停车。

在本车道避障时，目标函数里面的避障函数P(d_j,v_p|j)将发挥主要作用。避障函数的计算公式如下：

其中，d_j为在第j步预测时主车和障碍物之间的距离。

为了验证本发明的基于模型预测的无人驾驶汽车局部路径规划算法的有效性，在Matlab环境下搭建了包括直道高速行驶、弯道减速、换道超车、避障行驶、减速跟驰等系列场景进行验证，并用粒子群算法对目标函数进行优化求解。粒子群算法可以很好的利用前面介绍的曲线拟合得到的参考点曲率和曲线凹凸性，加快优化求解速度。

1、直道高速行驶

在如图6所示的弯道减速多项式曲线拟合模型示意图中，为车辆在直道且没有障碍物环境下高速行驶的场景，虚线的多圆代表主车，实线的多圆表示周围车辆。“.”表示模型预测路径规划生成的参考轨迹点。在每辆车辆的上方实时显示着当前车前车速。主车的最高参考速度V_top设置为18m/s。从图6中可以看出主车的实时速度达到了 18.07m/s，实现了在直道无障碍物条件下的高速行驶。

2、弯道减速

在如图7所示换道超车多项式曲线拟合模型示意图中，车辆经过弯道时的减速场景。可以看出当道路曲率变大的时候，车辆较最高参考速度V_top有了一个明显的降速，变为15.25m/s，保证了车辆在弯道行驶时的安全。

3、换道超车

如图8所示的规划避障多项式曲线拟合模型示意图中，一辆行驶缓慢的车辆出现在了主车行驶的道路前方，阻碍了主车保持较高速度行驶，并且相邻左侧车道满足有价值的换道条件，主车道中线切换到左侧车道，主车进行换道超车。

4、避障行驶

如图9所示的成功避障多项式曲线拟合模型中，主车行驶道路的侧前方出现了一个黑色圆形障碍物。经过避障流程分析，主车可在当前行驶道路内完成避障操作。如图10所示的减速跟驰多项式曲线拟合模型示意图中，发现模型预测规划生成的路径对障碍物进行了躲避，成功避开了障碍物，保证了车辆的安全行驶。

5、减速跟驰

如图10所示的减速跟驰多项式曲线拟合模型示意图中，跟驰过程中满足换道超车条件、开始超车多项式曲线拟合模型示意图，主车在行驶过程中发现前方存在一辆行驶较慢的车辆。然而主车旁边的相邻车道存在其他车辆，不满足换道超车条件。这时主车开始减速到与前方车辆速度大致相等，进行跟驰行驶。如图11所示的跟驰过程中满足换道超车条件、开始超车多项式曲线拟合模型示意图中，在跟驰过程中，当主车发现相邻车道出现了有价值的换道，主车立即进行换道超车。

Claims (1)

1.一种基于模型预测的无人驾驶汽车局部路径规划方法，其特征在于，该方法包括以下步骤：

步骤1、建立无人驾驶汽车运动学模型，表达式为：

其中，x,y表示车辆在坐标系中的位置，

表示车辆的航向角，v表示当前车辆的速度，δ表示前轮转向角，L表示车辆的轴距，T表示采样时间；

步骤2、在车辆路径规划过程中利用多圆实现车辆及障碍物所构成的驾驶环境的近似，具体包括以下过程：

2-1、确定车辆的长度和宽度；

2-2、应用进一法确定所需圆的个数g，表达式为

将矩形的长进行g等分，形成g个小矩形；

2-3、做小矩形的外接圆，外接圆的半径r的表达式为：

2-4、设矩形左下角坐标为(0,0)，确定各个圆的圆心坐标(x₀,y)₀，表达式为：

步骤3、进行车道线及道路边沿曲线的拟合，具体处理为：通过高精度地图和车载传感器获得在预测时域内车道线和道路边沿在建立的坐标系下的准确位置，通过计算，得到主车即将行驶车道的中间坐标，即两条车道线或车道线和路沿的中间位置，作为主车道中线；然后将车道线、道路边沿和主车道中线用四次多项式

进行拟合，得到四次多项式每一项系数的具体数值；接下来，对主车道中线进行处理，包括以下步骤：

3-1、进行参考点的选取：

首先，设定当前道路环境下允许行驶的最高速度v_top，计算出一个周期T内车辆行驶的距离S，表达式如下：

s＝v_top×T (5)

然后，结合车辆当前的位置和航向角，根据公式(6)依次推断出参考点的位置和航向角：

其中，x_line|i表示第i个参考点在X轴方向的分量，同理，y_line|i表示第i个参考点在Y轴方向的分量，

表示车辆在第个参考点时车i的当前航向角，a,b,c,d,e表示四次多项式的系数；

3-2、计算在每一个参考点(x_line|i,y_line|i,

)处的曲率值，计算公式如下：

其中，k_line|i表示在第i个参考点处的曲率值，y′_line|i和y″_line|i分别表示在第i个参考点处，y对x的一阶导数和二阶导数；

通过曲率大小和曲线凹凸性，初步判断车辆在参考点前轮打角的幅度和方向，为后续的优化求解提供初始值参考；

步骤4、构建目标函数J，表达式如下：

目标函数的第一项与第二项代表着预测值与主车道中线参考位置之间的偏差；第三项表示预测输出的航向角、主车道中线航向角之间的偏差，保证车辆航向角和道路航向角相同以及保证前轮转角不会发生突变；第四项保证车辆按照参考速度进行行驶；第五项的作用是为了躲避当前车道上存在的障碍物，避免发生碰撞；

约束条件表达如下：

d_other>0 (9a)

δ_min≤δ≤δ_max (9b)

v_min≤v≤v_max (9c)

其中，N表示车辆运动过程中的总步数，(x_r|j,y_r|j,

)表示第j步主车道中线参考点的位置和航向角，(x_p|j,y_p|j,

)表示第j步模型预测输出的路径规划预测值，v_ref表示当前道路环境下的车辆参考速度，v_p|j为第j步输出的车ω₁,ω₂,ω₃,ω₄,ω₅辆速度，P(d_j,v_p|j)表示避障函数，作为权重因子；

避障函数的计算公式如下：

约束条件公式(9a)中，d_other表示主车与周围车辆、障碍物以及道路边沿之间的距离，要求d_other＞0；

约束条件公式(9b)和(9c)为来自车辆自身的物理条件的约束，δ_min和δ_max分别表示车辆前轮转角的最小值和最大值，v_min和v_max分别表示车辆运行速度的最小值和最大值，保证路径规划出来的轨迹在实际的车辆运行过程中是可以实现的。

Images