学习自用 百度apollo

3.1产生换道策略，产生换道的状态，之后将结果保存在injector中

// /home/yuan/apollo-edu/modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.cc
// LaneChangeDecider 类的方法，用于更新车道变换状态信息
void LaneChangeDecider::UpdateStatus(double timestamp,
                                     ChangeLaneStatus::Status status_code,
                                     const std::string& path_id) {
  // 获取车道变换状态对象
  auto* lane_change_status = injector_->planning_context()
                                 ->mutable_planning_status()
                                 ->mutable_change_lane();
  
  // 设置时间戳、路径标识符和状态代码到车道变换状态对象中
  lane_change_status->set_timestamp(timestamp);  // 设置时间戳
  lane_change_status->set_path_id(path_id);      // 设置路径标识符
  lane_change_status->set_status(status_code);   // 设置状态代码
}

3.2产生借道决策

// 在默认情况下，不进行任何车道借用
reference_line_info->set_is_path_lane_borrow(false);

// 检查是否需要进行车道借用，如果需要，则进行车道借用
if (Decider::config_.path_lane_borrow_decider_config().allow_lane_borrowing() &&
    IsNecessaryToBorrowLane(*frame, *reference_line_info)) {
  reference_line_info->set_is_path_lane_borrow(true);
}

// 如果左侧和右侧均不可借用车道
if (!left_borrowable && !right_borrowable) {
  // 设置不处于车道借用场景中，并返回 false
  mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
  return false;
} else {
  // 设置处于车道借用场景中
  mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(true);
  
  // 如果左侧可借用车道，则添加左侧借用标记
  if (left_borrowable) {
    mutable_path_decider_status->add_decided_side_pass_direction(PathDeciderStatus::LEFT_BORROW);
  }
  
  // 如果右侧可借用车道，则添加右侧借用标记
  if (right_borrowable) {
    mutable_path_decider_status->add_decided_side_pass_direction(PathDeciderStatus::RIGHT_BORROW);
  }
}

同时，在进行借道决策时，会对左右借道进行判断。借道的状态保存在injetor里。

根据现有决策在参考线上进行采样，获得每个点在l ll的边界。有四种边界决策：GenerateRegularPathBound（自车道行驶）、GenerateFallbackPathBound（失败回退）、GenerateLaneChangePathBound、GeneratePullOverPathBound。最后将边界保存在SetCandidatePathBoundaries中，供下一步使用。

3.5路径优化

// 设置参考轨迹和参考速度
piecewise_jerk_problem.set_x_ref(std::move(weight_x_ref_vec), path_reference_l_ref);

// 设置权重参数
piecewise_jerk_problem.set_weight_x(w[0]);
piecewise_jerk_problem.set_weight_dx(w[1]);
piecewise_jerk_problem.set_weight_ddx(w[2]);
piecewise_jerk_problem.set_weight_dddx(w[3]);

// 设置缩放因子
piecewise_jerk_problem.set_scale_factor({1.0, 10.0, 100.0});

// 记录优化开始时间
auto start_time = std::chrono::system_clock::now();

// 设置状态变量的边界约束
piecewise_jerk_problem.set_x_bounds(lat_boundaries); // 设置状态变量 x 的边界
piecewise_jerk_problem.set_dx_bounds(-FLAGS_lateral_derivative_bound_default, FLAGS_lateral_derivative_bound_default); // 设置状态变量 dx 的边界
piecewise_jerk_problem.set_ddx_bounds(ddl_bounds); // 设置状态变量 ddx 的边界

// 从车辆参数估算横向加速度和 jerk 的边界
const auto& veh_param = common::VehicleConfigHelper::GetConfig().vehicle_param();
const double axis_distance = veh_param.wheel_base(); // 轴距
const double max_yaw_rate = veh_param.max_steer_angle_rate() / veh_param.steer_ratio() / 2.0; // 最大偏航角速率
const double jerk_bound = EstimateJerkBoundary(std::fmax(init_state[1], 1.0), axis_distance, max_yaw_rate); // 估算 jerk 的边界
piecewise_jerk_problem.set_dddx_bound(jerk_bound); // 设置 jerk 的边界

// 调用优化函数进行求解
bool success = piecewise_jerk_problem.Optimize(max_iter);

