Balancing Computation Speed and Quality: A Decentralized Motion Planning Method for Cooperative Lane Changes of Connected and Automated Vehicles
参考资料来源:《Balancing Computation Speed and Quality: A Decentralized Motion Planning Method for Cooperative Lane Changes of Connected and Automated Vehicles》一文
A .问题描述
注:该论文讲述的是如何在保证无碰撞的情况下,多车同时变道的情形(multi-vehicle motion planning 问题),其把该问题总结为一个优化问题。所以在该节请注意各个约束条件和目标函数。并假设道路上全是网联汽车(CAVS),不存在断联的汽车。
1.运动学约束
为了描述整个CAVS的运动学特性,使用自行车模型进行建模,即:
关于
(
1
a
)
,
(
1
b
)
,
(
1
e
)
(1a),(1b),(1e)
(1a),(1b),(1e)参考参考我之前写的这篇博客https://blog.csdn.net/m0_55918181/article/details/135550721即可理解,
(
1
c
)
,
(
1
d
)
,
(
1
f
)
(1c),(1d),(1f)
(1c),(1d),(1f)就是简单的求导,下面说明一些重要变量的含义。
变量说明:
- i i i: 第 i i i架无人车 ( i = 1 , 2 , . . . , N v ) (i= 1,2,...,N_{v}) (i=1,2,...,Nv)
- ϕ i \phi_{i} ϕi:前轮转角(与车辆坐标系)
- θ i \theta_{i} θi:方位角
- L w L_{w} Lw:前后轴距离
- L f L_{f} Lf:前轴与AB距离
- L r L_{r} Lr:后轴与CD距离
给
(
1
)
(1)
(1)式加一些上下界限制
2.避障约束
对于整个CAVS来说,避障意味着:车与道路边界之间不能发生碰撞;车与车之间不能发生碰撞,下面说明一下这两类碰撞约束如何形成。
如Fig.2,保证每辆车的四个角点位于上下边界内(车与道路边界之间的碰撞约束)
如Fig.2,保证车辆
i
i
i的四个角点与车辆
k
k
k的任意的一个四个角点之间围成的三角形和大于
i
i
i车的矩形面积(车与车之间的碰撞约束)
3.任务约束
设整个CAVS从初始车道变化到目标车道的整个动态过程时间为
t
f
t_{f}
tf,同时设置整个CAVS的保持速度为
v
s
t
v_{st}
vst,故设置下面的初始和终态约束
初始的各个变量设置
终态的各个变量设置
初始车道和目标车道设置(
O
r
i
g
i
Orig_{i}
Origi表示
i
i
i车的初始车道,
D
e
s
t
i
Dest_{i}
Desti表示
i
i
i车的目标车道)
4.目标函数和模型建立
建立如下的模型,目标函数为
t
f
t_{f}
tf,约束为动力学约束、避障约束和任务约束
决策变量为: j e r k i ( t ) , ω i ( t ) , t f jerk_{i}(t),\omega_{i}(t),t_{f} jerki(t),ωi(t),tf,其余变量可由决策变量推出
B.两步法求解
针对建立的优化问题,直接求解比较麻烦。作者使用两部分去求解,核心思想在于先调整个CAV车与车之间的差距(调整之后的队形叫做sparse formation),在此基础上进行优化(距离变大之后,就不需要考虑车与车之间的避障约束),具体流程图如下:
1 Sparse Formation Calculation
根据上图说明一下,CAVS的索引以及 d i , k ∗ {d^{*}_{i,k}} di,k∗;
- CAVS的索引可以采用如图所示的方法,具体为为 ( i , N L i ) (i,NL_{i}) (i,NLi), i i i表示第 i i i条车道, N L i NL_{i} NLi表示第 N L i NL_{i} NLi级别车辆;
-
x
i
,
j
x_{i,j}
xi,j表示第
i
i
i条车道第
j
j
j级车对应横轴坐标值,设CAVS车道变化到sparse formation时刻为
t
ζ
t_{\zeta}
tζ,则定义
d
i
,
k
∗
{d^{*}_{i,k}}
di,k∗为:即同一车道相邻两个车辆之间的间隔差
1.1 Blind Lane Change Motion Planning
首先,求解 P 1 P_{1} P1,记其解为 S 1 S_{1} S1,其是原来问题除去车与车之间的碰撞约束,所以称为 B l i n d Blind Blind
1.2 Calculation of d i , k ∗ {d^{*}_{i,k}} di,k∗ via Collision Recovery
在1.1的基础上,通过碰撞检测寻找
d
i
,
k
∗
{d^{*}_{i,k}}
di,k∗具体的数值,具体为依据0时刻各个无人车对应的x坐标从大到小排序,记该集合为
Ψ
=
Ψ
1
,
.
.
.
,
Ψ
N
v
\Psi=\Psi_{1},...,\Psi_{N_{v}}
Ψ=Ψ1,...,ΨNv。
Ψ
1
\Psi_{1}
Ψ1的轨迹优先级最高,所以不更新
S
1
S_{1}
S1;对于
Ψ
2
\Psi_{2}
Ψ2进行碰撞检测(车与车之间的碰撞约束),若发生碰撞,轨迹水平偏移
Δ
S
\Delta S
ΔS,直到不发生碰撞,并更新
S
1
S_{1}
S1,一直遍历到所有无人车,即可得到
S
1
S_{1}
S1和
d
i
,
k
∗
{d^{*}_{i,k}}
di,k∗,下图为该过程的伪代码:
2 Formation Reconfiguration Motion Planning
在得到
d
i
,
k
∗
{d^{*}_{i,k}}
di,k∗之后,需要知道如何从原来初始时刻位置到
d
i
,
k
∗
{d^{*}_{i,k}}
di,k∗,注意,这里仅仅是间隔调整,所以可以对之前的约束条件进行简化。
调整到sparse formation过程中,仅仅在水平方向发生运动
因为在水平方向发生运动,所以可以对之前的运动学约束进行简化
同一车道相邻无人车间距要大于等于0
t
ζ
t_{\zeta}
tζ约束
综上可以得到,求解从原来初始时刻位置到 d i , k ∗ {d^{*}_{i,k}} di,k∗的答案,记 P 2 P_{2} P2的解为 S 2 S_{2} S2
3 Simultaneous Lane Changes
- 执行 S 2 S_{2} S2变化到sparse formation
- 执行1.2更新之后的 S 1 S_{1} S1使其完成任务末端约束(即换道)
C 结论与思考
论文结论:
i) is suitable for online decision making, ii) performs consistently in handling various constraints and iii) strikes a balance between CPU time and task completion time.
这里说一下iii) 点
如上图,
g
a
p
m
a
x
gap_{max}
gapmax表示初始时刻不同车辆之间的最大间隔,可以发现执行两步法的时间对
g
a
p
m
a
x
gap_{max}
gapmax不敏感,证明了该方法的稳定性。就如论文最后总结的一样 strikes a balance between CPU time and task completion time.