Apollo Planning 模块技术深度解析

目录

• 1. 模块概述
• 2. 架构设计
• 3. 双层状态机机制
• 4. 规划器详解
• 5. 场景系统
• 6. 任务系统
• 7. 参考线生成
• 8. 交通规则处理
• 9. 并发设计与性能优化
• 10. 配置与调度
• 11. 扩展开发指南

---

1. 模块概述

1.1 功能定位

Planning（轨迹规划）模块是Apollo自动驾驶系统的"大脑"，负责根据感知、预测、定位和路由信息，为车辆规划出一条安全、舒适、高效的行驶轨迹。

核心功能：

• 路径规划（Path Planning）：规划车辆的空间路径
• 速度规划（Speed Planning）：规划车辆的速度曲线
• 决策逻辑（Decision Making）：处理交通规则、障碍物避让等决策
• 轨迹优化（Trajectory Optimization）：优化平滑度、安全性、舒适性

输出轨迹包含：

• 位置坐标（x, y）
• 速度（v）
• 加速度（a）
• 加加速度/急动度（jerk）
• 时间戳（t）
• 航向角（theta）
• 曲率（kappa）

1.2 目录结构

modules/planning/
├── planning_component/          # 组件入口和配置
│   ├── conf/                   # 配置文件
│   ├── dag/                    # DAG启动文件
│   ├── launch/                 # Launch启动文件
│   ├── planning_component.cc   # Planning组件实现
│   ├── planning_base.cc        # Planning基类
│   ├── on_lane_planning.cc     # 高精地图模式
│   └── navi_planning.cc        # 相对地图导航模式
├── planning_interface_base/     # 接口基类定义
│   ├── planner_base/           # Planner基类
│   ├── scenario_base/          # Scenario基类
│   ├── task_base/              # Task基类
│   └── traffic_rules_base/     # TrafficRule基类
├── planning_base/               # 基础数据结构和算法
│   ├── common/                 # 通用数据结构
│   │   ├── frame.h             # 规划帧
│   │   ├── obstacle.h          # 障碍物表示
│   │   ├── path/               # 路径数据结构
│   │   ├── speed/              # 速度数据结构
│   │   └── trajectory/         # 轨迹数据结构
│   ├── math/                   # 数学库
│   │   ├── curve1d/            # 一维曲线
│   │   ├── piecewise_jerk/     # 分段Jerk优化
│   │   └── discretized_points_smoothing/  # 离散点平滑
│   ├── reference_line/         # 参考线生成
│   └── gflags/                 # 全局标志
├── planners/                    # 规划器实现
│   ├── public_road/            # 公共道路规划器（主要）
│   ├── lattice/                # 网格规划器
│   ├── navi/                   # 导航规划器
│   └── rtk/                    # RTK回放规划器
├── scenarios/                   # 场景插件（12个）
│   ├── lane_follow/            # 车道保持（默认）
│   ├── pull_over/              # 靠边停车
│   ├── valet_parking/          # 代客泊车
│   ├── traffic_light_protected/        # 有保护交通灯
│   ├── traffic_light_unprotected_left_turn/   # 无保护左转
│   ├── traffic_light_unprotected_right_turn/  # 无保护右转
│   ├── stop_sign_unprotected/          # 无保护停止标志
│   ├── bare_intersection_unprotected/  # 无保护路口
│   ├── yield_sign/             # 让行标志
│   ├── emergency_pull_over/    # 紧急停车
│   ├── emergency_stop/         # 紧急制动
│   └── park_and_go/            # 泊车起步
├── tasks/                       # 任务插件（22个）
│   ├── lane_follow_path/       # 沿车道路径
│   ├── lane_change_path/       # 换道路径
│   ├── lane_borrow_path/       # 借道路径
│   ├── pull_over_path/         # 靠边路径
│   ├── path_decider/           # 路径决策
│   ├── speed_bounds_decider/   # 速度边界决策
│   ├── piecewise_jerk_speed/   # 分段Jerk速度优化
│   ├── open_space_trajectory_provider/  # 开放空间轨迹
│   └── ...                     # 其他任务
├── traffic_rules/               # 交通规则（9个）
│   ├── traffic_light/          # 交通灯
│   ├── stop_sign/              # 停止标志
│   ├── crosswalk/              # 人行道
│   ├── destination/            # 目的地
│   ├── keep_clear/             # 禁停区域
│   └── ...                     # 其他规则
├── pnc_map/                     # PNC地图（参考线基础）
└── planning_open_space/         # 开放空间规划（泊车）

1.3 输入输出

输入（订阅的Topic）

