概况

Lattice算法隶属于规划模块。规划模块以预测模块、routing模块、高精地图和定位的结果作为输入，通过算法，输出一条平稳、舒适、安全的轨迹，交给控制模块去执行。我们可以看到，规划模块在Apollo中是一个承上启下的重要模块。

在Apollo中，规划算法的输出是一系列轨迹点连成的轨迹。每一个轨迹点包含位置，速度，加速的等信息。

大致工作流程

如下图，红车是自动驾驶汽车，蓝车是其他障碍车，前面蓝色带尖头的曲线是蓝车的预测轨迹。也就是说，这是一个前方即将有车辆并入的场景。

此时，有些司机会按照上图中浅红色的轨迹，选择绕开蓝色的障碍车。另外有一些司机开车相对保守，会沿着右图中深红色较短的轨迹做一个减速，给蓝色障碍车让路。

可以看出，对同一场景，人类司机会有多种处理方法，同样的。Lattice Planner规划算法的第一步是采样足够多的轨迹，提供尽可能多的选择。

Lattice　planner的第二步是计算每一条轨迹计算的cost。这个const考虑了轨迹的可行性，安全性等因素。

有了轨迹的cost之后，第三步就是一个循环检测的过程。在这个过程中，我们每次会挑选出cost最低的轨迹，对其进行物理限制检测和碰撞检测，如果选出来的轨迹不能同时通过这两个检测，就将其筛除，考察下一条cost最低的轨迹。

上图中，假设我们首先挑选出cost最低的是深红色较短的轨迹。但我们发现即便猛踩刹车也无法执行这条轨迹。也就是说，这条轨迹超出了汽车的减速度上限。那么它就无法通过物理限制检测，我们会将其筛除。

假设我们下一条选出来cost最低的轨迹是右图中深红色较长的轨迹。我们会发现若沿着这条轨迹前进，红车会和蓝色障碍车发生碰撞。也就是说，这条轨迹轨迹无法通过碰撞检测。于是只能放弃这条轨迹，考虑下一条cost最低的。

这样的过程循环继续下去，假设我们现在挑选出右图中靠左边的深红色轨迹，它既符合汽车的物理性状，也不会有碰撞风险。

我们最终就将这条轨迹作为规划轨迹输出。

采样过程

首先是采样过程。在正式介绍采样过程之前，作为铺垫，我先来介绍一下Frenet坐标系。在二维平面中，我们通常采用X-Y坐标系来描述问题。但在自动驾驶规划问题中，我们的工作是基于道路的。这种情况下，X-Y坐标系并不是最方便的。所以我们这里需要使用基于车道线横向和纵向的Frenet坐标系。

那么如何用Frenet坐标系来表示一辆汽车的状态呢？首先我们有一条光滑的参考线（右图中红线），我们可以按右图所示将汽车的坐标点投影到参考线上，得到一个参考线上的投影点（图中蓝色点）。从参考线起点到投影点的路径长度就是汽车在Frenet坐标系下的纵向偏移量，用S表示。而投影点到汽车位置的距离则是汽车在Frenet坐标系下的横向偏移量，用L表示。因为参考线是足够光滑的，我们也可通过汽车的朝向、速度、加速度来计算出Frenet坐标系下，横向和纵向偏移量的一阶导和二阶导。

这里需要注意的是，我们将横向偏移量L设计成纵向偏移量S的函数。这是因为对于大多数的汽车而言，横向运动是由纵向运动诱发的。

实现

//FILE:modules/planning/planner/lattice/lattice_planner.h
class LatticePlanner : public PlannerWithReferenceLine {public:explicit LatticePlanner(const std::shared_ptr<DependencyInjector>& injector): PlannerWithReferenceLine(injector) {}common::Status Plan(const common::TrajectoryPoint& planning_init_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) override;common::Status PlanOnReferenceLine(const common::TrajectoryPoint& planning_init_point, Frame* frame,ReferenceLineInfo* reference_line_info) override;
};

LatticePlanner::Plan()

