论文解读——Trajectory Planning for BERTHA -a Local, Continuous Method
参考资料来源:《Trajectory Planning for BERTHA -a Local, Continuous Method》
注:本文按照论文原文顺序讲述,此方法是一个比较经典的在XYT优化的paper
A Preliminaries
- 轨迹规划是在全局坐标系进行
- map提供道路的左右边界
- Obstacle- and object data由立体视觉和雷达采集而来,静态障碍物为a set of stixels形式,动态障碍物为带速度矢量的矩形
Figure1中,白色车代表BERTHA,紫红色代表静态障碍物,带运动方向的蓝色为动态障碍物。
B Objective function
- 问题描述:轨迹规划的是无人车后轴中心点的运动轨迹 X ( t ) = ( x ( t ) , y ( t ) ) T X(t)=(x(t),y(t))^{T} X(t)=(x(t),y(t))T
- 目标函数的建立:
建立如(1)式的泛函指标,积分区间为
[
t
0
,
t
0
+
T
]
[t_{0},t_{0}+T]
[t0,t0+T]
具体而言, L L L由五项构成,分别为期望中心线项、期望速度项、加速度项、jerk项、tangent angle项
期望中心线项:
d
d
d代表距离函数
推荐速度项
v
d
e
s
v_{des}
vdes计算公式
加速度项
jerk项
tangent angle项
tangent angle计算
注:后续会详细说明距离函数和推荐速度详细计算
- 目标函数离散化
等时间间隔(间隔为
h
h
h)划分规划时间
利用有限差分近似速度和加速度项
泛函指标可以由原来积分形式转换为求和形式
注:轨迹规划的是无人车后轴中心点的运动轨迹 X ( t ) = ( x ( t ) , y ( t ) ) T X(t)=(x(t),y(t))^{T} X(t)=(x(t),y(t))T(而运动轨迹由一系列散点组成,并非连续)
C.Constraint functions
- 曲率约束
曲率的计算公式
- 加速度约束
- 外部约束:主要考虑道路边界、静态障碍物、动态障碍物的影响,其都用polygons的形式给出,类似于下图中(车辆动态障碍物),在每一个离散时间范围
[
t
i
,
t
i
+
1
]
[t_{i},t_{i+1}]
[ti,ti+1]内以一系列的多边形给出
为了考虑无人车的的几何特性,可以用一系列的等距圆来表征车的几何特性,后轴中心距离最后一个圆距离为 l m i n l_{min} lmin,后轴中心距离最前面一个圆距离为 l m a x l_{max} lmax,(a)图为一辆车情形,(b)图为一辆车前后时刻的情形,(c)图为一辆车整个轨迹的情形
最后可以得到如(11)式的外部约束, d ( p , x c ) d(p,x_{c}) d(p,xc)代表圆中心 x c x_{c} xc到多边形 d d d的距离, r v e h r_{veh} rveh代表圆的半径。
D Building constraint polygons from sensor data
- 先决策后规划:(a)图所示,从A到B规划一条优化某种平滑标准的轨迹,可能会产出许多局部最优点。(b)图所示,若先决策(对于障碍物往左或右nudge),最后规划的曲线是一个全局最优的
注:这个有点像EM planner一样,DP决策开辟凸空间
- 如何从传感器数据中建立
p
o
l
y
g
o
n
s
polygons
polygons多边形:如(a)中所示,将障碍物(single obstacle points)和边界(left and right bound)当成图的节点(其中较节点大的为道路边界)。如(b)中所示,节点之间比较相近,所以要切割最小数量的节点来区分。最后如(c)中所示,构建带有一定安全裕量的多边形。
E.Distance function
这一节主要介绍距离函数的设计过程,如下图(a)所示,
p
1
,
p
2
p_{1},p_{2}
p1,p2代表一个多边形中相邻的两个点,可以根据有限差分的方法计算其切向量
t
1
,
t
2
t_{1},t_{2}
t1,t2,
x
x
x为我们的投影点,所谓的的距离即是
n
λ
n_{\lambda}
nλ的模长(同时也具有方向),其需要满足
t
λ
n
λ
t_{\lambda}n_{\lambda}
tλnλ=0(即垂直)
点
p
λ
p_{\lambda}
pλ和
t
λ
t_{\lambda}
tλ计算公式如下:
所谓的梯度场,即是对
n
λ
n_{\lambda}
nλ单位化,(b)图为构造的梯度场,红色代表(多边形的边),黑色代表梯度,蓝色代表真实的点
注:按照原文的说法这样做是为了保证约束函数的连续可微
F.Re-planning scheme
因为车的前轮转角与车的曲率相关(车辆运动学模型),曲率由轨迹二阶导计算,所以希望轨迹三阶连续( C 2 c o n t i n u i t y C_{2} continuity C2continuity),同时有因为传感器不断获取数据,所以需要不断的re-planned轨迹。
如下图所示,黑色轨迹代表上一次迭代得到的轨迹,从
t
n
o
w
t_{now}
tnow开始,无人车沿着红色轨迹运动(be guided by the trajectory controller),假设新的轨迹规划时间小于
t
p
r
o
c
e
s
s
t_{process}
tprocess,这样就意味着旧轨迹控制器跟踪在
t
n
o
w
+
t
p
r
o
c
e
s
s
t_{now}+t_{process}
tnow+tprocess之前有效,为了保证反馈控制的连续性,所以新轨迹只能改变绿色的部分。同时为了保证(
C
2
c
o
n
t
i
n
u
i
t
y
C_{2} continuity
C2continuity),新轨迹在规划起点时应该选用上一个迭代得到的轨迹的三个点(
x
(
t
0
)
,
x
(
t
1
)
,
x
(
t
2
)
x(t_{0}),x(t_{1}),x(t_{2})
x(t0),x(t1),x(t2))
G.Constrained optimization
这个主要讲的是求解的技巧,这个先mark住,还是涉及到最优化和代码的问题,学会之后开一手专栏。