Planning Algorithm

一. 单一路段规划

从道路中提取Vehicle可能会到达的目标点，采用Cost Function对不同的路径进行评估，并挑选出最合适的路径。

根据当前的点以及此后可能到达的目标点进行路径规划，路径的选取相对来说比较柔和，使得在每一个拐点处它的曲率成线性变换。这个地方多采用多项式拟合的方法，在后面我们将看到三次多项式(cubic)或者高阶的五次多项式(quintic)将会被应用到，而且根据车速以及路况的不同，两种方式可以切换。对于每一个制定的路径，选取三种运动策略：

1. 提高线速度；

2. 降低线速度；

3. 保持线速度；

如上图所示，从一点出发到达每一个终点被划分为三种状态。

二. 路径估计

对于路径的估计非常关键，这里我们采用耗损函数来完成这一任务。对于每一条路径Ts，Cost Function——C(Ts)根据路径与障碍物的距离定义他的安全指数，指数越高说明安全性越低。如下图所示，对不同的路径以及不同的速度模式分别给出了其对应的安全指数。

现在我们引入几个参变量，Sf(Ts)，Tf(Ts)分别对应于路径的终止点以及终止时间，定义耗损函数

M(Ts) = C(Ts) + Phi(Ts)

其中Phi(Ts)是对目标点的评估函数。

Phi(Ts) = Phi_c(Ts) + Phi_h(Ts)

Phi_c(Ts) = -Ks*Sf(Ts) + Kt*Tf(Ts)

Phi_h(Ts) = Hd(Sf(Ts))  + Ht(Tf(Ts)) 

Phi_c(Ts)根据运行的距离以及时间评估，距离越短，时间越长，得分越高；Phi_h是为了确保运行时间至少持续了一个给定的时间长度，而且使得选择偏向于直接到达最远点的路径。

三. 多路段路径规划

由于障碍物的存在，使得在多路段规划的时候需要多次切换车道。但是如何从多条路径中选取适合的道路是一个棘手的问题，如下图所示。

为了简化这一问题，我们首先采用剪枝法删去多余的节点。我们建立一张Time——velocity的表，在每一个方框中的每一种类型速度保留一个节点。关于为什么对于每个初始节点要要保留不同类型的速度模式，而不是取出其中最好的，我们在后面的将会有介绍。

从初始的节点引出的9种借点模式，对应V_T表中的每一个格子，删除必要的点得到有图所示的节点图，可以看到每个格子中每种速度模式的节点最多只有一个。

实际效果图

总结

以上是生活随笔为你收集整理的Planning的全部内容，希望文章能够帮你解决所遇到的问题。

如果觉得生活随笔网站内容还不错，欢迎将生活随笔推荐给好友。