簡易PID算法的快速掃盲

網(wǎng)上關(guān)于PID算法的文章很多，但是感覺有必要自己再進行一次總結(jié)，抽絲剝繭地重新認識了一下PID；

• 1 前言

• 2 開環(huán)控制

• 3 閉環(huán)控制

• 4 PID

• 4.1 系統(tǒng)架構(gòu)

• 4.2 理論基礎(chǔ)

• 4.3 離散化

• 4.4 偽算法

• 5 C++實現(xiàn)

• 6 總結(jié)

1 前言

控制系統(tǒng)通常根據(jù)有沒有反饋會分為開環(huán)系統(tǒng)和閉環(huán)系統(tǒng)，在閉環(huán)系統(tǒng)的控制中，PID算法非常強大，其三個部分分別為；

• P：積分環(huán)節(jié)；
• I：比例環(huán)節(jié)；
• D：微分環(huán)節(jié)；

PID算法可以自動對控制系統(tǒng)進行準確且迅速的校正，因此被廣泛地應(yīng)用于工業(yè)控制系統(tǒng)。

2 開環(huán)控制

首先來看開環(huán)控制系統(tǒng)，如下圖所示，隆哥蒙著眼，需要走到虛線旗幟所表示的目標位置，由于缺少反饋（眼睛可以感知當前距離和位置，由于眼睛被蒙上沒有反饋，所以這也是一個開環(huán)系統(tǒng)），最終隆哥會較大概率偏離預(yù)期的目標，可能會運行到途中實線旗幟所表示的位置。

開環(huán)系統(tǒng)的整體結(jié)構(gòu)如下所示；

這里做一個不是很恰當?shù)谋扔鳎?/p>

• Input：告訴隆哥目標距離的直線位置（ 10米）；
• Controller：隆哥大腦中計算出到達目標所需要 走多少步；
• Process：雙腿作為執(zhí)行機構(gòu)，輸出了相應(yīng)的步數(shù)，但是最終仍然偏離了目標；

看來沒有反饋的存在，很難準確到達目標位置。

3 閉環(huán)控制

所以為了準確到達目標位置，這里就需要引入反饋，具體如下圖所示；

在這里繼續(xù)舉個不怎么恰當?shù)谋扔鳎宦「缰孬@光明之后，基本可以看到目標位置了；

• 第一步 Input：告訴隆哥目標距離的直線位置（ 10米）；
• 第二步 Controller：隆哥大腦中計算出到達目標所需要 走多少步；
• 第三步 Process：雙腿作為執(zhí)行機構(gòu)，輸出了相應(yīng)的步數(shù)，但是最終仍然偏離了目標；
• 第四步 Feedback： 通過視覺獲取到目前已經(jīng)前進的距離，（比如 前進了2米，那么還有 8米的偏差）；
• 第五步 err：根據(jù) 偏差重新計算所需要的步數(shù)，然后重復(fù)上述四個步驟，最終隆哥達到最終的目標位置。
  
  

4 PID

4.1 系統(tǒng)架構(gòu)

雖然在反饋系統(tǒng)下，隆哥最終到達目標位置，但是現(xiàn)在又來了新的任務(wù)，就是又快又準地到達目標位置。所以這里隆哥開始采用PID Controller，只要適當調(diào)整P，I和D的參數(shù)，就可以到達目標位置，具體如下圖所示；

隆哥為了最短時間內(nèi)到達目標位置，進行了不斷的嘗試，分別出現(xiàn)了以下幾種情況；

• 跑得太快，最終導(dǎo)致沖過了目標位置還得往回跑；
• 跑得太慢，最終導(dǎo)致到達目標位置所用時間太長；

經(jīng)過不斷的嘗試，終于找到了最佳的方式，其過程大概如下圖所示；這里依然舉一個不是很恰當?shù)谋扔鳎?/p>

• 第一步：得到與目標位置的距離偏差（比如最開始是 10米，后面會逐漸變小）；
• 第二步：根據(jù)誤差，預(yù)估需要多少速度，如何估算呢，看下面幾步；

P比例則是給定一個速度的大致范圍，滿足下面這個公式；

因此比例作用相當于某一時刻的偏差（err）與比例系數(shù) 的乘積，具體如下所示；

綠色線為上述例子中從初始位置到目標位置的距離變化；紅色線為上述例子中從初始位置到目標位置的偏差變化，兩者為互補的關(guān)系；

I積分則是誤差在一定時間內(nèi)的和，滿足以下公式；

如下圖所示；

紅色曲線陰影部分面積即為積分作用的結(jié)果，其不斷累積的誤差，最終乘以積分系數(shù) 就得到了積分部分的輸出；

D微分則是誤差變化曲線某處的導(dǎo)數(shù)，或者說是某一點的斜率，因此這里需要引入微分；

從圖中可知，當偏差變化過快，微分環(huán)節(jié)會輸出較大的負數(shù)，作為抑制輸出繼續(xù)上升，從而抑制過沖。

