CN109131326A - 一种具有换道辅助功能的自适应巡航控制器及其工作方法 - Google Patents

Abstract

本发明公开了一种具有换道辅助功能的自适应巡航控制器及其工作方法，所述的控制器包括感知系统、换道预警系统、换道辅助系统和ACC系统，所述的感知系统分别与换道预警系统、换道辅助系统和ACC系统连接，所述的换道辅助系统分别与换道预警系统和ACC系统双向连接，所述的换道辅助系统分别与换道轨迹规划路径跟踪器和MPC控制器连接。本发明不仅具有原ACC系统的全部功能，即可以通过车载雷达等传感器监测汽车前方的道路交通环境，对车辆进行纵向控制，使本车与前车保持安全距离行驶；还可以在前方有复杂交通状况或驾驶员有换道意图时避开障碍物进行自主换道；并可以有效避免事故发生，也极大改善ACC系统对驾驶员换道造成的不适感。

Description

一种具有换道辅助功能的自适应巡航控制器及其工作方法

技术领域

本发明涉及辅助驾驶技术领域，特别是一种具有换道辅助功能的自适应巡航控制器及其工作方法。

背景技术

汽车行业的发展趋势为“汽车四化”，即电动化、智能化、共享化和网联化，在新能源汽车的基础上结合单车智能技术，融合互联网科技最终实现智能网联的高级辅助驾驶技术将成为汽车行业未来的必然趋势。同时为应对交通事故所导致的全球安全性问题，高级驾驶辅助系统得到迅猛发展，自适应巡航系统(ACC:Adaptive Cruise Control)和换道辅助系统作为高级驾驶系统的重要组成部分，如何将二者同时应用于车上成为目前辅助驾驶急待解决的问题。

自适应巡航系统通过车载雷达等传感器技术以及驾驶员实时操作信息，利用模型预测算法控制算法进行车辆操作来保证跟车安全性，通过人、车、路三者的有效结合，大幅度提升了行驶安全性，改善了交通拥堵和疲劳驾驶。

现有ACC系统主要针对单一路况开展，主要研究车辆的跟车稳定行驶性，利用控制算法保持前后两车的安全距离，集中于车辆的纵向安全。但在行驶过程中，车辆路况复杂，现有ACC系统无法处理交叉路口、车辆汇流等复杂交通工况和间距固定的问题。

因此，为了克服现有ACC系统在交叉路口行驶或应用于电动汽车时的局限性，例如当驾驶员有换道意图或遇到对面车辆有交汇车辆时，常规ACC系统由于目标车依然存在，会对驾驶员的换道产生严重的干扰，造成事故。

发明内容

针对现有ACC系统无法处理交叉路口、车辆汇流等复杂交通工况和间距固定等问题，本发明要设计一种既能在遇到前方有突发状况时进行自主换道，又能在有车辆行驶时切换至ACC系统的具有换道辅助功能的自适应巡航控制器及其工作方法。

为了实现上述目的，本发明的技术方案如下：一种具有换道辅助功能的自适应巡航控制器，包括感知系统、换道预警系统、换道辅助系统和ACC系统，所述的感知系统、换道预警系统、换道辅助系统和ACC系统集成于整车控制器中，所述的感知系统分别与换道预警系统、换道辅助系统和ACC系统连接，所述的换道辅助系统分别与换道预警系统和ACC系统双向连接，所述的换道辅助系统分别与换道轨迹规划路径跟踪器和MPC控制器连接；所述的MPC控制器分别与ACC系统、节气门和控制踏板连接；所述的换道轨迹规划路径跟踪器与方向盘连接。

所述的感知系统包括RGB-D视觉传感器、单轴陀螺仪和光编码传感器，所述的RGB-D视觉传感器、单轴陀螺仪以及光编码器获得的信息作为输入对车辆进行定位和控制。在车辆行驶过程中，RGB-D视觉传感器用于检测前方障碍物，进行前方障碍物测距，确定可行驶安全区域。单轴陀螺仪和光编码器分别用于车辆角速度以及速度测量，获得车辆里程计信息。

所述的换道预警系统是当前方出现换道路况时报警并提醒驾驶员开始作换道准备。

