Planning-Apollo路径决策规划及问题

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进行横向运动。沿着参考线在道路前进方向上按照$\Delta s$的距离进行离散化，每个采样点的状态有$l_i,l_i^{\prime },l_i^{\prime \prime }$，按照$l_{i\to i+1}^{\prime \prime \prime }$ 的冲击度运动到状态$l_{i+1},l_{i+1}^{\prime },l_{i+1}^{\prime \prime }$。$l_i,l_i^{\prime },l_i^{\prime \prime }$是优化问题的决策变量。

$$l_{i\to i+1}^{\prime \prime \prime }=\frac{l_{i+1}^{\prime \prime }-l_i^{\prime \prime }}{\Delta s}\text{\hspace{1em}(1-1)}$$
由于相邻采样点之间的$l_{i\to i+1}^{\prime \prime \prime }$是常量，因此可以通过积分，根据采样点$i$的值可以得到采样点$i+1$的值：
$$\begin{array}{rl}{l}_{i+1}^{\prime \prime }& =l_i^{\prime \prime }+{\int }_0^{\Delta s}{l}_{i\to i+1}^{\prime \prime \prime }ds=l_i^{\prime \prime }+l_{i\to i+1}^{\prime \prime \prime }\times \Delta s\\ l_{i+1}^{\prime }& =l_i^{\prime }+{\int }_0^{\Delta s}{l}_{i\to i+1}^{\prime \prime }ds=l_i^{\prime }+l_i^{\prime \prime }\times \Delta s+\frac{1}{2}{l}_{i\to i+1}^{\prime \prime \prime }\times {\Delta s}^2\\ l_{i+1}& =l_i+{\int }_0^{\Delta s}{l}_{i\to i+1}^{\prime }ds=l_i+l_i^{\prime }\times \Delta s+\frac{1}{2}{l}_i^{\prime \prime }\times {\Delta s}^2+\frac{1}{6}{l}_{i\to i+1}^{\prime \prime \prime }\times {\Delta s}^3\end{array}\text{\hspace{1em}(1-2)}$$

1.2 优化目标

$$J=w_l\sum _0^{n-1}{l}_i^2+w_{l^{\prime }}\sum _0^{n-1}{l}_i^{\prime 2}+w_{l^{\prime \prime }}\sum _0^{n-1}{l}_i^{\prime \prime 2}+w_{l^{\prime \prime \prime }}\sum _0^{n-1}(\frac{l_{i+1}^{\prime \prime }-l_i^{\prime \prime }}{\Delta s}{)}^2+w_{obs}\sum _0^{n-1}(l_i-0.5\times (l_i^{min}+l_i^{max}){)}^2\text{\hspace{1em}(1-3)}$$

1.3 约束条件

由于在Frenet坐标系中会丢失道路的曲率信息，不能对车辆形成运动学约束，因此需要计算车辆在运动过程中的曲率，避免超过车辆的运动极限能力。
$$\kappa =\frac{(\frac{((l^{\prime \prime }+(\dot{{\kappa }_r}l+{\kappa }_r{l}^{\prime }))tan\Delta \theta )co{s}^2\Delta \theta }{1-{\kappa }_{r}l}+{\kappa }_r)cos\Delta \theta }{1-{\kappa }_{r}l}\text{\hspace{1em}(1-4)}$$
其中，${\kappa }_r$和$\dot{{\kappa }_r}$是参考线在点$p_r$处的曲率和曲率变化率，$\Delta \theta$是车辆和参考线点$p_r$处切线方向的角度差。显然上式过于复杂，对其进行简化：

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

