1. PID算法介绍

PID算法在先前自动驾驶之PID原理简述(简单易懂)中已经有所讲述，在此部分对一些关键性的内容进行介绍，其余部分就不展开介绍了(可以参考胡寿松的《自动控制原理》)。

1.1 时间连续与时间离散

时间离散的PID公式：
$$u(k)=K_{p}e(k)+K_I\sum _{i=0}e(i)+K_D[e(k)-e(k-1)]$$
时间连续的PID公式：
$$u(k)=K_p[e(t)+\frac{1}{T_i}{\int }_0^{t}e(t)dt+Td\frac{de(t)}{dt}]$$

1.2 位置式与增量式

位置式：对当前位置与目标状态的偏差进行控制$$u(k)=K_{p}e(k)+K_I\sum _{i=0}e(i)+K_D[e(k)-e(k-1)]$$
常见应用： 直立小车涉及角度、速度、转向多个PID控制；通过判断液位高度控制阀门开度
增量式: 以当前的控制量与前一个时刻的控制量作为偏差进行控制。$$\begin{array}{c}\Delta u(k)=u(k)-u(k-1)\\ =K_p[e(k)-e(k-1)]+K_{I}e(k)+K_D[e(k)-2e(k-1)+e(k-2)]\end{array}$$

增量式PID控制的主要优点为：
①控制增量$\Delta u(k)$的确定仅与最近3次的采样值有关，容易通过加权处理获得比较好的控制效果；
②计算机每次只输出控制增量，即对应执行机构位置的变化量，故机器发生故障时影响范围小、不会严重影响生产过程；
③对于一些工业控制上的机器，手动到自动切换时冲击小。当控制从手动向自动切换时，可以作到无扰动切换。

1.3 PID算法扩展

串级PID控制、模糊pid、自适应pid…

2. PID调试方法

PID参数快速整定
参数整定找最佳，从小到大顺序查，
先是比例后积分，最后再把微分加，
曲线振荡很频繁，比例度盘要放大，
曲线漂浮绕大湾，比例度盘往小扳，
曲线偏离回复慢，积分时间往下降，
曲线波动周期长，积分时间再加长，
曲线振荡频率快，先把微分降下来，
动差大来波动慢，微分时间应加长，
理想曲线两个波，前高后低四比一，
一看二调多分析，调节质量不会低。

3. APOLLO代码介绍

3.1 PID算法

Input：当前偏差error及采样时间ts
output：pid求解结果

3.2 积分饱和问题

针对积分饱和问题

根据当前偏差量进行计算.判断目标输出是否与偏差大小同方向且数值是否超限，若超限，则积分作用无意义。

当系统处于饱和状态时，对积分项加入负反馈。将$u$限幅后与原来的$u$作差。

3.3 纵向控制代码

3.3.1 构造函数

LonController::LonController()
    : name_(ControlConf_ControllerType_Name(ControlConf::LON_CONTROLLER)) {
  if (FLAGS_enable_csv_debug) {
    time_t rawtime;
    char name_buffer[80];
    std::time(&rawtime);
    std::tm time_tm;
    localtime_r(&rawtime, &time_tm);
    strftime(name_buffer, 80, "/tmp/speed_log__%F_%H%M%S.csv", &time_tm);
    speed_log_file_ = fopen(name_buffer, "w");
    if (speed_log_file_ == nullptr) {
      AERROR << "Fail to open file:" << name_buffer;
      FLAGS_enable_csv_debug = false;
    }
    if (speed_log_file_ != nullptr) {
      fprintf(speed_log_file_,
              "station_reference,"
              "station_error,"
              "station_error_limited,"
              "preview_station_error,"
              "speed_reference,"
              "speed_error,"
              "speed_error_limited,，"
              "preview_speed_reference,"
              "preview_speed_error,"
              "preview_acceleration_reference,"
              "acceleration_cmd_closeloop,"
              "acceleration_cmd,"
              "acceleration_lookup,"
              "acceleration_lookup_limit,"
              "speed_lookup,"
              "calibration_value,"
              "throttle_cmd,"
              "brake_cmd,"
              "is_full_stop,"
              "\r\n");

      fflush(speed_log_file_);
    }
    AINFO << name_ << " used.";
  }
}

