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

本文链接：https://blog.csdn.net/weixin_51200029/article/details/141554960

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

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

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

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

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

引言

各位小伙伴们大家好，本篇博客是自动驾驶决策规划算法数学基础的第一节，内容整理自 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 的平方在整个区间内取极小值的最优解。所以五次多项式在规划中特别常见，在某些论文上经常看到，比如两个离散点的轨迹点怎么连接？一般都是用五次多项式连接。

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

参考资料

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

后记：

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

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

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

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

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