增量式PID推导及C语言实现

PID控制器表达式为：

[u(t) = K_pe(t) + K_iint_0^t e( au)d au + K_dfrac{de(t)}{dt} ]

---

离散化:

令 $ t = nT,~T$为采样周期。可得:

[e(t) = e(nT) \\ int_0^t e( au)d au = sum_{i=0}^{n-1} e(iT) \\ frac{de(t)}{dt} = frac{e(nT)-e[(n-1)T]}{T} ]

因为(T)是固定的，不妨设为1，可简化为:

[e(t) = e(n) \\ int_0^t e( au)d au = sum_{i=0}^n e(i) \\ frac{de(t)}{dt} = e(n)-e(n-1) ]

所以:

[u(n) = K_pe(n) + K_isum_{i=0}^n e(i) + K_d[e(n)-e(n-1)] ]

---

增量化:

令 (Delta u(n) = u(n) - u(n-1)) 可得:

[ egin{aligned} Delta u(n) &= {K_pe(n) + K_isum_{i=0}^n e(i) + K_d[e(n)-e(n-1)]} \\ &~~~~ - {K_pe(n-1) + K_isum_{i=0}^{n-1} e(i) + K_d[e(n-1)-e(n-2)]} \\ &= (K_p + K_i + K_d)e(n) - (K_p + 2K_d)e(n-1) + K_de(n-2) end{aligned} ]

所以:

[ egin{aligned} u(n) &= u(n-1) + Delta u(n) \\ Delta u(n) &= a_0e(n) + a_1e(n-1) + a_2e(n-2) \\ a_0 &= K_p + K_i + K_d \\ a_1 &= -(K_p + 2K_d) \\ a_2 &= K_d end{aligned} ]

---

C语言实现

dpid.h:

#ifndef __DPID_H__
#define __DPID_H__

typedef struct
{
    float Kp, Ki, Kd;
    float a[3];
    float e[3];
}dpid_t;

void dpid_init(dpid_t *pid, float Kp, float Ki, float Kd);
float dpid(dpid_t *pid, float e);

#endif

dpid.c:

#include    "dpid.h"

void dpid_init(dpid_t *pid, float Kp, float Ki, float Kd)
{
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;

    pid->a[0] = Kp + Ki + Kd;
    pid->a[1] = -(Kp + 2*Kd);
    pid->a[2] = Kd;

    pid->e[0] = pid->e[1] = pid->e[2] = 0;
}

float dpid(dpid_t *pid, float e)
{
    //! 更新误差
    pid->e[2] = pid->e[1];
    pid->e[1] = pid->e[0];
    pid->e[0] = e;

    return pid->a[0]*pid->e[0]
           + pid->a[1]*pid->e[1]
           + pid->a[2]*pid->e[2];
}