Apollo Lattice Planner学习记录

[TOC]

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 等功能的量产的方案与其非常相似, 值得学习一下. 后面的过程基本上按照代码的执行顺序进行的解读.

启动流程

见下图:

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

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

LatticePlanner::Plan()

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

PlanOnReferenceLine()

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

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. 匹配在参考线上最近点

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

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() << "]";
    }
  }
}

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

建图 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).
根据有没有预测轨迹, 将障碍物分为虚拟/静态/动态障碍物.

1. 静态障碍物
   SetStaticObstacle(const Obstacle* obstacle,const std::vector<PathPoint>& discretized_ref_points)

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

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

3. 排序
   静态障碍物根据 start_s 大小排序 static_obs_sl_boundaries_ SL 图容器.
   动态障碍物直接存到 path_time_obstacles_ TS 图容器中.

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

获取限速

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

设置规划目标

设置为 proto msg

message PlanningTarget {
  optional StopPoint stop_point = 1; //内含停止 s 值,HARD/SOFT 类型
  optional double cruise_speed = 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);

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

纵向规划轨迹生成

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

巡航

GenerateSpeedProfilesForCruising(): 根据 planning_target 的巡航速度得到终止条件(终止条件采样器), 用 GenerateTrajectory1DBundle<4> 计算得速度的四次多项式 QuarticPolynomialCurve1d.
由于不需要确定末状态的 s 值, 所以只有五个变量(起始点 s、v、a 和末点的 v、a, 不需要求解五次多项式, 求解四次即可(Apollo 对性能还是很敏感的啊).
这里介绍一下巡航时采样器 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.

跟车/超车

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()).

停车

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

横向规划轨迹生成

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.

轨迹类型

这里稍微介绍一下 planning/module/trajectory1d 下面的几个轨迹类型.
StandingStillTrajectory1d: 在 s 处不的的轨迹描述.
ConstantJerkTrajectory1d: jerk 不变的轨迹描述, $p_0 + v_0 \times t + 0.5 \times a_0 * t^2 + jerk \times t^3 / 6.0$.
ConstantDecelerationTrajectory1d: 减速度不变的轨迹描述, $\text{end}_t = \text{init}_v / \text{deceleration}$, $\text{end}_s = \text{init}_v * \text{init}_v / (2.0 * \text{deceleration}) + \text{init}_s$.
PiecewiseJerkTrajectory1d: ConstantJerkTrajectory1d 的 vector, 即数段连接在一起的轨迹. 对外的接口是: 增加线段, 获取总长度, 获取 order 阶数时 t 处值.
PiecewiseAccelerationTrajectory1d: 多段段内加速度不变的曲线连接在一起的轨迹, 使用 s,v,a,t 的 vector 来表达.
PiecewiseTrajectory1d: 多段 Curve1d 连接在一起的轨迹.
PiecewiseBrakingTrajectoryGenerator: 在planning/lattice/trajectory_generation 下面, 分段刹车的轨迹, 得到为PiecewiseAccelerationTrajectory1d轨迹.

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

滤除不合格的轨迹

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

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

cost 计算

也通过 TrajectoryEvaluator 类的构造函数实现.

主要计算如下 5 个 cost.
时间点集 $T$ 一般为 [0,FLAGS_trajectory_time_length] 时间范围内以 FLAGS_trajectory_time_resolution 为分辨率的点.

(后面把这个时间段的选择表述为 [0:FLAGS_trajectory_time_resolution:FLAGS_trajectory_time_length]).

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

1.目标 cost

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

首先需要构造一个理想的目标速度轨迹,通过ComputeLongitudinalGuideVelocity()实现.

用TS图来表达的话为 $T$ 各个点上的 s 值 vector.
详细分为 3 种情况

• 不刹停
  生成 PiecewiseAccelerationTrajectory1d 匀速直线.

• 刹停, 现在开始刹
  生成 PiecewiseAccelerationTrajectory1d 减速到 0.

• 刹停, 未来某点开始刹直到刹停
  生成 PiecewiseBrakingTrajectoryGenerator::Generate(). 又分为几种情况.
  紧急刹车无法达到舒适性刹车, 立即最大刹停.
  可以舒适性刹车, 典型的情况如下: 加速到目标速度->匀速->舒适性减速->刹停.
  根据实际情况可以变成: 减速到目标速度->匀速->舒适性减速->刹停/ 减速到目标速度->舒适性减速->刹停/ 加速到目标速度->舒适性减速->刹停. 如下图:

• 如果时间点集还有剩余则用水平线补齐.
  这里使用 Piecewise 曲线, 通过一段段曲线添加的形式, 个人觉得非常值得借鉴.
  注: PiecewiseBrakingTrajectoryGenerator 类中所有的成员函数均为 static 类型, 应该是安全性考虑, 值得借鉴.

