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;