第 7 章 数字 PID 及其算法
7.3 AS-UII 的 PID 算法的程序设计
7.3.3 AS-UII 的闭环特性
应用 PID 算法进行反馈控制。根据控制量，我们采用位置式算法。
1）位置式算法 P 公式如：U=KEi+Uo;
PI 公式如：U=K(Ei+T/Ti*ΣEj)+Uo;
PID 公式如：U=K(Ei+T/Ti*ΣEj+Td/T*(Ei-Ei-1))+Uo;
因为左右轮的特性不一样，可以对他们分别控制。
这里 Uo 是控制量的基准，这里是 50，电机期望的速度是 50，E 是实际速度与期望 速度的偏差，U 是控制量，将做为 motor（）的控制参数。
代码如下：
/*用 PID 控制算法实现的走直线控制,位置式算法*/
/*左右轮有相同的设定值，对误差进行调整；根据左右轮的误差再对右轮进行调整，
使左右轮尽量保持一致，从而走直线。*/
float K1=0.05;
float Ti1=15.0;
float Td1=0.1;
float K2=0.2;
float Ti2=15.0;
float Td2=0.1;
float T=0.5;
float velocity_1;
float velocity_2;
float u_1_i;/*控制量*/
float u_2_i;
float error_1_i,error_1_i1,error_1_i2;
float error_2_i,error_2_i1,error_2_i2;
void main() {
motor(0,50);
motor(0,50);
rotation(1); /*将左右轮脉冲记数清零*/
rotation(2);
resettime(); /*系统时间复位*/
error_1_i1=0.0;
error_2_i1=0.0;
error_1_i2=0.0;
error_2_i2=0.0;
while(1) {
if(seconds()>4.0) { stop();
break;
} wait(T);
/*计算 error_i*/
velocity_1=(float)rotation(1)*0.619/T;
error_1_i=50.0-velocity_1;
velocity_2=(float)rotation(2)*0.659/T;
error_2_i=50.0-velocity_2;
printf("v1=%f",velocity_1);
printf("v2=%f\n",velocity_2);
u_1_i=K1*(error_1_i+T/Ti1*error_1_i2+Td1/T*(error_1_i-error_1_i1))+50.0;
u_2_i=K2*(error_2_i+T/Ti2*error_2_i2+Td2/T*(error_2_i-error_2_i1))+50.0;
/*
u_1_i=K1*error_1_i+50.0;
u_2_i=K2*error_2_i+50.0;
*/
/*
u_1_i=K1*(error_1_i+T/Ti1*error_1_i2)+50.0;
u_2_i=K2*(error_2_i+T/Ti2*error_2_i2)+50.0;
*/
motor(1,(int)u_1_i);
motor(2,(int)u_2_i);
/*将 error_i 的值赋给 error_i1,并将 error_i 叠加到 error_i2 上*/
error_1_i1=error_1_i;
error_2_i1=error_2_i;
error_1_i2=error_1_i2+error_1_i;
error_2_i2=error_2_i2+error_2_i;
} }
对偏移有如下规定：
A．先用 P 算法，保持 T=0.5 不变。
左轮比例系数 K 右轮比例系数 K 终点偏移距离
1 0.1 0.1 0．8
2 0.1 0.3 0.6
3 0.1 0.5 0.3
4 0.1 0.6 0.1，先左偏后右偏
5 0.1 1.0 -0.2
6 0.1 1.2 -0.4
7 0.5 0.5 0.6
8 0.5 0.8 0.2
9 0.5 1.2 0．1，先左偏后右偏
10 0.5 1．8 -0.1
11 1.0 1.0 0.3
12 1.0 1.6 几乎无偏差
13 1.0 2.0 -0.1，
14 5．0 5．0 运动曲线不平滑，会有突兀的变化 由以上数据可见，要使能力风暴走直线，右轮的比例系数应比左轮的比例系数大一点,但右 轮比左轮大到一定程度时走路会发生弯曲，并从偏移正值变为负值。另一方面，他们的比例 系数增大时，横向偏差越来越小。但当比例系数大到一定程度时，速度会有较大的振荡。
取上面最理想的比例系数 K1=1.0,K2=1.6 不变，改变 T。
T 偏移距离
1 0.1 走时左右微弱振荡
2 0.2 走时微弱振荡，左偏
3 0.5 偏差小
4 0.8 0．1
5 1．0 0．2
所以，T 取 0.5 最合适。
B．用 PI 算法，保持 K1=1.0 ，K2=1.6，T=0.5 不变。
左轮积分系数 Ti 右轮积分系数 Ti 偏移距离
1 10 5 终点几乎无误差，先
右偏后左偏
2 10 10 偏差小
3 10 20 0.1
4 20 5 -0.3
5 20 20 偏差小
6 20 30 0．1
7 25 5 -0.1
8 25 10 偏差小
9 25 25 0．2
PI 算法可以减小静差，用在这里，效果不是很大，甚至会因为积分累积引起 AS-UII 走路弯 曲。
C．PID 算法，取 K1＝1.0，K2=1.6，Ti1=20，Ti2=20，T=0.5
左轮微分系数 Td1 右轮微分系数 Td2 偏移距离
1 0.01 0.2 0.2
2 0.1 0.1 -0.1,偏差小
3 0.1 0.01 -0.2
PID 可以减小超调，在 PI 基础上加上微分因子，超调就小了，从 AS-UII 的弯曲程 度上就可看出。
2）增量式算法
P 算法公式如：∆Ui=Ui-Ui-1=K*(Ei-Ei-1);
PI 算法公式如：∆Ui=Ui-Ui-1=K*(Ei-Ei-1+T/Ti*Ei);
PID 算法公式如：∆Ui=Ui-Ui-1=K*(Ei-Ei-1+T/Ti*Ei+Td/T*(Ei-2Ei-1+Ei-2));
原来增量式算法是用于一些步进电机之类的对象，因为它的控制量是增量。以增量 为控制量可以减少切换时的冲击。但是我们这里的电机并非步进电机，所以输入的 控制量应是绝对数值。所以算法公式应改成如下：
P 算法公式如：Ui =K*(Ei-Ei-1)+ Ui-1;
PI 算法公式如：Ui=K*(Ei-Ei-1+T/Ti*Ei)+Ui-1;
PID 算法公式如：Ui=K*(Ei-Ei-1+T/Ti*Ei+Td/T*(Ei-2Ei-1+Ei-2))+Ui-1;
但是，这样的话，增量式算法和位置式算法的结果实质上是一样的。这里就不采用 了。