CN114475599A - 一种针对弯曲道路场景下的队列控制方法 - Google Patents

Abstract

本发明公开了一种针对弯曲道路场景下的队列控制方法，包括以下步骤,S1车辆纵向排队行驶并编号,S2所有车辆将自车信息发送给其他车辆，跟随车根据接收到的信息调整自身状态，S3为队列内的车辆建立车辆动力学模型，S4每个跟随车辆，以参考道路轨迹为控制目标，以与前车的最小安全距离为约束条件，设计代价函数并求解，得到一段预测时域内的最优的控制序列，S5将控制序列的第一个元素作用于自车系统进行控制，S6重复S4和S5，直至结束队列行驶。本发明可有效补充目前缺少的关于弯曲道路场景下的车辆队列控制方法，并实现较好的车辆队列跟踪效果。

Description

一种针对弯曲道路场景下的队列控制方法

技术领域

本发明涉及智能交通系统和智能网联汽车领域，特别是关于一种针对弯曲道路场景下的队列控制方法。

背景技术

随着汽车数量的不断增加，交通堵塞问题日益严重，传统的交通管理模型及车辆安全技术越来越难以应对这些挑战。而智能交通系统为解决这些问题提供了一条有效的途径。智能交通系统是一种融合了信息、传感、通讯、控制、计算机等多个学科先进技术的智能化交通网络管理系统，具备高效、实时、精准等优势。智能网联汽车是智能交通系统的重要组成部分，为实现智能交通具有重要的研究意义。

车辆队列控制是智能网联汽车领域一个重要的研究方向。它将所有位于同一车道的车辆，根据其临近车辆的状态信息，如车间距、车速等，自动调整本车的横纵向运动状态，以达到车辆队列中所有车辆的速度一致并实现期望的车间距。由于所有的车辆的运行状态相似，因此车辆队列行驶在缓解交通拥堵、提高交通容量、增强驾驶安全性等方面具有巨大的潜力，是智能网联汽车研究领域的热点。

目前，研究车辆队列控制方法的专利有不少，比如湖南大学拥有的专利“一种考虑经济性的车辆队列控制方法及系统”，西北工业大学拥有的专利“一种车辆队列最优协同控制方法”，东北大学秦皇岛分校拥有的专利“一种基于预设性能函数的有限时间车队控制方法”等，但是这些专利一般从经济性最优或控制方法最优的角度来研究车辆队列控制方法，而较少研究车辆队列控制实现的道路场景。实际道路既有直道又有弯曲道路，车辆进行队列行驶时，必须对不同道路场景作不同的考虑。

发明内容

针对现有车辆队列控制技术存在的不足，本发明的目的是提供一种针对弯曲道路场景下的队列控制方法，在进行车辆队列控制时，既在纵向上控制车速，使其与其他车辆保持相对安全的距离，也在横向上控制前轮转角，使其准确跟踪道路中心。本发明可保证车辆队列在弯曲道路场景中也能实现很好的跟踪效果。

为实现上述目的，本发明提供如下技术方案：

一种针对弯曲道路场景下的队列控制方法，包括以下步骤包括如下步骤：

步骤一，车辆纵向排队行驶，各车辆通过无线通信技术与相邻车辆之间进行信息交换，将第一辆车记为首车，也称领航车辆，编号为0，将剩余车辆称为跟随车辆，编号为1至N-1，其中N为队列内车辆总数；

步骤二，队列内各车均为智能车辆，具备自动驾驶相关功能，如环境感知、定位规划、运动控制能力，首车根据自动驾驶规划的道路行驶，并将自车的状态信息，如位置、速度、加速度等发送给其他车辆，跟随车根据接收到的信息调整自身状态；

步骤三，对队列中的每辆车，建立考虑动力学特性的车辆动力学模型，为车辆实现横向跟踪轨迹和纵向调整车速打下基础；

步骤四，每个跟随车辆，以参考道路轨迹为控制目标，以与前车的距离为约束条件，基于模型预测控制算法，设计代价函数并求解，得到一段预测时域内最优的控制序列；

