#include <quintic_spiral_path_with_derivation.h>

类 apollo::planning::QuinticSpiralPathWithDerivation< N > 继承关系图:

apollo::planning::QuinticSpiralPathWithDerivation< N > 的协作图:

Public 成员函数
	QuinticSpiralPathWithDerivation ()=default

	QuinticSpiralPathWithDerivation (const std::array< double, 3 > &start, const std::array< double, 3 > &end, const double delta_s)

	QuinticSpiralPathWithDerivation (const double theta0, const double kappa0, const double dkappa0, const double theta1, const double kappa1, const double dkappa1, const double delta_s)

virtual	~QuinticSpiralPathWithDerivation ()=default

double	evaluate (const size_t order, const size_t i, const size_t n)

double	ComputeCartesianDeviationX () const

double	ComputeCartesianDeviationY () const

std::pair< double, double >	DeriveCartesianDeviation (const size_t param_index)

double	DeriveKappaDerivative (const size_t param_index, const int i, const int n)

double	DeriveDKappaDerivative (const size_t param_index, const int i, const int n)

double	ComputeCartesianDeviationX (const double s) const

double	ComputeCartesianDeviationY (const double s) const

Public 成员函数继承自 apollo::planning::QuinticPolynomialCurve1d
	QuinticPolynomialCurve1d ()=default

	QuinticPolynomialCurve1d (const std::array< double, 3 > &start, const std::array< double, 3 > &end, const double param)

	QuinticPolynomialCurve1d (const double x0, const double dx0, const double ddx0, const double x1, const double dx1, const double ddx1, const double param)

	QuinticPolynomialCurve1d (const QuinticPolynomialCurve1d &other)

void	SetParam (const double x0, const double dx0, const double ddx0, const double x1, const double dx1, const double ddx1, const double param)

void	IntegratedFromQuarticCurve (const PolynomialCurve1d &other, const double init_value)

virtual	~QuinticPolynomialCurve1d ()=default

double	Evaluate (const std::uint32_t order, const double p) const override

double	ParamLength () const override

std::string	ToString () const override

double	Coef (const size_t order) const override

size_t	Order () const override

Public 成员函数继承自 apollo::planning::PolynomialCurve1d
	PolynomialCurve1d ()=default

virtual	~PolynomialCurve1d ()=default

Public 成员函数继承自 apollo::planning::Curve1d
	Curve1d ()=default

virtual	~Curve1d ()=default

静态 Public 属性
static const size_t	THETA0 = 0

static const size_t	KAPPA0 = 1

static const size_t	DKAPPA0 = 2

static const size_t	THETA1 = 3

static const size_t	KAPPA1 = 4

static const size_t	DKAPPA1 = 5

static const size_t	DELTA_S = 6

static const size_t	INDEX_MAX = 7

额外继承的成员函数
Protected 成员函数继承自 apollo::planning::QuinticPolynomialCurve1d
void	ComputeCoefficients (const double x0, const double dx0, const double ddx0, const double x1, const double dx1, const double ddx1, const double param)

