b站老王 自动驾驶决策规划学习记录（二）

自动驾驶决策规划之五次多项式

上一讲：b站老王 自动驾驶决策规划学习记录（一）

接着上一讲学习记录b站老王对自动驾驶规划系列的讲解
参考视频：自动驾驶决策规划算法第一章第一节 细说五次多项式

0 前言

有必要理解一下曲线插值法，对理解五次多项式很有帮助。
常用三次多项式曲线或五次多项式曲线规划无人车运动轨迹。多项式曲线一般为奇数，这是因为他的边界条件所导致。

边界条件一般包含车辆的初始状态和终止状态，所以两个状态有偶数个系数，也造成了方程是奇数多项式。

三次多项式：位置、速度（2 x 2 = 4）
五次多项式：位置、速度、加速度（3 x 2 = 6）
七次多项式：位置、速度、加速度、加加速度（4 x 2 = 8）

非常容易混淆的一点：
五次多项式曲线做路径规划时，y不是关于x的曲线，而是y和x都是关于t的曲线

1. 位置
   $$\begin{gathered} x(t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5 \\ y(t)=b_0+b_{1}t+b_2{t}^2+b_3{t}^3+b_4{t}^4+b_5{t}^5 \end{gathered}$$
2. 速度
   $$\begin{gathered} \dot{x}(t)=a_1+2a_{2}t+3a_3{t}^2+4a_4{t}^3+5a_5{t}^4 \\ \dot{y}(t)=b_1+2b_{2}t+3b_3{t}^2+4b_4{t}^3+5b_5{t}^4 \end{gathered}$$
3. 加速度
   $$\begin{gathered} \ddot{x}(t)=2a_2+6a_{3}t+12a_4{t}^2+20a_5{t}^3 \\ \ddot{y}(t)=2b_2+6b_{3}t+12b_4{t}^2+20b_5{t}^3 \end{gathered}$$

化为矩阵形式。
$$X=\left[\begin{array}{c}x(0)\\ \dot{x(0)}\\ \ddot{x(0)}\\ x(T)\\ \dot{x(T)}\\ \ddot{x(T)}\end{array}\right]=\left[\begin{array}{cccccc}{t}_0^5& t_0^4& t_0^3& t_0^2& t_0& 1\\ 5t_0^4& 4t_0^3& 3t_0^2& 2t_0& 1& 0\\ 20t_0^3& 12t_0^2& 6t_0& 2& 0& 0\\ t_1^5& t_1^4& t_1^3& t_1^2& t_1& 1\\ 5t_1^4& 4t_1^3& 3t_1^2& 2t_1& 1& 0\\ 20t_1^3& 12t_1^2& 6t_1& 2& 0& 0\end{array}\right]\left[\begin{array}{c}{a}_5\\ a_4\\ a_3\\ a_2\\ a_1\\ a_0\end{array}\right]=TXA$$
$$Y=\left[\begin{array}{c}y(0)\\ \dot{y(0)}\\ \ddot{y(0)}\\ y(T)\\ \dot{y(T)}\\ \ddot{y(T)}\end{array}\right]=\left[\begin{array}{cccccc}{t}_0^5& t_0^4& t_0^3& t_0^2& t_0& 1\\ 5t_0^4& 4t_0^3& 3t_0^2& 2t_0& 1& 0\\ 20t_0^3& 12t_0^2& 6t_0& 2& 0& 0\\ t_1^5& t_1^4& t_1^3& t_1^2& t_1& 1\\ 5t_1^4& 4t_1^3& 3t_1^2& 2t_1& 1& 0\\ 20t_1^3& 12t_1^2& 6t_1& 2& 0& 0\end{array}\right]\left[\begin{array}{c}{a}_5\\ b_4\\ b_3\\ b_2\\ b_1\\ b_0\end{array}\right]=TXB$$
在等式中，$X$矩阵和$Y$矩阵的数值我们都是已知的，$x(0),\dot{x(0)},\ddot{x(0)}$分别表示初始位置的横向坐标、速度、加速度；$t_0,t_1$也已知，表示初始位置和终点位置的时刻，$X，Y，T$已知，就可以求出$A$和$B$。

之后再设置时间间隔$△t$带入$T$矩阵，就能利用A矩阵和T矩阵求出初始位置和终点位置之间的位置，速度和加速度。

1 五次多项式

前面的引言部分是不是已经让你对五次多项式的使用有一个粗略的印象了呢

五次多项式：规划论文里的常客

车辆运动规划中。一个非常重要的指标就是舒适性，在物理中，衡量舒适性的物理量为跃度，Jerk。

$Jerk$:

$Jerk=\frac{da}{dt}$ , $a$ 为加速度

即$Jerk$是加速度的导数，$Jerk$的绝对值越小，$a$的变化越平缓，意味着越舒适。

设有一个质点的轨迹$s=f(t)$，则$Jerk=\frac{d^{3}f}{d{t}^3}$，若在$[0,T]$区间的时间中，$Jerk$的绝对值都比较小，那就意味着在$[0,T]$内规划的轨迹是比较舒适的。

将他变成数学问题：

若有一个函数 $s=f(t)$，那么什么样的$f(t)$使得在$[0,T]$$Jerk$的绝对值变化平缓？
因为绝对值处理频繁，一般改为平方

find $f(t)$ 使得${\int }_0^T(\frac{d^{3}f}{d{t}^3}{)}^{2}dt$最小

显然，积分${\int }_0^T(\frac{d^{3}f}{d{t}^3}{)}^{2}dt$是一个关于$f(t)$的泛函，积分的值取决于$f(t)$在$[0,T]$上的整体形状。

那么${\int }_0^T(\frac{d^{3}f}{d{t}^3}{)}^{2}dt$取极小值的$f(t)$是什么？

显然，当f(t)为二次或二次以下函数时，${\int }_0^T(\frac{d^{3}f}{d{t}^3}{)}^{2}dt=0$最小。
所以，若要让$Jerk$在$[0,T]$上绝对值最小，$f(t)$应取二次或二次以下函数。
但是真实情况远比想象的复杂
真实的$S=f(t)$往往带约束
$$\begin{gathered} S(0)=S_0，S(T)=S_n \\ \dot{S}(0)=v_0，\dot{S}(T)=v_n \\ \ddot{S}(0)=a_0，\ddot{S}(T)=a_n \end{gathered}$$

6个边界条件，$y=a{x}^2+bx+c$无法满足六个边界条件。

在真实情况下，往往要求带边界约束的泛函.

find $f(t)$ 使得${\int }_0^T(\frac{d^{3}f}{d{t}^3}{)}^{2}dt$最小
subject to :
$$\begin{gathered} S(0)=S_0，S(T)=S_n \\ \dot{S}(0)=v_0，\dot{S}(T)=v_n \\ \ddot{S}(0)=a_0，\ddot{S}(T)=a_n \end{gathered}$$

那么满足带约束的泛函${\int }_0^T(\frac{d^{3}f}{d{t}^3}{)}^{2}dt$极值问题的解是什么？
五次多项式

显然$f(t)$只可能是在$[0,T]$上是有界连续函数，因为无论是无界函数，还是有界间断函数都会使$Jerk$出现无穷大。
不妨设$S=f(0)+\dot{f}(0)t+\ddot{f}(0)t^2+...$
代入边界条件：
$S(0)=S_0=>f(0)=S_0$
$\dot{S}(0)=v_0=>\dot{f}(0)=v_0$
$\ddot{S}(0)=a_0=>\ddot{f}(0)=a_0$
所以$f(t)=S_0+v_{0}t+\frac{1}{2}{a}_0{t}^2+\frac{{f(0)}^{(3)}}{6}{t}^3+...$
因为$Jerk={f(t)}^{(3)}$，所以$S_0,v_0,a_0$的值不影响$Jerk$。

其次边界条件
$$\begin{gathered} S(0)=S_0，S(T)=S_n \\ \dot{S}(0)=v_0，\dot{S}(T)=v_n \\ \ddot{S}(0)=a_0，\ddot{S}(T)=a_n \end{gathered}$$
变成
$$\begin{gathered} S(0)=S_0，S(T)-S(0)=S_n-S_0 \\ \dot{S}(0)=v_0，\dot{S}(T)-\dot{S}(0)=v_n-v_0 \\ \ddot{S}(0)=a_0，\ddot{S}(T)-\ddot{S}(0)=a_n-a_0 \end{gathered}$$
因为$S_0,v_0,a_0$的值不影响$Jerk$，所以约束变为

$$\begin{gathered} S(T)-S(0)=S_n-S_0 \\ \dot{S}(T)-\dot{S}(0)=v_n-v_0 \\ \ddot{S}(T)-\ddot{S}(0)=a_n-a_0 \end{gathered}$$

记 $S_n-S_0=C_0,v_n-v_0=C_1,a_n-a_0=C_2$
则有

$$\begin{gathered} S(T)-S(0)={\int }_0^T\dot{f}dt=C_0 \\ \dot{S}(T)-\dot{S}(0)={\int }_0^T\ddot{f}dt=C_1 \\ \ddot{S}(T)-\ddot{S}(0)={\int }_0^T{f}^{(3)}dt=C_2 \end{gathered}$$

最终，问题变为求${\int }_0^T{f}^{(3)}dt$在约束${\int }_0^T(\dot{f}-\frac{C_0}{T})dt=0,{\int }_0^T(\ddot{f}-\frac{C_1}{T})dt=0,{\int }_0^T(f^{(3)}-\frac{C_2}{T})dt=0$下的极小值。

2 求解过程

泛函极值的必要条件为$Euler-Lagrarge$方程
使泛函${\int }_0^{T}L(f,\dot{f})dt$取极小值的f，满足E-L方程

$\frac{\partial L}{\partial f}-\frac{d}{dt}(\frac{\partial L}{\partial \dot{f}})=0$

求解泛函：${\int }_0^T{f}^{(3)}dt$

约束：${\int }_0^T(\dot{f}-\frac{C_0}{T})dt=0,{\int }_0^T(\ddot{f}-\frac{C_1}{T})dt=0,{\int }_0^T(f^{(3)}-\frac{C_2}{T})dt=0$

$Lagrarge乘子$

$$\begin{gathered} {\int }_0^T{f}^{(3)}dt+{\lambda }_1{\int }_0^T(\dot{f}-\frac{C_0}{T})dt+{\lambda }_2{\int }_0^T(\ddot{f}-\frac{C_1}{T})dt+{\lambda }_3{\int }_0^T(f^{(3)}-\frac{C_2}{T})dt \\ ={\int }_0^T({\lambda }_1\dot{f}+{\lambda }_2\ddot{f}+{\lambda }_3{f}^{(3)}+{f^{(3)}}^2-{\lambda }_1\frac{C_0}{T}-{\lambda }_2\frac{C_1}{T}-{\lambda }_3\frac{C_2}{T})dt \\ ={\int }_0^{T}Ldt \end{gathered}$$

$L=L(\dot{f},\ddot{f},f^{(3)})$
这里需要用到广义$Euler-Lagrarge$方程：$\frac{\partial L}{\partial f}-\frac{d}{dt}(\frac{\partial L}{\partial \dot{f}})+\frac{d^2}{d{t}^2}(\frac{\partial L}{\partial \ddot{f}})-\frac{d^3}{d{t}^3}(\frac{\partial L}{\partial f^{(3)}})=0$

将$\frac{\partial L}{\partial f}=0,\frac{\partial L}{\partial \dot{f}}={\lambda }_1,\frac{\partial L}{\partial \ddot{f}}={\lambda }_2,\frac{\partial L}{\partial f^{(3)}}={\lambda }_3+2f^{(3)}$代入广义$Euler-Lagrarge$方程得：
$\frac{d^3}{d{t}^3}({\lambda }_3+2f^{(3)})=0$
$$\begin{gathered} =>f^{(6)}(t)=0 \\ =>f^{(5)}(t)=a_0 \\ =>f^{(4)}(t)=a_{0}t+a_1 \\ =>f^{(3)}(t)=\frac{1}{2}{a}_0{t}^2+a_{1}t+a_2 \\ =>f^{(2)}(t)=\frac{1}{6}{a}_0{t}^3+\frac{1}{2}{a}_1{t}^2+a_{2}t+a_3 \\ =>f^{(1)}(t)=\frac{1}{24}{a}_0{t}^4+\frac{1}{6}{a}_1{t}^3+\frac{1}{2}{a}_{2}t+a_{3}t+a_4 \\ =>f(t)=\frac{1}{120}{a}_0{t}^5+\frac{1}{24}{a}_1{t}^4+\frac{1}{6}{a}_2{t}^3+\frac{1}{2}{a}_3{t}^2+a_{4}t+a_5 \end{gathered}$$

$f(t)=\frac{1}{120}{a}_0{t}^5+\frac{1}{24}{a}_1{t}^4+\frac{1}{6}{a}_2{t}^3+\frac{1}{2}{a}_3{t}^2+a_{4}t+a_5$

是五次多项式，是带约束的泛函${\int }_0^T{f}^{(3)}dt$取极值的解函数，所以五次多项式在规划中才这么常见。