步骤五，每个跟随车辆将该控制序列的第一个元素作用于自车进行控制，得到该时刻自车的实际状态信息，将该信息发送给其他车辆，用于其他车辆使用；

步骤六，重复步骤四和五，直到结束队列行驶；

该发明的技术方案有如下特征：

(1)步骤三中，建立的模型为三自由度横纵向耦合动力学模型，由车辆的以下参数建模得到：车辆质量，重力加速度，转动惯量，前轮侧偏刚度，后轮侧偏刚度，质心到前轮的距离，质心到后轮的距离，滚动阻力系数，空气纵向阻力系数，空气垂向升力数c_z，车辆纵向速度v_x，车辆横向速度v_y，车辆横摆角速度

车辆纵向位置X，车辆横向位置Y，车辆横摆角ψ，轮胎纵向力F_x，前轮转角δ。建模的得到的三自由度横纵向耦合动力学模型为：

将上述模型写成状态空间方程形式：

y＝g(x)

选择状态量为：

选择控制量为：u＝[F_x,δ]^T；

选择输出量为：

v_x为车辆纵向速度，v_y为车辆横向车速，

为车辆横摆角速度，X为车辆纵向位置，Y为车辆横向位置，ψ为车辆横摆角，F_x为轮胎纵向力，δ为前轮转角；

(2)步骤四中的控制目标选为：

即通过控制使得车辆与参考道路的纵向位置差、横向位置差、横摆角速度差及参考横纵向速度差趋近于0；

X_r为参考道路中心的参考纵向坐标，Y_r为参考道路中心的参考横向坐标，

为参考横摆角速度，v_xr为期望车速，v_yr为参考横向车速；

(3)步骤四中的代价函数设定为如下：

其中，η(k+i|k)为k+i时刻预测方程输出量，η_ref(k+i)为k+i时刻期望的参考量，Δu(k+i|k)为k+i时刻的控制增量，ρ为权重系数，ε为松弛因子，Q_Q,R_R为权重矩阵，N_p为预测时域，N_c为控制时域；

第一项

可拆解为如下五项：

分别表示为在预测时域N_p内跟随车辆与参考车速的速度误差、横向速度误差、与参考道路中心的纵向位置误差、横向位置误差、横摆角误差，反映了系统对参考量的跟踪能力；

第二项

表示在控制时域N_c内控制增量的大小，反映了系统对控制增量平稳变化的要求。对各项控制目标都设置了相应的权重，通过调整控制权重Q_Q,R_R的值可调整各性能的控制要求；

(4)步骤四中首车的约束条件考虑道路几何形状约束和车辆本身执行机构约束。道路几何形状约束体现在道路曲率和道路摩擦系数对车速的影响，即：

其中：g为重力加速度，μ为道路摩擦系数，ρ为道路曲率。

车辆本身执行机构约束体现在车辆加速和减速时的最大加减速度限制以及车辆转弯时的转角限制，也即为对控制量和控制增量的约束。

跟随车考虑与前车的最小安全距离约束，使其满足两车实际距离与期望间距的差值趋近于零：

τ≥d_fl-d_des≥0

d_fl为跟随车与前车的距离，d_des为跟随车与前车的期望间距，τ为一个极小的常数；

特别地，由于针对弯曲道路场景进行控制，只考虑纵向方向上的距离计算并不合理，将两车的绝对距离值作为两车的实际距离：

(X，Y)为跟随车辆的位置点，(X_l，Y_l)为前车的位置点(假设(X_l，Y_l)值已知)；

两车期望间距通过基于紧急制动的安全间距策略计算得到：

在间距策略中考虑不同车辆性能差异，将车辆的制动能力考虑进去，保证车辆安全，得到更为合理的两车跟车距离；

v_x为跟随车辆当前的速度，a_x为跟随车当前的加速度，α为车速控制系数，β为加速度控制系数，a_max为跟随车辆的最大制动减速度，v_l为领导车辆当前的速度，a_max，l为领导车辆的最大制动减速度；

(5)步骤四中使用的模型预测控制算法中的控制量和控制增量的约束条件为：

