Apollo control之PID算法

Apollo studio 官网：Apollo开发者社区 (baidu.com)

目录

1 PID简介 

2 PID调参思路

3 代码

4 解决积分饱和的方法

4.1 IC 积分遇限削弱法

4.2 BC 反馈抑制抗饱和

---

1 PID简介 

PID算法有时间离散的，也有时间连续的

同时增量式的PID也是有一定好处

2 PID调参思路

3 代码

为实现PID控制，我们可以利用离散的PID控制来实现，当采样时间足够短时，也能保持较好的性能，公式如下

需要注意的点就是积分初始值为0、微分初始值为0 

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_=false;
}
else{
diff=(error-previous_error_)*kd_/dt;
}
integral_ += error*dt*ki_;//积分的计算可以由下面两种方法替代
output=error*kp_+integral_+diff;
previous_error_=error;
previous_output_=output;
return output;
}

我们需要更新下面这四个参数来保证从头开始

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

4 解决积分饱和的方法

4.1 IC 积分遇限削弱法

// 积分遇限消弱法
double output_saturation_high=10.0;
double output_saturation_low=-10.0;
double u =error*kp_+integral_+error*dt*ki_+diff*kd_ ; // 算当前时刻理应输出大小
if (((error *u)> 0) &&
((u > output_saturation_high )||(u < output_saturation_low))) {
}
else {
integral_ += error*dt*ki_;
}

4.2 BC 反馈抑制抗饱和

// 反馈抑制抗饱和
double output_saturation_high=10.0;
double output_saturation_low=-10.0;
double kaw=ki_/kp_；
double u = error*kp_+integral_+error*dt*ki_+diff*kd_;
double aw_term =0.0;
if (u > output_saturation_high){
aw_term=(output_saturation_high-u)*kaw;
}
else if (u < output_saturation_low)
aw_term=(output_saturation_low-u)*kaw;
else{
}
integral_+=aw_term+error*dt*ki_;
}