/tmp/speed_log__%F_%H%M%S.csv中记录了不同的变量，LonController::LonController()将这些变量的标题放入文件中。

3.3.2 加载各种纵向控制的配置参数

Status LonController::Init(std::shared_ptr<DependencyInjector> injector,
                           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");
  }
  injector_ = injector;
  const LonControllerConf &lon_controller_conf =
      control_conf_->lon_controller_conf();
  double ts = lon_controller_conf.ts();//0.01
  // 反向超前滞后补偿
  bool enable_leadlag =
      lon_controller_conf.enable_reverse_leadlag_compensation();//false
// 位置偏差pid参数 低速pid参数
  station_pid_controller_.Init(lon_controller_conf.station_pid_conf());
  speed_pid_controller_.Init(lon_controller_conf.low_speed_pid_conf());

  if (enable_leadlag) {
    station_leadlag_controller_.Init(
        lon_controller_conf.reverse_station_leadlag_conf(), ts);
    speed_leadlag_controller_.Init(
        lon_controller_conf.reverse_speed_leadlag_conf(), ts);
  }

  vehicle_param_.CopyFrom(
      common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
// 数字滤波器
  SetDigitalFilterPitchAngle(lon_controller_conf);
//标定表
  LoadControlCalibrationTable(lon_controller_conf);
  controller_initialized_ = true;

  return Status::OK();
}

首先会加载纵向控制的配置文件 control_conf_ = control_conf;再进行一些安全性的检查。在获取纵向控制的配置信息之后，会将配置文件里的一些数值读取出来，比如控制频率double ts = lon_controller_conf.ts();//0.01，以及进行超前滞后补偿 bool enable_leadlag = lon_controller_conf.enable_reverse_leadlag_compensation();.

对位置pid的初始参数以及低速状态下的pid参数进行初始化
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);

3.3.3 二阶巴特沃斯低通滤波器《数字信号处理》

void LonController::SetDigitalFilterPitchAngle(
    const LonControllerConf &lon_controller_conf) {
  double cutoff_freq =
      lon_controller_conf.pitch_angle_filter_conf().cutoff_freq();
  double ts = lon_controller_conf.ts();
  SetDigitalFilter(ts, cutoff_freq, &digital_filter_pitch_angle_);
}

3.3.4 插值出油门开度

void LonController::LoadControlCalibrationTable(
    const LonControllerConf &lon_controller_conf) {
  const auto &control_table = lon_controller_conf.calibration_table();
  AINFO << "Control calibration table loaded";
  AINFO << "Control calibration table size is "
        << control_table.calibration_size();
  Interpolation2D::DataType xyz;
  for (const auto &calibration : control_table.calibration()) {
    xyz.push_back(std::make_tuple(calibration.speed(),
                                  calibration.acceleration(),
                                  calibration.command()));
  }
  //后面计算结果会在这里插值出油门开度
  control_interpolation_.reset(new Interpolation2D);
  ACHECK(control_interpolation_->Init(xyz))
      << "Fail to load control calibration table";
}

3.3.5 根据定位、底盘、规划信息计算输出指令cmd