u_min≤u(k+i|k)≤u_max，i＝0，1，…，N_c-1

Δu_min≤Δu(k+i|k)≤Δu_max，i＝0，1，…，N_c-1

(6)步骤四中中使用的车辆动力学模型及约束需进行线性化和离散化处理，以便于利用模型预测控制方法进行控制。

采用泰勒公式，将

在工作点[x_t，u_t-1]处进行线性化处理，并忽略高阶项，则有：

记

则将车辆动力学模型线性化后可得到用于模型预测控制的状态空间方程，方程如下：

其中，

采用前向欧拉进行离散化处理，即用下式来近似x对时间的导数；

其中：T为采样时间，则有：

x(k+1)＝(I+TA)x(k)+TBu(k)

记：A_k＝I+TA,B_k＝TB,C_k＝C，其中：I为单位矩阵，上式状态空间方程可化为：

根据模型预测控制原理，推导得到预测方程为：

Y＝Ψξ(k)+ΘΔU

其中：

采用同样方法对约束进行线性化和离散化处理有:

这样d_fl-d_des线性化之后可以表示成：

式中：d_fl是关于(X,Y,v_x,a_x)的线性函数，其中(X_r,Y_r,v_r,a_r)为参考点的信息(X_l,Y_l,v_l,a_l)为前车信息，均为已知量。

为便于状态量统一，将状态量选为

形式，则可记d_flr-d_desr＝a(X-X_r)+b(Y-Y_r)+c(v_x-v_r)+d(a_x-a_r)+e。

其中：

由于前车和后车的距离差需满足以下约束：

0≤d_fl-d_des≤τ

所以有：

0≤a(X-X_r)+b(Y-Y_r)+c(v_x-v_r)+d(a_x-a_r)+e≤τ

将上述不等式化为矩阵相乘形式，并将道路几何形状对车速的约束也写在一起，则有：

这里：

参考模型预测控制算法输出量约束处理的方法，上述约束可化为：

其中：

控制量和控制增量的约束条件可以转换为：

U_min≤D_kΔU+U_t≤U_max

ΔU_min≤ΔU≤ΔU_max

即：

其中：

I_2Nc是行数为2Nc的列向量，u(k-1)为上一时刻实际的控制量，U_max,U_min分别为控制时域内控制量的最大值、最小值集合，ΔU_max,ΔU_min分别为控制时域内控制增量的最小值、最大值集合。

上述带约束的模型预测控制问题可最终转换为如下二次规划问题求解：

s.t.MΔU≤γ

其中，H＝2(Θ^TQ_QΘ+R_R),g^T＝2E^TQ_QΘ,Ω＝E^TQ_QE+ρε²，Ω为常数，

(7)步骤五中在每个控制周期内完成对(6)中的优化问题进行求解，得到控制时域N_c内的最优控制增量序列如下：

ΔU^*＝[Δu^*(k)，Δu^*(k+1)，...，Δu^*(k+N_c-1)]^T

将该最优控制增量序列的第一个作为实际的控制增量作用于系统，即：

u(k)＝u(k-1)+Δu^*(k)

在下个控制周期内重复上述过程，如此反复即可实现车辆队列控制。

本发明的有益效果为：

本发明提出的针对弯曲道路场景下的队列控制方法，可有效补充目前缺少的关于弯曲道路场景下的车辆队列控制方法，并实现较好的车辆队列跟踪效果。

附图说明

图1是本发明一种针对弯曲道路场景下的队列控制方法的流程图。

具体实施方式

以下将结合附图，对本发明的技术方案具体实施过程进行详细说明。

实施例1，参照图1：

1.步骤一，车辆纵向排队行驶，各车辆通过无线通信技术与相邻车辆之间进行信息交换，将第一辆车记为首车，也称领航车辆，编号为0，将剩余车辆称为跟随车辆，编号为1至N-1，其中N为队列内车辆总数；

2.步骤二，队列内各车均为智能车辆，具备自动驾驶相关功能，如环境感知、定位规划、运动控制等能力，首车根据自动驾驶规划的道路行驶，并将自车的状态信息，如位置、速度、加速度等发送给其他车辆，跟随车根据接收到的信息调整自身状态，并且也将自身信息发送给其他车辆；

