自动驾驶规划控制-动作规划2

Lattice planner

The Lattice Planner algorithm is a local motion planner,
• Sample based motion planner;
• Plan in Frenet coordinate;
• The output is a smooth, safe and collision-free local trajectory that satisfies the vehicle’s kinematics and speed constraints which is directly feed into controller.（直接反馈到控制器中）

The basic process of Lattice planner:

• Frenet Coordinate

T

参考线上的可动坐标系，原点是从车辆到路径最近的r点， t 轴沿 r 处的切线tangiential方向，而 n 轴沿 r 处的pependicular方向,垂直于 t 轴。

Y

为了更好地表示：相比Cartesian coordinate 将车辆的运动沿着行驶方向分解横向与纵向运动，坐标系定义在参考线上变化，更方便规划轨迹
Frenet coordinate:
$$\left[\mathbf{s},\dot{\mathbf{s}},\ddot{\mathbf{s}},\mathbf{l},\dot{\mathbf{l}},\ddot{\mathbf{l}},{\mathbf{l}}^{\prime },{\mathbf{l}}^{\prime \prime }\right]$$
Cartesian coordinate
$\left[\overrightarrow{\mathbf{x}},{\mathbf{v}}_x,{\mathbf{a}}_x,{\mathbf{\theta }}_x,{\mathbf{\kappa }}_x\right]$

where $l^{{}^{\prime }}=\frac{dl}{ds}$ v_x是一个标量

Frenet coordinate and Cartesian coordinate transformation

• Cartesian to Frenet
  
  推导特别重要的公式,r(s)为原点到s(t)向量；
  $\vec{x}=\vec{r}+l\cdot \overrightarrow{n_c}$ （1）

判断方向左+ 右-,即车在参考线左边为正，这个由叉乘来判断方向，n_c是在t_c逆时针方向，
$$\begin{gathered} {\vec{n}}_c=\frac{1}{l}\left(x-x_c,y-y_r\right) \\ {\vec{t}}_c=\left(\cos {\theta }_r,\sin {\theta }_r\right) \\ {\vec{n}}_c\times \overrightarrow{t_c}= \\ \left(y-y_r\right)\cdot \cos {\theta }_r-\left(x-x_r\right)\sin {\theta }_r \end{gathered}$$
那么可以获得
$l=\mathrm{sign}\left(\left(y-y_r\right)\cos {\theta }_r-\left(x-x_r\right)\sin {\theta }_r\right)\sqrt{{\left(x-x_r\right)}^2+{\left(y-y_r\right)}^2}$

在Frenet 坐标系下$l^{{}^{\prime }}$
$l^{\prime }=\frac{dl}{ds}=\frac{dl/dt}{ds/dt}=\frac{\dot{l}}{\dot{s}}$
由（1）可知（向量转置）
$l=n_c^T(x-r)=(x-r{)}^T{n}_c$
关于时间的导数
$\dot{l}=(\dot{\mathbf{x}}-\dot{\mathbf{r}}{)}^T{\mathbf{n}}_{\mathbf{c}}+(\mathbf{x}-\mathbf{r}{)}^T{\dot{\mathbf{n}}}_{\mathbf{c}}=(\dot{\mathbf{x}}-\dot{\mathbf{r}}{)}^T{\dot{\mathbf{n}}}_{\mathbf{c}}=(\dot{\mathbf{x}}-\dot{\mathbf{r}}{)}^T{\mathbf{n}}_{\mathbf{c}}+{\left({\mathbf{ln}}_{\mathbf{c}}\right)}^T\dot{{\mathbf{n}}_{\mathbf{c}}}$ （2）

由速度定义可知：
$\dot{x}=v{\mathbf{t}}_{\mathbf{x}},\dot{r}=\dot{s}{\mathbf{t}}_c$（3）

由链式求导可知
${\mathbf{n}}_c=\frac{d{\mathbf{n}}_{\mathbf{c}}}{dt}=\frac{d{\mathbf{n}}_{\mathbf{c}}}{ds}\frac{ds}{dt}=\dot{s}\frac{d{\mathbf{n}}_c}{ds}$
根据Frenet坐标定义
$$\begin{gathered} \frac{d{\mathbf{n}}_c}{ds}=-\kappa {\mathbf{t}}_c \\ {\mathbf{n}}_c=-\kappa \dot{s}{\mathbf{t}}_c \end{gathered}$$ （4）
将(3),(4)带入（2）中