运行接口, 从 frame 中获取多条 reference_line_info 并逐条进行规划. 多条参考线形成跟车/换道/绕行等决策效果.

PlanOnReferenceLine()

1. 离散化参考线的点

ReferencePoint -> PathPoint，同时计算路径累计长度s.

auto ptr_reference_line =std::make_shared<std::vector<PathPoint>>(ToDiscretizedReferenceLine(reference_line_info->reference_line().reference_points()));

2. 匹配在参考线上最近点

PathPoint matched_point = PathMatcher::MatchToPath(*ptr_reference_line, planning_init_point.path_point().x(),planning_init_point.path_point().y());

先在reference_line找到欧式距离最近的点以及下一个点,然后线性插值得到垂直最近点. 具体参见《从Cartesian坐标系向Frenet坐标系下的转换中投影点的选取》

根据参考线起始点计算Frenet坐标系，并构造预测后障碍物查询散列表

  // 3. according to the matched point, compute the init state in Frenet frame.std::array<double, 3> init_s;std::array<double, 3> init_d;ComputeInitFrenetState(matched_point, planning_init_point, &init_s, &init_d);auto ptr_prediction_querier = std::make_shared<PredictionQuerier>(frame->obstacles(), ptr_reference_line);

建图(PathTimeGraph)获取speed_limit与PlanningTarget

http://mp.ofweek.com/auto/a445673027286
+

Apollo：Lattice Planner相关推荐

Apollo学习笔记（12）Lattice Planner规划算法
本文主要参考Apollo开发者社区,以及一些大神的博客,在此膜拜,文末会奉上相关链接. Lattice Planner 规划算法简介之前的相关的规划的算法都是放在无人驾驶专栏下的,Lattice P ...
Lattice Planner从学习到放弃（二）：二次规划的应用与调试
前言一.静态障碍物的引入二.单车道场景中面向静态障碍物的避让 1.轨迹规划 2.轨迹实车执行效果之失败案例三.调试过程记录 1.代码缺失与完善 2.参数的调整总结前言前情提要:Lattic ...
Apollo Lattice Planner学习记录
参考:<Apollo Lattice Planner学习记录> <[Baidu Apollo]6.2 Lattice Planner规划算法> <Apollo中Latti ...
Lattice Planner从学习到放弃（四）：问题与处理
lattice planner调试记录前情提要 1.单车道nudge的实现 2.SL图建立-bug描述与解决 3.狭窄路段通行过程中规划失败下一个议题前情提要 AEB仿真效果前期把apol ...
Baidu Apollo代码解析之Lattice Planner
大家好,我已经把CSDN上的博客迁移到了知乎上,欢迎大家在知乎关注我的专栏慢慢悠悠小马车(https://zhuanlan.zhihu.com/duangduangduang).希望大家可以多多交流, ...
自动驾驶规划 - Apollo Lattice Planner算法【1】
文章目录 Lattice Planner简介 Lattice Planner 算法思路 1. 离散化参考线的点 2. 在参考线上计算匹配点 3. 根据匹配点,计算Frenet坐标系的S-L值 4. p ...
自动驾驶之轨迹规划6——Apollo EM Motion Planner
1. 概述 <Baidu Apollo EM Motion Planner>是Apollo官方的出的文章(虽然是官方文章,但其中有表述不是很严谨,也有一些小错误,大家可以审视的研读这篇文章 ...
环境感知与规划专题（十一）——状态栅格规划器（State Lattice Planner）
前言前几篇中介绍了几种路径搜索算法,其中既有基于图搜索的路径搜索算法(如A*),也有基于采样的路径搜索算法(如RRT.RRT*).通过路径搜索算法,我们能够得到一条可通行的路径,然而,对于这条路径 ...
Apollo：planning模块概况
决策规划模块我们来看下Apollo中的planning.本文以Apollo项目2019年初的版本为基础进行讲解.版本:3.5 Apollo系统中的Planning模块实际上是整合了决策和规划两个功能 ...

Apollo：Lattice Planner

概况