CN112660124A - 一种用于换道场景的协同自适应巡航控制方法 - Google Patents

Abstract

本发明涉及一种具有换道辅助功能的协同自适应巡航控制方法，其主要包含以下步骤：步骤一：领航车辆根据车辆换道前后状态以及临界碰撞条件建立具有避障能力的换道轨迹函数；步骤二：领航车辆根据自车和前车车速、车身几何参数以及道路参数等在临界碰撞条件下计算车辆最优换道轨迹；步骤三：车队依据本发明给出的方法跟踪该最优轨迹。本发明主要有以下三个创新点：(1)结合多项式法和最优化方法给出一种具有避撞功能的最优换道轨迹计算方法；(2)针对车队非匀质车辆建立非线性队列模型，相比以往方法将车队简化为线性模型更为贴近实际；(3)所设计的方法能够同时保证车队换道时的横纵向队列稳定性。

Description

一种用于换道场景的协同自适应巡航控制方法

技术领域

本发明涉及一种先进辅助驾驶***(ADAS)的设计，特别是关于一种具有换道辅助功能 的协同自适应巡航控制***控制方法(CACC)的设计。

背景技术

现如今车辆保有量越来越大，随之带来了一系列问题，比如交通拥挤、环境污染以及交 通事故频发等。随着通信技术的发展，V2X技术正在受到人们越来越多的关注，其中协同 自适应巡航控制由于能够将车辆编队协同行驶，从而能够大大降低车辆行驶过程中的能源消 耗以及事故发生率等，具有相当大的发展潜力。

车辆编队行驶需要满足多种要求，首先是安全性，即要求车辆不能发生碰撞；舒适性， 即车辆加速度等不能太大；队列稳定性，即车队上游的误差不会随着向下游传播而放大。依 托于通信技术的发展，车队中的车辆能够获得前车的各种信息，比如前车期望轨迹、速度以 及加速度等，充分利用这些信息，车队中的车辆能够协同前进，保证交通***的高效和安全。

当前很多对CACC***的研究集中在单车道纵向行驶方面，比如夏维和孙涛等人把交 通信息作为可以获取的条件,采用模型预测控制方法，把乘坐舒适性,燃油经济性,安全性和跟 车性作为待优化目标,同时加入车辆自身能力限制设计了一种协同式自适应巡航***方法， 该方法能够提高车队的燃油经济性以及舒适性，并且能够达到一个最优的速度调节(中国专 利：CN201610945654、“基于交通信息的协同式自适应巡航***算法”)，但该算法忽略了车 队的队列稳定性，前车受到干扰而产生的跟踪误差可能会随着车队不断放大，从而导致碰撞 等事故的发生。

同纵向运动情况相比，车队在换道、超车等横向运动时情况更为复杂，需要考虑其横向动力 学、换道安全路线的选择以及横向的单车稳定性和队列稳定性等。针对这个问题，当前所做 的研究还比较少。清华大学的党睿娜等人设计了一种车辆多目标协调换道辅助自适应巡航控 制方法，该方法能够保证换道情况下车辆换道时的跟踪性、舒适性和安全性要求，但是只针 对单车换道巡航问题，不适用于车队换道巡航场景(中国专利：CN201410033746.2、一种车 辆多目标协调换道辅助自适应巡航控制方法)。

对于车队来说，在换道场景中首先要能规划出一条符合安全性和舒适性的平滑换道轨迹， 然后车队车辆跟踪这条轨迹行驶，在行驶过程中车队中车辆还要始终能够跟踪上期望的轨迹 并且在这个过程中还要始终满足安全性、舒适性、渐近稳定性以及队列稳定性要求。

发明内容

本发明主要解决的是有障碍条件下车队协同换道问题，主要包含以下几点：

第一、针对有障碍换道场景依据换道始末部分预定义状态设计一条具有避障功能的换道 轨迹函数；

第二、利用最优化方法在保证舒适性和安全性的条件下规划出领航车辆的最优换道轨迹；

第三、利用分布式模型预测控制方法协调车队车辆的实际行驶状态，要求满足跟踪性、 舒适性、安全性、渐近稳定性以及队列稳定性要求。

为解决以上技术问题，本发明采取以下技术方案：

