Apollo
中采用路径规划和速度规划解耦的方法,由EM Planner
演变而来,路径规划是基于参考线的规划,放弃了EM Planner
中的路径决策DP
过程。
1. 算法原理
百度已经将算法原理发表在《Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform》
中。基于参考线将规划问题解耦为SL
坐标系中的路径规划和ST
坐标系中的速度规划。
1.1 优化模型
Apollo
采用了piecewise-jerk
的方法,即分段冲击度,ADC
在每两个采样点之间以恒定的jerk
进行横向运动。沿着参考线在道路前进方向上按照
Δ
s
\Delta s
Δs的距离进行离散化,每个采样点的状态有
l
i
,
l
i
′
,
l
i
′
′
l_{i},l^{\prime}_{i},l^{\prime \prime}_{i}
li,li′,li′′,按照
l
i
→
i
+
1
′
′
′
l^{\prime \prime \prime}_{i \rightarrow i+1}
li→i+1′′′ 的冲击度运动到状态
l
i
+
1
,
l
i
+
1
′
,
l
i
+
1
′
′
l_{i+1},l^{\prime}_{i+1},l^{\prime \prime}_{i+1}
li+1,li+1′,li+1′′。
l
i
,
l
i
′
,
l
i
′
′
l_{i},l^{\prime}_{i},l^{\prime \prime}_{i}
li,li′,li′′是优化问题的决策变量。
l
i
→
i
+
1
′
′
′
=
l
i
+
1
′
′
−
l
i
′
′
Δ
s
(1-1)
l^{\prime \prime \prime}_{i \rightarrow i+1} = \frac{l^{\prime \prime}_{i+1} - l^{\prime \prime}_{i}}{\Delta s} \tag{1-1}
li→i+1′′′=Δsli+1′′−li′′(1-1)
由于相邻采样点之间的
l
i
→
i
+
1
′
′
′
l^{\prime \prime \prime}_{i \rightarrow i+1}
li→i+1′′′是常量,因此可以通过积分,根据采样点
i
i
i的值可以得到采样点
i
+
1
i+1
i+1的值:
l
i
+
1
′
′
=
l
i
′
′
+
∫
0
Δ
s
l
i
→
i
+
1
′
′
′
d
s
=
l
i
′
′
+
l
i
→
i
+
1
′
′
′
×
Δ
s
l
i
+
1
′
=
l
i
′
+
∫
0
Δ
s
l
i
→
i
+
1
′
′
d
s
=
l
i
′
+
l
i
′
′
×
Δ
s
+
1
2
l
i
→
i
+
1
′
′
′
×
Δ
s
2
l
i
+
1
=
l
i
+
∫
0
Δ
s
l
i
→
i
+
1
′
d
s
=
l
i
+
l
i
′
×
Δ
s
+
1
2
l
i
′
′
×
Δ
s
2
+
1
6
l
i
→
i
+
1
′
′
′
×
Δ
s
3
(1-2)
\begin{aligned} l^{\prime \prime}_{i+1} &= l^{\prime \prime}_{i} + \int^{\Delta s}_{0} {l^{\prime \prime \prime}_{i \rightarrow i+1}} ds = l^{\prime \prime}_{i} + l^{\prime \prime \prime}_{i \rightarrow i+1} \times \Delta s \\ l^{\prime}_{i+1} &= l^{\prime}_{i} + \int^{\Delta s}_{0} {l^{ \prime \prime}_{i \rightarrow i+1}} ds = l^{\prime}_{i} + l^{\prime \prime}_{i} \times \Delta s + \frac{1}{2} l^{\prime \prime \prime}_{i \rightarrow i+1} \times {\Delta s}^2 \\ l_{i+1} &= l_{i} + \int^{\Delta s}_{0} {l^{ \prime}_{i \rightarrow i+1}} ds = l_{i} + l^{\prime}_{i} \times \Delta s + \frac{1}{2} l^{\prime \prime}_{i} \times {\Delta s}^2 + \frac{1}{6} l^{\prime \prime \prime}_{i \rightarrow i+1} \times {\Delta s}^3 \end{aligned} \tag{1-2}
li+1′′li+1′li+1=li′′+∫0Δsli→i+1′′′ds=li′′+li→i+1′′′×Δs=li′+∫0Δsli→i+1′′ds=li′+li′′×Δs+21li→i+1′′′×Δs2=li+∫0Δsli→i+1′ds=li+li′×Δs+21li′′×Δs2+61li→i+1′′′×Δs3(1-2)
1.2 优化目标
J = w l ∑ 0 n − 1 l i 2 + w l ′ ∑ 0 n − 1 l i ′ 2 + w l ′ ′ ∑ 0 n − 1 l i ′ ′ 2 + w l ′ ′ ′ ∑ 0 n − 1 ( l i + 1 ′ ′ − l i ′ ′ Δ s ) 2 + w o b s ∑ 0 n − 1 ( l i − 0.5 × ( l i m i n + l i m a x ) ) 2 (1-3) J = w_l \sum^{n-1}_{0} {l^{2}_{i}} + w_{l^{\prime}} \sum^{n-1}_{0} {l^{\prime 2}_{i}} + w_{l^{\prime \prime}} \sum^{n-1}_{0} {l^{\prime \prime 2}_{i}} + w_{l^{\prime \prime \prime}} \sum^{n-1}_{0} ({\frac{l^{\prime \prime}_{i+1} - l^{\prime \prime}_{i}}{\Delta s}})^2 + w_{obs} \sum^{n-1}_{0} ({l_{i}} - 0.5 \times (l^{min}_{i} + l^{max}_{i}))^2 \tag{1-3} J=wl0∑n−1li2+wl′0∑n−1li′2+wl′′0∑n−1li′′2+wl′′′0∑n−1(Δsli+1′′−li′′)2+wobs0∑n−1(li−0.5×(limin+limax))2(1-3)
1.3 约束条件
由于在Frenet
坐标系中会丢失道路的曲率信息,不能对车辆形成运动学约束,因此需要计算车辆在运动过程中的曲率,避免超过车辆的运动极限能力。
κ
=
(
(
(
l
′
′
+
(
κ
r
˙
l
+
κ
r
l
′
)
)
t
a
n
Δ
θ
)
c
o
s
2
Δ
θ
1
−
κ
r
l
+
κ
r
)
c
o
s
Δ
θ
1
−
κ
r
l
(1-4)
\kappa = \frac{(\frac{((l^{\prime \prime} + (\dot{{\kappa}_{r}}l + \kappa_r l^{\prime}) )tan \Delta \theta)cos^2 \Delta \theta}{1-\kappa_r l}+\kappa_r){cos \Delta \theta}}{1-\kappa_r l} \tag{1-4}
κ=1−κrl(1−κrl((l′′+(κr˙l+κrl′))tanΔθ)cos2Δθ+κr)cosΔθ(1-4)
其中,
κ
r
\kappa_r
κr和
κ
r
˙
\dot{\kappa_r}
κr˙是参考线在点
p
r
p_r
pr处的曲率和曲率变化率,
Δ
θ
\Delta \theta
Δθ是车辆和参考线点
p
r
p_r
pr处切线方向的角度差。显然上式过于复杂,对其进行简化:
- 假设车辆几乎在沿着道路方向行驶,因此 Δ θ = 0 \Delta \theta = 0 Δθ=0;
- “横向加速度” l ′ ′ l^{\prime \prime} l′′是很小的,数量级在 1 0 − 2 10^{-2} 10−2,因此 l ′ ′ = 0 l^{\prime \prime}=0 l′′=0;
基于上述假设:
κ
≈
κ
r
1
−
κ
r
l
(1-5)
\kappa \approx \frac{\kappa_r}{1-\kappa_r l} \tag{1-5}
κ≈1−κrlκr(1-5)
根据车辆的阿克曼转向特性:
κ
r
1
−
κ
r
l
≤
t
a
n
(
δ
m
a
x
)
L
(1-6)
\frac{\kappa_r}{1-\kappa_r l} \leq \frac{tan(\delta_{max})}{L} \tag{1-6}
1−κrlκr≤Ltan(δmax)(1-6)
整理后得到:
t
a
n
(
δ
m
a
x
)
×
κ
r
×
l
−
t
a
n
(
δ
m
a
x
)
+
κ
r
×
L
≤
0
(1-7)
tan(\delta_{max}) \times \kappa_r \times l - tan(\delta_{max}) + \kappa_r \times L \leq 0 \tag{1-7}
tan(δmax)×κr×l−tan(δmax)+κr×L≤0(1-7)
同时各个决策变量需要满足上下边界约束:
l
m
i
n
≤
l
i
≤
l
m
a
x
l
m
i
n
′
≤
l
i
′
≤
l
m
a
x
′
l
m
i
n
′
′
≤
l
i
′
′
≤
l
m
a
x
′
′
l
m
i
n
′
′
′
≤
l
i
′
′
′
≤
l
m
a
x
′
′
′
(1-8)
l_{min} \leq l_i \leq l_{max} \\ l^{\prime}_{min} \leq l^{\prime}_i \leq l^{\prime}_{max} \\ l^{\prime \prime}_{min} \leq l^{\prime \prime}_i \leq l^{\prime \prime}_{max} \\ l^{\prime \prime \prime}_{min} \leq l^{\prime \prime \prime}_i \leq l^{\prime \prime \prime}_{max} \tag{1-8}
lmin≤li≤lmaxlmin′≤li′≤lmax′lmin′′≤li′′≤lmax′′lmin′′′≤li′′′≤lmax′′′(1-8)
因此,路径优化问题是由优化目标
(
1
−
3
)
(1-3)
(1−3),等式约束
(
1
−
2
)
(1-2)
(1−2)和不等式约束
(
1
−
7
)
(
1
−
8
)
(1-7)(1-8)
(1−7)(1−8)构成。
2. 代码实现
在Apollo
中路径规划的实现流程如下:
2.1 LaneChangeDecider
换道决策决定ADC
是否进行换道。目前Apollo
的体系是当有多条参考线时即进行换道。
- 如果不换道,在
PathBoundsDecider
中会将 l l l的边界限制在本车道内(如果不借道); - 如果换道,在
PathBoundsDecider
中会将 l l l的边界向目标车道一侧进行拓展;
2.2 PathLaneBorrowDecider
ADC
在借道工况中:判断本车道可通过性,如果在连续
n
n
n(参数配置)帧规划中本车道可以通行,则取消借道。
ADC
不在借道工况中:ADC
需要同时满足以下条件才可以进入借道工况:
- 必须只有一条参考线;
- 规划起点的速度不能过高(参数配置);
- 不能在
SIGNAL
、STOP_SIGN
和Junction
附近; - 不能在终点附近;
Block Obstacle
在ADC
一定范围内,并且堵塞原因不是Traffic Flow
;- 地图车道线线型(虚线)允许借道;
如果借道,在PathBoundsDecider
中会将
l
l
l的边界借道方向一侧进行拓展。
2.3 PathBoundsDecider
PathBoundsDecider
会根据换道决策和借道决策生成相应的
l
l
l的边界。
FallbackBound
+PullOverBound
;FallbackBound
+LaneChangeBound
;FallbackBound
+NoBorrow/LeftBorrow/RightBorrow
;
不管在何种决策下,PathBoundsDecider
都会生成一条FallbackBound
,其与NoBorrow
的区别是,不会删除Block Obstacle
后道路边界。
2.4 PiecewiseJerkPathOptimizer
会针对PathBoundsDecider
生成的每一条Bound
进行路径优化。
2.5 PathAssessmentDecider
PathAssessmentDecider
会依据设计好的规则筛选处最终的path
,并在规划路径上的采样点添加标签(IN_LANE
、OUT_ON_FORWARD_LANE
、OUT_ON_REVERSE_LANE
等),作为路径筛选的依据,并为速度规划提供限制。
路径筛选的规则是:
- 不能偏离参考线和
Road
太远; - 不能和
Static Obstacle
相撞; - 不能停止在对向车道上;
- 选择优先级最高的
Path
,排序规则:Regular path
优先于fallback path
;- 如果两条路径至少有一条是
self_lane
,并且两条路径长度的差大于15m
,选择路径长的; - 如果两条路径至少有一条是
self_lane
,并且两条路径长度的差小于5m
,是self_lane
的; - 如果两条路径都不是
self_lane
,并且两条路径长度的差大于25m
,选择路径长的; - 选择占据反向车道更少的路径;
- 如果有
block obstacle
,选择占据空间少的方向的路径; - 如果没有
block obstacle
,选择ADC
靠近方向的路径,使车辆少打方向盘; - 选择返回本车道更早的路径;
- 在上述情况无法区分的情况下选择左侧的路径;
2.6 PathDecider
遍历每个障碍物, 根据规则判断前面优化并筛选出来的path
生成对应的decisions
(GNORE
, STOP
, LEFT NUDGE
, RIGHT NUDGE
等)。
-
对以有
IGNORE/STOP/KEEP_CLEAR
决策的obstacle
不做处理; -
如果是
block obstacle
,并且不是借道工况,设为STOP
决策; -
不在
path
纵向范围内的障碍物设为IGNORE
决策; -
对于碰撞的
obstacle
,设为STOP
决策; -
根据位置关系设置
LEFT NUDGE
或者RIGHT NUDGE
的决策;
3. 问题
3.1 原理方面
- 由于是使用
sl
坐标系,dl,ddl,dddl
应该是 l ′ , l ′ ′ , l ′ ′ ′ l^{\prime},l^{\prime \prime},l^{\prime \prime \prime} l′,l′′,l′′′,公式 ( 1 − 2 ) (1-2) (1−2)的物理含义是否合适有待商榷; - 不能显式的处理航向角约束;
dl,ddl
的上下限约束必须包括零点,即下限必须小于零,上限必须大于零,否则会造成primal infeasible
求解失败;- 只是针对车辆质点(后轴中心)的建模,对于大型车辆的路径规划可能存在问题。例如当道路上不存在障碍物时,按照其规划方法,车辆后轴或者前轴必然在道路中心行驶,会使大型车辆超出道路边界,而人类驾驶大型车辆在弯道行驶时并不会沿着道路中心线。
3.2 代码实现方面
-
代码中没有实现公式 ( 1 − 7 ) (1-7) (1−7)的车辆行驶的曲率约束;
-
l ′ ′ l^{\prime \prime} l′′的约束处理不正确,代码错误地将 l ′ ′ l^{\prime \prime} l′′和曲率做了等价处理:
// piecewise_jerk_path_optimizer.cc const auto& veh_param = common::VehicleConfigHelper::GetConfig().vehicle_param(); const double lat_acc_bound = std::tan(veh_param.max_steer_angle() / veh_param.steer_ratio()) / veh_param.wheel_base(); std::vector<std::pair<double, double>> ddl_bounds; for (size_t i = 0; i < path_boundary.boundary().size(); ++i) { double s = static_cast<double>(i) * path_boundary.delta_s() + path_boundary.start_s(); double kappa = reference_line.GetNearestReferencePoint(s).kappa(); ddl_bounds.emplace_back(-lat_acc_bound - kappa, lat_acc_bound - kappa); }
4 算法改进
由于Apollo
采用的单质点模型,可以对车辆模型进行修改。如下图所示,可以使用无穷多个圆盘覆盖车身,这些圆盘的圆心致密的覆盖车体纵轴,其直径均为车宽。
绘制一条经过圆盘圆心且垂直于
s
s
s轴的直线,将该直线与圆盘的两个交点记为
A
A
A、
B
B
B。如果每一个圆形的两端交点
A
A
A、
B
B
B均与隧道左右边界不相撞,则整个车身一定不会发生碰撞。需要强调的是,上述结论仅在圆盘个数为无穷大时成立,并且这样会在车头车尾处增加半圆形冗余区域。可以建立避障约束:
η
⋅
tan
θ
+
l
(
s
)
+
L
b
2
≤
u
b
(
s
+
η
)
η
⋅
tan
θ
+
l
(
s
)
−
L
b
2
≥
l
b
(
s
+
η
)
∀
η
∈
[
−
L
r
cos
θ
,
(
L
f
+
L
w
)
cos
θ
]
(4-1)
\eta \cdot \tan \theta + l(s) + \frac{L_b}{2} \leq ub(s+\eta) \\ \eta \cdot \tan \theta + l(s) - \frac{L_b}{2} \geq lb(s+\eta) \\ \forall \eta \in [-L_r \cos \theta, (L_f + L_w)\cos \theta] \tag{4-1}
η⋅tanθ+l(s)+2Lb≤ub(s+η)η⋅tanθ+l(s)−2Lb≥lb(s+η)∀η∈[−Lrcosθ,(Lf+Lw)cosθ](4-1)
在求解过程中需要对
η
\eta
η离散化,显然
Q
P
QP
QP问题中不可能包含无穷数目约束条件,可以在
[
−
L
r
cos
θ
,
(
L
f
+
L
w
)
cos
θ
]
[-L_r \cos \theta, (L_f + L_w)\cos \theta]
[−Lrcosθ,(Lf+Lw)cosθ]区间上均匀采样
(
N
s
a
m
p
l
e
+
1
)
(N_{sample}+1)
(Nsample+1)个采样点
{
η
k
}
\{ \eta_k \}
{ηk}来表征连续变量
η
\eta
η,从而构成一下离散约束:
η
k
⋅
tan
θ
i
+
l
(
s
i
)
+
L
b
2
≤
u
b
(
s
i
+
η
k
)
η
k
⋅
tan
θ
i
+
l
(
s
i
)
−
L
b
2
≥
l
b
(
s
i
+
η
k
)
η
k
=
−
L
r
cos
θ
i
+
(
L
r
+
L
f
+
L
w
)
cos
θ
i
N
s
a
m
p
l
e
⋅
k
,
k
=
0
,
1
,
⋯
,
N
s
a
m
p
l
e
(4-2)
\eta_k \cdot \tan \theta_i + l(s_i) + \frac{L_b}{2} \leq ub(s_i+\eta_k) \\ \eta_k \cdot \tan \theta_i + l(s_i) - \frac{L_b}{2} \geq lb(s_i+\eta_k) \\ \eta_k = -L_r \cos \theta_i + \frac{(L_r + L_f + L_w)\cos \theta_i}{N_{sample}} \cdot k,k=0,1,\cdots,N_{sample} \tag{4-2}
ηk⋅tanθi+l(si)+2Lb≤ub(si+ηk)ηk⋅tanθi+l(si)−2Lb≥lb(si+ηk)ηk=−Lrcosθi+Nsample(Lr+Lf+Lw)cosθi⋅k,k=0,1,⋯,Nsample(4-2)
可以将
t
a
n
θ
tan \theta
tanθ替换为
l
′
l^{\prime}
l′,显然上述不等式为非线性约束。不等式左侧的采样点
η
k
\eta_k
ηk可能取值不是常数,这是因为
η
k
\eta_k
ηk是在与
c
o
s
θ
cos\theta
cosθ有关的区间上采样,而
c
o
s
θ
cos\theta
cosθ和
l
′
l^{\prime}
l′相关,因此采样点
η
k
\eta_k
ηk可能的取值区间长度是与
l
′
l^{\prime}
l′有关的变量,类似的情况也出现在不等式的右侧。可以将采样点的数目确定下来从而完成线性化。为了使
[
−
L
r
cos
θ
,
(
L
f
+
L
w
)
cos
θ
]
[-L_r \cos \theta, (L_f + L_w)\cos \theta]
[−Lrcosθ,(Lf+Lw)cosθ]与变量
l
′
l^{\prime}
l′解耦,可以利用
cos
θ
≤
1
\cos\theta \leq 1
cosθ≤1条件将其放宽至固定长度的区间
[
−
L
r
,
L
f
+
L
w
]
[-L_r, L_f + L_w]
[−Lr,Lf+Lw]。放宽采样点取值区间会使车辆行驶行为更加保守,但考虑到结构化道路上的车辆姿态角一般是不会显著偏离参考线的,因此假设是合理的。
但需要注意的是 θ \theta θ是车辆航向与参考线航向的偏差,由于没有利用到参考线的曲率信息,当车辆后轴中心完全沿着参考线行驶,即 l = 0 , l ′ = 0 , l ′ ′ = 0 l=0,l^{\prime}=0,l^{\prime \prime}=0 l=0,l′=0,l′′=0,当车辆尺寸较大时仍会出现 3.1 3.1 3.1中的问题4情况。
5 参考
- Apollo代码:https://github.com/ApolloAuto/apollo
- 李柏, 张友民, 彭晓燕, 欧阳亚坤, 孔旗. 自动驾驶决策规划技术理论与实践. 北京:铁道出版社. ISBN:9787113282523. 2021.11.