CN111260956A - 一种基于模型预测控制的车辆自动换道规划与控制方法 - Google Patents

Abstract

本发明提出的一种基于模型预测控制的车辆自动换道规划与控制方法。模型预测控制适用于解决多约束多目标的优化问题，可以同步实现换道轨迹规划与控制。本发明将换道运动规划表达为一个模型预测控制问题，采用车辆动力学模型描述换道运动，将车辆动力学极限和无碰撞安全车间距离作为优化问题的强制约束，将跟车距离作为优化问题的非强制约束，将换道轨迹的平滑性和快速性作为优化目标，从而实现了最优换道运动规划。本发明提出的方法简化了换道规划算法复杂度，联合使用软硬约束在保证换道安全的同时提高了换道规划求解效率。

Description

一种基于模型预测控制的车辆自动换道规划与控制方法

技术领域

本发明涉及一种自动驾驶运动规划技术领域，具体涉及一种基于模型预测控制的车辆自动换道规划与控制方法。

背景技术

车辆换道是一种风险极高的驾驶行为，驾驶员需要对复杂的交通场景进行交互，同时完成车辆的横纵向控制。换道行为引发的交通事故造成了极大的人员伤亡与财产损失，在高级辅助驾驶***或自动驾驶***中实现车辆自动换道具有重要意义，换道运动规划是自动换道功能的重要环节。

在现有自动换道方法中，自动换道运动规划任务通常分为换道轨迹规划和轨迹跟踪控制两个步骤。为了保证规划出的换道轨迹可以被车辆跟踪，在轨迹规划中需要考虑车辆动力学和车辆运动的动力学约束。同时在轨迹跟踪控制中也需要根据车辆动力学设计控制算法，因此这种分解式的规划方法考虑了两次车辆动力学，增加了算法的复杂度，造成了计算资源的浪费。因此，将轨迹规划与控制统一在一个框架中实现同步的规划和控制可以减少算法的设计和计算资源，同时减少自动驾驶算法***的复杂度。模型预测控制可以解决多约束和多目标优化问题，适用于实现同步规划和控制。

Sugie等采用非线性模型预测控制方法，使用二自由度车辆动力学模型，将车辆动力学极限和安全跟车距离作为硬约束，将舒适性作为优化目标，实现车辆换道控制。由于采用非线性动力学模型和非线性约束，使得换道规划的求解非常复杂，规划的实时性不足。

Nilsson等采用线性模型预测控制方法，将车辆横纵向运动解耦，采用匀加速运动学模型描述车辆运动，采用线性的跟车距离约束，实现了换道规划的快速求解。然而该方法所使用的运动学模型过于简化，不符合车辆动力学要求，同时与周围车辆保持了较大的安全距离，策略过于保守，使得换道轨迹求解常常无解。

Carvalho等同样采用线性模型预测控制方法，通过线性化方法将二自由度车辆动力学模型线性化，同时在安全跟车距离约束中加入了碰撞概率参数来调解规划策略的保守与激进程度，在保证车辆动力学的同时提高了换道轨迹的可解性。但这种在约束条件里添加概率参数的方式增加了优化问题的复杂度。

发明内容

本发明的目的是为了提高自动换道运动规划中多约束多目标优化的鲁棒性，本发明提出了一种基于模型预测控制的车辆自动换道规划与控制方法。

为了实现上述目的，本发明采用如下技术方案：

本发明提出的一种基于模型预测控制的车辆自动换道规划与控制方法，其特征在于，包括以下步骤：

S1构建车辆动力学模型

S1.1构建车辆动力学模型如下：

式中，符号

表示变量对时间的一阶导数；X为自车的运动状态向量，X＝[x y θv]，包括自车的纵向位置x、横向位置y、航向角θ和速度v；L为自车的轴距，T_L为自车由速度v变换到期望速度v_d所需时间；u为自车换道控制输入向量，u＝[v_d δ]，包括自车期望速度v_d和前轮转角δ；

S1.2将车辆动力学模型线性化，得到线性偏差模型