一种具有换道辅助功能的协同自适应巡航控制方法，包括步骤如下：

步骤一：车队首先匀速行驶，领航车辆根据自车预定义换道起点和终点纵向状态和道路 情况确定最优换道时间，设计具有避障功能的换道轨迹函数：

定义换道开始时刻为t_i，领航车辆状态为

终止时刻为t_f，领航车辆状 态为

临界碰撞时刻为t_c，领航车辆坐标为(X_c,Y_c)，根据多项式法建立 如下具有避障功能的换道轨迹函数：

步骤二：领航车辆根据自车和前车车速、车身几何参数以及最优换道时间等在临界碰撞 条件下计算车辆最优换道轨迹，以换道时纵向和横向舒适性和换道纵向距离最小为优化目标 建立代价函数采用最优化方法得到最优换道轨迹：

首先需要确定最优换道结束时刻t_f以及换道时与前车间距dist：

subject to

Eq.(1)

而后再根据得到的最优换道结束时间以及最优车间距可以得到最优换道轨迹X(t)和 Y(t)。

步骤三：设计分布式模型预测控制方法，保障车队行驶的安全性、跟踪性、舒适性、渐 近稳定性和队列稳定性；

车队依据给出的方法跟踪该最优轨迹，设计具有换道辅助功能的协同自适应巡航控制方 法，主要包含以下步骤：

首先建立单个车辆的局部最优控制方法，保证安全性、跟踪性以及舒适性：

(1)：针对车队中单个车辆i建立其非线性状态空间模型：

(2)：设计局部最优控制方法，保证安全性、跟踪性以及舒适性：

For i∈{1,2,...,N}at time t

subject to

(3)：设计分布式模型预测控制方法

每辆车同时求解其对应的局部最优控制问题，并针对车队中除领航车辆的其他车辆添加 额外约束

σ∈(x,y)来保证队 列稳定性。

本发明的上述方案具有以下有益效果：

(1)本发明针对有障碍换道场景设计了一条最优换道轨迹，使得换道过程能够平滑、 舒适且保证安全；

(2)本发明针对车队中单个车辆建立了其局部最优方法，保证了单个车辆行驶过程中 的跟踪性、安全性以及舒适性；

(3)本发明将单车控制方法拓展为适合车队的分布式模型预测控制方法，保证了车队 行驶过程中的渐近稳定性和队列稳定性。

附图说明

图1是无障碍场景换道示意图。

图2是有障碍场景换道示意图。

图3是车队拓扑结构图。

图4是单轨自行车模型。

图5是方法执行流程图。

图6a是车队分别在换道初速为15m/s时的运行轨迹；

图6b是车队分别在换道初速为25m/s时的运行轨迹。

图7a是车队在换道初速为15m/s时的纵向速度变化；

图7b是车队在换道初速为25m/s时的纵向速度变化。

图8a是车队分别在换道初速为15m/s(a)和25m/s(b)时的横向速度变化；

图8b是车队分别在换道初速为15m/s(a)和25m/s(b)时的横向速度变化。

图9a是车队分别在换道初速为15m/s时的纵向位置偏差；

图9b是车队分别在换道初速为25m/s(b)时的纵向位置偏差。

图10a是车队分别在换道初速为15m/s的横向位置偏差；

图10b是车队分别在换道初速为25m/s时的横向位置偏差。

图11a是车队分别在换道初速为15m/s时的纵向位置偏差绝对值；

图11b是车队分别在换道初速为25m/s时的纵向位置偏差绝对值。

图12a是车队分别在换道初速为15m/s时的横向位置偏差绝对值；

图12b是车队分别在换道初速为25m/s时的横向位置偏差绝对值。

具体实施方式

本发明提出的一种具有换道辅助功能的协同自适应巡航控制方法其包括以下步骤：步骤 一：领航车辆根据车辆换道前后状态以及临界碰撞条件建立具有避障能力的换道轨迹函数； 步骤二：领航车辆根据自车和前车车速、车身几何参数以及道路参数等在临界碰撞条件下计 算车辆最优换道轨迹；步骤三：设计分布式模型预测控制方法控制车队跟踪规划好的最优轨 迹行驶。

1)建立具有避障能力的换道轨迹函数

