Ros2 实时编程实践第三章 -- PID 控制原理与编码实现

1 前言和资料

本文是 Ros2 实时编程实践系列第三章，我们学习经典控制理论之 PID 控制算法，并着重讲解该算法的编码实现，为实现倒立摆样例做准备。掌握了 PID ，就相当于进入了控制工程的大门，也能为更高阶的控制理论学习打下基础。
本文参考资料如下：
（1）improving-the-beginners-pid-introduction
（2）Arduino-PID-Library
（3）初识PID算法
（4）啥是PID？ PID可以吃吗？
（5）PID算法的理解
（6）ArduPID-Library
（7）Arduino-PID-AutoTune-Library/

2 正文

2.1 PID 深入理解

（1）闭环控制系统：讲解 PID 之前，我们先解释什么是闭环控制系统。简单说就是一个有输入有输出的系统，输入能影响输出。一般情况下，人们也称输出为反馈，因此也叫闭环反馈控制系统。比如恒温水池，输入就是加热功率，输出就是水温度；比如冷库，输入是空调功率，输出是内部温度。
（2）什么是 PID：英文分解开就是：比例（proportional）、积分（integral）、微分（derivative），其根据系统反馈，通过比例，积分和微分三个部分的计算，动态调整系统输入，确保被控量稳定在人们设定的目标值附近。PID 是目前最常见的应用于闭环反馈控制系统的算法，三个部分可以只用一个（P，I，D），也可以只用两个（PI，PD），也可以三个一起用（PID），非常灵活。
（3）PID 控制原理图和数学表达式：

上面的控制原理图与下面的数学表达式是相互对应的。
$$\begin{gathered} input(t)=Kp\ast e(t)+Ki\ast {\int }_{}e(t)dt+Kd\ast \frac{d}{dt}e(t) \\ e(t)=setpoint-output(t) \end{gathered}$$
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) 对时间的导数。通过数学计算，可得导数为水池温度的斜率负数：
$$\frac{d}{dt}e(t)=\frac{d}{dt}(setpoint-output(t))=-\frac{d}{dt}output(t)=-(output(n)-output(n-1))/\Delta (t)$$
根据求导结果，我们分两种情况讨论微分部分对比例部分的作用：
当差值 e(t) 扩大时：微分部分将与比例部分同正负号，对比例部分进行补偿，更好的抑制差值扩大；
当差值 e(t) 缩小时：微分部分将与比例部分异号，对比例部分进行抵消，防止系统输出过冲。
综合两种情况，可以认为微分部分提供了一种预测性的调控作用，通过考虑差值 e(t) 的未来走势，更精细地调整系统输入，从而让系统输出逐渐收敛到目标值。
积分部分：只有比例和微分部分，在某些场景下会失灵。举例而言，假如我们只使用 PD 算法。此时水池的室外温度非常低，热量散失非常快。当加热到某个温度的时候（比如 30 度），温度可能再也无法上涨。这种情况，称之为系统的稳态误差。我们分两部分解释原因：
比例部分：由于差值 e(t) 不那么大了，比例部分会比较小，每次增加的热量正好被耗散掉，因此温度不会继续上升；
微分部分：由于温度基本恒定，微分部分将约为零，也无法对比例部分进行补偿。
解决办法是引入差值 e(t) 的积分，也就是 e(t) 乘以单位时间并不断累加，数学表达式如下：
$${\int }_{}e(t)dt=\sum (setpoint-output(t))\ast 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

2.2 PID 编码实现

这部分我们主要参考 Arduino 的 PID 库 Arduino-PID-Library，分八步实现一个实际可用的 PID 算法库。由于每一步都依赖前一步的知识，请读者逐步阅读。
特别提示：由于本节讲解 PID 的实现，我们将以 PID 作为第一视角，如果提到 input ，指的是 PID 算法输入，相当于上节中的系统输出 output(t)，即恒温水池的温度；如果提到 ouput，指的是 PID 算法输出，相当于上节中的系统输入 input(t)，即加热功率。

2.2.1 初始版本

代码实现 PID 算法，面临最大的困惑是如何实现积分和微分。正如上一节所说，积分可转化为差值 e(t) 乘以采样间隔并不断累加；微分可转换为求两次采样的差值 e(t) 的斜率。于是有了如下代码，请读者关注代码注释（可以直接拿去跑）。

#include <iostream>
#include <chrono>
#include <thread>

class PIDController {
public:
    explicit PIDController() {
        InitTime();
    }

    // 构造函数，初始化 PID 控制器的三个参数和系统初始时间
    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();
    }

    // Compute 是 PID 控制器的核心函数，根据设定值和算法输入值计算输出值
    double Compute(double setpoint, double input) {
        uint64_t now = GetMillis();
        // 两次采样间隔
        double time_change = static_cast<double>(now - last_time_);

        // 差值 error
        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;
    }

    // 设置 PID 控制器的三个参数
    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; // 上一次的差值 error
    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);

    // 假设我们控制的是一个恒温水池，我们希望将温度控制在36度，初始温度为20度
    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);

        // 模拟锅炉加热，假设加热器效率为0.1，温度会损失0.01
        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;
}

2.2.2 固定采样间隔

初始版本的 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();
    }

    // 设置 PID 控制器的三个参数，由于固定了采样时间，因此可以提前计算好 ki 和 kd，而不必每次在 Compute 函数中计算
    void set_tunings(double kp_para, double ki_para, double kd_para) {
        // 采样间隔，单位是毫秒，这里转换为秒。默认采样间隔是 1 秒
        double sample_time_in_sec = static_cast<double>(sample_time_) / 1000.0;
        kp_ = kp_para;
        // 由于积分部分是每次差值 e 乘以采样间隔，然后累加，因此这里提前将 ki 乘以固定的采样间隔，并保存起来，数学表达式如下：
        // 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;
        // 由于微分部分是每次差值 e 除以采样间隔，因此这里提前将 kd 除以固定的采样间隔，并保存起来，数学表达式如下：
        // derivative = kd * (error(n) - error(n-1)) / dt = (kd / dt) * (error(n) - error(n-1))
        kd_ = kd_para / sample_time_in_sec;
    }

    // 设置采样间隔，单位是毫秒
    // 当设置新的采样间隔时，需要按比例重新更新 ki 和 kd，并保存新的采样间隔
    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);

        // 积分部分
        // 由于采样间隔是固定的，且提前计算并保存到了 ki_ 中，因此这里只需要累加即可
        err_sum_ += error;

        // 微分部分
        // 由于采样间隔是固定的，且提前计算并保存到了 kd_ 中，因此这里只需要计算差值即可
        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;
    // 默认采样间隔是 1 秒
    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

    // 假设我们控制的是一个恒温水池，我们希望将温度控制在36度，初始温度为20度
    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);

        // 模拟锅炉加热，假设加热器效率为0.1，温度会损失0.01
        temperature += control_signal * 0.1;
        temperature *= 0.99;

        std::cout << "Temperature: " << temperature << std::endl;

        // 由于内部采样间隔固定为 1 秒，因此这里加快了计算周期
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }

    return 0;
}

2.2.3 消除 spike

spike 的英文含义是尖刺，这里指的是当系统运行过程中，突然改变 setpoint 时， PID 的微分部分会因 setpoint 的突然切换而生成一个极大的导数，导致算法输出值 output 将产生一次急剧变化，这就是 spike。比如恒温水池的初始 setpoint 是 36 度，运行过程中，突然改为 50 度。相当于在一个采样周期内，差值 error 突然增加了 14 ，再除以采样周期，数值将会非常大，如下图所示。

解决办法是将 setpoint 从 PID 的微分部分请出去，理论依据是：差值 error 的导数也是算法输入（恒温水池的温度）的斜率负数：
$$Kd\ast \frac{d}{dt}error=Kd\ast \frac{d}{dt}(setpoint-input)=Kd\ast (-\frac{d}{dt}input)=(-Kd/\Delta (t))(input(n)-input(n-1))$$
代码实现如下（看注释部分）

#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;

        // 由于差值 error 的导数也是算法输入的斜率负数，因此这里直接使用算法输入 input 计算微分部分
        double derivative = input - last_input_;
        // 由于差值 error 的导数也是算法输入的斜率负数，因此微分部分的符号是负数
        double output = kp_ * error + ki_ * err_sum_ - kd_ * derivative;

        // 由于差值 error 不再参与微分部分的计算，而是直接使用算法输入 input，因此这里不再保存 error，而是保存 input
        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);

    // 假设我们控制的是一个恒温水池，我们希望将温度控制在36度，初始温度为20度
    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);

        // 模拟锅炉加热，假设加热器效率为0.1，温度会损失0.01
        temperature += control_signal * 0.1;
        temperature *= 0.99;

        std::cout << "Temperature: " << temperature << std::endl;

        // 系统运行过程中，突然将目标温度从 36 度调整到 50 度
        if (i == 200) {
            setpoint = 50; 
            std::cout << "Setpoint changed to 50" << std::endl;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }

    return 0;
}

2.2.4 动态改参

好的 PID 算法，允许在系统运行过程中，调整 PID 参数。问题的关键是，运行中途修改 PID 参数，如何保持算法输出仍然平稳，对系统状态不产生额外冲击。
仔细分析 PID 的三个部分，当对应的参数改变时，影响最大的是积分部分，比例和微分两部分都只影响当前值，而积分部分将会更改历史值。
$$output=Kp\ast error+Ki\ast {\int }_{}errordt+Kd\ast \frac{d}{dt}error$$
解决办法是放弃先计算积分和，最后乘以积分系数的做法，而是让积分系数参与每一次积分运算并累加起来：
$$Ki\ast {\int }_{}errordt=Ki\ast \sum (setpoint-input)\ast t=\sum ((Ki\ast t)\ast (setpoint-input))$$
如此一来，即使更新了积分参数，也只影响当前值，历史值由于被存储起来，因此不会改变，代码实现如下（看注释部分）。

#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);

        // 改变积分部分的计算方式，直接累加 ki * error
        err_item_sum_ += ki_ * error;

        double derivative = input - last_input_;

        // 积分部分改为累加 ki * error，这里直接使用累加值
        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;
    // 由于积分部分改为累加 ki * error，因此不再保存 error 和（err_sum_），而是保存 ki * error 的和（err_item_sum_）
    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);

    // 假设我们控制的是一个恒温水池，我们希望将温度控制在36度，初始温度为20度
    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);

        // 模拟锅炉加热，假设加热器效率为0.1，温度会损失0.01
        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;
}

2.2.5 设置算法输出限制

通常情况下，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;
        }
    }

    // 设置 PID 算法输出的最小值和最大值
    // 当最小值大于最大值时，视为参数错误，不设置
    void set_output_limits(double min, double max) {
        if (min > max) {
            return;
        }
        out_min_ = min;
        out_max_ = max;

        // 将缓存值 last_output_ 和 err_item_sum_ 限制在最大值和最小值之间
        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_;

        // PID 输出计算，并限制在最大值和最小值之间
        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;

    // PID 算法输出的最小值和最大值
    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 算法输出（加热功率）的最小值为0，最大值为100
    pid.set_output_limits(0, 100);

    // 假设我们控制的是一个恒温水池，我们希望将温度控制在 90 度，初始温度为20度
    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);

        // 模拟锅炉加热，假设加热器效率为0.1，温度会损失0.01
        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;
}

2.2.6 添加开关控制

好的 PID 算法应允许使用者动态启停，比如恒温水池运行过程中，由于某种原因，管理人员需要停掉自动控制，改为手动控制，操作结束后，重新启动自动控制。
实现动态停止并不复杂，只要 PID 内部加一个开关标识，当关闭时，PID 算法内部不执行计算，外部直接使用人工操作值替代算法输出值进行控制。
但问题的关键是，当从手动模式重新改为自动模式时，需要保证恒温水池温度不出现大的抖动，即 PID 算法能接续人类的控制状态，实现平滑过渡。
解决办法是重新初始化：当从手动切换到自动时，将水池温度和人工操作值传给 PID ，更新 PID 内部的历史输入值和历史积分值。如此一来，当 PID 重新启动时，就能接续人类的控制结果，平滑启动，如图所示。

代码实现如下（看注释部分）

#include <iostream>
#include <chrono>
#include <thread>

// PID 工具模式枚举量：手动模式和自动模式
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_);
    }

    // 当从手动模式切换到自动模式时，重新初始化 PID 内部状态
    // 一是更新算法输入值，确保比例和微分部分按照新的状态重新计算
    // 二是更新算法积分部分的历史值，确保积分部分不会对新的算法输出产生扰动
    void InitInnaState(double input, double output) {
        last_input_ = input;
        err_item_sum_ = output;
        SetLimits(err_item_sum_);
    }

    // 设置 PID 控制器的工作模式：手动模式和自动模式
    // 当从手动模式切换到自动模式时，需要给出新的算法输入值和输出值，用于初始化 PID 内部状态
    void set_auto_mode(PID_MODE mode, double input = 0.0, double output = 0.0) {
        // 当识别出模式从手动切换到自动时，初始化 PID 内部状态
        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) {
        // 当 PID 控制器处于手动模式时，直接返回上一次的输出值，外部会使用人工操作值覆盖 PID 算法的输出值
        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);

    // 假设我们控制的是一个恒温水池，我们希望将温度控制在 36 度，初始温度为20度
    double setpoint = 36.0;
    double temperature = 20.0;

    std::this_thread::sleep_for(std::chrono::seconds(1));

    // 初始化时，设置 PID 控制器为自动模式
    pid.set_auto_mode(PID_MODE_AUTOMATIC);

    for (int i = 0; i < 1000; ++i) {
        // 当 i 等于 200 时，将 PID 控制器切换到手动模式
        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);

        // 切换到手动模式时，这里模拟人工的操作，人工操作值将覆盖 PID 算法的输出值
        if (i >= 200 && i < 250) {
            control_signal = 3;
        }
        if (i >= 250 && i <= 300) {
            control_signal = 4;
        }

        std::cout << "--> Control signal: " << control_signal << std::endl;

        // 模拟锅炉加热，假设加热器效率为0.1，温度会损失0.01
        temperature += control_signal * 0.1;
        temperature *= 0.99;

        std::cout << "<-- Temperature: " << temperature << std::endl;

        // 当 i 等于 300 时，将 PID 控制器重新切换到自动模式
        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;
}

2.2.7 添加正反向控制

本文中，我们一直拿恒温水池举例，在这里例子中，PID 的输入与输出值是正相关的，即加热功率增加，温度会增加。但现实中，也有很多负相关的例子，比如冷库，制冷功率就与温度负相关，功率越大，温度下降越快。
正常情况下，我们要求 PID 的三个系数必须一致。如果是正相关的情况，三个系数同时为正即可；如果是负相关的情况，三个系数同时为负即可。因此，我们需要为 PID 添加正反向控制。
代码实现如下（看注释部分）

#include <iostream>
#include <chrono>
#include <thread>

// 添加命名空间 YCAO_PIDLIB
namespace YCAO_PIDLIB {

enum PID_MODE: uint8_t {
    PID_MODE_MANUAL = 0,
    PID_MODE_AUTOMATIC = 1
};

// PID 方向枚举量：正向和反向
enum PID_DIRECTION: uint8_t {
    PID_DIRECT = 0,
    PID_REVERSE = 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();
    }

    // 设置 PID 控制器的三个参数，我们要求三个系数都大于等于0，但运行通过方向参数设置正向和反向的控制
    void set_tunings(double kp_para, double ki_para, double kd_para, PID_DIRECTION direction = PID_DIRECT) {
        if (kp_para < 0.0 || ki_para < 0.0 || kd_para < 0.0) {
            return;
        }

        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;

        // 如果方向参数是反向，那么三个系数都取反，即均为负值
        if (pid_direct_ == PID_REVERSE) {
            kp_ = 0 - kp_;
            ki_ = 0 - ki_;
            kd_ = 0 - kd_;
        }
    }

    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_ = last_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

