参考：《Apollo Lattice Planner学习记录》

《【Baidu Apollo】6.2 Lattice Planner规划算法》

《Apollo中Lattice规划器结构梳理》

EM Planner在之前的博客中通过对论文详细学习了一遍，这次通过代码学习一下Apollo的Lattice Planner。

一、背景

Apollo里基于参考线的Planner主要有EM Planner与Lattice Planner。两者区别如下:

EM Planner	Lattice Planner
横向纵向分开求解	横向纵向同时求解
参数较多(DP/QP, Path/Speed)	参数较少且统一
流程复杂	流程简单
单周期解空间受限	简单场景解空间较大
能适应复杂场景	适合简单场景
适合城市道路	适合高速场景

EM Planner在之前的博客中通过对论文详细学习了一遍，这次通过代码学习一下Apollo的Lattice Planner，听说某些ACC等功能的量产的方案与其非常相似，值得学习一下。后面的过程基本上按照代码的执行顺序进行的解读.

一个合格规划算法，必须满足几个条件：首先，必须能够使自动驾驶汽车到达目的地；其次，必须符合交规；第三，能够避免碰撞；最后，也需要能保证一定的舒适性。

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

二、启动流程

2.0 执行planning并启动选择planner

2.0.1 Planning的Proc执行结构

Apollo启动完成初始化后，执行其主要部分proc：
在这里插入图片描述通过调用planning_base_->RunOnce()函数来开启决策规划之旅，目前含有两个大的规划器：navi_planning和on_lane_planning，至于选取哪个，在程序初始化的时候被配置了，详情可以参考参考源中的介绍～目前学习lattice，位于on_lane_planning中
在这里插入图片描述

2.0.2 Planning搞起：RunOnce 结构

在这里，主要做了下图中的事：

2.0.3 RunOnce中选择使用Lattice planner

CyberRT的planning_component类里Init过程中运行planning_base类的RunOnce()函数周期性地运行规划主程序plan()，即根据config与scenario等决定哪一种planner.planner中的PlannerWithReferenceLine子类中有四个子类，其中之一是LatticePlanner。

在RunOnce执行中，调用了planner_->Plan，whose内部包含了：lattice、navi、public_road、rtk_replay四种规划器，找到了我们的lattice
在这里插入图片描述

lattice的主要流程见下图:

Sample Candidate Trajectories 采样候选轨迹
Assign Candidate Trajectories 分配候选轨迹
选择最低代价的轨迹+碰撞检测和道路限制
输出轨迹

LatticePlanner 类头文件如下(删除暂时无用的部分)：

//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;
};

2.1 LatticePlanner::Plan()

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

2.2 PlanOnReferenceLine()

运行主体。官方注释中分为⑦大步骤。
此部分的代码写的层次比较清晰，结合注释能把每一步的作用看懂，并把操作都封装到了一个个单独的类中。共7步，下面对每步进行解读.

2.2.1 获取参考线数据结构转换

obtain a reference line and transform it to the PathPoint format

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

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

2.2.2 匹配在参考线上最近点

《从Cartesian坐标系向Frenet坐标系下的转换中投影点的选取》

compute the matched point of the init planning point on the reference line

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

先在reference_line找到欧式距离最近的点以及下一个点，然后线性插值得到垂直最近点。

PathPoint PathMatcher::MatchToPath(const std::vector<PathPoint>& reference_line,
                                   const double x, const double y) {
  CHECK_GT(reference_line.size(), 0);

  auto func_distance_square = [](const PathPoint& point, const double x,
                                 const double y) {
    double dx = point.x() - x;
    double dy = point.y() - y;
    return dx * dx + dy * dy;
  };

  double distance_min = func_distance_square(reference_line.front(), x, y);
  std::size_t index_min = 0;

  for (std::size_t i = 1; i < reference_line.size(); ++i) {
    double distance_temp = func_distance_square(reference_line[i], x, y);
    if (distance_temp < distance_min) {
      distance_min = distance_temp;
      index_min = i;
    }
  }
  std::size_t index_start = (index_min == 0) ? index_min : index_min - 1;
  std::size_t index_end =
      (index_min + 1 == reference_line.size()) ? index_min : index_min + 1;

  if (index_start == index_end) {
    return reference_line[index_start];
  }
  //线性插值时注意把kappa,dkappa也做处理.
  return FindProjectionPoint(reference_line[index_start],
                             reference_line[index_end], x, y);
}

