百度apollo planning代码学习-Apollo\modules\planning\math\curve1d\QuarticPolynomialCurve1d类代码详解

概述

QuarticPolynomialCurve1d类是apollo planning模块下modules\planning\math\curve1d\quartic_polynomial_curve1d.cc /.h实现

从类名来看，应该是QuarticPolynomialCurve1d是四次多项式1维参数曲线类？

从代码来看QuarticPolynomialCurve1d类主要是实现：
1.根据起点处(参数param=0)的值，x0,dx0,ddx0，以及终点处（参数param=paramlength）的值，x1,dx1,ddx1，这里总共6个条件，只要其中5个就可以确定一条四次多项式；
2.根据五次多项式求导得到一条四次多项式；
3.根据3次多项式积分得到四次多项式；
4.储存四次多项式个各项系数；
5.对储存的四次多项式的插值，插值出指定参数处指定阶导数(0,1,2,3,4)的值

quartic_polynomial_curve1d.h

#pragma once

#include <array>
#include <string>

#include "modules/planning/math/curve1d/polynomial_curve1d.h"

namespace apollo {
namespace planning {

// 1D quartic polynomial curve: (x0, dx0, ddx0) -- [0, param] --> (dx1, ddx1)
//公有继承多项式1维参数曲线类
class QuarticPolynomialCurve1d : public PolynomialCurve1d {
 public:
 //默认构造函数
  QuarticPolynomialCurve1d() = default;

//四次多项式1维参数曲线类构造函数
//输入参数 初始状态3维(s,s',s''或d,d',d'')， 终端状态2维(调用时只输入了d',d'')？这样的话就5个方程了就可以确定一条四次多项式，只适用于终点位置不固定类似巡航这种场景
  QuarticPolynomialCurve1d(const std::array<double, 3>& start,
                           const std::array<double, 2>& end,
                           const double param);

//这个构造函数就是被上面的构造函数调用，就是根据
//param=0 x0,dx0,ddx0
//param=paramlength dx1 ddx1 5个方程求出四次多项式的五个系数，调用函数ComputeCoefficients来求解多项式的系数
  QuarticPolynomialCurve1d(const double x0, const double dx0, const double ddx0,
                           const double dx1, const double ddx1,
                           const double param);

//这个构造函数？输入一个四次多项式，然后把其系数和参数的总长度拷贝给类成员？
  QuarticPolynomialCurve1d(const QuarticPolynomialCurve1d& other);

//默认析构函数
  virtual ~QuarticPolynomialCurve1d() = default;

//多项式插值，具体原理很简单就不详述了，就是多项式求导，可以插值出指定参数处四次多项式指定阶导数对应的值。
  double Evaluate(const std::uint32_t order, const double p) const override;

  /**
   * Interface with refine quartic polynomial by meets end first order
   * and start second order boundary condition:
   * @param  x0    init point x location
   * @param  dx0   init point derivative
   * @param  ddx0  init point second order derivative
   * @param  x1    end point x location
   * @param  dx1   end point derivative
   * @param  param parameter length
   * @return       self
   */
   //用终端的一阶条件求解四次多项式
//param=0时，x=x0,dx=dx0,ddx=ddx0
//param=paramlength(这里就是p)时， x=x1 dx=dx1
//5个条件求解四次多项式各个系数存入类成员array数组coef_中
  QuarticPolynomialCurve1d& FitWithEndPointFirstOrder(
      const double x0, const double dx0, const double ddx0, const double x1,
      const double dx1, const double param);

  /**
   * Interface with refine quartic polynomial by meets end point second order
   * and start point first order boundary condition
   */
   //用终端的二阶条件求解四次多项式
//param=0时，x=x0,dx=dx0
//param=paramlength(这里就是p)时， x=x1 dx=dx1，ddx=ddx1
//5个条件求解四次多项式各个系数存入类成员array数组coef_中
  QuarticPolynomialCurve1d& FitWithEndPointSecondOrder(
      const double x0, const double dx0, const double x1, const double dx1,
      const double ddx1, const double param);

