写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
个人主页：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。

👍 点赞、评论、收藏，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货，还有思维的火花！

📚 系列专栏：【决策规划】系列，带您深入浅出，领略规划之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！

🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

---

---

引言

  各位小伙伴们大家好，本篇博客是自动驾驶决策规划算法数学基础的第一节，内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

  本篇博客讲解五次多项式，五次多项式是规划论文里的常客，本节将详细解释五次多项式的特殊性。

  本节难度较大，但不是特别重要，所以如果不理解的话，记住结论就可以了。

---

一、五次多项式在自动驾驶轨迹规划中的应用

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 的绝对值变化平缓。绝对值的处理比较烦，一般改成平方，则问题变为要找到 $f(t)$，使得$$min{\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]$ 上的整体形状。

2、二次函数与Jerk最小化的关系

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

  注意： $f(t)$ 的定义域和值域都应该是实数，不能有虚数，因为 $s=f(t)$， $t$ 和 $s$ 都是真实空间中的时间和轨迹坐标，都是实数。

  因为平方的最小值就是 $0$，只要让其三阶导数为 $0$ 就行，显然只要 $f(t)$ 是二次或二次以下的函数即可。显然当 $f(t)$ 是二次或者是二次以下的函数时，${\int }_0^T(\frac{d^{3}f}{d{t}^3}{)}^{2}dt=0$ 自然就是最小。

  所以要让 Jerk 在 $[0,T]$ 上的绝对值最小， $f(t)$ 应取二次或者二次以下的函数。

3、边界约束条件

  但是真实情况远比想象中的要复杂得多，因为真实的 $s=f(t)$，往往存在约束条件：
$$\begin{array}{cc}s\left(0\right)=s_0& s\left(T\right)=s_n\\ s\left(0\right)=v_0& s\left(T\right)=v_n\\ s\left(0\right)=a_0& s\left(T\right)=a_n\end{array}$$  有六个边界条件，但二次函数 $y=a{x}^2+bx+c$ 只有三个系数 $a、b、c$ ，没有办法满足六个边界条件，所以在这种情况下，往往需要求带边界约束条件的函数最值问题。

4、泛函极值问题的引入

  数学问题变成了找到这样的 $f(t)$，使得 ${\int }_0^T(\frac{d^{3}f}{d{t}^3}{)}^{2}dt=0$ 最小，但是有上述六个边界条件。

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

  问题的答案就是 五次多项式，所以五次多项式在规划中具有非常特殊的地位，可以在任何有关规划的论文里看到它的身影。

---

二、求解带约束的泛函极值问题——五次多项式推导

1、有界连续函数的选择

  下面具体讲解泛函极值问题，显然让泛函取极小值的解 $f(t)$ 只可能是 $[0,T]$ 上的有界连续函数。因为无论是无界函数还是有界间断函数，都会使 jerk 出现无穷大，显然无穷大不能让 Jerk 取最小值。如果是有界连续函数，不妨将 $s=f(t)$ 泰勒展开到二阶项：
$$s=f\left(t\right)=f\left(0\right)+\dot{f}\left(0\right)\cdot t+\frac{\ddot{f}\left(0\right)}{2}{t}^2+...$$  代入边界条件：
$$\begin{array}{rl}s\left(0\right)=s_0& \Rightarrow f\left(0\right)+\dot{f}\left(0\right)\cdot 0+...=s\left(0\right)\\ & \Rightarrow f\left(0\right)=s_0\end{array}$$$$\dot{s}(0)=v_0\Rightarrow \dot{f}(0)=v_0$$$$\ddot{s}(0)=a_0\Rightarrow \ddot{f}(0)=a_0$$  最终可得到：

2、边界条件对跃度的影响

  因为 Jerk 它是 $f(t)$ 的三阶导数。所以 $s_0$、$v_0$、$a_0$ 的值并不影响 Jerk，因为 $s_0$、$v_0$、$a_0$ 最多就影响到二次项。

  将六个边界条件恒等变形：