所述的换道辅助系统是当前方有复杂交汇车辆出现时，控制车辆进行自主换道操作，在自主换道过程中首先进行换道轨迹的规划，其次对经过规划的路径进行跟踪来实现换道子系统的完成；

所述的ACC系统是当前方有同向车辆或无车辆时，控制车辆自动切换至ACC系统，ACC系统采用模型预测方法实现间距可变的跟车过程。

一种具有换道辅助功能的自适应巡航控制器的工作方法，包括以下步骤：

A、感知系统采集车辆前方路况：感知系统实时采集车辆前方路况，并将路况信息实时传输给具有换道辅助功能的自适应巡航控制器；具有换道辅助功能的自适应巡航控制器对车辆前方路况信息进行判断，若感知系统监测出前方有交汇车辆驶来或者驾驶员有换道意图时，换道预警系统发出带有换道声音的信号同时触发换道辅助系统进行工作；当车辆前方有同向行驶车辆或需要进行跟随操作时开启ACC系统；

B、换道预警系统根据感知系统传来的信号判断是否有换道必要，发出警报信号并将信号传给换道辅助系统，同时接收换道辅助系统是否已经开始执行，若已执行则关闭换道预警系统，若未执行，则继续开启换道预警系统；

C、换道辅助系统根据前方路况判断是否存在障碍物，将换道轨迹规划分为有障碍物换道和无障碍物换道，两种轨迹规划都采用拟合多次项轨迹方程的方式，最后对规划路径利用模型预测方法进行换道路线轨迹跟踪完成换道任务；

具体工作方法如下：

C1、根据上层感知系统判断换道前方路况

通过感知系统中的RGB-D视觉传感器对车辆前方状况进行实时检测；

C2、判断前方是否有障碍物

若前方未检测到障碍物的存在，则启动无障碍物换道模式转步骤C21；若前方检测到有障碍物则启动有障碍物换道模式，转步骤C22；

C21、规划换道过程无障碍多项式换道路径

根据车辆的起始状态和目标状态得出一条期望换道路径，使车辆在规定时间内到达相邻车道；对车辆横纵向运动利用五次多项式拟合换道轨迹：

分别对五次多项式求一阶导和二阶导相应得到速度和加速度的五次多项式，结合车辆换道的始末状态得出换道轨迹如下：

其中:

A＝(a₅,a₄,a₃,a₂,a₁,a₀)

B＝(b₅,b₄,b₃,b₂,b₁,b₀)

式中，f(x)为横向换道轨迹多项式函数，f(y)分别为纵向换道轨迹多项式函数，T_6×6为多次项矩阵，A、B均为多项式系数矩阵，根据车辆换道起始时刻到结束时刻的平均速度以及换道时间确定，以此求得换道轨迹方程；转步骤C3；

C22、规划换道过程有障碍多项式换道路径

若换道过程中有障碍物出现，则对式(1)进行约束限制；在实际车辆前行过程中，纵向速度是不断变化的，因此引入车辆几何学模型的方法对其进行约束限制；

C221、选用动态圆包围的形式对车辆进行等价替代，即以车辆宽度为直径所处位置不断作圆；以圆扫过的面积替代车辆模型；

C222、对该车辆模型进行分析得出各动态圆的方程，并分析换道车辆不发生碰撞的几何条件，得出约束条件如下：

(x₂-x₁)²+(y₂-y₁)²＞(R₁+R₂)² (3)

式中x_f为车辆前端的横坐标，x_r为车辆后端的横坐标，y_f为车辆前端的纵坐标，y_r为车辆后端的纵坐标，x₁为障碍车的横坐标，x₂为本车的横坐标，y₁为障碍车的纵坐标，y₂为本车的纵坐标，R₁为障碍车等效动态圆圆心，R₂为本车等效动态圆圆心，同时忽略横摆角速度和质心侧偏角的影响，将式(2)代入式(3)得：

[x_1r+u_1x(x_1f-x_1r-x_2r-u_2x(x_2f-x_2r))]²+[y_1r+u_1y(y₁-y_1r)-y_2r-u_2y(y_2f-y_2r)]²＞(R₁+R₂)² (4)

其中，x_1r为障碍车后端的横坐标，x_1f为障碍车前端的横坐标，x_2r为本车后端的横坐标，x_2f为本车前端的横坐标，y_1r为障碍车后端的纵坐标，y_1f为障碍车前端的纵坐标，y_2r为本车后端的纵坐标，y_2f为本车前端的纵坐标，