    bool in_auto_ = false;

    PID_DIRECTION pid_direct_ = PID_DIRECT;

    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() {
    YCAO_PIDLIB::PIDController pid;
    // 设置 PID 的三个参数都是负值，即反向控制
    pid.set_tunings(1.0, 0.2, 0.02, YCAO_PIDLIB::PID_REVERSE);
    pid.set_sample_time(1000);
    pid.set_output_limits(-100.0, 0.0);

    // 假设我们控制的是一个冷库，目标是将温度控制在 -26 度，初始温度是 30 度
    double setpoint = -26.0;
    double temperature = 30.0;

    std::this_thread::sleep_for(std::chrono::seconds(1));

    pid.set_auto_mode(YCAO_PIDLIB::PID_MODE_AUTOMATIC);

    for (int i = 0; i < 1000; ++i) {
        double control_signal = pid.Compute(setpoint, temperature);

        // 模拟压缩机的控制，假设压缩机的热效率是 0.1，温度会损失 0.01
        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;
}

2.2.8 不依赖差值的比例部分

前面七步，我们按照标准的 PID 数学原理，实现了一个几乎完美的 PID 算法，这种实现我们姑且称之为 PID 算法的标准版本。但是，标准版本有一个问题：控制过程中，被控量（水池温度）很难不超过设定值，尽管最后会收敛到设定值。在绝大多数情况下，这都没有问题，但有些情况下，人们就是希望被控量平滑的逐步靠近设定值，而不要超过设定值，如图所示。

标准版本的 PID 算法之所以会出现这个问题，关键就是 PID 的积分部分。当被控量首次达到设定值时，比例部分就会失效，而积分部分却累加了大量的积分值，成为曲线继续冲高的“元凶”。只要当积分部分自己内部正负累加结果为零时，积分部分才会失效。
解决的办法是给积分部分找一个“帮手”，这个帮手能在被控量首次接近设定值时，主动抵消积分部分，让曲线失去冲高的动力。按理说，微分部分作为 PID 的阻尼角色，天然适合做这个帮手。但是实践证明，只有微分部分提供阻力，是不够的。标准版本的 PID 算法很难调出这样的参数，实现上图中的效果。
于是，我们要改造比例部分，让比例部分成为微分部分的帮手。具体做法是让比例部分不再依赖差值进行计算（Proportional on Error(PonE)），改为根据算法输入的初始值进行计算（Proportional on Measurement(PonM)），数学表达式如下：
$$\begin{gathered} Proportional=Kp\ast error=Kp\ast (setpoint-input) \\ Proportional=-Kp\ast (input-inpu{t}_{init}) \end{gathered}$$
进一步推导可得：
$$\begin{gathered} Proportional=-Kp\ast (input-inpu{t}_{init}) \\ =-Kp\ast [(inpu{t}_n-inpu{t}_{n-1})+(inpu{t}_{n-1}-inpu{t}_{n-2})+...+(inpu{t}_1-inpu{t}_{init})] \\ =-Kp\ast (inpu{t}_n-inpu{t}_{n-1})-Kp\ast (inpu{t}_{n-1}-inpu{t}_{n-2})-...-Kp\ast (inpu{t}_1-inpu{t}_{init}) \end{gathered}$$
根据上面的推理结果，我们可以在 PID 算法中加一个缓存量，存储比例部分累加值。由于积分部分已经有了一个累加值，因此我们可以把两个值合在一块，这样 P-I-D 就变成了 PI-D。而合起来的 PI 部分，在被控量首次接近设定值时，就可以实现自平衡，实现上图中的控制效果。
代码实现如下（看注释部分）

#include <iostream>
#include <chrono>
#include <thread>

namespace YCAO_PIDLIB {

enum PID_MODE: uint8_t {
    PID_MODE_MANUAL = 0,
    PID_MODE_AUTOMATIC = 1
};

enum PID_DIRECTION: uint8_t {
    PID_DIRECT = 0,
    PID_REVERSE = 1
};

// PID 比例部分的两种模式，一是以测例为基础，二是以差值 error 为基础
// Proportional on Measurement(PonM)
// Proportional on Error(PonE)
enum PID_P_MODE: uint8_t {
    PID_P_ON_M = 0,
    PID_P_ON_E = 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();
    }

    // 添加一个参数，用于设置 PID 比例部分的模式，默认是PID_P_ON_E
    void set_tunings(double kp_para, double ki_para, double kd_para, PID_DIRECTION direction = PID_DIRECT, PID_P_MODE p_mode = PID_P_ON_E) {
        if (kp_para < 0.0 || ki_para < 0.0 || kd_para < 0.0) {
            return;
        }

        // 设置 PID 比例部分的模式
        p_on_e_ = (p_mode == PID_P_ON_E);

        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;

        if (pid_direct_ == PID_REVERSE) {
            kp_ = 0 - kp_;
            ki_ = 0 - ki_;
            kd_ = 0 - kd_;
        }
    }

    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(p_i_item_sum_);
    }

    void InitInnaState(double input, double output) {
        last_input_ = input;
        // 初始化时，将比例和积分两个部分的累加值设置为上一次的输出值
        p_i_item_sum_ = last_output_;
        SetLimits(p_i_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);

        double derivative = input - last_input_;

        p_i_item_sum_ += ki_ * error;
        // 当比例部分的模式是 PonM 时，将比例部分与积分部分的累加值合并
        if (p_on_e_ == false) {
            p_i_item_sum_ -= kp_ * derivative;
        }
        SetLimits(p_i_item_sum_);

        double output = 0.0;
        if (p_on_e_ == true) {
            output = kp_ * error + p_i_item_sum_ - kd_ * derivative;
        } else {
            // 当比例部分的模式是 PonM 时，P-I-D 将是 PI-D，即比例和积分部分合并 
            output = p_i_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 out_min_ = 0.0;
    double out_max_ = 0.0;

    uint64_t last_time_ = 0UL;
    uint64_t sample_time_ = 1000UL; // 1 second

    bool in_auto_ = false;

    PID_DIRECTION pid_direct_ = PID_DIRECT;

    // 标识 PID 比例部分的模式，true 表示 Proportional on Error(PonE)，false 表示 Proportional on Measurement(PonM)
    bool p_on_e_ = true;
    // 比例和积分两个部分的累加值
    double p_i_item_sum_ = 0.0;

    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() {
    YCAO_PIDLIB::PIDController pid;
    // 测试 PonM 模式
    // 这个参数的运行结果就是被控量（水池温度）平滑的上升到设定值，不会出现超调
    pid.set_tunings(0.5, 0.05, 0.0, YCAO_PIDLIB::PID_DIRECT, YCAO_PIDLIB::PID_P_ON_M);
    pid.set_sample_time(1000);
    pid.set_output_limits(0.0, 100.0);

    // 假设我们控制的是一个恒温水池，我们希望将温度控制在 36 度，初始温度为20度
    double setpoint = 36.0;
    double temperature = 20.0;

    std::this_thread::sleep_for(std::chrono::seconds(1));

    pid.set_auto_mode(YCAO_PIDLIB::PID_MODE_AUTOMATIC, temperature);

    for (int i = 0; i < 1000; ++i) {
        double control_signal = pid.Compute(setpoint, temperature);

        // 模拟锅炉加热，假设加热器效率为0.1，温度会损失0.01
        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;
}

3 总结

更高阶的 Arduino PID 算法库，读者可以自行研究：ArduPID-Library，Arduino-PID-AutoTune-Library/。
本文的代码托管在我的 github 上，每一个都可以独立运行。另外，我也写了一个简单的脚本，帮助大家快速测试，查看 README 即可，github 链接：pid_demo。