这几个参数分别表示了在优化问题中对不同状态变量的加权，具体含义如下：

• weight_x (w[0])：表示对位置（x）的加权，用于调节优化过程中位置变量的重要性。
• weight_dx (w[1])：表示对速度（dx）的加权，用于调节优化过程中速度变量的重要性。
• weight_ddx (w[2])：表示对加速度（ddx）的加权，用于调节优化过程中加速度变量的重要性。
• weight_dddx (w[3])：表示对 jerk（jerk是加速度的变化率）的加权，用于调节优化过程中 jerk 变量的重要性。

通过调节这些权重参数，可以影响优化算法在不同状态变量上的收敛速度和结果，以满足具体问题的需求。通常情况下，这些权重参数需要根据具体的优化目标和约束条件进行调整，以获得最佳的优化结果。

调用piecewise_jerk_problem类进行求解，会设置一些权重以及一些约束，利用Optimize函数进行求解。

// 获取候选路径边界信息
const auto& path_boundaries = reference_line_info_->GetCandidatePathBoundaries();

// 输出候选路径边界数量
ADEBUG << "There are " << path_boundaries.size() << " path boundaries.";

// 获取参考路径数据
const auto& reference_path_data = reference_line_info_->path_data();

// 存储候选路径数据的容器
std::vector<PathData> candidate_path_data;