$$\begin{array}{rl}s\left(0\right)& =s_{0}s\left(T\right)-s\left(0\right)=s_n-s_0\\ \dot{s}\left(0\right)& =v_0\dot{s}\left(T\right)-\dot{s}\left(0\right)=v_n-v_0\\ \ddot{s}\left(0\right)& =a_0\ddot{s}\left(T\right)-\ddot{s}\left(0\right)=a_n-a_0\end{array}$$  又因为 $s_0$、$v_0$、$a_0$ 不影响 Jerk 所以前面的三个条件可以去掉，约束了就变成：
$$\begin{array}{r}s\left(T\right)-s\left(0\right)=s_n-s_0\\ \dot{s}\left(T\right)-\dot{s}\left(0\right)=v_n-v_0\\ \ddot{s}\left(T\right)-\ddot{s}\left(0\right)=a_n-a_0\end{array}$$  记 $s_n-s_0=c_0，v_n-v_0=c_1，a_n-a_0=c_2$，则有

  所以最终数学问题就变成了求 ${\int }_0^T(\frac{d^{3}f}{d{t}^3}{)}^{2}dt$ 在约束条件：

  下取极小值的 $f(t)$。

3、泛函极值问题的欧拉-拉格朗日方程

  泛函极值的必要条件是 欧拉-拉格朗日方程(Euler- Lagrange)。

  使泛函 ${\int }_0^{T}L(f,\dot{f})dt$ 取极小值的 $f$，满足 E-L 方程：
$$\frac{\partial L}{\partial f}-\frac{d}{dt}(\frac{\partial L}{\partial f})=0$$  用欧拉-拉格朗日方程计算泛函的极值，泛函是 ${\int }_0^T(\frac{d^{3}f}{d{t}^3}{)}^{2}dt$，有三个约束条件：

  先用拉格朗乘子把带约束的泛函化为无约束的泛函：

其中，$L=L\left(f,\ddot{f},\frac{d^{3}f}{d{t}^3}\right)$。

4、广义欧拉-拉格朗日方程

  这样就把带约束的泛函化为了无约束泛函，但无约束的泛函和 $\dot{f}、\ddot{f}$、$f$三阶导 都有关。所以不能直接用上面只能处理一阶导数的欧拉-拉格朗日方程，得用广义欧拉-拉格朗日方程：

  先算一下 $L$ 对 $f$ 各阶导数的偏导：

  代入 E-L 方程，可得到：

  对六次导数积分：
$$\begin{array}{rl}{f}^{\left(5\right)}\left(t\right)& =a_0\\ f^{\left(4\right)}\left(t\right)& =a_{0}t+a_1\\ f^{\left(3\right)}\left(t\right)& =\frac{1}{2}{a}_0{t}^2+a_{1}t+a_2\\ f^{\left(2\right)}\left(t\right)& =\frac{1}{6}{a}_0{t}^3+\frac{1}{2}{a}_1{t}^2+a_{2}t+a_3\\ f^{\left(1\right)}\left(t\right)& =\frac{1}{24}{a}_0{t}^4+\frac{1}{6}{a}_1{t}^3+\frac{1}{2}{a}_2{t}^2+a_{3}t+a_4\end{array}$$  最终得到五次多项式 $f(t)$：
$$f\left(t\right)=\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$$

---

三、总结

  五次多项式是带约束的 Jerk 的平方在整个区间内取极小值的最优解。所以五次多项式在规划中特别常见，在某些论文上经常看到，比如两个离散点的轨迹点怎么连接？一般都是用五次多项式连接。

  本篇博客到此结束，下一讲再见，欢迎关注！

---

参考资料

  自动驾驶决策规划算法第一章第一节 细说五次多项式

---

后记：

🌟 感谢您耐心阅读这篇关于 五次多项式详解 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡

🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