[PathPlanning]State Lattice Planner

StateLatticePlanner核心思路是先采样生成状态栅格地图，最后利用search方法生成自车运行轨迹。该planner有以下两种特性：

1. LatticePlanner本质上是对轨迹离散化采样，并不能得到全局最优轨迹。
2. LatticePlanner适用于运动有一定约束的对象。对于能够执行360度运动机器人而言，LatticePlanner会限制其运动能力。

StateLattice和Grids的主要区别是栅格节点包含自车的状态信息，借用维基百科对lattice graph的定义：

A lattice graph, mesh graph, or grid graph is a graph whose drawing, embedded in some Euclidean space ${\mathbb{R}}^n$, forms a regular tiling.

StateLattices考虑到每个顶点机器人的动力学模型，用来生成采样点之间的可行路径，而Grids只是简单的对空间维度进行等间距离散采样，由此采样得到的路径可能不符合机器人的运动模型。本质上两者的联系都是对连续的空间进行离散化，然后利用图搜索的算法生成机器人的运动轨迹。

状态栅格采样算法

StateLattice生成算法主要包括控制空间采样和状态空间采样。控制空间中的采样没有目的性，离散控制量得到的运动路径是不合格的，会使机器人运动到合理的运动环境之外，得到的局部轨迹仅仅有一部分可以使用；状态空间中的采样可以保证局部轨迹可用。

首先考虑包含5个变量的单车模型$q(t)=[x(t),y(t),\theta (t),\kappa (t),v(t)]$，其中，$x(t),y(t)$为全局位置坐标，$\theta (t)$为车辆航向角，$\kappa (t)$为路径曲率，$v(t)$为纵向速度，其运动学方程可以表述为：

$$\begin{array}{rl}\dot{x(t)}& =v(t)\cos \theta (t)\\ \dot{x(t)}& =v(t)\sin \theta (t)\\ \dot{\theta }(t)& =\kappa (t)v(t)\\ \dot{\kappa }(t)& =u(t)\end{array}\text{\hspace{1em}(1)}$$

其中，$u(t)$可以理解为转向速度。该模型的输入包含$u(t)$和$v(t)$，简单表示为$u=[u(t),v(t)]$。

• 控制空间采样是对给定的被控对象的控制输入$u$在时间$t$上进行离散采样，进而前向积分得到被控对象从任意初始状态$s_0$过度到终止状态$s_f$。因此，该方法很容易实现，下文不再介绍。

• 状态空间采样是对给定被控对象目标状态进行离散采样，反向计算区间拟合的参数。该方法可以建模成边界值问题(Boundary Value Problem, BVP)的求解，可以利用shooting method求解中间过程。

反向计算

反向计算方法是解决状态空间采样点之间的路径连接的方法。假设车辆初始状态为$q(s_0)=[x(s_0),y(s_0),\theta (s_0),\kappa (s_0)]$，终点状态为$q(s_f)=[x(s_f),y(s_f),\theta (s_f),\kappa (s_f)]$。

公式(1)中车辆模型描述了自车运动状态和时间之间的关系。转换到frenet下，可以描述为自车状态与路径长度的关系：

$$\begin{array}{rl}x(s)& ={\int }_0^s\cos \theta (s)ds\\ y(s)& ={\int }_o^s\sin \theta (s)ds\\ \theta (s)& ={\int }_0^s\kappa (s)ds\\ \kappa (s)& =u(s)\end{array}\text{\hspace{1em}(2)}$$

不难发现，输入项$u=[u(s)]$只包含转向速度，与速度无关。

道路模型

道路通过其中心线定义，通常也可以定位行为的参考线，可以表示为四维向量：
$$q(s)=[x(s),y(s),\theta (s),$$