3.步骤三，对队列中的每辆车，建立三自由度横纵向耦合动力学模型，模型包括以下参数：车辆质量m，重力加速度g，转动惯量I_z，前轮侧偏刚度C_f，后轮侧偏刚度C_r，质心到前轮的距离l_f，质心到后轮的距离l_r，滚动阻力系数f_r，空气纵向阻力系数c_x，空气垂向升力系数c_z，车辆纵向速度v_x，车辆横向速度v_y，车辆横摆角速度

车辆纵向位置X，车辆横向位置Y，车辆横摆角ψ，轮胎纵向力F_x，前轮转角δ。建模的得到的三自由度横纵向耦合动力学模型为：

模型的状态空间方程形式为：

y＝g(x)

选择状态量为：

选择控制量为：u＝[F_x，δ]^T；

选择输出量为：

v_x为车辆纵向速度，v_y为车辆横向车速，

为车辆横摆角速度，X为车辆纵向位置，Y为车辆横向位置，ψ为车辆横摆角，F_x为轮胎纵向力，δ为前轮转角；

利用模型预测控制算法计算得到控制量，通过控制轮胎纵向力和前轮转角即可对车辆加减速和转弯进行控制；

4.步骤四，每个跟随车辆，以与参考道路中心的纵向位置差、横向位置差、横摆角速度差及参考横纵向速度差趋近于0为目标，即使得：

其中，X_r为参考道路中心的参考纵向坐标，Y_r为参考道路中心的参考横向坐标，

为参考横摆角速度，v_xr为期望车速，v_yr为参考横向车速；

5.明确了以上控制目标，可将模型预测控制算法中的代价函数设定如下：

其中，η(k+i|k)为k+i时刻预测方程输出量，η_ref(k+i)为k+i时刻期望的参考量，Δu(k+i|k)为k+i时刻的控制增量，ρ为权重系数，ε为松弛因子，Q_Q，R_R为权重矩阵，N_p为预测时域，N_c为控制时域；

第一项

可拆解为如下五项：

分别表示为在预测时域N_p内跟随车辆与参考车速的速度误差、横向速度误差、与参考道路中心的纵向位置误差、横向位置误差、横摆角误差，反映了系统对参考量的跟踪能力；

第二项

表示在控制时域N_c内控制增量的大小，反映了系统对控制增量平稳变化的要求。对各项控制目标都设置了相应的权重，通过调整控制权重Q_Q，R_R的值可调整各性能的控制要求；

6.算法约束条件除考虑控制量和控制增量约束以外：

u_min≤u(k+i|k)≤u_max，i＝0，1，…，N_c-1

Δu_min≤Δu(k+i|k)≤Δu_max，i＝0，1，…，N_c-1

首车的约束条件还考虑道路几何形状约束和车辆本身执行机构约束。道路几何形状约束体现在道路曲率和道路摩擦系数对车速的影响，即：

其中：g为重力加速度，μ为道路摩擦系数，ρ为道路曲率。

车辆本身执行机构约束体现在车辆加速和减速时的最大加减速度限制以及车辆转弯时的转角限制，也即为对控制量和控制增量的约束。

跟随车的约束条件在前车约束条件的基础上考虑与前车的最小安全距离约束，使其满足两车实际距离与期望间距的差值趋近于零：

τ≥d_fl-d_des≥0

d_fl为跟随车与前车的距离，d_des为跟随车与前车的期望间距，τ为一个极小的常数；

特别地，由于针对弯曲道路场景进行控制，只考虑纵向方向上的距离计算并不合理，将两车的绝对距离值作为两车的实际距离：

(X，Y)为跟随车辆的位置点，(X_l，Y_l)为前车的位置点(假设(X_l，Y_l)值已知)；

两车期望间距通过基于紧急制动的安全间距策略计算得到：

在间距策略中考虑不同车辆性能差异，将车辆的制动能力考虑进去，保证车辆安全，得到更为合理的两车跟车距离；

