连续PID的离散化及代码实现

1. 公式

比例项可以让系统尽快达到一个小的误差范围，积分项对比例项作用产生的误差进行积分，也就是放大产生的误差，让其更快达到目标，但这时会导致系统过冲量较大，微分项的引入会抑制过冲。

作用：

• P：加快调节速度
• I：减小误差，从而消除静差
• D：改善系统的动态性能

进行离散化处理

假设系统采样时间为$△t$，$e(t)$将序列化为：

将输出$u(t)$序列化得到：

• 比例项：
  

• 积分项：
  

• 微分项：
  
  因此，连续PID式变为：
  
  上式即为位置式PID。

实际工程中，常用增量式PID：

即：

---

2. 算法伪代码

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

//循环 
//采样周期为dt
loop:
 //setpoint 设定值
 //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

---

3. 位置式PID算法的C++实现

头文件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.cpp

#ifndef _PID_SOURCE_
#define _PID_SOURCE_

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

测试代码

#include "pid.h"
#include 

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

4. 位置式PID算法Python版

class PositionalPID:
    def __init__(self, P, I, D):
        self.Kp = P
        self.Ki = I
        self.Kd = D

        self.SystemOutput = 0.0#系统输出值
        self.ResultValueBack = 0.0
        self.PidOutput = 0.0#PID控制器输出
        self.PIDErrADD = 0.0
        self.ErrBack = 0.0

    #设置一阶惯性环节系统  其中InertiaTime为惯性时间常数
    def SetInertiaTime(self, InertiaTime,SampleTime):
       self.SystemOutput = (InertiaTime * self.ResultValueBack + SampleTime * self.PidOutput) / (SampleTime + InertiaTime)
       self.ResultValueBack = self.SystemOutput

    #设置PID控制器参数
    def SetStepSignal(self,StepSignal):
        Err = StepSignal - self.SystemOutput
        KpWork = self.Kp * Err
        KiWork = self.Ki * self.PIDErrADD
        KdWork = self.Kd * (Err - self.ErrBack)
        self.PidOutput = KpWork + KiWork + KdWork
        self.PIDErrADD += Err

---

• 以上内容转自简易PID算法的快速扫盲（超详细+过程推导+C语言程序）
• 参考啥是PID？PID有什么用？PID可以让你控制你想控制的东西！
• 参考使用V-rep仿真（结合pythonAPI）实现机器人视觉巡线+pid调速