Baidu Apollo代码解析之轨迹规划中的轨迹评估代价函数

大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

---

Baidu Apollo中包含了2种轨迹规划方法：Lattice Planner 和 EM Planner。其中，Lattice主要是采样和剪枝的思想，EM主要是优化的思想。二者的目标都是求取代价最小的路径，那么，代价函数设计的好坏，就至关重要了。

Lattice Planner

Lattice Planner主要设计了6个代价函数（3.5版本），考虑了到达目标、平滑、避免碰撞、向心加速度、横向偏移、舒适性等因素。

• 纵向，到达设定速度、行驶距离

 

上式中，T是指轨迹时间长度，后面会有我对这个量的设置的理解。dist(T)是在指定时间内行驶的距离，也就是轨迹的长度。

• 纵向，平滑

• 纵向，避免碰撞

上式中，dist指自车纵向位置与考虑范围内的障碍物所占据路段之间的距离。

• 向心加速度

• 横向，偏移

上式中，offset(s)表示横向的偏移是由纵向的移动造成的，即offset是s的函数，w是反应offset(s)与初始横向偏移是同向还是反向的比例因子。

• 横向，舒适性

上式中，d(s)表示轨迹横向分量函数，等同于上面的offset(s)，s(t)表示轨迹纵向分量函数。

综上，一条轨迹的代价就是以上6项代价乘以相应的预先设定的权重。

关于上述中时长T的想法

• Apollo在上述部分中用到了很多次对于时间段t=0 ~ t=T内的状态遍历。那么T到底是多少呢？Apollo中绝大部分都是直接设置T = trajectory_time_length (8s)。我认为不对，应该T = curve.ParamLength。

• 有关curve的定义请读者详看Apollo中Curve1d，CubicPolynomialCurve1d，QuarticPolynomialCurve1d，QuinticPolynomialCurve1d等类的定义。curve类中有ParamLength的成员变量，保存了曲线方程的定义域终点。

• Lattice Planner 大体有以下几个步骤：采样，生成横纵向曲线方程，代价评估，合并横纵向曲线，将合并后的曲线离散化为轨迹点。

• 针对纵向轨迹的不同场景，比如cruise，end_condition_sampler 中有对时间的采样，如果终点采样时间为n，则时长应该是 0～n，即 curve.ParamLength = n，那么在 evaluator 中也应该是在 0～n 的时长区间内遍历。横向轨迹采样不涉及时间，因此对时长没有影响，时长仅由纵向采样确定，进一步的，最终的合并后轨迹的时长也是由纵向采样确定。

• 我没有找到Apollo中对长短不一的时长全部补全到 trajectory_time_length 的代码，即便有，也不合理。试想，超出curve.ParamLength 时刻后的轨迹是没有计算过的，对 n~trajectory_time_length 间的轨迹赋值的合理性依据何在呢？

• 我认为解决方法有2种。一是所有涉及到时间的地方，以 0～curve.ParamLength 作为遍历的区间范围。二是令curve.Ealuate(t, order) 在 t>curve.ParamLength 后全部返回0，无论阶数order。此处的curve仅仅是数学上曲线的抽象概念，因此Apollo把各种曲线的定义放在了math文件夹中，而不是物理意义上的轨迹。若是物理意义上的轨迹，方法二就不合理了。试想若curve表示物理上的path，则 curve.Ealuate(t, 0), t>curve.ParamLength 是0～t车行驶过的距离，怎么可能会是0呢？

• 最新更正：LatticeTrajectory1d class 是对curve的封装，是真正使用的物理意义上的轨迹。LatticeTrajectory1d.Evaluate(t, order) 中，对于 t>=curve.ParamLength 的情况，按照恒加速度进行了后续的推算，相当于把轨迹补全到了trajectory_time_length。不过，我仍然认为，只计算 0～curve.ParamLength 区间内的轨迹更合理。因为恒加速度的假设在现实世界中很少成立。

//Curve1d（3.4.5次多项式）是数学意义上的轨迹，只是单纯的一条方程；LatticeTrajectory1d 是物理意义上的轨迹，附加了其他信息
LatticeTrajectory1d::LatticeTrajectory1d(
    std::shared_ptr ptr_trajectory1d) {
  ptr_trajectory1d_ = ptr_trajectory1d;
}

//param和param_length都是时间，位置、速度（一阶导）、加速度（二阶导）都是关于时间t的函数
double LatticeTrajectory1d::Evaluate(const std::uint32_t order,
                                     const double param) const {
  double param_length = ptr_trajectory1d_->ParamLength();
  if (param < param_length) {
    return ptr_trajectory1d_->Evaluate(order, param);
  }

  // do constant acceleration extrapolation;
  // to align all the trajectories with time.
  //如果param >= param_length，则超出了轨迹方程的定义域，于是按恒定加速度假设继续推算，因此 >=3阶的求导都是0
  double p = ptr_trajectory1d_->Evaluate(0, param_length);
  double v = ptr_trajectory1d_->Evaluate(1, param_length);
  double a = ptr_trajectory1d_->Evaluate(2, param_length);

  double t = param - param_length;

  switch (order) {
    case 0:
      return p + v * t + 0.5 * a * t * t;
    case 1:
      return v + a * t;
    case 2:
      return a;
    default:
      return 0.0;
  }
}

• 本节所讨论的Apollo Lattice中的问题，是我在看代码时想到的，还没有验证过。我提出的解决方法也还没有验证。若有同学有更确凿的想法，欢迎一起交流。

EM Planner

EM Planner 将轨迹规划分为2部分优化：path即d-s，speed即s-t，优化分为2个步骤：DP动态规划求大致解，更像是给出nudge or overtake这样的决策，把优化问题变为凸优化问题，为后续的QP二次规划提供参考。QP则求精细的平滑的轨迹。我主要参考了论文，还没有去看EM的代码，优化的constraints不再赘述。

• DP path

上式中，横向轨迹d=f(s)，Cobs()中d是障碍物与自车的距离，dc是safety buffer，dn是nudge range，g(s)是guidance line function，我理解的是指参考线方程ReferenceLine。

• QP path

上式中，前三项主要评估smooth，最后一项评估guidance。g(s)与DP中的g(s)类似，但在此处具体指的是DP的输出结果。

• DP speed

上式中，S是piecewise linear speed profile function，其实就是s(t)，是s-t graph上的一条曲线。g()是针对大于或小于Vref的惩罚函数，类似于以上的guidance部分，Vref在每个时刻可能有不同的值。

• QP speed

上式中，Sref是DP输出的speed guidance profile，类似于上面的g(s)，guidance。

对比

从公式来看，Lattice是在已知轨迹的情况下评估，考虑的因素更为具体和全面。而EM是在轨迹未知的情况下求轨迹，考虑的因素更单纯，公式形式更简单。总体来说，轨迹的平滑、避免碰撞、与参考方程（参考线、前步输出、目标速度等）吻合等因素是均要纳入考虑的。

评估轨迹cost 时，可以结合局部的cost 和全局route 层面的cost，这样规划和决策会有一定程度的结合。