Apollo Planning 规划模块接口调用流程图与源码分析

---

  团队博客: 汽车电子社区

---

一、模块概述

  Planning模块是Apollo自动驾驶系统的"大脑"，负责根据感知和定位信息，结合地图数据，生成安全、舒适、高效的车辆行驶轨迹。该模块融合了多种规划算法，支持不同的驾驶场景和交通状况。

二、模块架构

2.1 目录结构

modules/planning/
├── planning_base/                  # 规划基础框架
├── planning_component/             # 规划主组件
├── planning_interface_base/        # 规划接口基类
├── planning_open_space/            # 开放空间规划
├── planners/                       # 多种规划算法实现
│   ├── lattice/                   # 栅格规划
│   ├── navi/                      # 导航规划
│   ├── public_road/               # 公共道路规划
│   └── rtk/                       # RTK规划
├── scenarios/                      # 场景化规划
├── tasks/                          # 规划任务
└── traffic_rules/                  # 交通规则

2.2 核心组件

  1. PlanningComponent - 规划主组件
  2. Frame - 规划帧数据结构
  3. ReferenceLineInfo - 参考线信息
  4. Obstacle - 障碍物信息
  5. DiscretizedTrajectory - 离散轨迹

三、接口调用流程图

3.1 整体规划流程图

3.2 Lattice规划详细流程图

3.3 场景化规划流程图

四、关键源码分析

4.1 主组件源码分析

4.1.1 PlanningComponent 核心源码分析

  类定义位置: modules/planning/planning_component/planning_component.h

  核心功能:
    - 继承自 cyber::Component<prediction::PredictionObstacles, canbus::Chassis, localization::LocalizationEstimate>
    - 作为Planning模块的主要入口点，接收预测、底盘和定位数据
    - 协调各种数据源，调用规划算法，发布轨迹结果

  关键成员变量:

private:
  // 订阅者 - 接收各种输入数据
  std::shared_ptr<cyber::Reader<perception::TrafficLightDetection>> traffic_light_reader_;
  std::shared_ptr<apollo::cyber::Client<apollo::external_command::LaneFollowCommand, apollo::external_command::CommandStatus>> rerouting_client_;
  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::Reader<control::ControlInteractiveMsg>> control_interactive_reader_;

  // 发布者 - 输出规划结果
  std::shared_ptr<cyber::Writer<ADCTrajectory>> planning_writer_;
  std::shared_ptr<cyber::Writer<routing::RoutingRequest>> rerouting_writer_;
  std::shared_ptr<cyber::Writer<PlanningLearningData>> planning_learning_data_writer_;
  std::shared_ptr<cyber::Writer<external_command::CommandStatus>> command_status_writer_;

  // 数据缓存和状态
  std::mutex mutex_;
  perception::TrafficLightDetection traffic_light_;
  routing::RoutingResponse routing_;
  planning::PadMessage pad_msg_;
  relative_map::MapMsg relative_map_;
  storytelling::Stories stories_;
  PlanningCommand planning_command_;
  control::ControlInteractiveMsg control_interactive_msg_;
  LocalView local_view_;

  // 核心规划对象
  std::unique_ptr<PlanningBase> planning_base_;
  std::shared_ptr<DependencyInjector> injector_;
  PlanningConfig config_;
  MessageProcess message_process_;

  初始化流程 (Init() 方法):