// 根据定位、底盘、规划信息计算输出指令cmd
Status LonController::ComputeControlCommand(
    const localization::LocalizationEstimate *localization,
    const canbus::Chassis *chassis,
    const planning::ADCTrajectory *planning_published_trajectory,
    control::ControlCommand *cmd) {
  localization_ = localization;
  chassis_ = chassis;

  trajectory_message_ = planning_published_trajectory;
  if (!control_interpolation_) {
    AERROR << "Fail to initialize calibration table.";
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
                  "Fail to initialize calibration table.");
  }

  if (trajectory_analyzer_ == nullptr ||
      trajectory_analyzer_->seq_num() !=
          trajectory_message_->header().sequence_num()) {
    trajectory_analyzer_.reset(new TrajectoryAnalyzer(trajectory_message_));
  }
  const LonControllerConf &lon_controller_conf =
      control_conf_->lon_controller_conf();

  auto debug = cmd->mutable_debug()->mutable_simple_lon_debug();
  debug->Clear();

  double brake_cmd = 0.0;
  double throttle_cmd = 0.0;
  double ts = lon_controller_conf.ts();
  double preview_time = lon_controller_conf.preview_window() * ts;//考虑预瞄 参数为20
  bool enable_leadlag =
      lon_controller_conf.enable_reverse_leadlag_compensation();

  if (preview_time < 0.0) {
    const auto error_msg =
        absl::StrCat("Preview time set as: ", preview_time, " less than 0");
    AERROR << error_msg;
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);
  }
  //根据当前车(x,y)转化到sl坐标系
  //对比匹配点s s' d d'等多个车辆状态数据
  //将数据保存在debug中，方便日志查询
  ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, ts,
                            debug);

  double station_error_limit = lon_controller_conf.station_error_limit();
  double station_error_limited = 0.0;

//考虑预瞄时纵向位置偏差的上下限不同
  if (FLAGS_enable_speed_station_preview) {
    station_error_limited =
        common::math::Clamp(debug->preview_station_error(),
                            -station_error_limit, station_error_limit);
  } else {
    station_error_limited = common::math::Clamp(
        debug->station_error(), -station_error_limit, station_error_limit);
  }
// 考虑挡位车速情况的参数设置
  if (trajectory_message_->gear() == canbus::Chassis::GEAR_REVERSE) {
    station_pid_controller_.SetPID(
        lon_controller_conf.reverse_station_pid_conf());
    speed_pid_controller_.SetPID(lon_controller_conf.reverse_speed_pid_conf());
    if (enable_leadlag) {
      station_leadlag_controller_.SetLeadlag(
          lon_controller_conf.reverse_station_leadlag_conf());
      speed_leadlag_controller_.SetLeadlag(
          lon_controller_conf.reverse_speed_leadlag_conf());
    }
  } else if (injector_->vehicle_state()->linear_velocity() <=
             lon_controller_conf.switch_speed()) {
    station_pid_controller_.SetPID(lon_controller_conf.station_pid_conf());
    speed_pid_controller_.SetPID(lon_controller_conf.low_speed_pid_conf());
  } else {
    station_pid_controller_.SetPID(lon_controller_conf.station_pid_conf());
    speed_pid_controller_.SetPID(lon_controller_conf.high_speed_pid_conf());
  }
//位置环输出到速度环，速度环为内环
//s=vt 在当前纵向偏差下的速度补偿
  double speed_offset =
      station_pid_controller_.Control(station_error_limited, ts);
  if (enable_leadlag) {
    speed_offset = station_leadlag_controller_.Control(speed_offset, ts);
  }

  debug->set_station_output_kp(station_pid_controller_.Output_Kp());
  debug->set_station_output_ki(station_pid_controller_.Output_Ki());
  debug->set_station_output_kd(station_pid_controller_.Output_Kd()); 

  double speed_controller_input = 0.0;
  double speed_controller_input_limit =
      lon_controller_conf.speed_controller_input_limit();
 // 根据预瞄计算速度环输入
  double speed_controller_input_limited = 0.0;
  if (FLAGS_enable_speed_station_preview) {
    speed_controller_input = speed_offset + debug->preview_speed_error();
  } else {
  // 跟匹配点的速度误差以及位置偏差求出的速度补偿
    speed_controller_input = speed_offset + debug->speed_error();
  }
  // 速度环输入限幅
  speed_controller_input_limited =
      common::math::Clamp(speed_controller_input, -speed_controller_input_limit,
                          speed_controller_input_limit);
