运动规划是无人车导航的和核心技术之一，在不同场景会有不同的应对算法。本文主要介绍在高速公路场景下的轨迹规划算法。

一、Frenet坐标系

无人车在运行中会交替使用笛卡尔坐标系和 Frenet坐标系，在进行运动路径规划时，普遍采用Frenet坐标系。 如图1 左图所示，以道路的中心线作为参考线，沿着参考线的方向为S轴，垂直参考线向外的为D轴，相比于右边的笛卡尔坐标系，Frenet 坐标系可以降低汽车的运动状态描述难度，因为参考线一直是道路的中心线，根据道路中线可以根据纵向距离s，横向距离d来表示汽车的当前位置。s和d，再加上运行的时间 t，就是汽车在轨迹规划时的参考依据。

图1. 无人车坐标系

二、Jerk最小化

Jerk最小化体现了关于物体运动过程中的颠簸程度，颠簸程度越小，乘车体验越好。 Jerk最小化这需要在运动过程中累计的加速度的变化总和尽可能小。以纵向距离s为例，如图2所示汽车要从A位置到B位置，已知两个时刻汽车的位置\(s\)，速度\(\dot{s}\)，加速度\(\ddot{s}\)和所运行的时间\(\Delta t\)，我们需要刻画\(\Delta t\)时间内的轨迹形式过程\(s(t)\)。

图2. 汽车运动状态参数

因为颠簸程度是与加速度的变化\(\dddot{s}\)相关，所以沿着这条轨迹行驶过程中累计的总颠簸量\(Jerk_{sum}=\int_{0}^{t_f}\dddot{s}(t)^2dt\)。如果要最小化这个\(Jerk_{sum}\)，就必须将\(s(t)\)的所有时间导数为0，即

\(\frac{d^ms}{dt^m}=0, \forall m\geqslant 6\)

通过这个性质，我们可以把\(s(t)\)表示为含有6个系数的5次多项式，即

\(s(t) = \alpha _0 + \alpha _1t+ \alpha _2t^2 + \alpha _3t^3+\alpha_4t^4+\alpha_5t^5\)

这只是纵向移动的s，同理横向移动的d也适用。

三、多项式求解

为了使\(Jerk_{sum}\)最小化，我们获得了一个含有6个系数的一维Jerk轨迹。关于求解这6个参数，我们可以借助已知的汽车状态，即在两个时刻的位置\(s\)，速度\(\dot{s}\)和加速度\(\ddot{s}\)。通过已知的\([s_i,\dot{s_i},\ddot{s_i},s_f,\dot{s_f},\ddot{s_f}, \Delta t]\)，可求解方程组

\(s(t) = \alpha _0 + \alpha _1t+ \alpha _2t^2 + \alpha _3t^3+\alpha_4t^4+\alpha_5t^5\)
\(\dot{s(t)} = \alpha _1+ 2\alpha _2t + 3\alpha _3t^2+ 4\alpha_4t^3+ 5\alpha_5t^4\)
\(\ddot{s(t)} = 2\alpha _2 + 6\alpha _3t+ 12\alpha_4t^2+20\alpha_5t^3\)

令\(t=0\)，可以直接得到\([\alpha _0, \alpha _1,\alpha _2]=[s_i,\dot{s_i},\frac{1}{2}\ddot{s_i}]\)，将\(\alpha _0\), \(\alpha _1\),\(\alpha _2\)的值带入后，得

\(s(t) = s_i + \dot{s_i}t+ \frac{1}{2}\ddot{s_i}t^2 + \alpha _3t^3+\alpha_4t^4+\alpha_5t^5\)
\(\dot{s(t)} = \dot{s_i}+ \ddot{s_i}t + 3\alpha _3t^2+ 4\alpha_4t^3+ 5\alpha_5t^4\)
\(\ddot{s(t)} = \ddot{s_i} + 6\alpha _3t+ 12\alpha_4t^2+20\alpha_5t^3\)

接着再将\([s_f,\dot{s_f},\ddot{s_f}, \Delta t]\)带入，即

最终获得该Jerk轨迹的具体参数。

标签：轨迹,Jerk,5t,3t,ddot,alpha,规划,dot
来源： https://www.cnblogs.com/yujingxiang/p/14463194.html