$\dot{l}={\left(v{\mathbf{t}}_{\mathbf{x}}-\dot{s}{\mathbf{t}}_{\mathbf{c}}\right)}^T{\mathbf{n}}_{\mathbf{c}}-{\left(\ln {\mathbf{n}}_{\mathbf{c}}\right)}^T\kappa \dot{s}{\mathbf{t}}_{\mathbf{c}}$
由于$t_c和n_c$正交，那么：
$\dot{l}=v{\mathbf{t}}_x^T{\mathbf{n}}_c$
由于$t_x和n_c$为单位向量 ${\theta }_x,{\theta }_r$分别为t_x,t_c与x轴的夹角，可参考

由于转置，$t_x$需要旋转90度，故

$\dot{l}=v\sin \left({\theta }_x-{\theta }_r\right)$ (5)
为了得到其他量，我们又需要对（1）下手，求关于时间的导数
$\dot{x}=\frac{d\left(r+{\ln }_c\right)}{dt}=\dot{r}+\dot{l}{n}_c+\dot{l}{n}_c$
带入（3）,（4）
$\dot{x}=\dot{s}\left(1-{\kappa }_{r}l\right){\mathbf{t}}_c+\dot{l}{n}_c$
由于$T_r和N_r$正交，假定v为正，取范数
$v=\sqrt{{\dot{s}}^2{\left(1-{\kappa }_{r}l\right)}^2+{\dot{l}}^2}$ ……(6)
由（5）,（6）推导消去$\dot{s}$可得
${\left(\frac{\dot{l}}{\dot{s}}\right)}^2={\tan }^2\left({\theta }_x-{\theta }_r\right){\left(1-{\kappa }_{r}l\right)}^2=l^{\prime 2}$
通常，我们假设参考线的曲率远小于车辆最大转弯角度。还假设横向误差很小（通常车道宽度小于 4m）。因此，我们可以假设 $||1-{\kappa }_r||>0$。并且假设角度误差不会很大，
$|{\theta }_x-{\theta }_r|<\pi /2$

$l^{\prime }=\tan \left({\theta }_x-{\theta }_r\right)\left(1-{\kappa }_{r}l\right)$ (7)
那么可以得到s的导数
$\dot{s}=\frac{v}{1-{\kappa }_{r}l}\cos \left({\theta }_x-{\theta }_r\right)$ （8）
现在开始计算 $l^{{}^{\prime \prime }}$
$l^{\prime \prime }=d{l}^{\prime }/ds$

$\kappa =\frac{d\theta }{ds}$

$v=\frac{d{s}_x}{dt}$

$\frac{d{\theta }_x}{ds}=\frac{d{s}_x}{ds}\frac{d{\theta }_x}{d{s}_x}$
代入v
$\frac{d{\theta }_x}{ds}=\frac{d{s}_x}{ds}\frac{d{\theta }_x}{d{s}_x}$
代入（8）
$\frac{d{\theta }_x}{ds}=\frac{1-{\kappa }_{r}l}{\cos \left({\theta }_x-{\theta }_r\right)}\frac{d{\theta }_x}{ds}$ (9)
用前面（7）式可以得
$l^{\prime \prime }=\frac{d{l}^{\prime }}{ds}=\frac{d\left(1-{\kappa }_{r}l\right)}{ds}\tan \left({\theta }_x-{\theta }_r\right)+\frac{1-{\kappa }_{r}l}{{\cos }^2\left({\theta }_x-{\theta }_r\right)}\frac{d\left({\theta }_x-{\theta }_r\right)}{ds}$ (10)
第一项
$\frac{d\left(1-{\kappa }_{r}l\right)}{ds}=-\left(d{\kappa }_{r}l+{\kappa }_r{l}^{\prime }\right)$
由（9）式可知
$\frac{d\left({\theta }_x-{\theta }_r\right)}{ds}=\frac{d{\theta }_x}{ds}-\frac{d{\theta }_r}{ds}=\frac{1-{\kappa }_{r}l}{\cos \left({\theta }_x-{\theta }_r\right)}\frac{d{\theta }_x}{d{s}_x}-\frac{d{\theta }_r}{ds}$
根据曲率的定义可知
$\frac{d{\theta }_x}{ds}={\kappa }_x,\frac{d{\theta }_r}{ds}={\kappa }_r$

