用于乐高机器人的PID控制器

糖伯虎

文章版权属于原作者及译者（中文乐高翻译团队）所有，

转载请注明出处（网站名字及原帖地址）。

译者前言

PID技术一直是大家很感兴趣的话题。要用好PID控制，并不是一件容易的事，需要有相当的高等数学基础和控制技术基础，这对很多乐高机器人爱好者来说，是一个难题。 J. Sluka （原文作者）没有从专业的角度去解释PID，而是以非常浅显的方式写出了这篇“A PID Controller For Lego Mindstorms Robots”（用于乐高机器人的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. Loop forever
   
5.    LightValue = read light sensor                     ! 当前光电传感器的读值
   
6.    error = LightValue - offset                        ! 减去offset（补偿量）计算error（误差）
   
7.    Turn = Kp * error                        ! “比例控制部分”, 我们希望马达的功率值改变多少？
   
8.    powerA = Tp + Turn                  ! A马达的功率值
   
9.    powerC = Tp - Turn                  ! C马达的功率值
   
10.    MOTOR A direction=forward power=powerA   ! 在马达模块中设置这个功率值
    
11.    MOTOR C direction=forward power=powerC   ! 设置另一个马达的功率值
    
12. end loop forever                              ! 结束这个循环，返回，进行下一次循环
    

复制代码

如果机器人在运行时，表现出的状态是远离线的边缘，而不是寻找线的边缘，你需要改变一下转向的方向。把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-G 1.1 版本只支持整数运算，NXT-G 2.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. Loop forever
   
5.    LightValue = read light sensor         ! 当前光电传感器的读值  
   
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.    MOTOR A direction=forward power=powerA! 在马达模块中设置这个功率值
    
12.    MOTOR C direction=forward power=powerC! 设置另一个马达的功率值
    
13. end loop forever                         ! 结束循环，返回，进行下一次循环

复制代码

糖伯虎

等一下，在第一个版本的P控制器中还有一个“微妙的问题”，是什么呢？

在这个例子中，有这样一个问题：在我们计算马达功率值时（如powerC=Tp-Turn），可能会得到一个负的马达功率值，这意味着马达反向转动，但是NXT-G程序中马达模块的数据接口无法“理解”这个数值。马达的功率值是一个在0到100之间的数值，马达的转动方向是由另外一个不同的输入接口控制的。当功率值为负数时，你需要在程序中设置马达的运转方向。方法如下：

If powerA > 0                         ! 马达功率值为正值时
   MOTOR A direction=forwardpower=powerA
else
   powerA = powerA * (-1)           ! 马达功率值为负值时，要做这一步运算
    MOTOR A direction=reverse power=powerA  !此时马达功率值为正值，还需要在控制面板上颠倒马达的转向
end If                                   

马达模块通过数据线接收功率值（powerA对应A马达），在马达的参数设置窗口，用复选框设置方向。

对C马达也需要进行相似的程序代码设置。这样，当计算出的马达功率值为负值时，就可以正确地控制马达了，P控制器就能够实现“零转弯半径转弯”，机器人可根据需要实现原地转弯。

还有其他的“微妙问题”。如果出现计算出的马达功率大于100的情况怎么办？实际上马达会将功率值认定为100。在P控制器（或PID控制器）中出现这种情况，并不十分太好。我们更希望控制器永远不会让马达做超出能力范围的事。如果计算出的马达功率值比100大不了多少（或比-100小不了多少），机器人运行情况还算OK；如果这个计算出的马达功率值比100大很多（或者比-100小很多），这就意味着控制器正经常性的失去控制能力。你需要考虑一下如何处理这种情况！

糖伯虎

P控制器概要

希望你已经对P（比例）控制器有了足够的了解，它还是相当简单的。用传感器测量你想控制的东西，将测量结果转换为error（误差）——对于巡线机器人来说，我们通过减掉黑和白光电传感器读值的平均值来实现，将error （误差）乘以一个叫Kp的比例系数，就得到了系统的修正值。在我们的巡线机器人例子中，我们通过加大/减小马达的功率值来应用这个修正值。这个叫Kp的比例系数要用有根据的推测来确定，并通过反复试验进行调整。
P控制器能够处理很多控制问题，不仅仅是用在乐高机器人巡线上。一般来说，在满足条件的情况下，P控制器都能良好工作。

