多项式曲线由于其微分、积分计算方便,在自动驾驶的路径规划、速度规划中有着广泛的应用,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