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

原创

已于 2022-10-06 20:44:02 修改 · 2k 阅读

14 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #c++ #几何学

于 2022-09-25 20:51:08 首次发布

本文介绍多项式曲线在自动驾驶运动规划中的作用，重点讨论三次、四次及五次多项式曲线的构造函数、系数计算及求值方法，并对比不同应用场景下的曲线选择。

自动驾驶运动规划中会用到各种曲线，主要用于生成车辆变道的轨迹，高速场景中使用的是五次多项式曲线，城市场景中使用的是分段多项式曲线（piecewise），相比多项式，piecewise能够生成更为复杂的路径。另外对于自由空间，可以使用A*搜索出的轨迹再加以cilqr加以平滑，也能直接供控制使用。下面的内容不一定都对，欢迎大家一起交流学习~

基础类

// 基类Curve1d，定义一维曲线（变量是时间）
class Curve1d {
 public:
  // 构造函数
  Curve1d() = default;
  // 拷贝构造函数
  Curve1d(const Curve1d& other) = default;
  // 有继承的基类的析构函数需要定义为虚函数
  virtual ~Curve1d() = default;
  // 纯虚函数，子类实现，曲线不同阶数对应的param时刻的结果
  virtual double Evaluate(const std::uint32_t order, const double param) const = 0;
  // 纯虚函数，子类实现，时长
  virtual double ParamLength() const = 0;
};

三次多项式曲线

$f(x)=C_{0}+C_{1}x+C_{2}x^{2}+C_{3}x^{3}$

构造函数定义

x0初始位置，dx0初始速度，ddx0初始加速度，x1终点位置，param时间长度，这个很好理解

// 定义了两种构造函数的实现

// 第一种构造函数的实现是直接调用第二种构造函数
CubicPolynomialCurve1d::CubicPolynomialCurve1d(const std::array<double, 3>& start, const double end, const double param)
    : CubicPolynomialCurve1d(start[0], start[1], start[2], end, param) {}

// 第二种构造函数，x0初始位置，dx0初始速度，ddx0初始加速度，x1终点位置，param时间长度
CubicPolynomialCurve1d::CubicPolynomialCurve1d(const double x0, const double dx0, const double ddx0, const double x1,
                                               const double param) {
  ComputeCoefficients(x0, dx0, ddx0, x1, param);
  param_ = param;
  start_condition_[0] = x0;
  start_condition_[1] = dx0;
  start_condition_[2] = ddx0;
  end_condition_ = x1;
}

计算多项式系数

方法1：一般地，自动驾驶里初始状态对应的是当前时刻，规划时长为param的轨迹，可以理解初始状态即为0时刻的状态，这样的话，多项式系数就很好求出来了

$C_{0}=f(0)=x_{0}$

$C_{1}=f{}'(0)=x_{1}$

$C_{2}=0.5f{}''(0)$

$C_{3}=(x_{1}-C_{0}-C_{1}x-C_2x^2)/x^3)(x=param)$

void CubicPolynomialCurve1d::ComputeCoefficients(const double x0, const double dx0, const double ddx0, const double x1,
                                                 const double param) {
  assert(param > 0.0);
  const double p2 = param * param;
  const double p3 = param * p2;
  coef_[0] = x0;
  coef_[1] = dx0;
  coef_[2] = 0.5 * ddx0;
  coef_[3] = (x1 - x0 - dx0 * param - coef_[2] * p2) / p3;
}

方法2：已知初始和终点的位置和速度，没有加速度，也比较简单，C2和C3需要手动推算下

$C_0=f(0)=x_0$