Control Controllers 控制器函数级源码解析

本文聚焦 modules/control/controllers/ 目录，按函数级粒度拆解两个核心控制器的实现：LatController（基于 LQR 的横向控制器）和 LonController（基于 PID 的纵向控制器）。

1. 模块定位

控制器是 Apollo 控制栈的执行核心。ControlComponent 通过 ControlTaskAgent 串行调用控制器插件，每个控制器专注于一个控制维度：

LatController：计算方向盘转角（steering），跟踪规划轨迹的横向偏差
LonController：计算油门/刹车指令（throttle/brake），跟踪规划轨迹的速度/位置

ControlComponent → ControlTaskAgent → [LatController, LonController]
                                           │              │
                                     steering_angle   throttle/brake
                                           │              │
                                           └──── ControlCommand ────→ Canbus

2. 目录结构

modules/control/controllers/
├── lat_based_lqr_controller/
│   ├── lat_controller.h / .cc             # LQR 横向控制器
│   ├── lat_controller_test.cc             # 单元测试
│   ├── proto/                             # 配置 Protobuf
│   ├── conf/                              # 默认配置
│   └── BUILD                              # Bazel 构建规则
└── lon_based_pid_controller/
    ├── lon_controller.h / .cc             # PID 纵向控制器
    ├── lon_controller_test.cc
    ├── util/
    │   ├── check_pit.h / .cc              # 坑检测工具
    │   └── BUILD
    ├── proto/                             # 配置 Protobuf
    ├── conf/                              # 默认配置
    └── BUILD

3. LatController — LQR 横向控制器

3.1 类声明

cpp

class LatController : public ControlTask {
 public:
  LatController();
  virtual ~LatController();
  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;
};

通过 CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN 注册为 ControlTask 插件。

3.2 车辆动力学模型

LatController 基于自行车模型（Bicycle Model），状态向量为：

x = [lateral_error, lateral_error_rate, heading_error, heading_error_rate]

状态方程：

ẋ = A·x + B·δ

其中 δ 为前轮转角，矩阵 A 和 B 由以下车辆参数决定：

参数	成员变量	说明
`cf`	`cf_`	前轮侧偏刚度
`cr`	`cr_`	后轮侧偏刚度
`lf`	`lf_`	前轴到质心距离
`lr`	`lr_`	后轴到质心距离
`mass`	`mass_`	车辆质量
`iz`	`iz_`	绕 z 轴转动惯量
`wheelbase`	`wheelbase_`	轴距
`steer_ratio`	`steer_ratio_`	方向盘到前轮转角传动比

3.3 `Init` — 初始化

cpp

common::Status LatController::Init(std::shared_ptr<DependencyInjector> injector);

加载配置 LatBaseLqrControllerConf
LoadControlConf：读取车辆参数、控制周期 ts_、侧偏刚度 cf_/cr_、前瞻参数等
初始化矩阵维度：matrix_a_(4×4)、matrix_b_(4×1)、matrix_q_(4×4)、matrix_r_(1×1)
InitializeFilters：初始化数字滤波器和均值滤波器
LoadLatGainScheduler：加载增益调度表（速度→增益插值）
初始化 Lead-Lag 补偿器和 MRAC 自适应控制器（若启用）

3.4 `ComputeControlCommand` — 核心计算

cpp

common::Status LatController::ComputeControlCommand(
    const localization::LocalizationEstimate* localization,
    const canbus::Chassis* chassis,
    const planning::ADCTrajectory* trajectory,
    ControlCommand* cmd);

算法流程：

Step 1：轨迹分析器更新

cpp

trajectory_analyzer_.Update(trajectory);

从 ADCTrajectory 中提取轨迹点序列
构建 TrajectoryAnalyzer 用于后续最近点匹配和误差计算

Step 2：状态更新

cpp

UpdateState(&debug, chassis);

