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);
结果:
可以看到没有问题