Apollo Control 控制模块接口调用流程图与源码分析

---

  团队博客: 汽车电子社区

---

一、模块概述

  Control模块是Apollo自动驾驶系统的"神经-肌肉接口"，负责将Planning模块输出的轨迹转换为具体的车辆控制指令，实现精确的轨迹跟踪。该模块采用多控制算法融合的策略，确保车辆安全、舒适、高效地执行规划轨迹。

二、模块架构

2.1 目录结构

modules/control/
├── control_component/          # 控制主组件
├── controllers/                # 控制器实现
│   ├── lat_based_lqr_controller/      # 基于LQR的横向控制
│   ├── lon_based_pid_controller/       # 基于PID的纵向控制
│   ├── mpc_controller/                 # 模型预测控制
│   └── demo_control_task/              # 演示控制任务
├── common/                     # 控制通用组件
├── proto/                      # 消息定义
└── tools/                      # 工具

2.2 核心组件

  1. ControlComponent - 控制主组件
  2. ControllerTaskAgent - 控制任务代理
  3. LatLonControllerSubmodule - 横纵向控制子模块
  4. MPCControllerSubmodule - MPC控制子模块
  5. LonBasedPIDController - 纵向PID控制器
  6. LatBasedLQRController - 横向LQR控制器

三、接口调用流程图

3.1 整体控制流程图

3.2 LQR横向控制流程图

3.3 PID纵向控制流程图

3.4 MPC控制流程图

四、关键源码分析

4.1 主组件源码分析

4.1.1 ControlComponent 核心源码分析

  类定义位置: modules/control/control_component/control_component.h

  核心功能:
    - 继承自 apollo::cyber::TimerComponent，是定时触发的组件
    - 接收定位、底盘、规划轨迹等数据
    - 计算油门、刹车、转向控制指令
    - 执行安全检查和状态监控

  关键成员变量:

class ControlComponent final : public apollo::cyber::TimerComponent {
 private:
  // 数据缓存
  localization::LocalizationEstimate latest_localization_;     // 最新定位数据
  canbus::Chassis latest_chassis_;                           // 最新底盘数据  
  planning::ADCTrajectory latest_trajectory_;                 // 最新轨迹数据
  external_command::CommandStatus planning_command_status_;   // 规划命令状态
  PadMessage pad_msg_;                                       // 驾驶指令
  common::Header latest_replan_trajectory_header_;            // 重规划轨迹头

  // 控制核心
  ControlTaskAgent control_task_agent_;                      // 控制任务代理
  ControlPipeline control_pipeline_;                        // 控制流水线

  // 状态管理
  bool estop_ = false;                                     // 紧急停止标志
  std::string estop_reason_;                                // 紧急停止原因
  bool pad_received_ = false;                                // 是否收到驾驶指令

  // 错误计数
  unsigned int status_lost_ = 0;                            // 状态丢失计数
  unsigned int status_sanity_check_failed_ = 0;             // 完整性检查失败计数
  unsigned int total_status_lost_ = 0;                      // 总状态丢失计数
  unsigned int total_status_sanity_check_failed_ = 0;       // 总完整性检查失败计数

  // 线程和同步
  std::mutex mutex_;                                       // 互斥锁
  LocalView local_view_;                                     // 本地数据视图

  // 依赖注入
  std::shared_ptr<DependencyInjector> injector_;            // 依赖注入器

  // 历史控制量
  double previous_steering_command_ = 0.0;                  // 上次转向指令

  // 自动驾驶状态
  bool is_auto_ = false;                                   // 自动驾驶状态
  bool from_else_to_auto_ = false;                          // 切换到自动驾驶

  // 订阅者
  std::shared_ptr<cyber::Reader<apollo::canbus::Chassis>> chassis_reader_;
  std::shared_ptr<cyber::Reader<PadMessage>> pad_msg_reader_;
  std::shared_ptr<cyber::Reader<apollo::localization::LocalizationEstimate>> localization_reader_;
  std::shared_ptr<cyber::Reader<apollo::planning::ADCTrajectory>> trajectory_reader_;
  std::shared_ptr<cyber::Reader<apollo::external_command::CommandStatus>> planning_command_status_reader_;

