Vehicle Motion Planning/Local Planner【Autonomous Vehicle Planning and Control】

局部规划的输出是带有速度信息的一系列轨迹点组成的轨迹，其保证了车辆控制器在车辆跟踪控制过程中的平稳性和安全性。

Stochastic Sampling Methods

RRT（RRT*）

在这个代码中，M是地图环境，x_init是起始位置，x_goal是目标位置。路径经过的随机节点是x_rand。

• 路径空间搜索的过程从起点开始，先随机撒点x_rand；
• 然后查找距离x_rand最近的节点x_near；
• 然后沿着x_near到x_rand方向用步长stepsize的距离得到x_new；
• (RRT*: 以x_new为圆点，r为半径画圆，找到cost最小从x_init - x_near - x_new的路径，此时cost最小路径中的x_near作为父节点。）
• CollisionFree(M, E)方法检测Edge(x_new, x_near)是否与地图M上的障碍物有碰撞，如果没有碰撞，则将这两个点之间增加一条边到图E中；
• 重复上述过程，直至达到目标位置。
  

Lattice 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:

1. 将车辆当前位姿信息转换到frenet坐标系下，获得车辆在frenet坐标系的初始状态；根据当前速度计算前瞻距离，获得前瞻点，获得车辆在前瞻点位置frenet坐标系下的目标状态。
2. 对轨迹状态（轨迹运行时间t，目标速度v，及到参考线的横向位移d）进行采样，通过这三个规划参数可以获得采样状态。
3. 构建横向位移和纵向位移的多项式规划函数s(t)，d(s)
4. 获得横向位移和纵向位移的规划函数后，进行时间插值就可以获得参考线frenet坐标系下的轨迹点
5. 最后将轨迹点从frenet坐标系转换到cartesian坐标系，就可以获得物理世界采样轨迹，由于横向和纵向都是通过高次多项式插值获得，以此cartesian坐标系下的轨迹也是光滑的。
6. 采样轨迹的碰撞检测、曲率约束及最优轨迹（cost）打分。采样轨迹是一系列满足速度约束的光滑轨迹，但其还需要满足无碰撞和车辆运动学曲率约束的强制约束，及远离障碍物和靠近参考线等组成的代价约束。采样轨迹的打分就是为了获得一条最优的满足约束条件的无碰撞光滑轨迹。该轨迹也是lattice输出到controller用于车辆跟随的轨迹。

Frenet coordinate and Cartesian coordinate transformation（1,5）

Frenet coordinate is a frame on the reference line, is a moving frame. Its origin point r is the nearest point from the vehicle to the path. The t-axis is along the tangiential direction at r while n-axis is pependicular to t-axis.

Cartesian to Frenet
The transformation equations from Cartesian to Frenet coordinates are as follows:

$$\begin{array}{rl}s& =s_r,\\ \dot{s}& =\frac{v_x\cos ({\theta }_x-{\theta }_r)}{1-k_{r}l},\\ \ddot{s}& =\frac{a_x\cos ({\theta }_x-{\theta }_r)-{\dot{s}}^2\left(l^{\prime }(k_x\frac{1-k_{r}l}{\cos ({\theta }_x-{\theta }_r)}-k_r)-(k_r^{\prime }l+k_r{l}^{\prime })\right)}{1-k_{r}l},\\ l& =\text{sign}\left((x_x-x_r)\cos ({\theta }_r)(y_x-y_r)\sin ({\theta }_r)\right)((x-x_r{)}^2+(y-y_r{)}^2{)}^{\frac{-1}{2}},\\ l^{\prime }& =(1-k_{r}l)\tan ({\theta }_x-{\theta }_r),\\ l^{\prime \prime }& =-({k^{\prime }}_{r}l+k_r{l}^{\prime })\tan ({\theta }_x-{\theta }_r)+\frac{1-k_{r}l}{{\cos }^2({\theta }_x-{\theta }_r)}\left(\frac{1-k_{r}l}{\cos ({\theta }_x-{\theta }_r)}{k}_x-k_r\right),\end{array}$$

Frenet to Cartesian
The transformation equations from Frenet to Cartesian coordinates are as follows:

$$\begin{array}{rl}{x}_x& =x_r-l\sin ({\theta }_r),\\ y_x& =y_r+l\cos ({\theta }_r),\\ {\theta }_x& =\arctan \left(\frac{l^{\prime }}{1-k_{r}l}\right)+{\theta }_r,\\ v_x& =\sqrt{[\dot{s}(1-k_{r}l{)}^2+(\dot{s}{l}^{\prime }{)}^2]},\\ a_x& =\ddot{s}\frac{1-k_{r}l}{\cos ({\theta }_x-{\theta }_r)}+\frac{{\dot{s}}^2}{\cos ({\theta }_x-{\theta }_r)}\left(l^{\prime }(k_x\frac{1-k_{r}l}{\cos ({\theta }_x-{\theta }_r)}-k_r)-(k_r^{\prime }l+k_r{l}^{\prime })\right),\\ k_x& =\left((l^{\prime \prime }+(k_r^{\prime }l+k_r{l}^{\prime })\tan ({\theta }_x-{\theta }_r))\frac{{\cos }^2({\theta }_x-{\theta }_r)}{(1-k_{r}l)}+k_r\right)\frac{\cos ({\theta }_x-{\theta }_r)}{1-k_{r}l},\end{array}$$

Lattice planner sampling（2）

Lattice规划器的轨迹采样，主要分为横向采样、纵向采样以及轨迹时间周期采样。 横向轨迹的采样需要涵盖多种横向运动状态，需要根据车道宽度设置横向采样的采样区间，通过横向采样间隔，形成不同的横向采样偏移量。纵向采样的采样区间可以通过前瞻点的位移长度s，作为基准采样长度，然后通过对轨迹速度ds进行采样。时间周期采样，就是对轨迹的运行周期时间进行采样。

而百度Apollo的轨迹采样，只对横向位移和纵向位移进行了采样，并设计了采样状态横向偏移量，-0.5，0.0和0.5，以及四个到达这些横向偏移量的纵向位移，分别为10，20，40，80来得到采样状态。所以Lattice规划器的轨迹采样主要是对轨迹横纵向状态进行采样，但采样方式可以根据环境情况进行调整。

speed planning（3）

有了前面的采样状态，现在需要做的是根据采样状态生成横向和纵向和规划函数.
两种规划函数都是通过多项式进行拟合求解生成。主要使用了4次和5次多項式拟合，从而满足了车辆运行过程中的一阶导，二阶导联系，也就是速度和加速度连续，保证了轨迹的平滑性要求。

longitudinal planning function 𝑠(𝑡)

在停车和跟车状态，都是五次多项式，但对于巡航状态，由于我们不需要确定状态的S值，所以只有五个变量，因此用四次多项式就可以了。

In stop and go, or spacing control (5th order)
The equations are given by:

• Position (s):
  $$s(t)=c_1{t}^5+c_2{t}^4+c_3{t}^3+c_4{t}^2+c_{5}t+c_6$$
• Velocity (v):
  $$v(t)=5c_1{t}^4+4c_2{t}^3+3c_3{t}^2+2c_{4}t+c_5$$
• Acceleration (a):
  $$a(t)=20c_1{t}^3+12c_2{t}^2+6c_{3}t+2c_4$$
  
  解方程组，即可得到多项式系数 。

In cruise control (4th order)
The equations are outlined as:

• Position (s):
  $$s(t)=b_1{t}^4+b_2{t}^3+b_3{t}^2+b_{4}t+b_5$$
• Velocity (v):
  $$v(t)=4b_1{t}^3+3b_2{t}^2+2b_{3}t+b_4$$
• Acceleration (a):
  $$a(t)=12b_1{t}^2+6b_{2}t+2b_3$$
  
  解方程组，即可得到多项式系数 。

lateral planning function 𝑙(𝑠)
对于横向轨迹也使用了五次多项式拟合。

lateral planning function 𝑙(𝑠)

Lateral fitting polynomial solution

The polynomial equations are:

• Lateral displacement ( d(s) ):
  $$d(s)=k_1{s}^5+k_2{s}^4+k_3{s}^3+k_4{s}^2+k_{5}s+k_6$$

• Lateral velocity ( d_v(t) ):
  $$d_v(t)=5k_1{s}^4+4k_2{s}^3+3k_3{s}^2+2k_{4}s+k_5$$

• Lateral acceleration ( d_a(t) ):
  $$d_a(t)=20k_1{s}^3+12k_2{s}^2+6k_{3}s+2k_4$$

Constraint function
The polynomial constraint functions are defined as:

• For ( s = s_0 ) (initial condition):
  $$d(s_0)=k_1{s}_0^5+k_2{s}_0^4+k_3{s}_0^3+k_4{s}_0^2+k_5{s}_0+k_6=d_0$$
  $$d_v(s_0)=5k_1{s}_0^4+4k_2{s}_0^3+3k_3{s}_0^2+2k_4{s}_0+k_5=s{d}_0$$
  $$d_a(s_0)=20k_1{s}_0^3+12k_2{s}_0^2+6k_3{s}_0+2k_4=ss{d}_0$$

• For ( s = s_1 ) (sampled condition):
  $$d(s_1)=k_1{s}_1^5+k_2{s}_1^4+k_3{s}_1^3+k_4{s}_1^2+k_5{s}_1+k_6=d_1$$
  $$d_v(s_1)=5k_1{s}_1^4+4k_2{s}_1^3+3k_3{s}_1^2+2k_4{s}_1+k_5=s{d}_1$$
  $$d_a(s_1)=20k_1{s}_1^3+12k_2{s}_1^2+6k_3{s}_1+2k_4=ss{d}_1$$

• 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

知道了位姿点在frenet坐标系和cartesian坐标系的相互转换关系，因此现在我们需要做的就是对横纵向轨迹函数s(t)和l(s)进行轨迹的时间细分形成规划函数的横纵向轨迹规划点s(t_i)和l(s(t_i))，该规划点是在frenet坐标系中，因此需要进行frenet坐标系到cartesian坐标系的坐标转换，从而形成控制器可用的采样轨迹。

Constraint evaluation

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

1. 开始轨迹评估：初始化轨迹评估过程。
2. 对每条轨迹进行约束评估：按序对每条轨迹进行检查。
3. 对每个样本进行约束评估：在每条轨迹中，逐个检查每个样本点是否符合速度、加速度和急动度的限制。
4. 如果样本点不符合约束，移除整条轨迹，回到第2步，检查下一条轨迹。如果样本点符合约束，继续检查轨迹的下一个样本。
5. 完成所有样本的检查：对一条轨迹的所有样本完成约束检查。
6. 完成所有轨迹的检查：当一条轨迹的所有样本点都符合约束条件后，继续进行下一条轨迹的评估，直到所有轨迹都被检查。

trajectory cost function（6）

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.（满足车辆的运动学控制要求，速度尽量不发生大突变，满足舒适性要求，且尽量远离障碍物和无碰撞要求）

The cost function J is defined as:

$$J=k_{longi}\times J_{longi}+k_{comfort}\times J_{comfort}+k_{collision}\times J_{collision}$$

• tracking cost, considering speed error, distance to travel.
• comfort cost, considering longitudinal jerk（有的考虑横向向误差，综合考虑了横向加速度误差及横向偏移误差的影响。）
• cost of collision to the near objects.

选择出代价值最好的一条轨迹输入到控制器，用于控制器的跟踪控制。

Longitudinal Objective achievement cost
to choose a feasible path that is nearest to the static reference path.（有停车指令时，高速cost会很大，相反，低速cost会很大通过V_ref）

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

Lateral offset Objective:
to choose a feasible path that is more close to the reference (center line)

Lateral Acceleration Objective:
to choose a feasible path that is having smoother lane change

最后选择出代价值最好的一条轨迹输入到控制器，用于控制器的跟踪控制。