CN111260956A - 一种基于模型预测控制的车辆自动换道规划与控制方法 - Google Patents
一种基于模型预测控制的车辆自动换道规划与控制方法 Download PDFInfo
- Publication number
- CN111260956A CN111260956A CN202010042022.XA CN202010042022A CN111260956A CN 111260956 A CN111260956 A CN 111260956A CN 202010042022 A CN202010042022 A CN 202010042022A CN 111260956 A CN111260956 A CN 111260956A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- lane
- follows
- lane change
- formula
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 230000008859 change Effects 0.000 title claims abstract description 81
- 238000000034 method Methods 0.000 title claims abstract description 39
- 239000011159 matrix material Substances 0.000 claims description 40
- 239000013598 vector Substances 0.000 claims description 35
- 150000001875 compounds Chemical class 0.000 claims description 28
- 238000004364 calculation method Methods 0.000 claims description 18
- 238000010276 construction Methods 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 9
- 238000002360 preparation method Methods 0.000 claims description 7
- NRNCYVBFPDDJNE-UHFFFAOYSA-N pemoline Chemical compound O1C(N)=NC(=O)C1C1=CC=CC=C1 NRNCYVBFPDDJNE-UHFFFAOYSA-N 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 15
- 238000005457 optimization Methods 0.000 abstract description 11
- 230000006870 function Effects 0.000 description 23
- 230000001133 acceleration Effects 0.000 description 4
- 230000008447 perception Effects 0.000 description 3
- 230000001360 synchronised effect Effects 0.000 description 2
- 206010039203 Road traffic accident Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000005312 nonlinear dynamic Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012552 review Methods 0.000 description 1
- 239000002699 waste material Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/16—Anti-collision systems
- G08G1/167—Driving aids for lane monitoring, lane changing, e.g. blind spot detection
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明提出的一种基于模型预测控制的车辆自动换道规划与控制方法。模型预测控制适用于解决多约束多目标的优化问题,可以同步实现换道轨迹规划与控制。本发明将换道运动规划表达为一个模型预测控制问题,采用车辆动力学模型描述换道运动,将车辆动力学极限和无碰撞安全车间距离作为优化问题的强制约束,将跟车距离作为优化问题的非强制约束,将换道轨迹的平滑性和快速性作为优化目标,从而实现了最优换道运动规划。本发明提出的方法简化了换道规划算法复杂度,联合使用软硬约束在保证换道安全的同时提高了换道规划求解效率。
Description
技术领域
本发明涉及一种自动驾驶运动规划技术领域,具体涉及一种基于模型预测控制的车辆自动换道规划与控制方法。
背景技术
车辆换道是一种风险极高的驾驶行为,驾驶员需要对复杂的交通场景进行交互,同时完成车辆的横纵向控制。换道行为引发的交通事故造成了极大的人员伤亡与财产损失,在高级辅助驾驶***或自动驾驶***中实现车辆自动换道具有重要意义,换道运动规划是自动换道功能的重要环节。
在现有自动换道方法中,自动换道运动规划任务通常分为换道轨迹规划和轨迹跟踪控制两个步骤。为了保证规划出的换道轨迹可以被车辆跟踪,在轨迹规划中需要考虑车辆动力学和车辆运动的动力学约束。同时在轨迹跟踪控制中也需要根据车辆动力学设计控制算法,因此这种分解式的规划方法考虑了两次车辆动力学,增加了算法的复杂度,造成了计算资源的浪费。因此,将轨迹规划与控制统一在一个框架中实现同步的规划和控制可以减少算法的设计和计算资源,同时减少自动驾驶算法***的复杂度。模型预测控制可以解决多约束和多目标优化问题,适用于实现同步规划和控制。
Sugie等采用非线性模型预测控制方法,使用二自由度车辆动力学模型,将车辆动力学极限和安全跟车距离作为硬约束,将舒适性作为优化目标,实现车辆换道控制。由于采用非线性动力学模型和非线性约束,使得换道规划的求解非常复杂,规划的实时性不足。
Nilsson等采用线性模型预测控制方法,将车辆横纵向运动解耦,采用匀加速运动学模型描述车辆运动,采用线性的跟车距离约束,实现了换道规划的快速求解。然而该方法所使用的运动学模型过于简化,不符合车辆动力学要求,同时与周围车辆保持了较大的安全距离,策略过于保守,使得换道轨迹求解常常无解。
Carvalho等同样采用线性模型预测控制方法,通过线性化方法将二自由度车辆动力学模型线性化,同时在安全跟车距离约束中加入了碰撞概率参数来调解规划策略的保守与激进程度,在保证车辆动力学的同时提高了换道轨迹的可解性。但这种在约束条件里添加概率参数的方式增加了优化问题的复杂度。
发明内容
本发明的目的是为了提高自动换道运动规划中多约束多目标优化的鲁棒性,本发明提出了一种基于模型预测控制的车辆自动换道规划与控制方法。
为了实现上述目的,本发明采用如下技术方案:
本发明提出的一种基于模型预测控制的车辆自动换道规划与控制方法,其特征在于,包括以下步骤:
S1构建车辆动力学模型
S1.1构建车辆动力学模型如下:
式中,符号表示变量对时间的一阶导数;X为自车的运动状态向量,X=[x y θv],包括自车的纵向位置x、横向位置y、航向角θ和速度v;L为自车的轴距,TL为自车由速度v变换到期望速度vd所需时间;u为自车换道控制输入向量,u=[vd δ],包括自车期望速度vd和前轮转角δ;
S1.2将车辆动力学模型线性化,得到线性偏差模型
设存在一满足步骤S1.1构建的车辆动力学模型、位于目标车道内并与自车初始纵向位置和行驶速度相等的参考车辆,令参考车辆换道控制输入向量为ur=[vdr δr],包括参考车辆换期望道速度vdr和前轮转角δr;令参考车辆的运动状态为Xr=[xr yr θr vr],包括参考车辆的纵向位置xr、横向位置yr、航向角θr和速度vr;
式中,
fx,r为连续***矩阵,表达式如下:
fu,r为连续输入矩阵,表达式如下:
S1.3使用一阶保持变换将步骤S1.2中得到的连续车辆状态空间方程离散化,得到离散的车辆预测状态为:
式中,
A为离散***矩阵,计算公式如下:
B是离散输入矩阵,计算公式如下:
根据离散的车辆动力学模型,在换道过程中,任意离散时刻k的预测状态为:
将换道过程中离散的车辆预测状态表示为矩阵形式:
其中,X0为自车初始状态,X0=[x0 y0 θ0 v0]T,x0,y0,θ0,v0分别为自车初始时刻的位置、航向角和行驶速度;Xr0为参考车辆初始状态,Xr0=[x0 yd θr0 v0]T,yd为目标车道中心线的横向位置,θr0为参考车辆的初始航向角;
S2构建换道问题约束条件
S2.1构建换道安全距离约束
在换道过程中,设自车与前车和后车之间需保持的纵向安全距离为ds,自车道前后车辆在tk时刻的预测纵向位置为和目标车道前后车辆在tk时刻的预测纵向位置为和自车道左右边界线的横向位置为yL和yR,目标车道左右边界线的横向位置为yTL和yTR;将自车换道行为分为换道准备阶段、进行阶段和调整阶段,各阶段所对应的离散预测范围分别为[0,Npre)、[Npre,Npost)和[Npost,Nfinal],Npre,Npost,Nfinal分别为各阶段对应的离散范围阈值,Nfinal=N;各阶段的换道安全距离约束分别如下:
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中矩阵形式的车辆预测状态代入矩阵形式的换道问题目标函数中,得到以下二次规划目标函数:
将上式整理得到二次规划目标函数为:
式中,
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.中公开的算法)以安全性和交通效率为目标,决定车辆下一步的驾驶行为,驾驶行为包括车辆目标车道和目标车速vt,当车辆行为决策输出为换道,即目标车道不是当前车道时,采用本发明提出的车辆自动换道规划与控制方法实现车辆换道。所有输入状态量均在以自车所在车道中心线的Frenet坐标系下表达,其中X方向即为沿车道中心线切线方向,Y方向为垂直车道线方向。本发明提出的的车辆自动换道规划与控制方法,具体包括以下步骤:
S1构建车辆动力学模型
S1.1使用车辆动力学模型描述自动驾驶汽车的运动,车辆动力学模型的表达式如下:
式中,符号表示变量对时间的一阶导数;X为自车的运动状态向量,X=[x y θv],包括自车的纵向位置x、横向位置y、航向角θ和速度v;L为自车的轴距,TL为自车由速度v变换到期望速度vd所需时间;u为自车换道控制输入向量,u=[vd δ],包括自车期望速度vd和前轮转角δ;自车的运动状态向量关于时间的一阶导数是关于自车的运动状态向量X和自车换道控制输入向量的函数。
S1.2将车辆动力学模型线性化,得到线性偏差模型
设存在一满足步骤S1.1构建的车辆动力学模型、位于目标车道内并与自车初始纵向位置和行驶速度相等的参考车辆,令参考车辆换道控制输入向量为ur=[vdr δr],包括参考车辆换期望道速度vdr和前轮转角δr,本实施例中取vdr=vt,δr=0,即令参考车辆以行为决策算法得到的目标速度沿车道中心线行驶,令参考车辆的运动状态为Xr=[xr yr θr vr],包括参考车辆的纵向位置xr、横向位置yr、航向角θr和速度vr,本实施例中θr=0;
式中,
fx,r为连续***矩阵(system matrix),表达式如下:
fu,r为连续输入矩阵(input matrix),表达式如下:
S1.3使用一阶保持变换将步骤S1.2中得到的连续车辆状态空间方程离散化,得到离散的车辆状态空间方程:
式中,
和分别是离散时刻tkk的车辆状态偏差向量和控制偏差向量;k=1,2,…,N,N为离散的预测范围,N=Tfinal/Δt,Tfinal,Δt分别为预测换道时长和离散时间步长,本实施例中Tfinal=4s,Δt=0.2s,N=20。
A为离散***矩阵,计算公式如下:
B是离散输入矩阵,计算公式如下:
根据离散的车辆动力学模型,在换道过程中,任意离散时刻k的预测状态为:
将换道过程中车辆的预测状态向量表示为矩阵形式:
其中,X0为自车初始状态,X0=[x0 y0 θ0 v0]T,x0,y0,θ0,v0分别为自车初始时刻的位置、航向角和行驶速度,均为已知量;Xr0为参考车辆初始状态,对于换道行为,本实施例中令参考车辆在目标车道中心线上匀速行驶且该参考车辆的初始纵向位置和行驶速度与自车相等,故Xr0=[x0 yd θr0 v0]T,yd为目标车道中心线的横向位置,θr0为参考车辆的初始航向角,本实施例中θr0=0。
S2构建换道问题约束条件
在换道过程中,车辆需要与周围其他车辆保持安全距离,车辆需要保持在车道边界内,车辆的控制输入不能超过动力学极限。换道过程中任意离散时刻车辆状态量和控制量都要满足上述约束,具体如下:
S2.1构建换道安全距离约束
如图2所示,在任意预测时刻tk,使用现有预测算法获得该时刻自车同车道前后车辆和目标车道自车前后车辆预测位置的期望和方差。为了保证换道过程中自车不与周围车辆发生碰撞,自车与前车和后车需要保持一定的纵向安全距离,本实施例中安全距离ds=5m。同时,自车应始终保持在道路范围内,因此车道的边界线即为自车的横向位置约束。设自车道前后车辆在tk时刻的预测纵向位置为和目标车道前后车辆在tk时刻的预测纵向位置为和自车道左右边界线的横向位置为yL和yR,目标车道左右边界线的横向位置为yTL和yTR。
如图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和使车辆按照期望的车速行驶,保证换道快速性。通过调整各项权重系数的大小,可以调整换道轨迹的舒适性和快速性,本例中取w1=102,w2=10,w3=104,w4=108,w5=106。
将换道问题目标函数表示为矩阵的形式,得到优化目标函数为:
将步骤S1.3中矩阵形式的车辆预测状态带入上述矩阵形式的换道问题目标函数,可得到:
将上述两式整理得到二次规划目标函数为:
式中,
S4求解二次规划目标函数
S3.3中得到了标准的二次规划(QP)问题,可通过已有优化方法求解,本例中采用Matlab中的quadprog函数求解,得到了最优的换道规划与控制输入向量V*,计算公式如下:
应用本发明方法时,将输入按照步骤S1~S4依次执行,得到最优的换道规划与控制输入向量V*,将该向量中的第一个控制量加上参考车的控制输入ur得到自车的控制输入将自车的控制输入给到传输至自辆执行器,即可实现换道规划与控制。
以上示意性地对本发明及其实施方式进行了描述,该描述没有限制性,附图中所示的也只是本发明的实施方式之一,实际并不局限于此。所以,如果本领域的普通技术人员受其启示,在不脱离本发明创造宗旨的情况下,不经创造性地设计出与该技术方案相似的方式及实施例,均应属于本发明的保护范围。
Claims (1)
1.一种基于模型预测控制的车辆自动换道规划与控制方法,其特征在于,包括以下步骤:
S1构建车辆动力学模型
S1.1构建车辆动力学模型如下:
式中,符号表示变量对时间的一阶导数;X为自车的运动状态向量,X=[x y θ v],包括自车的纵向位置x、横向位置y、航向角θ和速度v;L为自车的轴距,TL为自车由速度v变换到期望速度vd所需时间;u为自车换道控制输入向量,u=[vd δ],包括自车期望速度vd和前轮转角δ;
S1.2将车辆动力学模型线性化,得到线性偏差模型
设存在一满足步骤S1.1构建的车辆动力学模型、位于目标车道内并与自车初始纵向位置和行驶速度相等的参考车辆,令参考车辆换道控制输入向量为ur=[vdr δr],包括参考车辆换期望道速度vdr和前轮转角δr;令参考车辆的运动状态为Xr=[xr yr θr vr],包括参考车辆的纵向位置xr、横向位置yr、航向角θr和速度vr;
式中,
fx,r为连续***矩阵,表达式如下:
fu,r为连续输入矩阵,表达式如下:
S1.3使用一阶保持变换将步骤S1.2中得到的连续车辆状态空间方程离散化,得到离散的车辆预测状态为:
式中,
A为离散***矩阵,计算公式如下:
B是离散输入矩阵,计算公式如下:
根据离散的车辆动力学模型,在换道过程中,任意离散时刻k的预测状态为:
将换道过程中离散的车辆预测状态表示为矩阵形式:
其中,X0为自车初始状态,X0=[x0 y0 θ0 v0]T,x0,y0,θ0,v0分别为自车初始时刻的位置、航向角和行驶速度;Xr0为参考车辆初始状态,Xr0=[x0 yd θr0 v0]T,yd为目标车道中心线的横向位置,θr0为参考车辆的初始航向角;
S2构建换道问题约束条件
S2.1构建换道安全距离约束
在换道过程中,设自车与前车和后车之间需保持的纵向安全距离为ds,自车道前后车辆在tk时刻的预测纵向位置为和目标车道前后车辆在tk时刻的预测纵向位置为和自车道左右边界线的横向位置为yL和yR,目标车道左右边界线的横向位置为yTL和yTR;将自车换道行为分为换道准备阶段、进行阶段和调整阶段,各阶段所对应的离散预测范围分别为[0,Npre)、[Npre,Npost)和[Npost,Nfinal],Npre,Npost,Nfinal分别为各阶段对应的离散范围阈值,Nfinal=N;各阶段的换道安全距离约束分别如下:
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中矩阵形式的车辆预测状态代入矩阵形式的换道问题目标函数中,得到以下二次规划目标函数:
将上式整理得到二次规划目标函数为:
式中,
S4求解步骤S3构建的二次规划目标函数,得到最优的换道规划与控制输入向量V*,计算公式如下:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010042022.XA CN111260956B (zh) | 2020-01-15 | 2020-01-15 | 一种基于模型预测控制的车辆自动换道规划与控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010042022.XA CN111260956B (zh) | 2020-01-15 | 2020-01-15 | 一种基于模型预测控制的车辆自动换道规划与控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111260956A true CN111260956A (zh) | 2020-06-09 |
CN111260956B CN111260956B (zh) | 2021-03-16 |
Family
ID=70950439
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010042022.XA Active CN111260956B (zh) | 2020-01-15 | 2020-01-15 | 一种基于模型预测控制的车辆自动换道规划与控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111260956B (zh) |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111959492A (zh) * | 2020-08-31 | 2020-11-20 | 重庆大学 | 一种网联环境下考虑换道行为的hev能量管理分层控制方法 |
CN111967094A (zh) * | 2020-09-01 | 2020-11-20 | 吉林大学 | 一种基于Mobileye提供车道线方程的后向车道线推算方法 |
CN112286197A (zh) * | 2020-11-03 | 2021-01-29 | 交通运输部公路科学研究所 | 一种基于离散时空的车辆纵向运动速度优化方法 |
CN112429004A (zh) * | 2020-12-02 | 2021-03-02 | 北京理工大学 | 一种车辆自动换道控制方法 |
CN112965489A (zh) * | 2021-02-05 | 2021-06-15 | 北京理工大学 | 一种基于碰撞检测的智能车高速换道规划方法 |
CN113465625A (zh) * | 2021-08-17 | 2021-10-01 | 清华大学 | 局部路径规划方法和装置 |
CN113665574A (zh) * | 2021-10-22 | 2021-11-19 | 清华大学 | 智能汽车换道时长预测及拟人化轨迹规划方法 |
CN114220262A (zh) * | 2021-11-19 | 2022-03-22 | 东南大学 | 一种基于交通仿真的车辆换道轨迹偏差计算方法 |
CN115079699A (zh) * | 2022-06-30 | 2022-09-20 | 北京理工大学 | 一种基于模型预测控制的人机共驾汽车的运动规划方法 |
CN117574539A (zh) * | 2023-11-22 | 2024-02-20 | 苏州郅荣软件有限公司 | 一种车辆约束***优化参数验证*** |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2001015370A1 (en) * | 1999-08-25 | 2001-03-01 | Southwest Research Institute | A receiver estimation engine for a chaotic system |
CN103121451A (zh) * | 2013-03-19 | 2013-05-29 | 大连理工大学 | 一种弯路换道轨迹的跟踪控制方法 |
CN103754224A (zh) * | 2014-01-24 | 2014-04-30 | 清华大学 | 一种车辆多目标协调换道辅助自适应巡航控制方法 |
CN103754221A (zh) * | 2014-01-24 | 2014-04-30 | 清华大学 | 一种车辆自适应巡航控制*** |
CN109131326A (zh) * | 2018-08-08 | 2019-01-04 | 大连理工大学 | 一种具有换道辅助功能的自适应巡航控制器及其工作方法 |
-
2020
- 2020-01-15 CN CN202010042022.XA patent/CN111260956B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2001015370A1 (en) * | 1999-08-25 | 2001-03-01 | Southwest Research Institute | A receiver estimation engine for a chaotic system |
CN103121451A (zh) * | 2013-03-19 | 2013-05-29 | 大连理工大学 | 一种弯路换道轨迹的跟踪控制方法 |
CN103754224A (zh) * | 2014-01-24 | 2014-04-30 | 清华大学 | 一种车辆多目标协调换道辅助自适应巡航控制方法 |
CN103754221A (zh) * | 2014-01-24 | 2014-04-30 | 清华大学 | 一种车辆自适应巡航控制*** |
CN109131326A (zh) * | 2018-08-08 | 2019-01-04 | 大连理工大学 | 一种具有换道辅助功能的自适应巡航控制器及其工作方法 |
Non-Patent Citations (1)
Title |
---|
张海伦 等: "智能车自动换道横向控制策略的研究", 《机械设计》 * |
Cited By (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111959492B (zh) * | 2020-08-31 | 2022-05-20 | 重庆大学 | 一种网联环境下考虑换道行为的hev能量管理分层控制方法 |
CN111959492A (zh) * | 2020-08-31 | 2020-11-20 | 重庆大学 | 一种网联环境下考虑换道行为的hev能量管理分层控制方法 |
CN111967094A (zh) * | 2020-09-01 | 2020-11-20 | 吉林大学 | 一种基于Mobileye提供车道线方程的后向车道线推算方法 |
CN112286197A (zh) * | 2020-11-03 | 2021-01-29 | 交通运输部公路科学研究所 | 一种基于离散时空的车辆纵向运动速度优化方法 |
CN112286197B (zh) * | 2020-11-03 | 2024-02-13 | 交通运输部公路科学研究所 | 一种基于离散时空的车辆纵向运动速度优化方法 |
CN112429004A (zh) * | 2020-12-02 | 2021-03-02 | 北京理工大学 | 一种车辆自动换道控制方法 |
CN112429004B (zh) * | 2020-12-02 | 2022-03-15 | 北京理工大学 | 一种车辆自动换道控制方法 |
CN112965489A (zh) * | 2021-02-05 | 2021-06-15 | 北京理工大学 | 一种基于碰撞检测的智能车高速换道规划方法 |
CN113465625A (zh) * | 2021-08-17 | 2021-10-01 | 清华大学 | 局部路径规划方法和装置 |
CN113465625B (zh) * | 2021-08-17 | 2022-06-03 | 清华大学 | 局部路径规划方法和装置 |
CN113665574A (zh) * | 2021-10-22 | 2021-11-19 | 清华大学 | 智能汽车换道时长预测及拟人化轨迹规划方法 |
CN114220262A (zh) * | 2021-11-19 | 2022-03-22 | 东南大学 | 一种基于交通仿真的车辆换道轨迹偏差计算方法 |
CN114220262B (zh) * | 2021-11-19 | 2023-06-23 | 东南大学 | 一种基于交通仿真的车辆换道轨迹偏差计算方法 |
CN115079699A (zh) * | 2022-06-30 | 2022-09-20 | 北京理工大学 | 一种基于模型预测控制的人机共驾汽车的运动规划方法 |
CN117574539A (zh) * | 2023-11-22 | 2024-02-20 | 苏州郅荣软件有限公司 | 一种车辆约束***优化参数验证*** |
Also Published As
Publication number | Publication date |
---|---|
CN111260956B (zh) | 2021-03-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111260956B (zh) | 一种基于模型预测控制的车辆自动换道规划与控制方法 | |
CN110377039B (zh) | 一种车辆避障轨迹规划与跟踪控制方法 | |
Erlien et al. | Shared steering control using safe envelopes for obstacle avoidance and vehicle stability | |
Li et al. | Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles | |
CN109501799B (zh) | 一种车联网条件下的动态路径规划方法 | |
CN110471408B (zh) | 基于决策过程的无人驾驶车辆路径规划方法 | |
Gao et al. | Predictive control of autonomous ground vehicles with obstacle avoidance on slippery roads | |
US20220371594A1 (en) | Model-based design of trajectory planning and control for automated motor-vehicles in a dynamic environment | |
CN112947469A (zh) | 汽车换道轨迹规划与动态轨迹跟踪控制方法 | |
CN110155081B (zh) | 一种智能驾驶汽车的自适应避障控制*** | |
CN112937551B (zh) | 一种考虑驾驶员输入特征的车辆控制方法及*** | |
Wang et al. | Path planning and path tracking for collision avoidance of autonomous ground vehicles | |
Liu et al. | Real-time longitudinal and lateral state estimation of preceding vehicle based on moving horizon estimation | |
CN112660124A (zh) | 一种用于换道场景的协同自适应巡航控制方法 | |
CN113619574A (zh) | 一种车辆避让方法、装置、计算机设备和存储介质 | |
CN112092805B (zh) | 一种智能车辆避撞与降低碰撞伤害的集成控制方法和*** | |
Chu et al. | Autonomous high-speed overtaking of intelligent chassis using fast iterative model predictive control | |
Zhong et al. | Optimal lane change control of intelligent vehicle based on MPC | |
Xu et al. | Local path planning and tracking control of vehicle collision avoidance system | |
CN110654386A (zh) | 弯道下多智能电动汽车协作式巡航纵横向综合控制方法 | |
CN115447615A (zh) | 基于车辆运动学模型预测控制的轨迹优化方法 | |
Rahim et al. | A longitudinal speed controller for autonomous multi-trailer articulated heavy vehicles | |
Yuan et al. | Research on emergency collision avoidance system of man-machine cooperative driving vehicles based on additional yaw moment control | |
Shin et al. | Effects of wireless communication on integrated risk management based automated vehicle | |
Zhu et al. | Trajectory planning algorithm using Gauss pseudo-spectral method based on vehicle-infrastructure cooperative system |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |