前言
大家好，今天我们介绍一下经典控制理论中的PID控制算法，并着重讲解该算法的编码实现，为实现后续的倒立摆样例内容做准备。 众所周知，掌握了 PID ，就相当于进入了控制工程的大门，也能为更高阶的控制理论学习打下基础。 在很多的自动化控制领域。都会遇到PID控制算法，这种算法具有很好的控制模式，可以让系统具有很好的鲁棒性。
基本介绍
PID 深入理解
（1）闭环控制系统：讲解 PID 之前，我们先解释什么是闭环控制系统。简单说就是一个有输入有输出的系统，输入能影响输出。一般情况下，人们也称输出为反馈，因此也叫闭环反馈控制系统。比如恒温水池，输入就是加热功率，输出就是水温度；比如冷库，输入是空调功率，输出是内部温度。
（2）什么是PID：英文分解开就是：比例（proportional）、积分（integral）、微分（derivative），其根据系统反馈，通过比例，积分和微分三个部分的计算，动态调整系统输入，确保被控量稳定在人们设定的目标值附近。PID 是目前最常见的应用于闭环反馈控制系统的算法，三个部分可以只用一个（P，I，D），也可以只用两个（PI，PD），也可以三个一起用（PID），非常灵活。
（3）PID控制原理图与表达式：
上面的控制原理图与下面的数学表达式是相互对应的。
setpoint 为设定值，也叫目标值；output(t) 是系统反馈值，随时间变化；e(t) 是设定值与反馈值的差值，由于反馈总是作为被减数，因此也称为负反馈控制算法；Kp 是比例系数，Kp * e(t) 就是 PID 的比例部分；Ki 是积分系数，Ki 乘以 e(t) 对时间的积分，就是 PID 的积分部分；Kd 是微分系数，Kd 乘以 e(t) 对时间的微分，就是 PID 的微分部分。通常情况下，三个系数都是正数，但三个部分正负号并不一定相同，相互之间有抵消和补偿。三个部分之和，就是系统输入值 input(t)。整个控制系统的目标就是让差值 e(t) 稳定到 0。
（4）我们以恒温水池为例，讲解 PID 的三个部分：其中 input(t) 为加热功率，output(t) 为水池温度，setpoint 假设为 36 度， e(t) 为 setpoint 与当前温度的差值 。
比例部分：比例部分最直观，也比较容易理解，举例而言：假设当前水温为 20 度，差值 e 为 36 - 20 = 16 度，乘上比例系数 Kp ，得到加热功率，于是温度就会慢慢上涨；如果水温超过了设定温度，比如 40 度，差值 e 为 36 - 40 = -4 度，则停止加热，让热量耗散，温度就会慢慢下降。
微分部分：只有比例部分，我们可以想象出水池温度的变化通常会比较大，而且很难恒定，这样的水池不能算是恒温水池。解决办法是引入差值 e(t) 的微分，也就是 e(t) 对时间的导数。通过数学计算，可得导数为水池温度的斜率负数：
根据求导结果，我们分两种情况讨论微分部分对比例部分的作用：当差值 e(t) 扩大时：微分部分将与比例部分同正负号，对比例部分进行补偿，更好的抑制差值扩大；当差值 e(t) 缩小时：微分部分将与比例部分异号，对比例部分进行抵消，防止系统输出过冲。综合两种情况，可以认为微分部分提供了一种预测性的调控作用，通过考虑差值 e(t) 的未来走势，更精细地调整系统输入，从而让系统输出逐渐收敛到目标值。
积分部分：只有比例和微分部分，在某些场景下会失灵。举例而言，假如我们只使用 PD 算法。此时水池的室外温度非常低，热量散失非常快。当加热到某个温度的时候（比如 30 度），温度可能再也无法上涨。这种情况，称之为系统的稳态误差。我们分两部分解释原因：比例部分：由于差值 e(t) 不那么大了，比例部分会比较小，每次增加的热量正好被耗散掉，因此温度不会继续上升；微分部分：由于温度基本恒定，微分部分将约为零，也无法对比例部分进行补偿。解决办法是引入差值 e(t) 的积分，也就是 e(t) 乘以单位时间并不断累加，数学表达式如下：
假设温度停在了 30 度，不再上升，此时，积分部分会随着时间的推移而不断增加，相当于对比例部分进行补偿，从而增加加热功率，最终温度将继续上升。下面的动图比较形象地展示了三个参数对系统输出的影响：
（5）PID 为什么被称为启发式控制算法：
第一，PID 的三个参数并非基于严格的数学计算得到，而是靠工程师的直觉和经验。第二，PID 算法调参的目标是可用，只要实际效果不错就行，并不追求最优解。第三，PID 不依赖精确的数学模型，就能进行有效的控制。因此看起来更像是一种基于实践和实际效果的启发式方法，而不是一个理论上推导出来的控制策略。（6）介绍一种 PID 调参方法：Ziegler-Nichols（齐格勒-尼科尔斯）最终值振荡法第一，将微分系数 Kd 和积分系数 Kp 都设置为 0，只保留比例系数。第二，不断增加比例系数，直到达到无衰减的持续振荡，此时的比例系数称为 Ku ，此时的振荡周期为 Tu。第三，使用临界系数和振荡周期设置 PID 参数：
比例系数：Kp = 0.60 * Ku积分系数：Ki = 2 * Kp / Tu微分系数：Kd = Kp * Tu / 8
PID 编码实现
这部分我们主要参考 Arduino 的 PID 库 Arduino-PID-Library，分八步实现一个实际可用的 PID 算法库。接下来的每一步都需要大家认真的阅读，因为涉及到很多的细节。
特别提示：由于本节讲解 PID 的实现，我们将以 PID 作为第一视角，如果提到 input ，指的是 PID 算法输入，相当于上节中的系统输出 output(t)，即恒温水池的温度；如果提到 ouput，指的是 PID 算法输出，相当于上节中的系统输入 input(t)，即加热功率。
初始版本
代码实现 PID 算法，面临最大的困惑是如何实现积分和微分。正如上一节所说，积分可转化为差值 e(t) 乘以采样间隔并不断累加；微分可转换为求两次采样的差值 e(t) 的斜率。于是有了如下代码，请读者关注代码注释（可以直接拿去跑）。
#include <iostream>#include <chrono>#include <thread>
class PIDController {public:explicit PIDController() {InitTime();}
PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) { InitTime(); } void InitTime() { last_time_ = GetMillis(); } double Compute(double setpoint, double input) { uint64_t now = GetMillis(); double time_change = static_cast<double>(now - last_time_); double error = setpoint - input; printf("error: %f\n", error); err_sum_ += error * time_change; double derivative = (error - last_error_) / time_change; double output = kp_ * error + ki_ * err_sum_ + kd_ * derivative; last_error_ = error; last_time_ = now; return output; } void set_tunings(double kp_para, double ki_para, double kd_para) { kp_ = kp_para; ki_ = ki_para; kd_ = kd_para; }
private:double kp_;double ki_;double kd_;
double last_error_ = 0; double err_sum_ = 0; uint64_t last_time_ = 0; uint64_t GetMillis() { return std::chrono::duration_cast<std::chrono::milliseconds>( std::chrono::steady_clock::now().time_since_epoch()) .count(); }
};
int main() {PIDController pid;pid.set_tunings(10, 0.01, 0.01);
double setpoint = 36; double temperature = 20; std::this_thread::sleep_for(std::chrono::seconds(1)); for (int i = 0; i < 100; ++i) { double control_signal = pid.Compute(setpoint, temperature); temperature += control_signal * 0.1; temperature *= 0.99; std::cout << "Temperature: " << temperature << std::endl; std::this_thread::sleep_for(std::chrono::seconds(1)); } return 0;
}
固定采样间隔
初始版本的 PID 的采样间隔是由外部循环控制的，会导致两个问题：第一，无法获取一致的 PID 行为，因为外部有可能调用，也有可能不调用；第二，每次都要根据采样间隔计算微分和积分部分，这涉及到浮点运算。效率比较低。好的办法是固定采用间隔，两个问题都能解决，看下面的代码以及注释（可以直接拿去跑）。
#include <iostream>#include <chrono>#include <thread>
class PIDController {public:explicit PIDController() {InitTime();}
PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) { InitTime(); } void InitTime() { last_time_ = GetMillis(); } void set_tunings(double kp_para, double ki_para, double kd_para) { double sample_time_in_sec = static_cast<double>(sample_time_) / 1000.0; kp_ = kp_para; // sum = ki * (error(0) * dt + error(1) * dt + ... + error(n) * dt) = (ki * dt) * (error(0) + error(1) + ... + error(n)) ki_ = ki_para * sample_time_in_sec; // derivative = kd * (error(n) - error(n-1)) / dt = (kd / dt) * (error(n) - error(n-1)) kd_ = kd_para / sample_time_in_sec; } void set_sample_time(uint64_t new_sample_time) { if (new_sample_time > 0) { double ratio = static_cast<double>(new_sample_time) / static_cast<double>(sample_time_); ki_ = ki_ * ratio; kd_ = kd_ / ratio; sample_time_ = new_sample_time; } } double Compute(double setpoint, double input) { uint64_t now = GetMillis(); uint64_t time_change = now - last_time_; if (time_change < sample_time_) { return last_output_; } double error = setpoint - input; printf("error: %f\n", error); err_sum_ += error; double derivative = error - last_error_; double output = kp_ * error + ki_ * err_sum_ + kd_ * derivative; last_error_ = error; last_time_ = now; last_output_ = output; return output; }
private:double kp_;double ki_;double kd_;
double last_error_ = 0.0; double err_sum_ = 0.0; uint64_t last_time_ = 0UL; double last_output_ = 0.0; uint64_t sample_time_ = 1000UL; // 1 second uint64_t GetMillis() { return std::chrono::duration_cast<std::chrono::milliseconds>( std::chrono::steady_clock::now().time_since_epoch()) .count(); }
};
int main() {PIDController pid;pid.set_tunings(1, 0.2, 0.02);pid.set_sample_time(1000); // Set sample time to 1 second
double setpoint = 36; double temperature = 20; std::this_thread::sleep_for(std::chrono::seconds(1)); for (int i = 0; i < 1000; ++i) { double control_signal = pid.Compute(setpoint, temperature); temperature += control_signal * 0.1; temperature *= 0.99; std::cout << "Temperature: " << temperature << std::endl; std::this_thread::sleep_for(std::chrono::milliseconds(200)); } return 0;
}
消除 spike
spike 的英文含义是尖刺，这里指的是当系统运行过程中，突然改变 setpoint 时， PID 的微分部分会因 setpoint 的突然切换而生成一个极大的导数，导致算法输出值 output 将产生一次急剧变化，这就是 spike。比如恒温水池的初始 setpoint 是 36 度，运行过程中，突然改为 50 度。相当于在一个采样周期内，差值 error 突然增加了 14 ，再除以采样周期，数值将会非常大，如下图所示。
解决办法是将 setpoint 从 PID 的微分部分请出去，理论依据是：差值 error 的导数也是算法输入（恒温水池的温度）的斜率负数：
代码实现如下：
#include <iostream>#include <chrono>#include <thread>
class PIDController {public:explicit PIDController() {InitTime();}PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) {InitTime();}
void InitTime() { last_time_ = GetMillis(); } void set_tunings(double kp_para, double ki_para, double kd_para) { double sample_time_in_sec = static_cast<double>(sample_time_) / 1000.0; kp_ = kp_para; ki_ = ki_para * sample_time_in_sec; kd_ = kd_para / sample_time_in_sec; } void set_sample_time(uint64_t new_sample_time) { if (new_sample_time > 0) { double ratio = static_cast<double>(new_sample_time) / static_cast<double>(sample_time_); ki_ = ki_ * ratio; kd_ = kd_ / ratio; sample_time_ = new_sample_time; } } double Compute(double setpoint, double input) { uint64_t now = GetMillis(); uint64_t time_change = now - last_time_; if (time_change < sample_time_) { return last_output_; } double error = setpoint - input; printf("error: %f\n", error); err_sum_ += error; double derivative = input - last_input_; double output = kp_ * error + ki_ * err_sum_ - kd_ * derivative; last_input_ = input; last_time_ = now; last_output_ = output; return output; }
private:double kp_;double ki_;double kd_;
double last_input_ = 0.0; double err_sum_ = 0.0; uint64_t last_time_ = 0UL; double last_output_ = 0.0; uint64_t sample_time_ = 1000UL; // 1 second uint64_t GetMillis() { return std::chrono::duration_cast<std::chrono::milliseconds>( std::chrono::steady_clock::now().time_since_epoch()) .count(); }
};
int main() {PIDController pid;pid.set_tunings(1, 0.2, 0.02);pid.set_sample_time(1000);
double setpoint = 36; double temperature = 20; std::this_thread::sleep_for(std::chrono::seconds(1)); for (int i = 0; i < 1000; ++i) { double control_signal = pid.Compute(setpoint, temperature); temperature += control_signal * 0.1; temperature *= 0.99; std::cout << "Temperature: " << temperature << std::endl; if (i == 200) { setpoint = 50; std::cout << "Setpoint changed to 50" << std::endl; } std::this_thread::sleep_for(std::chrono::milliseconds(200)); } return 0;
}
动态改参
好的 PID 算法，允许在系统运行过程中，调整 PID 参数。问题的关键是，运行中途修改 PID 参数，如何保持算法输出仍然平稳，对系统状态不产生额外冲击。仔细分析 PID 的三个部分，当对应的参数改变时，影响最大的是积分部分，比例和微分两部分都只影响当前值，而积分部分将会更改历史值。
解决办法是放弃先计算积分和，最后乘以积分系数的做法，而是让积分系数参与每一次积分运算并累加起来：
如此一来，即使更新了积分参数，也只影响当前值，历史值由于被存储起来，因此不会改变，代码实现如下 。
#include <iostream>#include <chrono>#include <thread>
class PIDController {public:explicit PIDController() {InitTime();}PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) {InitTime();}
void InitTime() { last_time_ = GetMillis(); } void set_tunings(double kp_para, double ki_para, double kd_para) { double sample_time_in_sec = static_cast<double>(sample_time_) / 1000.0; kp_ = kp_para; ki_ = ki_para * sample_time_in_sec; kd_ = kd_para / sample_time_in_sec; } void set_sample_time(uint64_t new_sample_time) { if (new_sample_time > 0) { double ratio = static_cast<double>(new_sample_time) / static_cast<double>(sample_time_); ki_ = ki_ * ratio; kd_ = kd_ / ratio; sample_time_ = new_sample_time; } } double Compute(double setpoint, double input) { uint64_t now = GetMillis(); uint64_t time_change = now - last_time_; if (time_change < sample_time_) { return last_output_; } double error = setpoint - input; printf("error: %f\n", error); err_item_sum_ += ki_ * error; double derivative = input - last_input_; double output = kp_ * error + err_item_sum_ - kd_ * derivative; last_input_ = input; last_time_ = now; last_output_ = output; return output; }
private:double kp_;double ki_;double kd_;
double last_input_ = 0.0; double err_item_sum_ = 0.0; uint64_t last_time_ = 0UL; double last_output_ = 0.0; uint64_t sample_time_ = 1000UL; // 1 second uint64_t GetMillis() { return std::chrono::duration_cast<std::chrono::milliseconds>( std::chrono::steady_clock::now().time_since_epoch()) .count(); }
};
int main() {PIDController pid;pid.set_tunings(1, 0.2, 0.02);pid.set_sample_time(1000);
double setpoint = 36; double temperature = 20; std::this_thread::sleep_for(std::chrono::seconds(1)); for (int i = 0; i < 1000; ++i) { double control_signal = pid.Compute(setpoint, temperature); temperature += control_signal * 0.1; temperature *= 0.99; std::cout << "Temperature: " << temperature << std::endl; if (i == 200) { pid.set_tunings(1, 0.5, 0.02); std::cout << "PID coefficients changed, 1, 0.2, 0.02 ->1, 0.5, 0.02" << std::endl; } std::this_thread::sleep_for(std::chrono::milliseconds(200)); } return 0;
}
设置算法输出限制
通常情况下，PID 算法输出是有一定限制的，比如恒温水池的加热功率不可能无限大，更不可能小于零。当 PID 的算法输出为负数时，实际是停止加热，也就是功率为零。因此需要给 PID 算法添加限制范围，代码实现如下。补充：为了看到输出限制的作用，这次我们把目标温度定为 90 度。
#include <iostream>#include <chrono>#include <thread>
class PIDController {public:explicit PIDController() {InitTime();}PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) {InitTime();}
void InitTime() { last_time_ = GetMillis(); } void set_tunings(double kp_para, double ki_para, double kd_para) { double sample_time_in_sec = static_cast<double>(sample_time_) / 1000.0; kp_ = kp_para; ki_ = ki_para * sample_time_in_sec; kd_ = kd_para / sample_time_in_sec; } void set_sample_time(uint64_t new_sample_time) { if (new_sample_time > 0) { double ratio = static_cast<double>(new_sample_time) / static_cast<double>(sample_time_); ki_ = ki_ * ratio; kd_ = kd_ / ratio; sample_time_ = new_sample_time; } } void set_output_limits(double min, double max) { if (min > max) { return; } out_min_ = min; out_max_ = max; SetLimits(last_output_); SetLimits(err_item_sum_); } double Compute(double setpoint, double input) { uint64_t now = GetMillis(); uint64_t time_change = now - last_time_; if (time_change < sample_time_) { return last_output_; } double error = setpoint - input; printf("error: %f\n", error); err_item_sum_ += ki_ * error; SetLimits(err_item_sum_); double derivative = input - last_input_; double output = kp_ * error + err_item_sum_ - kd_ * derivative; SetLimits(output); last_input_ = input; last_time_ = now; last_output_ = output; return output; }
private:double kp_;double ki_;double kd_;
double last_input_ = 0.0; double last_output_ = 0.0; double err_item_sum_ = 0.0; double out_min_ = 0.0; double out_max_ = 0.0; uint64_t last_time_ = 0UL; uint64_t sample_time_ = 1000UL; // 1 second uint64_t GetMillis() { return std::chrono::duration_cast<std::chrono::milliseconds>( std::chrono::steady_clock::now().time_since_epoch()) .count(); } void SetLimits(double& val) { if (val > out_max_) { printf("val: %f > out_max_: %f\n", val, out_max_); val = out_max_; } else if (val < out_min_) { printf("val: %f > out_min_: %f\n", val, out_min_); val = out_min_; } else { ; // Do nothing } }
};
int main() {PIDController pid;pid.set_tunings(1, 0.5, 0.05);pid.set_sample_time(1000);pid.set_output_limits(0, 100);
double setpoint = 90; double temperature = 20; std::this_thread::sleep_for(std::chrono::seconds(1)); for (int i = 0; i < 1000; ++i) { double control_signal = pid.Compute(setpoint, temperature); temperature += control_signal * 0.1; temperature *= 0.99; std::cout << "Temperature: " << temperature << std::endl; std::this_thread::sleep_for(std::chrono::milliseconds(200)); } return 0;
}
添加开关控制
好的 PID 算法应允许使用者动态启停，比如恒温水池运行过程中，由于某种原因，管理人员需要停掉自动控制，改为手动控制，操作结束后，重新启动自动控制。实现动态停止并不复杂，只要 PID 内部加一个开关标识，当关闭时，PID 算法内部不执行计算，外部直接使用人工操作值替代算法输出值进行控制。但问题的关键是，当从手动模式重新改为自动模式时，需要保证恒温水池温度不出现大的抖动，即 PID 算法能接续人类的控制状态，实现平滑过渡。解决办法是重新初始化：当从手动切换到自动时，将水池温度和人工操作值传给 PID ，更新 PID 内部的历史输入值和历史积分值。如此一来，当 PID 重新启动时，就能接续人类的控制结果，平滑启动，如图所示。
#include <iostream>#include <chrono>#include <thread>
enum PID_MODE: uint8_t {PID_MODE_MANUAL = 0,PID_MODE_AUTOMATIC = 1};
class PIDController {public:explicit PIDController() {InitTime();}
PIDController(double kp_para, double ki_para, double kd_para) : kp_(kp_para), ki_(ki_para), kd_(kd_para) { InitTime(); } void InitTime() { last_time_ = GetMillis(); } void set_tunings(double kp_para, double ki_para, double kd_para) { double sample_time_in_sec = static_cast<double>(sample_time_) / 1000.0; kp_ = kp_para; ki_ = ki_para * sample_time_in_sec; kd_ = kd_para / sample_time_in_sec; } void set_sample_time(uint64_t new_sample_time) { if (new_sample_time > 0) { double ratio = static_cast<double>(new_sample_time) / static_cast<double>(sample_time_); ki_ = ki_ * ratio; kd_ = kd_ / ratio; sample_time_ = new_sample_time; } } void set_output_limits(double min, double max) { if (min > max) { return; } out_min_ = min; out_max_ = max; SetLimits(last_output_); SetLimits(err_item_sum_); } void InitInnaState(double input, double output) { last_input_ = input; err_item_sum_ = output; SetLimits(err_item_sum_); } void set_auto_mode(PID_MODE mode, double input = 0.0, double output = 0.0) { bool new_auto = (mode == PID_MODE_AUTOMATIC); if (new_auto == true && in_auto_ == false) { InitInnaState(input, output); } in_auto_ = new_auto; std::cout << "PID mode: " << (in_auto_ ? "Automatic" : "Manual") << std::endl; } double Compute(double setpoint, double input) { if (in_auto_ == false) { return last_output_; } uint64_t now = GetMillis(); uint64_t time_change = now - last_time_; if (time_change < sample_time_) { return last_output_; } double error = setpoint - input; printf("error: %f\n", error); err_item_sum_ += ki_ * error; SetLimits(err_item_sum_); double derivative = input - last_input_; double output = kp_ * error + err_item_sum_ - kd_ * derivative; SetLimits(output); last_input_ = input; last_time_ = now; last_output_ = output; return output; }
private:double kp_;double ki_;double kd_;
double last_input_ = 0.0; double last_output_ = 0.0; double err_item_sum_ = 0.0; double out_min_ = 0.0; double out_max_ = 0.0; uint64_t last_time_ = 0UL; uint64_t sample_time_ = 1000UL; // 1 second // PID 内部状态控制量：false 表示手动模式，true 表示自动模式 bool in_auto_ = false; uint64_t GetMillis() { return std::chrono::duration_cast<std::chrono::milliseconds>( std::chrono::steady_clock::now().time_since_epoch()) .count(); } void SetLimits(double& val) { if (val > out_max_) { printf("val: %f > out_max_: %f\n", val, out_max_); val = out_max_; } else if (val < out_min_) { val = out_min_; } else { ; // Do nothing } }
};
int main() {PIDController pid;pid.set_tunings(1, 0.2, 0.02);pid.set_sample_time(1000);pid.set_output_limits(0, 100);
double setpoint = 36.0; double temperature = 20.0; std::this_thread::sleep_for(std::chrono::seconds(1)); pid.set_auto_mode(PID_MODE_AUTOMATIC); for (int i = 0; i < 1000; ++i) { if (i == 200) { pid.set_auto_mode(PID_MODE_MANUAL); std::cout << "---->>> Switch to manual mode" << std::endl; } double control_signal = pid.Compute(setpoint, temperature); if (i >= 200 && i < 250) { control_signal = 3; } if (i >= 250 && i <= 300) { control_signal = 4; } std::cout << "--> Control signal: " << control_signal << std::endl; temperature += control_signal * 0.1; temperature *= 0.99; std::cout << "<-- Temperature: " << temperature << std::endl; if (i == 300) { pid.set_auto_mode(PID_MODE_AUTOMATIC, temperature, control_signal); std::cout << "---->>> Switch back to automatic mode" << std::endl; } std::this_thread::sleep_for(std::chrono::milliseconds(200)); } return 0;
}
