NXC下用PID算法巡线——算法初步验证成功

grant7788

调整参数后，实测，巡线一圈约耗时13秒。巡线准确。对结果基本满意。
用“传统”的巡线方法(单轮动作)，要跑到25秒左右，用PID节省时间一半。
不过我用的电池快没电了，这个测试结果仅供参考吧。

不过我发现这里有一个很棒的巡线算法，是用NXT-G写的，我用同样的测试条件，居然可以跑到11秒。
http://www.nxtprograms.com/NXT2/line_follower/index.html
大致看了一下他的算法，比较有趣...
可以算是只用了比例因子，I / D 都等于0的一个PID算法。

grant7788

前面发的程序中，读取的RGB值转换成亮度值时，用了一个简化。本来应该是：
    Y = 0.299*raw[0] + 0.587*raw[1] + 0.114*raw[2];
因为浮点运算比较耗CPU时间，一般会写成：
    Y = (299*raw[0] + 587*raw[1] + 114*raw[2])/1000;
不过这样还是会有一个耗时间的除法，可以用移位来代替除法。系数也要做相应的调整。最后得到：
    Y = (raw[0]*19595 + raw[1]*38469 + raw[2]*7472) >> 16;
这种优化算法在嵌入式控制，以及DSP中应该是是比较常见的。

grant7788

今天不能搞得象昨天那么迟了... 先去睡了。
明天如果有时间的话，把那个只用P的PID算法用NXC实现了玩。　

grant7788

唉，还是没忍住... 把那个高手的NXT-G程序写成了NXC，并调试了Gain的值，目前工作基本正常。
发现有个小问题... 有些颜色会造成干扰。目前的工作模式下，颜色传感器工作时是亮三色光，是否可设成红光？
明天再试吧... 代码上传如下... 速度和NXT-G差不多。主要是，电池快没电了，感觉测不大准。

1. // 参数设定值
   
2. #define iPower 90   // 初始速度，直线的话，此值越大越快；但若路径复杂的话此值过大会造成偏离。
   
3. #define iGain 60    // 增益。数字越大，跟随性越好；但过大的数值会造成振荡，过小的数值无法跟踪曲线。
   
4. 
   
5. int ReadSensorColorFast(int iPort)
   
6. {
   
7.     unsigned int raw[3]={0,0,0};
   
8.     int Y;
   
9. 
   
10.     ReadSensorColorRaw(iPort, raw);
    
11.     // Y = 0.299*raw[0] + 0.587*raw[1] + 0.114*raw[2];
    
12.     // Y = (299*raw[0] + 587*raw[1] + 114*raw[2])/1000;
    
13.     Y = (raw[0]*19595 + raw[1]*38469 + raw[2]*7472) >> 16;
    
14.     return Y;
    
15. }
    
16. 
    
17. task main()
    
18. {
    
19.     int iMin = 9999;
    
20.     int iMax = 0;
    
21.     int iRange, iMid;
    
22.     int light;
    
23.     float sCorrection;
    
24.     long degold, degnew;
    
25.    
    
26.     // 开传感器
    
27.     SetSensorColorFull(IN_3);
    
28.     Wait(200);
    
29. 
    
30.     OnFwd(OUT_B, 25);
    
31.     OnRev(OUT_C, 25);
    
32.     degnew = MotorRotationCount(OUT_B);
    
33.     degold = degnew;
    
34.     while(degnew - degold < 120)
    
35.     {
    
36.         light = ReadSensorColorFast(S3);
    
37.         if (light < iMin) iMin = light;
    
38.         if (light > iMax) iMax = light;
    
39.         degnew = MotorRotationCount(OUT_B);
    
40.     }
    
41.     iRange = iMax - iMin;
    
42.     iMid = (iMax + iMin) / 2;
    
43.    
    
44.     OnRev(OUT_B, 25);
    
45.     OnFwd(OUT_C, 25);
    
46.     light = iMax;
    
47.     while(light>iMid)
    
48.     {
    
49.         light = ReadSensorColorFast(S3);
    
50.     }
    
51.    
    
52.     while (true)
    
53.     {
    
54.         light = ReadSensorColorFast(S3);
    
55.         sCorrection = (iMid - light) * iGain / iRange;
    
56.         OnFwd(OUT_B, iPower + sCorrection);
    
57.         OnFwd(OUT_C, iPower - sCorrection);
    
58.     }
    
59. }

复制代码

grant7788

这段代码中，包含了自动寻找最高/最暗值的部分。蛮有趣的。

grant7788

555... 这段是错的。删了...

grant7788

汗个... 又这么晚了...　去睡了。
唉，得经常提醒自己，岁月不饶人啊... 不能这么熬夜了。
[g:02]

晓舟

哎呀！！这个帖子要给精华帖了！！让大家理解PID是一个很重要的内容~~其实我都不是太理解。。很多问题还是很奇怪的

lmd

回复 grant7788 的帖子

我得好好看看，特别是Kp、Ki  、Kd的三个参数的调试，试试看，谢LZ。

lmd

回复 grant7788 的帖子

LZ能否把你看到的那个自平衡机器人的程序共享一下，或者在哪个网上看到的给指条路。我弄了很长时间都没搞出来，有点灰心了。我不是编程高手，有的地方大概没注意，出了问题也没看出来。

liuxiaoyi002

加精华很正常，请楼主不必惊慌

grant7788

回复 394069341 的帖子

PID是自动控制领域里非常经典、非常古老、也是非常常用的一个算法，但是真正用好，非常难。难就难在三个参数的调试。
这种巡线的算法调试起来相对简单，不过随车子(轴距、传感器的位置、轮子大小)、路线的不同，也要分别调整。

grant7788

lmd 发表于 2011-2-7 15:43
回复 grant7788 的帖子

我得好好看看，特别是Kp、Ki  、Kd的三个参数的调试，试试看，谢LZ。

是的，不过巡线的小车程序还算比较容易调试。试过控温的程序... 那个调起来简直就是...

grant7788

liuxiaoyi002 发表于 2011-2-7 16:23
加精华很正常，请楼主不必惊慌

呵呵，到现在的这个份量，当然加精华我不会觉得惊慌；
可是昨天加精华时，试验还没做呢，当然比较惊慌... 万一程序运行效果不对，不是很那啥...

grant7788

lmd 发表于 2011-2-7 15:59
回复 grant7788 的帖子

LZ能否把你看到的那个自平衡机器人的程序共享一下，或者在哪个网上看到的给指条路。 ...

NXTProgram上的，NXT Segway。
http://www.nxtprograms.com/NXT2/segway/index.html