自动驾驶-决策规划算法十:曲线插值算法之五次多项式(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的五次多项式!
标准的多项式曲线只能在两个点确定的情况下求出一条确定的曲线,如果中间要有变化或者有其他约束,则需要修改矩阵参数;
五次多项式