Protected 属性继承自 apollo::planning::QuinticPolynomialCurve1d
std::array< double, 6 >	coef_ {{0.0, 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, 3 >	end_condition_ {{0.0, 0.0, 0.0}}

Protected 属性继承自 apollo::planning::PolynomialCurve1d
double	param_ = 0.0

详细描述

template<size_t N>
class apollo::planning::QuinticSpiralPathWithDerivation< N >

在文件 quintic_spiral_path_with_derivation.h 第 34 行定义.

构造及析构函数说明

◆ QuinticSpiralPathWithDerivation() [1/3]

template<size_t N>

apollo::planning::QuinticSpiralPathWithDerivation< N >::QuinticSpiralPathWithDerivation ( )

default

◆ QuinticSpiralPathWithDerivation() [2/3]

template<size_t N>

apollo::planning::QuinticSpiralPathWithDerivation< N >::QuinticSpiralPathWithDerivation	(	const std::array< double, 3 > &	start,
		const std::array< double, 3 > &	end,
		const double	delta_s
	)

在文件 quintic_spiral_path_with_derivation.h 第 191 行定义.

194 : QuinticSpiralPathWithDerivation<N>(start[0], start[1], start[2], end[0],

195 end[1], end[2], delta_s) {}

◆ QuinticSpiralPathWithDerivation() [3/3]

template<size_t N>

apollo::planning::QuinticSpiralPathWithDerivation< N >::QuinticSpiralPathWithDerivation	(	const double	theta0,
		const double	kappa0,
		const double	dkappa0,
		const double	theta1,
		const double	kappa1,
		const double	dkappa1,
		const double	delta_s
	)

在文件 quintic_spiral_path_with_derivation.h 第 116 行定义.

    : QuinticPolynomialCurve1d(x0, dx0, ddx0, x1, dx1, ddx1, s) {
  ACHECK(s > 0.0);
 
  auto gauss_points = common::math::GetGaussLegendrePoints<N>();
  gauss_points_ = gauss_points.first;
  gauss_point_weights_ = gauss_points.second;
 
  dx_ = ComputeCartesianDeviationX(s);
 
  dy_ = ComputeCartesianDeviationY(s);
 
  double s2 = s * s;
  double s3 = s2 * s;
  double s4 = s3 * s;
  double s5 = s2 * s3;
  double s6 = s3 * s3;
 
  for (size_t i = 0; i < 6; ++i) {
    for (size_t j = 0; j < 7; ++j) {
      coef_deriv_[i][j] = 0.0;
    }
  }
  // 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 / s5;
  coef_deriv_[5][1] = -3.0 / s4;
  coef_deriv_[5][2] = -0.5 / s3;
  coef_deriv_[5][3] = 6.0 / s5;
  coef_deriv_[5][4] = -3.0 / s4;
  coef_deriv_[5][5] = 0.5 / s3;
  coef_deriv_[5][6] = 30.0 * x0 / s6 + 12.0 * dx0 / s5 + 1.5 * ddx0 / s4 -
                      30.0 * x1 / s6 + 12.0 * dx1 / s5 - 1.5 * ddx1 / s4;
 
  // 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 / s4;
  coef_deriv_[4][1] = 8.0 / s3;
  coef_deriv_[4][2] = 1.5 / s2;
  coef_deriv_[4][3] = -15.0 / s4;
  coef_deriv_[4][4] = 7.0 / s3;
  coef_deriv_[4][5] = -1.0 / s2;
  coef_deriv_[4][6] = -60.0 * x0 / s5 - 24.0 * dx0 / s4 - 3.0 * ddx0 / s3 +
                      60.0 * x1 / s5 - 21.0 * dx1 / s4 + 2.0 * ddx1 / s3;
 
  // 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 / s3;
  coef_deriv_[3][1] = -6.0 / s2;
  coef_deriv_[3][2] = -1.5 / s;
  coef_deriv_[3][3] = 10.0 / s3;
  coef_deriv_[3][4] = -4.0 / s2;
  coef_deriv_[3][5] = 0.5 / s;
  coef_deriv_[3][6] = 30.0 * x0 / s4 + 12.0 * dx0 / s3 + 1.5 * ddx0 / s2 -
                      30.0 * x1 / s4 + 8.0 * dx1 / s3 - 0.5 * ddx1 / s2;
 
  // derive d
  // double d = 0.5 * ddx0;
  coef_deriv_[2][2] = 0.5;
 
  // derive e
  // double e = dx0;
  coef_deriv_[1][1] = 1.0;
 
  // derive f
  // double f = x0;
  coef_deriv_[0][0] = 1.0;
}

◆ ~QuinticSpiralPathWithDerivation()

template<size_t N>

virtual apollo::planning::QuinticSpiralPathWithDerivation< N >::~QuinticSpiralPathWithDerivation ( )

virtualdefault

成员函数说明

◆ ComputeCartesianDeviationX() [1/2]

template<size_t N>

double apollo::planning::QuinticSpiralPathWithDerivation< N >::ComputeCartesianDeviationX ( ) const

inline

在文件 quintic_spiral_path_with_derivation.h 第 51 行定义.

51{ return dx_; }

◆ ComputeCartesianDeviationX() [2/2]

template<size_t N>

double apollo::planning::QuinticSpiralPathWithDerivation< N >::ComputeCartesianDeviationX ( const double s ) const

inline

在文件 quintic_spiral_path_with_derivation.h 第 73 行定义.

                                                          {
    auto cos_theta = [this](const double s) {
      const auto a = Evaluate(0, s);
      return std::cos(a);
    };
    return common::math::IntegrateByGaussLegendre<N>(cos_theta, 0.0, s);
  }

◆ ComputeCartesianDeviationY() [1/2]

template<size_t N>

double apollo::planning::QuinticSpiralPathWithDerivation< N >::ComputeCartesianDeviationY ( ) const

inline

在文件 quintic_spiral_path_with_derivation.h 第 53 行定义.

53{ return dy_; }

◆ ComputeCartesianDeviationY() [2/2]

template<size_t N>

double apollo::planning::QuinticSpiralPathWithDerivation< N >::ComputeCartesianDeviationY ( const double s ) const

inline

在文件 quintic_spiral_path_with_derivation.h 第 81 行定义.

                                                          {
    auto sin_theta = [this](const double s) {
      const auto a = Evaluate(0, s);
      return std::sin(a);
    };
    return common::math::IntegrateByGaussLegendre<N>(sin_theta, 0.0, s);
  }

◆ DeriveCartesianDeviation()

template<size_t N>

std::pair< double, double > apollo::planning::QuinticSpiralPathWithDerivation< N >::DeriveCartesianDeviation ( const size_t param_index )

在文件 quintic_spiral_path_with_derivation.h 第 213 行定义.

                              {
  if (cache_cartesian_deriv_.find(param_index) !=
      cache_cartesian_deriv_.end()) {
    return cache_cartesian_deriv_[param_index];
  }
 
  const auto& g = gauss_points_;
  const auto& w = gauss_point_weights_;
 
  std::pair<double, double> cartesian_deviation = {0.0, 0.0};
  if (param_index != DELTA_S) {
    for (size_t i = 0; i < N; ++i) {
      double r = 0.5 * g[i] + 0.5;
      cartesian_deviation.first += w[i] * DeriveCosTheta(param_index, r);
      cartesian_deviation.second += w[i] * DeriveSinTheta(param_index, r);
    }
 
    cartesian_deviation.first *= param_ * 0.5;
    cartesian_deviation.second *= param_ * 0.5;
  } else {
    for (size_t i = 0; i < N; ++i) {
      double r = 0.5 * g[i] + 0.5;
      cartesian_deviation.first += w[i] * DeriveCosTheta(param_index, r);
      cartesian_deviation.second += w[i] * DeriveSinTheta(param_index, r);
    }
 
    cartesian_deviation.first *= param_ * 0.5;
    cartesian_deviation.second *= param_ * 0.5;
 
    for (size_t i = 0; i < N; ++i) {
      double r = 0.5 * g[i] + 0.5;
      auto theta = Evaluate(0, r * param_);
 
      cartesian_deviation.first += 0.5 * w[i] * std::cos(theta);
      cartesian_deviation.second += 0.5 * w[i] * std::sin(theta);
    }
  }
  cache_cartesian_deriv_[param_index] = cartesian_deviation;
  return cartesian_deviation;
}

◆ DeriveDKappaDerivative()

template<size_t N>

double apollo::planning::QuinticSpiralPathWithDerivation< N >::DeriveDKappaDerivative	(	const size_t	param_index,
		const int	i,
		const int	n
	)

在文件 quintic_spiral_path_with_derivation.h 第 285 行定义.

                                                        {
  auto key = param_index * INDEX_MAX + i;
  if (cache_dkappa_deriv_.find(key) != cache_dkappa_deriv_.end()) {
    return cache_dkappa_deriv_[key];
  }
 
  auto r = static_cast<double>(i) / static_cast<double>(n);
  double s = param_ * r;
  double s2 = s * s;
  double s3 = s2 * s;
 
  double derivative = 20.0 * coef_deriv_[5][param_index] * s3 +
                      12.0 * coef_deriv_[4][param_index] * s2 +
                      6.0 * coef_deriv_[3][param_index] * s +
                      2.0 * coef_deriv_[2][param_index];
 
  if (param_index == DELTA_S) {
    derivative +=
        60.0 * coef_[5] * s2 * r + 24.0 * coef_[4] * s * r + 6.0 * coef_[3] * r;
  }
 
  cache_dkappa_deriv_[key] = derivative;
  return derivative;
}

◆ DeriveKappaDerivative()

template<size_t N>

double apollo::planning::QuinticSpiralPathWithDerivation< N >::DeriveKappaDerivative	(	const size_t	param_index,
		const int	i,
		const int	n
	)

在文件 quintic_spiral_path_with_derivation.h 第 256 行定义.

                                                        {
  auto key = param_index * INDEX_MAX + i;
  if (cache_kappa_deriv_.find(key) != cache_kappa_deriv_.end()) {
    return cache_kappa_deriv_[key];
  }
 
  auto r = static_cast<double>(i) / static_cast<double>(n);
  double s = param_ * r;
  double s2 = s * s;
  double s3 = s2 * s;
  double s4 = s2 * s2;
 
  double derivative = 5.0 * coef_deriv_[5][param_index] * s4 +
                      4.0 * coef_deriv_[4][param_index] * s3 +
                      3.0 * coef_deriv_[3][param_index] * s2 +
                      2.0 * coef_deriv_[2][param_index] * s +
                      coef_deriv_[1][param_index];
 
  if (param_index == DELTA_S) {
    derivative += 20.0 * coef_[5] * s3 * r + 12.0 * coef_[4] * s2 * r +
                  6.0 * coef_[3] * s * r + 2.0 * coef_[2] * r;
  }
 
  cache_kappa_deriv_[key] = derivative;
  return derivative;
}

◆ evaluate()

template<size_t N>

double apollo::planning::QuinticSpiralPathWithDerivation< N >::evaluate	(	const size_t	order,
		const size_t	i,
		const size_t	n
	)

在文件 quintic_spiral_path_with_derivation.h 第 198 行定义.

                                                                    {
  auto key = order * 100 + n * 10 + i;
  if (cache_evaluate_.find(key) != cache_evaluate_.end()) {
    return cache_evaluate_[key];
  }
  auto res =
      Evaluate(order, static_cast<double>(i) / static_cast<double>(n) * param_);
  cache_evaluate_[key] = res;
  return res;
}