用于乐高机器人的PID控制器.docx
《用于乐高机器人的PID控制器.docx》由会员分享，可在线阅读，更多相关《用于乐高机器人的PID控制器.docx（20页珍藏版）》请在冰豆网上搜索。
用于乐高机器人的PID控制器
用于乐高机器人的PID控制器
PID技术一直是大家很感兴趣的话题。
要用好PID控制，并不是一件容易的事，需要有相当的高等数学基础和控制技术基础，这对很多乐高机器人爱好者来说，是一个难题。
J.Sluka（原文作者）没有从专业的角度去解释PID，而是以非常浅显的方式写出了这篇“APIDControllerForLegoMindstormsRobots”（用于乐高机器人的PID控制器），向缺乏专业基础的人士，解释如何在乐高机器人里使用PID控制器。
另，虽然原文用了一种非常简单的方法来说明PID控制器的用法，但是对于那些小学、初中没毕业的同学来说，还是有些过于复杂，所以，我并不建议乐友门盲目跟风（感觉会用PID就高端一点）学习PID。
在我看来，先有一定的数学基础后，再回头学习此篇更为适合！
PS:
在这篇文章里所有的代码都不是真正的程序代码，只是作者对编程的示意，或者说是用类似程序代码的方式对编程的内容进行解释。
作者希望读者能自行选择程序语言，自己完成PID控制器的编程。
PID控制器是一种常用的控制技术，常用于多种机械装置（如车辆、机器人、火箭）中。
用数学方式来描述PID控制器是非常复杂的。
本文描述了如何在使用NXT-G编程的LEGO机器人上创建PID控制器。
文中将以实例来说明如何创建PID来完成机器人巡线任务。
PID创建完成后，经过简单的修改就可以应用到其他地方，如，让机器人跑直线，做两轮平衡机器人。
其实学过微积分的人很容易理解PID的典型描述，本文是写给那些对PID几乎没有任何概念的读者，比如参加FLL比赛的3~8年级的孩子们。
考虑到大家可能不了解微积分，因此我尽量不使用微积分，从非常低的起点开始建造整个概念。
先来看看一个适于巡线的机器人是什么样的结构。
看下图，这个机器人用两个马达驱动，分别与车轮A、C连接，前端装有垂直向下的光电传感器，红圈标出的部分就是光电传感器能“看到”的部分。
带箭头的大长方形表示机器人的其余部分，箭头指示机器人的运动方向。
巡线是机器人的基本技术，也是大家学习机器人时最先要做的。
能够巡线的自动装置具有机器人的全部特点：
使用传感器收集周围环境的信息，并据此调整机器人的运动状态。
巡线机器人可以使用1个光电传感器、2个光电传感器、一打光电传感器或者装上你所有的光电传感器。
实际上，你使用的光电传感器越多，巡线的效果越好。
只使用1个光电传感器也可以让机器人精确的巡线（即使线条是有弧度的），但是机器人移动过快时容易“飞线”（“飞线”——指机器人脱离线条，不能继续沿着线移动）。
一般来说，使用的传感器越多，巡线的速度越快。
现在我们来试验第一个方法（非PID方式）。
巡线其实是让机器人沿着线的边缘走，因为如果沿着黑线本身走，当机器人偏离黑线，传感器“看到白色”时，我们不知道机器人到底在线的哪一边，是在线的右边还是左边？
如果沿着线的边缘走，当光电传感器“看到白色”，我们知道机器人在线边缘（线）的左边，当光电传感器“看到黑色”，我们知道机器人在线边缘的右边（在线上）。
因为机器人跟随的是线条的左边，因此这种方式被称为“左手法则”。
我们需要知道当光电传感器“看到白色”和“看到黑色”时返回的读数值。
一个典型的非校准传感器（数值0~100）“看到白色”会返回50，“看到黑色”会返回40。
我们可以在一条数据线段上标出光电传感器的读值，来帮助我们理解如何将光电传感器的读值变化转变为机器人的运动变化。
以下是我们画出的从“白”到“黑”的光电传感器读值。
我们把这个数值线段平分为两部分：
当光电传感器值小于45，让机器人左转；当光电传感器值大于45，让机器人右转。
在这里，我们不考虑机器人的转向动作的精确性。
在相对较直的线上，机器人的转向动作可以比较细小；在有很多弯的线上，机器人通常要有明显的转向动作。
做动作细小的转向时，你可以把速度快的轮子的马力值设置为50%，速度慢的轮子的马力值设置为20%。
有很多弯的现上做明显转向动作时，你可以在快的轮子上设置30%的马力值，在慢的轮子上使用缓停或停止。
无论你在轮子上设置什么数值的马力值，在做左右不同转向时，这个设置应该是一样的，即在一侧的轮子上设置较大的马力值，在另一侧轮子上设置较小的马力值（或设置为停止）。
这种巡线方式能够完成巡线任务，但效果并不是很好。
在比较直的线上完成巡线任务，在编程中设置动作细小的转弯方式，整体巡线效果看起来还算不错；但是如果线上有较大的弯度，你又采用明显的转向动作让机器人完成巡线，机器人就会来回摆动，横向穿过线条。
机器人只“知道”两件事情：
转左和转右。
用这种方法巡线，通常机器人的速度不会很快，而且看起来很糟糕。
即使线是直的，这种方法也不能使机器人走直线，甚至不能完全对准线的边缘。
如何使巡线更有效率呢？
让我们来调整一下。
把光电传感器的读值线段分成三部分。
当光电传感器值低于43时，我们让机器人转左。
光电传感器值在44到47之间时，我们让机器人直行。
光电传感器值大于47时，我们让机器人转右。
这在NXT-G程序中，可以在判断模块中选择yes/no来实现。
你实际上只需做两次判断，而不是三次。
第二种巡线方式效果比第一种方式好的多。
至少机器人有时会直接向前走了。
与第一种巡线方式一样，你依然要根据线的曲直特点来决定使用哪种转向方式（细小或者明显的转向动作）。
机器人依旧会有相当数量的来回摆动。
精明的读者也许会想“如果使用3个光电传感器是不会比2个光电传感器要好些呢？
在增加更多的光电传感器会怎样？
”这就是PID的开始了。
PID”中的“P”:
比例控制是关键
如果我们把光电传感器读值的数据线段分成更多的段，会怎样呢？
我们要解决的第一件事情是，当光电传感器读值的数据线段的分段数超过3段时，要如何确定“turn（转向）”的取值。
在我们的第一种巡线方式啊中，机器人只做两件事情，转左或转右，“turn（转向）”的数值是一样的，只是方向不同。
在第二种巡线方式中，我们在左右两个转向的基础上加上了“直行”。
在光电传感器读值的数据线段分段超过3个时，我们需要更多“种类”的“turn（转向）”。
为了帮助理解“更多种类的turn（转向）”，我们重新画出光电传感器读值的数据线段，并把它转换为图形。
X轴（水平线）为光电传感器读值值，与上面的光电传感器读值的数据线段一样。
Y轴（垂直线）是“turn（转向）”轴。
左边的图形表示的是我们第一种巡线方式——将光电传感器读值分成两段的情况，机器人只能做两件事（用蓝色的线表示），转左或转右，除了方向以外，转向值是一样的。
中间的图形是第二种巡线方式——将光电传感器读值分成三段的情况，中间增加的一段是机器人直行的部分（turn=0），转向部分与前面的第一种巡线方式是一样的。
右侧的图形是一个比例控制的巡线机器人，在两个极限点之间的转向变化很平滑。
如果光电传感器读取的光值表明机器人离线很近，机器人就做小的转弯；如果读取的光值表明机器人离线很远，机器人就做较大的转弯。
比例是一个重要的概念。
比例的意思就是在两个变量之间存在线性关系，简单的说，就是变量之间的关系呈现为一条直线（如右侧图形所示）。
直线的表达式为：
y=mx+b
这里，x,y是指直线上任意一点的坐标值（x,y），m是这条直线的斜率，b是直线在Y轴上的截距（当x=0时，直线通过Y轴上的点，该点在Y轴上的坐标值）。
直线斜率的定义为直线上任意两点y值的变化量除以x值的变化量。
我来把图形和表达式变得简单一些。
首先，我们将光电传感器读值线段（X轴）的中心点定为0，因为我们的光电传感器读值范围是40到50，我们把所有光电传感器读数都减掉45（这是40和50的平均值，（40+50）/2），得到的结果称为“error（误差）”。
当光电传感器读数为47时，可得到error=47-45=2。
这个error（误差）表明了机器人的光电传感器离线的边缘有多远。
当光电传感器正好在线的边缘上，“error（误差）”为0（因为此时光电传感器的读值为45，而我们要从光电传感器读值中减掉45）。
如果光电传感器全部处在白色的地方，“error（误差）”为+5，如果光电传感器全部处在黑色的地方，“error（误差）”为-5。
在上面的图形中，我已经用“error（误差）”来表示X坐标轴。
因为这条直线正好在原点处通过Y轴，因此b的取值为0，这样表达式会变得简单一些：
y=mx
或者使用我们的方法：
Turn=m*error
我们还没有对转向轴做出定义，所以现在我们确定转向的范围是从-1（最大左转）到+1（最大右转），0转向的意思就是直行。
上面图形中直线的斜率就可以用标为红色的两个点计算出来（其实直线上任意两点均可使用）。
斜率=m=（y值的变化量）/（x值的变化量）=（1-（-1））/（-5-5）=-2/10=-0.2
斜率是一个比例常量，用它乘以（x值）就可得到“（转向）”（y值）。
请一定记住这一点。
在各种PID文献中，斜率（也叫做比例常数、直线表达式中的m）被称作"K"。
各式各样的Ks出现在PID文献中。
你可以把K（或m，或斜率，或比例常数）看做是一个换算系数，用K把一个数字（光电传感器读值或我们例子中的error（误差））转换成另外一个数字（如Turn（转向））。
这就是K的作用，非常简单也非常强大。
那么在我们的直线表达式中使用这些新的变量名字：
Turn= K*（error）
用语言表达就是：
将误差值error乘以比例常数K得到所需的转向值Turn。
这个Turn 值就是P控制器的输出结果，因为它只涉及比例控制，被称为“比例控制部分”。
“error”的取值范围是由光电传感器的设置、巡线测试纸的颜色等因素决定的。
你可能已经注意到了，在最后一个图形里，直线没有延伸到error（误差）值-5到+5的范围以外。
在-5到+5的范围以外，我们就不能判断光电传感器到底离线有多远了。
当光电传感器完全看不到任何黑线时，它看到的所有“白色”都是一样的。
当光电传感器离线的边缘太远时，光电传感器读取到的光值变成恒定的数值，这就意味着光电传感器的读与error（误差）不再是比例关系。
我们只能在光电传感器相当接近黑线时，判断光电传感器离线的边缘有多远距离，在非常小的数值范围内，光电传感器的读值与这个距离是成比例的，因此，我们的光电传感器值要设置在能给出比例关系的有限的范围内。
超出这个范围，就只能给出机器人调整的正确方向，但数量大小是错误的，光电传感器读值或是误差会小于实际情况，这样在修正误差时，就不会有很好的效果。
在PID文献中，把传感器能给出比例响应的范围称为“比例范围”。
在PID控制中，比例范围是另一个非常重要的概念。
在我们巡线机器人的应用中，光电传感器读值的比例范围是40到50，误差的比例范围是-5到+5，马达的比例范围是-100（全马力后退）到+100（全马力前进）。
以下是有关比例范围的两个重要内容：
（1）我们希望比例范围尽可能的宽。
光电传感器的比例范围是相当小的，就是说，光电传感器必须很接近线的边缘，才能获得比例信息。
比例范围的宽度主要取决于光电传感器距离巡线测试纸的高度有多少。
如果光电传感器非常靠近巡线测试纸，如1/16英寸（约0.16厘米），那么光电传感器在巡线测试纸上看到范围只是一个很小的圆圈。
光电传感器的一个很小的移动就会产生-5到+5范围的error（误差），也就是比例范围。
你也许会说，光电传感器的视野狭窄，只能看到巡线测试纸的很小的一部分，光电传感器要非常接近线的边缘，读取的光电传感器值既不是“黑”，也不是“白”。
如果光电传感器距离巡线测试纸的高度高一些，那么光电传感器在巡线测试纸上看到的范围就是一个大一些的圆圈。
光电传感器距离巡线测试纸的高度大约为1/2英寸（大约1.27厘米）时，在巡线测试纸上能看到的范围是一个直径大约1/2英寸的圆圈。
光电传感器处于这个高度上，比例范围更大，因为光电传感器在距离线的边缘+/-1/2英寸宽度的范围内，就可以保持比例输出。
将光电传感器位置提高有两个缺点，光电传感器位置提高后更容易对环境光做出错误响应；在区分黑和白时，也与位置较低的光电传感器有些差异。
光电传感器距离巡线测试纸的高度足够大时，对黑色和白色所读取的值是一样的。
（2）在比例范围之外，控制器只能把机器人向正确的方向移动，但也只是趋向于正确。
控制器的比例响应是受比例范围限制的。
从P到实际的马达功率值
我们应该如何设置转向时的马达功率值呢？
做转向的一个方法是：
定义一个“目标功率”，我称之为"Tp"。
Tp是当error（误差）=0时，机器人做直行得两个马达功率值。
当error（误差）不为0时，我们用表达式Turn = K*（error）来计算如何改变两个马达的功率，一个马达的功率为Tp+Turn，另一个马达的功率为Tp-Turn。
注意，因为我们的error（误差）范围是-5到+5，Turn（转向）的值也会有正值和负值，相当于做不同方向的转向。
这正是我们所需要的，它能自动地正确设置马达功率值，确定哪一个马达速度快，哪一个马达速度慢。
我们假定左侧的马达接入端口A，其功率值为Tp+Turn 的值；右侧马达接入端口C，其功率值为Tp-Turn 的值。
当error 为正时，Turn值为正，Tp+Turn的值比Tp大，左侧的马达速度加快，右侧的马达速度减慢。
当error 改变符号变为负值时（这就意味着机器人已经越过线的边缘，看到“黑色”了），此时Tp+Turn的值比Tp小，左侧的马达速度减慢，Tp-Turn的值比Tp大，右侧的马达速度加快。
简单吗？
希望我们继续往下进行时，你会理解得更清楚一点。
P控制器的虚拟代码
首先我们要测出光电传感器读取黑色和白色时的光电传感器读值。
根据这两个数值，我们能够计算出offset（补偿量），将光电传感器读值减掉这个数值就可转换成 error（误差）值。
offset（补偿量）是白色和黑色光电传感器值的平均值。
为简单起见，我假定offset（补偿量）已经测量完毕，并存储在叫做offset的变量里。
（让机器人自己测量白和黑的光电传感器读值，并计算offset，会更好）
常数K被称为Kp（比例控制器中恒量K）。
要为Kp设定一个初始的推测值，然后通过反复试验来修正它。
我们可以根据机器人和传感器的特性估算出一个值：
将Tp（目标功率）设为50，当误差为0时，两个马达都以50的功率值转动；误差范围为-5到+5。
我们期望当误差从0变化到-5时，马达的功率值从50变化到0，就是说Kp（斜率——y的变化量除以x的变化量）为：
Kp =（0-50）/（-5-0）=10
我们用 Kp=10将error（误差）值转换为turn（转向）值。
这句话中，转换的意思是“error”（误差）每发生1个单位的变化，我们就将一个马达的功率值提高10，另一个马达的功率降低10.
虚拟代码如下：
1.Kp=10 !
初始化变量
2.offset=45
3.Tp=50
4.Loopforever
5. LightValue=readlightsensor !
当前光电传感器的读值
6. error=LightValue-offset !
减去offset（补偿量）计算error（误差）
7. Turn=Kp*error !
“比例控制部分”,我们希望马达的功率值改变多少？
8. powerA=Tp+Turn !
A马达的功率值
9. powerC=Tp-Turn !
C马达的功率值
10. MOTORAdirection=forwardpower=powerA !
在马达模块中设置这个功率值
11. MOTORCdirection=forwardpower=powerC !
设置另一个马达的功率值
12.endloopforever !
结束这个循环，返回，进行下一次循环
13.
复制代码
如果机器人在运行时，表现出的状态是远离线的边缘，而不是寻找线的边缘，你需要改变一下转向的方向。
把Kp的值变为-10，看看会怎样。
如果这样做可以纠正机器人的转向方向，就把Kp的值变回+10，将设置马达功率的两行代码做如下改动：
powerA=Tp-Turn
powerC=Tp+Turn
在这个P控制器里有两个“可调参数”和一个恒量。
恒量就是offset（补偿量）（黑色和白色光电传感器读值的平均数）。
你需要编写一小段程序，在巡线测试纸上用你的机器人来测量光电传感器读值。
你需要测量出“black（黑）”和“white白”的光电传感器读值，然后计算平均值，并把平均值写入P控制器程序中的offset变量。
几乎所有的巡线机器人都要做这一步工作，你可以人工进行，也可以通过编写程序代码让机器人自动完成。
Kp 值和Tp（目标功率）值是可调参数。
可调参数必须经过反复试验才能确定。
Kp决定了当机器人渐渐离开线的边缘时，控制器让机器人返回线的边缘的速度有多快；Tp决定了机器人沿着线向前移动的速度有多快。
如果线比较直，你可以将Tp的值设置的高一些，提高机器人的运行速度；将Kd 的值设置的小一些，使机器人的转向动作（修正）更细小。
如果线比较弯曲，尤其是有锐角弯时，要限制Tp的最大值。
如果Tp超过了最大值，无论怎样设置Kp，机器人在遇到曲线时，都会因为移动过快而“飞线”（机器人脱离线条）。
如果Tp值很小，机器人移动速度就会很慢，此时无论将Kp设置成任何数值，机器人都会完成巡线任务。
而我们的目标就是在保证机器人能够完成巡线的情况下，让它尽可能地跑的快一点。
我们推测出了Kp的一个初始值——Kp=10。
对于Tp（目标功率值），你可以从一个比较低的值开始，比如15（机器人会移动的非常慢）。
试一试，看看它的运行情况。
当机器人因转向过慢而出现“飞线”情况，就增大Kp，并继续尝试。
如果机器人因来回摆动、过于活跃而出现“飞线”情况，就减小Kp。
如果机器人巡线的状态非常好，就提高Tp，观察机器人在更快速度下的巡线情况。
尽管Kp通常不会有太大的变化，对于每一个新的Tp值，你都需要确定新的Kp 值。
沿着一条较直的线做巡线，通常比较简单。
沿着一条弯度不大的曲线巡线，有一点难。
沿着一条有急遽弯度的曲线巡线，是最难的。
如果机器人移动地很缓慢，那么即便是使用非常基本的控制器，机器人也几乎可以完成任何巡线任务。
我们想要的是机器人能够以非常好的速度完成巡线，能够处理有普通弯度的巡线任务（有着非常尖锐转角的巡线任务，通常需要采用更专业的巡线机器人）。
对于各种不同类型的巡线任务（线的宽度不同，转弯的尖锐程度不同等）来说，最好的P控制器很可能是不同的。
换句话说，为一条特定的线和特定的机器人而调整出来的P控制器（或者PID控制器），对其他的线和机器人来说，不一定适用。
程序代码可以在很多机器人（和很多巡线任务）上使用，但是Kp, Tp 和offset等参数必须要针对每一个机器人和每一种应用情况重新进行调整。
在一台不认识小数点的计算机上做数学运算会有一些问题
注意：
NXT-G1.1 版本只支持整数运算，NXT-G2.0 版本支持浮点运算。
如果使用2.0及以上版本的NXT-G程序，你无需了解以下内容，可以直接跳过这一部分。
在调整P控制器的过程中，你会对Kp的值做上下调整。
预期的Kp取值范围可能完全取决于P控制器是如何计算的、输入范围有多大、输出范围有多大等因素。
对于我们的巡线机器人的P控制器来说，输入范围是5个光值单位，输出范围是100个马达功率单位，因此似乎Kp值在100/5=20左右。
在一些例子当中，预期的Kp值可能不会那么大。
如果预期的Kp值为1会怎样？
因为在NXT-G中变量只能使用整数，调整Kp值时，你可以尝试使用的是...-2,-1,0,1,2,3,......你不能输入1.3，所以你不可能尝试Kp=1.3，你不能使用带小数点的数值！
但是当你把Kp值做最小的调整，从1调整到2时，机器人的“反应”可能会有很大的不同。
与Kp=1相比，当Kp=2时，机器人修正的误差会是两倍，在光电传感器值变化量相同时，马达功率的变化量也会是两倍。
而我们需要Kp做更精细的控制。
其实解决这个问题很容易。
我们要做的只是将Kp乘以10，增大整数范围。
当Kp接近1时，乘以100也是个好主意。
实际上，在程序中直接使用100*Kp，可能是最好的选择。
当Kp乘以100时，我们输入的数值就从1.3变为了130，没有小数点，NXT-G会喜欢这个数的。
但是不要忘记，要对结果进行转换。
当完成P控制部分的计算时，要对结果除以100。
还记得我们前面定义P控制器的表达式吗？
Turn= Kp*（error）
我们把Kp乘以100，就意味着计算出的 Turn是其实际值的100倍。
在使用Turn的值以前，必须要对它除以100。
因此，我们新的、改进过的巡线P控制器虚拟代码如下：
1.Kp=1000 !
记住，我们用Kp*100，因此这个Kp实际只有10！
2.offset=45 !
初始化其它两个变量
3.Tp=50
4.Loopforever
5. LightValue=readlightsensor !
当前光电传感器的读值
6. error=LightValue-offset !
减去offset（补偿量）计算error（误差）
7. Turn=Kp*error !
“比例控制部分”,我们希望马达的功率值改变多少？
8.Turn=Turn/100 !
记住消除Kp中因数100的影响!
9. powerA=Tp+Turn !
A马达的功率值
10. powerC=Tp-Turn !
C马达的功率值
11. MOTORAdirection=forwardpower=powerA!
在马达模块中设置这个功率值
12. MOTORCdirection=forwardpower=powerC!
设置另一个马达的功率值
13.endloopforever !
结束循环，返回，进行下一次循环
复制代码
等一下，在第一个版本的P控制器中还有一个“微妙的问题”，是什么呢？
在这个例子中，有这样一个问题：
在我们计算马达功率值时（如powerC=Tp-Turn），可能会得到一个负的马达功率值，这意味着马达反向转动，但是NXT-G程序中马达模块的数据接口无法“理解”这个数值。
马达的功率值是一个在0到100之间的数值，马达的转动方向是由另外一个不同的输入接口控制的。
当功率值为负数时，你需要在程序中设置马达的运转方向。
方法如下：
IfpowerA>0
升级会员 