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

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

目录

基础类

三次多项式曲线

四次多项式曲线

五次多项式曲线

下期预告：piecewise曲线 

---

基础类

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

---

三次多项式曲线

构造函数定义

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

1.  
   // 定义了两种构造函数的实现
2.  
    
3.  
   // 第一种构造函数的实现是直接调用第二种构造函数
4.  
   CubicPolynomialCurve1d::CubicPolynomialCurve1d(const std::array<double, 3>& start, const double end, const double param)
5.  
   : CubicPolynomialCurve1d(start[0], start[1], start[2], end, param) {}
6.  
    
7.  
   // 第二种构造函数，x0初始位置，dx0初始速度，ddx0初始加速度，x1终点位置，param时间长度
8.  
   CubicPolynomialCurve1d::CubicPolynomialCurve1d(const double x0, const double dx0, const double ddx0, const double x1,
9.  
   const double param) {
10.  
    ComputeCoefficients(x0, dx0, ddx0, x1, param);
11.  
    param_ = param;
12.  
    start_condition_[0] = x0;
13.  
    start_condition_[1] = dx0;
14.  
    start_condition_[2] = ddx0;
15.  
    end_condition_ = x1;
16.  
    }

计算多项式系数

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

 

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

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

, p为终点时刻

1.  
   // 注意这里的返回类型，用到了this指针，相当于返回一个实例化的类
2.  
   CubicPolynomialCurve1d& CubicPolynomialCurve1d::FitWithEndPointFirstOrder(const double x0, const double dx0, const double x1,
3.  
   const double dx1, const double param) {
4.  
   if (param <= 0.0) {
5.  
   return *this;
6.  
   }
7.  
   param_ = param;
8.  
   coef_[0] = x0;
9.  
   coef_[1] = dx0;
10.  
     
11.  
    double param2 = param * param;
12.  
    double param3 = param2 * param;
13.  
     
14.  
    double b0 = dx1 2 * coef_[1];
15.  
    double b1 = dx1 coef_[1];
16.  
     
17.  
    coef_[2] = (3 * x1 - b0 * param -3 * coef_[0]) / param2;
18.  
    coef_[3] = (2 * coef_[0] b1 * param - 2 * x1) / param3;
19.  
     
20.  
    return *this;
21.  
    }

方法3：通过四次多项式曲线来推算，这里用到四次多项式求导为三次多项式的关系

以此类推

1.  
   // 这里为什么要用静态类型转换static_cast
2.  
   void CubicPolynomialCurve1d::DerivedFromQuarticCurve(const PolynomialCurve1d& other) {
3.  
   assert(other.Order() == 4);
4.  
   param_ = other.ParamLength();
5.  
   for (size_t i = 1; i < 5; i) {
6.  
   coef_[i - 1] = other.Coef(i) * static_cast<double>(i);
7.  
   }
8.  
   }

多项式曲线阶数求值

这里代码有点没太看懂，p是param的几次方？

1.  
   double CubicPolynomialCurve1d::Evaluate(const std::uint32_t order, const double p) const {
2.  
   switch (order) {
3.  
   case 0: {
4.  
   return ((coef_[3] * p coef_[2]) * p coef_[1]) * p coef_[0];
5.  
   }
6.  
   case 1: {
7.  
   return (3.0 * coef_[3] * p 2.0 * coef_[2]) * p coef_[1];
8.  
   }
9.  
   case 2: {
10.  
    return 6.0 * coef_[3] * p 2.0 * coef_[2];
11.  
    }
12.  
    case 3: {
13.  
    return 6.0 * coef_[3];
14.  
    }
15.  
    default:
16.  
    return 0.0;
17.  
    }
18.  
    }

---

四次多项式曲线

构造函数

 四次多项式有五个系数，所以需要五个状态量

1.  
   QuarticPolynomialCurve1d::QuarticPolynomialCurve1d(
2.  
   const std::array<double, 3>& start, const std::array<double, 2>& end,
3.  
   const double param)
4.  
   : QuarticPolynomialCurve1d(start[0], start[1], start[2], end[0], end[1],
5.  
   param) {}
6.  
    
7.  
   QuarticPolynomialCurve1d::QuarticPolynomialCurve1d(
8.  
   const double x0, const double dx0, const double ddx0, const double dx1,
9.  
   const double ddx1, const double param) {
10.  
    param_ = param;
11.  
    start_condition_[0] = x0;
12.  
    start_condition_[1] = dx0;
13.  
    start_condition_[2] = ddx0;
14.  
    end_condition_[0] = dx1;
15.  
    end_condition_[1] = ddx1;
16.  
    ComputeCoefficients(x0, dx0, ddx0, dx1, ddx1, param);
17.  
    }

求解多项式系数

方法1：已知初始状态x0,dx0,ddx0和终点状态dx1,ddx1，相当于终点的位置是不确定的。代码中C3和C4计算没有使用C1和C2，是避免计算过程中带来的误差，是一种较好的处理方法。但阅读起来可能不直观，可以参考下面公式，实质是一样的。

