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控制器：

1. 将 Ki 和 Kd 的值置为0，即关闭控制器中的这些部分，将控制器作为一个简单的比例控制器。

2. 把Tp（目标功率值）设置的小一点。对于我们使用的马达来说，可以设为25.

3. 将 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.

4. 运行机器人，观察运行状态。如果它不能巡线，从线上脱离开，就提高Kp值；如果它剧烈摆动，就降低Kp 值。调整Kp值，直到机器人能够巡线，并且没有明显的摆动为止。我们称这时的Kp 值为\（在PID文献中，被称为临界值）

5. 使用Kc值作为Kp，运行机器人，试着找出机器人运行时的“振荡周期”是多少。这个测试不需要非常准确。振荡周期（Pc)是指机器人从线的一侧开始，

摆动到另一侧，再回到开始点的时间长短。对于典型的乐高机器人来说，Pc 大约是在0.5秒到1或2秒之间。

6. 你还需要知道，机器人控制系统的循环周期是多少。我将循环设置为一个固定的次数（如10,000），测量机器人完成全部循环次数的总时间（从开始到结束的时间，或机器人显示出结果的时间），每个循环的周期是测量时间除以循环次数。对于一个完整的PID控制器来说，使用NXT-G编程（在程序中不要使用发声、显示等模块，这些模块的使用会占用程序运行时间，影响测试结果），dT值应该在每个循环0.015秒到0.020秒之间。

7. 使用下表计算 Kp, Ki, 和 Kc 的值。如果你只想要一个P控制器，使用表中标注了P的那一行来计算Kp (Ki' 和 Kd' 均为0)。如果你想要一个PI控制器，就使用第二行来计算。如果你想要一个完整的PID控制器，就使用最后一行来计算。

8. 在实际操作时，那些K值都要用100乘以它们实际的值，但是在计算中你不需要考虑这个问题。这个因数100 ，在确定Kp = Kc 临界值时，就已经考虑在内了。

9. 运行机器人，看看它的表现。 10. 你可以调整Kp, Ki 和 Kd 的值直到获得最佳的性能。你可以从相当大的调整开始，如30%，然后尝试较小的调整，以获得最佳的（或者至少是可以接受的）效果。

11. 一旦你确定了一组好的K值，提高TP值，提高机器人的直线速度。 12. 对于新的TP值，要重新的调整K值，也许甚至要回到第1步，重复整个过程，

13. 不断地重复，直到机器人的表现是可以接受的。 Ziegler–Nichols方法给出的K'值 （循环时间恒定并等于 dT) 控制类型 Kp 0.50Kc 0.45Kc 0.60Kc Ki' 0 1.2KpdT/ Pc 2KpdT / Pc Kd' 0 0 P PI PID 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