设存在一满足步骤S1.1构建的车辆动力学模型、位于目标车道内并与自车初始纵向位置和行驶速度相等的参考车辆，令参考车辆换道控制输入向量为u_r＝[v_dr δ_r]，包括参考车辆换期望道速度v_dr和前轮转角δ_r；令参考车辆的运动状态为X_r＝[x_r y_r θ_r v_r]，包括参考车辆的纵向位置x_r、横向位置y_r、航向角θ_r和速度v_r；

根据自车和参考车辆的状态偏差

和换道控制输入偏差

将车辆动力学模型进行线性化，得到连续车辆状态空间方程

表达式如下：

式中，

f_x，r为连续***矩阵，表达式如下：

f_u,r为连续输入矩阵，表达式如下：

S1.3使用一阶保持变换将步骤S1.2中得到的连续车辆状态空间方程离散化，得到离散的车辆预测状态为：

式中，

和

分别是离散时刻t_k的车辆状态偏差向量和控制偏差向量；k＝1,2,…,N，N为离散的预测范围，N＝T_final/Δt，T_final,Δt分别为预测换道时长和离散时间步长；

A为离散***矩阵，计算公式如下：

B是离散输入矩阵，计算公式如下：

根据离散的车辆动力学模型，在换道过程中，任意离散时刻k的预测状态为：

将换道过程中离散的车辆预测状态表示为矩阵形式：

式中，

为初始的状态偏差向量，计算公式如下：

其中，X₀为自车初始状态，X₀＝[x₀ y₀ θ₀ v₀]^T，x₀,y₀,θ₀,v₀分别为自车初始时刻的位置、航向角和行驶速度；X_r0为参考车辆初始状态，X_r0＝[x₀ y_d θ_r0 v₀]^T，y_d为目标车道中心线的横向位置，θ_r0为参考车辆的初始航向角；

S2构建换道问题约束条件

S2.1构建换道安全距离约束

在换道过程中，设自车与前车和后车之间需保持的纵向安全距离为d_s，自车道前后车辆在t_k时刻的预测纵向位置为

和

目标车道前后车辆在t_k时刻的预测纵向位置为

和

自车道左右边界线的横向位置为y_L和y_R，目标车道左右边界线的横向位置为y_TL和y_TR；将自车换道行为分为换道准备阶段、进行阶段和调整阶段，各阶段所对应的离散预测范围分别为[0,N_pre)、[N_pre,N_post)和[N_post,N_final]，N_pre,N_post,N_final分别为各阶段对应的离散范围阈值，N_final＝N；各阶段的换道安全距离约束分别如下：

a)换道准备阶段

自车保持在当前车道内并与同车道前后车辆保持纵向安全距离，自车的位置(x_k,y_k)边界约束由当前车道左右边界和前后车辆的位置确定，表达式如下：

0≤k＜N_pre

b)换道进行阶段

自车从当前车道运动到目标车道，均需与两条车道上的车辆保持纵向安全距离，自车的位置(x_k,y_k)边界由当前车道左边界线、目标车道右边界线和两条车道内前后车辆位置确定，表达式如下：

N_pre≤k＜N_post

c)换道调整阶段

自车已经运动到目标车道内，需保持在目标车道内并与目标车道前后车辆保持纵向安全距离，自车位置(x_k,y_k)边界由目标车道边界线和目标车道前后车辆的位置确定，表达式如下：

N_post≤k≤N_final

S2.2构建跟车安全距离约束

设同一车道的前后车辆间保持的跟车距离为d_f，计算公式如下：

d_f＝d₀+τ_fv_f

式中，d₀为最小跟车距离，τ_f为跟车时距，v_f为跟车情景中后车的当前速度；

引入纵向跟车距离约束松弛量，构建跟车安全距离约束分别如下：

a)对于同车道前车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k同车道前车跟车距离约束松弛量；

b)对于同车道后车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k同车道后车跟车距离约束松弛量；

c)对于目标车道前车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k目标车道前车跟车距离约束松弛量；