bool PlanningComponent::Init() {
  // 1. 创建依赖注入器
  injector_ = std::make_shared<DependencyInjector>();

  // 2. 根据模式选择规划器
  if (FLAGS_use_navigation_mode) {
    planning_base_ = std::make_unique<NaviPlanning>(injector_);  // 导航模式
  } else {
    planning_base_ = std::make_unique<OnLanePlanning>(injector_);  // 道路模式
  }

  // 3. 加载配置文件
  ACHECK(ComponentBase::GetProtoConfig(&config_));

  // 4. 初始化消息处理（学习模式）
  if (FLAGS_planning_offline_learning || config_.learning_mode() != PlanningConfig::NO_LEARNING) {
    message_process_.Init(config_, injector_);
  }

  // 5. 初始化规划器
  planning_base_->Init(config_);

  // 6. 创建各种订阅者
  planning_command_reader_ = node_->CreateReader<PlanningCommand>(
      config_.topic_config().planning_command_topic(),
      [this](const std::shared_ptr<PlanningCommand>& planning_command) {
        std::lock_guard<std::mutex> lock(mutex_);
        planning_command_.CopyFrom(*planning_command);
      });

  traffic_light_reader_ = node_->CreateReader<TrafficLightDetection>(
      config_.topic_config().traffic_light_detection_topic(),
      [this](const std::shared_ptr<TrafficLightDetection>& traffic_light) {
        std::lock_guard<std::mutex> lock(mutex_);
        traffic_light_.CopyFrom(*traffic_light);
      });

  // ... 其他订阅者类似 ...

  // 7. 创建发布者
  planning_writer_ = node_->CreateWriter<ADCTrajectory>(
      config_.topic_config().planning_trajectory_topic());

  rerouting_client_ = node_->CreateClient<apollo::external_command::LaneFollowCommand, external_command::CommandStatus>(
      config_.topic_config().routing_request_topic());

  return true;
}

  主处理流程 (Proc() 方法):

bool PlanningComponent::Proc(
    const std::shared_ptr<prediction::PredictionObstacles>& prediction_obstacles,
    const std::shared_ptr<canbus::Chassis>& chassis,
    const std::shared_ptr<localization::LocalizationEstimate>& localization_estimate) {
  
  // 1. 检查重新路由请求
  CheckRerouting();

  // 2. 更新本地视图数据
  local_view_.prediction_obstacles = prediction_obstacles;
  local_view_.chassis = chassis;
  local_view_.localization_estimate = localization_estimate;

  // 3. 更新其他数据源（需要互斥锁保护）
  {
    std::lock_guard<std::mutex> lock(mutex_);
    if (!local_view_.planning_command || !common::util::IsProtoEqual(local_view_.planning_command->header(),
                                                                     planning_command_.header())) {
      local_view_.planning_command = std::make_shared<PlanningCommand>(planning_command_);
    }
    // 更新其他数据...
  }

  // 4. 执行规划
  ADCTrajectory trajectory_pb;
  planning_base_->Plan(&trajectory_pb);

  // 5. 发布轨迹
  planning_writer_->Write(std::make_shared<ADCTrajectory>(trajectory_pb));

  return true;
}

  关键技术特点:
    1. 多模式支持: 导航模式和道路模式两种规划方式
    2. 数据融合: 整合多种数据源（预测、定位、交通灯等）
    3. 线程安全: 使用互斥锁保护共享数据
    4. 学习支持: 支持离线学习和数据收集
    5. 路由重计算: 自动检测和触发重新路由

4.1.2 Frame 规划帧数据结构

  位置: modules/planning/planning_base/common/frame.h

  核心功能:
    - 封装单次规划周期所需的所有数据
    - 管理障碍物、参考线、规划结果等信息
    - 提供统一的数据访问接口

  关键成员变量:

class Frame {
 private:
  // 基础信息
  uint32_t sequence_num_;                              // 序列号
  LocalView local_view_;                               // 本地数据视图
  common::VehicleState vehicle_state_;                 // 车辆状态
  common::TrajectoryPoint planning_start_point_;       // 规划起始点

  // 参考线信息
  std::list<ReferenceLineInfo> reference_line_info_;  // 参考线信息列表
  ReferenceLineProvider *reference_line_provider_;     // 参考线提供者

  // 障碍物管理
  ThreadSafeIndexedObstacles obstacles_;              // 线程安全障碍物列表
  std::unordered_map<std::string, Obstacle> obstacles_by_id_;  // 按ID索引的障碍物