$l^{\prime \prime }=-\left(d{\kappa }_{r}l+{\kappa }_r{l}^{\prime }\right)\tan \left({\theta }_x-{\theta }_r\right)+\frac{1-{\kappa }_{r}l}{{\cos }^2\left({\theta }_x-{\theta }_r\right)}\left({\kappa }_x\frac{1-{\kappa }_{r}l}{\cos \left({\theta }_x-{\theta }_r\right)}-{\kappa }_r\right)$ （11）
求 $\ddot{s}$ 对（8）求关于时间的导数$\dot{v}=a$
解方程可知
$a=\ddot{s}\frac{1-{\kappa }_{r}l}{\cos \Delta \theta }+\frac{\dot{s}}{\cos \Delta \theta }\left(\left(1-{\kappa }_{r}l\right)\tan \Delta \theta \frac{d\Delta \theta }{ds}-d{\kappa }_{r}l-{\kappa }_r{l}^{\prime }\right),\Delta \theta ={\theta }_x-{\theta }_r$ (12)

• Frenet to Cartesian

相对简单点，这也是一种优势
$$\begin{gathered} x=x_r-l\sin {\theta }_r \\ y=y_r+l\cos {\theta }_r \end{gathered}$$
由（7）可知
${\theta }_x=\arctan 2\left(l^{\prime },1-{\kappa }_{r}l\right)+{\theta }_r$

不过${\theta }_x$需要限制

$v=\sqrt{{\dot{s}}^2{\left(1-{\kappa }_{r}l\right)}^2+{\dot{l}}^2}$
$\kappa 和a$由(11),(12)可得

$a=\ddot{s}\frac{1-{\kappa }_{r}l}{\cos \Delta \theta }+\frac{\dot{s}}{\cos \Delta \theta }\left(\left(1-{\kappa }_{r}l\right)\tan \Delta \theta \frac{d\Delta \theta }{ds}-d{\kappa }_{r}l-{\kappa }_r{l}^{\prime }\right),\Delta \theta ={\theta }_x-{\theta }_r$

$l^{\prime \prime }=-\left(d{\kappa }_{r}l+{\kappa }_r{l}^{\prime }\right)\tan \left({\theta }_x-{\theta }_r\right)+\frac{1-{\kappa }_{r}l}{{\cos }^2\left({\theta }_x-{\theta }_r\right)}\left({\kappa }_x\frac{1-{\kappa }_{r}l}{\cos \left({\theta }_x-{\theta }_r\right)}-{\kappa }_r\right)$

可算推完了

sampling

Lattice planner sampling includes:
• Lateral sampling
• Longitudinal sampling
• Time sampling

采样空间
• State space: Those arising from the environment
• Control space: Those arising from vehicle mobility

• Speed planning

•Formulate lateral planning function l(s) and longitudinal planning function s(t) using polynomials based on the sampling.

•Typically, 4th or 5th order polynomials are used to ensure the smoothness of the path.

？

In cruise control (4th order example)
4次多项式拟合，不关注jerk约束，符合实际巡航需要

• Trajectory planning

拟合多项式

约束函数

Constraint variables:
d_0 : initial lateral displacement
sd_0 : initial lateral speed
ssd_0 : initial lateral acceleration
d_1 : sampled lateral displacement
sd_1 : sampled lateral speed
ssd_1 : sampled lateral acceleration

？
For every trajectory generated, it needs to be evaluated to check if it violates any constraint and remove it if it does.

代价函数

Objective: to choose a feasible path that is nearest to the static reference path, and at the same time, avoid large speed change to ensure comfortability and stay away from obstacles.

Longitudinal Objective achievement cost : to choose a feasible path that is nearest to the static reference path.
红灯停，停车线
动态改变代价

Comfort Objective: to choose a feasible path that is having less jerk

Centripetal Objective: to choose a feasible path that is having less less centripetal （向心）accel jerk .

Collision Objective: to choose a path that is furthest from obstacles

横向偏移cost，一般选择中心线，方便变道，有更多的余量
Lateral Acceleration Objective: to choose a feasible path that is having smoother lane change

Reference

深蓝学院
State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments,Thomas M. Howard, Colin J. Green, and Alonzo Kelly
Frenet坐标推导过程整理