PID算法的实现

  PID 算法是一种应用及其广泛的控制方法。对于一个线性系统，PID 参数可以通过指定闭环极点的方法获得。在实际应用中，由于信号噪声，微分经常会带来不必要的干扰，因而，数字控制中经常会舍弃微分项。

 

  The PID equation in time domain is described as the following:

     y(t) = Kp* x(t) + Ki*Integrate(x(t)) + Kd * dx(t)                                  (1)

 

  The transfer function in s domain:

     F = Kp + Ki/s + Kd * s                                                                        (2)                               

 

   设采样时间为 Ts, 离散公式为：

     I(k) = I(k-1) + Ki*Ts * x(k)

     Y(k) = Kp * x(k) + I(k) + kd * (x(k) – x(k-1)) /Ts 

              = I(k) + (Kp + Kd/Ts) * x(k) – Kd/Ts * x(k-1)                                         (3)

 

  我发现很多人在使用所谓“增量式PID” , 其实我认为 (3) 更清晰。不过增量式PID很容易通过 (1) ，(2) 或者（3）得到。例如, 对 (1) 求导：

     dy = Kp * dx + Ki * x + Kd * ddx

  离散上式，可得增量式PID 迭代公式：

    (y(k) – y(k-1))/Ts = Kp*(x(k) – x(k-1))/Ts + Ki * x(k) + Kd*(x(k) + x(k-2) –2*x(k-1))/Ts^2

 

　 y(k) – y(k-1) = Kp*(x(k) – x(k-1)) + Ki * Ts* x(k) + Kd*(x(k) + x(k-2) –2*x(k-1)) /Ts

    = (Kp + Ki *Ts + Kd/Ts) * x(k) - (Kp –2*Kd/Ts) * x(k-1) – Kd/Ts *x(k-2)　　　　　    　(4)

 

Ｃ代码如下：

typedef struct PID

{

            float Kp;

            float Ki;

            float Kd;

            float x;

            float integrator;

} PID;

 

void PID_Initilize(PID* pid, float Kp, float Ki, float Kd, float Ts)

{

            pid->Kp = Kp * Ts;  

            pid->Ki = Ki;

            pid->Kd = Kd / Ts;

            pid->x = 0;

            pid->integrator->0;

}

 

float PID_Result(PID* pid, float x)

{

            float result;

             pid->integrator += Ki * x;

            result = pid->integrator + pid->Kp * x + pid->kd*(x - pid->x);          

            pid->x = x;

           return result;

}

 

上面的算法可以优化，例如， Ki*Ts, Kd/Ts 可以预先计算，使用 fixed-point 运算代替 floating-point 运算。这里给出一个使用 Analog Device DSP 的汇编程序：

PIController.h

 

.EXTERNAL             PI_Controller_Init_;

.EXTERNAL             PI_Controller_;

.EXTERNAL             Saturate_;

 

.MACRO PI_Controller_Init(%0);            

            I3 = ^%0; L3 = %%0; M3 = 1;

            call PI_Controller_Init_;

.ENDMACRO;

 

 

.MACRO PI_Controller(%0, %1, %2, %3);

            I3 = ^%0; L3 = 2; M3 = 1;

            my1 = %1; my0 = %2; ay0 = %3;

            call PI_Controller_;

.ENDMACRO;

 

.MACRO Saturate(%0);

            ay0 = %0;

            call Saturate_;

.ENDMACRO;

 

 

PIController.dsp

.MODULE/RAM/SEG=USER_PM1    PI_Controller_Module;   

 

#include <admc331.h>;      

#include <macro.H>;

#include <romutil.H>;

 

.ENTRY             PI_Controller_Init_;

.ENTRY             PI_Controller_;

.ENTRY             Saturate_;

 

 

{********************************************************************************

            Function:         PI Controller

           

            Inputs:            AR : I(K+1)

                                    my0: Ki

                                    my1: Kp      

                                    AY0: saturation level, positive

 

            Outputs :           AR : saturated result

{*******************************************************************************}

PI_Controller_Init_:

            ar = pass 0;

            DM(I3, M3) = ar;

            DM(I3, M3) = ar;

            rts;

           

PI_Controller_:

            ax0 = ar;       

           

            mr = ar * my1 (SS);            ! Yi(k+1) = Ki*I(k+1)

            if mv sat mr;

            sr = LSHIFT mr0 by -4 (LO);

            sr = sr or ASHIFT mr1 by -4 (HI);

            ay1 = dm(I3, M3);                 { I(k) }

            ar = sr0 + ay1; mr0 = ar;

            ay1 = dm(I3, M3);                 { I(k+1) }

            ar = sr1 + ay1 + C;            mr1 = ar;       

                       

            dm(I3, M3) = mr0;

            dm(I3, M3) = mr1;

           

            ar = ax0;

            mr = mr + ar * my0 (SS);    { Kp * I(k+1) }

            if mv sat mr;

           

            ar = mr1;

            call saturate_;

            rts;

 

 

{********************************************************************************

            Judge a variable whether saturated

           

            Inputs:             AR : value to be saturated

                                    AY0: saturation level, positive

 

            Outputs :           AR : saturated result

{*******************************************************************************}

Saturate_:

            af = pass ar;

            if lt jump negtv;

 

            af = ar - ay0;

            if GT ar = pass ay0;

            rts;

 

negtv:

            af = ar + ay0;

            if LT ar = - ay0;

 

            rts;

 

.ENDMOD;