PID 速度控制

糖伯虎

LEGO MINDSTORMS NXT的控制器能够同时驱动三组伺服马达，
伺服马达与一般马达主要差异在它多了回授系统。
LEGO Servo Motor能够回报马达转动角度讯息给控制器，
并且可以像步进马达一样控制旋转角度，解析达1度角，这真是不错的设计！

本文先讨论马达转速控制，
NXT玩家们控制马达时，应该会发现用NXT控制马达的角度或转数蛮容易，所以用NXT精确马达定位，不成问题。
但是想控制马达定速运转，这不是一件容易的事，
因为NXT使用不同电池或者换个地点(负载状况不同)，相同的程式产出的结果会不同。


马达转速惯用单位为rpm (Revolution(s) Per Minute，每分钟转数)，NXT马达转速不高，Power = 100，全速运转时约133 rpm。
使用NXC (BricxCC)程式，可以每秒计数NXT马达转角，所以133 rpm = 133 x 360° / 60 seconds = 800° / second。

我使用六颗一般充电电池，充饱电后全速可达850° / second，但是快没电时，全速运转约750° / second。

现在介绍PID Control，属于系统控制的领域，这方法可消弭一些外界变异或干扰影响，
让系统控制可预测，参数设定得好，就可以又快又稳又准。

PID (Proportional Integrative Derivative)

E(t) = R(t) – Y(t) // a set point R(t) to reach, measuring value Y(t)

P(t) = E(t)
I(t) = I(t-1) + E(t)
D(t) = E(t) – E(t-1)

U(t) = Kp*P(t) + Ki*I(t) + Kd*D(t)

以上为一般方程式，适用于任何可控制回授系统，
而下列两式为NXT马达特有方程式。

Power = U(t) / 8
Y(t) = U(t-2)

U(t)为系统估算出的转速，依据实验数据所知，Power = 100时，全速转速约为800° / second，因此Power = U(t) / 8。
而Y(t) = U(t-2)，是因为NXT马达转速回应有一秒延迟的现象(1 second deadtime)。

有兴趣的朋友试试这个程式，只用马达A不需组装：

1. 
   
2. #define Kp 40
   
3. #define Ki 40
   
4. #define Kd 8
   
5. task main(){
   
6.    long power;
   
7.    long set_speed,speed;
   
8.    long e0,e1;     //error
   
9.    long x0,x1;     //input
   
10.    long P,I,D,U;
    
11.    long time;
    
12.    string str;
    
13.    ResetAllTachoCounts(OUT_A);
    
14.    x0 = 0;
    
15.    e0 = 0;
    
16.    power = 0;
    
17.    set_speed = 500;   // degree/second
    
18.    I = 0;
    
19.    time = 0;
    
20.    while (1) {
    
21.       OnFwdRegEx(OUT_A, power, OUT_REGMODE_IDLE, RESET_NONE);
    
22.       x1 = MotorRotationCount(OUT_A);
    
23.       Wait(1000);                  // wait 1 second
    
24.       speed = x1 - x0;
    
25.       x0 = x1;
    
26.       e1 = set_speed - speed;
    
27.       P = e1;
    
28.       I = I + e1;
    
29.       D = e1 - e0;
    
30.       e0 = e1;
    
31.       U = (Kp*P + Ki*I + Kd*D) / 100;
    
32.       power = U / 8;
    
33.       if (power>100)
    
34.          power = 100;
    
35.       else if (power<-100)
    
36.          power = -100;
    
37. 
    
38.       str = NumToStr(speed);
    
39.       str = StrCat("Speed = ",str);
    
40.       TextOut(0, LCD_LINE1, str, true);
    
41.       str = NumToStr(time);
    
42.       str = StrCat("t = ",str);
    
43.       TextOut(0, LCD_LINE3, str, false);
    
44.       time += 1;
    
45.       str = NumToStr(P);
    
46.       str = StrCat("P = ",str);
    
47.       TextOut(0, LCD_LINE4, str, false);
    
48.       str = NumToStr(I);
    
49.       str = StrCat("I = ",str);
    
50.       TextOut(0, LCD_LINE5, str, false);
    
51.       str = NumToStr(D);
    
52.       str = StrCat("D = ",str);
    
53.       TextOut(0, LCD_LINE6, str, false);
    
54.       str = NumToStr(power);
    
55.       str = StrCat("Power = ",str);
    
56.       TextOut(0, LCD_LINE8, str, false);
    
57.    }
    
58. }
    
59. 
    

复制代码

PS.为了让程式码排版正常，程式中使用了全形空格当空行以及全形＜＞，编译前请留意修改。

另外附上网上试算表可以模拟试算，电机系学自动控制的同学，可以藉由这个实验把PID搞懂。
最后提醒各位，网路上有许多PID control文章，大家对一般方程式的定义有些许不同，
所以Kp、Ki、Kd数值没搞清楚定义前不要乱用一通，通常使用0 <= K <= 2。 NXT马达反应速度并不快，因此Kd = 0就可以了。
本程式的K值很大，是因为NXC无法使用小数，所以K = 40，代表K = 40 / 100 = 0.4。

magic1986wang

真佩服糖大的学习能力~找到了这么多资料，佩服佩服~
建议大家最好自己拿NXT的motor试一试，直接看PID的东西其实挺晦涩的

lzlwy2008

写的很不错哦~~~~~~~~~~~~~~~~~~~

magic1986wang

直接点击 “复制代码”貌似编译有问题，到空格的地方都会报错。
附件是我编译好的nxc文件

bluesky4016

学习了!

jason8547

太好了，可以用来做课件。

糖伯虎

回复 6# jason8547


    大学老师？拿pid做课件？

rong-312

好东西，有时间一定好好学习一下！

mayashall

RPM，我找了好久。
非常感谢糖的分享。

安步当车

恩，这个很好用啊。

wang0725

学习中,需要好好想一想

kentgong

写的很不错

dxlwf

这个程序能否在NXT-G里面编程来实现?