基于Frenet坐标系的智能驾驶运动规划方法
Baidu的State Lattice 方法
Frenet坐标系基本概念及坐标系建立
F
r
e
n
e
t
Frenet
Frenet 坐标系使用道路的中心线作为参考线，使用参考线的切线向量和法线向量建立坐标系，以车辆在参考轨迹上的投影点作为原点，坐标轴相互垂直，分别为
s
s
s 方向(即沿着参考线的方向，通常被称为纵向)和
d
d
d 方向(即参考线当前的法向，通常被称为横向)。
如下图所示，引用链接: link 中2.3章。
F r e n e t Frenet Frenet 坐标系在自动驾驶领域被广泛应用，常有学者研究基于 F r e n e t Frenet Frenet 坐标系的智能驾驶运动规划算法。那么为什么要研究基于 F r e n e t Frenet Frenet 坐标系下的规划方法尼?下面将详细阐述。
Frenet坐标系优势
可以参考这篇博客 link，里面讲的非常好。
车辆在公路上行驶时，道路的中心线(参考线)很容易就能找到，车辆的位置很容易用纵向距离(即沿道路的中心线方向的距离)和横向距离(即偏离参考线的距离)来表示。同时，
F
r
e
n
e
t
Frenet
Frenet 坐标系相比于笛卡尔坐标系来说，可以将车辆的二维运动问题解耦成两个一维运动问题，更便于求解。
因此需要将车辆和障碍物实时位置在笛卡尔坐标系空间的信息转换到
F
r
e
n
e
t
Frenet
Frenet 空间中。
关于
F
r
e
n
e
t
Frenet
Frenet 与 笛卡尔坐标系的转换推导参考下面两篇博客推导，后续有空自己再整理。
链接一: link
链接二: link
根据转化公式，可以将车辆及障碍物从笛卡尔坐标系空间下映射到
F
r
e
n
e
t
Frenet
Frenet 坐标系下。
横纵向轨迹解耦规划
内容参考：《智能驾驶技术_路径规划与导航控制》这本书4.4节
纵向轨迹规划
对于驾驶员和乘客来说，除了安全性之外，舒适性的体验也很重要。通常用加速度的变化率来表示舒适性。加速度变化率如下公式所示，
j
e
r
k
jerk
jerk 值越小，舒适度越高。
j
e
r
k
=
a
˙
=
d
a
d
t
jerk = \dot{a} = \frac{da}{dt}
jerk=a˙=dtda
T
a
k
a
h
a
s
h
i
Takahashi
Takahashi 的文章中已经证明，任何
j
e
r
k
jerk
jerk 最优化问题中的解都可以用五次多项式来表示。
L
o
c
a
l
p
a
t
h
p
l
a
n
n
i
n
g
a
n
d
m
o
t
i
o
n
c
o
n
t
r
o
l
f
o
r
A
G
V
i
n
p
o
s
i
t
i
o
n
i
n
g
Local \; path \;planning \;and \;motion \;control \;for \;AGV\; in \;positioning
LocalpathplanningandmotioncontrolforAGVinpositioning
link
因此，纵向轨迹表示为
s
(
t
)
=
a
0
+
a
1
t
+
a
2
t
2
+
a
3
t
3
+
a
4
t
4
+
a
5
t
5
s(t) = a_{0} + a_{1}t + a_{2}t^2 + a_{3}t^3 + a_{4}t ^4 + a_{5}t ^5
s(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5
假设本车在
F
r
e
n
e
t
Frenet
Frenet 坐标系下的初始状态
t
=
0
t=0
t=0，可以得到
s
(
0
)
=
a
0
s
′
(
0
)
=
a
1
=
v
v
e
h
s
′
′
(
0
)
=
2
a
2
=
a
v
e
h
s(0) = a_{0} \\ s'(0) = a_{1}=v_{veh} \\ s''(0)=2a_{2}=a_{veh}
s(0)=a0s′(0)=a1=vvehs′′(0)=2a2=aveh
因此只要能够确定目标状态，即可唯一确定纵向轨迹
s
(
t
)
s(t)
s(t)。目标状态分为
3
3
3种：巡航状态、跟随状态、超车状态。
(1) 巡航状态
若当前道路没有行人、其他车辆等障碍物，本车会以设定的最大巡航速度进行行驶，而且其实很多时候并不关注车辆行驶多长距离到达巡航速度，也并不要求精确地保持到某一个巡航速度，只要在巡航速度上下区间内就行。因此，巡航的目标状态
t
=
t
e
t = t_{e}
t=te 设定为
s
′
(
t
=
t
e
)
=
v
c
r
u
i
s
e
±
Δ
v
s
′
′
(
t
=
t
e
)
=
a
c
r
u
i
s
e
=
0
s'(t=t_{e})=v_{cruise} \pm \Delta v \\ s''(t=t_{e})=a_{cruise} = 0
s′(t=te)=vcruise±Δvs′′(t=te)=acruise=0
其中，
v
c
r
u
i
s
e
v_{cruise}
vcruise 为期望的巡航速度，
Δ
v
\Delta v
Δv 为速度采样间隔。
由于巡航状态不需要确定末状态的
S
S
S值， 所以只需要五个变量就可以了，巡航状态下的轨迹只需要四次多项式即可。
(2) 跟随状态
在本车道前方的规划空间内，存在有动态障碍物，且速度较快，无法进行超车，则本车道处于跟随状态，且与前车保持一定的安全距离，跟随前车行驶。在这里，安全距离定义为:
S
s
a
f
e
f
o
l
l
o
w
=
v
v
e
h
t
s
+
S
c
o
n
s
t
S_{safefollow} = v_{veh}t_{s} + S_{const}
Ssafefollow=vvehts+Sconst
其中，
t
s
t_{s}
ts 为安全时距，一般取值为
1.5
−
2.2
1.5 - 2.2
1.5−2.2，
S
c
o
n
s
t
S_{const}
Sconst为停车时辆车之间的安全间隔。结合实际情况，这里只考虑本车最近的动态障碍物。因此目标状态表示为
s
(
t
=
t
e
)
=
s
o
b
s
n
e
s
t
−
S
s
a
f
e
f
o
l
l
o
w
±
Δ
p
s
′
(
t
=
t
e
)
=
v
a
h
e
a
d
±
Δ
v
s
′
′
(
t
=
t
e
)
=
0
s(t=t_{e})=s_{obsnest}-S_{safefollow} \pm \Delta p \\ s'(t=t_{e})=v_{ahead} \pm \Delta v \\ s''(t=t_{e}) = 0
s(t=te)=sobsnest−Ssafefollow±Δps′(t=te)=vahead±Δvs′′(t=te)=0
其中，
s
o
b
s
n
e
s
t
s_{obsnest}
sobsnest 为距离本车最近障碍物与本车之间的距离。
(3) 超车状态
当本车前方的规划空间内，存在有静态障碍物或速度比较慢的动态障碍物时，存在可以超车的空间，回道时同样需要与后车保持安全距离，避免后车追尾，因此，目标状态可以表示为
s
(
t
=
t
e
)
=
s
o
b
s
n
e
s
t
+
S
s
a
f
e
o
v
e
r
t
a
k
e
±
Δ
p
s
′
(
t
=
t
e
)
=
v
a
h
e
a
d
s
′
′
(
t
=
t
e
)
=
0
s(t=t_{e})=s_{obsnest}+S_{safeovertake} \pm \Delta p \\ s'(t=t_{e})=v_{ahead} \\ s''(t=t_{e})=0
s(t=te)=sobsnest+Ssafeovertake±Δps′(t=te)=vaheads′′(t=te)=0
其中，
S
s
a
f
e
o
v
e
r
t
a
k
e
=
v
a
h
e
a
d
t
s
+
S
c
o
n
s
t
S_{safeovertake} = v_{ahead}t_{s} + S_{const}
Ssafeovertake=vaheadts+Sconst，
v
a
h
e
a
d
v_{ahead}
vahead为前车的速度。
当确定了起始状态和目标状态后， 带入纵向轨迹表达式中：
s
(
t
)
=
a
0
+
a
1
t
+
a
2
t
2
+
a
3
t
3
+
a
4
t
4
+
a
5
t
5
s(t) = a_{0} + a_{1}t + a_{2}t^2 + a_{3}t^3 + a_{4}t ^4 + a_{5}t ^5
s(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5
整理成矩阵形式可得:
[
t
3
t
4
t
5
3
t
2
4
t
3
5
t
4
6
t
12
t
2
20
t
3
]
[
a
3
a
4
a
5
]
=
[
s
(
t
)
−
a
0
−
a
1
t
−
a
2
t
2
s
′
(
t
)
−
a
1
−
2
a
2
t
s
′
′
(
t
)
−
2
a
2
t
]
\begin{gathered} \begin{bmatrix}t^3 & t^4 & t^5 \\ 3t^2 & 4t^3 & 5t^4 \\ 6t & 12t^2 & 20t^3\end{bmatrix} \begin{bmatrix}a_{3} \\ a_{4} \\ a_{5}\end{bmatrix} = \begin{bmatrix} s(t)-a_{0}-a_{1}t-a_{2}t^2\\ s'(t)-a_{1}-2a_{2}t \\ s''(t)-2a_{2}t\end{bmatrix} \end{gathered}
⎣⎡t33t26tt44t312t2t55t420t3⎦⎤⎣⎡a3a4a5⎦⎤=⎣⎡s(t)−a0−a1t−a2t2s′(t)−a1−2a2ts′′(t)−2a2t⎦⎤
将起始点和终止点带入上述矩阵中，解得纵向轨迹的系数
a
i
(
i
=
0
,
1
,
2
,
3
,
4
,
5
)
a_{i}(i=0,1,2,3,4,5)
ai(i=0,1,2,3,4,5)，从而确定纵向轨迹曲线。对于终点状态时间
t
e
t_{e}
te，当
t
e
t_{e}
te 太小，纵向规划的轨迹太短，无法处理极端情况，因此设置
t
e
∈
(
1
,
7
)
且
s
(
t
e
)
∈
S
a
r
e
a
t_{e} \in (1,7) 且 s(t_{e}) \in S_{area}
te∈(1,7)且s(te)∈Sarea
横向轨迹规划
事实上，横向运动是与纵向运动相互耦合的，而且横向运动是由纵向运动产生的，因此在
F
r
e
n
e
t
Frenet
Frenet 坐标空间下，将横向运动设计为
d
d
d 关于
s
s
s 的轨迹更为合适
d
(
s
)
=
b
0
+
b
1
s
+
b
2
s
2
+
b
3
s
3
+
b
4
s
3
+
b
4
s
4
d(s)=b_{0} + b_{1}s + b_{2}s^2 + b_{3}s^3 + b_{4}s^3+ b_{4}s^4
d(s)=b0+b1s+b2s2+b3s3+b4s3+b4s4
对于横向规划，希望车辆最终沿着全局最优参考路径的方向行驶，因此对于第
i
i
i 目标状态
d
i
′
=
0
d
i
′
′
=
0
d'_{i}=0 \\ d''_{i}=0
di′=0di′′=0
为了使横向规划具有避障的功能，对应每一条纵向轨迹末端，对其横向进行采样
d
(
s
i
)
j
=
d
i
n
t
e
r
v
a
l
(
j
)
，
且
d
(
s
i
)
∈
d
a
r
e
a
d(s_{i})_{j} = d_{interval}(j)，且d(s_{i}) \in d_{area}
d(si)j=dinterval(j)，且d(si)∈darea
其中，
s
i
s_{i}
si 表示第
i
i
i 条纵向轨迹，
d
i
n
t
e
r
v
a
l
d_{interval}
dinterval 为横向采样间隔集合，分布于
d
d
d 轴正负区域。当确定了横向轨迹的目标状态，类似纵向轨迹求解方法，可以得到在自适应规划空间的一系列横纵向组合轨迹簇，包含横纵向轨迹。
基于车辆约束的最优轨迹选择
根据纵向轨迹规划和横向轨迹规划，得到在轨迹空间内一系列的组合轨迹簇，如何在一些列的组合轨迹簇中，根据不同的行驶状态智能地决策最优的行驶轨迹，最重要的就是设计损失函数，通过对所有的候选轨迹簇进行损失函数值排序，损失值最低的即为当前最优的轨迹。
1. 舒适度
通常用下面公式来表示舒适性，即
j
e
r
k
jerk
jerk 越小，车辆加减速越平缓，舒适度越高。定义舒适度的损失函数为
C
c
o
m
f
o
r
t
=
∫
0
t
e
j
e
r
k
2
(
t
)
d
t
C_{comfort} = \int_0^{t_{e}} jerk^2(t) dt
Ccomfort=∫0tejerk2(t)dt
2. 规划速度损失
当速度轨迹的舒适度值相差不大，相对来讲都比较平稳时，更关注最终规划的末体速度是否可以达到期望速度，规划速度损失值定义为
C
L
o
n
o
f
f
s
e
t
=
a
b
s
(
s
′
(
t
e
)
−
v
d
e
s
i
r
e
)
/
v
d
e
s
i
r
e
C_{Lonoffset}=abs(s'(t_{e})-v_{desire})/v_{desire}
CLonoffset=abs(s′(te)−vdesire)/vdesire
其中，
v
d
e
s
i
r
e
v_{desire}
vdesire 为巡航、跟随或者超车状态下计算的期望速度。
3.横向偏移损失
对于横向轨迹，距离全局参考路径的横向偏差越小越好。因此，横向偏移损失定义为
C
L
a
n
e
o
f
f
s
e
t
=
∫
0
s
e
(
(
d
(
s
)
l
a
t
b
o
u
n
d
)
2
.
f
a
c
)
d
s
∫
0
s
e
(
d
(
s
)
∣
l
a
t
b
o
u
n
d
)
∣
.
f
a
c
)
d
s
f
a
c
=
{
10
∗
f
a
c
1
当
d
(
0
)
.
d
(
s
)
< 0
f
a
c
1
其他
l
a
t
b
o
u
n
d
=
{
5
∗
l
a
t
b
o
u
n
d
1
当
d
(
0
)
>
l
a
t
t
h
r
e
s
h
l
a
t
b
o
u
n
d
1
其他
C_{Laneoffset} = \frac{\int_0^{s_{e}}((\frac{d(s)}{latbound})^2.fac)ds}{\int_0^{s_{e}}(\frac{d(s)}{|{latbound})|}.fac)ds} \\ \\ \\ fac = \begin{cases} 10 * fac1 & \text {当 $d(0)$ . $d(s)$ < 0} \\ fac1 & \text{其他} \end{cases} \\ latbound = \begin{cases} 5 * latbound1 & \text{当$d(0) > latthresh$} \\ latbound1 & \text{其他} \end{cases} \\
CLaneoffset=∫0se(∣latbound)∣d(s).fac)ds∫0se((latboundd(s))2.fac)dsfac={10∗fac1fac1当 d(0) . d(s) < 0其他latbound={5∗latbound1latbound1当d(0)>latthresh其他
其中，
f
a
c
1
fac1
fac1 为权重系数，
l
a
t
b
o
u
n
d
1
latbound1
latbound1 为横向偏移基准值，
l
a
t
t
h
r
e
s
h
latthresh
latthresh 为横向偏移阈值。
4.横向舒适度
横向舒适度不仅与横向轨迹的平缓程度有关，同时也和其对应的纵向速度有关。定义横向舒适度函数为
C
L
a
t
e
c
o
m
f
o
r
t
=
m
a
x
(
∣
d
′
′
(
s
i
)
.
s
′
(
t
i
)
+
d
′
(
s
i
)
.
s
′
′
(
t
i
)
∣
)
满
足
t
i
∈
[
0
,
t
e
]
满
足
s
i
=
s
(
t
i
)
C_{Latecomfort} = max(|d''(s_{i}).s'(t_{i}) + d'(s_{i}).s''(t_{i})|) \\ 满足 t_{i}\in [0,t_{e}] \\ 满足 s_{i} = s(t_{i})
CLatecomfort=max(∣d′′(si).s′(ti)+d′(si).s′′(ti)∣)满足ti∈[0,te]满足si=s(ti)
5.障碍物势场
对于规划空间内的障碍物，建立横向轨迹与障碍物之间的势场函数，当轨迹靠近障碍物时，所受到的势场力增大，反之，受到的势场力减小。将其障碍物车辆膨胀为圆形区域，其半径
R
1
=
l
o
b
s
2
+
w
o
b
s
2
2
R_{1} =\frac{\sqrt{l_{obs}^2+w_{obs}^2}}{2}
R1=2lobs2+wobs2 (
l
o
b
s
l_{obs}
lobs 和
w
o
b
s
w_{obs}
wobs 为障碍物的长和宽)，该区域为不可犯区域，存在极高的碰撞风险；然后再根据车辆与障碍物之间的距离定义一个阈值
R
2
R_{2}
R2 用来划分高风险区域和低风险区域。
定义的障碍物势场函数为
C
o
b
s
=
{
∫
0
s
e
e
−
w
.
o
b
s
t
d
s
当
d
i
s
t
>
R
1
w
3
其他
C_{obs} = \begin{cases} \int_0^{s_{e}} e^{-w. obst} ds & \text{当 $dist$ > $R_{1}$} \\ w3 & \text{其他} \end{cases}
Cobs={∫0see−w.obstdsw3当 dist > R1其他
其中：
w
=
{
w
1
当
R
1
<
d
i
s
t
<
R
2
w
2
当
R
2
<
d
i
s
t
w = \begin{cases} w_{1} & \text{当 $R_{1}$ < $dist$ < $R_{2}$} \\ w_{2} & \text{当 $R_{2}$ < $dist$} \end{cases}
w={w1w2当 R1 < dist < R2当 R2 < dist
其中，
d
i
s
t
=
(
x
p
−
x
o
b
s
)
2
+
(
y
p
−
y
o
b
s
)
2
dist = \sqrt{(x_{p} - x_{obs})^2 + (y_{p} - y_{obs})^2}
dist=(xp−xobs)2+(yp−yobs)2，
(
x
p
,
y
p
)
(x_{p}, y_{p})
(xp,yp) 为轨迹的笛卡尔坐标，
(
x
o
b
s
,
y
o
b
s
)
(x_{obs}, y_{obs})
(xobs,yobs) 为障碍物的坐标，
w
1
,
w
2
,
w
3
w_{1},w_{2},w_{3}
w1,w2,w3 为惩罚系数，且
w
3
>
>
w
1
>
w
2
w_{3} >> w_{1} > w_{2}
w3>>w1>w2。
根据上述
5
5
5 中评价指标，可以对轨迹的优劣有效地进行全面评价。整体的损失函数通过公式表示
C
t
o
t
a
l
=
l
1
.
C
j
e
r
k
+
l
2
.
C
L
o
n
e
o
f
f
s
e
t
+
l
3
.
C
L
a
t
o
f
f
s
e
t
+
l
4
.
C
L
a
t
c
o
m
f
o
r
t
+
l
5
.
C
o
b
s
C_{total} = l_{1}.C_{jerk} + l_{2}.C_{Loneoffset} + l_{3}.C_{Latoffset} + l_{4}.C_{Latcomfort} + l_{5}.C_{obs}
Ctotal=l1.Cjerk+l2.CLoneoffset+l3.CLatoffset+l4.CLatcomfort+l5.Cobs
其中，
C
t
o
t
a
l
C_{total}
Ctotal 总体的损失函数，其值越小，该条横纵向组合轨迹的性能越好。
l
i
(
i
=
1
,
.
.
.
,
5
)
l_{i}(i = 1,...,5)
li(i=1,...,5)
表示每个性能指标所占的权重系数，对每个指标的侧重点不同，会导致不同的规划结果，根据上述公式可以对组合后的轨迹簇中筛选出最优的轨迹。
5801
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
