Frenet-Frame 下Mini Jerk 的轨迹生成方法

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

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

1,原理推导

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

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

其中 $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)$$是个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))$最小的通解。

2，场景应用

在Frenet坐标系下

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

让$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)$ 五次多项式了吗？
同理$s(t)$ 也可以由此得出。这样$d(t),s(t)$合起来就完成了Frenet下的轨迹生成。

• 场景二，低速场景下，横向位移由纵向位移驱动,横向位移$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$的五次多项式即可。

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