  // 规划结果
  PublishableTrajectory computed_trajectory_;          // 计算得到的轨迹
  ADCTrajectory current_frame_planned_trajectory_;    // 当前帧规划轨迹
  DiscretizedPath current_frame_planned_path_;        // 当前帧规划路径

  // 特殊场景信息
  OpenSpaceInfo open_space_info_;                     // 开放空间信息
  bool is_near_destination_ = false;                  // 是否接近目的地

  // 交通和信号信息
  perception::TrafficLight traffic_light_;            // 交通灯信息
  PadMessage::DrivingAction pad_msg_driving_action_;   // 驾驶动作
};

  核心接口:

class Frame {
 public:
  // 初始化接口
  common::Status Init(const common::VehicleStateProvider *vehicle_state_provider,
                      const std::list<ReferenceLine> &reference_lines,
                      const std::list<hdmap::RouteSegments> &segments,
                      const std::vector<routing::LaneWaypoint> &future_route_waypoints,
                      const EgoInfo *ego_info);

  // 开放空间初始化
  common::Status InitForOpenSpace(const common::VehicleStateProvider *vehicle_state_provider,
                                  const EgoInfo *ego_info);

  // 访问器接口
  const std::list<ReferenceLineInfo>& reference_line_info() const;
  std::list<ReferenceLineInfo>* mutable_reference_line_info();
  
  const std::vector<const Obstacle*> obstacles() const;
  Obstacle* Find(const std::string& id);

  // 查找参考线
  const ReferenceLineInfo* FindDriveReferenceLineInfo();
  const ReferenceLineInfo* FindTargetReferenceLineInfo();
  const ReferenceLineInfo* FindFailedReferenceLineInfo();

  // 创建虚拟障碍物
  const Obstacle* CreateStopObstacle(ReferenceLineInfo* const reference_line_info,
                                    const std::string& obstacle_id, 
                                    const double obstacle_s,
                                    double stop_wall_width = 4.0);

  const Obstacle* CreateStaticObstacle(ReferenceLineInfo* const reference_line_info,
                                      const std::string& obstacle_id,
                                      const double obstacle_start_s,
                                      const double obstacle_end_s);

  // 路由重计算
  bool Rerouting(PlanningContext* planning_context);

  // 预测时间对齐
  static void AlignPredictionTime(const double planning_start_time,
                                  prediction::PredictionObstacles* prediction_obstacles);
};

  Frame的数据流转过程:
    1. 数据初始化: 从LocalView加载感知、定位、预测数据
    2. 参考线构建: 基于高精地图和路由信息生成参考线
    3. 障碍物处理: 将预测障碍物转换为规划用的Obstacle对象
    4. 规划执行: 各种规划器基于Frame数据执行规划算法
    5. 结果存储: 将规划的轨迹和路径存储在Frame中
    6. 数据发布: 将Frame中的规划结果发布给Control模块

4.1.3 ReferenceLineInfo 参考线信息

  位置: modules/planning/planning_base/common/reference_line_info.h

  核心功能:
    - 封装单条参考线的所有信息
    - 提供SL坐标系下的规划环境
    - 管理参考线上的障碍物和决策信息

4.2 规划器算法分析

4.2.1 Lattice规划器

  位置: modules/planning/planners/lattice/lattice_planner.h

  核心功能:
    - 继承自 PlannerWithReferenceLine，是基于参考线的规划器
    - 使用Lattice坐标系进行路径和速度规划
    - 适用于结构化道路环境

  核心接口:

class LatticePlanner : public PlannerWithReferenceLine {
 public:
  // 获取规划器名称
  std::string Name() override { return "LATTICE"; }

  // 初始化规划器
  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;