// 遍历每个候选路径边界
for (const auto& path_boundary : path_boundaries) {
    // 获取当前路径边界的点数量
    size_t path_boundary_size = path_boundary.boundary().size();

 reference_line_info_->GetCandidatePathBoundaries();保存候选路径。

// 如果候选路径数据为空
if (candidate_path_data.empty()) {
    // 返回规划错误状态，提示路径优化器未能生成路径
    return Status(ErrorCode::PLANNING_ERROR, "Path Optimizer failed to generate path");
}

// 将候选路径数据移动到参考线信息中
reference_line_info_->SetCandidatePathData(std::move(candidate_path_data));

3.6选择最优路径

// 比较两个路径数据的函数，同时考虑阻挡障碍物
bool ComparePathData(const PathData& lhs, const PathData& rhs, const Obstacle* blocking_obstacle) {
    // 输出正在比较的路径标签信息
    ADEBUG << "Comparing " << lhs.path_label() << " and " << rhs.path_label();

    // 如果左侧路径数据为空，则左侧路径数据不可能比右侧路径数据更大
    if (lhs.Empty()) {
        ADEBUG << "LHS is empty.";
        return false;
    }

    // 如果右侧路径数据为空，则右侧路径数据比左侧路径数据更大
    if (rhs.Empty()) {
        ADEBUG << "RHS is empty.";
        return true;
    }

 调用ComparePathData函数，对路径进行两两比较。

// 将有效路径数据容器中的第一个路径数据赋值给参考线信息中的路径数据
*(reference_line_info->mutable_path_data()) = valid_path_data.front();

// 设置参考线信息中的阻挡障碍物，使用有效路径数据容器中第一个路径数据的阻挡障碍物ID
reference_line_info->SetBlockingObstacle(valid_path_data.front().blocking_obstacle_id());

将最优的路径保存在reference_line_info中。将阻塞障碍物最近的放在reference_line_info中，供速度规划进一步处理。

planning模块

这张图展示了一个自动驾驶车辆的决策和规划系统的结构。图中的结构分为两个主要层次：顶层（Top Layer）和底层（Bottom Layer），并且详细描述了不同的规划任务和决策过程。

### 图解说明：

1. **顶层和底层**：
   - **顶层**：通常负责高级决策和策略，如场景管理和任务分配。
   - **底层**：通常处理具体的路径规划和速度决策任务。

2. **场景管理器（Scenario Manager）**：
   - 这是顶层的一个组件，负责根据当前的驾驶环境和情境选择合适的驾驶策略。例如，它可以决定是进行超车（Pull Over），停车（Parking），还是处理交叉路口（Junction）等情况。

3. **规划任务**：
   - **车道内规划任务（On Lane Planning Tasks）**：
     - 这些任务涉及车道内的行驶决策，如路径决策（Path decision），包括借用车道（Lane Borrow）和变换车道（Lane Change）。
   - **开放空间规划任务（Open Space Planning Tasks）**：
     - 这些任务处理车辆在没有明确车道标记的开放空间中的行驶，如停车场或非正规道路。

4. **路径规划（Path Planning）**：
   - 负责生成车辆的行驶路径，确保路径的安全性和可行性。

5. **速度决策（Speed decision）**：
   - 包括决定车辆的行驶速度，以及如何响应周围环境的变化，例如交叉车道安全（Cross Lane Safety）和车道合并等待（Lane Merge Waiting）。

6. **速度规划（Speed Planning）**：
   - 实际计算车辆的速度轨迹，确保车辆按照安全和效率的方式行驶。

### 总结：
这张图表提供了一个自动驾驶系统在处理不同驾驶场景和任务时的决策和规划流程的概览。通过这种分层和模块化的设计，系统能够灵活地应对各种复杂的交通情况，确保行驶的安全性和效率。

PATH_BORROW_DECIDER相关配置
PATH_BORROW_DECIDER的相关配置集中在以下两个文件：modules/planning/conf/planning_config.pb.txt和modules/planning/conf/scenario/lane_follow_config.pb.txt

// modules/planning/conf/scenario/lane_follow_config.pb.txt
task_config: {
    // 设置任务类型为路径借道决策器
    task_type: PATH_LANE_BORROW_DECIDER
    // 配置路径借道决策器，允许借道
    path_lane_borrow_decider_config {
        allow_lane_borrowing: true
    }
}

// modules/planning/conf/planning_config.pb.txt
default_task_config: {
    // 设置默认任务配置的任务类型为路径借道决策器
    task_type: PATH_LANE_BORROW_DECIDER
    // 配置路径借道决策器，允许借道
    path_lane_borrow_decider_config {
        allow_lane_borrowing: true
    }
}

 

// modules/planning/tasks/deciders/path_bounds_decider/path_bounds_decider.cc

// 命名空间，用于限定作用域，这里是一个未命名的匿名命名空间
namespace {
    // PathBoundPoint 包含了路径边界点的信息：(s, l_min, l_max)
    using PathBoundPoint = std::tuple<double, double, double>;

    // PathBound 包含了一组 PathBoundPoint，表示路径的边界
    using PathBound = std::vector<PathBoundPoint>;

    // ObstacleEdge 包含了障碍物边缘的信息：(is_start_s, s, l_min, l_max, obstacle_id)
    using ObstacleEdge = std::tuple<int, double, double, double, std::string>;
}  // namespace

这张图展示了自动驾驶车辆中路径边界决策器（PathBoundsDecider）的处理流程（Process() 方法），以及在这个流程中如何生成不同类型的路径边界。流程图分为两部分：左侧是方法调用的顺序，右侧是每个方法所依赖的输入信息和生成的输出。

### 流程解释：

1. **初始化（Initialize）**:
   - 调用 `InitPathBoundsDecider()` 方法来初始化路径边界决策器。
   - 获取参考线信息、规划起始点、自动驾驶车辆（ADC）的位置和车道信息等。

2. **生成 Fallback 路径边界（Generate fallback path bound）**:
   - 调用 `GenerateFallbackPathBound()` 方法。
   - 依赖于 ADC 的位置和车道信息。
   - 生成的 Fallback 路径边界用于处理紧急情况或当正常路径不可用时作为备选。

3. **生成 Pull-over 路径边界（Generate pull-over path bound）**:
   - 调用 `GeneratePullOverPathBound()` 方法。
   - 生成的 Pull-over 路径边界用于车辆需要靠边停车的情况。

4. **生成 Lane Change 路径边界（Generate lane change path bound）**:
   - 调用 `GenerateLanePathBound()` 方法。
   - 生成的 Lane Change 路径边界用于车辆换道时使用。

5. **生成 Regular 路径边界（Generate regular path bound）**:
   - 调用 `GenerateRegularPathBound()` 方法。
   - 依赖于 ADC 的位置、车道信息和静态障碍物的位置。
   - 生成的 Regular 路径边界是车辆在正常行驶情况下使用的路径边界。

### 总结：
这个流程图清晰地展示了在自动驾驶系统中，如何根据不同的驾驶需求和环境条件，逐步生成适应各种情况的路径边界。每一步的生成都依赖于特定的输入信息，并产生明确的输出，这些输出将被用来指导自动驾驶车辆的行驶路径决策。