百度还有很多大厂都使用Lattice作为轨迹规划的方法，之前都是做低速规划，这部分接触的少，最近有机会接触Lattice规划，我来讲讲我的看法并给一个简单的Matlab实现，这部分使用Matlab实现的相关文章还是不多。

讲到Lattice肯定绕不过Optimal trajectories for time-critical street scenarios using discretized terminal manifolds经典，之前我也简单介绍过，柳梦璃：MATLAB路径规划算法由于当前理解的不够深入介绍的比较浅薄。

首先引入Frenet坐标系，见下图。对于车辆坐标系肯定首先想到直角坐标系。如果对于完全平直道路，其实直角坐标系和Frenet坐标系是重合的，下图中第二个case就是这种情况。Frenet坐标系以道路的中线作为其中的一个坐标轴为s轴，垂直与s轴作为d轴，这就造成了在s轴不同点所取得的d轴不平行。

这样做的好处，请想象如果我们能将s轴展开为直线，这样，如果就等同于上图的第二个case，从s轴上的任意点取得相同的d值对于道路中线的偏移是一样的。由于我们规划的轨迹就是想让车辆尽量沿着道路中线附近行驶，这样对于Frenet坐标系可以表示为规划轨迹的终点d（取d为比较小的值），对于直角坐标系却没有很好的表示方法。还有对于相同的s表示车辆需要行驶的路线长度一致，而直角坐标系下相同的x无法代表同样的意义。如果车辆一直沿着直线形式，采用直角坐标系绝对是最好的，但道路不是永远是直的，对于弯曲道路来说Frenet坐标系反而比直角坐标系自然。

在介绍Lattice之前，我们首先问一个问题，我们是否能够通过一次规划到达终点，显然在高速情况下终点在很远处，我们无法一次规划到终点，即使算力足够场景的变化也造成无法可以一次规划完成，因为我们不是全知全能，对场景的预测时间相对比较短。在不能一次到达终点的情况下我们规划所得就是到到某个中间状态的一段路径。这时候我们就会自然产生两种做法，一种不首先确定中间的状态，如DWA算法和很多Reactive的算法，很多基于MPC的方法，其实中间状态也不重要，这种算法路径的产生和路径所要达到的终点没有关系;一种就是路径是由由起点和终点所确定的没有终点就没法产生路径，如Lattice和触须算法。那我们再深入问一个问题，我们一次规划能否就只使用一个终点来产生路径，这显然不行，那么必然Lattice就是一种基于对终点采样的算法，一次先产生若干个终点再产生若干条路径，在这些路径中选择最好的一条。

这中规划思路反过来也要求Lattice使用Frenet坐标系，在Frenet坐标系下终点的位置可以表示为<s,d>的组合对，表达的意义很清晰，就是在前方道路中线附近的采样，试想如果采用直角坐标系终点位置该如何确定。

A. 终点位置采样，横向采样

下图代表一种采样方法，其中黑点为初始点，车辆当前的状态，红色为采样点，可见沿着s方向的采样就是沿着道路沿升方向采样，沿着d方向采样就是沿着道路扩展方向采样，使用Frenet表达采样方法符合直觉。

B. 速度曲线采样，纵向采样

当然我们还需要速度来控制车辆的油门和刹车。沿用横向采样的思路我们首先需要确定一个终点来规划整条曲线。很自然的问题是我们需不需要在速度规划时候考虑横向，答案为不需要，这里实际上使用PV(Path-Velocity)分离的思想。下图代表对于纵向的采样，分为两种情况是因为有不同的需求，如果不需对终点的位置做保证，如巡航工况就只需要采用下图右侧的采样策略，如果需要对位置同时也就要求如停车和跟车就采用下图左侧的采样策略，左侧产生的曲线s(t)为5阶曲线，右侧产生的v(t)为4阶曲线。

这里可以做第一阶段的总结，Lattice中采用PV分离的思想，曲线的产生依赖与终点的状态，采用Frenet坐标系。以下要解决的问题有PV分离后怎么结合，怎么由Frenet转换到直角坐标系中（控制算法大部分表示在直角坐标系中）和其相反问题，如何选择好的轨迹。