自动驾驶（七十）---------五阶轨迹规划之LatticePlan

63 阅读 0 评论 42 点赞

我是靠谱客的博主现实黄蜂，最近开发中收集的这篇文章主要介绍自动驾驶（七十）---------五阶轨迹规划之LatticePlan，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

规划是门很深的学问，也是自动驾驶的核心功能，所以很难通过一两篇文字讲清楚，所以我也会持续更新，今天要讲的是Apollo的规划方式，网上有一篇ppt的介绍和我自己对源码的分析，总结出这样一篇文章，原计划是去年年底把我的博客冲到5000以内，但是去年公司发生一些事受到影响，还好年后又趋于稳定，但是不免又懈怠了下来，不过始终不忘初心，对于提高自己自动驾驶的能力念念不忘，相信博客还会持续更新。

很多人质疑过：写博客就能提高自动驾驶能力吗？理论和实践还差的很远呢。我对这件事的理解是：理论实践相结合是最高目标，所谓“知行合一”，为此阳明先生亲口做出过解释：“知为行之始，行为知之成”。写博客能解决知的问题，至于行就需要日常工作生活中不断提高对自己的要求，切勿眼高手低、想一蹴而就，依靠长期的修行自然水到渠成。另外头条也会分享一些我的生活和感悟，相信以后自己回看也很有味道吧。

1. 规划的输入输出

规划模块以预测模块、routing模块、高精地图和定位的结果作为输入，通过算法，输出一条平稳、舒适、安全的轨迹，交给控制模块去执行。一个合格规划算法，必须满足几个条件：

1. 必须能够使自动驾驶汽车到达目的地；
2. 必须符合交规；
3. 能够避免碰撞；
4. 也需要能保证一定的舒适性。

2. 工作流程

1. 对于任何一个场景，都有无数的轨迹应对当前的场景，有的激进，有的保守，那么Lattice规划算法的第一步就是采样足够多的轨迹，提供尽可能多的选择。

2. 第二步是计算每一条轨迹计算的cost。这个cost考虑了轨迹的可行性、安全性等因素。

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

4. 假设我们现在挑选出轨迹，它既符合汽车的物理性状，也不会有碰撞风险，就将这条轨迹作为规划轨迹输出。

3. 采样过程

1. 首先我们可以通过计算得到自动驾驶汽车在Frenet坐标系下的在零时刻的起始状态，也就是汽车的当前状态。为了生成一条轨迹，第一步就是在Frenet坐标系下采样一个在T1时刻的末状态。

2. 第二步就是将末状态和起始状态做多项式拟合。分别形成横向和纵向的多项式轨迹。

3. 第三步就是二维合成。给定一个时刻T*，我们可以计算出在T*时刻的纵向偏移量和横向偏移量，再通过参考线，即可还原成一个二维平面中的轨迹点。

4 .计算Cost

我们前面提到，轨迹规划所需要满足的四点要求，分别是到达目的、符合交规，避免碰撞、平稳舒适。针对这四点要求，我们设计了六个cost，cost越高就表示越不满足要求。

1. 首先是到达目的的cost。这里分成两种情况，一个是存在停车指令（比如红灯）的情况，另一个是没有停车指令的。如果存在停车指令，相对大的车速，其对应的轨迹cost就越大；如果没有停车指令，那么低速轨迹的cost就会越大。

2. 第二个cost是横向偏移cost。设计这个cost是为了让自动驾驶汽车能尽量沿着道路中心行驶。那么像左图汽车靠道路一边行驶，和中图画龙的行驶轨迹，他们的cost都相对较高。

3. 第三个cost是碰撞cost。有碰撞风险，那么它的碰撞cost就会相对较高。

4. 第四个cost是纵向加加速度的cost。加加速度（jerk）是加速度对时间的导数，表示加速度的变化率。我们用加加速度的最大值值来表示这个cost。