代码学习点:用三目运算符代替简单的if-else

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

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);

其中构造了PredictionQuerier障碍物查询器，利用google protobuf库的InsertIfNotPresent()把reference_line里的障碍物提取出来成为散列表。

PredictionQuerier::PredictionQuerier(
    const std::vector<const Obstacle*>& obstacles,
    const std::shared_ptr<std::vector<common::PathPoint>>& ptr_reference_line)
    : ptr_reference_line_(ptr_reference_line) {
  for (const auto ptr_obstacle : obstacles) {
    if (common::util::InsertIfNotPresent(&id_obstacle_map_, ptr_obstacle->Id(),
                                         ptr_obstacle)) {
      obstacles_.push_back(ptr_obstacle);
    } else {
      AWARN << "Duplicated obstacle found [" << ptr_obstacle->Id() << "]";
    }
  }
}

2.2.4 建图(PathTimeGraph)获取speed_limit与PlanningTarget

parse the decision and get the planning target
由于Lattice主要适用于高速场景，因此驾驶任务主要分为：巡航，跟车，换道，刹停。完成这几样任务，需要得知目标速度 (道路限速、驾驶员设定、前车速度、曲率限速等，从reference_line数据结构中获取)，目标(无目标巡航；有目标时，跟车、换道、换道超车、刹停等)。
这些任务的决策是通过SL、TS图的分析得到的。

auto ptr_path_time_graph = std::make_shared<PathTimeGraph>(
    ptr_prediction_querier->GetObstacles(), *ptr_reference_line,
    reference_line_info, init_s[0],
    init_s[0] + FLAGS_speed_lon_decision_horizon, 0.0,
    FLAGS_trajectory_time_length, init_d);

double speed_limit =
    reference_line_info->reference_line().GetSpeedLimitFromS(init_s[0]);
reference_line_info->SetLatticeCruiseSpeed(speed_limit);

PlanningTarget planning_target = reference_line_info->planning_target();
if (planning_target.has_stop_point()) {
ADEBUG << "Planning target stop s: " << planning_target.stop_point().s()
        << "Current ego s: " << init_s[0];
}

（1）建图PathTimeGraph类

在构造智能指针的过程中构造PathTimeGraph()类。
输入列表为：

const std::vector<const Obstacle*>& obstacles, //障碍物
const std::vector<common::PathPoint>& discretized_ref_points, //参考线离散点
const ReferenceLineInfo* ptr_reference_line_info, //参考线信息
const double s_start, //s,t的始终
const double s_end, 
const double t_start,
const double t_end, 
const std::array<double, 3>& init_d //s的kappa信息

构造函数内容为：初始化成员变量，设置障碍物SetupObstacles(obstacles,，discretized_ref_points).

根据有没有预测轨迹，将障碍物分为虚拟/静态/动态障碍物。

a. 静态障碍物

SetStaticObstacle(const Obstacle* obstacle,const std::vector<PathPoint>& discretized_ref_points)

SL图	先用`ComputeObstacleBoundary()`计算`SLBoundary`(msg).具体为障碍物的顶点转化为Frenet下,求得`s`与`l`的上下限即为SLBoundary.接着获取参考线的长度以及道路宽度,过滤超出长度以及道路宽度(分左右侧)的障碍物.最后存到`static_obs_sl_boundaries_`中.
ST图	分别设置ST图的矩形四个角点,矩形的长度`t`由标定参数`FLAGS_trajectory_time_length`决定.最后存到`path_time_obstacle_map_`中.

b. 动态障碍物

SetDynamicObstacle()
类似与静态障碍物,只不过t维度上无法采用静止的一条直线描述,只能用0.1s - 8.0s ( FLAGS_trajectory_time_resolution 间隔 ) 采样描述出s维度上的2个点，然后连起来。每一个时间点先把障碍物用GetBoundingBox()转换为common::math::Box2d,然后得到四个顶点GetAllCorners()，也过滤掉超界障碍物，得到ST图，path_time_obstacle_map_。

c. 排序
静态障碍物根据start_s大小排序static_obs_sl_boundaries_SL图容器.
动态障碍物直接存到path_time_obstacles_ST图容器中。

其中，障碍物是通过common::math::Polygon2d类来表达的，此类包括了多边形的常用运算，例如求面积/距离/包含/重合，还有后续碰撞检测用的AABoundingBox()。

（2）获取限速