d)对于目标车道后车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k目标车道后车跟车距离约束松弛量；

S2.3构建控制输入约束如下：

δ_min＜δ^k＜δ_max

式中，v_min,v_max分别为自车速度变化量的最小值和最大值，δ_min,δ_max分别为自车前轮转角的最大值和最小值；

S3构建换道问题目标函数

设换道问题目标函数如下：

式中，

表示换道舒适性和快速性的代价函数，计算公式如下：

其中，w₁～w₅分别为权重系数；符号|·|₂表示二范数；

表示松弛量的代价函数，w_l为松弛量代价的权重系数，L_k为跟车距离约束松弛向量，

将换道问题目标函数表示为矩阵形式如下：

式中，Q＝diag(0,w₁,w₂,w₃)，为状态权重矩阵；R＝diag(w₄,w₅)，为控制输入权重矩阵；

将步骤S1.3中矩阵形式的车辆预测状态代入矩阵形式的换道问题目标函数中，得到以下二次规划目标函数：

将上式整理得到二次规划目标函数为：

式中，

V为换道规划与控制输入向量

H为黑塞Hessian矩阵，

I为单位矩阵；

F梯度向量，

S4求解步骤S3构建的二次规划目标函数，得到最优的换道规划与控制输入向量V*，计算公式如下：

本发明的特点及有益效果：

本发明采用线性模型预测控制，使用非线性车辆动力学模型，采用参考***偏差实现模型的线性化，加快了求解速度。此外，联合使用强制约束和非强制约束，将车辆换道所需的最小间距作为优化问题的强制约束，将纵向跟车距离作为非强制约束，保证安全性的同时提升问题的可解性，从而提高了换道规划算法的鲁棒性。

附图说明

图1是本发明提出的一种车辆自动换道规划与控制方法的流程图。

图2是本发明所述方法中车辆安全约束边界图。

图3是本发明所述方法中换道不同阶段的边界约束图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不限定本发明的保护范围。

为了更好地理解本发明，以下详细阐述一个本发明一种基于模型预测控制的车辆自动换道规划与控制方法的应用实例。