  /*
   * Integrated from cubic curve with init value
   */
   //根据输入的3次多项式积分得到四次多项式
//输入参数3次多项式，还有四次多项式param=0时的x初值，最后的积分得到的四次多项式放入类成员coef_里
  QuarticPolynomialCurve1d& IntegratedFromCubicCurve(
      const PolynomialCurve1d& other, const double init_value);

  /*
   * Derived from quintic curve
   */
   //从五次多项式求导得到四次多项似乎，输入的就是一个五次多项式，最后求导的四次多项式放入类成员coef_里
  QuarticPolynomialCurve1d& DerivedFromQuinticCurve(
      const PolynomialCurve1d& other);

//返回类成员 参数总长度(时间t，或纵向长度s)
  double ParamLength() const override { return param_; }

//返回调试字符串，其实就是多项式的系数及其参数总长度
  std::string ToString() const override;

//获取四次多项式指定幂次项的系数
  double Coef(const size_t order) const override;

//四次多项式的最高幂次当然是4了
  size_t Order() const override { return 4; }

 private:
 //param=0时，x=x0,dx=dx0,ddx=ddx0
//param=paramlength(这里就是p)时， dx=dx1，ddx=ddx1
//5个条件求解四次多项式各个系数存入类成员array数组coef_中
  void ComputeCoefficients(const double x0, const double dx0, const double ddx0,
                           const double dx1, const double ddx1,
                           const double param);

  std::array<double, 5> coef_ = {{0.0, 0.0, 0.0, 0.0, 0.0}};
  std::array<double, 3> start_condition_ = {{0.0, 0.0, 0.0}};
  std::array<double, 2> end_condition_ = {{0.0, 0.0}};
};

}  // namespace planning
}  // namespace apollo

quartic_polynomial_curve1d.cc

#include "modules/planning/math/curve1d/quartic_polynomial_curve1d.h"

#include "absl/strings/str_cat.h"
#include "absl/strings/str_join.h"

#include "cyber/common/log.h"