附图1所示为车辆无障碍换道情况示意图

L_i和L_f分别为车队领航车辆换道起始和终止时刻的状态，定义换道开始时刻为t_i，其 位置坐标为(X_i,Y_i)，终止时刻为t_f，其位置坐标为(X_f,Y_f)。换道过程轨迹规划就是要找到一条从L_i到L_f的连续光滑曲线，这条曲线需要满足的约束条件为换道起始位置以及换道终止位置车辆的运动状态参量，即换道初始状态

换道终止状态

换道初始状态以及终止状态一共有12个参数，可以满足12个未知量的求解，因此可以 采用五次多项式来表示车辆横向位置X(t)和纵向位置Y(t)，即：

将车辆换道前后状态值代入上式即可确定换道轨迹。

附图2所示为有障碍换道场景示意图。该图中障碍是指车队前方车速较慢的车辆。

定义图中对应换道始末时刻车队前方车速较慢的车辆状态分别为P_i和P_f。为了避免碰 撞，保证安全性，这里我们考虑一种换道时的临界碰撞状态，即在t_c时刻，车队领航车辆右 前角与车队前方车辆左后角纵向位置相同，并且领航车辆横向位移为车队前方车辆的宽度W， 由于车辆换道时其横向位移与纵向位移是单调递增的，因此，只要在t_c时刻不发生碰撞， 以后就不会再发生碰撞。L_c和P_c分别为临界碰撞时刻车队领航车辆以及车队前方车辆的状 态。临界状态满足以下方程：

这里x_c和y_c分别为领航车辆换道初始时刻到临界碰撞时刻的纵向和横向位移；

为换道初始时刻到临界碰撞时刻车队前方车辆的纵向位移；W_p和L_p分别为车队前方车辆的宽度和长度；dist为换道时刻两车车间距；

为领航车辆在 t_c时刻的横摆角；h为领航车辆对角线的半长；α为领航车辆对角线与车身纵向中心线的夹角。

在实际换道过程中，横向速度一般远小于纵向速度，因此ψ_c＜＜α，这里将临界碰撞方 程可改写为：

考虑到车辆轨迹规划时需要满足的临界碰撞条件，这里需要在无碰撞换道轨迹规划的基 础上添加某一待定参数，为了兼顾换道时效性与舒适性，这里只增加纵向运动方程的高次项。 因此，调整公式(8)为：

2)计算车辆最优换道轨迹

本文设定车队首先以

匀速行驶一段时间，在t_i时刻开始换道，在t_f时刻换道结束，接 着再以

匀速行驶一段时间。那么车辆在换道始末位置的状态可以表示为

和

领航车辆以及车队前方车辆车身参数以及在换道过程中可以预先定义的状态如表1所示，在表中各项参数已知的情况下，需要确定的就是换道时刻t_f以及领航车辆与前方车辆的车间距离dist，我们将这两个变量作为自由变 量建立如下最优问题：

subject to

Eq.(12)

这里w₁,w₂,w₃,w₄,w₅是权重系数，惩罚函数J(dist_,t_f)是为了保证换道过程横纵向的舒适性， 同时要求换道过程在纵向方向距离尽可能小。

在确定了不同速度下的最优换道时间t_f和最优间距dist后，车辆的最优换道轨迹X(t)和Y(t) 也就确定了。表1是规划过程中采用的相关参数，本发明分别在

和

做了两组对比实验，规划出的最优路径和横纵向速度变化情况分别如附图6， 7，8中相关曲线所示。

3)设计分布式模型预测控制方法

(1)***模型

如附图3所示，为了保证方法处理的实时性，本发明针对车队中单个车辆i采用较为常 用的单轨自行车模型。

这里定义每辆车的状态为：

输 出为：

控制量：u_i(t)＝[u_a,i,u_δ,i]^T，干扰量

这里

和

分别是车辆在世界坐标系下的纵向和横向速度，那么我们可以得到每辆车的 状态空间形式为：

这里

和

都是二次连续可微的，