整理式(4)得：

α²a₆ ²+βb₆+γ＞0 (5)

其中α、β、γ是关于时间的函数，a₆、b₆是障碍换道中六次项的系数，根据求根公式确定a₆的范围：

C223、确定有障碍时的换道轨迹方程

基于式(5)确定换道的多项式系数，同时考虑在换道时间内用动态圆建立的车辆模型不能与障碍车辆的前后端存在交点，在车辆汇流处，假设在某一时刻本车的车头到达前车车尾处，则称其为碰撞时刻，若要两车不发生碰撞，此时刻本车在目标车道至少产生一个宽度为车身宽度的位移，分别对本车前后端进行临界碰撞检测，并根据本车车体长宽给出一边界值求a₆，以此确定多项式系数矩阵A、B的值代入式(1)形成有障碍物的换道轨迹方程；

C3、确定换道轨迹跟踪过程

换道轨迹跟踪采用模型预测算法即MPC法，将经过步骤C2确定的换道轨迹方程代入MPC轨迹跟踪控制器中进行跟踪；

车辆从一个已知点出发，记为车辆的初始状态，以车辆运动学作为预测方程基础，根据所需目标建立轨迹误差模型、车辆当前状态信息和未来控制预测车辆状态信息，建立目标函数和约束方程，得到约束条件和目标函数，将其输出量即车轮转角作用于车辆上，最终跟上期望轨迹；过程中最核心部分为建立目标函数和约束条件，具体步骤如下：

C31、建立轨迹跟踪目标函数

以车辆动力学模型建立的模型预测控制为基础，结合车辆行驶时的约束条件，对换道轨迹进行跟踪；轨迹跟踪的目的是使实际轨迹与期望轨迹误差最小，同时使控制量达到最小要求；

式中，Γ_y为轨迹目标函数中横向位移对应权重系数，为对角矩阵；Γ_u为轨迹目标函数中状态量对应权重系数，为对角矩阵；y(k+i)为第k+i时刻轨迹的实际位置，y_des(k+i)是预测期望位置，u(k+i)为第k+i时刻的状态量；

C32、建立轨迹跟踪约束条件

换道轨迹跟踪的约束条件有两个，分别是控制量不能超过规定极限值、前轮转角不能超过极限值，即：

u_min≤u(k+i)≤u_max

δ_min≤δ≤δ_max

D、当感知系统检测到行驶车道的前方有其他前行车辆时立刻触发ACC系统工作，ACC系统根据本车和前车之间的相对距离和相对车速信息，通过与车辆的ABS、发动机控制系统、自动变速器控制系统协调工作，使本车与前车始终保持安全距离行驶；若感知系统检测出前方有同方向行驶车辆且转角传感器未检测到从方向盘传来的转角信号，则开启ACC系统，ACC系统同样采用模型预测算法；

D1、建立ACC系统目标函数

ACC系统的目标是为了控制车距误差和相对速度，但为充分反映ACC系统的动态变化规律，将前车加速度作为ACC系统的干扰的，并通过对本车加速度和加速度变化率动态特性的考虑来提高状态空间模型的鲁棒性和控制精度；

为了减小每一步期望加速度的变化量，使得控制信号变化平缓，将期望加速度变化量作为第一控制目标：

其中：s表示期望加速度变化量的权重矩阵，舍去对优化问题不产生作用的无用项ρ₁，得到目标函数第一项:

第二控制目标为：预测输出值与参考输出值之间的差值最小化；该控制目标写成最小二范数的形式：

舍去对优化问题不产生作用的无用项ρ₂，得到目标函数第二项:

其中：

q＝diag[q₁,q₂,q₃,q₄]

其中：Y_r为预测时域内每一步的参考输出组成的矩阵；fa为第一步的参考输出矩阵，q为每一步输出的权重矩阵，q₁为对角矩阵Q中第一行第一列的权重系数，q₂为对角矩阵中第二行第二列的权重系数，q₃为对角矩阵中第三行第三列的权重系数，q₄为对角矩阵中第四行第四列的权重系数；

D2、建立ACC系统约束条件