基于上述假设：
$$\kappa \approx \frac{{\kappa }_r}{1-{\kappa }_{r}l}\text{\hspace{1em}(1-5)}$$
根据车辆的阿克曼转向特性：
$$\frac{{\kappa }_r}{1-{\kappa }_{r}l}\le \frac{tan({\delta }_{max})}{L}\text{\hspace{1em}(1-6)}$$
整理后得到：
$$tan({\delta }_{max})\times {\kappa }_r\times l-tan({\delta }_{max})+{\kappa }_r\times L\le 0\text{\hspace{1em}(1-7)}$$
同时各个决策变量需要满足上下边界约束：
$$\begin{gathered} l_{min}\le l_i\le l_{max} \\ {l}_{min}^{\prime }\le l_i^{\prime }\le l_{max}^{\prime } \\ {l}_{min}^{\prime \prime }\le l_i^{\prime \prime }\le l_{max}^{\prime \prime } \\ {l}_{min}^{\prime \prime \prime }\le l_i^{\prime \prime \prime }\le l_{max}^{\prime \prime \prime }\text{\hspace{1em}(1-8)} \end{gathered}$$
因此，路径优化问题是由优化目标$(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^{\prime },l^{\prime \prime },l^{\prime \prime \prime }$，公式$(1-2)$的物理含义是否合适有待商榷；
• 不能显式的处理航向角约束；
• dl,ddl的上下限约束必须包括零点，即下限必须小于零，上限必须大于零，否则会造成primal infeasible求解失败；
• 只是针对车辆质点(后轴中心)的建模，对于大型车辆的路径规划可能存在问题。例如当道路上不存在障碍物时，按照其规划方法，车辆后轴或者前轴必然在道路中心行驶，会使大型车辆超出道路边界，而人类驾驶大型车辆在弯道行驶时并不会沿着道路中心线。

3.2 代码实现方面

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

• $l^{\prime \prime }$的约束处理不正确，代码错误地将$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> ddl_bounds;
      for (size_t i = 0; i < path_boundary.boundary().size(); ++i) {
        double s = static_cast(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$均与隧道左右边界不相撞，则整个车身一定不会发生碰撞。需要强调的是，上述结论仅在圆盘个数为无穷大时成立，并且这样会在车头车尾处增加半圆形冗余区域。可以建立避障约束：
$$\begin{gathered} \eta \cdot \tan \theta +l(s)+\frac{L_b}{2}\le ub(s+\eta ) \\ \eta \cdot \tan \theta +l(s)-\frac{L_b}{2}\ge lb(s+\eta ) \\ \forall \eta \in [-L_r\cos \theta ,(L_f+L_w)\cos \theta ]\text{\hspace{1em}(4-1)} \end{gathered}$$
在求解过程中需要对$\eta$离散化，显然$QP$问题中不可能包含无穷数目约束条件，可以在$[-L_r\cos \theta ,(L_f+L_w)\cos \theta ]$区间上均匀采样$(N_{sample}+1)$个采样点$\{{\eta }_k\}$来表征连续变量$\eta$，从而构成一下离散约束：
$$\begin{gathered} {\eta }_k\cdot \tan {\theta }_i+l(s_i)+\frac{L_b}{2}\le ub(s_i+{\eta }_k) \\ {\eta }_k\cdot \tan {\theta }_i+l(s_i)-\frac{L_b}{2}\ge 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}\text{\hspace{1em}(4-2)} \end{gathered}$$
可以将$tan\theta$替换为$l^{\prime }$，显然上述不等式为非线性约束。不等式左侧的采样点${\eta }_k$可能取值不是常数，这是因为${\eta }_k$是在与$cos\theta$有关的区间上采样，而$cos\theta$和$l^{\prime }$相关，因此采样点${\eta }_k$可能的取值区间长度是与$l^{\prime }$有关的变量，类似的情况也出现在不等式的右侧。可以将采样点的数目确定下来从而完成线性化。为了使$[-L_r\cos \theta ,(L_f+L_w)\cos \theta ]$与变量$l^{\prime }$解耦，可以利用$\cos \theta \le 1$条件将其放宽至固定长度的区间$[-L_r,L_f+L_w]$。放宽采样点取值区间会使车辆行驶行为更加保守，但考虑到结构化道路上的车辆姿态角一般是不会显著偏离参考线的，因此假设是合理的。

但需要注意的是$\theta$是车辆航向与参考线航向的偏差，由于没有利用到参考线的曲率信息，当车辆后轴中心完全沿着参考线行驶，即$l=0,l^{\prime }=0,l^{\prime \prime }=0$，当车辆尺寸较大时仍会出现$3.1$中的问题4情况。

5 参考

1. Apollo代码：https://github.com/ApolloAuto/apollo
2. 李柏, 张友民, 彭晓燕, 欧阳亚坤, 孔旗. 自动驾驶决策规划技术理论与实践. 北京：铁道出版社. ISBN：9787113282523. 2021.11.