参见图1，本发明提出的一种基于模型预测控制的车辆自动换道规划与控制方法，该方法的输入包括感知***、障碍物运动预测算法和车辆行为决策的输出。感知***通过传感器及感知算法(为本领域的公知技术，如可采用综述——杜伟松.基于机器视觉的智能汽车环境感知算法研究综述[J].汽车工业研究,2018(04):26-33中公开的算法)实时获取自车周围环境中其他车辆的运动状态和道路结构信息，车辆运动状态包括世界坐标系下车辆几何中心的坐标位置、速度和加速度，道路结构信息包括道路边界和车道线边界线位置和车道宽度、道路曲率等信息。障碍物运动预测算法(为本领域的公知技术，如可采用综述Stéphanie Lefèvre,Vasquez D,Laugier C.A survey on motion prediction and riskassessment for intelligent vehicles[J].ROBOMECH Journal,2014,1(1):1-14.中公开的算法)根据其他车辆当前时刻及历史状态，预测未来时刻其他车辆的位置，预测位置包括坐标的期望值和方差。车辆行为决策算法(为本领域的公知技术，如可采用文章Wei J,Snider J M,Gu T,et al.A Behavioral Planning Framework for Autonomous Driving[C]//Intelligent Vehicles Symposium.IEEE,2014.中公开的算法)以安全性和交通效率为目标，决定车辆下一步的驾驶行为，驾驶行为包括车辆目标车道和目标车速v_t，当车辆行为决策输出为换道，即目标车道不是当前车道时，采用本发明提出的车辆自动换道规划与控制方法实现车辆换道。所有输入状态量均在以自车所在车道中心线的Frenet坐标系下表达，其中X方向即为沿车道中心线切线方向，Y方向为垂直车道线方向。本发明提出的的车辆自动换道规划与控制方法，具体包括以下步骤：

S1构建车辆动力学模型

S1.1使用车辆动力学模型描述自动驾驶汽车的运动，车辆动力学模型的表达式如下：

式中，符号

表示变量对时间的一阶导数；X为自车的运动状态向量，X＝[x y θv]，包括自车的纵向位置x、横向位置y、航向角θ和速度v；L为自车的轴距，T_L为自车由速度v变换到期望速度v_d所需时间；u为自车换道控制输入向量，u＝[v_d δ]，包括自车期望速度v_d和前轮转角δ；自车的运动状态向量关于时间的一阶导数

是关于自车的运动状态向量X和自车换道控制输入向量的函数。

S1.2将车辆动力学模型线性化，得到线性偏差模型

设存在一满足步骤S1.1构建的车辆动力学模型、位于目标车道内并与自车初始纵向位置和行驶速度相等的参考车辆，令参考车辆换道控制输入向量为u_r＝[v_dr δ_r]，包括参考车辆换期望道速度v_dr和前轮转角δ_r，本实施例中取v_dr＝v_t,δ_r＝0，即令参考车辆以行为决策算法得到的目标速度沿车道中心线行驶，令参考车辆的运动状态为X_r＝[x_r y_r θ_r v_r]，包括参考车辆的纵向位置x_r、横向位置y_r、航向角θ_r和速度v_r，本实施例中θ_r＝0；

根据自车和参考车辆的状态偏差

和换道控制输入偏差

将车辆动力学模型进行线性化，得到连续车辆状态空间方程

表达式如下：

式中，

f_x,r为连续***矩阵(system matrix)，表达式如下：

f_u,r为连续输入矩阵(input matrix)，表达式如下：

f_x,r和f_u,r是通过将方程

在参考状态和参考输入(X_r,u_r)附近泰勒展开、保留2阶项得到，具体如下：

式中，

和

是函数f相对于状态量X和控制量u在参考状态和参考输入处的雅可比矩阵。

S1.3使用一阶保持变换将步骤S1.2中得到的连续车辆状态空间方程离散化，得到离散的车辆状态空间方程：

式中，

和

分别是离散时刻t_kk的车辆状态偏差向量和控制偏差向量；k＝1,2,…,N，N为离散的预测范围，N＝T_final/Δt，T_final,Δt分别为预测换道时长和离散时间步长，本实施例中T_final＝4s，Δt＝0.2s，N＝20。

A为离散***矩阵，计算公式如下：

B是离散输入矩阵，计算公式如下：

根据离散的车辆动力学模型，在换道过程中，任意离散时刻k的预测状态为：

将换道过程中车辆的预测状态向量表示为矩阵形式：

即：

式中，

为初始的状态偏差向量，计算公式如下：

其中，X₀为自车初始状态，X₀＝[x₀ y₀ θ₀ v₀]^T，x₀,y₀,θ₀,v₀分别为自车初始时刻的位置、航向角和行驶速度，均为已知量；X_r0为参考车辆初始状态，对于换道行为，本实施例中令参考车辆在目标车道中心线上匀速行驶且该参考车辆的初始纵向位置和行驶速度与自车相等，故X_r0＝[x₀ y_d θ_r0 v₀]^T，y_d为目标车道中心线的横向位置，θ_r0为参考车辆的初始航向角，本实施例中θ_r0＝0。

S2构建换道问题约束条件

在换道过程中，车辆需要与周围其他车辆保持安全距离，车辆需要保持在车道边界内，车辆的控制输入不能超过动力学极限。换道过程中任意离散时刻车辆状态量和控制量都要满足上述约束，具体如下：

S2.1构建换道安全距离约束

如图2所示，在任意预测时刻t_k，使用现有预测算法获得该时刻自车同车道前后车辆和目标车道自车前后车辆预测位置的期望和方差。为了保证换道过程中自车不与周围车辆发生碰撞，自车与前车和后车需要保持一定的纵向安全距离，本实施例中安全距离d_s＝5m。同时，自车应始终保持在道路范围内，因此车道的边界线即为自车的横向位置约束。设自车道前后车辆在t_k时刻的预测纵向位置为

和

目标车道前后车辆在t_k时刻的预测纵向位置为

和

自车道左右边界线的横向位置为y_L和y_R，目标车道左右边界线的横向位置为y_TL和y_TR。

如图3所示，将换道分为准备阶段、进行阶段和调整阶段，设换道准备阶段持续时间为0～T_pre，换道进行阶段持续时间为T_pre～T_post，换道调整阶段持续时间为T_post～T_final，将换道各阶段持续时间以固定时间步长Δt＝0.2s离散化，各阶段所对应的离散预测范围分别为[0,T_pre)、[N_pre,N_post)和[N_post,N_final]，N_pre,N_post,N_final分别为各阶段对应的离散范围阈值，N_final＝N。本实施例中取T_pre＝1s,T_post＝3s,T_final＝4s，N_pre＝5,N_post＝15,N_final＝20。

每个换道阶段边界约束不同，分别描述如下：

a)换道准备阶段

