PID控制及代码

PID控制公式:

所谓的PID,指的就是三个项的参数,分别是线性项参数Kp,积分项Ki,以及微分项参数Kd.

当采样时间短,可以采用一阶差分代替一阶微分,用累加代替积分:

//实现PID控制
double PIDController::Control(const double error, const double dt)
{
  if (dt <= 0)
  {
    return previous_output_;
  }
  double diff = 0;
  double output = 0;

  if (first_hit_)    //first_hit_: 用来选择是否计算diff
  {
    first_hit_ = false;
  }
  else
  {
    diff = (error - previous_error_) / dt;
  }
    
  integral_ += ki_ * error * dt;        //积分环节  

  previous_error_ = error;
  output = -kp_ * error - integral_ + diff * kd_;  
  previous_output_ = output;
  return output;
}

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