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

PID控制器表达式为：

\[u(t) = K_pe(t) + K_i\int_0^t e(\tau)d\tau + K_d\frac{de(t)}{dt} \]

---

离散化:

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

\[e(t) = e(nT) \\\\ \int_0^t e(\tau)d\tau = \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(\tau)d\tau = \sum_{i=0}^n e(i) \\\\ \frac{de(t)}{dt} = e(n)-e(n-1) \]

所以:

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

---

增量化:

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

\[ \begin{aligned} \Delta u(n) &= \{K_pe(n) + K_i\sum_{i=0}^n e(i) + K_d[e(n)-e(n-1)]\} \\\\ &~~~~ - \{K_pe(n-1) + K_i\sum_{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} \]

所以:

\[ \begin{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];
}