Lattice Planner从学习到放弃（三）：动态障碍物的处理与应用

前言

    干货不多啊，该有的前边都有了，这篇只是把动态障碍物是如何在lattice planner中被用于规划与决策的梳理。不过也触发了自己对L2和L4的差异本质和实现路径的思考。
    之前虽然已经可以实现对静态障碍物停车、绕行等避障，也踩了一些难受的坑，详情见二次规划的应用与调试，但是受限于对障碍物动态轨迹的处理，车在行驶过程中面对障碍物显得很是生硬：
    一旦车道被占据，立刻规划停车；
    一旦前车插队，立刻边打方向规避一边减速直至被逼停；
    一旦被逼停，前车走很远后才起步加速；
    总之，正因为缺少对动态信息的处理，导致每一帧障碍物均被作为静止对待，因此自车能离多远离多远.。得益于感知的小伙伴提供了障碍物的预测轨迹，目前规划时长为2s，但是引入障碍物的动态信息处理后，效果颇为改观。

    可以看到，前车切入过程中，自车减速跟随，整个过程很平稳。不再会突然减速或者往右转向同时大减速 (原因：静态障碍物的话会尝试nudge操作)。

---

动态障碍物的内容差异

    个人对L2与L4异同的拙见：
    L2中，由于车道的限制，自车仅需考虑车道内的速度规划，即行为仅有跟车、减速停车，ACC、AEB对于障碍物通过时距(TTC)+运动学分析便可实现。
    L4较L2最大区别是除纵向以外，引入横向规划，实现了横向自由，正是由于横向约束的解放，求解维度增加，原有的算法理念不再适用。此时便需要更清晰的描述障碍物的运动关系，也就是ST、SL图，即障碍物一定时间内的行驶轨迹。

动态障碍物的前处理

    在函数Obstacle::CreateObstacles(*local_view_.prediction_obstacles)中对障碍物进行，没有复杂操作，静止的就直接存，运动的则将每一个轨迹点视为一个障碍物，只是id为perception_id+trajectory_index，即id=2_12，代表id为2的障碍物的第12个轨迹点。
    在apollo框架中，planning使用的是经过处理后的障碍物信息，即感知→预测之后，
    数据格式如下，很清晰

message PredictionObstacle {
  optional apollo.perception.PerceptionObstacle perception_obstacle = 1;
  optional double timestamp = 2;  // GPS time in seconds
  // the length of the time for this prediction (e.g. 10s)
  optional double predicted_period = 3;
  // can have multiple trajectories per obstacle
  repeated Trajectory trajectory = 4;
  // estimated obstacle intent
  optional ObstacleIntent intent = 5;
  optional ObstaclePriority priority = 6;
  optional bool is_static = 7 [default = false];
  // Feature history latest -> earliest sequence
  repeated Feature feature = 8;
}

    目前仅仅使用了其中的轨迹信息，由于还未涉及博弈等，决策部分也不健全，障碍物的意图并没有用起来，后续还要继续深入才行。

障碍物动态信息的解析

    以实际弯道中的跟车场景为例，frenet的优点很明显，经过基于车道中心线的投影，障碍物何时进出车道，在车道内轨迹如何变的很直观。

前车车速	自车车速	预测轨迹时长
1.6m/s	0.5m/s	2s

    在此场景中，上车前想象的是，如果粗暴的以车辆初始状态预测8s的话，自车的ST解析以及自车速度规划大致如图1，自车纵向速度规划加速至同前车接近后维持跟车状态：
    但实际发现了出入，发现自车无法达到前车车速，对障碍物的ST信息进行检查后发现，可参见函数：

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

    构建出的障碍物ST实际如下图所示，受限于预测时长2s，导致2s障碍物的位置维持在最后一帧，相当于就是静止了：

    但还未到此结束～，障碍物ST信息被压入后，为了后续的快速计算，lattice planner对障碍物做了很大程度的简化，最终的ST信息：(0, 15.6)，(2, 19.1)，(2, 23.9)，(0, 21.1)，如下如所示：直接拉皮手术。也就回到了一开始的设定：障碍物不做变加速运动，所以其ST一定是平行四边形。（这个设定感觉有坑额…）
    详情请锁定QueryPathTimeObstacleSamplePoints()函数，无论是超车还是跟车，速度采样是直接按四边形顶点进行插值的。

EndConditionSampler::QueryPathTimeObstacleSamplePoints() const {
  //配置自行车模型，dt=0.06
  const auto& vehicle_config =
      common::VehicleConfigHelper::instance()->GetConfig();
  std::vector<SamplePoint> sample_points;
  for (const auto& path_time_obstacle :
       ptr_path_time_graph_->GetPathTimeObstacles()) {
    std::string obstacle_id = path_time_obstacle.id();
    QueryFollowPathTimePoints(vehicle_config, obstacle_id, &sample_points);
    QueryOvertakePathTimePoints(vehicle_config, obstacle_id, &sample_points);
  }
  return sample_points;
}

    障碍物预测时长和planning规划时长要保持一致，这个问题说实话之前有考虑过，却没想到事情的发展是这样的，肤浅了…

思考：局部路径规划的存在意义

    今天调试完做下复盘，局部路径规划的本质是什么？为什么要大费周折的开发局部路径规划？L2已经大规模应用了，LKA、ACC和AEB三者配合好，单车道内完全可以释放驾驶员了(让我们短暂抛开法律约束飞一会…)，L2和L4区别在哪里呢？如果仅仅为了换道，局部路径规划存在的意义还大么。
    以下仅仅是个人理解，希望大佬能够指点迷津：
    单纯从技术角度看，L2的横向控制目标源自车道线，纵向目标源自自车巡航车速与前车车速，在单车道内ACC和AEB承担舒适性与安全性，LKA承担车道内行驶，组合很完美，但是车辆只能被约束到单车道内——意味着车辆要想抵达目的地，必须人工换道。法律定义L2事故责任在于驾驶员，所以L2级驾驶辅助需要驾驶员，此逻辑不通啊。更像是L2离不开人来换道，所以法律界定L2责任在驾驶员。换下角度，若L2级可实现自主换道，就不需要驾驶员操作了，也就变成L4级自动驾驶了？
    那L2为什么不能自主换道：
    1.LKA直接利用的摄像头数值化的车道线，若想换道，没有平滑的跨车道路线可依赖
    2.即使有了换道的路线，ACC的车速目标严重依赖于前车，而换道过程中是没有合理的车速供ACC参考的
    L2想要突破车道的封印，行车路线与速度分布必须有人为此努力，由此诞生了规划模块，为控制输出包含横纵向信息的轨迹。当然，有了planning，单车道内一些避障也得到了质的改变。
    所以未来除感知外，决策与规划两块将是决定自动驾驶能力的决战之地，前者抽象更适用机器学习，后者基于规则则更加安全可控。

下一阶段

    截至目前，lattice的一些局限性也开始暴露：

• 横向撒点法过于生硬，换用二次规划的话速度提高很快：一个是全覆盖性的采样，一个是有向性的求解，差距不言而喻
• 纵向依然采用的采样法，没有先验信息的指导，必然产生很多无用速度轨迹，同时，纵向速度的变化引起自车与障碍物间运动关系的变化，进而影响到横向关系，虽说横纵向分开可以降维便于求解，但二者终归是无法割裂的，需要的这种迭代关系，应该便是EM Planner的目标了。

  下一阶段目标：
  1.routing→referencelines过程解析
  2.决策机制以及机器学习的引入
  3.EM Planner解析与应用

    希望早日完成版图，向大佬迈进