apollo PIDController代码分析

pid_controller.h

定义基类PIDController，就是我们了解的pid控制器的基本写法，除此之外什么也没有。
apollo的函数都是各个单词首字母大写，字母之间不加下划线，变量的都是小写，字母之间加下划线，末尾也加下划线。
成员函数：
public:
Init(); //重置参数、读取配置文件的参数
SetPID(); //在Init()中被调用，读取配置文件中的pid参数，读取kp,ki,kd,kaw是个啥玩意？
Reset(); //重置其中某些参数
Control(); //计算误差的积分、积分项、比例项，没啥可说的，普通的PID核心实现
IntegratorSaturationStatus(); //return integrator_saturation_status_;
IntegratorHold(); //return integrator_hold_;
SetIntegratorHold(); //获取是否要计算积分项的标志位

protected:
kp_;
ki_;
kd_;
kaw_;
previous_error_;
previous_output_;
integral_;
integrator_saturation_high_;
integrator_saturation_low_;
first_hit_;
integrator_enabled_;
integrator_hold_;
integrator_saturation_status_;
output_saturation_high_;
output_saturation_low_;
output_saturation_status_;

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file pid_controller.h
 * @brief Defines the PIDController class.
 */

#pragma once

#include "modules/control/proto/pid_conf.pb.h"

/**
 * @namespace apollo::control
 * @brief apollo::control
 */
namespace apollo {
namespace control {

/**
 * @class PIDController
 * @brief A proportional-integral-derivative controller for speed and steering
 using defualt integral hold
 */
class PIDController {
 public:
  /**
   * @brief initialize pid controller
   * @param pid_conf configuration for pid controller
   */
  void Init(const PidConf &pid_conf);

  /**
   * @brief set pid controller coefficients for the proportional,
   * integral, and derivative
   * @param pid_conf configuration for pid controller
   */
  void SetPID(const PidConf &pid_conf);

  /**
   * @brief reset variables for pid controller
   */
  void Reset();

  /**
   * @brief compute control value based on the error
   * @param error error value, the difference between
   * a desired value and a measured value
   * @param dt sampling time interval
   * @return control value based on PID terms
   */
  virtual double Control(const double error, const double dt);

  virtual ~PIDController() = default;

  /**
   * @brief get saturation status
   * @return saturation status
   */
  int IntegratorSaturationStatus() const;

  /**
   * @brief get status that if integrator is hold
   * @return if integrator is hold return true
   */
  bool IntegratorHold() const;

  /**
   * @brief set whether to hold integrator component at its current value.
   * @param hold
   */
  void SetIntegratorHold(bool hold);

 protected:
  double kp_ = 0.0;
  double ki_ = 0.0;
  double kd_ = 0.0;
  double kaw_ = 0.0;
  double previous_error_ = 0.0;
  double previous_output_ = 0.0;
  double integral_ = 0.0;
  double integrator_saturation_high_ = 0.0;
  double integrator_saturation_low_ = 0.0;
  bool first_hit_ = false;
  bool integrator_enabled_ = false;
  bool integrator_hold_ = false;
  int integrator_saturation_status_ = 0;
  // Only used for pid_BC_controller and pid_IC_controller
  double output_saturation_high_ = 0.0;
  double output_saturation_low_ = 0.0;
  int output_saturation_status_ = 0;
};

}  // namespace control
}  // namespace apollo

pid_controller.cc

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/control/common/pid_controller.h"

#include <cmath>

#include "cyber/common/log.h"

namespace apollo {
namespace control {

double PIDController::Control(const double error, const double dt) {
  if (dt <= 0) {
    AWARN << "dt <= 0, will use the last output, dt: " << dt;
    return previous_output_;
  }
  double diff = 0;
  double output = 0;

  if (first_hit_) {
    first_hit_ = false;
  } else {
    diff = (error - previous_error_) / dt;
  }
  // integral hold
  if (!integrator_enabled_) {
    integral_ = 0;
  } else if (!integrator_hold_) {
    integral_ += error * dt * ki_;
    // apply Ki before integrating to avoid steps when change Ki at steady state
    if (integral_ > integrator_saturation_high_) {
      integral_ = integrator_saturation_high_;
      integrator_saturation_status_ = 1;
    } else if (integral_ < integrator_saturation_low_) {
      integral_ = integrator_saturation_low_;
      integrator_saturation_status_ = -1;
    } else {
      integrator_saturation_status_ = 0;
    }
  }
  previous_error_ = error;
  output = error * kp_ + integral_ + diff * kd_;  // Ki already applied
  previous_output_ = output;
  return output;
}

void PIDController::Reset() {
  previous_error_ = 0.0;
  previous_output_ = 0.0;
  integral_ = 0.0;
  first_hit_ = true;
  integrator_saturation_status_ = 0;
  output_saturation_status_ = 0;
}

void PIDController::Init(const PidConf &pid_conf) {
  previous_error_ = 0.0;
  previous_output_ = 0.0;
  integral_ = 0.0;
  first_hit_ = true;
  integrator_enabled_ = pid_conf.integrator_enable();
  integrator_saturation_high_ =
      std::fabs(pid_conf.integrator_saturation_level());
  integrator_saturation_low_ =
      -std::fabs(pid_conf.integrator_saturation_level());
  integrator_saturation_status_ = 0;
  integrator_hold_ = false;
  output_saturation_high_ = std::fabs(pid_conf.output_saturation_level());
  output_saturation_low_ = -std::fabs(pid_conf.output_saturation_level());
  output_saturation_status_ = 0;
  SetPID(pid_conf);
}

void PIDController::SetPID(const PidConf &pid_conf) {
  kp_ = pid_conf.kp();
  ki_ = pid_conf.ki();
  kd_ = pid_conf.kd();
  kaw_ = pid_conf.kaw();
}

int PIDController::IntegratorSaturationStatus() const {
  return integrator_saturation_status_;
}

bool PIDController::IntegratorHold() const { return integrator_hold_; }

void PIDController::SetIntegratorHold(bool hold) { integrator_hold_ = hold; }

}  // namespace control
}  // namespace apollo