Planning-Apollo路径决策规划及问题_apollo路径dp-优快云博客

本文链接：https://blog.youkuaiyun.com/mpt0816/article/details/121518174

本文解析了Apollo中采用的优化模型，通过piecewise-jerk方法进行路径和速度规划，详细介绍了优化目标、约束条件及其实现细节。探讨了原理中的问题与代码实现中的不足，并提出了车辆模型改进方案。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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$ 的距离进行离散化，每个采样点的状态有 $li,li′,li′′l_{i},l^{\prime}_{i},l^{\prime \prime}_{i}$ ，按照 $li→i+1′′′l^{\prime \prime \prime}_{i \rightarrow i+1}$ 的冲击度运动到状态 $li+1,li+1′,li+1′′l_{i+1},l^{\prime}_{i+1},l^{\prime \prime}_{i+1}$ 。 $li,li′,li′′l_{i},l^{\prime}_{i},l^{\prime \prime}_{i}$ 是优化问题的决策变量。

在这里插入图片描述

$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′′′l^{\prime \prime \prime}_{i \rightarrow i+1}$ 是常量，因此可以通过积分，根据采样点 $i$ 的值可以得到采样点 $i + 1$ 的值：
$\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}$

1.2 优化目标

$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}$

1.3 约束条件

由于在Frenet坐标系中会丢失道路的曲率信息，不能对车辆形成运动学约束，因此需要计算车辆在运动过程中的曲率，避免超过车辆的运动极限能力。
$\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}$
其中， $κr\kappa_r$ 和 $κr˙\dot{\kappa_r}$ 是参考线在点 $p_r$ 处的曲率和曲率变化率， $Δθ\Delta \theta$ 是车辆和参考线点 $p_r$ 处切线方向的角度差。显然上式过于复杂，对其进行简化：

假设车辆几乎在沿着道路方向行驶，因此 $Δθ=0\Delta \theta = 0$ ；
“横向加速度” $l′′l^{\prime \prime}$ 是很小的，数量级在 $10^{-2}$ ，因此 $l′′=0l^{\prime \prime}=0$ ；

基于上述假设：
$\kappa \approx \frac{\kappa_r}{1-\kappa_r l} \tag{1-5}$
根据车辆的阿克曼转向特性：
$\frac{\kappa_r}{1-\kappa_r l} \leq \frac{tan(\delta_{max})}{L} \tag{1-6}$
整理后得到：
$tan(\delta_{max}) \times \kappa_r \times l - tan(\delta_{max}) + \kappa_r \times L \leq 0 \tag{1-7}$
同时各个决策变量需要满足上下边界约束：
$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}$
因此，路径优化问题是由优化目标 $(1 - 3)$ ，等式约束 $(1 - 2)$ 和不等式约束 $(1 - 7) (1 - 8)$ 构成。

2. 代码实现

在Apollo中路径规划的实现流程如下：

在这里插入图片描述

2.1 LaneChangeDecider

换道决策决定ADC是否进行换道。目前Apollo的体系是当有多条参考线时即进行换道。

如果不换道，在PathBoundsDecider中会将 $l$ 的边界限制在本车道内(如果不借道)；
如果换道，在PathBoundsDecider中会将 $l$ 的边界向目标车道一侧进行拓展；

2.2 PathLaneBorrowDecider

ADC在借道工况中：判断本车道可通过性，如果在连续 $n$ (参数配置)帧规划中本车道可以通行，则取消借道。

ADC不在借道工况中：ADC需要同时满足以下条件才可以进入借道工况：

必须只有一条参考线；
规划起点的速度不能过高(参数配置)；
不能在SIGNAL、STOP_SIGN 和Junction附近；
不能在终点附近；
Block Obstacle在ADC一定范围内，并且堵塞原因不是Traffic Flow；
地图车道线线型（虚线）允许借道；

如果借道，在PathBoundsDecider中会将 $l$ 的边界借道方向一侧进行拓展。

2.3 PathBoundsDecider

PathBoundsDecider会根据换道决策和借道决策生成相应的 $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}$ ，公式 $(1 - 2)$ 的物理含义是否合适有待商榷；
不能显式的处理航向角约束；
dl,ddl的上下限约束必须包括零点，即下限必须小于零，上限必须大于零，否则会造成primal infeasible求解失败；
只是针对车辆质点(后轴中心)的建模，对于大型车辆的路径规划可能存在问题。例如当道路上不存在障碍物时，按照其规划方法，车辆后轴或者前轴必然在道路中心行驶，会使大型车辆超出道路边界，而人类驾驶大型车辆在弯道行驶时并不会沿着道路中心线。

在这里插入图片描述

3.2 代码实现方面

代码中没有实现公式 $(1 - 7)$ 的车辆行驶的曲率约束；

$l′′l^{\prime \prime}$ 的约束处理不正确，代码错误地将 $l′′l^{\prime \prime}$ 和曲率做了等价处理：

// 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$ 轴的直线，将该直线与圆盘的两个交点记为 $A$ 、 $B$ 。如果每一个圆形的两端交点 $A$ 、 $B$ 均与隧道左右边界不相撞，则整个车身一定不会发生碰撞。需要强调的是，上述结论仅在圆盘个数为无穷大时成立，并且这样会在车头车尾处增加半圆形冗余区域。可以建立避障约束：
$\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}$
在求解过程中需要对 $η\eta$ 离散化，显然 $Q P$ 问题中不可能包含无穷数目约束条件，可以在 $[−Lrcos⁡θ,(Lf+Lw)cos⁡θ][-L_r \cos \theta, (L_f + L_w)\cos \theta]$ 区间上均匀采样 $N_{sample}+1)$ 个采样点 ${ηk}\{ \eta_k \}$ 来表征连续变量 $η\eta$ ，从而构成一下离散约束：
$\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}$
可以将 $\theta$ 替换为 $l′l^{\prime}$ ，显然上述不等式为非线性约束。不等式左侧的采样点 $ηk\eta_k$ 可能取值不是常数，这是因为 $ηk\eta_k$ 是在与 $cosθcos\theta$ 有关的区间上采样，而 $cosθcos\theta$ 和 $l′l^{\prime}$ 相关，因此采样点 $ηk\eta_k$ 可能的取值区间长度是与 $l′l^{\prime}$ 有关的变量，类似的情况也出现在不等式的右侧。可以将采样点的数目确定下来从而完成线性化。为了使 $[−Lrcos⁡θ,(Lf+Lw)cos⁡θ][-L_r \cos \theta, (L_f + L_w)\cos \theta]$ 与变量 $l′l^{\prime}$ 解耦，可以利用 $cos⁡θ≤1\cos\theta \leq 1$ 条件将其放宽至固定长度的区间 $L_r, L_f + L_w]$ 。放宽采样点取值区间会使车辆行驶行为更加保守，但考虑到结构化道路上的车辆姿态角一般是不会显著偏离参考线的，因此假设是合理的。