自车保持在当前车道内并与同车道前后车辆保持纵向安全距离d_s，自车的位置(x_k,y_k)边界约束由当前车道左右边界和前后车辆的位置确定，表达式如下：

0≤k＜N_pre

b)换道进行阶段

自车从当前车道运动到目标车道，均需与两条车道上的车辆均保持纵向安全距离d_s，自车的位置(x_k,y_k)边界由当前车道左边界线、目标车道右边界线和两条车道内前后车辆的位置确定，表达式如下：

N_pre≤k＜N_post

c)换道调整阶段

自车已经运动到目标车道内，调整阶段需要自车保持在目标车道内并与目标车道前后车辆保持纵向安全距离d_s，自车位置(x_k,y_k)边界由目标车道边界线和目标车道前后车辆的位置确定，表达式如下：

N_post≤k≤N_final

S2.2构建跟车安全距离约束

为了增加换道轨迹的安全性，需要使自车尽量远离前后车辆，在跟车场景中，需要使同一车道的前车和后车保持一定的跟车距离d_f：

d_f＝d₀+τ_fv_f

式中，d₀为最小跟车距离，τ_f为跟车时距，v_f是跟车情景中后车的当前速度，本实施例中取d₀＝5m,τ_f＝1.5s，对自车与当前车道前车及目标车道前车，v_f即为自车速度，对当前车道后车与自车，v_f为后车的速度，对目标车道后车与自车，v_f为目标车道后车的速度，即v_f的值确定，为已知量。

高速行驶时跟车距离较大(本方法在低速行驶时同样适用)，如果将跟车距离作为强制约束，则需要很大的车间间隙才能完成换道，车辆换道行为过于保守，因此，本发明引入纵向跟车距离约束松弛量(均为待求量)，将跟车距离作为非强制约束分别如下：

a)对于同车道前车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k同车道前车跟车距离约束松弛量；

b)对于同车道后车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k同车道后车跟车距离约束松弛量；

c)对于目标车道前车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k目标车道前车跟车距离约束松弛量；

d)对于目标车道后车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k目标车道后车跟车距离约束松弛量为。

S2.3构建控制输入约束

车辆加速、制动和转向***存在设计动力学极限，同时为了保证换道舒适性车辆也要避免较大的加速度和转向角度，因此对控制输入施加约束：

δ_min＜δ^k＜δ_max

其中v_min,v_max分别为自车速度变化量的最小值和最大值，δ_min,δ_max分别为自车前轮转角的最大值和最小值。

S3构建换道问题目标函数

换道规划与控制的目标包括换道的舒适性和快速性，同时为了保持S2.2中构建的跟车距离约束，在目标函数中加入松弛量代价值，设设换道问题目标函数为：

式中，

表示舒适性和快速性代价函数，计算公式如下：

其中，w_i,i＝1,2...5是权重系数，符号|·|₂表示二范数，w₁|y_k-y_d|₂项使车辆尽快完成换道行为，w₂|θ_k|₂和w₅|δ|₂项使车辆不会有较大的转向角，表征舒适性，w₃|v_k-v₀|₂和

