Apollo：多项式曲线类

最新推荐文章于 2023-08-10 14:35:12 发布

OceanStar的学习笔记

最新推荐文章于 2023-08-10 14:35:12 发布

阅读量454

点赞数 2

分类专栏： ROS 文章标签：算法

原文链接：https://zhuanlan.zhihu.com/p/568126622

版权

ROS 专栏收录该内容

84 篇文章 46 订阅

订阅专栏

多项式曲线由于其微分、积分计算方便，在自动驾驶的路径规划、速度规划中有着广泛的应用，apollo平台Planning算法中也为此单独做一个功能类供上层调用。

Curve1d

路径：apollo/modules/planning/math/curve1d/curve1d.h

作用：

基类，定义接口

class Curve1d {
 public:
  Curve1d() = default;

  virtual ~Curve1d() = default;
  //计算在曲线param位置，order阶多项式曲线的函数值
  virtual double Evaluate(const std::uint32_t order,
                          const double param) const = 0;

  virtual double ParamLength() const = 0;

  virtual std::string ToString() const = 0;
};

基础类：PolynomialCurve1d

路径：apollo/modules/planning/math/curve1d/polynomial_curve1d.h

作用：在Curve1d的基础上继续进行封装，增加适用于多项式曲线的接口函数（与螺旋线曲线做对比）

namespace apollo {
namespace planning {

class PolynomialCurve1d : public Curve1d {
 public:
  PolynomialCurve1d() = default;
  virtual ~PolynomialCurve1d() = default;
  //读取高阶多项式的系数
  virtual double Coef(const size_t order) const = 0;
  virtual size_t Order() const = 0;

