百度Apollo 中纵向控制源码解析

严正声明：本文系作者yue597215286原创，未经允许，严禁转载！

    上篇博文中，我们主要介绍了control模块的入口和车辆横向控制的介绍，有兴趣的可以参考链接Apollo control 模块源码解析

    此篇博文，我们主要介绍下Apollo内部的纵向控制代码和原理解析：

    话不多少，首先我们需要看的源代码为modules/control/controller/lon_controller.h和lon_controller.cc 在Apollo control 模块源码解析中我们得知，controller的先后顺序是 Init()函数和ComputeControlCommand()函数。即在modules/control/control.cc中Control::Init()

  // set controller 
  if (!controller_agent_.Init(&control_conf_).ok()) {
    std::string error_msg = "Control init controller failed! Stopping...";
    buffer.ERROR(error_msg);
    return Status(ErrorCode::CONTROL_INIT_ERROR, error_msg);
  }

和

Status Control::ProduceControlCommand(ControlCommand *control_command) {
...
Status status_compute = controller_agent_.ComputeControlCommand(
        &localization_, &chassis_, &trajectory_, control_command);
...
}

    让我们从lon_controller.cc 的Init()函数一窥究竟：

Status LonController::Init(const ControlConf *control_conf) {
  control_conf_ = control_conf;
  if (control_conf_ == nullptr) {
    controller_initialized_ = false;
    AERROR << "get_longitudinal_param() nullptr";
    return Status(ErrorCode::CONTROL_INIT_ERROR,
                  "Failed to load LonController conf");
  }
  const LonControllerConf &lon_controller_conf =
      control_conf_->lon_controller_conf();

  station_pid_controller_.Init(lon_controller_conf.station_pid_conf());
  speed_pid_controller_.Init(lon_controller_conf.low_speed_pid_conf());

  SetDigitalFilterPitchAngle(lon_controller_conf);

  LoadControlCalibrationTable(lon_controller_conf);
  controller_initialized_ = true;

  return Status::OK();
}

在这里面，主要对conf文件的导入和两个PID控制器(station_pid_controller_和speed_pid_controller_)的初始化,还有对根据conf文件数字滤波器和标定表进行设置。PID的控制，是目前Apollo作用于纵向速度控制的主要方式，Apollo的PID控制器逻辑相对简单些，具体的代码存放在modules/control/common/pid_controller.cc中，可以查看相关的代码，下面我用一张简图将内部PID的逻辑描述下：