在ACC系统的模型预测问题中，考虑车辆的物理条件限制；其中期望加速度的最大值最小值是模型预测问题中最常见的约束；期望加速度的上下界所形成的约束如下式所示：

即U_min≤U_c≤U_max，其中u_min为期望加速度的最小值，u_max为期望加速度的最大值，u_k为第k时刻的状态量，u_k+1为第k+1时刻的状态量，u_k+c-1为第k+c-1时刻的状态量；

为了达到理想的控制目标，使得ACC系统的性能指标满足控制目标的需求，对车间距Δx、相对速度v_rel、本车加速度a和本车加速度变化率j构成的ACC系统性能指标进行约束，定义向量χ＝[Δx,v,a,j]^T，对预测空间内的每一步的χ进行约束，用公式表示为：

即：

其中：

χ_min、χ_max分别表示χ的最小值和最大值，因此，关于ACC系统性能指标的约束条件为：

结束。

与现有技术相比，本发明具有以下有益效果：

1、本发明设计了一种兼具换道功能和ACC功能的具有换道辅助系统的控制器，控制器根据感知系统传来的实时信息进行ACC系统或换道辅助系统切换，避免复杂交通状况的碰撞事故，减缓装载ACC系统换道的严重不适感。

2、本发明在换道过程中考虑了有障碍的情况，通过动态圆来对障碍车进行几何模拟，拟合避障多次项路径，避免换道交通事故发生。

3、本发明利用MPC控制算法，综合考虑安全性、经济性和舒适性对，其加入恰当的动力学约束，设计了轨迹跟踪控制策略和ACC系统。

4、本发明设计了一种考虑前车速度趋势的可变车间时距策略，该策略通过引入对前车未来速度扰动的考虑以及饱和函数的处理，提高间距控制的前瞻性和抗干扰能力，有效的平衡了车辆在行驶过程中的安全性和跟车性。

5、综上所述，本发明不仅具有原ACC系统的全部功能，即可以通过车载雷达等传感器监测汽车前方的道路交通环境，对车辆进行纵向控制，使本车与前车保持安全距离行驶；还可以在前方有复杂交通状况或驾驶员有换道意图时避开障碍物进行自主换道；并可以有效避免事故发生，也极大改善ACC系统对驾驶员换道造成的不适感。

附图说明

图1为本发明的整体框架图。

图2为本发明的方法流程图。

图3为本发明的换道辅助系统的工作方法流程图。

图4为模型预测原理图。

图5为自适应续航原理图。

具体实施方式

下面结合附图对本发明进行进一步地描述。如图1所示，一种具有换道辅助功能的自适应巡航控制器，包括感知系统、换道预警系统、换道辅助系统和ACC系统，所述的感知系统、换道预警系统、换道辅助系统和ACC系统集成于整车控制器中，所述的感知系统分别与换道预警系统、换道辅助系统和ACC系统连接，所述的换道辅助系统分别与换道预警系统和ACC系统双向连接，所述的换道辅助系统分别与换道轨迹规划路径跟踪器和MPC控制器连接；所述的MPC控制器分别与ACC系统、节气门和控制踏板连接；所述的换道轨迹规划路径跟踪器与方向盘连接。

如图1-5所示，一种具有换道辅助功能的自适应巡航控制器的工作方法，包括以下步骤：

1、根据安装在车辆前窗玻璃的RGB-D视觉传感器检测前方车辆和障碍物，安装在转角轴上的陀螺仪和光编码器检测车辆转角信号。若检测到前方同一车道有交汇车辆，换道预警系统发出警告并触发换道辅助系统工作；同时让测得的转角信号与转向行为模型进行对比，相似度阈值设定为0.9，若超过阈值则说明驾驶员有换道意图，启动换道辅助系统；若感知系统检测到前方有同向车辆，自动触发ACC系统，车辆保持安全距离行驶。

2、换道辅助系统的工作方法如图3所示，采用动态圆多次项和模型预测轨迹跟踪相结合的方式，首先通过视觉传感器监测前方是否有障碍物，若无障碍物根据初始状态进行五次多项式求解目标得出轨迹方程；若有障碍物，则运用动态圆模型将车辆等效成无数个动态圆进行约束限制，求解有障碍物时的六次多项式，得到有障碍物时的轨迹方程。