 protected:
  double param_ = 0.0;
};

多项式曲线

所有多项式是曲线类均继承于基类Curve1d。分为三阶（cubic）、四阶（quartic）、五阶（quintic）多项式曲线、五阶螺旋曲线等等

3阶：CubicPolynomialCurve1d

路径：apollo/modules/planning/math/curve1d/http://cubic_polynomial_curve1d.cc
在这里插入图片描述

系数求解：

void CubicPolynomialCurve1d::ComputeCoefficients(const double x0,
                                                 const double dx0,
                                                 const double ddx0,
                                                 const double x1,
                                                 const double param) {
  DCHECK(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;
}

4阶：QuarticPolynomialCurve1d

路径：modules/planning/math/curve1d/http://quartic_polynomial_curve1d.cc

系数求解（利用P1点的1、2阶导数）：

void QuarticPolynomialCurve1d::ComputeCoefficients(
    const double x0, const double dx0, const double ddx0, const double dx1,
    const double ddx1, const double p) {
  CHECK_GT(p, 0.0);

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

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

  double p2 = p * p;
  double p3 = p2 * p;

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

四阶多项式曲线还可以从三阶曲线积分而来：

在这里插入图片描述

函数接口为：

//other为四阶多项式曲线，init_value为0时的四阶曲线函数值
QuarticPolynomialCurve1d& QuarticPolynomialCurve1d::IntegratedFromCubicCurve(
    const PolynomialCurve1d& other, const double init_value) {
  CHECK_EQ(other.Order(), 3U);
  param_ = other.ParamLength();
  coef_[0] = init_value;
  for (size_t i = 0; i < 4; ++i) {
    coef_[i + 1] = other.Coef(i) / (static_cast<double>(i) + 1);
  }
  return *this;
}

5阶：QuinticPolynomialCurve1d

路径：apollo/modules/planning/math/curve1d/http://quintic_polynomial_curve1d.cc
在这里插入图片描述

系数求解：

void QuinticPolynomialCurve1d::ComputeCoefficients(
    const double x0, const double dx0, const double ddx0, const double x1,
    const double dx1, const double ddx1, const double p) {
  CHECK_GT(p, 0.0);

  coef_[0] = x0;
  coef_[1] = dx0;
  coef_[2] = ddx0 / 2.0;

  const double p2 = p * p;
  const double p3 = p * p2;

  // the direct analytical method is at least 6 times faster than using matrix
  // inversion.
  const double c0 = (x1 - 0.5 * p2 * ddx0 - dx0 * p - x0) / p3;
  const double c1 = (dx1 - ddx0 * p - dx0) / p2;
  const double c2 = (ddx1 - ddx0) / p;

  coef_[3] = 0.5 * (20.0 * c0 - 8.0 * c1 + c2);
  coef_[4] = (-15.0 * c0 + 7.0 * c1 - c2) / p;
  coef_[5] = (6.0 * c0 - 3.0 * c1 + 0.5 * c2) / p2;
}

Curve即贝塞尔曲线，其数学基础是早在 1912 年就广为人知的伯恩斯坦多项式。但直到 1959 年，当时就职于雪铁龙的法国数学家 Paul de Casteljau 才开始对它进行图形化应用的尝试，并提出了一种数值稳定的 de Casteljau 算法。然而贝塞尔曲线的得名，却是由于 1962 年另一位就职于雷诺的法国工程师 Pierre Bézier 的广泛宣传。他使用这种只需要很少的控制点就能够生成复杂平滑曲线的方法，来辅助汽车车体的工业设计。

螺旋曲线

五次螺旋曲线：QuinticSpiralPath

在这里插入图片描述

QuinticSpiralPath::QuinticSpiralPath(const double x0, const double dx0,
                                     const double ddx0, const double x1,
                                     const double dx1, const double ddx1,
                                     const double p)
    : QuinticPolynomialCurve1d(x0, dx0, ddx0, x1, dx1, ddx1, p) {
  ACHECK(p > 0.0);

  double p2 = p * p;
  double p3 = p2 * p;
  double p4 = p3 * p;
  double p5 = p2 * p3;
  double p6 = p3 * p3;

  // derive a
  // double a = -6.0 * x0 / p5 - 3.0 * dx0 / p4 - 0.5 * ddx0 / p3 + 6.0 * x1 /
  // p5 - 3.0 * dx1 / p4 + 0.5 * ddx1 / p3;
  coef_deriv_[5][0] = -6.0 / p5;

  coef_deriv_[5][1] = -3.0 / p4;

  coef_deriv_[5][2] = -0.5 / p3;

  coef_deriv_[5][3] = 6.0 / p5;

  coef_deriv_[5][4] = -3.0 / p4;

  coef_deriv_[5][5] = 0.5 / p3;

  coef_deriv_[5][6] = 30.0 * x0 / p6 + 12.0 * dx0 / p5 + 1.5 * ddx0 / p4 -
                      30.0 * x1 / p6 + 12.0 * dx1 / p5 - 1.5 * ddx1 / p4;

  // derive b
  // double b = 15.0 * x0 / p4 + 8.0 * dx0 / p3 + 1.5 * ddx0 / p2 - 15.0 * x1 /
  // p4 + 7.0 * dx1 / p3 - ddx1 / p2;
  coef_deriv_[4][0] = 15.0 / p4;

  coef_deriv_[4][1] = 8.0 / p3;

  coef_deriv_[4][2] = 1.5 / p2;

  coef_deriv_[4][3] = -15.0 / p4;

  coef_deriv_[4][4] = 7.0 / p3;

  coef_deriv_[4][5] = -1.0 / p2;

  coef_deriv_[4][6] = -60.0 * x0 / p5 - 24.0 * dx0 / p4 - 3.0 * ddx0 / p3 +
                      60.0 * x1 / p5 - 21.0 * dx1 / p4 + 2.0 * ddx1 / p3;

  // derive c
  // double c = -10.0 * x0 / p3 - 6.0 * dx0 / p2 - 1.5 * ddx0 / p + 10.0 * x1 /
  // p3 - 4.0 * dx1 / p2 + 0.5 * ddx1 / p;
  coef_deriv_[3][0] = -10.0 / p3;

  coef_deriv_[3][1] = -6.0 / p2;

  coef_deriv_[3][2] = -1.5 / p;

  coef_deriv_[3][3] = 10.0 / p3;

  coef_deriv_[3][4] = -4.0 / p2;

  coef_deriv_[3][5] = 0.5 / p;

  coef_deriv_[3][6] = 30.0 * x0 / p4 + 12.0 * dx0 / p3 + 1.5 * ddx0 / p2 -
                      30.0 * x1 / p4 + 8.0 * dx1 / p3 - 0.5 * ddx1 / p2;

  // derive d
  // double d = 0.5 * ddx0;
  coef_deriv_[2][0] = 0.0;

  coef_deriv_[2][1] = 0.0;

  coef_deriv_[2][2] = 0.5;

  coef_deriv_[2][3] = 0.0;

  coef_deriv_[2][4] = 0.0;

  coef_deriv_[2][5] = 0.0;

  coef_deriv_[2][6] = 0.0;

  // derive e
  // double e = dx0;
  coef_deriv_[1][0] = 0.0;

  coef_deriv_[1][1] = 1.0;

  coef_deriv_[1][2] = 0.0;

  coef_deriv_[1][3] = 0.0;

  coef_deriv_[1][4] = 0.0;

  coef_deriv_[1][5] = 0.0;

  coef_deriv_[1][6] = 0.0;

  // derive f
  // double f = x0;
  coef_deriv_[0][0] = 1.0;

  coef_deriv_[0][1] = 0.0;

  coef_deriv_[0][2] = 0.0;

  coef_deriv_[0][3] = 0.0;

  coef_deriv_[0][4] = 0.0;

  coef_deriv_[0][5] = 0.0;

  coef_deriv_[0][6] = 0.0;
}

在这里插入图片描述

分段五次螺旋曲线：PiecewiseQuinticSpiralPath

如果对一个起点和一个终点，可以使用五次螺旋曲线来描述。如果对于多个离散点，则需要使用分段五次螺旋曲线来进行描述。

在这里插入图片描述

对应的代码如下

PiecewiseQuinticSpiralPath::PiecewiseQuinticSpiralPath(const double theta,
                                                       const double kappa,
                                                       const double dkappa)
    : last_theta_(theta), last_kappa_(kappa), last_dkappa_(dkappa) {
  accumulated_s_.push_back(0.0);
}

void PiecewiseQuinticSpiralPath::Append(const double theta, const double kappa,
                                        const double dkappa,
                                        const double delta_s) {
  double s = delta_s + accumulated_s_.back();
  accumulated_s_.push_back(s);

  pieces_.emplace_back(last_theta_, last_kappa_, last_dkappa_, theta, kappa,
                       dkappa, delta_s);

  last_theta_ = theta;
  last_kappa_ = kappa;
  last_dkappa_ = dkappa;
}

问题

为什么多项式曲线和螺旋曲线呢？

因为曲线在不同坐标系下的表示不同。
在这里插入图片描述

链接

怎么理解贝塞尔曲线？
https://zhuanlan.zhihu.com/p/471457420
https://docs.godotengine.org/zh_CN/latest/classes/class_path2d.html#class-path2d
https://zhuanlan.zhihu.com/p/452122391