  // 发布者
  std::shared_ptr<cyber::Writer<ControlCommand>> control_cmd_writer_;
  std::shared_ptr<cyber::Writer<LocalView>> local_view_writer_;
  std::shared_ptr<cyber::Writer<ControlInteractiveMsg>> control_interactive_writer_;

  // 监控
  common::monitor::MonitorLogBuffer monitor_logger_buffer_;
};

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

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

  // 2. 加载控制流水线配置
  ACHECK(cyber::common::GetProtoFromFile(FLAGS_pipeline_file, &control_pipeline_));

  // 3. 初始化控制任务代理（非子模块模式）
  if (!FLAGS_is_control_ut_test_mode) {
    if (!FLAGS_use_control_submodules &&
        !control_task_agent_.Init(injector_, control_pipeline_).ok()) {
      monitor_logger_buffer_.ERROR("Control init controller failed!");
      return false;
    }
  }

  // 4. 创建订阅者
  chassis_reader_ = node_->CreateReader<Chassis>(chassis_reader_config);
  trajectory_reader_ = node_->CreateReader<ADCTrajectory>(planning_reader_config);
  planning_command_status_reader_ = node_->CreateReader<external_command::CommandStatus>(
      planning_command_status_reader_config);
  localization_reader_ = node_->CreateReader<LocalizationEstimate>(localization_reader_config);
  pad_msg_reader_ = node_->CreateReader<PadMessage>(pad_msg_reader_config);

  // 5. 创建发布者
  if (!FLAGS_use_control_submodules) {
    control_cmd_writer_ = node_->CreateWriter<ControlCommand>(FLAGS_control_command_topic);
  } else {
    local_view_writer_ = node_->CreateWriter<LocalView>(FLAGS_control_local_view_topic);
  }
  control_interactive_writer_ = node_->CreateWriter<ControlInteractiveMsg>(FLAGS_control_interative_topic);

  // 6. 初始化车辆状态
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  pad_msg_.set_action((enum DrivingAction)FLAGS_action);

  return true;
}

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

bool ControlComponent::Proc() {
  // 1. 检查输入数据完整性
  if (!CheckInput(&local_view_).ok()) {
    AERROR << "Control input data check failed!";
    return false;
  }

  // 2. 检查时间戳同步
  if (!CheckTimestamp(local_view_).ok()) {
    AERROR << "Control timestamp check failed!";
    return false;
  }

  // 3. 检查驾驶指令
  if (!CheckPad().ok()) {
    AERROR << "Control pad check failed!";
    return false;
  }

  // 4. 生成控制指令
  ControlCommand control_command;
  auto status = ProduceControlCommand(&control_command);
  if (!status.ok()) {
    AERROR << "Produce control command failed! " << status.error_message();
    ResetAndProduceZeroControlCommand(&control_command);
  }

  // 5. 获取车辆俯仰角
  GetVehiclePitchAngle(&control_command);

  // 6. 发布控制指令
  if (!FLAGS_use_control_submodules) {
    control_cmd_writer_->Write(std::make_shared<ControlCommand>(control_command));
  } else {
    // 子模块模式，发布本地视图
    local_view_writer_->Write(std::make_shared<LocalView>(local_view_));
  }

  // 7. 发布交互消息
  PublishControlInteractiveMsg();

  return true;
}

  数据回调处理:

// 底盘数据回调
void ControlComponent::OnChassis(const std::shared_ptr<Chassis> &chassis) {
  std::lock_guard<std::mutex> lock(mutex_);
  latest_chassis_.CopyFrom(*chassis);
}

// 轨迹数据回调
void ControlComponent::OnPlanning(
    const std::shared_ptr<ADCTrajectory> &trajectory) {
  std::lock_guard<std::mutex> lock(mutex_);
  latest_trajectory_.CopyFrom(*trajectory);
}