UpdateState：
- 获取自车位置 (x, y) 和航向角 theta
- ComputeLateralErrors：计算横向误差
  - 在轨迹上找到匹配点
  - 计算横向误差 lateral_error、航向误差 heading_error
  - 计算误差变化率 lateral_error_rate、heading_error_rate
- UpdateDrivingOrientation：倒车模式下翻转驾驶方向

Step 3：矩阵更新

cpp

UpdateMatrix();
UpdateMatrixCompound();

UpdateMatrix：

根据当前车速更新状态矩阵 A 和控制矩阵 B
矩阵元素是车速的函数（自行车模型线性化）
离散化：Ad = I + A·ts，Bd = B·ts

UpdateMatrixCompound：

若启用 preview 控制器，扩展状态矩阵以包含前瞻信息
preview_window_ 个前瞻周期的状态叠加

Step 4：LQR 求解

cpp

matrix_k_ = SolveLQR(matrix_adc_, matrix_bdc_, matrix_q_updated_, matrix_r_,
                      lqr_max_iteration_, lqr_eps_);

调用 LinearQuadraticRegulator 迭代求解 Riccati 方程
得到最优反馈增益矩阵 K
matrix_q_updated_：使用增益调度表根据速度调整状态权重

Step 5：前馈补偿

cpp

double steer_angle_feedforward = ComputeFeedForward(ref_curvature);

cpp

double LatController::ComputeFeedForward(double ref_curvature) const {
  double kv = lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_;
  double steer_angle_feedforward = wheelbase_ * ref_curvature
      + kv * v * v * ref_curvature - matrix_k_(0, 2) *
      (lr_ * ref_curvature - lf_ * mass_ * v * v * ref_curvature / 2 / cr_ / wheelbase_);
  return steer_angle_feedforward * steer_ratio_ * 180 / M_PI / steer_single_direction_max_degree_;
}

基于稳态曲率的前馈补偿
公式考虑了车辆参数 kv（不足转向梯度）
将弧度转换为方向盘百分比

Step 6：方向盘指令计算

cpp

double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0);
double steer_angle = steer_angle_feedback + steer_angle_feedforward;

反馈项：-K·x（LQR 最优控制律）
前馈项：稳态曲率补偿
总转角 = 反馈 + 前馈

Step 7：后处理

Lead-Lag 补偿器（若启用）：改善相位裕度
MRAC 自适应控制器（若启用）：在线调整增益
数字滤波器：平滑输出
限幅：[-steer_single_direction_max_degree_, +steer_single_direction_max_degree_]
横向加速度限制：max_lat_acc_ 约束

Step 8：输出

cpp

cmd->set_steering_target(steer_angle);
cmd->set_steering_rate(steer_rate);

3.5 `ComputeLateralErrors` — 横向误差计算

cpp

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

在轨迹上找到与自车最近的匹配点
计算横向误差 l（自车到轨迹的法向距离）
计算航向误差 Δθ = θ_vehicle - θ_trajectory
计算误差变化率（微分）
填充 SimpleLateralDebug 用于调试输出

3.6 Look-Ahead/Look-Back 控制

cpp

bool enable_look_ahead_back_control_ = false;
double lookahead_station_low_speed_ = 0.0;
double lookback_station_low_speed_ = 0.0;
double lookahead_station_high_speed_ = 0.0;
double lookback_station_high_speed_ = 0.0;

前瞻控制：不使用当前位置的误差，而是使用前方某点的预测误差
低速时使用 lookahead_station_low_speed_，高速时使用 lookahead_station_high_speed_
倒车时使用 lookback_station_ 参数
改善低速和高速场景的跟踪性能

3.7 增益调度

cpp

std::unique_ptr<Interpolation1D> lat_err_interpolation_;
std::unique_ptr<Interpolation1D> heading_err_interpolation_;

根据车速插值调整 matrix_q_ 中横向误差和航向误差的权重
低速时更重视横向误差，高速时更重视航向误差
使用 Interpolation1D 做一维插值

4. LonController — PID 纵向控制器

4.1 类声明

cpp

class LonController : public ControlTask {
 public:
  LonController();
  virtual ~LonController();
  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;
};

