基于稀疏化与渐进约束动态优化的多车协同换道方法与流程

on intelligent transportation systems,2015,16(3):1249
‑
1258中，desiraju等人根据目标车道的不同，将车辆进行分组，然后依次执行每组的车道变更。
9.在文献：bai l,hong l,xiao d,et al.centralized and optimal motion planning for large
‑
scale agv systems:a generic approach[j].advances in engineering software,2017,106(apr.):33
‑
46中，li采用正交配置直接转录法(orthogonal collocation direct transcription,ocdt)，将最优控制问题中的状态/控制变量全部离散化为决策变量，从而将最优控制问题转化为非线性规划(non
‑
linear programing,nlp)问题，然后采用内点法(interior
‑
point method,ipm)对nlp问题进行求解。
[0010]
需要指出的是，在处理多车协同换道问题中，上述文献公开的算法还存在一些不足之处。首先，没有充分考虑每个cav的运动能力，也没有考虑整个团队的运动能力：相反，它们受到特定类型的函数或指定原则的高度限制，在降低模型复杂度的同时却破坏了问题的完整性。其次，协同换道问题的求解算法不能兼顾求解质量与求解速度。现有多车协同换道方法大多是联合规划，且各种分布式算法将集中式命题中的约束条件分解或解耦处理，并将各部分结果合并而形成最终结果，从而使多车协同能力受到制约，导致求解质量下降。最后，应用正交配置直接转录法(orthogonal collocation direct transcription,ocdt)和内点法(interior
‑
point method,ipm)求解优化换道问题并不会导致收敛性最优，因为nlp问题包含了大量避撞约束，且较强非线性的运动学方程在离散化后也传递到了nlp问题中，所以nlp求解困难。


技术实现要素：