  // 单参考线规划接口
  common::Status PlanOnReferenceLine(
      const common::TrajectoryPoint& planning_init_point, Frame* frame,
      ReferenceLineInfo* reference_line_info) override;
};

  Lattice规划流程:
    1. 参考线处理: 获取并处理当前参考线信息
    2. 障碍物投影: 将障碍物投影到SL坐标系
    3. 纵向采样: 在S方向（纵向）进行时间和距离采样
    4. 横向采样: 在L方向（横向）进行偏移采样
    5. 轨迹生成: 组合纵向和横向采样点生成候选轨迹
    6. 代价评估: 计算每条轨迹的安全性和舒适性代价
    7. 轨迹优化: 使用二次规划优化最优轨迹
    8. 轨迹输出: 生成最终的规划轨迹

4.2.2 Navi规划器（导航模式）

  位置: modules/planning/planners/navi/navi_planner.h

  核心功能:
    - 专为导航模式设计，不依赖高精地图
    - 使用相对地图和FLU（前-左-上）坐标系
    - 支持巡航、跟车、超车、变道、停车等任务

  核心特点:

class NaviPlanner : public PlannerWithReferenceLine {
 public:
  std::string Name() override { return "NAVI"; }

  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;

 private:
  // 任务注册和管理
  void RegisterTasks();
  
  // 速度规划
  std::vector<common::SpeedPoint> GenerateInitSpeedProfile(...);
  std::vector<common::SpeedPoint> DummyHotStart(...);
  std::vector<common::SpeedPoint> GenerateSpeedHotStart(...);
  
  // 降级方案
  void GenerateFallbackPathProfile(...);
  void GenerateFallbackSpeedProfile(...);
  SpeedData GenerateStopProfile(...) const;

 private:
  apollo::common::util::Factory<NaviTaskType, NaviTask> task_factory_;  // 任务工厂
  std::vector<std::unique_ptr<NaviTask>> tasks_;                       // 任务列表
};

  Navi规划特点:
    1. 相对地图: 不依赖高精地图，使用相对位置信息
    2. 任务导向: 基于具体驾驶任务进行规划
    3. 轻量化: 适合低算力环境运行
    4. 实时性: 优化的算法确保实时响应

4.3 场景化规划分析

4.3.1 场景管理

  Apollo Planning采用场景化规划架构，将复杂的驾驶场景分解为具体的场景类型，每种场景都有专门的规划策略。

  场景基类: modules/planning/planning_interface_base/scenario_base/scenario.h

  主要场景类型:

    1. LaneFollowScenario - 车道保持场景
    2. PullOverScenario - 靠边停车场景
    3. TrafficLightProtectedScenario - 受保护红绿灯场景
    4. TrafficLightUnprotectedLeftTurn - 无保护左转场景
    5. TrafficLightUnprotectedRightTurn - 无保护右转场景
    6. StopSignUnprotected - 无保护停车标志场景
    7. YieldSign - 让行标志场景
    8. EmergencyStop - 紧急停车场景
    9. ValetParking - 代客泊车场景
    10. ParkAndGo - 停车起步场景

  场景示例分析 - LaneFollowScenario:

class LaneFollowScenario : public Scenario {
 public:
  // 获取场景上下文
  ScenarioContext* GetContext() override { return nullptr; }

  // 判断是否可以从其他场景转换到当前场景
  bool IsTransferable(const Scenario* other_scenario,
                      const Frame& frame) override;
};

  场景转换流程:
    1. 场景识别: 基于当前环境状态判断最适合的场景
    2. 转换条件: 检查场景间的转换条件是否满足
    3. 状态保持: 维护场景特定的状态信息
    4. 任务执行: 执行场景内的具体规划任务

  场景配置示例:

scenario_config: {
  scenario_type: LANE_FOLLOW
  start_condition: {
    lane_follow_start_condition: {
      require_lane_keeping: true
      min_lane_length: 30.0
    }
  }
  end_condition: {
    lane_follow_end_condition: {
      max_deviation: 1.5
      min_distance_to_destination: 5.0
    }
  }
}