• 传感器需要有足够宽的动态范围（不幸的是，我们的巡线机器人却不是这样）
• 被控制的东西（在我们的例子里是马达）也需要有足够宽的动态范围。每个马达在功率值上的宽动态范围应该很接近（NXT马达在这一方面非常好）。
• 传感器和被控制的东西必须响应迅速。“迅速”的意思是“比系统内发生的任何变化都快”。控制马达时，通常不太可能获得马达的“迅速”响应，因为马达需要一定的时间来改变功率。就是说机器人的动作要比P控制器的命令滞后，这对P控制器的精确控制，会产生一定的困难。
  

糖伯虎

在控制器中加入“I”：PI控制器

（“I”：会给我们带来什么呢？）

为了提高P控制器的响应速度，我们在表达式中加入一个新的部分——积分，PID中的“I”。积分是高等数学中非常重要的内容，在这里，我们只需要直截了当地使用它。

积分用于计算误差的动态求和。

每次我们读取光电传感器的值，并计算error（误差）时，我们将error（误差）加到一个变量中，这个变量我们称之为integral（积分）。

integral（积分）= integral（积分）+ error（误差）

这个表达式不是普通的数学表达方式，它使用了将一系列数值累加的方法，这个方法在编程中经常使用。在计算机程序里，这个表达式有着和数学不相同的含义。（在本文中，会用文字加重的方式来表明这是编程的方式，而不是普通的数学表达式。）这个“=”是赋值的意思，意味着将它右边的计算结果赋值给左边的那个变量名，就是计算机把原有的integral的值加上error的值，将结果赋值给integral。

接下来，同P的部分一样，我们对integral乘以一个比例常数，这是另一个K。因为这个比例常数与积分部分有关，所以我们称其为Ki。与P比例控制部分相同，我们把integral（积分）乘以一个常量，会得到一个修正值。我们要把这个修正值加到Turn变量中去。

Turn= Kp*(error) + Ki*(integral)

上面就是PI控制器的基本表达式。Turn是对马达的修正，Kp*(error) 是比例控制部分， Ki*(integral)是积分控制部分。

积分控制部分有什么作用呢？如果误差在几次循环中都保持同样的值，积分部分就会越来越大。例如，如果我们读取光电传感器值，计算出error为1，很短时间后，我们再次读取光电传感器值，这一次error为2，第三次的error还是为2，那么此时的integral将是1+2+2=5。Integral为5，但这一步的error只为2。在修正量中，积分部分能产生很大的影响，但通常来说，它需要比较长的时间才能发挥作用。

积分控制的另一个作用是能去除小的误差。在巡线过程中，如果光电传感器非常接近线的边缘，但又不是正好在线的边缘上，那么error会很小，只能产生一个很小的修正量。你可以通过改变比例控制中的 Kp来修正这个小的error，但经常会产生机器人的振荡（来回摇摆）。积分控制部分就可以完美地修正小的误差，因为integral（积分）是对errors（误差）的累加，几个连续的小误差可以使integral（积分）大到足以发生作用。

我们可以把积分控制理解为控制器的"memory"（存储器）。Integral（积分）表现的是error（误差）累积的过程，可以持续向控制器提供修正误差的方法。

糖伯虎

有关积分的一些微妙问题

