【自动驾驶】决策规划算法 | 数学基础（一）五次多项式详解

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝


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

---

文章目录

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

• 1、跃度的定义与舒适性的关系
• 2、二次函数与Jerk最小化的关系
• 3、边界约束条件
• 4、泛函极值问题的引入

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

• 1、有界连续函数的选择
• 2、边界条件对跃度的影响
• 3、泛函极值问题的欧拉-拉格朗日方程
• 4、广义欧拉-拉格朗日方程

• 三、总结
• 参考资料

---

引言

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

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

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

---

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

1、跃度的定义与舒适性的关系

  在车辆的运动规划中，非常重要的指标就是舒适性，衡量舒适性的物理量叫 跃度(Jerk)。 jerk 的定义是
$Jerk=\frac{da}{dt}$  其中， $a$ 为加速度，也就是说 *Jerk * 其实是加速度的导数， *Jerk * 的绝对值越小，$a$

$s=f(t)$，则 $Jerk=\frac{d^{3}f}{dt^{3}}$，如果在 $[0,T]$ 的时间内， Jerk 的绝对值都比较小，意味着在整个$[0,T]$

$s=f(t)$，什么样的 $f(t)$ 会使得在 $[0,T]$ 时间内的 Jerk 的绝对值变化平缓。绝对值的处理比较烦，一般改成平方，则问题变为要找到 $f(t)$，使得$min\int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt$显然积分 $\int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt$ 是关于 $f(t)$ 的泛函，积分值取决于 $f(t)$ 在$[0,T]$

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

使积分 $\int_{0}^{T}(\frac{d^{3}f}{dt^{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}{dt^{3}})^{2}dt=0$

  所以要让 Jerk 在 $[0,T]$ 上的绝对值最小， $f(t)$

3、边界约束条件

$s=f(t)$，往往存在约束条件：
$\begin{matrix} 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{matrix}$  有六个边界条件，但二次函数 $y=ax^2+bx+c$ 只有三个系数 $a 、b、 c$

4、泛函极值问题的引入

$f(t)$，使得 $\int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt=0$

满足带约束的泛函 $\int_{0}^{T}(\frac{d^{3}f}{dt^{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{aligned} 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{aligned}$$\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{aligned} 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{aligned}$  又因为 $s_0$、$v_0$、$a_0$ 不影响 Jerk 所以前面的三个条件可以去掉，约束了就变成：

$\begin{aligned} 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{aligned}$  记 $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}{dt^{3}})^{2}dt$

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

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

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

$\int_0^TL(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}{dt^{3}})^{2}dt$，有三个约束条件：

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

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

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

$\dot f、\ddot f$、$f$三阶导 都有关。所以不能直接用上面只能处理一阶导数的欧拉-拉格朗日方程，得用广义欧拉-拉格朗日方程：

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

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

  对六次导数积分：
$\begin{aligned} f^{\left( 5 \right)}\left( t \right) &=a_0\\ f^{\left( 4 \right)}\left( t \right) &=a_0t+a_1\\ f^{\left( 3 \right)}\left( t \right) &=\frac{1}{2}a_0t^2+a_1t+a_2\\ f^{\left( 2 \right)}\left( t \right) &=\frac{1}{6}a_0t^3+\frac{1}{2}a_1t^2+a_2t+a_3\\ f^{\left( 1 \right)}\left( t \right) &=\frac{1}{24}a_0t^4+\frac{1}{6}a_1t^3+\frac{1}{2}a_2t^2+a_3t+a_4\\ \end{aligned}$  最终得到五次多项式 $f(t)$：
$f\left( t \right) =\frac{1}{120}a_0t^5+\frac{1}{24}a_1t^4+\frac{1}{6}a_2t^3+\frac{1}{2}a_3t^2+a_4t+a_5$

---

三、总结

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

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

---

参考资料

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

---

后记：

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

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

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

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

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