4.4 配置文件分析

4.4.1 主配置文件

  Planning配置: modules/planning/planning_base/conf/planning_config.pb.txt

planning_config: {
  task: TOPIC_LOGIC
  planner_type: PUBLIC_ROAD_PLANNER
  planner_type: LATTICE
  planner_type: NAVI
  
  learning_mode: NO_LEARNING  # 或 OFFLINE_LEARNING, ONLINE_LEARNING
  
  topic_config: {
    planning_command_topic: "/apollo/planning"
    traffic_light_detection_topic: "/apollo/perception/traffic_light"
    planning_pad_topic: "/apollo/planning/pad"
    planning_trajectory_topic: "/apollo/planning"
    routing_request_topic: "/apollo/routing_request"
    story_telling_topic: "/apollo/storytelling"
    planning_learning_data_topic: "/apollo/planning/learning_data"
    relative_map_topic: "/apollo/relative_map"
    control_interative_topic: "/apollo/control/interactive"
    planning_command_status: "/apollo/planning/command_status"
  }
  
  default_planning_type: LANE_FOLLOW
  
  enable_reference_line_stitching: true
  enable_reference_line_provider: true
  
  warm_start: true
  
  max_frame_time: 1000  # ms
  
  # 规划算法参数
  lattice_planner_config: {
    trajectory_time: 8.0
    time_density: 0.1
    
    # 纵向规划参数
    longitudinal_sampling: {
      sample_interval: 5.0
      num_sample_points: 8
      distance_interval: 2.0
    }
    
    # 横向规划参数
    lateral_sampling: {
      sample_resolution: 0.5
      max_sample_points: 5
    }
    
    # 代价函数权重
    cost_weight: {
      obstacle_weight: 50.0
      comfort_weight: 10.0
      reference_line_weight: 1.0
    }
  }
}

4.4.2 DAG配置文件

  启动配置: modules/planning/planning_component/dag/planning.dag

# 定义组件
component {
  name: "PlanningComponent"
  config_path: "conf/planning"
  config_name: "planning_config.pb.txt"
  schedule_type: "SCHEDULER_CHOREOGRAPHER"
  retry: 3
}

# 定义调度器
scheduler {
  type: "SCHEDULER_CHOREOGRAPHER"
  routine: "CYBER_CPU"
  priority: 3
}

# 定义依赖
# 输入依赖
data_service: {
  readers: [
    {
      channel: "/apollo/prediction/obstacles"
      qos_profile: {
        reliability: "RELIABILITY_RELIABLE"
        durability: "DURABILITY_VOLATILE"
      }
    },
    {
      channel: "/apollo/canbus/chassis"
      qos_profile: {
        reliability: "RELIABILITY_RELIABLE"
        durability: "DURABILITY_VOLATILE"
      }
    },
    {
      channel: "/apollo/localization/pose"
      qos_profile: {
        reliability: "RELIABILITY_RELIABLE"
        durability: "DURABILITY_VOLATILE"
      }
    }
  ]
}

# 输出配置
data_service: {
  writers: [
    {
      channel: "/apollo/planning"
      qos_profile: {
        reliability: "RELIABILITY_RELIABLE"
        durability: "DURABILITY_VOLATILE"
      }
    }
  ]
}

4.5 消息接口定义

4.5.1 ADCTrajectory消息

  位置: modules/common_msgs/planning_msgs/planning.proto

