介绍一种依靠车辆中心线的Mini Jerk 的轨迹生成方法。

注：下文中两点上标，都是3点上标。

1,原理推导

‘’’
如果让车开的稳就得保证轨迹加速的平滑，加速的平滑度可以用jerk的L2模来度量，jerk就是加速度的导数。即

$J_{t}(p(t)):=\int_{t_{0}}^{t_{1}} \ddot{p}^{2}(\tau) \mathrm{d} \tau$[1]

其中 p p p 为车行驶的位置， p p p上三个点表示对时间的三次导数，表示jerk物理量。

根据最优化原理[2]：

$C=\frac{1}{2} \int_{0}^{T} L d t$

其中

$L=\left(\frac{d^{3} x}{d t^{3}}\right)^{2}+\left(\frac{d^{3} y}{d t^{3}}\right)^{2}$

要想使C取得最小值，L应满足：

$\frac{\partial L}{\partial x}-\frac{d}{d t}\left(\frac{\partial L}{\partial x}\right) \cdots+(-1)^{n} \frac{d^{n}}{d t^{n}} \frac{\partial L}{\partial\left(x^{(n)}\right)}=0$ $\frac{\partial L}{\partial y}-\frac{d}{d t}\left(\frac{\partial L}{\partial y}\right) \cdots+(-1)^{n} \frac{d^{n}}{d t^{n}} \frac{\partial L}{\partial\left(y^{(n)}\right)}=0$

从而

$\frac{d^{3}}{d t^{3}} \frac{\partial\left(\ddot{x}^{2}\right)}{\partial(\ddot{x})}=0$ $\frac{d^{3}}{d t^{3}} \frac{\partial\left(\ddot{y}^{2}\right)}{\partial(\ddot{y})}=0$

然后

$\frac{d^{6} x}{d t^{6}}=0$ $\frac{d^{6} y}{d t^{6}}=0$

也就是说
x ( t ) , y ( t ) x(t),y(t) x(t),y(t)是个5次多项式

$\left.\begin{array}{l}x(\ell)=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{array}\right\}$

至此，我们明白了，5次多项式是使 J t ( p ( t ) ) J_{t}(p(t)) Jt​(p(t))最小的通解。

2，场景应用

在Frenet坐标系下

• 场景一，在高速场景下，横向位移 d d d，和纵向位移 s s s 可以互相独立

让 d ( t ) d(t) d(t) 为：

$d(t)=a_{d 0}+a_{d 1} t+a_{d 2} t^{2}+a_{d 3} t^{3}+a_{d 4} t^{4}+a_{d s} t^{5}$

给出起始，终止边界条件

$\left[d_{s}, \dot{d}_{s}, \ddot{d}_{s}, 0\right],\left[d_{e}, \dot{d}_{e}, \ddot{d}_{e}, T\right]$

不就可以解出 d ( t ) d(t) d(t) 五次多项式了吗？
同理 s ( t ) s(t) s(t) 也可以由此得出。这样 d ( t ) , s ( t ) d(t),s(t) d(t),s(t)合起来就完成了Frenet下的轨迹生成。

• 场景二，低速场景下，横向位移由纵向位移驱动,横向位移 d d d为：

$d(s)=a_{d 0}+a_{d 1} s+a_{d 2} s^{2}+a_{d 3} s^{3}+a_{d 4} s^{4}+a_{d s} s^{5}$

解此关于 s s s的五次多项式即可。

3， 效果展示

这个方法已经被Python Robot实现

https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning/FrenetOptimalTrajectory
效果如下：

参考文献

[1] Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame 2010 Moritz Werling, Julius Ziegler, So¨ren Kammel, and Sebastian Thrun

[2] LOCAL PATH PLANNING AND MOTION CONTROL FOR AGV IN POSITIONING 1989 Arata Talcahashi Taltero Hongo Yoshilci Ninomiya Gunji Sugimoto