Lattice与Piecewise方法明显的不同，Lattice是沿参考线分解，横向运动（l,l',l''），纵向运动（s,s',s''），然后进行轨迹合成。
 

引言：

 Apollo中在2.5版本就引入了Lattice palnner，该算法基于Frenet坐标系，通过采样的方式生成轨迹。在横向采样中，Apollo引入了优化的思想，这与之后的Piecewise jerk Path optimization同根同源。

虽然该算法本身具有一定的缺陷，但作为一个体系完整的规划器，依旧很值得我们学习。

基础工作：ReferenceLine、Pncmap、障碍物映射（数据流动）等

Apollo 中 Planning 模块的框架_牛仔很忙吧的博客-程序员宅基地

0. 启动流程

主流程在planning\planner\lattice\lattice_planner.cc，需要调用以下模块：

common/math/cartesian_frenet_conversion.cc  //坐标转换
common/math/path_matcher.cc
planning/common/planning_gflags.cc
planning/constraint_checker/collision_checker.cc  //碰撞检测
planning/constraint_checker/constraint_checker.cc //约束检测
planning/lattice/behavior/path_time_graph.cc
planning/lattice/behavior/prediction_querier.cc
planning/lattice/trajectory_generation/backup_trajectory_generator.cc
planning/lattice/trajectory_generation/lattice_trajectory1d.cc
planning/lattice/trajectory_generation/trajectory1d_generator.cc
planning/lattice/trajectory_generation/trajectory_combiner.cc  //轨迹拼接
planning/lattice/trajectory_generation/trajectory_evaluator.cc //轨迹评估

CyberRT的planning_component类里Init过程中运行palnning_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);
}

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

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图

分别设置ST图的矩形四个角点,矩形的长度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(),也过滤掉超界障碍物,得到ST图,path_time_obstacle_map_.

3.排序

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

其中,障碍物是通过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

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

  ADEBUG << "Trajectory_Generation_Time = "
         << (Clock::NowInSeconds() - current_time) * 1000;
  current_time = Clock::NowInSeconds();

进入GenerateTrajectoryBundles函数

void Trajectory1dGenerator::GenerateTrajectoryBundles(
    const PlanningTarget& planning_target,
    Trajectory1DBundle* ptr_lon_trajectory_bundle,
    Trajectory1DBundle* ptr_lat_trajectory_bundle) {
  //纵向速度规划
  GenerateLongitudinalTrajectoryBundle(planning_target,
                                       ptr_lon_trajectory_bundle);
  
  //横向轨迹规划
  GenerateLateralTrajectoryBundle(ptr_lat_trajectory_bundle);
}

5.1 Trajectory1dGenerator类-规划轨迹生成器

构造函数中首先计算TS以及VS图中的FeasibleRegion.具体为根据车辆纵向加速度的上下限计算SUpper,SLower,VUpper,VLower,TLower.

5.2  纵向规划轨迹生成

五次多项式（跟车/超车、停车）、四次多项式（巡航）

void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle(
    const PlanningTarget& planning_target,
    Trajectory1DBundle* ptr_lon_trajectory_bundle) const {
  // cruising trajectories are planned regardlessly.
  GenerateSpeedProfilesForCruising(planning_target.cruise_speed(),
                                   ptr_lon_trajectory_bundle);

  GenerateSpeedProfilesForPathTimeObstacles(ptr_lon_trajectory_bundle);

  if (planning_target.has_stop_point()) {
    GenerateSpeedProfilesForStopping(planning_target.stop_point().s(),
                                     ptr_lon_trajectory_bundle);
  }
}