其中，v_x为跟随车辆当前的速度，a_x为跟随车当前的加速度，α为车速控制系数，β为加速度控制系数，a_max为跟随车辆的最大制动减速度，v_l为领导车辆当前的速度，a_max，l为领导车辆的最大制动减速度；

7.需将建立的车辆动力学模型及约束进行线性化和离散化处理，以便于利用模型预测控制方法进行控制。

采用泰勒公式，将

在工作点[x_t，u_t-1]处进行线性化处理，并忽略高阶项，则有：

记

则将车辆动力学模型线性化后可得到用于模型预测控制的状态空间方程，方程如下：

其中，

采用前向欧拉进行离散化处理，即用下式来近似x对时间的导数。

其中：T为采样时间，则有：

x(k+1)＝(I+TA)x(k)+TBu(k)

记：A_k＝I+TA,B_k＝TB,C_k＝C，其中：I为单位矩阵，上式状态空间方程可化为：

根据模型预测控制原理，推导得到预测方程为：

Y＝Ψξ(k)+ΘΔU

其中：

采用同样方法对约束进行线性化和离散化处理有:

这样d_fl-d_des线性化之后可以表示成：

式中：d_fl是关于(X，Y，v_x，a_x)的线性函数，其中(X_r，Y_r，v_r，a_r)为参考点的信息，(X_l，Y_l，v_l，a_l)为前车信息，均为已知量。

为便于状态量统一，将状态量选为

形式，则可记

d_flr-d_desr＝a(X-X_r)+b(Y-Y_r)+c(v_x-v_r)+d(a_x-a_r)+e。

其中：

由于前车和后车的距离差需满足以下约束：

0≤d_fl-d_des≤τ

所以有：

0≤a(X-X_r)+b(Y-Y_r)+c(v_x-v_r)+d(a_x-a_r)+e≤τ

将上述不等式化为矩阵相乘形式，并将道路几何形状对车速的约束也写在一起，则有：

这里：

参考模型预测控制算法输出量约束处理的方法，上述约束可化为：

其中：

控制量和控制增量的约束条件可以转换为：

U_min≤D_kΔU+U_t≤U_max

ΔU_min≤ΔU≤ΔU_max

即：

其中：

I_2Nc是行数为2Nc的列向量，u(k-1)为上一时刻实际的控制量，U_max，U_min分别为控制时域内控制量的最大值、最小值集合，ΔU_max，ΔU_min分别为控制时域内控制增量的最小值、最大值集合。

以上带约束的模型预测控制问题可最终转换为如下二次规划问题求解：

s.t.MΔU≤γ

其中，H＝2(Θ^TQ_QΘ+R_R)，g^T＝2E^TQ_QΘ，Ω＝E^TQ_QE+ρε²，Ω为常数，

8.步骤五中在每个控制周期内完成对7中的优化问题进行求解，得到控制时域N_c内的最优控制增量序列如下：

ΔU^*＝[Δu^*(k)，Δu^*(k+1)，...，Δu^*(k+N_c-1)]^T

将该最优控制增量序列的第一个作为实际的控制增量作用于车辆系统，即：

u(k)＝u(k-1)+Δu^*(k)

随后将自车的实际状态发送给其他车辆，进入下一个循环；

9.每个跟随车辆重复步骤四和五，直至队列控制结束。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (8)

1.一种针对弯曲道路场景下的队列控制方法，其特征在于，包括以下步骤：

S1：车辆纵向排队行驶，各车辆通过无线通信技术与相邻车辆之间进行信息交换，将第一辆车记为首车，也称领航车辆，编号为0，将剩余车辆称为跟随车辆，编号为1至N-1，其中N为队列内车辆总数；

S2：首车根据自动驾驶规划的道路行驶，并将自车的状态信息发送给其他车辆，跟随车根据接收到的状态信息调整自身状态，所述状态信息包括位置、速度和加速度；

S3：对队列中的每辆车，建立考虑动力学特性的车辆动力学模型，为车辆实现横向跟踪轨迹和纵向调整车速打下基础；