4.2 双环 PID 架构

LonController 采用级联 PID 控制结构：

位置误差 ──> Station PID ──> 速度参考 ──> Speed PID ──> 加速度指令
                                                         │
                                                   标定表插值
                                                         │
                                                    油门/刹车

外环（Station PID）：位置误差 → 速度参考
内环（Speed PID）：速度误差 → 加速度指令
标定表：加速度 → 油门/刹车值

4.3 核心成员

cpp

PIDController speed_pid_controller_;      // 速度 PID
PIDController station_pid_controller_;    // 位置 PID
LeadlagController speed_leadlag_controller_;    // 速度 Lead-Lag
LeadlagController station_leadlag_controller_;  // 位置 Lead-Lag
std::unique_ptr<Interpolation2D> control_interpolation_;  // 2D 标定表

4.4 `Init` — 初始化

加载 LonBasedPidControllerConf 配置
初始化 speed_pid_controller_ 和 station_pid_controller_ 的 PID 参数
初始化 Lead-Lag 控制器参数
InitControlCalibrationTable：加载 (速度, 加速度) → 油门/刹车 2D 标定表
SetDigitalFilterPitchAngle：初始化坡度角滤波器

4.5 `ComputeControlCommand` — 核心计算

Step 1：轨迹分析

cpp

ComputeLongitudinalErrors(trajectory_analyzer, preview_time, ts, &debug);

计算位置误差 station_error = s_ref - s_actual
计算速度误差 speed_error = v_ref - v_actual
支持前瞻（preview）补偿

Step 2：外环 — 位置 PID

cpp

double station_pid_output = station_pid_controller_.Control(station_error, ts);

输入：位置误差
输出：速度参考增量
reference_spd_cmd_ = reference_spd_ + station_pid_output

Step 3：内环 — 速度 PID

cpp

double speed_pid_output = speed_pid_controller_.Control(speed_error, ts);

输入：速度误差
输出：加速度闭环修正量

Step 4：加速度合成

cpp

double acceleration_cmd = speed_pid_output + preview_acceleration_reference;

总加速度 = PID 闭环修正 + 前瞻加速度前馈

Step 5：Lead-Lag 补偿

cpp

if (enable_speed_leadlag_) {
  acceleration_cmd = speed_leadlag_controller_.Control(acceleration_cmd, ts);
}

Step 6：标定表插值

cpp

double calibration_value = control_interpolation_->Interpolate(
    std::make_pair(reference_spd_, acceleration_cmd));

从 2D 标定表中查表：(当前速度, 目标加速度) → 油门/刹车值
正值 → 油门，负值 → 刹车

Step 7：停车逻辑

IsStopByDestination：到达目的地停车
IsPedestrianStopLongTerm：行人导致的长时间停车
IsFullStopLongTerm：完全停车状态
SetParkingBrake：长时间停车时启用电子驻车（EPB）

Step 8：输出

cpp

cmd->set_throttle(calibration_value > 0 ? calibration_value : 0.0);
cmd->set_brake(calibration_value < 0 ? -calibration_value : 0.0);

4.6 `ComputeLongitudinalErrors` — 纵向误差计算

cpp

void LonController::ComputeLongitudinalErrors(
    const TrajectoryAnalyzer* trajectory, double preview_time, double ts,
    SimpleLongitudinalDebug* debug);

在轨迹上找到匹配点
计算纵向位置误差 station_error
计算速度误差 speed_error
若启用 preview：在 preview_time 处计算前瞻误差
填充 SimpleLongitudinalDebug

4.7 CheckPit 坑检测

cpp

class CheckPit {
 public:
  static bool CheckInPit(SimpleLongitudinalDebug* debug,
                         const LonBasedPidControllerConf* conf,
                         double speed, bool replan);
};

检测车辆是否陷入"坑"状态（长时间低速或停车后无法正常起步）
用于触发特殊恢复逻辑

4.8 停车状态管理

cpp

