Lattice Planner规划算法.docVIP

Lattice Planner规划算法 　　人类在驾驶时面对不同的路况会有不同的解决方案，自动驾驶系统也是如此。而决策规划就如大脑进行状况预测。这一预测过程涵盖了路况分析、行为决策、动作规划和反馈控制等多重内容，同时还要重视安全和舒适两大要点，进而构成了自动驾驶系统的核心部分。 　　想要同时兼顾这些条件并不容易，当前在业界关于决策规划也并没有成熟统一的解决方案。但一些优秀的规划算法却可以很好的完成这些任务。比如Lattice Planner，遵循先采样后选择的逻辑，依据从安全性、舒适性和交规抽象出的cost函数来保障轨迹的平滑。 　　上周，来自百度Apollo的高级研发工程师许珂诚，在Apollo开发者社群内分享了有关Lattice Plannner的内容，帮助开发者更加了解Apollo核心算法的逻辑与细节。 　　Lattice Planner 总体概览 　　 　　Lattice算法隶属于规划模块。规划模块以预测模块、Routing模块、高精地图和定位的结果作为输入，通过算法，输出一条平稳、舒适、安全的轨迹，交给控制模块去执行。我们可以看到，规划模块在Apollo中是一个承上启下的重要模块。 　　 　　这是Apollo中规划模块的工作流程。首先是依据Routing和定位，通过平滑算法，生成一条平滑的参考线（平滑的道路中心线）。再通过规划算法，生成一条符合交规，安全舒适的规划轨迹。那么Lattice算法就是Apollo开源平台中，其中的一种规划算法。 　　 　　一个合格规划算法，必须满足几个条件。首先，必须能够使自动驾驶汽车到达目的地；其次，必须符合交规；第三，能够避免碰撞；最后，也需要能保证一定的舒适性。在Apollo中，规划算法的输出是一系列轨迹点连成的轨迹。每一个轨迹点包含位置，速度，加速的等信息。 　　2Lattice 规划算法的工作流程 　　 　　下面介绍一下Lattice规划算法的工作流程。我们以右图中的场景为例。其中红车是我们的自动驾驶汽车，蓝车是其他障碍车，前面蓝色带尖头的曲线是蓝车的预测轨迹。那么这是一个前方即将有车辆并入的场景。 　　 　　面对这样的场景，有些司机会按照右图中浅红色的轨迹，选择绕开蓝色的障碍车。另外有一些司机开车相对保守，会沿着右图中深红色较短的轨迹做一个减速，给蓝色障碍车让路。 　　 　　既然对于同一个场景，人类司机会有多种处理方法，那么Lattice规划算法的第一步就是采样足够多的轨迹，提供尽可能多的选择。 　　 　　Lattice规划算法的第二步是计算每一条轨迹计算的cost。这个cost考虑了轨迹的可行性、安全性等因素。会在后面为大家详细介绍。 　　 　　那么有了轨迹的cost以后，第三步就是一个循环检测的过程。在这个过程中，每次会先挑选出cost最低的轨迹，对其进行物理限制检测和碰撞检测。如果挑出来的轨迹不能同时通过这两个检测，就将其筛除，考察下一条cost最低的轨迹。 　　 　　以右图为例，假设首先挑选出cost最低的是深红色较短的轨迹。但我们发现即便猛踩刹车也无法执行这条轨迹。也就是说，这条轨迹超出了汽车的减速度上限。那么它就无法通过物理限制检测，我们会将其筛除。 　　 　　假设下一条选出来cost最低的轨迹是右图中深红色较长的轨迹。会发现若沿着这条轨迹前进，红车会和蓝色障碍车发生碰撞。也就是说，这条轨迹轨迹无法通过碰撞检测。于是只能放弃这条轨迹，考虑下一条cost最低的。 　　 　　这样的过程循环继续下去，假设现在挑选出右图中靠左边的深红色轨迹，它既符合汽车的物理性状，也不会有碰撞风险。 　　 　　最终就将这条轨迹作为规划轨迹输出。 　　那么下面将对每一个步骤，进行详细的说介绍。 　　3Lattice Planner的采样过程 　　 　　首先是采样过程。在正式介绍采样过程之前，作为铺垫，先来介绍一下Frenet坐标系。在二维平面中，通常采用X-Y坐标系来描述问题。但在自动驾驶规划问题中，我们的工作是基于道路的。这种情况下，X-Y坐标系并不是最方便的。所以我们这里需要使用基于车道线横向和纵向的Frenet坐标系。 　　那么如何用Frenet坐标系来表示一辆汽车的状态呢？首先有一条光滑的参考线（右图中红线），可以按右图所示将汽车的坐标点投影到参考线上，得到一个参考线上的投影点（图中蓝色点）。从参考线起点到投影点的路径长度就是汽车在Frenet坐标系下的纵向偏移量，用S表示。而投影点到汽车位置的距离则是汽车在Frenet坐标系下的横向偏移量，用L表示。因为参考线是足够光滑的，也可通过汽车的朝向、速度、加速度来计算出Frenet坐标系下，横向和纵向偏移量的一阶导和二阶导。 　　这里需要注意的是，我们将横向偏移量L设计成纵向偏移量S的函数。这是因为对于大多数的汽车而言，横向运动是由纵向运