一、纵向规划

void QuarticPolynomialCurve1d::ComputeCoefficients(
    const float x0, const float dx0, const float ddx0, const float dx1,
    const float ddx1, const float p) {
  if (p <= 0.0) {
    std::cout << "p should be greater than 0 at line 140." << std::endl;
  }

  coef_[0] = x0;
  coef_[1] = dx0;
  coef_[2] = 0.5 * ddx0;

  float b0 = dx1 - ddx0 * p - dx0;
  float b1 = ddx1 - ddx0;

  float p2 = p * p;
  float p3 = p2 * p;

  coef_[3] = (3 * b0 - b1 * p) / (3 * p2);
  coef_[4] = (-2 * b0 + b1 * p) / (4 * p3);
}

参考：

1. 自动驾驶规划模块学习笔记-多项式曲线

2. Apollo Lattice Planner学习记录