apollo决策规划学习--慢速障碍物超车

巨人的肩膀：

Apollo planning 框架
Apollo Planning 代码学习（一）
apollo介绍之planning模块
Apollo Planning决策规划代码详细解析 (6):LaneChangeDecider

1

主要的函数流程如下：
PlanningComponent::Proc() -> OnLanePlanning::RunOnce() ->
OnLanePlanning::Plan(）-> PublicRoadPlanner::Plan() ->
Scenario::Process() -> LaneFollowStage::Process() -> LaneFollowStage::PlanOnReferenceLine()

PlanningComponent::Proc()
Proc主要是检查数据，并且执行注册好的planning，生成路线并且发布。
OnLanePlanning::RunOnce()
该函数的实现主要分为三个部分:车辆状态更新、查看全局路径是否有更新、Planning模块运行需要时间
OnLanePlanning::Plan(） PublicRoadPlanner::Plan()
依据decider和optimizer进行规划，调用具体的planner进行规划，更新场景，决策当前应该执行什么场景，获取当前场景
Scenario::Process()
执行当前场景的任务
LaneFollowStage::Process()
遍历所有的参考线，直到找到可用来规划的参考线后退出
LaneFollowStage::PlanOnReferenceLine()
顺序执行stage中的任务

2

场景选择阶段：ScenarioDispatchNonLearning()函数默认从lanefollow场景开始判断，首先根据驾驶员的意图来安排场景，如果不是默认的lanefollow场景，直接输出当前场景；如果是lanefollow场景，会依次判断是否属于别的场景；即剩余场景之间的跳转必须经过lanefollow这个场景。

switch (current_scenario_->scenario_type()) {
      case ScenarioConfig::LANE_FOLLOW: 车道保持
      case ScenarioConfig::PULL_OVER: 靠边
        break;
      case ScenarioConfig::BARE_INTERSECTION_UNPROTECTED: 无保护十字路口
      case ScenarioConfig::EMERGENCY_PULL_OVER: 紧急靠边
      case ScenarioConfig::PARK_AND_GO: 停车和启动
      case ScenarioConfig::STOP_SIGN_PROTECTED: 停止标志
      case ScenarioConfig::STOP_SIGN_UNPROTECTED:
      case ScenarioConfig::TRAFFIC_LIGHT_PROTECTED: 信号灯
      case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN: 信号灯无保护左转
      case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN:
      case ScenarioConfig::VALET_PARKING: 代客泊车
      case ScenarioConfig::DEADEND_TURNAROUND: 路端调头

3

lane_follow的task列表：

 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决定
  task_type: PATH_BOUNDS_DECIDER 根据借道信息、道路宽度生成FallbackPathBound
								 根据借道信息、道路宽度生成、障碍物边界生成PullOverPathBound、LaneChangePathBound、		
					             RegularPathBound中的一种
					             将车道线和障碍物转为上图中的边界
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER 根据之前decider决策的reference line和path bound,以及横向约束，将最优路径求解问
  						     			   题转化为二次型规划问题;
 										   调用osqp库求解最优路径;
  task_type: PATH_ASSESSMENT_DECIDER       选出之前规划的备选路径中排序最靠前的路径;
										   添加一些必要信息到路径中
  task_type: PATH_DECIDER				在上一个任务中获得了最优的路径，PathDecider的功能是 根据静态障碍物做出自车的决策， 
  									    对于前方的静态障碍物是忽略、stop还是nudge
  task_type: RULE_BASED_STOP_DECIDER    根据一些规则设定停止标志
  task_type: ST_BOUNDS_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER  将规划路径上障碍物的st bounds加载到路径对应的st图上
										  计算并生成路径上的限速信息
  task_type: SPEED_HEURISTIC_OPTIMIZER  使用DP求解一条最优路径
  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

LANE_CHANGE_DECIDER

它的作用主要有两点：

判断当前是否进行变道，以及变道的状态，并将结果存在变量lane_change_status中；
变道过程中将目标车道的reference line放置到首位，变道结束后将当前新车道的reference line放置到首位

静态障碍物的nudge是用的lane_change，至于lane_change和lane_borrow的区别目前还不懂，之后再补

补：lane_chage的触发条件是有一个以上的参考线，而lane_borrow的条件是只能有一条参考线

PathReuseDecider

它的作用主要是换道时：

根据横纵向跟踪偏差，来决策是否需要重新规划轨迹；
如果横纵向跟踪偏差，则根据上一时刻的轨迹生成当前周期的轨迹，以尽量保持轨迹的一致性

Path_Lane_Borrow_Decider

需要进入借道场景才可以触发绕行功能。

需要满足下面条件才能判断是否可以借道：

○ 只有一条参考线，才能借道
○ 起点速度小于最大借道允许速度
○ 阻塞障碍物必须远离路口
○ 阻塞障碍物会一直存在
○ 阻塞障碍物与终点位置满足要求
○ 为可侧面通过的障碍物

PathBoundsDecider

分四种情况对pathBound进行计算，按照处理顺序分别是fallback备用、pullover靠边停车、lanechange换道、regular常规，不同的boundary对应不同的应用场景，
其中fallback对应的path bound一定会生成，其余3个只有一个被激活，即按照顺序一旦有有效的boundary生成，就结束该task。

对于决策规划–超车任务来说，这里会生成三种path bound ：fallback、regular/self、regular/left/forward

PathAssessmentDecider

它的作用主要是：

○ 选出之前规划的备选路径中排序最靠前的路径；
○ 添加一些必要信息到路径中。

在这里debug时候观察到，regular/self的路径被排到了最前面，发现是因为排序的ComparePathData中设置的规则：

if (lhs_on_selflane || rhs_on_selflane) {
    if (std::fabs(lhs_path_length - rhs_path_length) >
        kSelfPathLengthComparisonTolerance) {
      return lhs_path_length > rhs_path_length;
    } else {
      return lhs_on_selflane;
    }

log一下发现regular/self的路径长度和regular/left的路径长度竟然一样，这是因为啥呢，继续往前看。
来看看这个path_bound是怎么生成的
->PathBoundsDecider::GenerateRegularPathBound()

直到这时，在这里发现了apollo就没给动态障碍物写生成借道边界的逻辑：
看issue里面怎么说：

https://github.com/ApolloAuto/apollo/issues/8808
Hi,
Thanks for the question. The current path-decision can take care of slow-speed obstacles. Here are a few parameters that you can tune to achieve your goal:

path_decider_obstacle_utils.cc line 40: Here you can modify it by deleting the "IsStatic()" check and increase the FLAGS_static_obstacle_speed_threhold to be the slow-speed threshold you want. The ADC will try to side-pass any obstacle below this given speed.

path_lane_borrow_decider.cc line 103: The FLAGS_lane_borrow_max_speed is the speed for the ADC to start side-pass. If you want the ADC to be able to start side-pass at a faster speed, you can increase this value (at the cost of reduced safety so please be careful).

path_lane_borrow_decider.cc line 110: Only when the ADC thinks the blocking obstacle is a long-term one, it will start side-pass. Here you can specify how many frames an obstacle is blocking can it be defined as a "long-term" blocking obstacle. For example, if you specify it to be 10, then only when the ADC sees a slow-car ahead for 10frames (1sec) will the ADC start to pass the slow-car.

path_bounds_decider.cc line 270: Here you see that the boundary is determined by lane-width, if you want, you can replace it with the function GetBoundaryFromRoads, what that will do is to use the entire road (likely consisting multiple lanes) as the boundary, ADC will try to use any path that it can find on the road, without considering whether it needs to borrow neighbor lane or not, etc. Normally, we don't suggest use this because it might create some safety issues. But as you can see, at line 319, when the ADC needs to pull-over, especially in emergency situation, it will use this method to find a path.

Hope this answers your questions. Thanks.

根据大神的指点开始调试
改变静态障碍物判定逻辑后发现小车可以变道，但是速度保持在和前车相同
这可以说明PATH_LANE_BORROW_DECIDER给的借道判断正确

根据SPEED_BOUNDS_PRIORI_DECIDER模块，我们先来看看是否受到了限速

curr_speed_limit = std::fmax(speed_bounds_config_.lowest_speed(),
                      std::min({speed_limit_from_reference_line,
                              speed_limit_from_centripetal_acc,
                              speed_limit_from_nearby_obstacles}));

speed_limit_from_reference_line 是根据map得到的限制
speed_limit_from_centripetal_acc 路径曲率带来的速度限制
speed_limit_from_nearby_obstacles 根据障碍物得到的速度限制

speed decider 流程图

注：超车时，在ST图中障碍物上方采样，即自车速度大于障碍物车速度。

根据log，得到AINFO:ADC在ST图上位于障碍物下方。但是想overtake需要在障碍物上方。如图：

查看GetSTLocation()函数，该函数是根据路径决策、速度曲线、障碍物ST边界获取到STLocation。
动态障碍物的ST边界应该是平行四边形
先看看输入的路径决策、速度曲线分别为
reference_line_info->path_decision()
reference_line_info->speed_data()
回到上一个SPEED_BOUNDS_PRIORI_DECIDER，观察到给的speed_limit并不小。
判断还是路径规划有问题，主要看PATH_BOUNDS_DECIDER：

Task初始化，根据参考线建立frenet坐标系，计算自车在frenet坐标系中的位置(s,l,s’,l’),同时计算自车到车道中心线的距离以及左右车道宽度。

接下来生成path bound，每个path bound都会在后续生成一条轨迹。

生成RegularPathBound：

Status PathBoundsDecider::GenerateRegularPathBound(
    const ReferenceLineInfo& reference_line_info,
    const LaneBorrowInfo& lane_borrow_info, PathBound* const path_bound,
    std::string* const blocking_obstacle_id,
    std::string* const borrow_lane_type) {
  // 1. Initialize the path boundaries to be an indefinitely large area.
  //将路径边界初始化为一个无限大的区域。
  if (!InitPathBoundary(reference_line_info, path_bound)) {
    const std::string msg = "Failed to initialize path boundaries.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // PathBoundsDebugString(*path_bound);

  // 2. Decide a rough boundary based on lane info and ADC's position
  //基于道路信息和车辆生成粗略边界
  if (!GetBoundaryFromLanesAndADC(reference_line_info, lane_borrow_info, 0.1,
                                  path_bound, borrow_lane_type)) {
    const std::string msg =
        "Failed to decide a rough boundary based on "
        "road information.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // PathBoundsDebugString(*path_bound);

  // TODO(jiacheng): once ready, limit the path boundary based on the
  //                 actual road boundary to avoid getting off-road.

  // 3. Fine-tune the boundary based on static obstacles基于静态障碍微调边界
  PathBound temp_path_bound = *path_bound;
  if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),//将不是静态的排除
                                      path_bound, blocking_obstacle_id)) {
    const std::string msg =
        "Failed to decide fine tune the boundaries after "
        "taking into consideration all static obstacles.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // Append some extra path bound points to avoid zero-length path data.
  int counter = 0;
  while (!blocking_obstacle_id->empty() &&
         path_bound->size() < temp_path_bound.size() &&
         counter < kNumExtraTailBoundPoint) {
    path_bound->push_back(temp_path_bound[path_bound->size()]);
    counter++;
  }
  // PathBoundsDebugString(*path_bound);

  // 4. Adjust the boundary considering dynamic obstacles
  // TODO(all): may need to implement this in the future.


  ADEBUG << "Completed generating path boundaries.";
  return Status::OK();
}

根据车辆和障碍物以及道路信息生成bound，即下图：
由于障碍物是非静态的，所以根据预测的轨迹障碍物的end_s会有一个延长，生成的boundary会造成s-ref被堵塞，故进行boundary的调整。
成功：

可以看到决策块也变成了overtake。

如果对您有参考价值。希望不吝点赞：）。