使车辆按照期望的车速行驶，保证换道快速性。通过调整各项权重系数的大小，可以调整换道轨迹的舒适性和快速性，本例中取w₁＝10²,w₂＝10,w₃＝10⁴,w₄＝10⁸,w₅＝10⁶。

表示松弛量的代价函数，w_l为松弛量代价的权重系数，L_k为跟车距离约束松弛向量，

将换道问题目标函数表示为矩阵的形式，得到优化目标函数为：

式中，Q＝diag(0,w₁,w₂,w₃)是状态权重矩阵，R＝diag(w₄,w₅)是控制输入权重矩阵。为了保证安全性，在优化目标中，应使松弛量

的值尽量小。

将步骤S1.3中矩阵形式的车辆预测状态带入上述矩阵形式的换道问题目标函数，可得到：

为，

为。

将上述两式整理得到二次规划目标函数为：

式中，

V为换道规划与控制输入向量

H为黑塞(Hessian)矩阵，

I为单位矩阵；

F梯度向量，

S4求解二次规划目标函数

S3.3中得到了标准的二次规划(QP)问题，可通过已有优化方法求解，本例中采用Matlab中的quadprog函数求解，得到了最优的换道规划与控制输入向量V^*，计算公式如下：

应用本发明方法时，将输入按照步骤S1～S4依次执行，得到最优的换道规划与控制输入向量V^*，将该向量中的第一个控制量

加上参考车的控制输入u_r得到自车的控制输入

将自车的控制输入

给到传输至自辆执行器，即可实现换道规划与控制。

以上示意性地对本发明及其实施方式进行了描述，该描述没有限制性，附图中所示的也只是本发明的实施方式之一，实际并不局限于此。所以，如果本领域的普通技术人员受其启示，在不脱离本发明创造宗旨的情况下，不经创造性地设计出与该技术方案相似的方式及实施例，均应属于本发明的保护范围。

Claims (1)

1.一种基于模型预测控制的车辆自动换道规划与控制方法，其特征在于，包括以下步骤：

S1构建车辆动力学模型

S1.1构建车辆动力学模型如下：

式中，符号

表示变量对时间的一阶导数；X为自车的运动状态向量，X＝[x y θ v]，包括自车的纵向位置x、横向位置y、航向角θ和速度v；L为自车的轴距，T_L为自车由速度v变换到期望速度v_d所需时间；u为自车换道控制输入向量，u＝[v_d δ]，包括自车期望速度v_d和前轮转角δ；

S1.2将车辆动力学模型线性化，得到线性偏差模型

设存在一满足步骤S1.1构建的车辆动力学模型、位于目标车道内并与自车初始纵向位置和行驶速度相等的参考车辆，令参考车辆换道控制输入向量为u_r＝[v_dr δ_r]，包括参考车辆换期望道速度v_dr和前轮转角δ_r；令参考车辆的运动状态为X_r＝[x_r y_r θ_r v_r]，包括参考车辆的纵向位置x_r、横向位置y_r、航向角θ_r和速度v_r；

根据自车和参考车辆的状态偏差

和换道控制输入偏差

将车辆动力学模型进行线性化，得到连续车辆状态空间方程

表达式如下：

式中，

f_x,r为连续***矩阵，表达式如下：

f_u,r为连续输入矩阵，表达式如下：

S1.3使用一阶保持变换将步骤S1.2中得到的连续车辆状态空间方程离散化，得到离散的车辆预测状态为：

式中，

和

分别是离散时刻t_k的车辆状态偏差向量和控制偏差向量；k＝1,2,…,N，N为离散的预测范围，N＝T_final/Δt，T_final,Δt分别为预测换道时长和离散时间步长；

A为离散***矩阵，计算公式如下：

B是离散输入矩阵，计算公式如下：

根据离散的车辆动力学模型，在换道过程中，任意离散时刻k的预测状态为：

将换道过程中离散的车辆预测状态表示为矩阵形式：

