Apollo Lattice Planner原理学习记录

本文主要是对之前学习Apollo lattice planner的一些总结，可能会有很多不严谨的地方，仅仅写出自己的理解，欢迎讨论。

规划器大体结构

规划器主要分为两部分

1. OpenSpacePlanner，主要用于非结构化道路，车辆上主要是泊车等场景，主要用到算法是OBCA家族的优化算法
2. OnLanePlanner， 面对行车道这类结构化场景，相比于非结构化场景，主要特点是在参考线上进行规划，且速度较快，有更多的规则限制，同时会面对更多决策方面的问题。主要使用的有两个算法，PublicRoadPlanner和LatticePlanner，分别侧重不同的场景。

本文主要是对LatticePlanner进行阅读记录。
该规划器的核心函数位于Apollo/apollo/modules/planning/planner/lattice/lattice_planner.cc中，

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

算法可以简单分以下几个步骤

1. 参考线离散化

从routing获得参考线
格式转换

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

2. 计算投影点

计算规划起点在参考线的投影点

  // 2. 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());

3. 建立frenet坐标系

ComputeInitFrenetState(matched_point, planning_init_point, &init_s, &init_d);

4. 计算障碍物ST图

auto ptr_prediction_querier = std::make_shared<PredictionQuerier>(
      frame->obstacles(), ptr_reference_line);
  // 解析决策,计算障碍物的s-t图
  // 4. parse the decision and get the planning target.
  // 传入obs,参考线,参考线信息,自车当前点的s值,sl坐标系下规划终点的s值,轨迹的t,自车当前点的l相关值;
  // 传入这么多构建一个PathTimeGraph对象,会跟据障碍物类型生成对应的ST图
  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,//200m
      FLAGS_trajectory_time_length, init_d);//8 secondss

障碍物来源：感知
分类：静态 动态
考虑范围：
纵向：200m 横向：车道内
时间：8s
静态障碍物

动态障碍物，需要根据时间查询障碍物的预测位置

5. 生成横纵项轨迹簇

入口

// 5. generate 1d trajectory bundle for longitudinal and lateral respectively.
//  分别生成横纵向轨迹簇 
// 构造一个实例化类对象  输入：初始s d 均是向量 s s’ s''  障碍物地图
  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;
//  generate lateral and longitude trajectory bundles 轨迹生成
  trajectory1d_generator.GenerateTrajectoryBundles(
      planning_target, &lon_trajectory1d_bundle, &lat_trajectory1d_bundle);

横纵向解耦 纵向速度规划+横向路径规划
巡航速度
限速段
高精地图
城市道路 15.67m/s
高速道路 29.06m/s
判断有无停车点（高精地图）
纵向速度规划
无障碍物：基于VT进行采样，每个采样时刻进行6次采样，并进行4次多项式拟合
有障碍物：基于ST进行采样，每个采样时刻进行超车和跟车的决策，然后进行5次多项式拟合
有停止点
无障碍物：

有障碍物：

有停止点：

轨迹生成
巡航速度
限速段
高精地图
城市道路 15.67m/s
高速道路 29.06m/s
判断有无停车点（高精地图）
纵向轨迹生成
无障碍物
有障碍物
有停止点
横向轨迹生成（5次多项式）

轨迹生成
横向轨迹生成（OSQP）
二次优化问题
状态量：
x=[l_1,l_2,…,l_60,l_1’,l_2’,…,l_60’,l_1’’,l_2’’,…,l_60’’]^T
优化目标：
J=w_l∑_i=0^n−1▒l_i2+w_l’∑_i=0^{n−1▒l_i’}2+w_l’’∑_i=0^{n−1▒l_i’’}2+w_l’’’∑_i=0^{n−1▒l_i’’’}2+w_ref∑_i=0^{n−1▒(l_i−l_iref)}2
约束：
边界约束l_i∈[l_ileft,l_irigℎt]
轨迹生成
横向轨迹生成（OSQP）
轨迹连续性约束

整理

使用OSQP求解器计算出最优的轨迹
对每条纵向轨迹对判断是否可行
速度范围（-0.1，40）m/s
加速度范围（-4.5，4）m/s2
加加速度范围（-4，2）m/s3
计算代价并排序，找到最优轨迹
纵向目标代价
cost_speed=∑_t=0^lengtℎ▒t2∙|v_ref−v_evaluate|/∑_t=0^lengtℎ▒t2
cost_dist=1/1+dist
cost_obj=w_speed∙cost_speed+w_dist∙cost_dist/w_speed+w_dist

v_evaluate

6. 轨迹评估

入口

 // 构建一个实例化对象
//  判断是否可行 计算每条横纵向轨迹对的代价并排序
  TrajectoryEvaluator trajectory_evaluator(
      init_s, planning_target, lon_trajectory1d_bundle, lat_trajectory1d_bundle,
      ptr_path_time_graph, ptr_reference_line);

纵向
纵向舒适性代价(jerk)
cost_jerk=∑_t=0^{lengtℎ▒(jerk/2)}2/1+∑_t=0^lengtℎ▒|jerk/2|
纵向碰撞代价
dist={█(obs_low−buffer_yield−s_t s_t<obs_low−buffer_yield@s_t−obs_up−buffer_overtake s_t>obs_up+buffer_overtake )┤

          cost=e^−dist^2/2σ^2

cost_collision=∑_t=0^{lengtℎ▒cost}2/∑_t=0^lengtℎ▒cost
向心加速度代价
轨迹评估
纵向
向心加速度代价
a_t=v_t^2∙k_t
cost_centrAcc=∑_t=0^{lengtℎ▒a_t}2/∑_t=0^lengtℎ▒|a_t|
横向
横向偏移代价
cost_LatOffset=∑_s=0^{s_values▒w_s∙(l_s/3)}2/∑_s=0^s_values▒w_s∙|l_s/3| {█(w_s=1 l 同侧@w_s=10 l 异侧)┤
横向舒适性代价
cost_LatComfort=max( l ̈ )=max(l’’∙s ̇^2+l’∙s ̈ )
将横纵向轨迹对与其代价放入优先队列，按cost排序

7. 轨迹融合

 double trajectory_pair_cost =
        trajectory_evaluator.top_trajectory_pair_cost();//取代价最小的轨迹对的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());

横纵向轨迹融合
有效性检查

8. 碰撞检测

// 输入：障碍物 初始s 初始d 离散参考线 参考线信息 障碍物ST图
  // 只是建立碰撞检测的类，把障碍物环境更新到predicted_bounding_rectangles_中，以备后面的碰撞检测
  CollisionChecker collision_checker(frame->obstacles(), init_s[0], init_d[0],
                                     *ptr_reference_line, reference_line_info,
                                     ptr_path_time_graph);
  ...
 // check collision with other obstacles
  if (collision_checker.InCollision(combined_trajectory)) {
     ++collision_failure_count;
     continue;
   }

对于轨迹上每个轨迹点使用OBB方式建立boundingbox
粗检测：

精确检测：使用分离轴定理，将车体和障碍物都看做矩形，做4次投影，判断是否有交集