message ADCTrajectory {
  // 消息头
  apollo.common.Header header = 1;
  
  // 总体轨迹信息
  double total_path_length = 2;           // 总路径长度
  double total_path_time = 3;            // 总路径时间
  
  // 轨迹点序列
  repeated apollo.common.TrajectoryPoint trajectory_point = 4;
  
  // 车辆状态
  apollo.common.EngageAdvice engage_advice = 5;
  apollo.common.VehicleSignal signal = 6;
  
  // 规划相关信息
  Debug debug = 7;                        // 调试信息
  DrivingAction driving_action = 8;       // 驾驶行为
  ErrorType error_code = 9;              // 错误类型
  
  // 输出路径
  apollo.common.Path path = 10;          // 路径信息
  
  // 参考线信息
  repeated DebugReferenceLine debug_reference_line = 11;
  
  // 决策信息
  DecisionResult decision_result = 12;
  
  // 车辆位置信息
  apollo.common.PointENU estop = 13;     // 紧急停止位置
  
  // 限速信息
  double right_of_way_status = 14;        // 路权状态
  apollo.common.PathPoint lane_change_info = 15;  // 变道信息
}

message TrajectoryPoint {
  apollo.common.PathPoint path_point = 1;  // 路径点
  double v = 2;                           // 速度
  double a = 3;                           // 加速度
  double relative_time = 4;               // 相对时间
  
  // 控制量
  double steer = 5;                       // 转向角
  double kappa = 6;                       // 曲率
  double dkappa = 7;                      // 曲率变化率
  
  // 加速度信息
  double s = 8;                           // 弧长
  double d = 9;                           // 横向偏移
  double dd = 10;                         // 横向速度
  double ddd = 11;                        // 横向加速度
}

4.5.2 PlanningCommand消息

message PlanningCommand {
  // 命令头
  apollo.common.Header header = 1;
  
  // 命令类型
  enum CommandType {
    UNKNOWN = 0;
    FOLLOW = 1;                          // 跟随
    CHANGE_LANE = 2;                     // 变道
    PULL_OVER = 3;                       // 靠边停车
    STOP = 4;                            // 停车
    START = 5;                           // 起步
    SWITCH_MODE = 6;                      // 模式切换
  }
  
  CommandType command_type = 2;
  
  // 命令参数
  bool is_replan = 3;                    // 是否重新规划
  double planning_time = 4;              // 规划时间
  
  // 目标点信息
  apollo.common.PointENU parking_point = 5;  // 停车点
  apollo.common.PointENU destination = 6;    // 目的地
  
  // 路由信息
  routing::RoutingResponse routing_response = 7;
  
  // 跟随参数
  FollowCommand follow_command = 8;       // 跟随命令
  LaneChangeCommand lane_change_command = 9;  // 变道命令
}

五、性能优化特点

5.1 实时性能保障

  - 多线程架构: PlanningComponent使用多线程处理数据
  - 时间限制: 设置最大规划时间限制(1秒)
  - 增量规划: 基于上一帧结果进行增量更新
  - 缓存机制: 预计算和缓存常用数据

5.2 算法优化

  - 分层规划: 路径规划和速度规划分离
  - 采样优化: 智能采样减少计算量
  - GPU加速: 部分计算使用GPU并行加速
  - 启发式搜索: 使用启发式算法提高搜索效率

5.3 内存优化

  - 对象池: 重用对象减少内存分配
  - 预分配: 预先分配内存空间
  - 智能指针: 使用RAII管理内存
  - 数据压缩: 压缩中间结果数据

5.4 容错机制

  - 降级策略: 主算法失败时启用备用算法
  - 安全停止: 异常情况下的安全停车策略
  - 状态恢复: 从异常状态恢复到正常状态
  - 监控告警: 实时监控模块运行状态

六、总结

  Planning模块通过以下关键技术实现了智能的路径规划：

    1. 场景化规划: 根据驾驶场景选择合适的规划策略
    2. 多算法融合: 结合多种规划算法的优势
    3. 实时响应: 优化的算法确保实时性要求
    4. 安全优先: 多重安全保障机制
    5. 学习能力: 支持在线学习和离线训练

  该模块是Apollo系统的决策核心，为Control模块提供了安全、舒适、高效的行驶轨迹，是实现自动驾驶智能决策的关键组件。