// 定位数据回调
void ControlComponent::OnLocalization(
    const std::shared_ptr<LocalizationEstimate> &localization) {
  std::lock_guard<std::mutex> lock(mutex_);
  latest_localization_.CopyFrom(*localization);
}

// 驾驶指令回调
void ControlComponent::OnPad(const std::shared_ptr<PadMessage> &pad) {
  std::lock_guard<std::mutex> lock(mutex_);
  pad_msg_.CopyFrom(*pad);
  CheckAutoMode(&latest_chassis_);
}

  关键技术特点:
    1. 定时触发: 使用TimerComponent确保固定频率执行
    2. 数据安全: 互斥锁保护共享数据访问
    3. 模块化: 支持子模块模式和传统模式
    4. 安全检查: 多层数据完整性和时间戳检查
    5. 状态监控: 实时监控控制状态和错误统计
  6. 故障恢复: 异常时自动切换到零控制指令

4.1.2 ControlTaskAgent 控制任务代理

  位置: modules/control/control_component/controller_task_base/control_task_agent.h

  核心功能:
    - 管理控制配置文件中声明的所有控制器
    - 根据控制流水线配置协调控制器执行
    - 实现控制任务的调度和管理

  核心接口:

class ControlTaskAgent {
 public:
  // 初始化控制任务代理
  common::Status Init(std::shared_ptr<DependencyInjector> injector,
                      const ControlPipeline &control_pipeline);

  // 计算控制指令
  common::Status ComputeControlCommand(
      const localization::LocalizationEstimate *localization,
      const canbus::Chassis *chassis, 
      const planning::ADCTrajectory *trajectory,
      control::ControlCommand *cmd);

  // 重置控制任务代理
  common::Status Reset();

 private:
  std::vector<std::shared_ptr<ControlTask>> controller_list_;  // 控制器列表
  std::shared_ptr<DependencyInjector> injector_ = nullptr;    // 依赖注入器
};

  控制流水线配置:

control_pipeline: {
  control_tasks: [
    {
      name: "LatLonControllerSubmodule"
      type: "ControlTask"
      config_path: "conf/control"
      config_file: "lat_lon_controller_conf.pb.txt"
    }
  ]
}

4.2 控制器算法分析

4.2.1 横向LQR控制器

  位置: modules/control/controllers/lat_based_lqr_controller/lat_controller.h

  核心功能:
    - 继承自 ControlTask，是基于LQR的横向控制器
    - 计算转向目标以跟踪参考轨迹
    - 基于车辆动力学模型和线性二次调节器

  核心接口:

class LatController : public ControlTask {
 public:
  // 初始化控制器
  common::Status Init(std::shared_ptr<DependencyInjector> injector) override;

  // 计算控制指令
  common::Status ComputeControlCommand(
      const localization::LocalizationEstimate *localization,
      const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
      ControlCommand *cmd) override;

  // 重置控制器
  common::Status Reset() override;

  // 停止控制器
  void Stop() override;

  // 获取控制器名称
  std::string Name() const override;

 protected:
  // 状态更新
  void UpdateState(SimpleLateralDebug *debug, const canbus::Chassis *chassis);
  void UpdateDrivingOrientation();           // 更新驾驶方向
  void UpdateMatrix();                       // 更新控制矩阵
  void UpdateMatrixCompound();               // 更新复合矩阵

  // 前馈控制
  double ComputeFeedForward(double ref_curvature) const;

  // 误差计算
  void ComputeLateralErrors(const double x, const double y, const double theta,
                          const double linear_v, const double angular_v,
                          const double linear_a,
                          const TrajectoryAnalyzer &trajectory_analyzer,
                          SimpleLateralDebug *debug,
                          const canbus::Chassis *chassis);

  // 配置加载和初始化
  bool LoadControlConf();
  void InitializeFilters();
  void LoadLatGainScheduler();

 private:
  // 配置和参数
  LatBaseLqrControllerConf lat_based_lqr_controller_conf_;  // LQR控制器配置
  common::VehicleParam vehicle_param_;                     // 车辆参数