namespace apollo {
namespace planning {
//四次多项式1维参数曲线类构造函数
//输入参数 初始状态3维(s,s',s''或d,d',d'')， 终端状态2维(调用时只输入了d',d'')？这样的话就5个方程了就可以确定一条四次多项式，只适用于终点位置不固定类似巡航这种场景
QuarticPolynomialCurve1d::QuarticPolynomialCurve1d(
    const std::array<double, 3>& start, const std::array<double, 2>& end,
    const double param)
    : QuarticPolynomialCurve1d(start[0], start[1], start[2], end[0], end[1],
                               param) {}

//这个构造函数就是被上面的构造函数调用，就是根据
//param=0 x0,dx0,ddx0
//param=paramlength dx1 ddx1 5个方程求出四次多项式的五个系数，调用函数ComputeCoefficients来求解多项式的系数
QuarticPolynomialCurve1d::QuarticPolynomialCurve1d(
    const double x0, const double dx0, const double ddx0, const double dx1,
    const double ddx1, const double param) {
  param_ = param;
  start_condition_[0] = x0;
  start_condition_[1] = dx0;
  start_condition_[2] = ddx0;
  end_condition_[0] = dx1;
  end_condition_[1] = ddx1;
  ComputeCoefficients(x0, dx0, ddx0, dx1, ddx1, param);
}

//这个构造函数？输入一个四次多项式，然后把其系数和参数的总长度拷贝给类成员？
QuarticPolynomialCurve1d::QuarticPolynomialCurve1d(
    const QuarticPolynomialCurve1d& other) {
  param_ = other.param_;
  coef_ = other.coef_;
}

//多项式插值，具体原理很简单就不详述了，就是多项式求导，可以插值出指定参数处四次多项式指定阶导数对应的值。
double QuarticPolynomialCurve1d::Evaluate(const std::uint32_t order,
                                          const double p) const {
  switch (order) {
    case 0: {
      return (((coef_[4] * p + coef_[3]) * p + coef_[2]) * p + coef_[1]) * p +
             coef_[0];
    }
    case 1: {
      return ((4.0 * coef_[4] * p + 3.0 * coef_[3]) * p + 2.0 * coef_[2]) * p +
             coef_[1];
    }
    case 2: {
      return (12.0 * coef_[4] * p + 6.0 * coef_[3]) * p + 2.0 * coef_[2];
    }
    case 3: {
      return 24.0 * coef_[4] * p + 6.0 * coef_[3];
    }
    case 4: {
      return 24.0 * coef_[4];
    }
    default:
      return 0.0;
  }
}

//用终端的一阶条件求解四次多项式
//param=0时，x=x0,dx=dx0,ddx=ddx0
//param=paramlength(这里就是p)时， x=x1 dx=dx1
//5个条件求解四次多项式各个系数存入类成员array数组coef_中
QuarticPolynomialCurve1d& QuarticPolynomialCurve1d::FitWithEndPointFirstOrder(
    const double x0, const double dx0, const double ddx0, const double x1,
    const double dx1, const double p) {
  CHECK_GT(p, 0.0);

  param_ = p;

  coef_[0] = x0;

  coef_[1] = dx0;

  coef_[2] = 0.5 * ddx0;

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

  double b0 = x1 - coef_[0] - coef_[1] * p - coef_[2] * p2;
  double b1 = dx1 - dx0 - ddx0 * p;

  coef_[4] = (b1 * p - 3 * b0) / p4;
  coef_[3] = (4 * b0 - b1 * p) / p3;
  return *this;
}

//用终端的二阶条件求解四次多项式
//param=0时，x=x0,dx=dx0
//param=paramlength(这里就是p)时， x=x1 dx=dx1，ddx=ddx1
//5个条件求解四次多项式各个系数存入类成员array数组coef_中
QuarticPolynomialCurve1d& QuarticPolynomialCurve1d::FitWithEndPointSecondOrder(
    const double x0, const double dx0, const double x1, const double dx1,
    const double ddx1, const double p) {
  CHECK_GT(p, 0.0);

  param_ = p;

  coef_[0] = x0;

  coef_[1] = dx0;

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

  double b0 = x1 - coef_[0] - coef_[1] * p;
  double b1 = dx1 - coef_[1];
  double c1 = b1 * p;
  double c2 = ddx1 * p2;

  coef_[2] = (0.5 * c2 - 3 * c1 + 6 * b0) / p2;
  coef_[3] = (-c2 + 5 * c1 - 8 * b0) / p3;
  coef_[4] = (0.5 * c2 - 2 * c1 + 3 * b0) / p4;

  return *this;
}

//根据输入的3次多项式积分得到四次多项式
//输入参数3次多项式，还有四次多项式param=0时的x初值，最后的积分得到的四次多项式放入类成员coef_里
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;
}

//从五次多项式求导得到四次多项似乎，输入的就是一个五次多项式，最后求导的四次多项式放入类成员coef_里
QuarticPolynomialCurve1d& QuarticPolynomialCurve1d::DerivedFromQuinticCurve(
    const PolynomialCurve1d& other) {
  CHECK_EQ(other.Order(), 5U);
  param_ = other.ParamLength();
  for (size_t i = 1; i < 6; ++i) {
    coef_[i - 1] = other.Coef(i) * static_cast<double>(i);
  }
  return *this;
}

//param=0时，x=x0,dx=dx0,ddx=ddx0
//param=paramlength(这里就是p)时， dx=dx1，ddx=ddx1
//5个条件求解四次多项式各个系数存入类成员array数组coef_中
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);
}

//利用absl库将四次多项式的系数以及参数总长度转化成字符串并连接起来变成一个大的string并返回，便于调试
std::string QuarticPolynomialCurve1d::ToString() const {
  return absl::StrCat(absl::StrJoin(coef_, "\t"), param_, "\n");
}

//获取四次多项式指定幂次项的系数(例如4次项系数，3次项系数...)
double QuarticPolynomialCurve1d::Coef(const size_t order) const {
  CHECK_GT(5U, order);
  return coef_[order];
}

}  // namespace planning
}  // namespace apollo