5. 第五个cost是横向加速度的cost。设计这个cost是为了平稳地换道。那么像左图猛打方向盘的轨迹，它的横向加速度cost就会相对较大。

6. 最后一个cost是向心加速度cost。设计这个cost是为了在转弯或调头的时候能够减速慢行。在弯道处，车速慢的轨迹，其向心加速度cost就会相对较低，那么就会更容易被率先挑选出来。

这六个cost的加权求和就是轨迹的总cost。开发者可以根据产品的需要，调试这六个权重。

对于换道场景，Lattice算法仅仅需要对目标车道对应的参考线做一次采样+选择的流程。本车道和目标车道均能产生一条最优轨迹。给换道轨迹的cost上增加额外的车道优先级的cost，再将两条轨迹比较，选择cost较小的那条即可。

5. 代码简介

1. Lattice planner程序入口：

Status LatticePlanner::PlanOnReferenceLine(const TrajectoryPoint& planning_init_point, Frame* frame,ReferenceLineInfo* reference_line_info)；

2. 获得当前参考线（车辆未来轨迹），并通过TodiscretizedReferenceLine进行参考线的离散化.

auto ptr_reference_line = std::make_shared<std::vector<PathPoint>>(ToDiscretizedReferenceLine( reference_line_info->reference_line().reference_points()));
3. 使用规划原点匹配第一步离散化后的轨迹，找出匹配点matched_point。

PathPoint matched_point = PathMatcher::MatchToPath(*ptr_reference_line, planning_init_point.path_point().x(), planning_init_point.path_point().y());
4.使用ComputeInitFrenetState计算当前状态下，Frenet坐标系下的点，S和D值，横向速度，纵向速度等。

ComputeInitFrenetState(matched_point, planning_init_point, &init_s, &init_d);
5.加入感知信息，将之前感知模块内容加入到path_time_grapth，也就是所谓的ST-graph

auto ptr_prediction_querier = std::make_shared<PredictionQuerier>( frame->obstacles(), ptr_reference_line);
加入感知信息时，里面涉及多个函数，具体包括障碍物筛选，去掉与车辆未来轨迹不发生冲突的障碍物，设置动态障碍物。

在这里还有一个planning_target判断，即判断是否有停车点，如果有的话打印出停车点的s值（Frenet坐标系下的纵向值）.

6. 这一步很关键，该步是生成横纵向轨迹：

Trajectory1dGenerator trajectory1d_generator(init_s, init_d, ptr_path_time_graph, ptr_prediction_querier);
std::vector<std::shared_ptr<Curve1d>> lon_trajectory1d_bundle;
std::vector<std::shared_ptr<Curve1d>> lat_trajectory1d_bundle;
trajectory1d_generator.GenerateTrajectoryBundles(planning_target, &lon_trajectory1d_bundle, &lat_trajectory1d_bundle);
涉及的函数很多，具体包括纵向轨迹生成以及横向轨迹生成，通过设置初始状态、末状态，通过五次多项式拟合，得出相关系数，再通过求导，即可得出速度，再求导得出加速度。再求导得出的加加速度，也就是Jerk，这是用来评价轨迹的舒适度的变量。

7. 横向控制是根据纵向控制来进行采样其中横向位移为{0，-0.5,0.5}、纵向位移为{10,20,40,80}分别采样，最后得到多组横向的轨迹，同样也是使用5次多项式拟合。注意：横向的轨迹是以纵向位移s为自变量.

void Trajectory1dGenerator::GenerateLateralTrajectoryBundle();

8.轨迹评价函数上一步生成了许多条轨迹，这里就需要选择出cost最小的一条轨迹作为未来轨迹，

TrajectoryEvaluator trajectory_evaluator(init_s, planning_target, lon_trajectory1d_bundle, lat_trajectory1d_bundle, ptr_path_time_graph, ptr_reference_line);
Evaluate评估的时候有5个cost值，分别是