Channel	类型	描述	频率
/apollo/prediction	PredictionObstacles	障碍物预测信息（主触发）	10Hz
/apollo/canbus/chassis	Chassis	车辆底盘状态（速度、档位等）	100Hz
/apollo/localization/pose	LocalizationEstimate	车辆定位信息	100Hz
/apollo/perception/traffic_light	TrafficLightDetection	交通灯感知	10Hz
/apollo/routing	RoutingResponse	全局路由路径	按需
/apollo/external_command/*	各种命令	外部控制命令	按需
/apollo/storytelling	Stories	故事讲述（场景信息）	10Hz

输出（发布的Topic）

Channel	类型	描述	频率
/apollo/planning	ADCTrajectory	规划轨迹（主输出）	10Hz
/apollo/planning/command_status	CommandStatus	命令执行状态	10Hz
/apollo/external_command/lane_follow	LaneFollowCommand	重路由请求	按需

---

2. 架构设计

2.1 整体架构

┌─────────────────────────────────────────────────────────────┐
│                    Planning Component                        │
│  (Entry Point: Triggered by PredictionObstacles)            │
└────────────────────────┬────────────────────────────────────┘│▼┌───────────────────────────────┐│      PlanningBase             ││  (Abstract Base Class)        │└───────────┬───────────────────┘│┌───────────┴───────────┐│                       │▼                       ▼
┌─────────────────┐    ┌──────────────────┐
│ OnLanePlanning  │    │  NaviPlanning    │
│ (高精地图模式)   │    │  (相对地图模式)   │
└────────┬────────┘    └──────────────────┘│▼
┌─────────────────────────────────────────┐
│          Planner (规划器)                │
│  ┌────────────────────────────────────┐ │
│  │  PublicRoadPlanner (主要)          │ │
│  │  - ScenarioManager                 │ │
│  │  - Traffic Decider                 │ │
│  └────────────────────────────────────┘ │
│  ┌────────────────────────────────────┐ │
│  │  LatticePlanner                    │ │
│  └────────────────────────────────────┘ │
│  ┌────────────────────────────────────┐ │
│  │  RTKReplayPlanner                  │ │
│  └────────────────────────────────────┘ │
└─────────────────────────────────────────┘│▼
┌─────────────────────────────────────────┐
│     Scenario (场景) - Top Layer FSM     │
│  ┌────────────────────────────────────┐ │
│  │  LaneFollowScenario (默认)         │ │
│  │  PullOverScenario                  │ │
│  │  ValetParkingScenario              │ │
│  │  TrafficLight* Scenarios (3种)     │ │
│  │  StopSign/YieldSign Scenarios      │ │
│  │  Emergency* Scenarios (2种)        │ │
│  └────────────────────────────────────┘ │
└─────────────────────────────────────────┘│▼
┌─────────────────────────────────────────┐
│      Stage (阶段) - Bottom Layer FSM    │
│  ┌────────────────────────────────────┐ │
│  │  Approach Stage                    │ │
│  │  Intersection Cruise Stage         │ │
│  │  Creep Stage                       │ │
│  │  ...                               │ │
│  └────────────────────────────────────┘ │
└─────────────────────────────────────────┘│▼
┌─────────────────────────────────────────┐
│          Task (任务)                     │
│  ┌────────────────────────────────────┐ │
│  │  Path Generation Tasks             │ │
│  │  - LaneFollowPath                  │ │
│  │  - LaneChangePath                  │ │
│  │  - LaneBorrowPath                  │ │
│  └────────────────────────────────────┘ │
│  ┌────────────────────────────────────┐ │
│  │  Decider Tasks                     │ │
│  │  - PathDecider                     │ │
│  │  - SpeedBoundsDecider              │ │
│  └────────────────────────────────────┘ │
│  ┌────────────────────────────────────┐ │
│  │  Speed Optimizer Tasks             │ │
│  │  - PiecewiseJerkSpeed              │ │
│  │  - PathTimeHeuristic               │ │
│  └────────────────────────────────────┘ │
└─────────────────────────────────────────┘

2.2 PlanningComponent - 组件入口

文件位置：planning_component/planning_component.h

class PlanningComponent final: public cyber::Component<prediction::PredictionObstacles,canbus::Chassis,localization::LocalizationEstimate> {public:bool Init() override;// 主入口：当收到PredictionObstacles时触发bool Proc(const std::shared_ptr<prediction::PredictionObstacles>&prediction_obstacles,const std::shared_ptr<canbus::Chassis>& chassis,const std::shared_ptr<localization::LocalizationEstimate>&localization_estimate) override;private:void CheckRerouting();        // 检查是否需要重新路由bool CheckInput();            // 检查输入有效性// 订阅者std::shared_ptr<cyber::Reader<perception::TrafficLightDetection>>traffic_light_reader_;std::shared_ptr<cyber::Reader<planning::PadMessage>> pad_msg_reader_;std::shared_ptr<cyber::Reader<relative_map::MapMsg>> relative_map_reader_;std::shared_ptr<cyber::Reader<storytelling::Stories>> story_telling_reader_;std::shared_ptr<cyber::Reader<PlanningCommand>> planning_command_reader_;// 发布者std::shared_ptr<cyber::Writer<ADCTrajectory>> planning_writer_;std::shared_ptr<cyber::Writer<external_command::CommandStatus>>command_status_writer_;// 核心规划实例std::unique_ptr<PlanningBase> planning_base_;   // OnLanePlanning 或 NaviPlanningstd::shared_ptr<DependencyInjector> injector_;  // 依赖注入器// 配置和本地视图PlanningConfig config_;LocalView local_view_;MessageProcess message_process_;
};

Proc函数流程：

1. 收到PredictionObstacles（主触发）↓
2. 获取最新的Chassis和LocalizationEstimate↓
3. CheckInput() - 检查输入有效性↓
4. 更新LocalView（本地视图）↓
5. planning_base_->RunOnce(local_view, &adc_trajectory)↓
6. CheckRerouting() - 检查是否需要重路由↓
7. 发布ADCTrajectory和CommandStatus

2.3 PlanningBase - 规划基类

文件位置：planning_component/planning_base.h

class PlanningBase {public:explicit PlanningBase(const std::shared_ptr<DependencyInjector>& injector);virtual ~PlanningBase();virtual apollo::common::Status Init(const PlanningConfig& config);virtual std::string Name() const = 0;// 核心函数：执行一次规划virtual void RunOnce(const LocalView& local_view,ADCTrajectory* const adc_trajectory) = 0;virtual apollo::common::Status Plan(const double current_time_stamp,const std::vector<common::TrajectoryPoint>& stitching_trajectory,ADCTrajectory* const trajectory) = 0;protected:const hdmap::HDMap* hdmap_ = nullptr;         // 高精地图PlanningConfig config_;                       // 配置TrafficDecider traffic_decider_;              // 交通规则决策器std::unique_ptr<Frame> frame_;                // 当前规划帧std::shared_ptr<Planner> planner_;            // 规划器std::unique_ptr<PublishableTrajectory> last_publishable_trajectory_;std::shared_ptr<DependencyInjector> injector_;
};

2.4 OnLanePlanning - 高精地图模式（主要）

文件位置：planning_component/on_lane_planning.h

class OnLanePlanning : public PlanningBase {public:void RunOnce(const LocalView& local_view,ADCTrajectory* const adc_trajectory) override;apollo::common::Status Plan(const double current_time_stamp,const std::vector<common::TrajectoryPoint>& stitching_trajectory,ADCTrajectory* const trajectory) override;private:// 生成参考线std::unique_ptr<ReferenceLineProvider> reference_line_provider_;
};

RunOnce流程：

void OnLanePlanning::RunOnce(const LocalView& local_view,ADCTrajectory* const adc_trajectory) {// 1. 更新frame（包含当前状态、障碍物、参考线等）const double start_timestamp = Clock::NowInSeconds();frame_->Init(local_view, stitching_trajectory);// 2. 应用交通规则（全局决策）traffic_decider_.Execute(frame_.get());// 3. 调用Planner进行规划auto status = planner_->Plan(planning_start_point, frame_.get(), adc_trajectory);// 4. 填充轨迹元数据FillPlanningPb(start_timestamp, adc_trajectory);
}

---

3. 双层状态机机制

3.1 设计思想

Apollo Planning从3.5版本开始采用**双层状态机（Two-Layer State Machine）**机制：

┌────────────────────────────────────────────────────┐
│              Top Layer: Scenario FSM               │
│   (场景状态机 - 根据环境切换场景)                    │
│                                                    │
│  LaneFollow → TrafficLight → Intersection         │
│       ↑            ↓              ↓                │
│       └────────  PullOver ←───  Emergency         │
└────────────────────────────────────────────────────┘│▼
┌────────────────────────────────────────────────────┐
│           Bottom Layer: Stage FSM                  │
│   (阶段状态机 - 场景内部的执行流程)                  │
│                                                    │
│  Approach → Creep → Intersection_Cruise → Finish  │
└────────────────────────────────────────────────────┘

优势：

1. 模块化：每个场景独立，互不影响
2. 可维护性：修改一个场景不会影响其他场景
3. 可扩展性：轻松添加新场景
4. 调试友好：问题定位更准确

3.2 Scenario - 场景基类

文件位置：planning_interface_base/scenario_base/scenario.h

class Scenario {public:virtual ~Scenario() = default;// 初始化场景virtual bool Init(std::shared_ptr<DependencyInjector> injector,const std::string& name);// 获取场景上下文virtual ScenarioContext* GetContext() = 0;// 判断是否可以从other_scenario切换到当前场景virtual bool IsTransferable(const Scenario* other_scenario,const Frame& frame) {return false;}// 处理场景逻辑（执行当前Stage）virtual ScenarioResult Process(const common::TrajectoryPoint& planning_init_point, Frame* frame);// 进入场景时的回调virtual bool Enter(Frame* frame) { return true; }// 退出场景时的回调virtual bool Exit(Frame* frame) { return true; }// 创建Stagestd::shared_ptr<Stage> CreateStage(const StagePipeline& stage_pipeline);const std::string& Name() const { return name_; }protected:ScenarioResult scenario_result_;              // 场景执行结果std::shared_ptr<Stage> current_stage_;        // 当前执行的Stagestd::unordered_map<std::string, const StagePipeline*> stage_pipeline_map_;std::shared_ptr<DependencyInjector> injector_;ScenarioPipeline scenario_pipeline_config_;   // 场景配置std::string name_;
};

Scenario::Process流程：

ScenarioResult Scenario::Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) {// 如果没有当前Stage，创建第一个Stageif (current_stage_ == nullptr) {current_stage_ = CreateStage(scenario_pipeline_config_.stage(0));}// 执行当前Stageauto stage_result = current_stage_->Process(planning_init_point, frame);// 根据Stage结果决定下一步switch (stage_result.GetStageStatus()) {case StageStatusType::RUNNING:// Stage还在运行，继续return scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_PROCESSING);case StageStatusType::FINISHED:// Stage完成，切换到下一个Stageauto next_stage_name = current_stage_->NextStage();if (next_stage_name.empty()) {// 没有下一个Stage，场景完成return scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_DONE);}// 创建下一个Stagecurrent_stage_ = CreateStage(stage_pipeline_map_[next_stage_name]);return scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_PROCESSING);case StageStatusType::ERROR:return scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_UNKNOWN);}
}

3.3 Stage - 阶段基类

文件位置：planning_interface_base/scenario_base/stage.h

class Stage {public:virtual bool Init(const StagePipeline& config,const std::shared_ptr<DependencyInjector>& injector,const std::string& config_dir, void* context);// 执行Stage逻辑（运行Task列表）virtual StageResult Process(const common::TrajectoryPoint& planning_init_point, Frame* frame) = 0;const std::string& Name() const;const std::string& NextStage() const { return next_stage_; }protected:// 在参考线上执行TaskStageResult ExecuteTaskOnReferenceLine(const common::TrajectoryPoint& planning_start_point, Frame* frame);// 在开放空间执行TaskStageResult ExecuteTaskOnOpenSpace(Frame* frame);virtual StageResult FinishScenario();std::vector<std::shared_ptr<Task>> task_list_;  // Task列表std::shared_ptr<Task> fallback_task_;           // 失败后的兜底Taskstd::string next_stage_;                        // 下一个Stage名称void* context_;                                 // 场景上下文std::shared_ptr<DependencyInjector> injector_;StagePipeline pipeline_config_;
};

ExecuteTaskOnReferenceLine流程：

StageResult Stage::ExecuteTaskOnReferenceLine(const common::TrajectoryPoint& planning_start_point, Frame* frame) {// 遍历所有参考线for (auto& reference_line_info : *frame->mutable_reference_line_info()) {// 在每条参考线上执行所有Taskfor (auto& task : task_list_) {const double start_timestamp = Clock::NowInSeconds();// 执行Taskauto ret = task->Execute(frame, &reference_line_info);if (!ret.ok()) {AERROR << "Failed to run task: " << task->Name();// 执行fallback_taskif (fallback_task_) {fallback_task_->Execute(frame, &reference_line_info);}}const double time_diff_ms = (Clock::NowInSeconds() - start_timestamp) * 1000;RecordDebugInfo(&reference_line_info, task->Name(), time_diff_ms);}}return StageResult(StageStatusType::RUNNING);
}

3.4 ScenarioManager - 场景管理器

文件位置：planners/public_road/scenario_manager.h

class ScenarioManager final {public:bool Init(const std::shared_ptr<DependencyInjector>& injector,const PlannerPublicRoadConfig& planner_config);Scenario* mutable_scenario() { return current_scenario_.get(); }// 更新场景（检查是否需要切换）void Update(const common::TrajectoryPoint& ego_point, Frame* frame);void Reset(Frame* frame);private:std::shared_ptr<Scenario> current_scenario_;     // 当前场景std::shared_ptr<Scenario> default_scenario_type_; // 默认场景（LaneFollow）std::vector<std::shared_ptr<Scenario>> scenario_list_; // 场景列表（按优先级）
};

Update流程（场景切换）：

void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,Frame* frame) {// 检查当前场景是否完成if (current_scenario_->GetStatus() == ScenarioStatusType::STATUS_DONE) {// 场景完成，切换到默认场景current_scenario_ = default_scenario_type_;}// 遍历场景列表（按优先级从高到低）for (auto& scenario : scenario_list_) {// 检查是否可以切换到这个场景if (scenario->IsTransferable(current_scenario_.get(), *frame)) {// 退出当前场景current_scenario_->Exit(frame);// 切换到新场景current_scenario_ = scenario;current_scenario_->Reset();current_scenario_->Enter(frame);AINFO << "Switch to scenario: " << scenario->Name();return;}}
}

---

4. 规划器详解

4.1 Planner基类

文件位置：planning_interface_base/planner_base/planner.h

class Planner {public:virtual ~Planner() = default;virtual std::string Name() = 0;virtual common::Status Init(const std::shared_ptr<DependencyInjector>& injector,const std::string& config_path = "") = 0;virtual common::Status Plan(const common::TrajectoryPoint& planning_init_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) = 0;virtual void Stop() = 0;
};// 带参考线的Planner
class PlannerWithReferenceLine : public Planner {// 增加了参考线相关功能
};

4.2 PublicRoadPlanner - 公共道路规划器（主要）

文件位置：planners/public_road/public_road_planner.h

class PublicRoadPlanner : public PlannerWithReferenceLine {public:std::string Name() override { return "PUBLIC_ROAD"; }common::Status Init(const std::shared_ptr<DependencyInjector>& injector,const std::string& config_path = "") override;common::Status Plan(const common::TrajectoryPoint& planning_init_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) override;void Reset(Frame* frame) override { scenario_manager_.Reset(frame); }private:ScenarioManager scenario_manager_;      // 场景管理器（核心）PlannerPublicRoadConfig config_;Scenario* scenario_ = nullptr;
};

Plan函数流程：

common::Status PublicRoadPlanner::Plan(const common::TrajectoryPoint& planning_init_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) {// 1. 更新场景（检查是否需要切换）scenario_manager_.Update(planning_init_point, frame);scenario_ = scenario_manager_.mutable_scenario();// 2. 执行当前场景auto result = scenario_->Process(planning_init_point, frame);// 3. 检查执行结果if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_UNKNOWN) {return Status(ErrorCode::PLANNING_ERROR, "Scenario planning error");}// 4. 从Frame中获取最优轨迹const auto* best_ref_info = frame->FindDriveReferenceLineInfo();if (!best_ref_info) {return Status(ErrorCode::PLANNING_ERROR, "No drive reference line");}// 5. 填充输出轨迹ptr_computed_trajectory->CopyFrom(best_ref_info->trajectory());return Status::OK();
}

4.3 LatticePlanner - 网格规划器

特点：

• 基于采样的规划方法
• 生成多条候选轨迹
• 通过代价函数选择最优轨迹
• 适合动态环境

核心流程：

1. 纵向采样（S方向）- 采样不同的终止时间和终止速度↓
2. 横向采样（L方向）- 采样不同的横向偏移↓
3. 轨迹组合- 纵向 + 横向 = 候选轨迹↓
4. 碰撞检测- 过滤不安全的轨迹↓
5. 代价评估- 舒适性、安全性、效率↓
6. 选择最优轨迹

4.4 RTKReplayPlanner - RTK回放规划器

用途：

• 录制轨迹回放
• 测试和验证
• 不需要规划，直接回放

4.5 NaviPlanner - 导航规划器

用途：

• 相对地图模式（无高精地图）
• 适合高速公路等简单场景
• 实时生成局部地图

---

5. 场景系统

5.1 场景分类

Apollo Planning支持12种场景，按优先级分类：

高优先级场景（紧急情况）

1. EmergencyStopScenario - 紧急制动

   • 触发条件：收到紧急停车命令
   • 行为：立即减速停车

2. EmergencyPullOverScenario - 紧急靠边停车

   • 触发条件：收到PULL_OVER命令
   • 行为：就近寻找安全位置靠边停车

中优先级场景（特殊路况）

3. ValetParkingScenario - 代客泊车

   • 触发条件：routing中包含parking_id
   • 阶段：Approaching → Parking

4. PullOverScenario - 靠边停车

   • 触发条件：接近终点 && enable_pull_over_at_destination=true
   • 阶段：Approaching → Retry → Slow_Down

5. ParkAndGoScenario - 泊车起步

   • 触发条件：车辆在停车位需要起步
   • 使用OpenSpace算法

6. TrafficLightProtectedScenario - 有保护交通灯

   • 触发条件：前方有有保护的交通灯路口
   • 阶段：Approach → Intersection_Cruise

7. TrafficLightUnprotectedLeftTurnScenario - 无保护左转

   • 触发条件：前方有无保护左转交通灯
   • 阶段：Approach → Creep → Intersection_Cruise

8. TrafficLightUnprotectedRightTurnScenario - 无保护右转

   • 触发条件：前方有无保护右转交通灯
   • 阶段：Approach → Creep → Intersection_Cruise

9. StopSignUnprotectedScenario - 无保护停止标志

   • 触发条件：前方有停止标志
   • 阶段：Pre_Stop → Stop → Creep → Intersection_Cruise

10. YieldSignScenario - 让行标志

    • 触发条件：前方有让行标志
    • 阶段：Approach → Creep

11. BareIntersectionUnprotectedScenario - 无保护路口

    • 触发条件：前方是无保护路口
    • 阶段：Approach → Intersection_Cruise

默认场景

12. LaneFollowScenario - 车道保持（默认）
    • 触发条件：其他场景都不满足时的默认场景
    • 阶段：Lane_Follow_Default_Stage
    • 功能：沿车道行驶、避障、换道

5.2 场景实现示例 - LaneFollowScenario

配置文件：scenarios/lane_follow/conf/scenario_config.pb.txt

scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGEstage_config: {stage_type: LANE_FOLLOW_DEFAULT_STAGEenabled: truetask_type: LANE_CHANGE_PATHtask_type: LANE_BORROW_PATHtask_type: LANE_FOLLOW_PATHtask_type: PATH_DECIDERtask_type: SPEED_BOUNDS_DECIDERtask_type: PIECEWISE_JERK_SPEEDtask_type: RSS_DECIDER
}

代码实现：

class LaneFollowScenario : public Scenario {public:bool Init(std::shared_ptr<DependencyInjector> injector,const std::string& name) override {// 加载配置if (!Scenario::Init(injector, name)) {return false;}// 初始化场景特定逻辑return true;}bool IsTransferable(const Scenario* other_scenario,const Frame& frame) override {// LaneFollow是默认场景，总是可以切入return true;}ScenarioContext* GetContext() override {return &context_;}private:LaneFollowContext context_;
};

5.3 场景配置

主配置文件：planning_component/conf/planning_config.pb.txt

standard_planning_config {planner_type: PUBLIC_ROADplanner_public_road_config {# 场景列表（按优先级从高到低）scenario {name: "EMERGENCY_STOP"type: "EmergencyStopScenario"}scenario {name: "EMERGENCY_PULL_OVER"type: "EmergencyPullOverScenario"}scenario {name: "VALET_PARKING"type: "ValetParkingScenario"}scenario {name: "PULL_OVER"type: "PullOverScenario"}scenario {name: "TRAFFIC_LIGHT_PROTECTED"type: "TrafficLightProtectedScenario"}scenario {name: "TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN"type: "TrafficLightUnprotectedLeftTurnScenario"}scenario {name: "TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN"type: "TrafficLightUnprotectedRightTurnScenario"}scenario {name: "STOP_SIGN_UNPROTECTED"type: "StopSignUnprotectedScenario"}scenario {name: "YIELD_SIGN"type: "YieldSignScenario"}scenario {name: "BARE_INTERSECTION_UNPROTECTED"type: "BareIntersectionUnprotectedScenario"}scenario {name: "LANE_FOLLOW"type: "LaneFollowScenario"}}
}

---

6. 任务系统

6.1 Task基类

文件位置：planning_interface_base/task_base/task.h

class Task {public:virtual ~Task() = default;const std::string& Name() const;virtual bool Init(const std::string& config_dir,const std::string& name,const std::shared_ptr<DependencyInjector>& injector);// 在参考线上执行virtual common::Status Execute(Frame* frame,ReferenceLineInfo* reference_line_info);// 不需要参考线执行（OpenSpace）virtual common::Status Execute(Frame* frame);protected:template <typename T>bool LoadConfig(T* config);Frame* frame_;ReferenceLineInfo* reference_line_info_;std::shared_ptr<DependencyInjector> injector_;std::string name_;
};

6.2 Task分类

6.2.1 路径生成Task（PathGeneration）

基类：PathGeneration : public Task

子类：

1. LaneFollowPath - 沿车道路径

   • 功能：生成沿当前车道的路径
   • 算法：DP + QP优化

2. LaneChangePath - 换道路径

   • 功能：生成换道轨迹
   • 算法：多项式曲线

3. LaneBorrowPath - 借道路径

   • 功能：临时借用相邻车道绕障
   • 算法：FEM路径优化

4. PullOverPath - 靠边停车路径

   • 功能：生成靠边停车轨迹
   • 算法：混合A*

5. ReusePath - 重用路径

   • 功能：重用上一周期的路径
   • 条件：环境变化不大

6. FallbackPath - 兜底路径

   • 功能：其他路径失败时的备选
   • 策略：减速保持当前车道

LaneFollowPath实现示例：

class LaneFollowPath : public PathGeneration {public:common::Status Execute(Frame* frame,ReferenceLineInfo* reference_line_info) override {// 1. 生成路径边界PathBound path_bound;GeneratePathBound(&path_bound);// 2. DP路径规划（粗略路径）DpRoadGraph dp_road_graph;std::vector<PathPoint> dp_path;dp_road_graph.FindPathTunnel(reference_line_info, &dp_path);// 3. QP路径优化（平滑路径）PathData path_data;QpSplinePathOptimizer qp_optimizer;qp_optimizer.Process(dp_path, path_bound, &path_data);// 4. 保存到ReferenceLineInforeference_line_info->SetPathData(path_data);return Status::OK();}
};

6.2.2 决策Task（Decider）

基类：Decider : public Task

子类：

1. PathDecider - 路径决策

   • 功能：决定对障碍物的处理方式（绕行、停车、超车等）
   • 输出：障碍物决策（NUDGE, STOP, OVERTAKE等）

2. SpeedBoundsDecider - 速度边界决策

   • 功能：根据障碍物、交通规则确定速度边界
   • 输出：ST图的速度上下界

3. RuleBasedStopDecider - 基于规则的停车决策

   • 功能：根据交通规则决定停车点
   • 场景：红绿灯、停止标志、人行道等

4. RssDecider - RSS安全决策

   • 功能：责任敏感安全模型
   • 输出：安全约束

5. OpenSpaceRoiDecider - 开放空间ROI决策

   • 功能：泊车场景的感兴趣区域
   • 输出：ROI边界

PathDecider实现示例：

class PathDecider : public Decider {public:common::Status Execute(Frame* frame,ReferenceLineInfo* reference_line_info) override {const auto& path_data = reference_line_info->path_data();const auto& obstacles = reference_line_info->path_decision()->obstacles();// 遍历所有障碍物for (const auto& obstacle : obstacles.Items()) {// 静态障碍物处理if (!obstacle->IsVirtual() && obstacle->IsStatic()) {ObjectDecisionType object_decision;// 判断是否需要Nudge（轻微绕行）if (IsNudgeNeeded(obstacle, path_data)) {object_decision.mutable_nudge()->set_type(ObjectNudge::LEFT_NUDGE);object_decision.mutable_nudge()->set_distance_l(FLAGS_nudge_distance);}// 判断是否需要Stopelse if (IsStopNeeded(obstacle, path_data)) {double stop_distance = CalculateStopDistance(obstacle, path_data);object_decision.mutable_stop()->set_distance_s(stop_distance);}path_decision->AddLateralDecision("path_decider", obstacle->Id(), object_decision);}}return Status::OK();}
};

6.2.3 速度优化Task（SpeedOptimizer）

基类：SpeedOptimizer : public Task

子类：

1. PiecewiseJerkSpeed - 分段Jerk速度优化

   • 功能：生成平滑的速度曲线
   • 算法：二次规划（QP）
   • 目标：最小化Jerk（加加速度）

2. PiecewiseJerkSpeedNonlinear - 非线性速度优化

   • 功能：更复杂的速度优化
   • 算法：非线性规划（NLP）
   • 适用：复杂场景

3. PathTimeHeuristic - 启发式路径时间优化

   • 功能：快速生成速度曲线
   • 算法：启发式搜索
   • 适用：实时性要求高的场景

PiecewiseJerkSpeed实现：

class PiecewiseJerkSpeed : public SpeedOptimizer {public:common::Status Execute(Frame* frame,ReferenceLineInfo* reference_line_info) override {// 1. 获取速度边界（来自SpeedBoundsDecider）const auto& st_graph_data = reference_line_info->st_graph_data();const auto& speed_limit = st_graph_data.speed_limit();// 2. 构建QP问题PiecewiseJerkSpeedProblem piecewise_jerk_problem(init_point.v(), init_point.a(), 0.0,  // 初始状态delta_t, total_length);// 3. 添加约束// - 速度约束：v_min <= v <= v_max// - 加速度约束：a_min <= a <= a_max// - Jerk约束：jerk_min <= jerk <= jerk_maxpiecewise_jerk_problem.set_x_bounds(speed_bounds);piecewise_jerk_problem.set_dx_bounds(accel_bounds);piecewise_jerk_problem.set_ddx_bounds(jerk_bounds);// 4. 设置代价函数权重piecewise_jerk_problem.set_weight_x(FLAGS_weight_target_speed);piecewise_jerk_problem.set_weight_dx(FLAGS_weight_accel);piecewise_jerk_problem.set_weight_ddx(FLAGS_weight_jerk);// 5. 求解if (!piecewise_jerk_problem.Optimize()) {return Status(ErrorCode::PLANNING_ERROR, "Speed optimization failed");}// 6. 提取结果std::vector<double> s, v, a;piecewise_jerk_problem.GetOptimalTrajectory(&s, &v, &a);// 7. 生成SpeedDataSpeedData speed_data;for (size_t i = 0; i < s.size(); ++i) {speed_data.AppendSpeedPoint(s[i], i * delta_t, v[i], a[i], 0.0);}reference_line_info->SetSpeedData(speed_data);return Status::OK();}
};

6.2.4 轨迹优化Task（TrajectoryOptimizer）

基类：TrajectoryOptimizer : public Task

子类：

1. OpenSpaceTrajectoryProvider - 开放空间轨迹提供者
   • 功能：泊车场景的轨迹规划
   • 算法：Hybrid A* + 轨迹优化
   • 输出：完整的泊车轨迹

6.3 Task执行顺序

典型的Task执行顺序（LaneFollow场景）：

1. LaneChangeDecider（是否需要换道）↓
2. PathReuseDecider（是否可以重用路径）↓
3. LaneChangePath / LaneBorrowPath / LaneFollowPath（根据决策生成路径）↓
4. PathDecider（路径决策）↓
5. SpeedBoundsDecider（速度边界决策）↓
6. STBoundsDecider（ST图边界决策）↓
7. PiecewiseJerkSpeed（速度优化）↓
8. RssDecider（RSS安全检查）

---

7. 参考线生成

7.1 参考线的作用

参考线（Reference Line）是Planning模块的基础：

• 提供车辆前后一定范围（默认150m前，50m后）的局部路径
• 基于全局路由路径，但经过平滑处理
• 包含车道信息、道路信息、动态障碍物信息
• 作为路径规划的参考基准

7.2 ReferenceLineProvider - 参考线提供者

特点：

• 独立线程：周期性运行（默认50ms），不阻塞主规划流程
• 提前计算：在主规划线程需要时已经准备好
• 缓存机制：缓存最近的参考线结果

文件位置：planning_base/reference_line/reference_line_provider.h

class ReferenceLineProvider {public:void Start();void Stop();// 获取参考线（给主规划线程调用）bool GetReferenceLines(std::list<ReferenceLine>* reference_lines,std::list<hdmap::RouteSegments>* segments);// 更新路由（当收到新的RoutingResponse时）void UpdateRoutingResponse(const routing::RoutingResponse& routing);private:// 参考线生成线程（周期运行）void GenerateThread();// 创建参考线bool CreateReferenceLine(std::list<ReferenceLine>* reference_lines,std::list<hdmap::RouteSegments>* segments);std::unique_ptr<std::thread> thread_;std::atomic<bool> is_stop_{false};std::mutex reference_lines_mutex_;std::list<ReferenceLine> reference_lines_;std::list<hdmap::RouteSegments> route_segments_;routing::RoutingResponse routing_;PncMap pnc_map_;
};

7.3 参考线生成流程

1. RoutingResponse（全局路由）↓
2. PncMap::GetRouteSegments()（获取车辆附近的道路段）↓
3. CreateRouteSegments()（转换为参考线格式）↓
4. SmoothReferenceLine()（平滑处理）↓├─> 离散点平滑（默认）│   - FEM位置偏差优化│   - Cos Theta优化│   - SQP优化│├─> 螺旋线平滑│   - 五次螺旋线拟合│└─> 样条曲线平滑- QP样条优化↓
5. ReferenceLine（平滑后的参考线）↓
6. ReferenceLineInfo（添加障碍物、决策信息等）

7.4 参考线平滑算法

离散点平滑（默认）

算法：FEM（Finite Element Method）位置偏差优化

优化目标：

minimize:w1 * Σ(deviation²)          # 偏离原始路径的代价+ w2 * Σ(curvature²)        # 曲率平滑代价+ w3 * Σ(curvature_change²) # 曲率变化率代价subject to:- 道路边界约束- 曲率约束- 曲率变化率约束

配置文件：conf/discrete_points_smoother_config.pb.txt

fem_pos_deviation_smoother {weight_fem_pos_deviation: 1.0e10weight_path_length: 1.0weight_ref_deviation: 1.0apply_curvature_constraint: truemax_curvature: 0.2
}

螺旋线平滑

算法：五次螺旋线（Quintic Spiral）

特点：

• 曲率连续变化
• 适合高速场景
• 计算速度快

样条曲线平滑

算法：QP Spline优化

特点：

• 基于样条曲线拟合
• 参数化表示
• 便于求导和优化

7.5 ReferenceLine与ReferenceLineInfo

ReferenceLine：

class ReferenceLine {public:// 几何信息double Length() const;ReferencePoint GetReferencePoint(double s) const;// 坐标转换bool SLToXY(const SLPoint& sl_point, Vec2d* xy_point) const;bool XYToSL(const Vec2d& xy_point, SLPoint* sl_point) const;// 道路信息bool IsOnRoad(const Vec2d& xy_point) const;bool GetLaneWidth(double s, double* left_width, double* right_width) const;private:std::vector<ReferencePoint> reference_points_;  // 参考点列表hdmap::Path map_path_;                          // 高精地图路径
};

ReferenceLineInfo：

class ReferenceLineInfo {public:// 核心数据const ReferenceLine& reference_line() const;PathData* mutable_path_data();SpeedData* mutable_speed_data();DiscretizedTrajectory* mutable_trajectory();PathDecision* mutable_path_decision();// ST图数据const STGraphData& st_graph_data() const;// 代价计算double Cost() const { return cost_; }void AddCost(double cost) { cost_ += cost; }private:ReferenceLine reference_line_;PathData path_data_;              // 路径数据SpeedData speed_data_;            // 速度数据DiscretizedTrajectory trajectory_; // 最终轨迹PathDecision path_decision_;      // 路径决策STGraphData st_graph_data_;       // ST图数据double cost_ = 0.0;               // 总代价
};

---

8. 交通规则处理

8.1 TrafficDecider - 交通规则决策器

作用：在执行Scenario之前，对所有场景生效的全局交通规则进行处理。

文件位置：planning_interface_base/traffic_rules_base/traffic_decider.h

class TrafficDecider {public:bool Init(const PlanningConfig& config);// 执行所有交通规则apollo::common::Status Execute(Frame* frame,ReferenceLineInfo* reference_line_info);private:std::vector<std::shared_ptr<TrafficRule>> rule_list_;  // 规则列表
};

Execute流程：

Status TrafficDecider::Execute(Frame* frame,ReferenceLineInfo* reference_line_info) {// 遍历所有交通规则for (const auto& rule : rule_list_) {// 应用规则auto ret = rule->ApplyRule(frame, reference_line_info);if (!ret.ok()) {AERROR << "Failed to apply rule: " << rule->Name();}}return Status::OK();
}

8.2 TrafficRule基类

class TrafficRule {public:virtual ~TrafficRule() = default;virtual bool Init(const std::string& name,const std::shared_ptr<DependencyInjector>& injector);// 应用规则virtual common::Status ApplyRule(Frame* const frame,ReferenceLineInfo* const reference_line_info);protected:std::string name_;std::shared_ptr<DependencyInjector> injector_;
};

8.3 交通规则列表

1. TrafficLight - 交通灯规则

   • 检测前方交通灯状态
   • 红灯：在停止线前设置虚拟停止墙
   • 绿灯：允许通过
   • 黄灯：根据距离决定是否停车

2. StopSign - 停止标志规则

   • 在停止标志前完全停车
   • 观察周围环境后才能继续

3. Crosswalk - 人行道规则

   • 检测人行道附近的行人
   • 有行人时在人行道前停车
   • 行人通过后继续

4. Destination - 目的地规则

   • 接近终点时设置停止墙
   • 确保车辆准确停在目的地

5. KeepClear - 禁停区域规则

   • 检测禁停区域（黄色网格）
   • 如果禁停区域内有障碍物阻塞，在区域外停车等待
   • 确保不在禁停区域内停车

6. ReferenceLineEnd - 参考线终点规则

   • 在参考线终点前设置停止墙
   • 防止车辆超出规划范围

7. Rerouting - 重路由规则

   • 检测道路是否被阻塞
   • 长时间被阻塞时触发重路由请求

8. BacksideVehicle - 后向车辆规则

   • 处理后方车辆
   • 决定是否忽略后方车辆

9. YieldSign - 让行标志规则

   • 在让行标志处检测冲突车辆
   • 有冲突时停车让行

8.4 交通规则实现示例 - TrafficLight

class TrafficLight : public TrafficRule {public:Status ApplyRule(Frame* const frame,ReferenceLineInfo* const reference_line_info) override {// 1. 获取前方的交通灯const auto& traffic_lights = frame->GetTrafficLights();if (traffic_lights.empty()) {return Status::OK();  // 没有交通灯，直接返回}// 2. 找到最近的交通灯const auto& nearest_traffic_light = FindNearestTrafficLight(reference_line_info->reference_line(), traffic_lights);if (nearest_traffic_light == nullptr) {return Status::OK();}// 3. 获取交通灯状态const auto& signal = nearest_traffic_light->color();// 4. 根据信号灯状态决策if (signal == perception::TrafficLight::RED ||signal == perception::TrafficLight::YELLOW) {// 红灯或黄灯：设置停止墙const auto& stop_line_s = GetStopLineS(nearest_traffic_light);// 检查是否已经越过停止线if (adc_front_edge_s < stop_line_s) {// 创建虚拟停止障碍物const std::string stop_wall_id ="TL_" + nearest_traffic_light->id().id();auto* path_decision = reference_line_info->path_decision();auto* stop_wall = path_decision->AddObstacle(PathObstacle(stop_wall_id));// 设置停止决策ObjectDecisionType stop_decision;stop_decision.mutable_stop()->set_distance_s(-FLAGS_stop_distance_buffer);stop_decision.mutable_stop()->set_stop_heading(stop_line_heading);path_decision->AddLongitudinalDecision("traffic_light",stop_wall_id,stop_decision);}}// 绿灯：不做处理，车辆可以通过return Status::OK();}
};

8.5 配置文件

traffic_rule_config.pb.txt：

rule {name: "BACKSIDE_VEHICLE"type: "BacksideVehicle"
}
rule {name: "CROSSWALK"type: "Crosswalk"
}
rule {name: "DESTINATION"type: "Destination"
}
rule {name: "KEEP_CLEAR"type: "KeepClear"
}
rule {name: "REFERENCE_LINE_END"type: "ReferenceLineEnd"
}
rule {name: "REROUTING"type: "Rerouting"
}
rule {name: "STOP_SIGN"type: "StopSign"
}
rule {name: "TRAFFIC_LIGHT"type: "TrafficLight"
}
rule {name: "YIELD_SIGN"type: "YieldSign"
}

---

9. 并发设计与性能优化

9.1 双线程架构

Planning模块采用双线程架构：

┌────────────────────────────────────────────────────┐
│        Main Planning Thread (主规划线程)            │
│  - 触发：PredictionObstacles到达（10Hz）            │
│  - 任务：                                          │
│    1. 应用交通规则                                  │
│    2. 更新场景                                     │
│    3. 执行Stage和Task                              │
│    4. 生成轨迹                                     │
│  - 优先级：高                                      │
└────────────────────────────────────────────────────┘┌────────────────────────────────────────────────────┐
│   Reference Line Thread (参考线生成线程)            │
│  - 触发：周期性（50ms）                             │
│  - 任务：                                          │
│    1. 从PncMap获取路由段                           │
│    2. 生成原始参考线                                │
│    3. 平滑参考线                                   │
│    4. 缓存结果                                     │
│  - 优先级：低                                      │
└────────────────────────────────────────────────────┘

优势：

• 解耦：参考线生成不阻塞主规划流程
• 提前计算：参考线在需要时已经准备好
• 稳定性：参考线生成失败不影响主流程（可以使用缓存）

9.2 线程同步机制

ReferenceLineProvider中的线程同步：

class ReferenceLineProvider {private:// 参考线生成线程void GenerateThread() {while (!is_stop_) {std::this_thread::sleep_for(std::chrono::milliseconds(FLAGS_reference_line_period));std::list<ReferenceLine> reference_lines;std::list<hdmap::RouteSegments> segments;// 生成参考线if (CreateReferenceLine(&reference_lines, &segments)) {// 加锁更新缓存std::lock_guard<std::mutex> lock(reference_lines_mutex_);reference_lines_ = std::move(reference_lines);route_segments_ = std::move(segments);}}}// 主规划线程调用（获取参考线）bool GetReferenceLines(std::list<ReferenceLine>* reference_lines,std::list<hdmap::RouteSegments>* segments) {// 加锁读取缓存std::lock_guard<std::mutex> lock(reference_lines_mutex_);if (reference_lines_.empty()) {return false;}*reference_lines = reference_lines_;*segments = route_segments_;return true;}std::mutex reference_lines_mutex_;              // 互斥锁std::list<ReferenceLine> reference_lines_;      // 缓存的参考线
};

9.3 并行优化

9.3.1 多参考线并行处理

当有多条参考线时（例如多车道），可以并行处理：

// planning.conf配置
--nouse_multi_thread_to_add_obstacles  // 默认禁用多线程添加障碍物// 代码中的并行处理（可选）
void Frame::AddObstacles(const std::vector<Obstacle>& obstacles) {if (FLAGS_use_multi_thread_to_add_obstacles) {// 并行添加障碍物到多条参考线std::vector<std::future<void>> futures;for (auto& ref_line_info : reference_line_info_) {futures.push_back(cyber::Async(&ReferenceLineInfo::AddObstacles,&ref_line_info, obstacles));}// 等待所有线程完成for (auto& future : futures) {future.get();}} else {// 串行处理for (auto& ref_line_info : reference_line_info_) {ref_line_info.AddObstacles(obstacles);}}
}

9.3.2 轨迹平滑并行化

// planning.conf配置
--enable_parallel_trajectory_smoothing// 代码实现
void ParallelSmoothTrajectory() {if (FLAGS_enable_parallel_trajectory_smoothing) {// 路径平滑和速度优化并行执行auto path_future = cyber::Async(&SmoothPath, path_data);auto speed_future = cyber::Async(&SmoothSpeed, speed_data);path_future.get();speed_future.get();}
}

9.4 性能优化技术

9.4.1 路径重用

// planning.conf
--enable_reference_line_stitching=false  // 参考线拼接

ReusePath Task：

class ReusePath : public PathGeneration {public:Status Execute(Frame* frame, ReferenceLineInfo* reference_line_info) override {const auto& history = frame->history();// 检查是否可以重用上一帧的路径if (IsReusable(history->latest_frame(), frame)) {// 直接复用const auto& last_path = history->latest_frame()->path_data();reference_line_info->SetPathData(last_path);ADEBUG << "Reuse path from last frame";return Status::OK();}return Status(ErrorCode::PLANNING_ERROR, "Cannot reuse path");}private:bool IsReusable(const Frame* last_frame, const Frame* current_frame) {// 检查条件：// 1. 时间间隔短// 2. 车辆位置变化小// 3. 障碍物变化小// 4. 参考线变化小return CheckTimeGap() && CheckPositionChange() &&CheckObstacleChange() && CheckReferenceLineChange();}
};

9.4.2 计算优化

离散化控制：

// planning_gflags
--trajectory_time_length=8.0           // 轨迹时长（秒）
--trajectory_time_resolution=0.1       // 时间分辨率（秒）
--trajectory_space_resolution=1.0      // 空间分辨率（米）

迭代锚点平滑：

// planning.conf
--use_iterative_anchoring_smoother     // 使用迭代锚点平滑器

S曲线速度平滑：

// planning.conf
--nouse_s_curve_speed_smooth           // 禁用S曲线速度平滑（提升性能）

9.4.3 Smoother性能优化

// planning.conf
--enable_smoother_failsafe             // 启用平滑器失败保护
--enable_parallel_trajectory_smoothing // 启用并行轨迹平滑

9.5 调度配置

DAG配置：planning_component/dag/planning.dag

module_config {module_library : "modules/planning/planning_component/libplanning_component.so"components {class_name : "PlanningComponent"config {name: "planning"config_file_path: "modules/planning/planning_component/conf/planning_config.pb.txt"flag_file_path: "modules/planning/planning_component/conf/planning.conf"# 订阅的Channelreaders: [{channel: "/apollo/prediction"# PredictionObstacles作为主触发},{channel: "/apollo/canbus/chassis"qos_profile: {depth : 15}pending_queue_size: 50},{channel: "/apollo/localization/pose"qos_profile: {depth : 15}pending_queue_size: 50}]}}
}

关键参数：

• qos_profile.depth: 消息队列深度（15）
• pending_queue_size: 待处理队列大小（50）

9.6 性能监控

Planning模块内置性能监控：

// 每个Task执行时间记录
void Stage::RecordDebugInfo(ReferenceLineInfo* reference_line_info,const std::string& name,const double time_diff_ms) {auto* debug = reference_line_info->mutable_debug();auto* task_stats = debug->mutable_planning_data()->mutable_task_stats()->Add();task_stats->set_name(name);task_stats->set_time_ms(time_diff_ms);
}

输出示例：

LaneFollowPath: 12.3ms
PathDecider: 3.5ms
SpeedBoundsDecider: 5.2ms
PiecewiseJerkSpeed: 18.7ms
Total: 45.6ms

---

10. 配置与调度

10.1 核心配置文件

10.1.1 planning_config.pb.txt - 主配置

路径：planning_component/conf/planning_config.pb.txt

# 规划模式选择
standard_planning_config {planner_type: PUBLIC_ROAD    # 使用PublicRoadPlanner# PublicRoadPlanner配置planner_public_road_config {# 场景列表（按优先级从高到低）scenario {name: "EMERGENCY_STOP"type: "EmergencyStopScenario"}scenario {name: "EMERGENCY_PULL_OVER"type: "EmergencyPullOverScenario"}scenario {name: "VALET_PARKING"type: "ValetParkingScenario"}scenario {name: "PULL_OVER"type: "PullOverScenario"}# ... 其他场景scenario {name: "LANE_FOLLOW"type: "LaneFollowScenario"}}
}# 导航规划配置（相对地图模式）
navigation_planning_config {planner_type: NAVIplanner_navi_config {# NaviPlanner配置}
}

10.1.2 planning.conf - Gflags配置

路径：planning_component/conf/planning.conf

# 全局配置
--flagfile=modules/common/data/global_flagfile.txt# 交通规则配置文件
--traffic_rule_config_filename=modules/planning/planning_component/conf/traffic_rule_config.pb.txt# 速度限制
--planning_upper_speed_limit=20.00      # 最高速度限制（m/s）
--default_cruise_speed=11.18            # 默认巡航速度（m/s，40km/h）# 障碍物处理
--ignore_overlapped_obstacle=true       # 忽略重叠障碍物
--nonstatic_obstacle_nudge_l_buffer=0.4 # 动态障碍物横向缓冲（m）# 换道配置
--prioritize_change_lane                # 优先换道
--min_length_for_lane_change=5.0       # 换道最小长度（m）# 参考线平滑器选择
--smoother_config_filename=modules/planning/planning_component/conf/discrete_points_smoother_config.pb.txt
# --smoother_config_filename=modules/planning/planning_component/conf/qp_spline_smoother_config.pb.txt
# --smoother_config_filename=modules/planning/planning_component/conf/spiral_smoother_config.pb.txt# 参考线配置
--enable_reference_line_stitching=false # 参考线拼接# 性能优化
--enable_smoother_failsafe              # 平滑器失败保护
--enable_parallel_trajectory_smoothing  # 并行轨迹平滑
--nouse_s_curve_speed_smooth           # 不使用S曲线速度平滑
--use_iterative_anchoring_smoother     # 使用迭代锚点平滑器# 多线程配置
--nouse_multi_thread_to_add_obstacles  # 不使用多线程添加障碍物# 目的地配置
--destination_check_distance=4.0        # 目的地检查距离（m）
--enable_pull_over_at_destination=false # 到达目的地时自动靠边停车# ST图配置
--use_st_drivable_boundary=false       # 使用ST可行驶边界# 调试配置
--enable_print_curve=true              # 打印曲线
--export_chart=true                    # 导出图表
# --enable_record_debug=true           # 记录调试信息

10.1.3 场景配置示例

LaneFollow场景配置：scenarios/lane_follow/conf/scenario_config.pb.txt

scenario_type: LANE_FOLLOWstage_type: LANE_FOLLOW_DEFAULT_STAGEstage_config: {stage_type: LANE_FOLLOW_DEFAULT_STAGEenabled: true# Task列表（按执行顺序）task_type: LANE_CHANGE_PATHtask_type: LANE_BORROW_PATHtask_type: LANE_FOLLOW_PATHtask_type: PATH_DECIDERtask_type: SPEED_BOUNDS_DECIDERtask_type: PIECEWISE_JERK_SPEEDtask_type: RSS_DECIDERtask_config: {task_type: LANE_CHANGE_PATHlane_change_path_config {# LaneChangePath配置参数}}# ... 其他Task配置
}

10.2 启动方式

10.2.1 使用 cyber_launch

cyber_launch start modules/planning/planning_component/launch/planning.launch

planning.launch：

<cyber><module><name>planning</name><dag_conf>modules/planning/planning_component/dag/planning.dag</dag_conf><process_name>planning</process_name></module>
</cyber>

10.2.2 使用 mainboard

mainboard -d modules/planning/planning_component/dag/planning.dag

10.3 关键参数调优

速度相关参数

# 最高速度限制（m/s）
--planning_upper_speed_limit=20.0       # 72 km/h# 巡航速度（m/s）
--default_cruise_speed=11.18            # 40 km/h# 速度缓冲区（m/s）
--speed_lower_bound=-0.1
--speed_upper_bound=50.0# 加速度限制（m/s²）
--longitudinal_acceleration_lower_bound=-4.5
--longitudinal_acceleration_upper_bound=4.0# Jerk限制（m/s³）
--longitudinal_jerk_lower_bound=-4.0
--longitudinal_jerk_upper_bound=4.0

路径相关参数

# 横向缓冲区（m）
--static_obstacle_nudge_l_buffer=0.3    # 静态障碍物
--nonstatic_obstacle_nudge_l_buffer=0.4 # 动态障碍物# 换道参数
--min_length_for_lane_change=5.0        # 换道最小长度（m）
--lane_change_prepare_length=20.0       # 换道准备距离（m）# 路径平滑
--path_smoothing_max_iteration=1000     # 最大迭代次数

安全相关参数

# 停车缓冲距离（m）
--stop_distance_buffer=0.5# 障碍物安全距离（m）
--follow_min_distance=3.0               # 跟车最小距离# RSS安全参数
--rss_response_time=1.0                 # 反应时间（秒）
--rss_max_accel_for_brake=2.0          # 最大制动加速度

---

11. 扩展开发指南

11.1 添加新场景

步骤1：定义场景类

// scenarios/my_scenario/my_scenario.h
#pragma once#include "modules/planning/planning_interface_base/scenario_base/scenario.h"namespace apollo {
namespace planning {// 场景上下文（存储场景状态）
struct MyScenarioContext : public ScenarioContext {bool is_initialized = false;double target_position = 0.0;
};class MyScenario : public Scenario {public:MyScenarioContext* GetContext() override {return &context_;}// 判断是否可以切入此场景bool IsTransferable(const Scenario* other_scenario,const Frame& frame) override {// 实现场景切入条件// 例如：检测到特定的地图元素或命令if (DetectMyScenarioTrigger(frame)) {return true;}return false;}bool Enter(Frame* frame) override {AINFO << "Enter MyScenario";context_.is_initialized = false;return true;}bool Exit(Frame* frame) override {AINFO << "Exit MyScenario";return true;}private:MyScenarioContext context_;bool DetectMyScenarioTrigger(const Frame& frame) {// 实现触发条件检测return false;}
};}  // namespace planning
}  // namespace apollo

步骤2：定义Stage

// scenarios/my_scenario/stage_approach.h
#pragma once#include "modules/planning/planning_interface_base/scenario_base/stage.h"namespace apollo {
namespace planning {class MyScenarioStageApproach : public Stage {public:StageResult Process(const common::TrajectoryPoint& planning_init_point,Frame* frame) override {// 获取场景上下文auto context = GetContextAs<MyScenarioContext>();// 执行Taskauto ret = ExecuteTaskOnReferenceLine(planning_init_point, frame);if (ret.GetStageStatus() == StageStatusType::ERROR) {return FinishStage();}// 检查是否完成当前Stageif (IsApproachFinished(frame)) {// 切换到下一个Stagenext_stage_ = "MY_SCENARIO_STAGE_EXECUTE";return FinishStage();}return ret;}private:bool IsApproachFinished(Frame* frame) {// 判断接近阶段是否完成return false;}
};}  // namespace planning
}  // namespace apollo

步骤3：配置场景Pipeline

scenario_config.pb.txt：

scenario_type: MY_SCENARIO# Stage 1: Approach
stage_type: MY_SCENARIO_STAGE_APPROACH
stage_config: {stage_type: MY_SCENARIO_STAGE_APPROACHenabled: truetask_type: LANE_FOLLOW_PATHtask_type: PATH_DECIDERtask_type: SPEED_BOUNDS_DECIDERtask_type: PIECEWISE_JERK_SPEED
}# Stage 2: Execute
stage_type: MY_SCENARIO_STAGE_EXECUTE
stage_config: {stage_type: MY_SCENARIO_STAGE_EXECUTEenabled: truetask_type: MY_CUSTOM_TASKtask_type: PIECEWISE_JERK_SPEED
}

步骤4：注册场景插件

cyberfile.xml：

<package><name>my_scenario</name><version>1.0.0</version><class type="Scenario" name="MyScenario"><plugin_lib>modules/planning/scenarios/my_scenario/libmy_scenario.so</plugin_lib></class><class type="Stage" name="MY_SCENARIO_STAGE_APPROACH"><plugin_lib>modules/planning/scenarios/my_scenario/libmy_scenario.so</plugin_lib></class><class type="Stage" name="MY_SCENARIO_STAGE_EXECUTE"><plugin_lib>modules/planning/scenarios/my_scenario/libmy_scenario.so</plugin_lib></class>
</package>

步骤5：添加到Planner配置

planning_config.pb.txt：

standard_planning_config {planner_type: PUBLIC_ROADplanner_public_road_config {# 在合适的优先级位置添加新场景scenario {name: "MY_SCENARIO"type: "MyScenario"}# ... 其他场景}
}

11.2 添加新Task

步骤1：定义Task类

// tasks/my_task/my_task.h
#pragma once#include "modules/planning/planning_interface_base/task_base/common/decider.h"
// 或者继承其他基类：
// #include "modules/planning/planning_interface_base/task_base/common/path_generation.h"
// #include "modules/planning/planning_interface_base/task_base/common/speed_optimizer.h"namespace apollo {
namespace planning {class MyTask : public Decider {public:bool Init(const std::string& config_dir,const std::string& name,const std::shared_ptr<DependencyInjector>& injector) override {if (!Task::Init(config_dir, name, injector)) {return false;}// 加载配置MyTaskConfig config;if (!LoadConfig(&config)) {AERROR << "Failed to load config for " << Name();return false;}// 初始化成员变量my_param_ = config.my_param();return true;}common::Status Execute(Frame* frame,ReferenceLineInfo* reference_line_info) override {// 实现Task逻辑// 1. 获取输入数据const auto& path_data = reference_line_info->path_data();const auto& obstacles = reference_line_info->path_decision()->obstacles();// 2. 执行算法ProcessMyAlgorithm(path_data, obstacles);// 3. 输出结果// 例如：设置决策// reference_line_info->path_decision()->AddDecision(...);return Status::OK();}private:void ProcessMyAlgorithm(const PathData& path_data,const IndexedList<Obstacle>& obstacles) {// 实现具体算法}double my_param_;
};}  // namespace planning
}  // namespace apollo

步骤2：配置Task

my_task_config.pb.txt：

my_param: 1.5
another_param: true

步骤3：注册Task插件

cyberfile.xml：

<package><name>my_task</name><version>1.0.0</version><class type="Task" name="MY_TASK"><plugin_lib>modules/planning/tasks/my_task/libmy_task.so</plugin_lib></class>
</package>

步骤4：在Stage中使用Task

scenario_config.pb.txt：

stage_config: {stage_type: MY_STAGEenabled: truetask_type: LANE_FOLLOW_PATHtask_type: MY_TASK           # 添加新Tasktask_type: PATH_DECIDERtask_type: PIECEWISE_JERK_SPEEDtask_config: {task_type: MY_TASK# Task配置路径会自动加载}
}

11.3 添加新TrafficRule

步骤1：定义TrafficRule类

// traffic_rules/my_rule/my_rule.h
#pragma once#include "modules/planning/planning_interface_base/traffic_rules_base/traffic_rule.h"namespace apollo {
namespace planning {class MyRule : public TrafficRule {public:bool Init(const std::string& name,const std::shared_ptr<DependencyInjector>& injector) override {if (!TrafficRule::Init(name, injector)) {return false;}// 加载配置MyRuleConfig config;if (!LoadConfig(&config)) {return false;}threshold_ = config.threshold();return true;}common::Status ApplyRule(Frame* const frame,ReferenceLineInfo* const reference_line_info) override {// 实现交通规则逻辑// 1. 检测特定条件if (DetectMyRuleCondition(frame, reference_line_info)) {// 2. 应用决策ApplyDecision(frame, reference_line_info);}return Status::OK();}private:bool DetectMyRuleCondition(Frame* const frame,ReferenceLineInfo* const reference_line_info) {// 检测规则触发条件return false;}void ApplyDecision(Frame* const frame,ReferenceLineInfo* const reference_line_info) {// 应用决策（例如设置停止墙）const std::string virtual_obstacle_id = "MY_RULE_STOP_WALL";// 创建虚拟障碍物auto* path_decision = reference_line_info->path_decision();auto* virtual_obstacle = path_decision->AddObstacle(PathObstacle(virtual_obstacle_id));// 设置停止决策ObjectDecisionType stop_decision;stop_decision.mutable_stop()->set_distance_s(stop_distance_);path_decision->AddLongitudinalDecision("my_rule",virtual_obstacle_id,stop_decision);}double threshold_;double stop_distance_;
};}  // namespace planning
}  // namespace apollo

步骤2：注册TrafficRule

cyberfile.xml：

<package><name>my_rule</name><version>1.0.0</version><class type="TrafficRule" name="MyRule"><plugin_lib>modules/planning/traffic_rules/my_rule/libmy_rule.so</plugin_lib></class>
</package>

步骤3：添加到TrafficRule配置

traffic_rule_config.pb.txt：

rule {name: "MY_RULE"type: "MyRule"
}
rule {name: "TRAFFIC_LIGHT"type: "TrafficLight"
}
# ... 其他规则

11.4 调试技巧

启用调试日志

# planning.conf
--enable_record_debug=true              # 记录调试信息
--enable_print_curve=true               # 打印曲线
--export_chart=true                     # 导出图表# 设置日志级别
export GLOG_v=4                         # Verbose日志级别
export GLOG_logtostderr=1               # 输出到stderr

使用Cyber Monitor

# 监控Planning输出
cyber_monitor# 选择 /apollo/planning
# 可以看到实时的轨迹点、速度、加速度等信息

使用Cyber Recorder

# 录制数据
cyber_recorder record -a -o planning_test.record# 回放数据
cyber_recorder play -f planning_test.record -l

可视化调试

# 启动Dreamview
bash scripts/bootstrap.sh# 在浏览器打开 http://localhost:8888
# 选择Module Controller -> Planning
# 在Layer Menu中启用Planning相关图层

---

总结

Apollo Planning模块是一个高度模块化、可扩展的轨迹规划系统：

核心特性

1. 双层状态机架构

   • Scenario层：场景切换
   • Stage层：阶段管理
   • 模块化、易维护、可扩展

2. 插件化设计

   • Planner插件：4种规划器
   • Scenario插件：12种场景
   • Task插件：22种任务
   • TrafficRule插件：9种交通规则

3. 双线程架构

   • 主规划线程：实时轨迹生成
   • 参考线线程：周期性生成参考线
   • 解耦、高效、稳定

4. 丰富的优化算法

   • 路径优化：DP + QP、FEM、Hybrid A*
   • 速度优化：Piecewise Jerk、QP、NLP
   • 参考线平滑：离散点平滑、螺旋线、样条曲线

技术亮点

• 场景化管理：不同场景独立实现，互不干扰
• 任务流水线：灵活配置Task执行顺序
• 交通规则引擎：全局生效的决策逻辑
• 参考线生成：提前计算、缓存机制
• 性能优化：路径重用、并行优化、参数调优

扩展性

• 清晰的接口设计（Scenario、Stage、Task、TrafficRule基类）
• 插件化注册机制
• 配置驱动的参数系统
• 完善的调试工具

Planning模块为Apollo自动驾驶系统提供了强大的决策和规划能力，是实现安全、舒适、高效自动驾驶的核心大脑。