式中，

为初始的状态偏差向量，计算公式如下：

其中，X₀为自车初始状态，X₀＝[x₀ y₀ θ₀ v₀]^T，x₀,y₀,θ₀,v₀分别为自车初始时刻的位置、航向角和行驶速度；X_r0为参考车辆初始状态，X_r0＝[x₀ y_d θ_r0 v₀]^T，y_d为目标车道中心线的横向位置，θ_r0为参考车辆的初始航向角；

S2构建换道问题约束条件

S2.1构建换道安全距离约束

在换道过程中，设自车与前车和后车之间需保持的纵向安全距离为d_s，自车道前后车辆在t_k时刻的预测纵向位置为

和

目标车道前后车辆在t_k时刻的预测纵向位置为

和

自车道左右边界线的横向位置为y_L和y_R，目标车道左右边界线的横向位置为y_TL和y_TR；将自车换道行为分为换道准备阶段、进行阶段和调整阶段，各阶段所对应的离散预测范围分别为[0,N_pre)、[N_pre,N_post)和[N_post,N_final]，N_pre,N_post,N_final分别为各阶段对应的离散范围阈值，N_final＝N；各阶段的换道安全距离约束分别如下：

a)换道准备阶段

自车保持在当前车道内并与同车道前后车辆保持纵向安全距离，自车的位置(x_k,y_k)边界约束由当前车道左右边界和前后车辆的位置确定，表达式如下：

0≤k＜N_pre

b)换道进行阶段

自车从当前车道运动到目标车道，均需与两条车道上的车辆保持纵向安全距离，自车的位置(x_k,y_k)边界由当前车道左边界线、目标车道右边界线和两条车道内前后车辆位置确定，表达式如下：

N_pre≤k＜N_post

c)换道调整阶段

自车已经运动到目标车道内，需保持在目标车道内并与目标车道前后车辆保持纵向安全距离，自车位置(x_k,y_k)边界由目标车道边界线和目标车道前后车辆的位置确定，表达式如下：

N_post≤k≤N_final

S2.2构建跟车安全距离约束

设同一车道的前后车辆间保持的跟车距离为d_f，计算公式如下：

d_f＝d₀+τ_fv_f

式中，d₀为最小跟车距离，τ_f为跟车时距，v_f为跟车情景中后车的当前速度；

引入纵向跟车距离约束松弛量，构建跟车安全距离约束分别如下：

a)对于同车道前车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k同车道前车跟车距离约束松弛量；

b)对于同车道后车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k同车道后车跟车距离约束松弛量；

c)对于目标车道前车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k目标车道前车跟车距离约束松弛量；

d)对于目标车道后车，纵向跟车距离约束表示为：

式中，

为预测时刻t_k目标车道后车跟车距离约束松弛量；

S2.3构建控制输入约束如下：

δ_min＜δ^k＜δ_max

式中，v_min,v_max分别为自车速度变化量的最小值和最大值，δ_min,δ_max分别为自车前轮转角的最大值和最小值；

S3构建换道问题目标函数

设换道问题目标函数如下：

式中，

表示换道舒适性和快速性的代价函数，计算公式如下：

其中，w₁～w₅分别为权重系数；符号|·|₂表示二范数；

表示松弛量的代价函数，w_l为松弛量代价的权重系数，L_k为跟车距离约束松弛向量，

将换道问题目标函数表示为矩阵形式如下：

式中，Q＝diag(0,w₁,w₂,w₃)，为状态权重矩阵；R＝diag(w₄,w₅)，为控制输入权重矩阵；

将步骤S1.3中矩阵形式的车辆预测状态代入矩阵形式的换道问题目标函数中，得到以下二次规划目标函数：

将上式整理得到二次规划目标函数为：

式中，

V为换道规划与控制输入向量

H为黑塞矩阵，

I为单位矩阵；

F梯度向量，

S4求解步骤S3构建的二次规划目标函数，得到最优的换道规划与控制输入向量V^*，计算公式如下：

Images