写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_CSDN博客,期待与您一同探索 移动机器人 领域的无限可能。
🔍 本文系 清流君 原创之作,荣幸在CSDN首发🐒
若您觉得内容有价值,还请评论告知一声,以便更多人受益。
转载请注明出处,尊重原创,从我做起。
👍 点赞、评论、收藏,三连走一波,让我们一起养成好习惯😜
在这里,您将收获的不只是技术干货,还有思维的火花!
📚 系列专栏:【决策规划】系列,带您深入浅出,领略规划之美。🖊
愿我的分享能为您带来启迪,如有不足,敬请指正,让我们共同学习,交流进步!
🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~
文章目录
引言
各位小伙伴们大家好,本篇博客是自动驾驶决策规划算法数学基础的第一节,内容整理自 B站知名up主 忠厚老实的老王 的视频,作为博主的学习笔记,分享给大家共同学习。
本篇博客讲解五次多项式,五次多项式是规划论文里的常客,本节将详细解释五次多项式的特殊性。
本节难度较大,但不是特别重要,所以如果不理解的话,记住结论就可以了。
一、五次多项式在自动驾驶轨迹规划中的应用
1、跃度的定义与舒适性的关系
在车辆的运动规划中,非常重要的指标就是舒适性,衡量舒适性的物理量叫 跃度(Jerk)。 jerk 的定义是
J
e
r
k
=
d
a
d
t
Jerk=\frac{da}{dt}
Jerk=dtda 其中,
a
a
a 为加速度,也就是说 *Jerk * 其实是加速度的导数, *Jerk * 的绝对值越小,
a
a
a 的变化越平缓,也就意味着越舒适。
设有质点的轨迹 s = f ( t ) s=f(t) s=f(t),则 J e r k = d 3 f d t 3 Jerk=\frac{d^{3}f}{dt^{3}} Jerk=dt3d3f,如果在 [ 0 , T ] [0,T] [0,T] 的时间内, Jerk 的绝对值都比较小,意味着在整个 [ 0 , T ] [0,T] [0,T] 曲线内,规划的轨迹比较舒适。
数学问题变成如果有函数 s = f ( t ) s=f(t) s=f(t),什么样的 f ( t ) f(t) f(t) 会使得在 [ 0 , T ] [0,T] [0,T] 时间内的 Jerk 的绝对值变化平缓。绝对值的处理比较烦,一般改成平方,则问题变为要找到 f ( t ) f(t) f(t),使得 m i n ∫ 0 T ( d 3 f d t 3 ) 2 d t min\int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt min∫0T(dt3d3f)2dt显然积分 ∫ 0 T ( d 3 f d t 3 ) 2 d t \int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt ∫0T(dt3d3f)2dt 是关于 f ( t ) f(t) f(t) 的泛函,积分值取决于 f ( t ) f(t) f(t) 在 [ 0 , T ] [0,T] [0,T] 上的整体形状。
2、二次函数与Jerk最小化的关系
使积分 ∫ 0 T ( d 3 f d t 3 ) 2 d t \int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt ∫0T(dt3d3f)2dt取极小值的 f ( t ) f(t) f(t) 是什么?
注意: f ( t ) f(t) f(t) 的定义域和值域都应该是实数,不能有虚数,因为 s = f ( t ) s=f(t) s=f(t), t t t 和 s s s 都是真实空间中的时间和轨迹坐标,都是实数。
因为平方的最小值就是 0 0 0,只要让其三阶导数为 0 0 0 就行,显然只要 f ( t ) f(t) f(t) 是二次或二次以下的函数即可。显然当 f ( t ) f(t) f(t) 是二次或者是二次以下的函数时, ∫ 0 T ( d 3 f d t 3 ) 2 d t = 0 \int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt=0 ∫0T(dt3d3f)2dt=0 自然就是最小。
所以要让 Jerk 在 [ 0 , T ] [0,T] [0,T] 上的绝对值最小, f ( t ) f(t) f(t) 应取二次或者二次以下的函数。
3、边界约束条件
但是真实情况远比想象中的要复杂得多,因为真实的
s
=
f
(
t
)
s=f(t)
s=f(t),往往存在约束条件:
s
(
0
)
=
s
0
s
(
T
)
=
s
n
s
(
0
)
=
v
0
s
(
T
)
=
v
n
s
(
0
)
=
a
0
s
(
T
)
=
a
n
\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}
s(0)=s0s(0)=v0s(0)=a0s(T)=sns(T)=vns(T)=an 有六个边界条件,但二次函数
y
=
a
x
2
+
b
x
+
c
y=ax^2+bx+c
y=ax2+bx+c 只有三个系数
a
、
b
、
c
a 、b、 c
a、b、c ,没有办法满足六个边界条件,所以在这种情况下,往往需要求带边界约束条件的函数最值问题。
4、泛函极值问题的引入
数学问题变成了找到这样的 f ( t ) f(t) f(t),使得 ∫ 0 T ( d 3 f d t 3 ) 2 d t = 0 \int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt=0 ∫0T(dt3d3f)2dt=0 最小,但是有上述六个边界条件。
满足带约束的泛函 ∫ 0 T ( d 3 f d t 3 ) 2 d t \int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt ∫0T(dt3d3f)2dt 极值问题的解是什么呢?
问题的答案就是 五次多项式,所以五次多项式在规划中具有非常特殊的地位,可以在任何有关规划的论文里看到它的身影。
二、求解带约束的泛函极值问题——五次多项式推导
1、有界连续函数的选择
下面具体讲解泛函极值问题,显然让泛函取极小值的解
f
(
t
)
f(t)
f(t) 只可能是
[
0
,
T
]
[0,T]
[0,T] 上的有界连续函数。因为无论是无界函数还是有界间断函数,都会使 jerk 出现无穷大,显然无穷大不能让 Jerk 取最小值。如果是有界连续函数,不妨将
s
=
f
(
t
)
s=f(t)
s=f(t) 泰勒展开到二阶项:
s
=
f
(
t
)
=
f
(
0
)
+
f
˙
(
0
)
⋅
t
+
f
¨
(
0
)
2
t
2
+
.
.
.
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+...
s=f(t)=f(0)+f˙(0)⋅t+2f¨(0)t2+... 代入边界条件:
s
(
0
)
=
s
0
⇒
f
(
0
)
+
f
˙
(
0
)
⋅
0
+
.
.
.
=
s
(
0
)
⇒
f
(
0
)
=
s
0
\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}
s(0)=s0⇒f(0)+f˙(0)⋅0+...=s(0)⇒f(0)=s0
s
˙
(
0
)
=
v
0
⇒
f
˙
(
0
)
=
v
0
\dot{s}(0)=v_0\Rightarrow\dot{f}(0)=v_0
s˙(0)=v0⇒f˙(0)=v0
s
¨
(
0
)
=
a
0
⇒
f
¨
(
0
)
=
a
0
\ddot{s}(0)=a_0\Rightarrow\ddot{f}(0)=a_0
s¨(0)=a0⇒f¨(0)=a0 最终可得到:
2、边界条件对跃度的影响
因为 Jerk 它是 f ( t ) f(t) f(t) 的三阶导数。所以 s 0 s_0 s0、 v 0 v_0 v0、 a 0 a_0 a0 的值并不影响 Jerk,因为 s 0 s_0 s0、 v 0 v_0 v0、 a 0 a_0 a0 最多就影响到二次项。
将六个边界条件恒等变形:
s
(
0
)
=
s
0
s
(
T
)
−
s
(
0
)
=
s
n
−
s
0
s
˙
(
0
)
=
v
0
s
˙
(
T
)
−
s
˙
(
0
)
=
v
n
−
v
0
s
¨
(
0
)
=
a
0
s
¨
(
T
)
−
s
¨
(
0
)
=
a
n
−
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)s˙(0)s¨(0)=s0 s(T)−s(0)=sn−s0=v0 s˙(T)−s˙(0)=vn−v0=a0 s¨(T)−s¨(0)=an−a0 又因为
s
0
s_0
s0、
v
0
v_0
v0、
a
0
a_0
a0 不影响 Jerk 所以前面的三个条件可以去掉,约束了就变成:
s
(
T
)
−
s
(
0
)
=
s
n
−
s
0
s
˙
(
T
)
−
s
˙
(
0
)
=
v
n
−
v
0
s
¨
(
T
)
−
s
¨
(
0
)
=
a
n
−
a
0
\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(T)−s(0)=sn−s0s˙(T)−s˙(0)=vn−v0s¨(T)−s¨(0)=an−a0 记
s
n
−
s
0
=
c
0
,
v
n
−
v
0
=
c
1
,
a
n
−
a
0
=
c
2
s_n-s_0=c_0,v_n-v_0=c_1,a_n-a_0=c_2
sn−s0=c0,vn−v0=c1,an−a0=c2,则有
所以最终数学问题就变成了求
∫
0
T
(
d
3
f
d
t
3
)
2
d
t
\int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt
∫0T(dt3d3f)2dt 在约束条件:
下取极小值的
f
(
t
)
f(t)
f(t)。
3、泛函极值问题的欧拉-拉格朗日方程
泛函极值的必要条件是 欧拉-拉格朗日方程(Euler- Lagrange)。
使泛函
∫
0
T
L
(
f
,
f
˙
)
d
t
\int_0^TL(f,\dot{f})dt
∫0TL(f,f˙)dt 取极小值的
f
f
f,满足 E-L 方程:
∂
L
∂
f
−
d
d
t
(
∂
L
∂
f
)
=
0
\frac{\partial L}{\partial f}-\frac{d}{dt}(\frac{\partial L}{\partial f})=0
∂f∂L−dtd(∂f∂L)=0 用欧拉-拉格朗日方程计算泛函的极值,泛函是
∫
0
T
(
d
3
f
d
t
3
)
2
d
t
\int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt
∫0T(dt3d3f)2dt,有三个约束条件:
先用拉格朗乘子把带约束的泛函化为无约束的泛函:
其中,
L
=
L
(
f
,
f
¨
,
d
3
f
d
t
3
)
L=L\left( f,\ddot{f},\frac{d^3f}{dt^3} \right)
L=L(f,f¨,dt3d3f)。
4、广义欧拉-拉格朗日方程
这样就把带约束的泛函化为了无约束泛函,但无约束的泛函和 f ˙ 、 f ¨ \dot f、\ddot f f˙、f¨、 f f f三阶导 都有关。所以不能直接用上面只能处理一阶导数的欧拉-拉格朗日方程,得用广义欧拉-拉格朗日方程:
先算一下
L
L
L 对
f
f
f 各阶导数的偏导:
代入 E-L 方程,可得到:
对六次导数积分:
f
(
5
)
(
t
)
=
a
0
f
(
4
)
(
t
)
=
a
0
t
+
a
1
f
(
3
)
(
t
)
=
1
2
a
0
t
2
+
a
1
t
+
a
2
f
(
2
)
(
t
)
=
1
6
a
0
t
3
+
1
2
a
1
t
2
+
a
2
t
+
a
3
f
(
1
)
(
t
)
=
1
24
a
0
t
4
+
1
6
a
1
t
3
+
1
2
a
2
t
2
+
a
3
t
+
a
4
\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(5)(t)f(4)(t)f(3)(t)f(2)(t)f(1)(t)=a0=a0t+a1=21a0t2+a1t+a2=61a0t3+21a1t2+a2t+a3=241a0t4+61a1t3+21a2t2+a3t+a4 最终得到五次多项式
f
(
t
)
f(t)
f(t):
f
(
t
)
=
1
120
a
0
t
5
+
1
24
a
1
t
4
+
1
6
a
2
t
3
+
1
2
a
3
t
2
+
a
4
t
+
a
5
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
f(t)=1201a0t5+241a1t4+61a2t3+21a3t2+a4t+a5
三、总结
五次多项式是带约束的 Jerk 的平方在整个区间内取极小值的最优解。所以五次多项式在规划中特别常见,在某些论文上经常看到,比如两个离散点的轨迹点怎么连接?一般都是用五次多项式连接。
本篇博客到此结束,下一讲再见,欢迎关注!
参考资料
后记:
🌟 感谢您耐心阅读这篇关于 五次多项式详解 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡
🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