  // 轨迹分析器
  TrajectoryAnalyzer trajectory_analyzer_;                 // 轨迹分析器

  // 车辆物理参数
  double ts_ = 0.0;              // 控制时间间隔
  double cf_ = 0.0;              // 前轮侧偏刚度
  double cr_ = 0.0;              // 后轮侧偏刚度
  double wheelbase_ = 0.0;        // 轴距
  double mass_ = 0.0;             // 车辆质量
  double lf_ = 0.0;              // 前轮到质心距离
  double lr_ = 0.0;              // 后轮到质心距离
  double iz_ = 0.0;              // 转动惯量
};

  LQR控制算法原理:
    1. 状态空间模型: 建立车辆横向动力学状态方程
    2. 二次型代价函数: 定义横向误差和控制输入的代价
    3. Riccati方程求解: 计算最优反馈增益矩阵
    4. 状态反馈: 根据状态误差计算转向控制量

  状态向量定义:

// 状态向量 x = [lateral_error, lateral_error_rate, heading_error, heading_error_rate]
// 控制向量 u = steering_angle

4.2.2 纵向PID控制器

  位置: modules/control/controllers/lon_based_pid_controller/lon_controller.h

  核心功能:
    - 继承自 ControlTask，是基于PID的纵向控制器
    - 计算刹车和油门值以实现速度跟踪
    - 使用PID控制算法和前馈控制

  核心接口:

class LonController : public ControlTask {
 public:
  // 初始化控制器
  common::Status Init(std::shared_ptr<DependencyInjector> injector) override;

  // 计算控制指令
  common::Status ComputeControlCommand(
      const localization::LocalizationEstimate *localization,
      const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
      control::ControlCommand *cmd) override;

  // 重置控制器
  common::Status Reset() override;

  // 停止控制器
  void Stop() override;

  // 获取控制器名称
  std::string Name() const override;

 protected:
  // 计算纵向误差
  void ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory,
                                const double preview_time, const double ts,
                                SimpleLongitudinalDebug *debug);

  // 获取路径剩余
  void GetPathRemain(SimpleLongitudinalDebug *debug);

  // 停止条件检查
  bool IsStopByDestination(SimpleLongitudinalDebug *debug);
  bool IsPedestrianStopLongTerm(SimpleLongitudinalDebug *debug);
  bool IsFullStopLongTerm(SimpleLongitudinalDebug *debug);

  // 设置停车制动
  void SetParkingBrake(const LonBasedPidControllerConf *conf,
                       control::ControlCommand *control_command);

 private:
  // 数据引用
  const localization::LocalizationEstimate *localization_ = nullptr;
  const canbus::Chassis *chassis_ = nullptr;
  const planning::ADCTrajectory *trajectory_message_ = nullptr;

  // 控制器和分析器
  std::unique_ptr<Interpolation2D> control_interpolation_;     // 控制插值器
  std::unique_ptr<TrajectoryAnalyzer> trajectory_analyzer_;    // 轨迹分析器

  // PID控制器
  PIDController speed_pid_controller_;        // 速度PID控制器
  PIDController station_pid_controller_;      // 位置PID控制器

  // 前馈控制器
  LeadlagController speed_leadlag_controller_;     // 速度前馈控制器
  LeadlagController station_leadlag_controller_;   // 位置前馈控制器

  // 数字滤波器
  common::DigitalFilter digital_filter_pitch_angle_;  // 俯仰角滤波器

  // 控制状态
  double previous_acceleration_ = 0.0;           // 上次加速度
  double reference_spd_ = 0.0;                   // 参考速度
  double reference_spd_cmd_ = 0.0;               // 参考速度指令
  double previous_acceleration_reference_ = 0.0;   // 上次参考加速度

  // 配置和参数
  LonBasedPidControllerConf lon_based_pidcontroller_conf_;  // PID控制器配置
  calibration_table calibration_table_;                     // 校准表
};

  PID控制算法原理:
    1. 速度误差计算: error = target_speed - current_speed
    2. PID三项计算:
       - P项: P_out = Kp * error
       - I项: I_out = Ki * integral(error)
       - D项: D_out = Kd * derivative(error)
    3. 控制量合成: control_output = P_out + I_out + D_out
    4. 前馈补偿: 基于参考轨迹的前馈控制
    5. 限幅处理: 限制加速度和控制量的范围