3、根据上一步规划得到的轨迹方程进行轨迹跟踪，轨迹跟踪方法通过模型预测算法实现，模型预测算法步骤如图4所示。首先，对换道轨迹方程建模：将运动车辆用运动学方程表示，并与给定轨迹参考轨迹进行对比相减得出轨迹误差函数；其次，将轨迹方程进行离散化，根据车辆运动学确定控制状态变量为其中，分别为纵横速度，为横摆角速度，X,Y分别为横纵向位移。控制量为转角δ，输出量为横向位置，在计算过程中将路面摩擦系数和滑移率视为已知常量，对其进行离散化处理得出轨迹跟踪第一项目标函数，除此之外应对控制量进行优化约束，形成目标函数的第二项。再次，对离散后的轨迹方程进行初始点采集，经过算法计算后继续对离散点进行采样；接着，将采集到的点与期望离散点通过MPC算法对其进行求解；最后，判断采集到的离散点是否为轨迹跟踪终止点，若是终止点，则结束模型预测算法的轨迹跟踪；若不是终止点，则选取下一个采样点，继续进行算法计算实现对车辆的连续轨迹跟踪。

若系统开启ACC系统，利用模型预测实现自适应巡航。ACC系统的目标是让车能够进行安全行驶，辅助驾驶员减少交通事故发生，因此目标函数为车间距，两辆车相对速度为主。原理如图5，比较本车与前车的实际距离速度和期望距离速度之间的差值，以此建立误差模型，并根据误差模型建立目标函数，同时将车辆运动学模型作为约束条件；利用模型预测算法对建立的目标函数进行求解来确定车辆的行驶速度以保持安全距离；并将此时的车辆状态反馈给下一时刻的误差模型、进行下一次的求解完成自适应巡航。

通过多项式拟合和模型预测算结合，将所设计换道算法与自适应巡航算法相结合能够大大降低交叉路口或复杂车况的交通事故。

本发明不局限于本实施例，任何在本发明披露的技术范围内的等同构思或者改变，均列为本发明的保护范围。

Claims (2)

1.一种具有换道辅助功能的自适应巡航控制器，其特征在于：包括感知系统、换道预警系统、换道辅助系统和ACC系统，所述的感知系统、换道预警系统、换道辅助系统和ACC系统集成于整车控制器中，所述的感知系统分别与换道预警系统、换道辅助系统和ACC系统连接，所述的换道辅助系统分别与换道预警系统和ACC系统双向连接，所述的换道辅助系统分别与换道轨迹规划路径跟踪器和MPC控制器连接；所述的MPC控制器分别与ACC系统、节气门和控制踏板连接；所述的换道轨迹规划路径跟踪器与方向盘连接；

所述的感知系统包括RGB-D视觉传感器、单轴陀螺仪和光编码传感器，所述的RGB-D视觉传感器、单轴陀螺仪以及光编码器获得的信息作为输入对车辆进行定位和控制；在车辆行驶过程中，RGB-D视觉传感器用于检测前方障碍物，进行前方障碍物测距，确定可行驶安全区域；单轴陀螺仪和光编码器分别用于车辆角速度以及速度测量，获得车辆里程计信息；

所述的换道预警系统是当前方出现换道路况时报警并提醒驾驶员开始作换道准备；

所述的换道辅助系统是当前方有复杂交汇车辆出现时，控制车辆进行自主换道操作，在自主换道过程中首先进行换道轨迹的规划，其次对经过规划的路径进行跟踪来实现换道子系统的完成；

所述的ACC系统是当前方有同向车辆或无车辆时，控制车辆自动切换至ACC系统，ACC系统采用模型预测方法实现间距可变的跟车过程。

2.一种具有换道辅助功能的自适应巡航控制器的工作方法，其特征在于：包括以下步骤：

A、感知系统采集车辆前方路况：感知系统实时采集车辆前方路况，并将路况信息实时传输给具有换道辅助功能的自适应巡航控制器；具有换道辅助功能的自适应巡航控制器对车辆前方路况信息进行判断，若感知系统监测出前方有交汇车辆驶来或者驾驶员有换道意图时，换道预警系统发出带有换道声音的信号同时触发换道辅助系统进行工作；当车辆前方有同向行驶车辆或需要进行跟随操作时开启ACC系统；