我隐藏了一个小问题（其实也不是小问题，但是我们要把它变成小问题）—— 时间。积分计算的其实是 error×(单位时间） 的总和，单位时间(dT)是我们从上一次读取光电传感器值到这一次读取光电传感器值的时间间隔。

integral= integral+ error*(dT)

因此，我们每次向integral 中增加的应该是error×dT。测量机器人的dT是相当容易的事。在每次读取光电传感器值时，我们可以读取计时器的值。如果我们从当前时间中减掉上一次的时间，就得到了从上一次读值起的dT。如果不去测量这个dT，不做乘法计算，是不是会更好一些呢？如果这个dT总是相同值呢？如果我们每一次加入到integral中的部分，其dT值都是相同的，我们就能够把因数dT从 error×(dT)中提取出来，只做求和的运算。

integral= integral+ error

实际上只有当我们需要用integral做另外的运算时，我们才需要去乘以dT。因此我们可以把这个时间因数藏起来。在PI控制器中，积分部分的表达式是Ki ×(integral)×dT，其中Ki是一个需要我们进行微调的系数（就像Kp一样），所以为什么不用一个新的Ki来代替Ki ×dT这一部分呢？这个新的Ki与原来的是不同的，但是因为我们哪个都不知道，所以用哪一个、叫什么，都没有关系。无论它叫什么、代表什么含义，我们都需要通过反复试验来找到相对准确的值。

所以，只要把所有的时间步进值——dT 设定成相同的（或大致相同的）值，我们就可以从积分控制部分中去除时间因素。

糖伯虎

积分具有记忆性

关于integral（积分），还有最后一个要注意的细节。通常情况下，integral（积分）只能趋近于0，我们加入到integral（积分）中的error（误差）值绝大多数都是符号相异的，在integral为0 时，它对控制器是不起任何作用的。例如，在经过几次循环后，error（误差）值为1,2,2,3,2,1，相加后得到integral（积分）为11。在最后一点的 error（误差）仅为1，比在那一点的 integral 要小很多。让integral趋近于0 的方法只有一个，就是加入负的error（误差）来平衡早期加入的正的error（误差），来逐渐减少integral。如，下面几个error（误差）是-2，-2，-3，则integral（积分）会从11降到4，还需要加入更多的负 error（误差），才会使积分降到0。此外，integral（积分）期望error（误差）在正负误差之间均匀分布。

如果巡线机器人在线边缘的左侧，而积分部分累积的误差也在线的左侧，那么积分控制部分不仅要把机器人送回线的边缘，还要使机器人越过线，到线的右侧。如果有大的误差持续一段时间，积分会趋向于无穷。这在包含有积分控制的控制器里，会引起一些问题。当积分部分设法修正的误差很大时，积分的这种倾向会引起超调，我们必须在程序上做一些调整以避免出现问题。解决integral（积分）趋向于无穷问题，有两个常见的解决方案：（1）将integral（积分）置零——每次error（误差）为0，或error（误差）改变符号，就将变量integral（积分）设置为0；（2）当计算一个新的integral时，对累积的integral乘以一个小于1的因数来抑制积分：

integral = (2/3)*integral + error

这样，每次循环会把积分值降低1/3。如果你认为积分控制部分是一个控制器的“memory”（存储器），那么这种抑制会在一段“较长”的时间后强迫它变得健忘起来。

糖伯虎

PI控制器的虚拟代码

在控制器中加入积分控制部分，我们需要增加新的变量Ki 和integral。别忘了，为了进行整数运算，我们要把Ks乘以100。

Kp = 1000                 ! 记住我们用 Kp*100，因此Kp实际为10
Ki = 100               !记住我们用 Ki*100，因此Ki实际为1
offset = 45                 ! 初始化变量
Tp = 50
integral = 0           ! 在这个变量里存储积分值
Loopforever
   LightValue = read light sensor    ! 当前光电传感器读值
   error = LightValue - offset       !减去offset（补偿量）计算error（误差）
   integral = integral + error       ! 新增的积分控制部分
   Turn = Kp*error + Ki*integral     ! “比例控制部分”+“积分控制部分”
   Turn = Turn/100                   ! 记住消除Kp，Ki中因数100的影响 !
   powerA = Tp + Turn                ! A马达的功率值
   powerC = Tp - Turn                ! C马达的功率值
   MOTOR A direction=forwardpower=powerA  ! 在马达模块里设置A马达的功率值和转向
   MOTOR C direction=forwardpower=powerC  ! 在马达模块里设置C马达的功率值和转向
end loop forever                    ! 结束循环，返回，进行下一次循环

糖伯虎

在控制器中加入“D”：完整的PID控制器

（“D”：接下来会发生什么？）

我们的控制器里有了比例控制部分，用于纠正当前的误差；有了积分控制部分，用于纠正过去的误差。有没有一种办法能让我们及时预测未来，对还没发生的误差进行纠正呢？

这就要用到高等数学里的另一个概念——导数，就是PID中的D。像积分一样，导数也是高等数学中相当重要的数学方法。

假定误差的下一个变化与当前最后一个变化是相同的，我们据此来预测将来。

这个意思是说，下一个误差的期望值是：当前误差+前两次传感器采样误差的变化量。在两个连续点之间的误差变化量就叫做导数。导数是一条直线的斜率。

看上去，计算起来有些复杂。用数据举例能帮助我们说明这个问题。让我们假设当前误差是2，前一个误差是5，那么我们预测的下一个误差会是多少呢？误差的变化，也就是导数，是：

          （当前误差）-（前一个误差）

按照我们的数值就是 2 - 5 = -3 。因此，当前的导数就是-3 。我们使用导数预测下一个误差就是

      （下一个误差） = （当前误差）+ （当前导数）

按照我们假定的数值就是2 + （-3） = -1 。因此，我们预测下一个误差会是-1 。在实践中，我们实际上并不是要完全一致地预测下一个误差。相反地，我们只是在控制器的公式中直接使用导数。

导数控制部分，与积分控制部分一样，实际上也包含时间因素，正式的导数控制部分是：

       Kd(导数)/(dT)

与比例控制和微分控制一样，我们需要对导数乘上一个常量。因为这个常量是与导数相关的，因此被称之为Kd。注意，在导数控制部分，我们是除以dT，而在积分控制部分，我们是乘以dT。我们会和在积分控制部分一样，采用同样的技巧从导数控制部分去掉这个dT。如果在每一个循环中dT的值相同，分数Kd/dT就是一个常量。我们可以用另外一个Kd来代替Kd/dT。同先前的Ks一样，这个K值是未知的，需要通过反复试验来确定，因此它是Kd/dT 或是一个新的 Kd，都没有关系。

现在我们可以写出PID控制器的完整公式了：

      Turn （转向）= Kp*error(误差) + Ki*integral(积分)+ Kd*derivative(导数)

显然，我们可以“预测将来”了，但是这么做有什么帮助？预测又能准确到什么程度呢？
如果当前误差比前一个误差更糟糕，导数控制部分就会纠正这一误差。如果当前误差比前一误差要好一些，导数控制控制部分就会停止控制器去纠正这个误差。第二个非常有用的作用是，误差越接近于0，我们就越接近想正确停下的那个点。但是系统可能需要一段时间来响应马达功率的变化，因此我们在误差趋近于0之前，就要开始降低马达的功率，以防止过冲。不用担心导数控制部分的方程式很复杂，你所要做的只有一件事，就是按照正确的顺序做减法运算。所谓正确的顺序，就是用“当前”减去“前一个”。因此在计算导数时，我们要用“当前误差”减去“前一个误差”。

糖伯虎

PID控制器的虚拟代码

在控制器中加上导数控制部分，我们需要为Kd增加一个新的变量，还需要增加一个变量来记录最后一个误差。同样不要忘记，我们在Ks上乘以100，来进行整数运算。

Kp = 1000                             ! 记住我们用 Kp*100，因此Kp实际为10
Ki = 100                              ! 记住我们用 Ki*100，因此Ki实际为1
Kd = 10000                           ! 记住我们用 Kd*100 ，因此Kd实际为100
offset= 45                           ! 初始化变量
Tp = 50
integral = 0                          ! 用于存储积分的变量
lastError =0                         ! 用于存储最后一个误差值的变量
derivative = 0                        ! 用于存储导数的变量
Loop forever
   LightValue = read light sensor     ! 当前光电传感器的读值
   error = LightValue - offset        ! 减掉 offset（补偿量），计算误差值
   integral = integral + error        ! 计算积分值
   derivative = error - lastError     ! 计算导数值
   Turn = Kp*error + Ki*integral + Kd*derivative  ! “比例控制部分”+“积分控制部分”+“导数控制部分”
   Turn = Turn/100                      ! 记住消除Kp，Ki和 Kd中因数100的影响!
   powerA = Tp + Turn                 ! A马达功率值
   powerC = Tp - Turn                 ! C马达功率值
   MOTOR A direction=forwardpower=PowerA   !在马达模块中设置A马达的功率值和转向
   MOTOR C direction=forwardpower=PowerC   ! 在马达模块中设置A马达的功率值和转向
   lastError = error                  ! 把当前误差存储在变量lastError中，作为下一次循环的最后一个误差
end loop forever                      ! 结束循环，返回，进行下一次循环

以上就是PID控制器用于巡线机器人的完整代码。这其中最困难的部分，就是如何把 Kp, Ki 和 Kd调整到最好的，至少是调整到还说的过去。

糖伯虎

调整PID控制器，不使用复杂的数学方法

（但是我们还是要做一些计算）

在本文中，我使用了其他人总结出来的PID控制器调整的方法，测量几个系统参数就可以让你非常好地计算出 Kp,Ki 和 Kd的值。有几种技术可用于计算Ks，其中之一就叫做 "Ziegler–Nichols方法" 。通过谷歌搜索可以找到很多讲述这种技术的网页。我所使用的版本几乎是直接使用了维基网页——PID控制器中的内容（在很多其他的地方也可以找到相同的内容），我只做了一点小小的改动，包括下表中所示计算过程中的循环时间。
按以下步骤调整PID控制器：

• 将 Ki 和 Kd 的值置为0，即关闭控制器中的这些部分，将控制器作为一个简单的比例控制器。
• 把Tp（目标功率值）设置的小一点。对于我们使用的马达来说，可以设为25.
• 将 Kp 设置为一个“合理”的值，什么是合理的？
  
  
  1）用我们想让马达功率达到的最大值（100）除以能使用的最大误差值。对于我们的巡线机器人，我们假定这个最大误差是5，所以推测出Kp值为 100/5=20。当误差为+5，马达的功率将达到100。当误差为0，马达的功率会在 Tp （目标功率值）上。
  
  
  2）或者，将Kp 值设为 1 （或100），看看会发生什么。
  
  
  3）如果你要把 K's的值乘以100，在这里，1就要记成100，20记成2000，100记成10000.
  
  
• 运行机器人，观察运行状态。如果它不能巡线，从线上脱离开，就提高Kp值；如果它剧烈摆动，就降低Kp 值。调整Kp值，直到机器人能够巡线，并且没有明显的摆动为止。我们称这时的Kp 值为"Kc" （在PID文献中，被称为临界值）
• 使用Kc值作为Kp，运行机器人，试着找出机器人运行时的“振荡周期”是多少。这个测试不需要非常准确。振荡周期（Pc)是指机器人从线的一侧开始，摆动到另一侧，再回到开始点的时间长短。对于典型的乐高机器人来说，Pc 大约是在0.5秒到1或2秒之间。
• 你还需要知道，机器人控制系统的循环周期是多少。我将循环设置为一个固定的次数（如10,000），测量机器人完成全部循环次数的总时间（从开始到结束的时间，或机器人显示出结果的时间），每个循环的周期是测量时间除以循环次数。对于一个完整的PID控制器来说，使用NXT-G编程（在程序中不要使用发声、显示等模块，这些模块的使用会占用程序运行时间，影响测试结果），dT值应该在每个循环0.015秒到0.020秒之间。
• 使用下表计算 Kp, Ki, 和 Kc 的值。如果你只想要一个P控制器，使用表中标注了P的那一行来计算Kp (Ki' 和 Kd' 均为0)。如果你想要一个PI控制器，就使用第二行来计算。如果你想要一个完整的PID控制器，就使用最后一行来计算。
• 在实际操作时，那些K值都要用100乘以它们实际的值，但是在计算中你不需要考虑这个问题。这个因数100 ，在确定Kp =  Kc 临界值时，就已经考虑在内了。
• 运行机器人，看看它的表现。
• 你可以调整Kp, Ki 和 Kd 的值直到获得最佳的性能。你可以从相当大的调整开始，如30%，然后尝试较小的调整，以获得最佳的（或者至少是可以接受的）效果。
• 一旦你确定了一组好的K值，提高TP值，提高机器人的直线速度。
• 对于新的TP值，要重新的调整K值，也许甚至要回到第1步，重复整个过程，
• 不断地重复，直到机器人的表现是可以接受的。
  
  