4.2.3 MPC模型预测控制器

  位置: modules/control/controllers/mpc_controller/mpc_controller.h

  核心功能:
    - 继承自 ControlTask，是横纵向一体化的MPC控制器
    - 同时计算转向和油门/刹车控制指令
    - 基于模型预测控制原理实现最优控制

  核心特点:
    - 横纵向一体化: 同时处理横向和纵向控制问题
    - 预测优化: 基于车辆模型预测未来状态
    - 约束处理: 能够处理控制约束和状态约束
    - 多目标优化: 同时优化轨迹跟踪精度、乘坐舒适性和控制能耗

4.3 消息接口定义

4.3.1 ControlCommand消息

  位置: modules/common_msgs/control_msgs/control_cmd.proto

message ControlCommand {
  // 消息头
  apollo.common.Header header = 1;
  
  // 主要控制量
  double steering = 2;              // 转向角 [-100, 100]
  double throttle = 3;             // 油门 [0, 100]  
  double brake = 4;                 // 刹车 [0, 100]
  double parking_brake = 5;         // 手刹 [0, 1]
  
  // 高级控制
  double steering_rate = 6;         // 转向角速度
  double acceleration = 7;           // 目标加速度
  
  // 驾驶模式
  GearPosition gear_location = 8;    // 档位位置
  DrivingMode driving_mode = 9;     // 驾驶模式
  
  // 速度控制
  double speed = 10;                // 目标速度
  
  // 高精度控制
  double steering_target = 11;      // 目标转向角
  double throttle_target = 12;      // 目标油门
  double brake_target = 13;          // 目标刹车
  
  // 校准和调试
  bool is_calibration_mode = 14;     // 校准模式
  apollo.common.Debug debug = 15;    // 调试信息
  
  // 信号控制
  apollo.common.VehicleSignal signal = 16;  // 车辆信号
  
  // 其他控制
  bool is_pad_msg = 17;             // 是否为驾驶指令
  bool is_estop = 18;               // 紧急停止
  bool has_driver = 19;              // 是否有驾驶员
}

message VehicleSignal {
  // 转向信号
  TurnSignal turn_signal = 1;        // 转向灯信号
  
  // 照明信号
  bool high_beam = 2;               // 远光灯
  bool low_beam = 3;                // 近光灯
  bool left_light = 4;              // 左示廓灯
  bool right_light = 5;             // 右示廓灯
  
  // 警告信号
  bool horn = 6;                    // 喇叭
  bool emergency_light = 7;          // 双闪
}

enum TurnSignal {
  TURN_NONE = 0;                    // 无转向
  TURN_LEFT = 1;                   // 左转
  TURN_RIGHT = 2;                   // 右转
}

enum GearPosition {
  GEAR_NEUTRAL = 0;                 // 空档
  GEAR_DRIVE = 1;                   // 前进档
  GEAR_REVERSE = 2;                 // 倒档
  GEAR_PARKING = 3;                 // 停车档
  GEAR_LOW = 4;                     // 低速档
  GEAR_INVALID = 5;                 // 无效档位
}

enum DrivingMode {
  COMPLETE_MANUAL = 0;              // 完全手动
  COMPLETE_AUTO_DRIVE = 1;           // 完全自动驾驶
  AUTO_STEER_ONLY = 2;             // 仅自动转向
  AUTO_SPEED_ONLY = 3;              // 仅自动速度
}

// 紧急停止消息
message Estop {
  bool is_estop = 1;                // 是否紧急停止
  string reason = 2;                 // 停止原因
}

4.4 配置文件分析

4.4.1 控制主配置

  Control配置: modules/control/conf/control_conf.pb.txt