綜上， ，分別增加其中一項參數(shù)會對系統(tǒng)造成的影響總結(jié)如下表所示；

4.2 理論基礎(chǔ)

上面扯了這么多，無非是為了初步理解PID在負反饋系統(tǒng)中的調(diào)節(jié)作用，下面開始推導(dǎo)一下算法實現(xiàn)的具體過程；PID控制器的系統(tǒng)框圖如下所示；

因此不難得出輸入 和輸出 的關(guān)系；

是比例增益； 是積分增益； 是微分增益；

4.3 離散化

在數(shù)字系統(tǒng)中進行PID算法控制，需要對上述算法進行離散化；假設(shè)系統(tǒng)采樣時間為 則將輸入 序列化得到；

將輸出 序列化得到；

• 比例項：
• 積分項：
• 微分項：

所以最終可以得到式①，也就是網(wǎng)上所說的位置式PID：

將式①再做一下簡化；

最終得到增量式PID的離散公式如下：

4.4 偽算法

這里簡單總結(jié)一下增量式PID實現(xiàn)的偽算法；

previous_error := 0  //上一次偏差
integral := 0   //積分和

//循環(huán) 
//采樣周期為dt
loop:
 //setpoint 設(shè)定值
 //measured_value 反饋值
    error := setpoint ? measured_value //計算得到偏差
    integral := integral + error × dt //計算得到積分累加和
    derivative := (error ? previous_error) / dt //計算得到微分
    output := Kp × error + Ki × integral + Kd × derivative //計算得到PID輸出
    previous_error := error //保存當前偏差為下一次采樣時所需要的歷史偏差
    wait(dt) //等待下一次采用
    goto loop

5 C++實現(xiàn)

這里是增量式PID算法的C語言實現(xiàn)；

pid.cpp

#ifndef _PID_SOURCE_
#define _PID_SOURCE_

#include <iostream>
#include <cmath>
#include "pid.h"

using namespace std;

class PIDImpl
{
    public:
        PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki );
        ~PIDImpl();
        double calculate( double setpoint, double pv );

    private:
        double _dt;
        double _max;
        double _min;
        double _Kp;
        double _Kd;
        double _Ki;
        double _pre_error;
        double _integral;
};


PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki )
{
    pimpl = new PIDImpl(dt,max,min,Kp,Kd,Ki);
}
double PID::calculate( double setpoint, double pv )
{
    return pimpl->calculate(setpoint,pv);
}
PID::~PID() 
{
    delete pimpl;
}


/**
 * Implementation
 */
PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ) :
    _dt(dt),
    _max(max),
    _min(min),
    _Kp(Kp),
    _Kd(Kd),
    _Ki(Ki),
    _pre_error(0),
    _integral(0)
{
}

double PIDImpl::calculate( double setpoint, double pv )
{
    
    // Calculate error
    double error = setpoint - pv;

    // Proportional term
    double Pout = _Kp * error;

    // Integral term
    _integral += error * _dt;
    double Iout = _Ki * _integral;

    // Derivative term
    double derivative = (error - _pre_error) / _dt;
    double Dout = _Kd * derivative;

    // Calculate total output
    double output = Pout + Iout + Dout;

    // Restrict to max/min
    if( output > _max )
        output = _max;
    else if( output < _min )
        output = _min;

    // Save error to previous error
    _pre_error = error;

    return output;
}

PIDImpl::~PIDImpl()
{
}

#endif

pid.h

#ifndef _PID_H_
#define _PID_H_

class PIDImpl;
class PID
{
    public:
        // Kp -  proportional gain
        // Ki -  Integral gain
        // Kd -  derivative gain
        // dt -  loop interval time
        // max - maximum value of manipulated variable
        // min - minimum value of manipulated variable
        PID( double dt, double max, double min, double Kp, double Kd, double Ki );

        // Returns the manipulated variable given a setpoint and current process value
        double calculate( double setpoint, double pv );
        ~PID();

    private:
        PIDImpl *pimpl;
};

#endif

pid_example.cpp

#include "pid.h"
#include <stdio.h>

int main() {

    PID pid = PID(0.1, 100, -100, 0.1, 0.01, 0.5);

    double val = 20;
    for (int i = 0; i < 100; i++) {
        double inc = pid.calculate(0, val);
        printf("val:% 7.3f inc:% 7.3f\n", val, inc);
        val += inc;
    }

    return 0;
}

編譯并測試；

g++ -c pid.cpp -o pid.o
# To compile example code:
g++ pid_example.cpp pid.o -o pid_example

6 總結(jié)

本文總結(jié)了PID控制器算法在閉環(huán)系統(tǒng)中根據(jù)偏差變化的具體調(diào)節(jié)作用，每個環(huán)節(jié)可能對系統(tǒng)輸出造成什么樣的變化，給出了位置式和增量式離散PID算法的推導(dǎo)過程，并給出了位置式算法的C++程序?qū)崿F(xiàn)。

由于作者能力和水平有限，文中難免存在錯誤和紕漏，請不吝賜教。