GenerateLongitudinalTrajectoryBundle()函数.纵向主要是在TS图中可以直观感受到,S的一阶导数为V,即可得到VS图(这里没有使用官方的ST图的表述是因为与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的求解函数求得速度轨迹.

5.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.

void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(
    Trajectory1DBundle* ptr_lat_trajectory_bundle) const {
  //是否使用优化轨迹，true，采用五次多项式规划
  if (!FLAGS_lateral_optimization) {
    auto end_conditions = end_condition_sampler_.SampleLatEndConditions();

    // Use the common function to generate trajectory bundles.
    GenerateTrajectory1DBundle<5>(init_lat_state_, end_conditions,
                                  ptr_lat_trajectory_bundle);
  } else {
    double s_min = init_lon_state_[0];
    double s_max = s_min + FLAGS_max_s_lateral_optimization; //FLAGS_max_s_lateral_optimization = 60

    double delta_s = FLAGS_default_delta_s_lateral_optimization; //规划间隔为1m
    //横向边界
    auto lateral_bounds =
        ptr_path_time_graph_->GetLateralBounds(s_min, s_max, delta_s);

    // LateralTrajectoryOptimizer lateral_optimizer;
    std::unique_ptr<LateralQPOptimizer> lateral_optimizer(
        new LateralOSQPOptimizer);
    // 采用的是OSQP求解器
    lateral_optimizer->optimize(init_lat_state_, delta_s, lateral_bounds);

    auto lateral_trajectory = lateral_optimizer->GetOptimalTrajectory();

    ptr_lat_trajectory_bundle->push_back(
        std::make_shared<PiecewiseJerkTrajectory1d>(lateral_trajectory));
  }
}

5.4 轨迹类型

这里稍微介绍一下planning/module/trajectory1d下面的几个轨迹类型.
StandingStillTrajectory1d:在s处不的的轨迹描述.
ConstantJerkTrajectory1d:jerk不变的轨迹描述,
ConstantDecelerationTrajectory1d:减速度不变的轨迹描述,
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.
时间点集TT一般为[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图来表达的话为TT各个点上的s值vector.
详细分为3种情况

不刹停
生成PiecewiseAccelerationTrajectory1d匀速直线.

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

刹停,未来某点开始刹直到刹停

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

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

cost分为2部分,speed偏差与轨迹总距离(总距离与cost成反比这一点有点想不明白…).事先计算出理想参考点与评价轨迹的速度之差,评价轨迹的总距离.

2. 纵向舒适性cost(jerk)

3. 纵向碰撞cost

Cost of longitudinal collision
通过PathTimeGraph::GetPathBlockingIntervals()函数获取TT上截取所有障碍物的s跨度(参考纵向采样时的线性插值).
TrajectoryEvaluator::LonCollisionCost()计算碰撞cost.
首先计算时间titi处纵向轨迹的s值,si

4. 纵向向心加速度cost

找到titi处匹配的参考线上的曲率ki.

5. 横向偏移cost

Cost of lateral offsets

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

6. 横向舒适度cost

cost of later comfort

最后把6项cost加权相加得到最终的总cost.

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

碰撞检测环境创建

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

• 滤除的障碍物有:

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

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

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

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

流程如下:

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

7.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()方法把轨迹点放入到离散轨迹中,完成拼接.

7.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结果,过程是检查报错结果的那些指标有没有出界,都没有出界的情况下合格.

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

如下图,只要观测到

即可证明没有碰撞.需要以自车的四个边为投影轴做检测直到发项有一个满足即可退出. 具体的计算过程,我会在Apollo::Common::Math解读篇进行分析.

7.5 保存合格轨迹

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

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

7.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.7 失败报错

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

经过这7步,完成了lattice_planner.

补充：

轨迹拼接（规划起点、规划周期、Repaln问题）

总结：

Planning Lattice规划器 - 知乎

Apollo Lattice Planner学习记录 - Challenging-eXtraordinary

下面这篇博客，作者自己按照自己的思路实现了一遍，分析的比较详细 

基于Frenet坐标系的无人车轨迹规划详解与实现_robinvista的博客-程序员宅基地_apollo frenet坐标

Frenet坐标系与Cartesian坐标系互转_昔风不起，唯有努力生存！-程序员宅基地_frenet坐标系

百度 Apollo 轨迹规划技术分享笔记 - 知乎   提问汇总

Baidu Apollo代码解析之轨迹规划中的轨迹评估代价函数 - 知乎  Lattice Planner 和 EM Planner

自动驾驶路径规划-Lattice Planner算法 半杯茶的小酒杯，汇总了一些提问

下面这个实践总结的很详细：

Apollo中Lattice规划器结构梳理_王不二的路-程序员宅基地_apollo lattice

 Lattice Planner从学习到放弃（一）.额不....到实践_王不二的路-程序员宅基地_lattice plan

Lattice Planner从学习到放弃（二）：二次规划的应用与调试_王不二的路-程序员宅基地_二次规划求解器

关于动态障碍物的处理，需要思考学习： 

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

 Apollo中Lattice轨迹碰撞检测_王不二的路-程序员宅基地_apollo 碰撞检测

 社群分享内容 | Lattice Planner规划算法

Apollo规划代码ros移植-Lattice规划框架_夏融化了这季节的博客-程序员宅基地

Apollo规划代码ros移植-Lattcie的二次规划_夏融化了这季节的博客-程序员宅基地

Lattice Planner规划算法原理 - 知乎

状态空间lattice算法梳理（按执行逻辑） - 知乎

 Lattice规划与Matlab实现（1） - 知乎

关于多项式的理解：

Lattice Planner-Matlab - 知乎

关于五次多项式：还有很多疑问，对多项式的理解？

轨迹优化 | Minimum-jerk - 知乎（以及高飞的课程）

自动驾驶决策规划算法第一章第一节 细说五次多项式_哔哩哔哩_bilibili

五次多项式轨迹（matlab） - 知乎

MATLAB中的支持

Lattice Planner-Matlab - 知乎

Highway Trajectory Planning Using Frenet Reference Path- MATLAB & Simulink- MathWorks 中国