Apollo Planning学习（2）-------路径规划

佛系的小鹿

已于 2023-02-12 20:56:09 修改

阅读量2.1k

点赞数 4

文章标签：学习动态规划算法 c++ 自动驾驶

于 2023-02-03 23:42:39 首次发布

本文链接：https://blog.csdn.net/m0_71477431/article/details/128784573

版权

本次学习的Apollo版本为6.0版本，因为从5.0开始轨迹规划算法主要使用的就是public road，所以本次主要学习该算法，该算法的核心思想是PV解耦，即Path-Velocity的解耦，其主要包含两个过程：1.路径规划，2.速度规划。
路径规划其实已经发展很多年，从早期的机器人到现在的无人驾驶，主要的方法包括采样法，图搜索法，数值优化法等，具体可以查阅相关文献阅读。本篇文章主要讲述apollo轨迹规划模块里面的路径规划，之后在更新学习速度规划的文章。
与之前EM规划和Lattice规划不同，当前6.0版本使用的路径规划，更加的灵活方便，原因主要是采用了数值优化的思想，通过边界约束等，保证了密集障碍物场景的灵活性，也同时避免了EM规划中DP消耗大量时间的缺点。
在之前的学习中了解到PublicRoadPlanner::Plan()中注册了，创建，更新了场景再根据不同的场景对应的不同的stage去完成相应阶段下的tasks。由于场景的差异性，task与stage也有所不同，因此本文只讲述默认情况下的lane follow scenario，其他场景的分析方式大同小异。

关于Apollo6.0处理路径规划的算法原理，百度已经发表在《Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform》。基于参考线将规划问题解耦为SL坐标系中的路径规划和ST坐标系中的速度规划。

主要代码的梳理学习从LaneFollowStage::PlanOnReferenceLine这个函数展开

Status LaneFollowStage::PlanOnReferenceLine(
    const TrajectoryPoint& planning_start_point, Frame* frame, ReferenceLineInfo* reference_line_info) {
    // 当前先验信息判断是否当前参考线是可换道的车道，如果不是那么增加cost。
  if (!reference_line_info->IsChangeLanePath()) {
    reference_line_info->AddCost(kStraightForwardLineCost);
  }
  auto ret = Status::OK();
  // 随后，开始了task的process过程，不同的stage有不同的task，具体可通过 conf/scenario文件夹下的pb.txt
  for (auto* task : task_list_) {
    ret = task->Execute(frame, reference_line_info);
    RecordDebugInfo(reference_line_info, task->Name(), time_diff_ms);
    if (!ret.ok()) {
      AERROR << "Failed to run tasks[" << task->Name()
             << "], Error message: " << ret.error_message();
      break;
    }
  }
  RecordObstacleDebugInfo(reference_line_info);
  // check path and speed results for path or speed fallback
  reference_line_info->set_trajectory_type(ADCTrajectory::NORMAL);
  // 如果执行失败，则规划fallback轨迹
  if (!ret.ok()) {
    PlanFallbackTrajectory(planning_start_point, frame, reference_line_info);
  }
// 本次只学习路径规划部分，后面的以后再学
...
}

在路径规划中我们主要关注lane follow场景，配置文件为modules\planning\conf\scenario\lane_follow_config.pb.txt

scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: LANE_CHANGE_DECIDER
  task_type: PATH_REUSE_DECIDER
  task_type: PATH_LANE_BORROW_DECIDER
  task_type: PATH_BOUNDS_DECIDER
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
  task_type: PATH_ASSESSMENT_DECIDER
  task_type: PATH_DECIDER
  task_type: RULE_BASED_STOP_DECIDER
  task_type: ST_BOUNDS_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER
  task_type: SPEED_HEURISTIC_OPTIMIZER
  task_type: SPEED_DECIDER
  task_type: SPEED_BOUNDS_FINAL_DECIDER
  # task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_type: RSS_DECIDER

......