[0011]
本发明的目的是提供一种基于稀疏化与渐进约束动态优化的多车协同换道方法，以解决现有技术多车协同换道算法存在约束条件多、安全性差、舒适性低、协作能力受限等问题。
[0012]
为了达到上述目的，本发明所采用的技术方案为：
[0013]
基于稀疏化与渐进约束动态优化的多车协同换道方法，包括以下步骤：
[0014]
步骤1、建立车辆运动学约束：
[0015]
在网联自动驾驶车辆cav的换道场景中，设y＝lb及y＝rb分别构成了道路左右两侧隔离带的边沿线，y＝orig与y＝dest分别表示原车道和目标车道的车道中心线；在t0时刻，设l个车道上共有n
v
个cav，且所有的cav都行驶在车道中心线上；
[0016]
采用二自由度模型描述网联自动驾驶车辆cav的运动学特性，并设车辆i矩形轮廓的四个顶点分别定义为a
i
、b
i
、c
i
和d
i
，其中a
i
代表车头左侧顶点，其余三个顶点按照顺时针方向依次确定，根据微分方程如公式(1)所示定义任意一个网联自动驾驶车辆cavi的动力学约束，i＝1,2,
…
,n
v
，公式(1)如下：
[0017]
[0018]
公式(1)中，t0、t
f
分别为运动的起始及终止时刻，t0已知，但t
f
未固定；(x
i
(t),y
i
(t))为车辆i的后轮轴中点坐标；v
i
(t)及a
i
(t)分别为沿车体纵轴方向的速度及加速度，以使车辆前进方向为正方向；φ
i
(t)为车辆i前轮偏转角，以左转方向为正方向；ω
i
(t)为车辆i前轮偏转角速度；θ
i
(t)则为车辆i在坐标系中的姿态角；l
w
为车辆几何参数中的前后轮轴距，并设车辆几何参数中的前悬距离为l
f
，车辆几何参数中的后悬距离为l
r
，车辆几何参数中的车宽为l
b
；
[0019]
此外，设定以下约束如公式(2)所示：
[0020][0021]
公式(2)中，φ
max
、a
max
、v
max
以及ω
max
分别为最大允许偏转角度值、最大角速度、安全速度和最大偏转角度；|θ
i
(t)|≤π/2和v
i
(t)≥0表示禁止车辆逆向行驶或倒车行驶；
[0022]
步骤2、建立碰撞约束，包括车辆间的碰撞约束、车辆与道路边沿线的碰撞约束、边值约束，过程如下：
[0023]
(2.1)、建立车辆间的避撞约束：
[0024]
车辆在整个运动时域[t0,t
f
]上应避免碰撞，设场景中不存在运动行为不可控的车辆，并确立平面坐标系oxy：将道路右侧边沿线设置为x轴，以道路前进方向为正方向；设置y轴正方向指向道路左侧边沿线；
[0025]
采用矩形近似描述车辆，设同一场景中存在车辆i与车辆j，为防止两车相撞，要求车辆i矩形轮廓的四个顶点位于车辆j矩形轮廓的外部，且车辆j矩形轮廓的四个顶点位于车辆i矩形轮廓的外部；因此对应任意车辆，设该车辆矩形的四个顶点分别为a、b、c、d，并另设一个非顶点的点p，为限制点p处于矩形abcd的外部，采用基于图形面积的代数不等式建立约束条件如公式(3)所示：
[0026]
s
△
pab
s
△
pbc
s
△
pcd
s
△
pda
＞s
△
abcd
(3)，
[0027]
公式(3)中，s
△
pab
、s
△
pbc
、s
△
pcd
和s
△
pda
分别表示三角形pab、pbc、pcd和pda的面积；s
□
abcd
表示矩形abcd面积；
[0028]
确定了约束条件公式(3)后，由车辆几何结构关系计算出车辆i矩形轮廓的四个顶点的坐标，如公式(4)所示：
[0029][0030]
公式(4)中，x
i
、y
i
以及θ
i
均为时间t的函数，即对应为x
i
(t)、y
i
(t)以及θ
i
(t)的简写；在任意时刻t，根据x
i
、y
i
以及θ
i
由公式(4)得到车辆i矩形轮廓顶点a
i
、b
i
、c
i
以及d
i
的坐标值。其中，a
ix
、a
iy
分别为顶点a
i
的x轴坐标和y轴坐标；b
ix
、b
iy
分别为顶点b
i
的x轴坐标和y轴坐标；c
ix
、c
iy
分别为顶点c
i
的x轴坐标和y轴坐标；d
ix
、d
iy
分别为顶点d
i
的x轴坐标和y轴坐标；
[0031]
由此，车辆i与车辆j的避撞约束条件可表述为如公式(5)所示：
[0032][0033]
公式(5)中，和分别为三角形xa
i
b
i
、xb
i
c
i
、xc
i
d
i
和xd
i
a
i
的面积；和分别表示三角形ya
i
b
i
、yb
i
c
i
、yc
i
d
i
和yd
i
a
i
的面积；分别表示车辆i和j的车身面积，的取值均为(l
r
l
w
l
f
)
·
l
b
；a
j
、b
j
、c
j
以及d
j
分别表示车辆j矩形轮廓顶点的坐标值；
[0034]
根据公式(5)，进一步得到为全部n
v
辆车之间的避撞约束条件，如公式(6)所示：
[0035][0036]
(2.2)、建立车辆与道路边沿线的避撞约束：
[0037]
将矩形车辆与边沿线的不碰撞等价于车辆矩形轮廓上的四个顶点位于边沿线的指定一侧，根据公式(4)确定x
i
、y
i
和θ
i
后，车辆i的四个顶点a
i
、b
i
、c
i
和d
i
即可确定；
[0038]
设y轴上的四个顶点的坐标表示为y
ai
、y
bi
、y
ci
和y
di
，那么车辆i没有碰到y＝lb或y＝rb的要求如公式(7)所示：
[0039][0040]
公式(7)中，γ表示y轴上的四个顶点坐标y
ai
、y
bi
、y
ci
和y
di
的集合；
[0041]
(2.3)、建立边值约束：
[0042]
在初始时刻t0，车辆i沿着车道中心线y＝orig行驶，其位置x
i
(t0)、y
i
(t0)即已给
出，其它变量需满足公式(8)：
[0043]
[v
i
(t0),a
i
(t0),φ
i
(t0),ω
i
(t0),θ
i
(t0)]＝[v
st
,0,0,0,0],i＝1,
…
,n
v
(8)，
[0044]
公式(8)中，v
i
(t0)及a
i
(t0)为初始时刻t0车体纵轴方向的速度及加速度；φ
i
(t0)为初始时刻t0的车辆前轮偏转角；ω
i
(t0)为初始时刻t0的前轮偏转角速度；θ
i
(t0)为初始时刻t0车辆在坐标系中的姿态角；v
st
＞0为预设的各车辆共同平稳行驶速度；
[0045]
在终止时刻t
f
，车辆i应沿目标车道平稳行驶，则终止时刻约束条件可建立为如公式(9)所示：
[0046]
[v
i
(t
f
),a
i
(t
f
),φ
i
(t
f
),ω
i
(t
f
),θ
i
(t
f
)]＝[v
st
,0,0,0,0],i＝1,
…
,n
v
(9)，
[0047]
公式(9)中，v
i
(t
f
)及a
i
(t
f
)为终止时刻t
f
车体纵轴方向的速度及加速度；φ
i
(t
f
)为终止时刻t
f
的车辆前轮偏转角；ω
i
(t
f
)为终止时刻t
f
的前轮偏转角速度；θ
i
(t
f
)为终止时刻t
f
车辆在坐标系中的姿态角；
[0048]
设车辆i从车道y＝orig换入车道y＝dest，则有公式(10)：
[0049]
[y
i
(t0),θ
i
(t0),y
i
(t
f
),θ
i
(t
f
)]＝[orig,0,dest,0](10)；
[0050]
步骤3、建立换道运动规划优化目标：
[0051]
将换道任务完成时间表述为t
f
‑
t0，换道平缓性表述为综合考虑换道速度和换道质量时，可将换道运动规划任务建立为如公式(11)所示的最优控制问题：
[0052][0053]
约束条件：运动学约束(1),(2)；
[0054]
避撞约束(6),(7)；
[0055]
边值约束(8)
‑
(10).
[0056]
公式(11)中，j为综合优化指数；λ为权重系数，反映了优化目标对任务完成的时效性与舒适性的综合考虑，取λ＞0；x
i
(t)、y
i
(t)、θ
i
(t)、v
i
(t)和φ
i
(t)属于状态变量；a＝[a
i
(t)]和ω＝[ω
i
(t)]属于控制变量；
[0057]
步骤4、对公式(11)进行稀疏优化求解，使j最小化，由此得到多车协同换道策略，过程如下：
[0058]
(4.1)、稀疏编队算法重构：
[0059]
将完整的协同换道过程分为两个阶段：[t0,t1]为阶段一，此阶段车辆不换道，各车辆通过加/减速来调整车间距，以去除车辆间的避撞约束，形成稀疏队形；[t1,t
f
]为阶段二，此阶段各车辆在无需考虑与其它车辆间的避撞约束下同时换道，形成盲目换道；
[0060]
首先估计出理想的稀疏队形，设在t0时刻车道j上有nl
j
辆车，则有公式(12)：
[0061]
x
j,k
(t0)
‑
x
j,k 1
(t0)＝d
j,k
,j＝1,
···
,lk＝1,
···
,nl
j
‑
1(12)，
[0062]
公式(12)中，x
j,k
(t0)和x
j,k 1
(t0)分别表示在t0时刻车道j上第个k车辆和第k 1个车辆的位置，d
j,k
表示t0时刻第j个车道上相邻车辆k和k 1之间的距离，且需要满足d
j,k
≥l
r
l
w
l
f
；l表示总车道数；d
j,k
表示t0时刻第j个车道上相邻车辆k和k 1之间的距离，且需要满足d
j,k
≥l
r
l
w
l
f
；
[0063]
稀疏队形需在t＝t1时刻被重构，则有如下公式(13)所示：
[0064][0065]
公式(13)中，x
j,k
(t1)和x
j,k 1
(t1)分别表示在t1时刻车道j上第个k车辆和第k 1个车辆的位置，表示在t1时刻每个车道上相邻车辆间的距离；
[0066]
稀疏队形的计算即t1时刻相邻车辆间的距离的计算，可转化为利用正交配置直接转录法(orthogonalcollocationdirecttranscription,ocdt)和内点法(interior
‑
pointmethod,ipm)求解一个多车盲目换道运动规划子问题如公式(14)所示：
[0067][0068]
约束条件：运动学约束(1),(2)；
[0069]
避撞约束(7)；
[0070]
边值约束(8)
‑
(10).
[0071]
计算得到公式(14)的最优解和最优解时对应的由此得到理想的稀疏队形；
[0072]
根据公式(14)得到理想的稀疏队形后，按稀疏化要求车辆编队在t1时刻形成上述理想的稀疏队形，且车辆在此阶段不换道，由此将约束条件公式(1)简化为公式(15)：
[0073][0074]
而约束条件公式(2)保持不变；
[0075]
为了限制同一车道内的相邻车辆之间不发生前后碰撞，即当nl
j
≥2时，将避撞约束公式(6)简化为如公式(16)所示：
[0076]
x
j,k
(t)
‑
x
j,k 1
(t)≥l
f
l
w
l
r
,j＝1,
…
,l,k＝1,
···
,nl
j
‑
1,t∈[t0,t1](16)，
[0077]
t1时刻的边值约束需满足公式(17)：
[0078]
[v
i
(t1),a
i
(t1),φ
i
(t1),ω
i
(t1),θ
i
(t1)]＝[v
st
,0,0,0,0],i＝1,
…
,n
v
(17)，
[0079]
因此，可将稀疏编队重构表述为如公式(18)所示：
[0080][0081]
约束条件:运动学约束(15),(2)；
[0082]
避撞约束(16)；
[0083]
边值约束(8),(17).
[0084]
然后通过ocdt和ipm法对公式(18)进行数值计算，所得解对应着车辆编队在阶段一的运动过程；
[0085]
当cav编队从车距为{d
i,k
}的初始队形转为车距为的稀疏队形后，视为无约束换道立即开始，即阶段二开始；在阶段二，各车辆换道轨迹的形态与子问题公式(14)的最优解一致，先根据阶段一公式(14)解出求解出(后将中各车辆的轨迹向x轴正方向适当平移，使阶段一公式(18)求解的与更新后的衔接成完整的车辆换道运动轨迹
[0086]
(4.2)、渐进约束动态优化：
[0087]
以稀疏编队重构算法形成的车辆换道运动轨迹为基础，引入渐进约束动态优化
方法，以获得最优控制问题公式(11)的无碰撞解，由此基于公式(11)的无碰撞解实现多车协同换道控制。
[0088]
进一步的，步骤4的(4.1)中，计算公式(14)获得之后，随即对中多车盲目换道过程进行碰撞检测；若是无碰撞解，则视初始队形为稀疏队形，此时得到计算过程直接结束；反之若中存在碰撞，则说明当前队形不够稀疏，此时引入碰撞恢复算法进行稀疏化调整，直至所有的碰撞都能避免，以得到
[0089]
进一步的，为了计算首先根据t0时刻车辆在x轴的坐标为n
v
个车辆分配优先级，车辆i的x
i
(t0)越大，则优先级越高；然后将所有车辆的优先级记录于指标集中，其中元素ψ
i
表示第i个优先级的车辆；基于盲目换道的最优解以及指标集ψ，采用试探法计算出充分稀疏的队形，由此得到
[0090]
进一步的，步骤4的(4.2)中，首先将优化时域[t0,t
f
]平均划分为n
fe
段定义一系列简化的子问题p1,p2···
；为逐步增加避撞约束，使p1在[0,t
f
/n
fe
]内引入避撞约束，p2在[t
f
/n
fe
,2
·
t
f
/n
fe
]内引入避撞约束；以此类推，当k≥3时，p
k
在[(k
‑
1)
·
t
f
/n
fe
,k
·
t
f
/n
fe
]内引入避障约束；
[0091]
然后引入渐进约束动态优化方法，过程如下：
[0092]
首先，将稀疏编队重构算法得到最优解记为第一个子问题p1的初始猜测，计算求得p1的最优解并作为求解第二个子问题p2时初始猜测；再次，将获得第二个子问题p2的最优解作为第三个子问题p3的初始猜测；依次类推迭代，此迭代过程持续进行，直到获得无碰撞解。
[0093]
本发明面向网联自动驾驶车辆的多车协同换道，在现有的集中最优控制问题求解方法的基础上，研究车辆队列的稀疏化方法，并探索一种逐步优化的动态方法，以提高多车协同换道的安全性和通行效率，寻找通用的多车协同换道方案的最优解。本发明主要创新有：1)为了系统地描述换道任务，在现有的集中式最优控制问题中引入边值约束条件，形成一种多车协同换道的精准建模；2)为简化数值求解过程，探索一种基于稀疏化原理的两阶段换道方法，形成一个先稀疏处理后整体换道的多车换道方案；3)为优化稀疏后的换道轨迹，提出一种渐进约束的动态优化方法，得到最优控制问题的无碰撞解，提高了协同换道的统一性和安全性。
附图说明
[0094]
图1是本发明实施例中cav换道场景图。
[0095]
图2是本发明实施例中二自由度车辆运动模型示意图。
[0096]
图3是本发明实施例中基于图形面积的约束条件示意图。
[0097]
图4是本发明实施例中案例1下的最优换道轨迹示意图。
[0098]
图5是本发明实施例中案例2下的最优换道轨迹示意图。
[0099]
图6是本发明实施例中案例1中不同λ下的2车道头车的优化轨迹图。
具体实施方式
[0100]
下面结合附图和实施例对本发明进一步说明。
[0101]
本发明基于稀疏化与渐进约束动态优化的多车协同换道方法过程如下：
[0102]
1、多车协同换道基本命题构建
[0103]
本发明以集中最优控制问题的形式阐述了协同换道的多车运动规划方案。该方案由车辆运动学约束、避撞约束、边值约束以及优化目标等四个部分组成。一种cav换道场景如图1所示，其中y＝lb及y＝rb分别构成了道路左右两侧隔离带的边沿线；y＝orig与y＝dest分别表示原车道和目标车道的车道中心线。在t0时刻，假设l个车道上共有n
v
个cav，且所有的cav都行驶在车道中心线上。
[0104]
1.1、车辆运动学约束
[0105]
为了描述cav的运动学特性，本发明采用二自由度模型，具体如图(2)所示。二自由度模型特点是将车辆的两只前轮、两只后轮分别向车体纵轴方向合并为虚拟单轮，通过操控虚拟前轮的转角角度、虚拟后轮的线速度这两个独立变量来实现车辆运动
[18]
。为方便描述起见，将车辆i矩形轮廓的四个顶点分别定义为a
i
，b
i
，c
i
和d
i
，其中a
i
代表车头左侧顶点，其余三个顶点按照顺时针方向依次确定。二自由度模型遵循以下几点假设：
[0106]
1)忽略车辆在垂直于路面方向上的运动；
[0107]
2)忽略车轮在行驶中受到的阻力及地面摩擦力；
[0108]
3)车轮与地面始终保持良好的滚动接触。
[0109]
以下微分方程定义了cavi(i＝1,2,
…
,n
v
)的动力学约束：
[0110][0111]
公式(1)中，t0、t
f
分别为运动的起始及终止时刻，t0已知，但t
f
未固定；(x
i
(t),y
i
(t))为车辆i的后轮轴中点坐标；v
i
(t)及a
i
(t)分别为沿车体纵轴方向的速度及加速度，以使车辆前进方向为正方向；φ
i
(t)为车辆i前轮偏转角，以左转方向为正方向；ω
i
(t)为车辆i前轮偏转角速度；θ
i
(t)则为车辆i在坐标系中的姿态角；l
w
为车辆几何参数中的前后轮轴距，并设车辆几何参数中的前悬距离为l
f
，车辆几何参数中的后悬距离为l
r
，车辆几何参数中的车宽为l
b
；此外，还施加了以下约束：
[0112][0113]
公式(2)中，φ
max
、a
max
、v
max
以及ω
max
分别为最大允许偏转角度值、最大角速度、安全速度和最大偏转角度；|θ
i
(t)|≤π/2和v
i
(t)≥0表示禁止车辆逆向行驶或倒车行驶；a
i
(t)和v
i
(t)的约束保证了行车安全性；a
max
和ω
max
保证了行车舒适性。
[0114]
1.2、避撞约束
[0115]
车辆在整个运动时域[t0,t
f
]上应避免碰撞。假设场景中不存在运动行为不可控的车辆。在建模过程中，首先需确立平面坐标系oxy，如图2所示。可将道路右侧边沿线设置为x轴，以道路前进方向为正方向；设置y轴正方向指向道路左侧边沿线；坐标系原点o可结合具体任务确定。在换道过程中，潜在的碰撞风险可能发生于：1)各协同运动车辆之间；2)车辆与道路两侧边沿线之间
[19]
。因而可将避撞约束分成车辆间的避撞约束和车辆与道路边沿线间的避撞约束两种。
[0116]
1.2.1、车辆间的避撞约束
[0117]
为实现精细化建模，本发明按文献bail,wangk,shaoz.time
‑
optimalmaneuverplanninginautomaticparallelparkingusingasimultaneousdynamicoptimizationapproach[j].ieeetransactionsonintelligenttransportationsystems,2016,17(11):3263
‑
3274中公开的方法，采用矩形近似描述车辆。假设同一场景中存在车辆i与车辆j，为防止两车相撞，需要求车辆i矩形轮廓的四个顶点位于车辆j矩形轮廓的外部，且车辆j矩形轮廓的四个顶点位于车辆i矩形轮廓的外部。
[0118]
如图3所示，为限制点p处于矩形abcd的外部，可采用如文献：lib,shaoz.aunifiedmotionplanningmethodforparkinganautonomousvehicleinthepresenceofirregularlyplacedobstacles[j].knowledge
‑
basedsystems,2015,86:11
‑
20中公开的基于图形面积的代数不等式建立约束条件，具体为：
[0119]
s
△
pab
s
△
pbc
s
△
pcd
s
△
pda
＞s
□
abcd
(3)
[0120]
公式(3)中，s
△
pab
、s
△
pbc
、s
△
pcd
和s
△
pda
分别表示三角形pab、pbc、pcd和pda的面积；s
□
abcd
表示矩形abcd面积；
[0121]
确定了约束条件式(3)，接着，由车辆几何结构关系计算出车辆i矩形轮廓的四个顶点的坐标：
[0122][0123]
公式(4)中，x
i
、y
i
以及θ
i
均为时间t的函数，即对应为x
i
(t)、y
i
(t)以及θ
i
(t)的简写；在任意时刻t，根据x
i
、y
i
以及θ
i
由公式(4)得到车辆i矩形轮廓顶点a
i
、b
i
、c
i
以及d
i
的坐标值。其中，a
ix
、a
iy
分别为顶点a
i
的x轴坐标和y轴坐标；b
ix
、b
iy
分别为顶点b
i
的x轴坐标和y轴坐标；c
ix
、c
iy
分别为顶点c
i
的x轴坐标和y轴坐标；d
ix
、d
iy
分别为顶点d
i
的x轴坐标和y轴坐标。至此，车辆i与车辆j的避撞约束条件可表述为：
[0124][0125]
公式(5)中，和分别为三角形xa
i
b
i
、xb
i
c
i
、xc
i
d
i
和xd
i
a
i
的面积；和分别表示三角形ya
i
b
i
、yb
i
c
i
、yc
i
d
i
和yd
i
a
i
的面积；分别表示车辆i和j的车身面积，的取值均为(l
r
l
w
l
f
)
·
l
b
；a
j
、b
j
、c
j
以及d
j
分别表示车辆j矩形轮廓顶点的坐标值。
[0126]
根据公式(5)，可进一步得到为全部n
v
辆车之间的避撞约束条件，具体为：
[0127][0128]
需要注意的是，约束条件公式(5)、(6)中的变量均为时间t的函数，这些条件并非仅在某一时刻生效，而是在整个运动时域[t0,t
f
]上满足。
[0129]
1.2.2、车辆与道路边沿线间的避撞约束
[0130]
在车辆换道过程中，车辆与车辆之间不应碰撞，而车辆与道路边沿线之间也不应发生碰撞。如文献parkj,kimd,khuh.emergencycollisionavoidancebysteeringincriticalsituations[j].internationaljournalofautomotivetechnology,2021,22(1)(1):173
‑
184中所述，矩形车辆与边沿线的不碰撞，等价于车辆矩形轮廓上的四个顶点位于边沿线的指定一侧。根据式(4)，一旦x
i
，y
i
和θ
i
(i＝1,2,
…
,n
v
)确定，车辆i的四个拐角点(即a
i
，b
i
，c
i
和d
i
)即可确定。假设y轴上的四个拐角点的坐标表示为y
ai
，y
bi
，y
ci
和y
di
，那么车辆i没有碰到y＝lb或y＝rb的要求如下：
[0131][0132]
公式(7)中，γ表示y轴上的四个顶点坐标y
ai
、y
bi
、y
ci
和y
di
的集合。
[0133]
1.3、边值约束
[0134]
协同换道的目标是控制cav从原车道换到目标车道。在初始时刻t0，车辆i沿着车道中心线y＝orig行驶，其位置(即x
i
(t0)及y
i
(t0))已给出，其它变量需满足：
[0135]
[v
i
(t0),a
i
(t0),φ
i
(t0),ω
i
(t0),θ
i
(t0)]＝[v
st
,0,0,0,0],i＝1,
…
,n
v
(8)，
[0136]
公式(8)中，v
i
(t0)及a
i
(t0)为初始时刻t0车体纵轴方向的速度及加速度；φ
i
(t0)为初始时刻t0的车辆前轮偏转角；ω
i
(t0)为初始时刻t0的前轮偏转角速度；θ
i
(t0)为初始时刻t0车辆在坐标系中的姿态角；v
st
＞0为预设的各车辆共同平稳行驶速度。
[0137]
在终止时刻t
f
，车辆i应沿目标车道平稳行驶，则终止时刻约束条件可建立为：
[0138]
[v
i
(t
f
),a
i
(t
f
),φ
i
(t
f
),ω
i
(t
f
),θ
i
(t
f
)]＝[v
st
,0,0,0,0],i＝1,
…
,n
v
(9)，
[0139]
公式(9)中，v
i
(t
f
)及a
i
(t
f
)为终止时刻t
f
车体纵轴方向的速度及加速度；φ
i
(t
f
)为终止时刻t
f
的车辆前轮偏转角；ω
i
(t
f
)为终止时刻t
f
的前轮偏转角速度；θ
i
(t
f
)为终止时刻t
f
车辆在坐标系中的姿态角。
[0140]
设车辆i从车道y＝orig换入车道y＝dest，则有公式(10)：
[0141]
[y
i
(t0),θ
i
(t0),y
i
(t
f
),θ
i
(t
f
)]＝[orig,0,dest,0](10)，
[0142]
1.4、换道运动规划优化目标
[0143]
在面向车联网的多车协同换道中，换道任务应尽快完成，且换道过程应尽量平缓。换道任务完成时间可表述为t
f
‑
t0，换道平缓性可表述为据此，综合考虑换道速度和换道质量时，网联自动驾驶车辆多车协同换道运动规划任务可建立为以下最优控制问题如公式(11)：
[0144][0145]
约束条件：运动学约束(1),(2)；
[0146]
避撞约束(6),(7)；
[0147]
边值约束(8)
‑
(10).
[0148]
公式(11)中，j为综合优化指数；λ为权重系数，反映了优化目标对任务完成的时效性与舒适性的综合考虑，取λ＞0；x
i
(t)、y
i
(t)、θ
i
(t)、v
i
(t)和φ
i
(t)属于状态变量；a＝[a
i
(t)]和ω＝[ω
i
(t)]属于控制变量。
[0149]
2、多车协同换道稀疏优化求解
[0150]
为得到多车协同换道策略，需对优化问题公式(11)进行求解，即求解符合约束条件的所有控制变量以及t
f
，使得j最小化，最终获得所有车辆的运动轨迹。但高维的避撞约束使公式(11)难以解决，需要采用一些方法从公式(11)中暂时去除避撞约束，例如车辆稀疏化；然后逐步添加约束条件以形成原问题。本发明研究一种稀疏化和渐进约束动态优化方法，首先提出稀疏编队重构算法，求解一个去除了避撞约束的简化问题；接着提出渐进约束动态优化算法，在稀疏化的基础上逐步增加避障约束，迭代求解，直到得到原问题公式(11)的最优解，也即得到优化后的换道轨迹。
[0151]
2.1、稀疏编队重构算法
[0152]
该算法将完整的协同换道过程分为两个阶段：[t0,t1]为阶段一，此阶段车辆不换道，各车辆通过加/减速来调整车间距，以去除车辆间的避撞约束；[t1,t
f
]为阶段二，此阶段各车辆在无需考虑与其它车辆间的避撞约束下同时换道。为方便表述，本发明使用稀疏队形来表示去除避撞约束时形成的队形，将无视与其它车辆间的避撞约束进行换道的行为称为盲目换道。
[0153]
2.1.1、稀疏队形计算
[0154]
在编队稀疏化调整的过程中，首要任务是估计出理想的稀疏队形。假设在t0时刻车道j上有nl
j
辆车，则：
[0155]
x
j,k
(t0)
‑
x
j,k 1
(t0)＝d
j,k
,j＝1,
···
,lk＝1,
···
,nl
j
‑
1(12)，
[0156]
公式(12)中，x
j,k
(t0)和x
j,k 1
(t0)分别表示在t0时刻车道j上第个k车辆和第k 1个车辆的位置，d
j,k
表示t0时刻第j个车道上相邻车辆k和k 1之间的距离，且需要满足d
j,k
≥l
r
l
w
l
f
；l表示总车道数。稀疏队形需在t＝t1时刻被重构，则有如下定义：
[0157][0158]
公式(13)中，x
j,k
(t1)和x
j,k 1
(t1)分别表示在t1时刻车道j上第个k车辆和第k 1个车辆的位置，表示在t1时刻每个车道上相邻车辆间的距离。
[0159]
稀疏队形的计算，也即t1时刻相邻车辆间的距离的计算，可转化为利用正交配置直接转录法(orthogonal collocation direct transcription,ocdt)和内点法
(interior
‑
point method,ipm)求解一个多车盲目换道运动规划子问题：
[0160][0161]
约束条件：运动学约束(1),(2)；
[0162]
避撞约束(7)；
[0163]
边值约束(8)
‑
(10).
[0164]
相比于公式(11)，子问题公式(14)中不包括公式(6)，也即去除了车辆间的高维避障约束。因此求解过程简单，记其最优解为
[0165]
需要指出的是，求解公式(14)时忽略了车辆间的碰撞冲突，所得的解通常存在碰撞。因此，在获得之后，随即对中多车盲目换道过程进行碰撞检测。若是无碰撞解，则视初始队形为稀疏队形，此时盲目与非盲目行驶毫无差异，计算过程直接结束；反之，若中存在碰撞，则说明当前队形不够稀疏，需引入碰撞恢复算法进行稀疏化调整，直至所有的碰撞都能避免，以得到碰撞恢复算法为文献shi b,su y,wang c,et al.study on intelligent collision avoidance and recovery path planning system for the waterjet
‑
propelled unmanned surface vehicle[j].ocean engineering,2019,182(jun.15):489
‑
498中公开的算法。
[0166]
为计算首先根据t0时刻车辆在x轴的坐标为n
v
个车辆分配优先级，车辆i的x
i
(t0)越大，则优先级越高。然后将所有车辆的优先级记录于指标集中，其中元素ψ
i
表示第i个优先级的车辆。基于盲目换道解以及指标集ψ，采用一种试探法来计算出充分稀疏的队形，其伪代码如算法1所示：
[0167]
算法1.的计算
[0168]
1.导入
[0169]
2.将车辆优先级记录于指标集ψ；
[0170]
3.for n＝2:n
v
,do
[0171]
4.判断ψ
n
的换道轨迹是否与{ψ1,ψ2,
···
,ψ
n
‑1}的换道轨迹相撞；
[0172]
5.while碰撞存在,do
[0173]
6.将轨迹整体向x负方向上水平平移单位距离δs以更新ψ
n
的换道轨迹；
[0174]
7.判断ψ
n
更新后的换道轨迹是否与中记录的{ψ1,ψ2,
···
,ψ
n
‑1}换道轨迹相撞；
[0175]
8.end while
[0176]
9.将车辆{ψ
n
}更新的换道轨迹记录于
[0177]
10.end for
[0178]
11.根据公式(13)和计算
[0179]
12.结束
[0180]
算法1的核心思想是尝试不断拉大各车辆间的距离，直到车间距足以使盲目换道轨迹变得安全为止。算法1的步骤6中的参数δs代表了每次尝试拉大车间距的最小单位平
移长度。
[0181]
2.1.2稀疏队形重构
[0182]
在通过算法1估计出理想的稀疏队形后，接下来的问题是如何使车辆编队从初始队形转为这一稀疏队形。稀疏化要求车辆编队在t1时刻形成上述理想队形，且车辆在此阶段不换道。如此，则运动学约束条件(1)简化为：
[0183][0184]
而约束条件公式(2)则保持不变。
[0185]
由于在此期间车辆沿车道中心线行驶，不同车道内车辆之间不会发生碰撞，所有车辆也不会与道路边沿线相撞。因此只需限制同一车道内的相邻车辆之间不发生前后碰撞，也就是说，当nl
j
≥2时，避撞约束公式(6)简化为：
[0186]
x
j,k
(t)
‑
x
j,k 1
(t)≥l
f
l
w
l
r
,j＝1,
…
,l,k＝1,
···
,nl
j
‑
1,t∈[t0,t1](16)，
[0187]
t1时刻的边值约束需满足：
[0188]
[v
i
(t1),a
i
(t1),φ
i
(t1),ω
i
(t1),θ
i
(t1)]＝[v
st
,0,0,0,0],i＝1,
…
,n
v
(17)，
[0189]
因此，编队重构方案表述为：
[0190][0191]
约束条件:运动学约束(15),(2)；
[0192]
避撞约束(16)；
[0193]
边值约束(8),(17).
[0194]
求解公式(18)时，所使用的避撞约束(16)的规模随n
v
呈线性增长，因此求解简单。通过ocdt和ipm法对对式(18)进行数值计算，所得解对应着车辆编队在阶段一的运动过程。
[0195]
2.1.3无约束换道
[0196]
当cav编队从车距为{d
i,k
}的初始队形转为车距为的稀疏队形后，无约束换道立即开始。在阶段二，各车辆换道轨迹的形态与子问题(14)的最优解一致，先根据阶段一公式(14)解出后将中各车辆的轨迹向x轴正方向适当平移，使阶段一公式(18)求解的与更新后的衔接成完整的车辆换道运动轨迹
[0197]
2.2渐进约束动态优化方法
[0198]
步骤2.1所述的稀疏编队重构算法，为最优控制问题公式(11)的分布式计算过程，其在简化求解的同时也破坏了问题的完整性；将两阶段最优解合并为最终结果，制约了多车协同能力，导致求解质量下降；同时，先稀疏、后换道的方法也使得换道时间变长。为优化行驶轨迹，需研究如何通过逐步添加避撞约束以将优化问题还原到原问题。
[0199]
本发明以稀疏编队重构算法形成的换道轨迹为基础，引入一种渐进约束动态优化方法，以获得原问题公式(11)的无碰撞解。具体说，首先将优化时域[t0,t
f
]平均划分为n
fe
段定义一系列简化的子问题p1,p2···
；为逐步增加避撞约束，使p1在[0,t
f
/n
fe
]内引入避撞约束，p2在[t
f
/n
fe
,2
·
t
f
/n
fe
]内引入避撞约束；以此类推，当k≥3时，p
k
在[(k
‑
1)
·
t
f
/n
fe
,k
·
t
f
/n
fe
]内引入避障约束。
[0200]
渐进约束动态优化方法的思路如下。首先，将稀疏编队重构算法得到最优解记为
l
b
1.942mω
max
0.3rad/sv
st
10.0m/sn
fe
10
[0227]
(2)、结果与分析
[0228]
实验中，设置lb＝9.375m、rb＝
‑
5.625m、n
v
＝12及λ＝10，以说明本发明所提方法在随机配置下的换道性能。此处，随机性体现在1)每个cav的初始位置随机给出；2)每个车道运行的cav数目随机确定；3)每个车道上相邻cav之间的初始车间距均匀分布于[0,gap
max
]，其中gap
max
为最大车间距，取gap
max
＝5m。
[0229]
应用随机生成的初始条件，考虑两种具有不同换道需求的情况，分别记为案例1和2，案例1和2的优化换道轨迹如图4、5所示。从图4和5可以看出，所制定的最优控制问题在两种换道需求下均能实现无碰撞的运行轨迹；且即使一些车辆没有换道意图，在协同运动过程中仍允许其短暂偏离自身车道，以便必要时临时为其他cav动态地腾出空间，为编队运动提供额外的灵活性，实现真正意义上的协同运动。仿真结果表明，本发明所提换道方法既能体现对不同换道需求的统一性与有效性，又能保证车辆的协同运动。
[0230]
对比案例1和案例2的换道时间可知，针对同一种初始条件，案例1中t
f
＝9.9012s，而案例2中t
f
＝8.6717s；案例2换道完成较快。但案例1中的运行轨迹较案例2中的运行轨迹更平滑，交通流整体制动幅度不大。
[0231]
在最优控制问题中，权重系数λ反映了优化目标对任务完成的时效性、行驶轨迹的平缓性的综合考量。因此，选取案例1下2车道的头车，设置不同的λ，仿真可得如图6所示的优化轨迹图。从图6可知，λ＝10时轨迹较为平坦；随着λ的减少，轨迹逐渐弯曲，且行驶相同的路程所需时间也逐渐变小，车辆更关心最小化完成时间。仿真结果验证了λ的物理含义：λ取值越大，对车辆行驶的舒适性要求越高；相反，λ取值越小，轨迹的平滑性越低，换道完成时间越快。
[0232]
本发明针对现有多车协同换道存在舒适性低、求解速度慢等问题，在构建集中最优控制问题模型的基础上，本发明提出了一种基于稀疏化和渐进约束动态优化的多车协同换道方法。将队列稀疏化算法引入最优控制问题求解方法中，形成一种先稀疏后换道的两阶段换道方案；采用渐进约束的动态优化方法，在稀疏解基础上探究最优控制问题的可行解。仿真结果表明，所提方法在保证求解速度与质量的同时，改善了车辆通行效率，提供了更为舒适的行车体验。
[0233]
本发明所述的实施例仅仅是对本发明的优选实施方式进行的描述，并非对本发明构思和范围进行限定，在不脱离本发明设计思想的前提下，本领域中工程技术人员对本发明的技术方案作出的各种变型和改进，均应落入本发明的保护范围，本发明请求保护的技术内容，已经全部记载在权利要求书中。