control_conf: {
  # 控制任务配置
  control_task: "LatLonControllerSubmodule"
  
  # 车辆参数
  calibration_table: {
    throttle: [0.0, 0.2, 0.4, 0.6, 0.8, 1.0]
    brake: [0.0, 0.2, 0.4, 0.6, 0.8, 1.0]
    acc: [0.0, 1.0, 2.0, 3.0, 4.0, 5.0]
  }
  
  # 控制器参数
  lon_controller_conf: {
    pid_conf: {
      integrator_enable: true
      integrator_saturation_level: 3.0
      kp: 0.8
      ki: 0.3
      kd: 0.2
    }
  }
  
  lat_controller_conf: {
    mpc_controller_conf: {
      matrix_q: [10.0, 10.0, 1.0, 1.0]      # 状态权重矩阵
      matrix_r: [1.0, 1.0]                   # 控制权重矩阵
      matrix_k: [1.0, 1.0]                   # 终端权重矩阵
      cutoff_freq: 10.0                        # 截止频率
      admissible_enable: true                   # 启用容许控制
      max_iteration: 100                        # 最大迭代次数
      max_acceleration: 2.0                     # 最大加速度
      min_acceleration: -3.0                    # 最小加速度
    }
  }
  
  # 安全配置
  safety_conf: {
    max_steer_angle: 45.0                       # 最大转向角
    max_steer_angle_rate: 90.0                  # 最大转向角速度
    max_acceleration: 2.0                        # 最大加速度
    max_deceleration: -3.0                       # 最大减速度
    steer_angle_rate_gain: 2.0                   # 转向角速度增益
  }
}

4.4.2 DAG配置文件

  启动配置: modules/control/control_component/dag/control.dag

# 定义组件
timer_component {
  name: "ControlComponent"
  config_path: "conf/control"
  config_name: "control_conf.pb.txt"
}

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

# 定义定时器
timer {
  name: "ControlTimer"
  interval: 100  # 100ms = 10Hz
  components: ["ControlComponent"]
}

# 定义数据服务
data_service {
  readers: [
    {
      channel: "/apollo/planning"
      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"
      }
    }
  ]
  writers: [
    {
      channel: "/apollo/control"
      qos_profile: {
        reliability: "RELIABILITY_RELIABLE"
        durability: "DURABILITY_VOLATILE"
      }
    }
  ]
}

五、性能优化特点

5.1 实时性能保障

  - 高频执行: 控制循环频率10Hz确保实时响应
  - 增量计算: 基于上次控制结果进行增量更新
  - 预测优化: MPC控制器使用滑动窗口优化
  - 并行处理: 横纵向控制可并行计算

5.2 算法优化

  - 数值稳定: 使用数值稳定的LQR和PID算法
  - 滤波技术: 多级滤波减少噪声影响
  - 自适应控制: 根据车速自适应调整控制参数
  - 约束处理: MPC处理控制约束和状态约束

5.3 安全保障

  - 多层检查: 数据完整性、时间戳、驾驶模式检查
  - 故障检测: 实时监控传感器和控制状态
  - 紧急停止: 多种紧急停止触发机制
  - 限幅保护: 控制指令限幅防止过激动作

5.4 容错机制

  - 降级控制: 主控制器失败时切换备用控制
  - 零控制保持: 异常时保持零控制状态
  - 状态恢复: 从异常状态平滑恢复
  - 人机接管: 支持人工接管和权限切换

六、总结

  Control模块通过以下关键技术实现了精确的车辆控制：

    1. 多控制器融合: LQR、PID、MPC等多种控制算法
    2. 横纵向解耦/集成: 既支持分离控制也支持一体化控制
    3. 模型预测: MPC实现基于模型的预测控制
    4. 安全优先: 多层安全保障和故障处理机制
  5. 实时响应: 优化的算法确保实时性要求

  该模块是Apollo系统的执行机构，将规划的虚拟轨迹转换为车辆的实际物理动作，是实现自动驾驶精确控制的关键组件。通过与底盘、执行器的紧密配合，确保车辆能够安全、舒适、高效地执行规划指令。