B、换道预警系统根据感知系统传来的信号判断是否有换道必要，发出警报信号并将信号传给换道辅助系统，同时接收换道辅助系统是否已经开始执行，若已执行则关闭换道预警系统，若未执行，则继续开启换道预警系统；

C、换道辅助系统根据前方路况判断是否存在障碍物，将换道轨迹规划分为有障碍物换道和无障碍物换道，两种轨迹规划都采用拟合多次项轨迹方程的方式，最后对规划路径利用模型预测方法进行换道路线轨迹跟踪完成换道任务；

具体工作方法如下：

C1、根据上层感知系统判断换道前方路况

通过感知系统中的RGB-D视觉传感器对车辆前方状况进行实时检测；

C2、判断前方是否有障碍物

若前方未检测到障碍物的存在，则启动无障碍物换道模式转步骤C21；若前方检测到有障碍物则启动有障碍物换道模式，转步骤C22；

C21、规划换道过程无障碍多项式换道路径

根据车辆的起始状态和目标状态得出一条期望换道路径，使车辆在规定时间内到达相邻车道；对车辆横纵向运动利用五次多项式拟合换道轨迹：

分别对五次多项式求一阶导和二阶导相应得到速度和加速度的五次多项式，结合车辆换道的始末状态得出换道轨迹如下：

其中:

A＝(a₅,a₄,a₃,a₂,a₁,a₀)

B＝(b₅,b₄,b₃,b₂,b₁,b₀)

式中，f(x)为横向换道轨迹多项式函数，f(y)分别为纵向换道轨迹多项式函数，T_6×6为多次项矩阵，A、B均为多项式系数矩阵，根据车辆换道起始时刻到结束时刻的平均速度以及换道时间确定，以此求得换道轨迹方程；转步骤C3；

C22、规划换道过程有障碍多项式换道路径

若换道过程中有障碍物出现，则对式(1)进行约束限制；在实际车辆前行过程中，纵向速度是不断变化的，因此引入车辆几何学模型的方法对其进行约束限制；

C221、选用动态圆包围的形式对车辆进行等价替代，即以车辆宽度为直径所处位置不断作圆；以圆扫过的面积替代车辆模型；

C222、对该车辆模型进行分析得出各动态圆的方程，并分析换道车辆不发生碰撞的几何条件，得出约束条件如下：

(x₂-x₁)²+(y₂-y₁)²＞(R₁+R₂)² (3)

式中x_f为车辆前端的横坐标，x_r为车辆后端的横坐标，y_f为车辆前端的纵坐标，y_r为车辆后端的纵坐标，x₁为障碍车的横坐标，x₂为本车的横坐标，y₁为障碍车的纵坐标，y₂为本车的纵坐标，R₁为障碍车等效动态圆圆心，R₂为本车等效动态圆圆心，同时忽略横摆角速度和质心侧偏角的影响，将式(2)代入式(3)得：

[x_1r+u_1x(x_1f-x_1r-x_2r-u_2x(x_2f-x_2r))]²+[y_1r+u_1y(y₁-y_1r)-y_2r-u_2y(y_2f-y_2r)]²＞(R₁+R₂)² (4)

其中，x_1r为障碍车后端的横坐标，x_1f为障碍车前端的横坐标，x_2r为本车后端的横坐标，x_2f为本车前端的横坐标，y_1r为障碍车后端的纵坐标，y_1f为障碍车前端的纵坐标，y_2r为本车后端的纵坐标，y_2f为本车前端的纵坐标，

整理式(4)得：

α²a₆ ²+βb₆+γ＞0 (5)

其中α、β、γ是关于时间的函数，a₆、b₆是障碍换道中六次项的系数，根据求根公式确定a₆的范围：

C223、确定有障碍时的换道轨迹方程

基于式(5)确定换道的多项式系数，同时考虑在换道时间内用动态圆建立的车辆模型不能与障碍车辆的前后端存在交点，在车辆汇流处，假设在某一时刻本车的车头到达前车车尾处，则称其为碰撞时刻，若要两车不发生碰撞，此时刻本车在目标车道至少产生一个宽度为车身宽度的位移，分别对本车前后端进行临界碰撞检测，并根据本车车体长宽给出一边界值求a₆，以此确定多项式系数矩阵A、B的值代入式(1)形成有障碍物的换道轨迹方程；

