apollo planning math curve1d quintic_polynomial_curve1d

1. 描述

dp path 的时候点与点之间通过五次多项式连接,

apollo 中有源代码就不用自己造轮子了,来看一下具体的使用

2. 代码

源代码在这里:

modules/planning/math/curve1d/quintic_polynomial_curve1d.cc
modules/planning/math/curve1d/quintic_polynomial_curve1d.h

直接用就好了,

看一下测试的代码

#include <iostream>
#include <vector>
#include <string>
#include <queue>
#include <memory>
#include <algorithm>
#include <cmath>
#include <array>

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

#include <gtest/gtest.h>

using namespace std;

namespace apollo {
namespace planning {

TEST(QuinticPolynomialCurve1dTest, basic_test) {
  // double x0 = 0.0;
  // double dx0 = 1.0;
  // double ddx0 = 0.8;

  // double x1 = 10.0;
  // double dx1 = 5.0;
  // double ddx1 = 0.0;

  // double t = 8.0;

  //构造
  //t0时刻位置120,速度,加速度都是0
  //t1时刻位置60,速度,加速度都是0
  double x0 = 120.0;
  double dx0 = 0.0;
  double ddx0 = 0.0;

  double x1 = 60.0;
  double dx1 = 0.0;
  double ddx1 = 0.0;

  double t = 1.0;

  QuinticPolynomialCurve1d curve(x0, dx0, ddx0, x1, dx1, ddx1, t);
  auto e_x0 = curve.Evaluate(0, 0.0);
  auto e_dx0 = curve.Evaluate(1, 0.0);
  auto e_ddx0 = curve.Evaluate(2, 0.0);

  auto e_x1 = curve.Evaluate(0, t);
  auto e_dx1 = curve.Evaluate(1, t);
  auto e_ddx1 = curve.Evaluate(2, t);

  auto e_t = curve.ParamLength();

  EXPECT_NEAR(x0, e_x0, 1.0e-6);
  EXPECT_NEAR(dx0, e_dx0, 1.0e-6);
  EXPECT_NEAR(ddx0, e_ddx0, 1.0e-6);

  EXPECT_NEAR(x1, e_x1, 1.0e-6);
  EXPECT_NEAR(dx1, e_dx1, 1.0e-6);
  EXPECT_NEAR(ddx1, e_ddx1, 1.0e-6);

  EXPECT_NEAR(t, e_t, 1.0e-6);


  for(int i = 0; i <= 5; i++){
    cout << curve.Coef(i) << ",";
  }
  cout << endl;
}

TEST(QuinticPolynomialCurve1dTest, IntegratedFromQuarticCurve) {
  QuarticPolynomialCurve1d quartic_curve(2, 1, 4, 3, 2, 4);
  QuinticPolynomialCurve1d quintic_curve;
  quintic_curve.IntegratedFromQuarticCurve(quartic_curve, 1);
  for (double value = 0.0; value < 4.1; value += 0.1) {
    EXPECT_NEAR(quartic_curve.Evaluate(0, value),
                quintic_curve.Evaluate(1, value), 1e-8);
    EXPECT_NEAR(quartic_curve.Evaluate(1, value),
                quintic_curve.Evaluate(2, value), 1e-8);
    EXPECT_NEAR(quartic_curve.Evaluate(2, value),
                quintic_curve.Evaluate(3, value), 1e-8);
    EXPECT_NEAR(quartic_curve.Evaluate(3, value),
                quintic_curve.Evaluate(4, value), 1e-8);
  }
}
}  // namespace planning
}  // namespace apollo

• 结果:

seivl@seivl-Default-string:~/me/代码/规划/bbox_test/build_$ ./bbox_test 
Running main() from /home/seivl/gtest/googletest/googletest/src/gtest_main.cc
[==========] Running 2 tests from 1 test suite.
[----------] Global test environment set-up.
[----------] 2 tests from QuinticPolynomialCurve1dTest
[ RUN      ] QuinticPolynomialCurve1dTest.basic_test
120,0,0,-600,900,-360,
[       OK ] QuinticPolynomialCurve1dTest.basic_test (0 ms)
[ RUN      ] QuinticPolynomialCurve1dTest.IntegratedFromQuarticCurve
[       OK ] QuinticPolynomialCurve1dTest.IntegratedFromQuarticCurve (0 ms)
[----------] 2 tests from QuinticPolynomialCurve1dTest (0 ms total)

[----------] Global test environment tear-down
[==========] 2 tests from 1 test suite ran. (0 ms total)
[  PASSED  ] 2 tests.

可以看到五次多项式的系数分别是:

120,0,0,-600,900,-360,

• 验证

用 matlab 来算一下:

theta_s = 120; theta_f = 60; tf = 1;
theta_fd = 0; theta_fdd = 0; theta_sd = 0; theta_sdd = 0;
a0_5 = theta_s; a1_5 = theta_sd; a2_5 = theta_sdd / 2;
a3_5 = (20*theta_f - 20*theta_s - (8*theta_fd + 12*theta_sd)*tf - (3*theta_sdd - theta_fdd)*tf^2) / (2*tf^3);
a4_5 = (30*theta_s - 30*theta_f + (14*theta_fd + 16*theta_sd)*tf + (3*theta_sdd - 2*theta_fdd)*tf^2) / (2*tf^4);
a5_5 = (12*theta_f - 12*theta_s - (6*theta_fd + 6*theta_sd)*tf - (theta_sdd - theta_fdd)*tf^2) / (2*tf^5);

结果：

可以看到没有问题