Ziegler–Nichols方法给出的K'值   （循环时间恒定并等于  dT)
控制类型	Kp	Ki'	Kd'
P	0.50Kc	0	0
PI	0.45Kc	1.2KpdT/ Pc	0
PID	0.60Kc	2KpdT / Pc	KpPc / (8dT)

Ki' 和 Kd' 上的符号只是要提醒你——Ki' 和 Kd'已经考虑了时间的因素，即ki'= ki*dt，kd’=kd/dt （假定dT为恒定值）。

这里有一个我自己做机器人测试的测量数据。Kc为300，当Kp=Kc时，机器人的摆动周期大约为0.8秒，因此Pc为0.8。我测量Pc的方法是，每当机器人摆动到一个特定的方向，就大声数出来。循环时间dT为0.014秒/每个循环，用程序运行10,000次循环时，NXT上显示的程序运行时间和循环次数相除所获得。使用上表中PID控制器的各计算公式，我们得到：

Kp = (0.60)(Kc) =(0.60)(300) = 180

Ki  = 2(Kp)(dT) / (Pc) =2(180)(0.014) / (0.8) = 6.3 (which is rounded to 6)（四舍五入为6）

Kd = (Kp)(Pc) / ((8)(dT)) =(180)(0.8) / ((8)(0.014)) = 1286

在进一步的反复试验后，最终的Kp, Ki 和Kd值分别为220,7 和500。别忘了所有这些K值均已乘以100，因此，它们的实际值为 2.2 ，0.07和5 。