C3、确定换道轨迹跟踪过程

换道轨迹跟踪采用模型预测算法即MPC法，将经过步骤C2确定的换道轨迹方程代入MPC轨迹跟踪控制器中进行跟踪；

车辆从一个已知点出发，记为车辆的初始状态，以车辆运动学作为预测方程基础，根据所需目标建立轨迹误差模型、车辆当前状态信息和未来控制预测车辆状态信息，建立目标函数和约束方程，得到约束条件和目标函数，将其输出量即车轮转角作用于车辆上，最终跟上期望轨迹；过程中最核心部分为建立目标函数和约束条件，具体步骤如下：

C31、建立轨迹跟踪目标函数

以车辆动力学模型建立的模型预测控制为基础，结合车辆行驶时的约束条件，对换道轨迹进行跟踪；轨迹跟踪的目的是使实际轨迹与期望轨迹误差最小，同时使控制量达到最小要求；

式中，Γ_y为轨迹目标函数中横向位移对应权重系数，为对角矩阵；Γ_u为轨迹目标函数中状态量对应权重系数，为对角矩阵；y(k+i)为第k+i时刻轨迹的实际位置，y_des(k+i)是预测期望位置，u(k+i)为第k+i时刻的状态量；

C32、建立轨迹跟踪约束条件

换道轨迹跟踪的约束条件有两个，分别是控制量不能超过规定极限值、前轮转角不能超过极限值，即：

u_min≤u(k+i)≤u_max

δ_min≤δ≤δ_max

D、当感知系统检测到行驶车道的前方有其他前行车辆时立刻触发ACC系统工作，ACC系统根据本车和前车之间的相对距离和相对车速信息，通过与车辆的ABS、发动机控制系统、自动变速器控制系统协调工作，使本车与前车始终保持安全距离行驶；若感知系统检测出前方有同方向行驶车辆且转角传感器未检测到从方向盘传来的转角信号，则开启ACC系统，ACC系统同样采用模型预测算法；

D1、建立ACC系统目标函数

ACC系统的目标是为了控制车距误差和相对速度，但为充分反映ACC系统的动态变化规律，将前车加速度作为ACC系统的干扰的，并通过对本车加速度和加速度变化率动态特性的考虑来提高状态空间模型的鲁棒性和控制精度；

为了减小每一步期望加速度的变化量，使得控制信号变化平缓，将期望加速度变化量作为第一控制目标：

其中：s表示期望加速度变化量的权重矩阵，舍去对优化问题不产生作用的无用项ρ₁，得到目标函数第一项:

第二控制目标为：预测输出值与参考输出值之间的差值最小化；该控制目标写成最小二范数的形式：

舍去对优化问题不产生作用的无用项ρ₂，得到目标函数第二项:

其中：

q＝diag[q₁,q₂,q₃,q₄]

其中：Y_r为预测时域内每一步的参考输出组成的矩阵；fa为第一步的参考输出矩阵，q为每一步输出的权重矩阵，q₁为对角矩阵Q中第一行第一列的权重系数，q₂为对角矩阵中第二行第二列的权重系数，q₃为对角矩阵中第三行第三列的权重系数，q₄为对角矩阵中第四行第四列的权重系数；

D2、建立ACC系统约束条件

在ACC系统的模型预测问题中，考虑车辆的物理条件限制；其中期望加速度的最大值最小值是模型预测问题中最常见的约束；期望加速度的上下界所形成的约束如下式所示：

即U_min≤U_c≤U_max，其中u_min为期望加速度的最小值，u_max为期望加速度的最大值，u_k为第k时刻的状态量，u_k+1为第k+1时刻的状态量，u_k+c-1为第k+c-1时刻的状态量；

为了达到理想的控制目标，使得ACC系统的性能指标满足控制目标的需求，对车间距Δx、相对速度v_rel、本车加速度a和本车加速度变化率j构成的ACC系统性能指标进行约束，定义向量χ＝[Δx,v,a,j]^T，对预测空间内的每一步的χ进行约束，用公式表示为：

即：

其中：

χ_min、χ_max分别表示χ的最小值和最大值，因此，关于ACC系统性能指标的约束条件为：

结束。