上述task中，根据名称可以看出，path都是与路径相关，从rule_based之后则是与速度规划相关。故本节主要关注：
stage_type: LANE_FOLLOW_DEFAULT_STAGE
enabled: true
task_type: LANE_CHANGE_DECIDER
task_type: PATH_REUSE_DECIDER
task_type: PATH_LANE_BORROW_DECIDER
task_type: PATH_BOUNDS_DECIDER
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
上面的task按照任务顺序执行：
1.lane change decider：
在这里插入图片描述

该决策器主要是用来处理refer_line_info，内部有个状态机，根据换道成功时间与换道失败时间以及当前位置与目标位置来切换状态，以此来处理refer_line_info的changelane信息，主要就是更新换道状态。
这一部分也影响到了path bound decider ：如果最终的结果是不换道，在PathBoundsDecider中会将L的边界限制在本车道内(如果不借道)；反之在PathBoundsDecider中会将L的边界向目标车道一侧进行拓展。
具体细节上的处理见Apollo Planning学习（3）-------LANE_CHANGE_DECIDER
2.path reuse decider：
在这里插入图片描述

该决策器主要是用来处理路径是否可以重用，提高帧之间的平滑性。
如果感知模块对障碍物的感应不稳定（上下跳动）导致路径规划模块不稳定（也同样上下跳动），所以Apollo设计了路径重用的决策，如果上一帧的路径没有与障碍物发生碰撞，则采用上一帧的路径。
3.path lane borrow decider：
在这里插入图片描述

ADC在借道工况中：判断本车道可通过性，如果在连续n(参数配置)帧规划中本车道可以通行，则取消借道。
ADC不在借道工况中：ADC需要同时满足必要条件才可以进入借道工况。
具体细节上的处理见：Apollo Planning学习（4）-------PATH_LANE_BORROW_DECIDER
这里同lanechangedecider一样，借道还是不借道也会影响到后面的PathBoundsDecider，如果借道，在PathBoundsDecider中会将l ll的边界借道方向一侧进行拓展。
4.path bounds decider：
在这里插入图片描述

该决策器主要是用来处理根据前面的决策器更新的状态信息（例如，换道情况，借道情况）来生成相应的L的边界。
在该决策器中分为四个场景进行处理，按处理的顺序分别是fallback、pull over、lane change、regular，不同的boundary对应不同的应用场景，其中fallback对应的path bound一定会生成，其余3个只有一个被激活，即按照顺序一旦有有效的boundary生成，就结束该task。
FallbackBound+PullOverBound；
FallbackBound+LaneChangeBound；
FallbackBound+NoBorrow/LeftBorrow/RightBorrow；
不管在何种决策下，PathBoundsDecider都会生成一条FallbackBound，其与NoBorrow的区别是，不会删除Block Obstacle后道路边界。
具体细节上的处理见：Apollo Planning学习（5）-------PATH_BOUNDS_DECIDER
5.piecewise jerk path optimize：
在这里插入图片描述

该决策器主要是基于二次规划算法，对每个边界规划出最优路径。
这里推荐听一下b站老王讲的十分透彻B站老王—二次规划
6.path assessment decider：
在这里插入图片描述
该决策器主要处理是会依据设计好的规则筛选处最终的path，并在规划路径上的采样点添加标签（IN_LANE、OUT_ON_FORWARD_LANE、OUT_ON_REVERSE_LANE等），作为路径筛选的依据，并为速度规划提供限制。
具体细节上的处理见：Apollo Planning学习(6)-------PATH_ASSESSMENT_DECIDER
7.path decider：
在这里插入图片描述
该决策器主要处理是遍历每个障碍物, 根据规则判断前面优化并筛选出来的path生成对应的decisions(GNORE, STOP, LEFT NUDGE, RIGHT NUDGE等)。

对以有IGNORE/STOP/KEEP_CLEAR决策的obstacle不做处理；
如果是block obstacle，并且不是借道工况，设为STOP决策；
不在path纵向范围内的障碍物设为IGNORE决策；
对于碰撞的obstacle，设为STOP决策；
根据位置关系设置LEFT NUDGE或者RIGHT NUDGE的决策；

具体细节上的处理见：Apollo Planning学习（7）-------PATH_DECIDER