1.  
   void QuarticPolynomialCurve1d::ComputeCoefficients(
2.  
   const double x0, const double dx0, const double ddx0, const double dx1,
3.  
   const double ddx1, const double p) {
4.  
   CHECK_GT(p, 0.0);
5.  
    
6.  
   coef_[0] = x0;
7.  
   coef_[1] = dx0;
8.  
   coef_[2] = 0.5 * ddx0;
9.  
    
10.  
    double b0 = dx1 - ddx0 * p - dx0;
11.  
    double b1 = ddx1 - ddx0;
12.  
     
13.  
    double p2 = p * p;
14.  
    double p3 = p2 * p;
15.  
     
16.  
    coef_[3] = (3 * b0 - b1 * p) / (3 * p2);
17.  
    coef_[4] = (-2 * b0 b1 * p) / (4 * p3);
18.  
    }

方法2和3：终点或初始位置的加速度不确定，已知其他参数，推导过程与上面类似，注意这里返回的是实例化的类

1.  
   QuarticPolynomialCurve1d& QuarticPolynomialCurve1d::FitWithEndPointFirstOrder(
2.  
   const double x0, const double dx0, const double ddx0, const double x1,
3.  
   const double dx1, const double p) {
4.  
   CHECK_GT(p, 0.0);
5.  
    
6.  
   param_ = p;
7.  
    
8.  
   coef_[0] = x0;
9.  
    
10.  
    coef_[1] = dx0;
11.  
     
12.  
    coef_[2] = 0.5 * ddx0;
13.  
     
14.  
    double p2 = p * p;
15.  
    double p3 = p2 * p;
16.  
    double p4 = p3 * p;
17.  
     
18.  
    double b0 = x1 - coef_[0] - coef_[1] * p - coef_[2] * p2;
19.  
    double b1 = dx1 - dx0 - ddx0 * p;
20.  
     
21.  
    coef_[4] = (b1 * p - 3 * b0) / p4;
22.  
    coef_[3] = (4 * b0 - b1 * p) / p3;
23.  
    return *this;
24.  
    }
25.  
    QuarticPolynomialCurve1d& QuarticPolynomialCurve1d::FitWithEndPointSecondOrder(
26.  
    const double x0, const double dx0, const double x1, const double dx1,
27.  
    const double ddx1, const double p) {
28.  
    CHECK_GT(p, 0.0);
29.  
     
30.  
    param_ = p;
31.  
     
32.  
    coef_[0] = x0;
33.  
     
34.  
    coef_[1] = dx0;
35.  
     
36.  
    double p2 = p * p;
37.  
    double p3 = p2 * p;
38.  
    double p4 = p3 * p;
39.  
     
40.  
    double b0 = x1 - coef_[0] - coef_[1] * p;
41.  
    double b1 = dx1 - coef_[1];
42.  
    double c1 = b1 * p;
43.  
    double c2 = ddx1 * p2;
44.  
     
45.  
    coef_[2] = (0.5 * c2 - 3 * c1 6 * b0) / p2;
46.  
    coef_[3] = (-c2 5 * c1 - 8 * b0) / p3;
47.  
    coef_[4] = (0.5 * c2 - 2 * c1 3 * b0) / p4;
48.  
     
49.  
    return *this;
50.  
    }

方法4和5：根据三次多项式积分或五次多项式微分得到

1.  
   QuarticPolynomialCurve1d& QuarticPolynomialCurve1d::IntegratedFromCubicCurve(
2.  
   const PolynomialCurve1d& other, const double init_value) {
3.  
   CHECK_EQ(other.Order(), 3U);
4.  
   param_ = other.ParamLength();
5.  
   coef_[0] = init_value;
6.  
   for (size_t i = 0; i < 4; i) {
7.  
   coef_[i 1] = other.Coef(i) / (static_cast<double>(i) 1);
8.  
   }
9.  
   return *this;
10.  
    }
11.  
     
12.  
    QuarticPolynomialCurve1d& QuarticPolynomialCurve1d::DerivedFromQuinticCurve(
13.  
    const PolynomialCurve1d& other) {
14.  
    CHECK_EQ(other.Order(), 5U);
15.  
    param_ = other.ParamLength();
16.  
    for (size_t i = 1; i < 6; i) {
17.  
    coef_[i - 1] = other.Coef(i) * static_cast<double>(i);
18.  
    }
19.  
    return *this;
20.  
    }

多项式曲线阶数求值