S4：每个跟随车辆，以参考道路轨迹为控制目标，以与前车的距离为约束条件，基于模型预测控制算法，设计代价函数并求解，得到一段预测时域内最优的控制序列；

S5：每个跟随车辆将该控制序列的第一个元素作用于自车进行控制，得到该时刻自车的实际状态信息，将该信息发送给其他车辆，用于其他车辆使用；

S6：重复步骤S4和S5，直到结束队列行驶。

2.根据权利要求书1所述的一种针对弯曲道路场景下的队列控制方法，其特征在于，所述对队列中的每辆车，建立考虑动力学特性的车辆动力学模型的步骤具体包括：

建立三自由度横纵向耦合动力学模型，由车辆的以下参数建模得到：车辆质量m，重力加速度g，转动惯量I_z，前轮侧偏刚度C_f，后轮侧偏刚度C_r，质心到前轮的距离l_f，质心到后轮的距离l_r，滚动阻力系数f_r，空气纵向阻力系数c_x，空气垂向升力系数c_z，车辆纵向速度v_x，车辆横向速度v_y，车辆横摆角速度

车辆纵向位置X，车辆横向位置Y，车辆横摆角ψ，轮胎纵向力F_x，前轮转角δ；三自由度横纵向耦合动力学模型为：

将所述三自由度横纵向耦合动力学模型写成状态空间方程形式：

y＝g(x)

选择状态量为：

选择控制量为：u＝[F_x，δ]^T；

选择输出量为：

v_x为车辆纵向速度，v_y为车辆横向车速，

为车辆横摆角速度，X为车辆纵向位置，Y为车辆横向位置，ψ为车辆横摆角，F_x为轮胎纵向力，δ为前轮转角。

3.根据权利要求1所述的一种针对弯曲道路场景下的队列控制方法，其特征在于，所述S4中的控制目标选为：

即通过控制使得车辆与参考道路的纵向位置差、横向位置差、横摆角速度差及参考横纵向速度差趋近于0；

X_r为参考道路中心的参考纵向坐标，Y_r为参考道路中心的参考横向坐标，

为参考横摆角速度，v_xr为期望车速，v_yr为参考横向车速。

4.根据权利要求1所述的一种针对弯曲道路场景下的队列控制方法，其特征在于：所述代价函数设定为如下：

其中，η(k+i|k)为k+i时刻预测方程输出量，η_ref(k+i)为k+i时刻期望的参考量，Δu(k+i|k)为k+i时刻的控制增量，ρ为权重系数，ε为松弛因子，Q_Q，R_R为权重矩阵，N_p为预测时域，N_c为控制时域；

第一项

可拆解为如下五项：

分别表示为在预测时域N_p内跟随车辆与参考车速的速度误差、横向速度误差、与参考道路中心的纵向位置误差、横向位置误差、横摆角误差，反映了系统对参考量的跟踪能力；

第二项

表示在控制时域N_c内控制增量的大小，反映了系统对控制增量平稳变化的要求；对各项控制目标都设置了相应的权重，通过调整控制权重Q_Q，R_R的值可调整各性能的控制要求。

5.根据权利要求2所述的一种针对弯曲道路场景下的队列控制方法，其特征在于，首车的约束条件考虑道路几何形状约束和车辆本身执行机构约束；道路几何形状约束体现在道路曲率和道路摩擦系数对车速的影响，即：

其中：g为重力加速度，μ为道路摩擦系数，ρ为道路曲率；

车辆本身执行机构约束体现在车辆加速和减速时的最大加减速度限制以及车辆转弯时的转角限制，也即为对控制量和控制增量的约束；

跟随车的约束条件在前车约束条件的基础上，考虑与前车的最小安全距离约束，使其满足两车实际距离与期望间距的差值趋近于零：

τ≥d_fl-d_des≥0

d_fl为跟随车与前车的距离，d_des为跟随车与前车的期望间距，τ为一个极小的常数；

特别地，由于针对弯曲道路场景进行控制，只考虑纵向方向上的距离计算并不合理，将两车的绝对距离值作为两车的实际距离：

(X，Y)为跟随车辆的位置点，(X_l，Y_l)为前车的位置点(假设(X_l，Y_l)值已知)；

