PID非常好的光感巡线思路

在一台不认识小数点的计算机上做数学运算会有一些问题

注意：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控制器虚拟代码如下：
Kp = 1000记住，我们用Kp*100 ，因此这个Kp实际只有10！
offset = 45 ! 初始化其它两个变量
Tp = 50
Loop forever
LightValue = read light sensor ! 当前光电传感器的读值
error = LightValue - offset ! 减去offset（补偿量）计算error（误差）
Turn = Kp * error ! “比例控制部分”, 我们希望马达的功率值改变多少？
Turn = Turn/100 !记住消除Kp中因数100的影响!
powerA = Tp + Turn ! A马达的功率值
powerC = Tp - Turn ! C马达的功率值
MOTOR A direction=forward power=powerA! 在马达模块中设置这个功率值
MOTOR C direction=forward power=powerC! 设置另一个马达的功率值
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=reversepower=powerA!此时马达功率值为正值，还需要在控制面板上颠倒马达的转向
end If
马达模块通过数据线接收功率值（powerA对应A马达），在马达的参数设置窗口，用复选框设置方向。
对C马达也需要进行相似的程序代码设置。这样，当计算出的马达功率值为负值时，就可以正确地控制马达了，P控制器就能够实现“零转弯半径转弯”，机器人可根据需要实现原地转弯。
还有其他的“微妙问题”。如果出现计算出的马达功率大于100的情况怎么办？实际上马达会将功率值认定为100。在P控制器（或PID控制器）中出现这种情况，并不十分太好。我们更希望控制器永远不会让马达做超出能力范围的事。如果计算出的马达功率值比100大不了多少（或比-100小不了多少），机器人运行情况还算OK；如果这个计算出的马达功率值比100大很多（或者比-100小很多），这就意味着控制器正经常性的失去控制能力。你需要考虑一下如何处理这种情况！

P控制器概要

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

1.传感器需要有足够宽的动态范围（不幸的是，我们的巡线机器人却不是这样）

2.被控制的东西（在我们的例子里是马达）也需要有足够宽的动态范围。每个马达在功率值上的宽动态范围应该很接近（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调整到最好的，至少是调整到还说的过去。