//求解速度环输出，给出加速度结果
//v=at
  double acceleration_cmd_closeloop = 0.0;

  acceleration_cmd_closeloop =
      speed_pid_controller_.Control(speed_controller_input_limited, ts);
  debug->set_pid_saturation_status(
      speed_pid_controller_.IntegratorSaturationStatus());
  if (enable_leadlag) {
    acceleration_cmd_closeloop =
        speed_leadlag_controller_.Control(acceleration_cmd_closeloop, ts);
    debug->set_leadlag_saturation_status(
        speed_leadlag_controller_.InnerstateSaturationStatus());
  }
  debug->set_speed_output_kp(speed_pid_controller_.Output_Kp());
  debug->set_speed_output_ki(speed_pid_controller_.Output_Ki());
  debug->set_speed_output_kd(speed_pid_controller_.Output_Kd()); 

  if (chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
    speed_pid_controller_.Reset_integral();
    station_pid_controller_.Reset_integral();
  }
//数字滤波器的俯仰角补偿
  double slope_offset_compensation = digital_filter_pitch_angle_.Filter(
      GRA_ACC * std::sin(injector_->vehicle_state()->pitch()));

  if (std::isnan(slope_offset_compensation)) {
    slope_offset_compensation = 0;
  }

  debug->set_slope_offset_compensation(slope_offset_compensation);