速度限制优先级：ReferenceLine的每段s处的限速>Lane限速(lane_waypoint.lane->lane().speed_limit())>地图道路限速(hdmap::Road::HIGHWAY))。
通过ReferenceLine::GetSpeedLimitFromS(const double s)函数获取。ReferenceLine类有私有成员变量struct SpeedLimit与std::vector<SpeedLimit>记录每段处的限速。

（3）设置规划目标

设置为proto msg

message PlanningTarget {
  optional StopPoint stop_point = 1; //内含停止s值,HARD/SOFT类型
  optional double cruise_speed = 2;
}

2.2.5 采样生成横+纵向轨迹群

generate 1d trajectory bundle for longitudinal and lateral respectively

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);

（1）Trajectory1dGenerator类-规划轨迹生成器
构造函数中首先计算TS以及VS图中的FeasibleRegion。具体为根据车辆纵向加速度的上下限计算SUpper,SLower,VUpper,VLower,TLower。

（2）纵向规划轨迹生成

GenerateLongitudinalTrajectoryBundle()函数。纵向主要是在TS图中可以直观感受到，S的一阶导数为V，即可得到VS图（这里没有使用官方的ST图的表述是因为与SL图表示一致，自变量在前因变量在后）。
输出的标准数据结构是struct SamplePoint即带速度的STPoint，最小的分辨率为运行周期0.1s.

a. 巡航

