本发明涉及一种自动驾驶运动规划技术领域，具体涉及一种基于模型预测控制的车辆自动换道规划与控制方法。
背景技术：
车辆换道是一种风险极高的驾驶行为，驾驶员需要对复杂的交通场景进行交互，同时完成车辆的横纵向控制。换道行为引发的交通事故造成了极大的人员伤亡与财产损失，在高级辅助驾驶系统或自动驾驶系统中实现车辆自动换道具有重要意义，换道运动规划是自动换道功能的重要环节。
在现有自动换道方法中，自动换道运动规划任务通常分为换道轨迹规划和轨迹跟踪控制两个步骤。为了保证规划出的换道轨迹可以被车辆跟踪，在轨迹规划中需要考虑车辆动力学和车辆运动的动力学约束。同时在轨迹跟踪控制中也需要根据车辆动力学设计控制算法，因此这种分解式的规划方法考虑了两次车辆动力学，增加了算法的复杂度，造成了计算资源的浪费。因此，将轨迹规划与控制统一在一个框架中实现同步的规划和控制可以减少算法的设计和计算资源，同时减少自动驾驶算法系统的复杂度。模型预测控制可以解决多约束和多目标优化问题，适用于实现同步规划和控制。
sugie等采用非线性模型预测控制方法，使用二自由度车辆动力学模型，将车辆动力学极限和安全跟车距离作为硬约束，将舒适性作为优化目标，实现车辆换道控制。由于采用非线性动力学模型和非线性约束，使得换道规划的求解非常复杂，规划的实时性不足。
nilsson等采用线性模型预测控制方法，将车辆横纵向运动解耦，采用匀加速运动学模型描述车辆运动，采用线性的跟车距离约束，实现了换道规划的快速求解。然而该方法所使用的运动学模型过于简化，不符合车辆动力学要求，同时与周围车辆保持了较大的安全距离，策略过于保守，使得换道轨迹求解常常无解。
carvalho等同样采用线性模型预测控制方法，通过线性化方法将二自由度车辆动力学模型线性化，同时在安全跟车距离约束中加入了碰撞概率参数来调解规划策略的保守与激进程度，在保证车辆动力学的同时提高了换道轨迹的可解性。但这种在约束条件里添加概率参数的方式增加了优化问题的复杂度。
技术实现要素：
本发明的目的是为了提高自动换道运动规划中多约束多目标优化的鲁棒性，本发明提出了一种基于模型预测控制的车辆自动换道规划与控制方法。
为了实现上述目的，本发明采用如下技术方案：
本发明提出的一种基于模型预测控制的车辆自动换道规划与控制方法，其特征在于，包括以下步骤：
s1构建车辆动力学模型
s1.1构建车辆动力学模型如下：
式中，符号
s1.2将车辆动力学模型线性化，得到线性偏差模型
设存在一满足步骤s1.1构建的车辆动力学模型、位于目标车道内并与自车初始纵向位置和行驶速度相等的参考车辆，令参考车辆换道控制输入向量为ur＝[vdrδr]，包括参考车辆换期望道速度vdr和前轮转角δr；令参考车辆的运动状态为xr＝[xryrθrvr]，包括参考车辆的纵向位置xr、横向位置yr、航向角θr和速度vr；
根据自车和参考车辆的状态偏差
式中，
fx，r为连续系统矩阵，表达式如下：
fu,r为连续输入矩阵，表达式如下：
s1.3使用一阶保持变换将步骤s1.2中得到的连续车辆状态空间方程离散化，得到离散的车辆预测状态为：
式中，
a为离散系统矩阵，计算公式如下：
b是离散输入矩阵，计算公式如下：
根据离散的车辆动力学模型，在换道过程中，任意离散时刻k的预测状态为：
将换道过程中离散的车辆预测状态表示为矩阵形式：
式中，
其中，x0为自车初始状态，x0＝[x0y0θ0v0]t，x0,y0,θ0,v0分别为自车初始时刻的位置、航向角和行驶速度；xr0为参考车辆初始状态，xr0＝[x0ydθr0v0]t，yd为目标车道中心线的横向位置，θr0为参考车辆的初始航向角；
s2构建换道问题约束条件
s2.1构建换道安全距离约束
在换道过程中，设自车与前车和后车之间需保持的纵向安全距离为ds，自车道前后车辆在tk时刻的预测纵向位置为
a)换道准备阶段
自车保持在当前车道内并与同车道前后车辆保持纵向安全距离，自车的位置(xk,yk)边界约束由当前车道左右边界和前后车辆的位置确定，表达式如下：
0≤k＜npre
b)换道进行阶段
自车从当前车道运动到目标车道，均需与两条车道上的车辆保持纵向安全距离，自车的位置(xk,yk)边界由当前车道左边界线、目标车道右边界线和两条车道内前后车辆位置确定，表达式如下：
npre≤k＜npost
c)换道调整阶段
自车已经运动到目标车道内，需保持在目标车道内并与目标车道前后车辆保持纵向安全距离，自车位置(xk,yk)边界由目标车道边界线和目标车道前后车辆的位置确定，表达式如下：
npost≤k≤nfinal
s2.2构建跟车安全距离约束
设同一车道的前后车辆间保持的跟车距离为df，计算公式如下：
df＝d0 τfvf
式中，d0为最小跟车距离，τf为跟车时距，vf为跟车情景中后车的当前速度；
引入纵向跟车距离约束松弛量，构建跟车安全距离约束分别如下：
a)对于同车道前车，纵向跟车距离约束表示为：
式中，
b)对于同车道后车，纵向跟车距离约束表示为：
式中，
c)对于目标车道前车，纵向跟车距离约束表示为：
式中，
d)对于目标车道后车，纵向跟车距离约束表示为：
式中，
s2.3构建控制输入约束如下：
δmin＜δk＜δmax
式中，vmin,vmax分别为自车速度变化量的最小值和最大值，δmin,δmax分别为自车前轮转角的最大值和最小值；
s3构建换道问题目标函数
设换道问题目标函数如下：
式中，
其中，w1～w5分别为权重系数；符号|·|2表示二范数；
将换道问题目标函数表示为矩阵形式如下：
式中，q＝diag(0,w1,w2,w3)，为状态权重矩阵；r＝diag(w4,w5)，为控制输入权重矩阵；
将步骤s1.3中矩阵形式的车辆预测状态代入矩阵形式的换道问题目标函数中，得到以下二次规划目标函数：
将上式整理得到二次规划目标函数为：
式中，
v为换道规划与控制输入向量
h为黑塞hessian矩阵，
f梯度向量，
s4求解步骤s3构建的二次规划目标函数，得到最优的换道规划与控制输入向量v*，计算公式如下：
本发明的特点及有益效果：
本发明采用线性模型预测控制，使用非线性车辆动力学模型，采用参考系统偏差实现模型的线性化，加快了求解速度。此外，联合使用强制约束和非强制约束，将车辆换道所需的最小间距作为优化问题的强制约束，将纵向跟车距离作为非强制约束，保证安全性的同时提升问题的可解性，从而提高了换道规划算法的鲁棒性。
附图说明
图1是本发明提出的一种车辆自动换道规划与控制方法的流程图。
图2是本发明所述方法中车辆安全约束边界图。
图3是本发明所述方法中换道不同阶段的边界约束图。
具体实施方式
为使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不限定本发明的保护范围。
为了更好地理解本发明，以下详细阐述一个本发明一种基于模型预测控制的车辆自动换道规划与控制方法的应用实例。
参见图1，本发明提出的一种基于模型预测控制的车辆自动换道规划与控制方法，该方法的输入包括感知系统、障碍物运动预测算法和车辆行为决策的输出。感知系统通过传感器及感知算法(为本领域的公知技术，如可采用综述——杜伟松.基于机器视觉的智能汽车环境感知算法研究综述[j].汽车工业研究,2018(04):26-33中公开的算法)实时获取自车周围环境中其他车辆的运动状态和道路结构信息，车辆运动状态包括世界坐标系下车辆几何中心的坐标位置、速度和加速度，道路结构信息包括道路边界和车道线边界线位置和车道宽度、道路曲率等信息。障碍物运动预测算法(为本领域的公知技术，如可采用综述stéphanielefèvre,vasquezd,laugierc.asurveyonmotionpredictionandriskassessmentforintelligentvehicles[j].robomechjournal,2014,1(1):1-14.中公开的算法)根据其他车辆当前时刻及历史状态，预测未来时刻其他车辆的位置，预测位置包括坐标的期望值和方差。车辆行为决策算法(为本领域的公知技术，如可采用文章weij,sniderjm,gut,etal.abehavioralplanningframeworkforautonomousdriving[c]//intelligentvehiclessymposium.ieee,2014.中公开的算法)以安全性和交通效率为目标，决定车辆下一步的驾驶行为，驾驶行为包括车辆目标车道和目标车速vt，当车辆行为决策输出为换道，即目标车道不是当前车道时，采用本发明提出的车辆自动换道规划与控制方法实现车辆换道。所有输入状态量均在以自车所在车道中心线的frenet坐标系下表达，其中x方向即为沿车道中心线切线方向，y方向为垂直车道线方向。本发明提出的的车辆自动换道规划与控制方法，具体包括以下步骤：
s1构建车辆动力学模型
s1.1使用车辆动力学模型描述自动驾驶汽车的运动，车辆动力学模型的表达式如下：
式中，符号
s1.2将车辆动力学模型线性化，得到线性偏差模型
设存在一满足步骤s1.1构建的车辆动力学模型、位于目标车道内并与自车初始纵向位置和行驶速度相等的参考车辆，令参考车辆换道控制输入向量为ur＝[vdrδr]，包括参考车辆换期望道速度vdr和前轮转角δr，本实施例中取vdr＝vt,δr＝0，即令参考车辆以行为决策算法得到的目标速度沿车道中心线行驶，令参考车辆的运动状态为xr＝[xryrθrvr]，包括参考车辆的纵向位置xr、横向位置yr、航向角θr和速度vr，本实施例中θr＝0；
根据自车和参考车辆的状态偏差
式中，
fx,r为连续系统矩阵(systemmatrix)，表达式如下：
fu,r为连续输入矩阵(inputmatrix)，表达式如下：
fx,r和fu,r是通过将方程
式中，
s1.3使用一阶保持变换将步骤s1.2中得到的连续车辆状态空间方程离散化，得到离散的车辆状态空间方程：
式中，
a为离散系统矩阵，计算公式如下：
b是离散输入矩阵，计算公式如下：
根据离散的车辆动力学模型，在换道过程中，任意离散时刻k的预测状态为：
将换道过程中车辆的预测状态向量表示为矩阵形式：
即：
其中，x0为自车初始状态，x0＝[x0y0θ0v0]t，x0,y0,θ0,v0分别为自车初始时刻的位置、航向角和行驶速度，均为已知量；xr0为参考车辆初始状态，对于换道行为，本实施例中令参考车辆在目标车道中心线上匀速行驶且该参考车辆的初始纵向位置和行驶速度与自车相等，故xr0＝[x0ydθr0v0]t，yd为目标车道中心线的横向位置，θr0为参考车辆的初始航向角，本实施例中θr0＝0。
s2构建换道问题约束条件
在换道过程中，车辆需要与周围其他车辆保持安全距离，车辆需要保持在车道边界内，车辆的控制输入不能超过动力学极限。换道过程中任意离散时刻车辆状态量和控制量都要满足上述约束，具体如下：
s2.1构建换道安全距离约束
如图2所示，在任意预测时刻tk，使用现有预测算法获得该时刻自车同车道前后车辆和目标车道自车前后车辆预测位置的期望和方差。为了保证换道过程中自车不与周围车辆发生碰撞，自车与前车和后车需要保持一定的纵向安全距离，本实施例中安全距离ds＝5m。同时，自车应始终保持在道路范围内，因此车道的边界线即为自车的横向位置约束。设自车道前后车辆在tk时刻的预测纵向位置为
如图3所示，将换道分为准备阶段、进行阶段和调整阶段，设换道准备阶段持续时间为0～tpre，换道进行阶段持续时间为tpre～tpost，换道调整阶段持续时间为tpost～tfinal，将换道各阶段持续时间以固定时间步长δt＝0.2s离散化，各阶段所对应的离散预测范围分别为[0,tpre)、[npre,npost)和[npost,nfinal]，npre,npost,nfinal分别为各阶段对应的离散范围阈值，nfinal＝n。本实施例中取tpre＝1s,tpost＝3s,tfinal＝4s，npre＝5,npost＝15,nfinal＝20。
每个换道阶段边界约束不同，分别描述如下：
a)换道准备阶段
自车保持在当前车道内并与同车道前后车辆保持纵向安全距离ds，自车的位置(xk,yk)边界约束由当前车道左右边界和前后车辆的位置确定，表达式如下：
0≤k＜npre
b)换道进行阶段
自车从当前车道运动到目标车道，均需与两条车道上的车辆均保持纵向安全距离ds，自车的位置(xk,yk)边界由当前车道左边界线、目标车道右边界线和两条车道内前后车辆的位置确定，表达式如下：
npre≤k＜npost
c)换道调整阶段
自车已经运动到目标车道内，调整阶段需要自车保持在目标车道内并与目标车道前后车辆保持纵向安全距离ds，自车位置(xk,yk)边界由目标车道边界线和目标车道前后车辆的位置确定，表达式如下：
npost≤k≤nfinal
s2.2构建跟车安全距离约束
为了增加换道轨迹的安全性，需要使自车尽量远离前后车辆，在跟车场景中，需要使同一车道的前车和后车保持一定的跟车距离df：
df＝d0 τfvf
式中，d0为最小跟车距离，τf为跟车时距，vf是跟车情景中后车的当前速度，本实施例中取d0＝5m,τf＝1.5s，对自车与当前车道前车及目标车道前车，vf即为自车速度，对当前车道后车与自车，vf为后车的速度，对目标车道后车与自车，vf为目标车道后车的速度，即vf的值确定，为已知量。
高速行驶时跟车距离较大(本方法在低速行驶时同样适用)，如果将跟车距离作为强制约束，则需要很大的车间间隙才能完成换道，车辆换道行为过于保守，因此，本发明引入纵向跟车距离约束松弛量(均为待求量)，将跟车距离作为非强制约束分别如下：
a)对于同车道前车，纵向跟车距离约束表示为：
式中，
b)对于同车道后车，纵向跟车距离约束表示为：
式中，
c)对于目标车道前车，纵向跟车距离约束表示为：
式中，
d)对于目标车道后车，纵向跟车距离约束表示为：
式中，
s2.3构建控制输入约束
车辆加速、制动和转向系统存在设计动力学极限，同时为了保证换道舒适性车辆也要避免较大的加速度和转向角度，因此对控制输入施加约束：
δmin＜δk＜δmax
其中vmin,vmax分别为自车速度变化量的最小值和最大值，δmin,δmax分别为自车前轮转角的最大值和最小值。
s3构建换道问题目标函数
换道规划与控制的目标包括换道的舒适性和快速性，同时为了保持s2.2中构建的跟车距离约束，在目标函数中加入松弛量代价值，设设换道问题目标函数为：
式中，
其中，wi,i＝1,2...5是权重系数，符号|·|2表示二范数，w1|yk-yd|2项使车辆尽快完成换道行为，w2|θk|2和w5|δ|2项使车辆不会有较大的转向角，表征舒适性，w3|vk-v0|2和
将换道问题目标函数表示为矩阵的形式，得到优化目标函数为：
式中，q＝diag(0,w1,w2,w3)是状态权重矩阵，r＝diag(w4,w5)是控制输入权重矩阵。为了保证安全性，在优化目标中，应使松弛量
将步骤s1.3中矩阵形式的车辆预测状态带入上述矩阵形式的换道问题目标函数，可得到：
将上述两式整理得到二次规划目标函数为：
式中，
v为换道规划与控制输入向量
h为黑塞(hessian)矩阵，
f梯度向量，
s4求解二次规划目标函数
s3.3中得到了标准的二次规划(qp)问题，可通过已有优化方法求解，本例中采用matlab中的quadprog函数求解，得到了最优的换道规划与控制输入向量v*，计算公式如下：
应用本发明方法时，将输入按照步骤s1～s4依次执行，得到最优的换道规划与控制输入向量v*，将该向量中的第一个控制量
以上示意性地对本发明及其实施方式进行了描述，该描述没有限制性，附图中所示的也只是本发明的实施方式之一，实际并不局限于此。所以，如果本领域的普通技术人员受其启示，在不脱离本发明创造宗旨的情况下，不经创造性地设计出与该技术方案相似的方式及实施例，均应属于本发明的保护范围。
1.一种基于模型预测控制的车辆自动换道规划与控制方法，其特征在于，包括以下步骤：
s1构建车辆动力学模型
s1.1构建车辆动力学模型如下：
式中，符号
s1.2将车辆动力学模型线性化，得到线性偏差模型
设存在一满足步骤s1.1构建的车辆动力学模型、位于目标车道内并与自车初始纵向位置和行驶速度相等的参考车辆，令参考车辆换道控制输入向量为ur＝[vdrδr]，包括参考车辆换期望道速度vdr和前轮转角δr；令参考车辆的运动状态为xr＝[xryrθrvr]，包括参考车辆的纵向位置xr、横向位置yr、航向角θr和速度vr；
根据自车和参考车辆的状态偏差
式中，
fx,r为连续系统矩阵，表达式如下：
fu,r为连续输入矩阵，表达式如下：
s1.3使用一阶保持变换将步骤s1.2中得到的连续车辆状态空间方程离散化，得到离散的车辆预测状态为：
式中，
a为离散系统矩阵，计算公式如下：
b是离散输入矩阵，计算公式如下：
根据离散的车辆动力学模型，在换道过程中，任意离散时刻k的预测状态为：
将换道过程中离散的车辆预测状态表示为矩阵形式：
式中，
其中，x0为自车初始状态，x0＝[x0y0θ0v0]t，x0,y0,θ0,v0分别为自车初始时刻的位置、航向角和行驶速度；xr0为参考车辆初始状态，xr0＝[x0ydθr0v0]t，yd为目标车道中心线的横向位置，θr0为参考车辆的初始航向角；
s2构建换道问题约束条件
s2.1构建换道安全距离约束
在换道过程中，设自车与前车和后车之间需保持的纵向安全距离为ds，自车道前后车辆在tk时刻的预测纵向位置为
a)换道准备阶段
自车保持在当前车道内并与同车道前后车辆保持纵向安全距离，自车的位置(xk,yk)边界约束由当前车道左右边界和前后车辆的位置确定，表达式如下：
0≤k＜npre
b)换道进行阶段
自车从当前车道运动到目标车道，均需与两条车道上的车辆保持纵向安全距离，自车的位置(xk,yk)边界由当前车道左边界线、目标车道右边界线和两条车道内前后车辆位置确定，表达式如下：
npre≤k＜npost
c)换道调整阶段
自车已经运动到目标车道内，需保持在目标车道内并与目标车道前后车辆保持纵向安全距离，自车位置(xk,yk)边界由目标车道边界线和目标车道前后车辆的位置确定，表达式如下：
npost≤k≤nfinal
s2.2构建跟车安全距离约束
设同一车道的前后车辆间保持的跟车距离为df，计算公式如下：
df＝d0 τfvf
式中，d0为最小跟车距离，τf为跟车时距，vf为跟车情景中后车的当前速度；
引入纵向跟车距离约束松弛量，构建跟车安全距离约束分别如下：
a)对于同车道前车，纵向跟车距离约束表示为：
式中，
b)对于同车道后车，纵向跟车距离约束表示为：
式中，
c)对于目标车道前车，纵向跟车距离约束表示为：
式中，
d)对于目标车道后车，纵向跟车距离约束表示为：
式中，
s2.3构建控制输入约束如下：
δmin＜δk＜δmax
式中，vmin,vmax分别为自车速度变化量的最小值和最大值，δmin,δmax分别为自车前轮转角的最大值和最小值；
s3构建换道问题目标函数
设换道问题目标函数如下：
式中，
其中，w1～w5分别为权重系数；符号|·|2表示二范数；
将换道问题目标函数表示为矩阵形式如下：
式中，q＝diag(0,w1,w2,w3)，为状态权重矩阵；r＝diag(w4,w5)，为控制输入权重矩阵；
将步骤s1.3中矩阵形式的车辆预测状态代入矩阵形式的换道问题目标函数中，得到以下二次规划目标函数：
将上式整理得到二次规划目标函数为：
式中，
v为换道规划与控制输入向量
h为黑塞矩阵，
f梯度向量，
s4求解步骤s3构建的二次规划目标函数，得到最优的换道规划与控制输入向量v*，计算公式如下：