//控制结果u（k）
  double acceleration_cmd =
      acceleration_cmd_closeloop + debug->preview_acceleration_reference() +
      FLAGS_enable_slope_offset * debug->slope_offset_compensation();
  debug->set_is_full_stop(false);
  //计算当前轨迹剩余距离
  GetPathRemain(debug);

  if ((trajectory_message_->trajectory_type() ==
       apollo::planning::ADCTrajectory::UNKNOWN) &&
      std::abs(cmd->steering_target() - chassis->steering_percentage()) > 20) {
    acceleration_cmd = 0;
    ADEBUG << "Steering not reached";
    debug->set_is_full_stop(true);
    speed_pid_controller_.Reset_integral();
    station_pid_controller_.Reset_integral();
  }

  // At near-stop stage, replace the brake control command with the standstill
  // acceleration if the former is even softer than the latter
  if (((trajectory_message_->trajectory_type() ==
        apollo::planning::ADCTrajectory::NORMAL) ||
       (trajectory_message_->trajectory_type() ==
        apollo::planning::ADCTrajectory::SPEED_FALLBACK)) &&
      ((std::fabs(debug->preview_acceleration_reference()) <=
            control_conf_->max_acceleration_when_stopped() &&
        std::fabs(debug->preview_speed_reference()) <=
            vehicle_param_.max_abs_speed_when_stopped()) ||
       std::abs(debug->path_remain()) <
           control_conf_->max_path_remain_when_stopped())) {
    acceleration_cmd =
        (chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
            ? std::max(acceleration_cmd,
                       -lon_controller_conf.standstill_acceleration())
            : std::min(acceleration_cmd,
                       lon_controller_conf.standstill_acceleration());
    ADEBUG << "Stop location reached";
    debug->set_is_full_stop(true);
    speed_pid_controller_.Reset_integral();
    station_pid_controller_.Reset_integral();
  }

  double throttle_lowerbound =
      std::max(vehicle_param_.throttle_deadzone(),
               lon_controller_conf.throttle_minimum_action());
  double brake_lowerbound =
      std::max(vehicle_param_.brake_deadzone(),
               lon_controller_conf.brake_minimum_action());
  double calibration_value = 0.0;
  // u(k)分前进倒车两种情况
  double acceleration_lookup =
      (chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
          ? -acceleration_cmd
          : acceleration_cmd;

  double acceleration_lookup_limited =
      vehicle_param_.max_acceleration() +
      FLAGS_enable_slope_offset * debug->slope_offset_compensation();
  double acceleration_lookup_limit = 0.0;
// 根据计算出的加速度与车速查找标定表，插值出当前油门开度
  if (FLAGS_use_acceleration_lookup_limit) {
    acceleration_lookup_limit =
        (acceleration_lookup > acceleration_lookup_limited)
            ? acceleration_lookup_limited
            : acceleration_lookup;
  }
// 计算结果限幅后输出
  if (FLAGS_use_preview_speed_for_table) {
    if (FLAGS_use_acceleration_lookup_limit) {
      calibration_value = control_interpolation_->Interpolate(std::make_pair(
          debug->preview_speed_reference(), acceleration_lookup_limit));
    } else {
      calibration_value = control_interpolation_->Interpolate(std::make_pair(
          debug->preview_speed_reference(), acceleration_lookup));
    }
  } else {
    if (FLAGS_use_acceleration_lookup_limit) {
      calibration_value = control_interpolation_->Interpolate(
          std::make_pair(chassis_->speed_mps(), acceleration_lookup_limit));
    } else {
      calibration_value = control_interpolation_->Interpolate(
          std::make_pair(chassis_->speed_mps(), acceleration_lookup));
    }
  }

  if (acceleration_lookup >= 0) {
    if (calibration_value >= 0) {
      throttle_cmd = std::max(calibration_value, throttle_lowerbound);
    } else {
      throttle_cmd = throttle_lowerbound;
    }
    brake_cmd = 0.0;
  } else {
    throttle_cmd = 0.0;
    if (calibration_value >= 0) {
      brake_cmd = brake_lowerbound;
    } else {
      brake_cmd = std::max(-calibration_value, brake_lowerbound);
    }
  }

  debug->set_station_error_limited(station_error_limited);
  debug->set_speed_offset(speed_offset);
  debug->set_speed_controller_input_limited(speed_controller_input_limited);
  debug->set_acceleration_cmd(acceleration_cmd);
  debug->set_throttle_cmd(throttle_cmd);
  debug->set_brake_cmd(brake_cmd);
  debug->set_acceleration_lookup(acceleration_lookup);
  debug->set_acceleration_lookup_limit(acceleration_lookup_limit);
  debug->set_speed_lookup(chassis_->speed_mps());
  debug->set_calibration_value(calibration_value);
  debug->set_acceleration_cmd_closeloop(acceleration_cmd_closeloop);

  if (FLAGS_enable_csv_debug && speed_log_file_ != nullptr) {
    fprintf(speed_log_file_,
            "%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f,"
            "%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %d,\r\n",
            debug->station_reference(), debug->station_error(),
            station_error_limited, debug->preview_station_error(),
            debug->speed_reference(), debug->speed_error(),
            speed_controller_input_limited, debug->preview_speed_reference(),
            debug->preview_speed_error(),
            debug->preview_acceleration_reference(), acceleration_cmd_closeloop,
            acceleration_cmd, debug->acceleration_lookup(),
            debug->acceleration_lookup_limit(), debug->speed_lookup(),
            calibration_value, throttle_cmd, brake_cmd, debug->is_full_stop());
  }

  // if the car is driven by acceleration, disgard the cmd->throttle and brake
  cmd->set_throttle(throttle_cmd);
  cmd->set_brake(brake_cmd);
  if (FLAGS_use_acceleration_lookup_limit) {
    cmd->set_acceleration(acceleration_lookup_limit);
  } else {
    cmd->set_acceleration(acceleration_cmd);
  }

  if (std::fabs(injector_->vehicle_state()->linear_velocity()) <=
          vehicle_param_.max_abs_speed_when_stopped() ||
      chassis->gear_location() == trajectory_message_->gear() ||
      chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
    cmd->set_gear_location(trajectory_message_->gear());
  } else {
    cmd->set_gear_location(chassis->gear_location());
  }

  return Status::OK();
}

if (!control_interpolation_) {
首先检测初始化后的标定表是否是有效的。
if (trajectory_analyzer_ == nullptr ||
再判断planning模块传过来的指针是否是空的（ trajectory_analyzer_是一种轨迹分析的工具，计算当前车辆的状态偏差，例如横纵向偏差）。

auto debug = cmd->mutable_debug()->mutable_simple_lon_debug(); debug->Clear();
debug中存储了一些控制量的信息
double brake_cmd = 0.0;
初始化刹车
double throttle_cmd = 0.0;
初始化油门
double ts = lon_controller_conf.ts();
获取配置文件中模块执行频率
double preview_time = lon_controller_conf.preview_window() * ts;//考虑预瞄 参数为20
考虑预瞄的问题

ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, ts, debug);
根据当前车(x,y)转化到sl坐标系;
对比匹配点s s’ d d’等多个车辆状态数据;
将数据保存在debug中，方便日志查询.

if (FLAGS_enable_speed_station_preview) {
考虑预瞄时纵向位置偏差的上下限不同

if (trajectory_message_->gear() == canbus::Chassis::GEAR_REVERSE) {
考虑挡位车速情况的参数设置,根据不同车辆行驶状况设置不同的pid参数。

4. PID算法实践

Apollo控制之控制仿真实践
在终端中输入以下指令，启动dreamview

aem bootstrap start

运行成功后，点击左上角dreamview按钮。当网页中出现apollo后， 即表示dreamview启动成功了， 为了便于调试，在该页面上，我们选择车辆(vehicle) 为MKZ，关闭mute,选择地图Apollo Virutal Map。

点击Sim Control

点击Profile, 下载动力学模型，Mkz Model (lincolnmkz动力学模型)，Point Mass Model (惯性模型) 。然后选择Dynamic Model,选择Mkz Model.

启动Planning, Routing, Control,

在Route Editing页面，选择起点，终点，选择SendRouting Request。发送前点开recorder

查看车辆前方能够正常生成规划线，此时切换到Tasks页面，点击StartAuto。

点击Dreamview左侧菜单栏Tasks按钮，关闭LockTask Panel,打开PNC monitor功能按钮，点击右侧Control按钮，可以查看控制相关的数据。

通过下拉右边栏，可查看控制相关参数的变化，比如右侧第二个图表speed,蓝色曲线表示车辆的实际车速，青绿色的表示规划的参考车速，可看到实际车速与规划速度的跟随情况，从而可以进行控制效果。

查看纵向控制参数配置点击在线编辑，打开控制参数配置文件/apollo workspace/modules/control/conf/control co nf.pb.txt,可查看apollo默认的控制参数。

从配置文件内，可查看纵向PID控制参数如下，纵向PID主要包含2层串联pid控制器，位置PID和速度PID, PID调节参数:位置PID参数kp, ki, kd, 速度PID参数kp, ki, kd。 在速度PID参数中，根据不同当前车速做了PID参数选择，switch speed表示速度限，速度小于该值时，速度PID执行low_ speed pid conf,反之，执行high speed pid conf。

(1)将所有pid参数置零

lon_controller_conf {
 station_pid_conf {
  integrator_enable: false
  integrator_saturation_level: 0.3
  kp: 0.0
  ki: 0.0
  kd: 0.0
 }
 low_speed_pid_conf {
  integrator_enable: true
  integrator_saturation_level: 0.3
  kp: 0.0
  ki: 0.0
  kd: 0.0
 }
  high_speed_pid_conf {
   integrator_enable: true
   integrator_saturation_level: 0.3
   kp: 0.0
   ki: 0.0
   kd: 0.0
 }
  switch_speed: 3.0

然后重复5.3步骤并点击recorder按钮，进行录制数据包。可以通过PNC monitor对控制效果进行简单分析。
(2)然后将low speed pid conf中kp设置为3.0，high_ speed pid. conf设置为3.0，达到猛加急减的效果

lon_controller_conf {
 station_pid_conf {
  integrator_enable: false
  integrator_saturation_level: 0.3
  kp: 0.0
  ki: 0.0
  kd: 0.0
 }
 low_speed_pid_conf {
  integrator_enable: true
  integrator_saturation_level: 0.3
  kp: 3.0
  ki: 0.0
  kd: 0.0
 }
  high_speed_pid_conf {
   integrator_enable: true
   integrator_saturation_level: 0.3
   kp: 3    .0
   ki: 0.0
   kd: 0.0
 }
  switch_speed: 3.0

打开notebook脚本进行数据分析

% matplotlib notebook
run control_info.py --path /apollo/data/bag/******

左上图，蓝色曲线为车辆实时速度曲线，绿色线为规划出的参考速度