bool IsStopByDestination(SimpleLongitudinalDebug* debug);     // 目的地停车
bool IsPedestrianStopLongTerm(SimpleLongitudinalDebug* debug); // 行人长时间停车
bool IsFullStopLongTerm(SimpleLongitudinalDebug* debug);       // 完全停车
void SetParkingBrake(const LonBasedPidControllerConf* conf,
                     control::ControlCommand* control_command); // 电子驻车

目的地停车：检测到规划轨迹的停车原因码为 DESTINATION
行人停车：行人导致停车超过阈值时间后触发 EPB
完全停车：速度为 0 且持续时间超过阈值
电子驻车：长时间停车时自动启用 EPB，起步时释放

5. 控制器协作

5.1 串行执行

ControlTaskAgent 按 pipeline.pb.txt 配置的顺序串行调用控制器：

protobuf

# conf/pipeline.pb.txt 示例
task {
  name: "lat_controller"
  type: "LatController"
}
task {
  name: "lon_controller"
  type: "LonController"
}

5.2 共享输入

两个控制器共享相同的输入：

LocalizationEstimate：自车位置和姿态
Chassis：车速、加速度、方向盘角度
ADCTrajectory：规划轨迹

5.3 输出合并

LatController 设置 cmd->steering_target
LonController 设置 cmd->throttle 和 cmd->brake
最终 ControlCommand 包含完整的控制指令

6. 关键设计决策

6.1 LQR vs PID

特性	LatController (LQR)	LonController (PID)
控制维度	横向（方向盘）	纵向（油门/刹车）
算法	线性二次调节器	级联 PID + 标定表
模型依赖	自行车模型（需精确车辆参数）	经验标定表
前馈	曲率前馈	加速度前馈（preview）
自适应	增益调度 + MRAC	Lead-Lag 补偿

6.2 Preview 控制

横向：preview_window_ 个控制周期的前瞻状态叠加
纵向：preview_time 的前瞻加速度前馈
改善高速跟踪性能，减少相位滞后

6.3 多层滤波

数字滤波器（DigitalFilter）：平滑传感器噪声
均值滤波器（MeanFilter）：平滑横向/航向误差
坡度角滤波器：平滑坡度估计

6.4 安全保护

方向盘转角限幅：steer_single_direction_max_degree_
横向加速度限制：max_lat_acc_
最小速度保护：minimum_speed_protection_（防止除零）
电子驻车：长时间停车自动启用

贡献者

Steven Moder

页面历史

最后编辑于 5 天前

查看完整历史

Control Controllers 控制器函数级源码解析 ​

1. 模块定位 ​

2. 目录结构 ​

3. LatController — LQR 横向控制器 ​

3.1 类声明 ​

3.2 车辆动力学模型 ​

3.3 Init — 初始化 ​

3.4 ComputeControlCommand — 核心计算 ​

Step 1：轨迹分析器更新 ​

Step 2：状态更新 ​

Step 3：矩阵更新 ​

Step 4：LQR 求解 ​

Step 5：前馈补偿 ​

Step 6：方向盘指令计算 ​

Step 7：后处理 ​

Step 8：输出 ​

3.5 ComputeLateralErrors — 横向误差计算 ​

3.6 Look-Ahead/Look-Back 控制 ​

3.7 增益调度 ​

4. LonController — PID 纵向控制器 ​

4.1 类声明 ​

4.2 双环 PID 架构 ​

4.3 核心成员 ​

4.4 Init — 初始化 ​

4.5 ComputeControlCommand — 核心计算 ​

4.6 ComputeLongitudinalErrors — 纵向误差计算 ​

4.7 CheckPit 坑检测 ​

4.8 停车状态管理 ​

5. 控制器协作 ​

5.1 串行执行 ​

5.2 共享输入 ​

5.3 输出合并 ​

6. 关键设计决策 ​

6.1 LQR vs PID ​

6.2 Preview 控制 ​

6.3 多层滤波 ​

6.4 安全保护 ​

贡献者