以上公式中m_i为车辆质量，v_x,i和v_y,i分别是沿着车身方向和垂直于车身方向的速度，ψ_i为车辆的横摆角，l_f,i和l_r,i分别是车辆质心与前后轮中心的距离，ω_i为角速度，I_i为车辆的转 动惯量，C_f和C_r分别是前后轮轮胎的侧偏刚度，α_f,i和α_r,i分别是前后轮的侧偏角，δ_i为车辆 转向角，a_i和u_a,i分别是车辆i沿着车身方向实际的加速度以及控制器给出的控制量，u_δ,i是 控制器给出的期望转角，T_a,i和T_δ,i分别是发动机以及转向***执行器的延迟时间。

如附图4所示,首车提供参考轨迹，本发明中车队中跟随车辆能够接收到首车和临近前车的 信息。车队的状态，输出，输入函数和干扰函数分别定义为：

此时车队的离散动力 学方程为：

这里

Υ＝diag{γ₁,...,γ_N}，Z＝diag{ζ₁...ζ_N}，

(2)控制目标

安全性：安全性是车辆行驶过程中的重中之重，本文采用文献[3]中的“trianglearea criterion”来避免碰撞，如图2示，第i辆车的四个角分别为{A_i,B_i,C_i,D_i},根据此法则，可以将 避免碰撞的约束条件表示如下：

这里i,k＝p表示车队前方车速较慢的车辆。

跟踪性：这里主要是指横纵向位置误差趋近于零，即

队列稳定性：队列稳定性是指车队上游车辆由于受到干扰而产生的位置误差等在向下游 传播时应该越来越小，即

这里α_X,i≤1,α_Y,i≤1。

(3)方法设计

本文采用分布式非线性模型预测控制方法。该方法能够处理多约束控制问题，适用于车 辆运动这类具有安全性、稳定性、舒适性以及经济性等多种要求的控制问题。

定义车队中第i辆车期望输出为

期望控制量为 u_des,i(t)＝[u_ades,i,u_δdes,i]^T。

车辆行驶过程中，我们采用固定车距策略，即车辆之间的期望车间距为一固定值d，再 结合规划出的轨迹，我们可以得到车队中每辆车的期望输出和期望控制量。

这里X_des(t),Y_des(t)分别是规划出的期望横纵向位置，

是规划出的期望横纵向速 度，a_Xdes(t)是规划出的横向期望加速度，

是规划出的期望路径 曲率，这里为了符合实验中车辆实际换道的需要，规定转角值逆时针为正，顺时针为负。

(4)局部最优控制问题

首先分析在预测时域[t,t+p-1]内单个车辆的最优控制问题。

这里定义：

分别为预测时所用输出以及控制量。

假定的输出以及控制量，实际为前车上一步计算出来的最优量。

求解优化问题得到的最优量。

优化问题：

For i∈{1,2,...,N}at time t

subject to

Eq.(15)

在这里对控制量的约束包含了加速度以及转弯角度两部分限制，其中加速度限制是为了 保证驾驶员的舒适性，而转弯角度限制取决于车辆轮胎的物理限制。

定义损失函数：

这里Q_i≥0,R_i≥0,F_i≥0,G_i≥0是权重系数，D＝[d,0,0,0]^T。

(5)分布式模型预测控制方法

上一小节介绍的是单个车辆的局部最优控制问题，这一节将之扩展到整个车队，建立分 布式模型预测控制方法。

1°初始化。

在t＝0时刻，我们定义车队中的每辆车以初始期望速度匀速行驶，并保持期望的车间距 d，据此我们可以得到每辆车的初始状态x_i(0)和初始输入u_i(0)＝[0,0]^T。然后我们可以得到车 辆i在初始时刻向其他车辆传递的假定控制量和输出为：

这里y_i ^p(k|0)可以由以下公式得到：

2°DMPC方法迭代

对于t＞0的任意时刻，车队中的车辆i执行以下步骤：

步骤一.车辆i依据当前状态x_i(t)，当前假定输出

以及接收到的车辆i-1的假定输出

来解决 其局部最优控制问题并得到最优控制序列

在求解过程中需要注意以下两点：

A.对车辆i＝1来说，其接收到的前车假定输出为时刻[t,t+p-1]内规划出的期望输出

[y_des(t),y_des(t+1)...y_des(t+p-1)]。

B.对车辆i＝2,3N..来说，相比于首车i＝1求解过程中需要满足额外约束条件