两车期望间距通过基于紧急制动的安全间距策略计算得到：

在间距策略中考虑不同车辆性能差异，将车辆的制动能力考虑进去，保证车辆安全，得到更为合理的两车跟车距离；

其中：v_x为跟随车辆当前的速度，a_x为跟随车当前的加速度，α为车速控制系数，β为加速度控制系数，a_max为跟随车辆的最大制动减速度，v_l为领导车辆当前的速度，a_max，l为领导车辆的最大制动减速度。

6.根据权利要求书5所述的一种针对弯曲道路场景下的队列控制方法，其特征在于，其对控制量和控制增量的约束条件为:

u_min≤u(k+i|k)≤u_max，i＝0，1，…，N_c-1

Δu_min≤Δu(k+i|k)≤Δu_max，i＝0，1，…，N_c-1。

7.根据权利要求书5所述的一种针对弯曲道路场景下的队列控制方法，其特征在于，将建立的所述三自由度横纵向耦合动力学模型及所述对控制量和控制增量的约束进行线性化和离散化处理，以便于利用模型预测控制方法进行控制；

采用泰勒公式，将

在工作点[x_t，u_t-1]处进行线性化处理，并忽略高阶项，则有:

记

则将车辆动力学模型线性化后可得到用于模型预测控制的状态空间方程，方程如下：

其中，

采用前向欧拉进行离散化处理，即用下式来近似x对时间的导数；

其中：T为采样时间，则有：

x(k+1)＝(I+TA)x(k)+TBu(k)

记：A_k＝I+TA，B_k＝TB，C_k＝C，其中：I为单位矩阵，上式状态空间方程可化为：

根据模型预测控制原理，推导得到预测方程为：

Y＝Ψξ(k)+ΘΔU

其中：

采用同样方法对控制量和控制增量的约束进行线性化和离散化处理有:

这样d_fl-d_des线性化之后可以表示成：

式中：d_fl是关于(X，Y，v_x，a_x)的线性函数，其中(X_r，Y_r，v_r，a_r)为参考点的信息，(X_l，Y_l，v_l，a_l)为前车信息，均为已知量；

为便于状态量统一，将状态量选为

形式，则可记d_flr-d_desr＝a(X-X_r)+b(Y-Y_r)+c(v_x-v_r)+d(a_x-a_r)+e；

其中：

由于前车和后车的距离差需满足以下约束：

0≤d_fl-d_des≤τ

所以有：

0≤a(X-X_r)+b(Y-Y_r)+c(v_x-v_r)+d(a_x-a_r)+e≤τ

将上述不等式化为矩阵相乘形式，并将道路几何形状对车速的约束也写在一起，则有：

这里：

参考模型预测控制算法输出量约束处理的方法，上述约束可化为：

其中：

控制量和控制增量的约束条件可以转换为：

U_min≤D_kΔU+U_t≤U_max

ΔU_min≤ΔU≤ΔU_max

即：

其中：

I_2Nc是行数为2Nc的列向量，u(k-1)为上一时刻实际的控制量，U_max，U_min分别为控制时域内控制量的最大值、最小值集合，ΔU_max，ΔU_min分别为控制时域内控制增量的最小值、最大值集合；

上述带约束的模型预测控制问题可最终转换为如下二次规划问题求解：

s.t.MΔU≤γ

其中，H＝2(Θ^TQ_QΘ+R_R)，g^T＝2E^TQ_QΘ，Ω＝E^TQ_QE+ρε²，Ω为常数，

8.根据权利要求书7所述的一种针对弯曲道路场景下的队列控制方法，其特征在于，在每个控制周期内完成对优化问题进行求解，得到控制时域N_c内的最优控制增量序列如下：

ΔU^*＝[Δu^*(k)，Δu^*(k+1)，...，Δu^*(k+N_c-1)]^T

将该最优控制增量序列的第一个作为实际的控制增量作用于系统，即：

u(k)＝u(k-1)+Δu^*(k)

在下个控制周期内重复上述过程，如此反复即可实现车辆队列控制。

Images