cost 分为 2 部分, speed 偏差与轨迹总距离(总距离与 cost 成反比这一点有点想不明白…). 事先计算出理想参考点与评价轨迹的速度之差, 评价轨迹的总距离.
$$
\text{cost}{speed} = \frac{\sum{i=0}^{T} t_i^2|v_{ref} - v_i|}{\sum_{i=0}^{T} t_i^2} \\
\text{cost}{dis} = \frac{1}{1+\Delta s} \\
\text{cost}{obj} = \frac{w_{speed}\text{cost}{speed} + w{dis}\text{cost}{dis}}{w{speed} + w_{dis}} \\
w_{speed} = 1 \\
w_{dis} =10
$$

2. 纵向舒适性 cost(jerk)

Cost of longitudinal jerk
$$
\text{cost}{jerk} = \frac{ \sum{i=0}^{T} (\frac{j_i}{j_{max}})^2}{\epsilon + \frac{j_i}{j_{max}}}
$$

3. 纵向碰撞 cost

Cost of longitudinal collision
通过 PathTimeGraph::GetPathBlockingIntervals() 函数获取 $T$ 上截取所有障碍物的 s 跨度(参考纵向采样时的线性插值).
TrajectoryEvaluator::LonCollisionCost() 计算碰撞 cost.
首先计算时间 $t_i$ 处纵向轨迹的 s 值, $s_i$
$$
\Delta s_i =
\begin{cases}
0& s_{lower_i} - b_{yield} - s_i \qquad s_i < s_{lower_i} - b_{yield} \\
1& s_i - (s_{upper_i} + b_{overtake}) \qquad s_i > s_{lower_i} + b_{overtake}
\end{cases}\\
\text{cost}i = e^{\frac{-d^2}{2 \sigma ^2}} \\
\text{cost}{colli} = \frac{ \sum_{i=0}^{T} \text{cost}i^2}{\sum{i=0}^{T} \text{cost}_i + \epsilon}\\
\sigma = 0.5
$$

4. 纵向向心加速度 cost

找到 $t_i$ 处匹配的参考线上的曲率 $k_i$.
$$
\text{cost}{centri} = \frac{\sum{i=0}^{T} (v_i^2 k_i)^2 }{\sum_{i=0}^{T} |v_i^2 k_i|}
$$

5. 横向偏移 cost

Cost of lateral offsets

先获取比较的 s 的 span: 标定量 FLAGS_speed_lon_decision_horizon 与评价轨迹长度的小值, [0:FLAGS_trajectory_space_resolution:S].
$$
\text{cost}{latoff} = \frac{\sum{i=0}^{S} w {(\frac{l_{i}}{l_{max})}^2 }}{ \sum_{i=0}^{S} w \frac{l_{i}}{l_{max}} }\\
w =
\begin{cases}
& 1 \quad l_{i}与l_{max}同侧 \\
& 10 \quad l_{i}与l_{max}异侧
\end{cases}
$$

6. 横向舒适度 cost

Cost of lateral comfort
$$
\text{cost}{latcom} = \max { {|l_i’’ v_i^2 + l_i a_i|} } \Big|{i=0}^{T}
$$

最后把 6 项 cost 加权相加得到最终的总 cost.
$$
\text{cost}{total} = w{obj} \text{cost}{obj} + w{jerk} \text{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> \text{cost}_queue_; 中.

碰撞检测环境创建

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

• 滤除的障碍物有:

1. 虚拟障碍物.
2. 障碍物与自车在同一车道内并且在自车后.
3. 障碍物与自车在同一车道内但不在 PathTimeGraph 内.

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

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. 当然这个工作大批量的情况下会由仿真部门自动检测, 但初期布置的情况下需要手动观察.

如下图, 只要观测到
$$
a_p - b_p - c_p > \text{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.

观码感受

1. 所有有分母的地方都加上了极小值 FLAGS_numerical_epsilon, 非常好的习惯值得借鉴.

2. 操作与数据类尽量做到了分离.

3. 大量使用智能指针完成内存管理, 例如std::shared_ptr,std::unique_ptr.

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

   std::priority_queue<PairCost, std::vector<PairCost>, CostComparator> \text{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 为空.

5. 可以通过 = delete; 方式不使用构造函数, 创造出只用来操作的类, 降低构造成本.

6. auto lattice_traj_ptr = std::dynamic_pointer_cast<LatticeTrajectory1d>(trajectory_pair.first); 中的数据类型转换.

7. 多项式计算时使用模板函数, GenerateTrajectory1DBundle<4>.

结语

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

参考链接:

https://mp.weixin.qq.com/s/jKy4AJjZhuqWADW9h7JCzQ

https://blog.csdn.net/weixin_34945803/article/details/106628352

https://zhuanlan.zhihu.com/p/99761177