自动驾驶-决策规划算法十：曲线插值算法之五次多项式(C++)

自动驾驶-决策规划算法十：曲线插值算法之五次多项式(C++)

摘要：

本文介绍无人驾驶决策规划算法中，曲线插值算法中的五次多项式。

语言用的是C++，图形基于easyX库输出。

如有错误或者遗漏之处，欢迎指出。

//由于本文关注的是曲线插值算法，所以并没有对航向角进行规划，也没有加上控制，所以在动态展示时只用平移来示意车辆沿着轨迹行驶。

//代码中有很多调试用的代码，也一并保留并注释掉，记录了我调试问题和测试一些代码时的过程；

//限于时间关系，公式就不用公式编辑器了，就直接输入了，大家看得懂就行；

---

附赠自动驾驶学习资料和量产经验：链接

基本概念：

/*

全局规划，一般也叫路线规划（route planning），只考虑路径，不考虑速度；

局部规划，一般也叫运动规划（motion planning）或轨迹规划（trajectory planning），要考虑路径和速度；

局部规划通常有两种分类方式：

一是路径与速度解耦，分为局部路径规划（path planning）和速度规划（speed planning），通常是在路径的规划点上赋上速度值；

二是横向与纵向解耦，分为横向规划（path planning）和纵向规划（path planning）；

*/

/*

曲线插值算法:

车辆在某些特定约束下（安全、快速、路线短、体感等），进行路径的曲线拟合，常见的有多项式曲线，双圆弧段曲线，正统函数曲线，贝塞尔曲线，B样条曲线等；

核心思想：

基于预先选定的曲线类型，根据车辆期望达到的状态（位置、航向角、速度、加速度等），将这些期望值作为边界条件代入曲线方程进行求解，求出曲线相关系数；

所有系数计算出后，曲线轨迹规划完成；

*/

/*

多项式曲线：通常有三项，五项，七项；

一般项数是奇数，边界条件一般包括车辆的起点和终点状态，

因此两倍的车辆状态导致有唯一解的方程系数为偶数，偶数个系数也就对应奇数个项数；只有这样方程才会有唯一解；

三次多项式，最多能确定每个期望点的位置和速度；

x(t) = a0 + a1t + a2t^2 + a3t^3; //纵向位置

y(t) = b0 + b1t + b2t^2 + b3t^3; //横向位置

五次多项式，最多能确定每个期望点的位置、速度和加速度；

x(t) = a0 + a1t + a2t^2 + a3t^3 + a4t^4 + a5t^5; //纵向位置

y(t) = b0 + b1t + b2t^2 + b3t^3 + b4t^4 + b5t^5; //横向位置

七次多项式，最多能确定每个期望点的位置、速度、加速度和加加速度（冲击度，jerk）；

x(t) = a0 + a1t + a2t^2 + a3t^3 + a4t^4 + a5t^5 + a6t^6 + a7t^7; //纵向位置

y(t) = b0 + b1t + b2t^2 + b3t^3 + b4t^4 + b5t^5 + b6t^6 + b7t^7; //横向位置

*/

/*

五次多项式可以保证加速度连续，所以较常用。以五次多项式为例：

设起点时间为t0，对应的位置、速度、加速度分别为(x(t0), y(t0)), (x’(t0), y’(t0)), (x’‘(t0), y’'(t0));

设终点时间为t1，对应的位置、速度、加速度分别为(x(t1), y(t1)), (x’(t1), y’(t1)), (x’‘(t1), y’'(t1));

以上起点和终点的时间、位置、速度、加速度都为已知量：

终点时间为t1减去起点时间t0表示车辆预期到达终点位置时所花的时间；

终点的位置、速度、加速度表示车辆预期到达终点位置时的运动状；

x(t0) = a0 + t0a1 + t0^2 a2 + t0^3 a3 + t0^4 a4 + t0^5 a5;

y(t0) = b0 + t0b1 + t0^2 b2 + t0^3 b3 + t0^4 b4 + t0^5 b5;

x’(t0) = a1 + 2t0a2 + 3t0^2 a3 + 4t0^3 a4 + 5t0^4 a5;

y’(t0) = b1 + 2t0b2 + 3t0^2 b3 + 4t0^3 b4 + 5t0^4 b5;

x’'(t0) = 2a0 + 6t0a3 + 12t0^2 a4 + 20t0^3 a5;

y’'(t0) = 2b0 + 6t0b3 + 12t0^2 b4 + 20t0^3 b5;

x(t1) = a0 + t1a1 + t1^2 a2 + t1^3 a3 + t1^4 a4 + t1^5 a5;

y(t1) = b0 + t1b1 + t1^2 b2 + t1^3 b3 + t1^4 b4 + t1^5 b5;

x’(t1) = a1 + 2t1a2 + 3t1^2 a3 + 4t1^3 a4 + 5t1^4 a5;

y’(t1) = b1 + 2t1b2 + 3t1^2 b3 + 4t1^3 b4 + 5t1^4 b5;

x’'(t1) = 2a0 + 6t1a3 + 12t1^2 a4 + 20t1^3 a5;

y’'(t1) = 2b0 + 6t1b3 + 12t1^2 b4 + 20t1^3 b5;

12个方程，12个未知数（a0 - a5, b0 - b5），可以求出唯一的解；

把上述12个方程用矩阵表达为：

也就是X = T x A; Y = T x B;

这样A = T^(-1) X; B = T^(-1) Y;

A矩阵和B矩阵就可以求出，这样这条曲线就确定了；

曲线上每个点的导数就表示车辆经过该点时的速度；

多项式轨迹规划是路径与速度耦合的结果；

*/

/*

除了多项式曲线，还有双圆弧段曲线，不过通常用于泊车、三点式调头等需要中途停车场景，因为曲率会突变，只有停车后原地打方向才允许发生；

*/

/*

注意，五次多项式是指横向/纵向位置关于时间t的五次多项式，而不是横向位置y关于纵向位置x的五次多项式！

标准的多项式曲线只能在两个点确定的情况下求出一条确定的曲线，如果中间要有变化或者有其他约束，则需要修改矩阵参数；

五次多项式