1.  
   double QuarticPolynomialCurve1d::Evaluate(const std::uint32_t order,
2.  
   const double p) const {
3.  
   switch (order) {
4.  
   case 0: {
5.  
   return (((coef_[4] * p coef_[3]) * p coef_[2]) * p coef_[1]) * p
6.  
   coef_[0];
7.  
   }
8.  
   case 1: {
9.  
   return ((4.0 * coef_[4] * p 3.0 * coef_[3]) * p 2.0 * coef_[2]) * p
10.  
    coef_[1];
11.  
    }
12.  
    case 2: {
13.  
    return (12.0 * coef_[4] * p 6.0 * coef_[3]) * p 2.0 * coef_[2];
14.  
    }
15.  
    case 3: {
16.  
    return 24.0 * coef_[4] * p 6.0 * coef_[3];
17.  
    }
18.  
    case 4: {
19.  
    return 24.0 * coef_[4];
20.  
    }
21.  
    default:
22.  
    return 0.0;
23.  
    }
24.  
    }

---

五次多项式曲线

整体思路和四次多项式类型，附上代码，可以参考上面思路理解

在具体代码实现时，数值求解比矩阵计算效率更高

1.  
   QuinticPolynomialCurve1d::QuinticPolynomialCurve1d(
2.  
   const std::array<double, 3>& start, const std::array<double, 3>& end,
3.  
   const double param)
4.  
   : QuinticPolynomialCurve1d(start[0], start[1], start[2], end[0], end[1],
5.  
   end[2], param) {}
6.  
    
7.  
   QuinticPolynomialCurve1d::QuinticPolynomialCurve1d(
8.  
   const double x0, const double dx0, const double ddx0, const double x1,
9.  
   const double dx1, const double ddx1, const double param) {
10.  
    ComputeCoefficients(x0, dx0, ddx0, x1, dx1, ddx1, param);
11.  
    start_condition_[0] = x0;
12.  
    start_condition_[1] = dx0;
13.  
    start_condition_[2] = ddx0;
14.  
    end_condition_[0] = x1;
15.  
    end_condition_[1] = dx1;
16.  
    end_condition_[2] = ddx1;
17.  
    param_ = param;
18.  
    }
19.  
    void QuinticPolynomialCurve1d::ComputeCoefficients(
20.  
    const double x0, const double dx0, const double ddx0, const double x1,
21.  
    const double dx1, const double ddx1, const double p) {
22.  
    CHECK_GT(p, 0.0);
23.  
     
24.  
    coef_[0] = x0;
25.  
    coef_[1] = dx0;
26.  
    coef_[2] = ddx0 / 2.0;
27.  
     
28.  
    const double p2 = p * p;
29.  
    const double p3 = p * p2;
30.  
     
31.  
    // the direct analytical method is at least 6 times faster than using matrix
32.  
    // inversion.
33.  
    const double c0 = (x1 - 0.5 * p2 * ddx0 - dx0 * p - x0) / p3;
34.  
    const double c1 = (dx1 - ddx0 * p - dx0) / p2;
35.  
    const double c2 = (ddx1 - ddx0) / p;
36.  
     
37.  
    coef_[3] = 0.5 * (20.0 * c0 - 8.0 * c1 c2);
38.  
    coef_[4] = (-15.0 * c0 7.0 * c1 - c2) / p;
39.  
    coef_[5] = (6.0 * c0 - 3.0 * c1 0.5 * c2) / p2;
40.  
    }
41.  
    void QuinticPolynomialCurve1d::IntegratedFromQuarticCurve(
42.  
    const PolynomialCurve1d& other, const double init_value) {
43.  
    CHECK_EQ(other.Order(), 4U);
44.  
    param_ = other.ParamLength();
45.  
    coef_[0] = init_value;
46.  
    for (size_t i = 0; i < 5; i) {
47.  
    coef_[i 1] = other.Coef(i) / (static_cast<double>(i) 1);
48.  
    }
49.  
    }
50.  
    double QuinticPolynomialCurve1d::Evaluate(const uint32_t order,
51.  
    const double p) const {
52.  
    switch (order) {
53.  
    case 0: {
54.  
    return ((((coef_[5] * p coef_[4]) * p coef_[3]) * p coef_[2]) * p
55.  
    coef_[1]) *
56.  
    p
57.  
    coef_[0];
58.  
    }
59.  
    case 1: {
60.  
    return (((5.0 * coef_[5] * p 4.0 * coef_[4]) * p 3.0 * coef_[3]) * p
61.  
    2.0 * coef_[2]) *
62.  
    p
63.  
    coef_[1];
64.  
    }
65.  
    case 2: {
66.  
    return (((20.0 * coef_[5] * p 12.0 * coef_[4]) * p) 6.0 * coef_[3]) *
67.  
    p
68.  
    2.0 * coef_[2];
69.  
    }
70.  
    case 3: {
71.  
    return (60.0 * coef_[5] * p 24.0 * coef_[4]) * p 6.0 * coef_[3];
72.  
    }
73.  
    case 4: {
74.  
    return 120.0 * coef_[5] * p 24.0 * coef_[4];
75.  
    }
76.  
    case 5: {
77.  
    return 120.0 * coef_[5];
78.  
    }
79.  
    default:
80.  
    return 0.0;
81.  
    }
82.  
    }

下期预告：piecewise曲线 

这篇好文章是转载于：学新通技术网