这里σ∈(x,y)。

步骤二.利用上一步得到的最优控制序列求出最优状态和输出：

步骤三.计算假定输出

首先计算假定输入

接着计算假定控制输出：

步骤四.将最优控制序列的第一项作为控制输入：

步骤五.t＝t+1，回到1)迭代计算，直到t＝t_stop

(6)仿真实验结果

为了验证本发明中方法的有效性，这里给出了方法的仿真结果图分别如附图6-12所示， (a)和(b)分别代表换道初始速度为15m/s和25m/s两种情况下车队行驶情况。附图6中车 队领航车辆运行轨迹几乎与规划出的最优换道轨迹完全重合，后续车辆也完全跟踪上了各自 的期望轨迹，整个跟踪过程轨迹平滑并且没有发生碰撞现象；附图7是换道时纵向速度的变 化图，从图中可以看出两种速度情况下速度偏离期望速度的绝对值最大在0.004m/s左右， 相对于本身速度来说几乎可以忽略不记，整体纵向行驶过程非常平稳；附图8是两种初始速 度下车队横向速度变化对比图，从图中可以看出横向的速度跟踪效果非常好；附图9和附图 10分别是两种速度情况下车队车辆与临近前车之间纵向和横向位置偏差的实际值，从图中 可以看出，车队中第二辆车与领航车辆的车间距误差与车队后面车辆与临近前车的横纵向跟 踪误差值变化情况都是相反的，这是由于每辆车在求解其局部最优函数时不但要考虑跟踪其 临近前车，并且还要考虑跟踪期望轨迹，因此第二辆车跟踪领航车辆发生位置偏差时后面车 辆会在保证安全的前提下产生“补偿”效果；附图11和附图12是两种速度情况下车队横纵 向跟踪误差的绝对值，这是为了验证车队行驶过程中是否满足队列稳定性，从图中可以看出， 两种速度情况下不论是纵向还是横向的跟踪误差绝对值都是随着车辆序号的增加越来越小， 这就证明了车队在行驶过程中满足了队列稳定性。

表1车队中车辆相关车辆参数

	m<sub>i</sub>(kg)	l<sub>f,i</sub>(m)	l<sub>r,i</sub>(m)	I<sub>i</sub>(kg·m<sup>2</sup>)	τ<sub>a,i</sub>(s)	τ<sub>δ,i</sub>(s)
							1	1751	1.22	1.73	2583.3	0.73	0.73
2	1506	1.15	1.65	2111.1	0.65	0.65
							3	1891	1.27	1.76	2869.2	0.77	0.77
4	1699	1.21	1.71	2480.0	0.71	0.71
							5	1959	1.29	1.79	3012.4	0.79	0.79
6	1255	1.08	1.58	1664.8	0.58	0.58

表2轨迹规划过程所用参数

Claims (1)

1.一种具有换道辅助功能的协同自适应巡航控制方法，包括步骤如下：

步骤一：车队首先匀速行驶，领航车辆根据自车预定义换道起点和终点纵向状态和道路情况确定最优换道时间，设计具有避障功能的换道轨迹函数：

定义换道开始时刻为t_i，领航车辆状态为

终止时刻为t_f，领航车辆状态为

临界碰撞时刻为t_c，领航车辆坐标为(X_c,Y_c)，根据多项式法建立如下具有避障功能的换道轨迹函数：

步骤二：领航车辆根据自车和前车车速、车身几何参数以及最优换道时间等在临界碰撞条件下计算车辆最优换道轨迹，计算车辆最优换道轨迹；

首先需要确定最优换道结束时刻t_f以及换道时与前车间距dist：

subject to

Eq.(1)

而后再根据得到的最优换道结束时间以及最优车间距可以得到最优换道轨迹X(t)和Y(t)；

步骤三：车队依据给出的方法跟踪该最优轨迹，设计具有换道辅助功能的协同自适应巡航控制方法，主要包含以下步骤：

(1)：针对车队中单个车辆i建立其非线性状态空间模型：

(2)：设计局部最优控制问题

(3)：设计分布式模型预测控制方法

每辆车同时求解其对应的局部最优控制问题，并针对车队中除领航车辆的其他车辆添加额外约束

来保证队列稳定性。

Images