GenerateSpeedProfilesForCruising(): 根据planning_target的巡航速度得到终止条件(终止条件采样器)，用GenerateTrajectory1DBundle<4>计算得速度的四次多项式QuarticPolynomialCurve1d 。
由于不需要确定末状态的S值，所以只有五个变量(起始点s、v、a和末点的v、a，不需要求解五次多项式，求解四次即可(apollo对性能还是很敏感的啊)。

通过两层循环来完成采样。外层循环将速度从零到上限值按等间隔均匀遍历。内层循环遍历到达末状态速度的时间，从1秒到8秒按1秒的间隔均匀遍历。由于巡航状态不需要指明到达末状态的S值，所以这里只需要用四次多项式拟合即可。

这里介绍一下巡航时采样器 EndConditionSampler::SampleLonEndConditionsForCruising()。
需要输入巡航速度然后输出采样点时间点处的速度即可。具体是采样9个点，范围是：[FLAGS_polynomial_minimal_param,FLAGS_trajectory_time_length*i]。
实际值好象是[0.01, 1, 2, 3, 4…7, 8]s。
在每个时刻，计算FeasibleRegion里对应个采样时间点处的速度上下限，用线性插值的方式增加采样点。如下图：

总采样点为2+6×8=50个。

然后再说一下曲线的类，继承关系如下：
Curve1d->PolynomialCurve1d->QuarticPolynomialCurve1d/QuinticPolynomialCurve1d
Curve1d->LatticeTrajectory1d

对于每段轨迹的初始化，使用智能指针管理。
auto ptr_trajectory1d = std::make_shared<LatticeTrajectory1d>(std::shared_ptr<Curve1d>(new QuarticPolynomialCurve1d())
构造函数里有QuarticPolynomialCurve1d::ComputeCoefficients()函数计算出多项式系数，得到纵向规划曲线bundle。

b. 跟车/超车

在介绍跟车/超车的采样逻辑之前，需要介绍一下S-T图的概念。以下图中的场景为例，蓝色障碍车从车道右侧切入，在T_in时刻开始进入当前车道。那么这个场景对应的S-T图就如下图中右图所示。从T_in时刻开始出现一块斜向上的阴影区域。这块阴影区域的高度就是蓝色障碍车的车身长，上边界表示车头，下边界表示车尾，斜率表示车速。

如果上述场景变成这样，障碍车从T_in时刻进入车道，然后在T_out时刻离开车道。那么这个场景对应的S-T图就会缩短。

有了S-T图的概念，观察下图中的两条规划轨迹。红色的是一条跟车轨迹，绿色的是超车轨迹。这两条轨迹反映在S-T图中，就如右图所示。红色的跟车轨迹在蓝色阴影区域下方，绿色的超车轨迹在蓝色阴影区域上方。

采样末状态时，就可以分别在S-T图中障碍物对应的阴影区域的上方和下方分别采样。上方的末状态对应超车，下方的末状态对应跟车。

如果有多个障碍物，就对这些障碍物分别采样超车和跟车所对应的末状态。

那么总结下来就是遍历所有和车道有关联的障碍物，对他们分别采样超车和跟车的末状态，然后用多项式拟合即可获得一系列纵向轨迹。

GenerateSpeedProfilesForPathTimeObstacles()：此时的采样器为EndConditionSampler::SampleLonEndConditionsForPathTimePoints()。

不管是跟车还是超车是通过 QueryPathTimeObstacleSamplePoints() 函数根据车辆尺寸(front_edge_to_center)，分别查询跟车 QueryFollowPathTimePoints() 与超车QueryOvertakePathTimePoints()均采集采样点，而在TS图里跟车与超车分别意味着在障碍物的下方/上方采点。
这里是不对跟车/超车进行决策。决策是通过后续的cost计算来决策的。
具体过程是先查询障碍物的四个角点：PathTimeGraph::GetObstacleSurroundingPoints()。跟车的话，对下方2角点进行线性插值得到FLAGS_time_min_density分辨率下的采样点。
超车的话，对上方2角点进行线性插值。
提一下这里的线性插值，下图TS图中的斜率代表了障碍物的速度，匀速时为直线，加速时为开口向上的多项式曲线，减速时开口向下。但是这里采用线性插值是有点问题的。减速时插值的直线在实际曲线的下方，也就是比实际更严格。但是加速时插值的直线在实际曲线的上方，对于前期缓慢加速，后期突然快速加速的前车误差最大，可能导致碰撞(还是说交给后面的碰撞检测来填坑?）。
采样点SamplePoint里的速度则是通过函数PredictionQuerier::ProjectVelocityAlongReferenceLine() 把障碍物的速度投射到Frenet坐标系中得到速度。只不过考虑到超车后的安全，s方向上加了一个offset：FLAGS_default_lon_buffer。然后得到的采样点用FeasibleRegion过滤掉不满足的点得到最终的纵向终止点集end_s_conditions。

对所有终止点求解五次多项式得到速度轨迹，求解函数Trajectory1dGenerator::GenerateTrajectory1DBundle<5> 为common function的求解函数。
轨迹曲线：auto ptr_trajectory1d = std::make_shared<LatticeTrajectory1d>(std::shared_ptr<Curve1d>(new QuinticPolynomialCurve1d()).

c. 停车

在停车状态中，给定停车点，末状态的速度和加速度都是零，所以末状态是确定的。

那么我们只需用一层循环来采样到达停车点的时间即可。

GenerateSpeedProfilesForStopping()：由开关planning_target.has_stop_point()来控制。
此时的采样器为EndConditionSampler::SampleLonEndConditionsForCruising()。
由于终止点的速度/加速度都为0，采样点需要决定的数据为2个，一个是刹停距离std::max(init_s_[0], ref_stop_point)，其中ref_stop_point为输入的参考刹停距离，
第二个是时间间隔同采样器SampleLonEndConditionsForCruising()。然后用common function的求解函数求得速度轨迹。

（3）横向规划轨迹生成

GenerateLateralTrajectoryBundle()函数。横向的采样器为EndConditionSampler::SampleLatEndConditions()。
采样点为d方向：0.0, -0.5, 0.5三个点，s方向：10.0, 20.0, 40.0, 80.0四个点，一共12个点。
换道的横向轨迹规划是通过调整referenceLine来实现的。

通过FLAGS_lateral_optimization开关控制是否经过优化的方式还是直接计算五次多项式的形式计算横向轨迹。
直接计算五次多项式同纵向的common function。
优化计算是通过继承了LateralQPOptimizer类的LateralOSQPOptimizer来完成的.输出的轨迹类型为PiecewiseJerkTrajectory1d。

（4）横纵向轨迹组合

将巡航、跟车或超车/停车这三种状态纵向轨迹组合起来，就可以获得所有纵向轨迹。再将所有纵向轨迹和所有横向轨迹两两配对二维合成，就可以完成轨迹采样的工作。

（5）轨迹类型

这里稍微介绍一下planning/module/trajectory1d下面的几个轨迹类型：

StandingStillTrajectory1d：在s处不的的轨迹描述.
ConstantJerkTrajectory1d：jerk不变的轨迹描述， $p_{0}+v_{0} \times t+0.5 \times a_{0} * t^{2}+j e r k \times t^{3} / 6.0$ 。
ConstantDecelerationTrajectory1d：减速度不变的轨迹描述， $e n d_{t}=i n i t_{v} / \text { deceleration }$ ， $end_s_ = init_v * init_v / (2.0 * deceleration) + init_send_s_ = init_v * init_v / (2.0 * deceleration) + init_s$
PiecewiseJerkTrajectory1d：ConstantJerkTrajectory1d的vector，即数段连接在一起的轨迹。对外的接口是：增加线段，获取总长度，获取order阶数时 t 处值。
PiecewiseAccelerationTrajectory1d：多段段内加速度不变的曲线连接在一起的轨迹,使用s,v,a,t的vector来表达。
PiecewiseTrajectory1d：多段Curve1d连接在一起的轨迹。
PiecewiseBrakingTrajectoryGenerator：在planning/lattice/trajectory_generation下面,分段刹车的轨迹，得到为PiecewiseAccelerationTrajectory1d轨迹。

2.2.6 计算轨迹cost并排序，过滤可能碰撞的轨迹

first, evaluate the feasibility of the 1d trajectories according to dynamic constraints.
second, evaluate the feasible longitudinal and lateral trajectory pairs and sort them according to the cost.

TrajectoryEvaluator trajectory_evaluator(
      init_s, planning_target, lon_trajectory1d_bundle, lat_trajectory1d_bundle,
      ptr_path_time_graph, ptr_reference_line);
// Get instance of collision checker and constraint checker
CollisionChecker collision_checker(frame->obstacles(), init_s[0], init_d[0],
                                    *ptr_reference_line, reference_line_info,
                                    ptr_path_time_graph);

（1）滤除不合格的轨迹

通过TrajectoryEvaluator类的构造函数实现：

过停止位置的纵向轨迹；
ConstraintChecker1d::IsValidLongitudinalTrajectory过滤v/a/jerk超限的纵向轨迹；
ConstraintChecker1d::IsValidLateralTrajectory横向轨迹的滤除好像还在调试。

（2）cost计算

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

cost计算也通过TrajectoryEvaluator类的构造函数实现。

主要计算如下5个cost。时间点集 $T$ 一般为[0,FLAGS_trajectory_time_length]时间范围内以FLAGS_trajectory_time_resolution为分辨率的点。(后面把这个时间段的选择表述为[0:FLAGS_trajectory_time_resolution:FLAGS_trajectory_time_length])。

注意，下面的顺序并没有完全按照源码里的顺序来解读。

a. 到达目的的cost

Cost of missing the objective, e.g., cruise, stop, etc.

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

怎么实现这样的效果呢？针对这两种情况分别设计了参考速度。左图蓝线表示没有停车指令时的参考速度。可以看到这种情况下，绿色的加速轨迹会获得一个较小的cost，而红色的减速轨迹会获得一个相对较大的cost。那么如果存在停车指令，参考速度就会想右图中的蓝色曲线一样呈下降趋势。那么这种情况下，同样的两条轨迹，他们的cost大小关系就会正好相反。

首先需要构造一个理想的目标速度轨迹，通过ComputeLongitudinalGuideVelocity()实现。
用TS图来表达的话为 $T$ 各个点上的 s 值vector。详细分为3种情况：

I、不刹停
生成PiecewiseAccelerationTrajectory1d匀速直线。
II-1、刹停，现在开始刹
生成PiecewiseAccelerationTrajectory1d减速到0。
II-2、刹停，未来某点开始刹直到刹停
生成PiecewiseBrakingTrajectoryGenerator::Generate()。又分为几种情况：
紧急刹车无法达到舒适性刹车，立即最大刹停；
可以舒适性刹车，典型的情况如下：加速到目标速度->匀速->舒适性减速->刹停。
根据实际情况可以变成：减速到目标速度->匀速->舒适性减速->刹停/ 减速到目标速度->舒适性减速->刹停/ 加速到目标速度->舒适性减速->刹停。如下图：

如果时间点集还有剩余则用水平线补齐。

这里使用Piecewise曲线，通过一段段曲线添加的形式，个人觉得非常值得借鉴。
注：PiecewiseBrakingTrajectoryGenerator类中所有的成员函数均为static类型，应该是安全性考虑，值得借鉴。

到达目的的cost 分为2部分，speed偏差与轨迹总距离（总距离与cost成反比这一点有点想不明白…）。事先计算出理想参考点与评价轨迹的速度之差，评价轨迹的总距离。
$\begin{gathered} \text { cost }_{\text {speed }}=\frac{\sum_{i=0}^{T} t_{i}^{2}\left|v_{r e f}-v_{i}\right|}{\sum_{i=0}^{T} t_{i}^{2}} \\ \operatorname{cost}_{\text {dis }}=\frac{1}{1+\Delta s} \\ \operatorname{cost}_{\text {obj }}=\frac{w_{\text {speed }} \operatorname{cost}_{\text {speed }}+w_{\text {dis }} \operatorname{cost}_{\text {dis }}}{w_{\text {speed }}+w_{\text {dis }}} \\ w_{\text {speed }}=1 \\ w_{\text {dis }}=10 \end{gathered}$

b. 纵向舒适性cost (jerk)

Cost of longitudinal jerk

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

$\operatorname{cost}_{j e r k}=\frac{\sum_{i=0}^{T}\left(\frac{j_{i}}{j_{\max }}\right)^{2}}{\epsilon+\frac{j i}{j_{m a x}}}$

c. 纵向碰撞cost

Cost of longitudinal collision

下图中的两条轨迹，反映在右图S-T图中，我们可以发现红色的轨迹和蓝色障碍车在S-T图中的阴影区域有重叠，说明有碰撞风险，那么它的碰撞cost就会相对较高。而绿色的轨迹在S-T图中反映出来的碰撞风险较小，那么它的碰撞cost就相对较低。

通过PathTimeGraph::GetPathBlockingIntervals()函数获取 $T$ 上截取所有障碍物的s跨度(参考纵向采样时的线性插值)。
TrajectoryEvaluator::LonCollisionCost()计算碰撞cost。
首先计算时间 $t_{i}$ 处纵向轨迹的 $s$ 值 $s_{i}$ ：
$\begin{gathered} \Delta s_{i}=\left\{\begin{array}{ccc} 0 & \text { slower }_{i}-b_{y i e l d}-s_{i} & s_{i}<s_{\text {lower }_{i}}-b_{\text {yield }} \\ 1 & s_{i}-\left(s_{\text {upper }_{i}}+b_{\text {overtake }}\right) & s_{i}>s_{\text {lower }_{i}}+b_{\text {overtake }} \end{array}\right. \\ \operatorname{cost}_{i}=e^{\frac{-d^{2}}{2 \sigma^{2}}} \\ \operatorname{cost}_{\text {coll } i}=\frac{\sum_{i=0}^{T} \operatorname{cost}_{i}^{2}}{\sum_{i=0}^{T} \operatorname{cost}_{i}+\epsilon} \\ \sigma=0.5 \end{gathered}$

d. 纵向向心加速度cost

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

找到 $t_{i}$ 处匹配的参考线上的曲率 $k_{i}$ ：
$\text { cost }_{\text {centri }}=\frac{\sum_{i=0}^{T}\left(v_{i}^{2} k_{i}\right)^{2}}{\sum_{i=0}^{T}\left|v_{i}^{2} k_{i}\right|}$

e. 横向偏移cost

Cost of lateral offsets

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

先获取比较的 s 的span：标定量FLAGS_speed_lon_decision_horizon与评价轨迹长度的小值，[0:FLAGS_trajectory_space_resolution:S]。

f. 横向舒适度cost

Cost of lateral comfort

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

$\cos t_{l a t \operatorname{com}}=\left.\max \left|l_{i}^{\prime \prime} v_{i}^{2}+l_{i} a_{i}\right|\right|_{i=0} ^{T}$

最后把6项 cost 加权相加得到最终的总cost：
$\operatorname{cost}_{{total }}=w_{ {obj }} \operatorname{cost}_{{obj }}+w_{{jerk }} \operatorname{cost}_{ {jerk }}+w_{ {centri }} \text { cost }_{ {centri }}+w_{{centri }} \text { cost }_{ centri }+w_{ {latoff }} \text { cost}_{latoff }+w_{latcom }\text { cost}_{{latcom }}$

同时把纵向横向的轨迹组合以及其cost放入优先队列 std::priority_queue<PairCost, std::vector<PairCost>, CostComparator> cost_queue_;中.

（3）碰撞检测环境创建

CollisionChecker的构造函数里BuildPredictedEnvironment()函数创建检测环境，具体是滤除不相关的障碍物以及在障碍物形状上加offset膨胀。

滤除的障碍物有：

虚拟障碍物；
障碍物与自车在同一车道内并且在自车后；
障碍物与自车在同一车道内但不在PathTimeGraph内。

膨胀有效障碍物,在Box2d的横纵向各扩张标定系数倍。

2.2.7 过滤超界轨迹，拼接横纵向轨迹，如果没有合格轨迹生成backup轨迹

always get the best pair of trajectories to combine; return the first collision-free trajectory

流程如下：

（1）队列操作

选取cost最大的轨迹，并pop出去队列top指向第二大的轨迹。

while (trajectory_evaluator.has_more_trajectory_pairs()) {
double trajectory_pair_cost =
    trajectory_evaluator.top_trajectory_pair_cost();
auto trajectory_pair = trajectory_evaluator.next_top_trajectory_pair();

// combine two 1d trajectories to one 2d trajectory
auto combined_trajectory = TrajectoryCombiner::Combine(
    *ptr_reference_line, *trajectory_pair.first, *trajectory_pair.second,
    planning_init_point.relative_time());

// check longitudinal and lateral acceleration
// considering trajectory curvatures
auto result = ConstraintChecker::ValidTrajectory(combined_trajectory);
// check collision with other obstacles
if (collision_checker.InCollision(combined_trajectory)) {
    ++collision_failure_count;
    continue;
}
// put combine trajectory into debug data
const auto& combined_trajectory_points = combined_trajectory;
num_lattice_traj += 1;
reference_line_info->SetTrajectory(combined_trajectory);
reference_line_info->SetCost(reference_line_info->PriorityCost() +
                                trajectory_pair_cost);
reference_line_info->SetDrivable(true);

// cast
auto lattice_traj_ptr =
    std::dynamic_pointer_cast<LatticeTrajectory1d>(trajectory_pair.first);
if (!lattice_traj_ptr) {
    ADEBUG << "Dynamically casting trajectory1d ptr. failed.";
}

if (lattice_traj_ptr->has_target_position()) {
    ADEBUG << "Ending Lon. State s = " << lattice_traj_ptr->target_position()
            << " ds = " << lattice_traj_ptr->target_velocity()
            << " t = " << lattice_traj_ptr->target_time();
}
break;
}

（2）实现横向/纵向轨迹的拼接

TrajectoryCombiner::Combine()

在时间[0:FLAGS_trajectory_time_resolution:FLAGS_trajectory_time_length]上每个时刻分别根据纵横向轨迹计算得到s,ds,dds与l,dl,ddl；
找到参考线上匹配的最近的点,然后将Frenet坐标系下的坐标转换到笛卡尔坐标系下，得到x,y,s,theta,kappa,dkappa,relative_time的TrajectoryPoint。
通过DiscretizedTrajectory::AppendTrajectoryPoint()方法把轨迹点放入到离散轨迹中，完成拼接。

（3）约束验证

ConstraintChecker::ValidTrajectory()
分为 VALID, LON_VELOCITY_OUT_OF_BOUND, LON_ACCELERATION_OUT_OF_BOUND,LON_JERK_OUT_OF_BOUND, LAT_VELOCITY_OUT_OF_BOUND, LAT_ACCELERATION_OUT_OF_BOUND, LAT_JERK_OUT_OF_BOUND,CURVATURE_OUT_OF_BOUND结果。过程是检查报错结果的那些指标有没有出界，都没有出界的情况下合格。

（4）碰撞检测

CollisionChecker::InCollision()
在每个TrajectoryPoint处构造一个shift后的自车Box2d,检查之前滤除完成后障碍物vectorpredicted_bounding_rectangles_中的每个障碍物与自车HasOverlap()的情况. Shift的vector为Vec2d shift_vec{shift_distance * std::cos(ego_theta),shift_distance * std::sin(ego_theta)};,其中double shift_distance = ego_length / 2.0 - vehicle_config.vehicle_param().back_edge_to_center();。

碰撞检测是通过矩形Box2d重叠HasOverlap()实现的。

将自车/障碍物抽象成Box2d有如下2种方式：

AABB—Axis Aligned Bounding Box
OBB—Oriented Bounding Box

AABB更简单，构造上只要找到x，y坐标轴上的极值即可，操作上只需通过x，y坐标的比较很快实现，缺点是没有那么精确，存在大块空白区，容易引起误检。区别如下图：

Apollo使用AABB来构造Box2d.但自车的几何中心与后轮中心不重叠，需要Shift一个vector。

虽然AABB没有这么精确，Apollo通过2段检测法来平衡性能与效果。先用AABB做粗略检测。如果障碍物与自车在AABB重叠了，进行下一步分离轴定理检测。

这一步让我想起了做车辆计划工程师review汽车零件之间clearance的工作，在CAD软件上通过截不同的轴面观测零件到截面的距离来获取clearance。当然这个工作大批量的情况下会由仿真部门自动检测，但初期布置的情况下需要手动观察。

如下图，只要观测到ap−bp−cp>bufferap−bp−cp>buffer，即可证明没有碰撞。需要以自车的四个边为投影轴做检测直到发项有一个满足即可退出，具体的计算过程，我会在Apollo::Common::Math解读篇进行分析。

（5）保存合格轨迹

能够跨过这么多道坎还能留存下来的是可行驶的轨迹无疑了.将轨迹相关信息压入ReferenceLineInfo中,包括SetTrajectory(),SetCost(),SetDrivable()。返回执行状态Status::OK()。最后的输出为带cost的一束拼接轨迹，供后续使用。

是的，到现在还是不清楚前文提出问题，超车的话是怎么改变ReferenceLine的选择，需要进一步研究一下代码。

（6）backup轨迹

那要是没有轨迹能跨过这些坎呢？还有backup轨迹。BackupTrajectoryGenerator类生成backup轨迹。原理是对a进行固定采样{-0.1, -1.0, -2.0, -3.0, -4.0}生成固定减速度的轨迹ConstantDecelerationTrajectory1d。根据新的初始条件重新规划横向GenerateLateralTrajectoryBundle()。之后的过程与上面的障碍物检测,横纵向轨迹拼接一致。

if (num_lattice_traj > 0) {
ADEBUG << "Planning succeeded";
num_planning_succeeded_cycles += 1;
reference_line_info->SetDrivable(true);
return Status::OK();
} else {
AERROR << "Planning failed";
if (FLAGS_enable_backup_trajectory) {
    AERROR << "Use backup trajectory";
    BackupTrajectoryGenerator backup_trajectory_generator(
        init_s, init_d, planning_init_point.relative_time(),
        std::make_shared<CollisionChecker>(collision_checker),
        &trajectory1d_generator);
    DiscretizedTrajectory trajectory =
        backup_trajectory_generator.GenerateTrajectory(*ptr_reference_line);

    reference_line_info->AddCost(FLAGS_backup_trajectory_cost);
    reference_line_info->SetTrajectory(trajectory);
    reference_line_info->SetDrivable(true);
    return Status::OK();

} else {
    reference_line_info->SetCost(std::numeric_limits<double>::infinity());
}}

（7）失败报错

还是无法找到合格的轨迹只能报错，Status(ErrorCode::PLANNING_ERROR, "No feasible trajectories")。

经过这7步，完成了lattice_planner。

三、观码感受

所有有分母的地方都加上了极小值FLAGS_numerical_epsilon，非常好的习惯值得借鉴；
操作与数据类尽量做到了分离；
大量使用智能指针完成内存管理，例如std::shared_ptr,std::unique_ptr。

定义优先队列，排序函数是用struct来实现的，第一次知道这种用法需要加强学习。

std::priority_queue<PairCost, std::vector<PairCost>, CostComparator> cost_queue_;

struct CostComparator: public std::binary_function<const PairCost&, const PairCost&, bool> {bool operator()(const PairCost& *left*, const PairCost& *right*) const {return *left*.second > *right*.second;}};

同时对队列的操作前都会检查队列是否已经pop为空。

可以通过= delete;方式不使用构造函数，创造出只用来操作的类，降低构造成本。
auto lattice_traj_ptr = std::dynamic_pointer_cast<LatticeTrajectory1d>(trajectory_pair.first);中的数据类型转换。
多项式计算时使用模板函数，GenerateTrajectory1DBundle<4>。

四、结语

看代码前还是要知道代码的算法与操作，如果硬啃代码还是会很痛苦(想起了螺旋线拟合那段硬啃代码的时光)。
这次在十一假期用了2天时间完成了研读，再加上后续补图的时间比之前看螺旋线拟合部分要顺利很多。
Apollo是一个骨肉相连的完整系统，要搞清楚一个组件可能要看很多相关的自定义数据类与操作类，但是也因此如果了解一个组件，理解了整体架构的思想后面会越看越快。
一句话还是要多看.

五、参考链接

开发者说 | Lattice Planner从学习到实践（一）

Apollo中Lattice轨迹碰撞检测_王不二的路-CSDN博客_apollo 碰撞检测

向量应用分离轴定理 - 知乎

Apollo Lattice Planner学习记录