PID非常好的光感巡線(xiàn)思路

在一臺不認識小數點(diǎn)的計算機上做數學(xué)運算會(huì )有一些問(wèn)題

注意：NXT-G 1.1 版本只支持整數運算，NXT-G 2.0 版本支持浮點(diǎn)運算。如果使用2.0及以上版本的NXT-G程序，你無(wú)需了解以下內容，可以直接跳過(guò)這一部分。
在調整P控制器的過(guò)程中，你會(huì )對Kp的值做上下調整。 預期的Kp取值范圍可能完全取決于P控制器是如何計算的、輸入范圍有多大、輸出范圍有多大等因素。對于我們的巡線(xiàn)機器人的P控制器來(lái)說(shuō)，輸入范圍是5個(gè)光值單位，輸出范圍是100個(gè)馬達功率單位，因此似乎Kp值在100/5=20 左右。在一些例子當中，預期的Kp值可能不會(huì )那么大。如果預期的Kp值為1 會(huì )怎樣？因為在NXT-G中變量只能使用整數，調整Kp值時(shí)，你可以嘗試使用的是...-2,-1, 0, 1, 2, 3, ...... 你不能輸入1.3，所以你不可能?chē)L試Kp=1.3，你不能使用帶小數點(diǎn)的數值！但是當你把Kp值做最小的調整，從1調整到2時(shí)，機器人的“反應”可能會(huì )有很大的不同。與Kp=1 相比，當Kp=2時(shí)，機器人修正的誤差會(huì )是兩倍，在光電傳感器值變化量相同時(shí)，馬達功率的變化量也會(huì )是兩倍。而我們需要Kp做更精細的控制。
其實(shí)解決這個(gè)問(wèn)題很容易。我們要做的只是將Kp乘以10 ，增大整數范圍。當Kp接近1 時(shí)，乘以100 也是個(gè)好主意。實(shí)際上，在程序中直接使用100*Kp，可能是最好的選擇。當Kp乘以100 時(shí)，我們輸入的數值就從1.3 變?yōu)榱?30，沒(méi)有小數點(diǎn)，NXT-G會(huì )喜歡這個(gè)數的。
但是不要忘記，要對結果進(jìn)行轉換。當完成P控制部分的計算時(shí)，要對結果除以100。還記得我們前面定義P控制器的表達式嗎？

Turn=Kp*(error)

我們把Kp乘以100，就意味著(zhù)計算出的Turn是其實(shí)際值的100倍。在使用Turn的值以前，必須要對它除以100。

因此，我們新的、改進(jìn)過(guò)的巡線(xiàn)P控制器虛擬代碼如下：
Kp = 1000記住，我們用Kp*100 ，因此這個(gè)Kp實(shí)際只有10！
offset = 45 ! 初始化其它兩個(gè)變量
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! 在馬達模塊中設置這個(gè)功率值
MOTOR C direction=forward power=powerC! 設置另一個(gè)馬達的功率值
end loop forever ! 結束循環(huán)，返回，進(jìn)行下一次循環(huán) !

等一下，在第一個(gè)版本的P控制器中還有一個(gè)“微妙的問(wèn)題”，是什么呢？

在這個(gè)例子中，有這樣一個(gè)問(wèn)題：在我們計算馬達功率值時(shí)（如powerC=Tp-Turn），可能會(huì )得到一個(gè)負的馬達功率值，這意味著(zhù)馬達反向轉動(dòng)，但是NXT-G程序中馬達模塊的數據接口無(wú)法“理解”這個(gè)數值。馬達的功率值是一個(gè)在0到100之間的數值，馬達的轉動(dòng)方向是由另外一個(gè)不同的輸入接口控制的。當功率值為負數時(shí)，你需要在程序中設置馬達的運轉方向。方法如下：
If powerA > 0 ! 馬達功率值為正值時(shí)
MOTOR A direction=forwardpower=powerA
else
powerA = powerA * (-1) ! 馬達功率值為負值時(shí)，要做這一步運算
MOTOR A direction=reversepower=powerA!此時(shí)馬達功率值為正值，還需要在控制面板上顛倒馬達的轉向
end If
馬達模塊通過(guò)數據線(xiàn)接收功率值（powerA對應A馬達），在馬達的參數設置窗口，用復選框設置方向。
對C馬達也需要進(jìn)行相似的程序代碼設置。這樣，當計算出的馬達功率值為負值時(shí)，就可以正確地控制馬達了，P控制器就能夠實(shí)現“零轉彎半徑轉彎”，機器人可根據需要實(shí)現原地轉彎。
還有其他的“微妙問(wèn)題”。如果出現計算出的馬達功率大于100的情況怎么辦？實(shí)際上馬達會(huì )將功率值認定為100。在P控制器（或PID控制器）中出現這種情況，并不十分太好。我們更希望控制器永遠不會(huì )讓馬達做超出能力范圍的事。如果計算出的馬達功率值比100大不了多少（或比-100小不了多少），機器人運行情況還算OK；如果這個(gè)計算出的馬達功率值比100大很多（或者比-100小很多），這就意味著(zhù)控制器正經(jīng)常性的失去控制能力。你需要考慮一下如何處理這種情況！

P控制器概要

希望你已經(jīng)對P（比例）控制器有了足夠的了解，它還是相當簡(jiǎn)單的。用傳感器測量你想控制的東西，將測量結果轉換為error（誤差）——對于巡線(xiàn)機器人來(lái)說(shuō)，我們通過(guò)減掉黑和白光電傳感器讀值的平均值來(lái)實(shí)現，將error（誤差）乘以一個(gè)叫Kp的比例系數，就得到了系統的修正值。在我們的巡線(xiàn)機器人例子中，我們通過(guò)加大/減小馬達的功率值來(lái)應用這個(gè)修正值。這個(gè)叫Kp的比例系數要用有根據的推測來(lái)確定，并通過(guò)反復試驗進(jìn)行調整。
P控制器能夠處理很多控制問(wèn)題，不僅僅是用在樂(lè )高機器人巡線(xiàn)上。一般來(lái)說(shuō)，在滿(mǎn)足條件的情況下，P控制器都能良好工作。

1.傳感器需要有足夠寬的動(dòng)態(tài)范圍（不幸的是，我們的巡線(xiàn)機器人卻不是這樣）

2.被控制的東西（在我們的例子里是馬達）也需要有足夠寬的動(dòng)態(tài)范圍。每個(gè)馬達在功率值上的寬動(dòng)態(tài)范圍應該很接近（NXT馬達在這一方面非常好）。

傳感器和被控制的東西必須響應迅速。“迅速”的意思是“比系統內發(fā)生的任何變化都快”?？刂岂R達時(shí)，通常不太可能獲得馬達的“迅速”響應，因為馬達需要一定的時(shí)間來(lái)改變功率。就是說(shuō)機器人的動(dòng)作要比P控制器的命令滯后，這對P控制器的精確控制，會(huì )產(chǎn)生一定的困難。

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

（“I”：會(huì )給我們帶來(lái)什么呢？）

為了提高P控制器的響應速度，我們在表達式中加入一個(gè)新的部分——積分，PID中的“I”。積分是高等數學(xué)中非常重要的內容，在這里，我們只需要直截了當地使用它。
積分用于計算誤差的動(dòng)態(tài)求和。
每次我們讀取光電傳感器的值，并計算error（誤差）時(shí)，我們將error（誤差）加到一個(gè)變量中，這個(gè)變量我們稱(chēng)之為integral（積分）。
integral（積分）=integral（積分）+error（誤差）
這個(gè)表達式不是普通的數學(xué)表達方式，它使用了將一系列數值累加的方法，這個(gè)方法在編程中經(jīng)常使用。在計算機程序里，這個(gè)表達式有著(zhù)和數學(xué)不相同的含義。（在本文中，會(huì )用文字加重的方式來(lái)表明這是編程的方式，而不是普通的數學(xué)表達式。）這個(gè)“=”是賦值的意思，意味著(zhù)將它右邊的計算結果賦值給左邊的那個(gè)變量名，就是計算機把原有的integral的值加上error的值，將結果賦值給integral。

接下來(lái)，同P的部分一樣，我們對integral乘以一個(gè)比例常數，這是另一個(gè)K。因為這個(gè)比例常數與積分部分有關(guān)，所以我們稱(chēng)其為Ki。與P比例控制部分相同，我們把integral（積分）乘以一個(gè)常量，會(huì )得到一個(gè)修正值。我們要把這個(gè)修正值加到Turn變量中去。

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

上面就是PI控制器的基本表達式。Turn是對馬達的修正，Kp*(error) 是比例控制部分，Ki*(integral)是積分控制部分。
積分控制部分有什么作用呢？如果誤差在幾次循環(huán)中都保持同樣的值，積分部分就會(huì )越來(lái)越大。例如，如果我們讀取光電傳感器值，計算出error為1，很短時(shí)間后，我們再次讀取光電傳感器值，這一次error為2，第三次的error還是為2，那么此時(shí)的integral將是1+2+2=5。Integral為5，但這一步的error只為2。在修正量中，積分部分能產(chǎn)生很大的影響，但通常來(lái)說(shuō)，它需要比較長(cháng)的時(shí)間才能發(fā)揮作用。
積分控制的另一個(gè)作用是能去除小的誤差。在巡線(xiàn)過(guò)程中，如果光電傳感器非常接近線(xiàn)的邊緣，但又不是正好在線(xiàn)的邊緣上，那么error會(huì )很小，只能產(chǎn)生一個(gè)很小的修正量。你可以通過(guò)改變比例控制中的Kp來(lái)修正這個(gè)小的error，但經(jīng)常會(huì )產(chǎn)生機器人的振蕩（來(lái)回搖擺）。積分控制部分就可以完美地修正小的誤差，因為integral（積分）是對errors（誤差）的累加，幾個(gè)連續的小誤差可以使integral（積分）大到足以發(fā)生作用。
我們可以把積分控制理解為控制器的"memory"（存儲器）。Integral（積分）表現的是error（誤差）累積的過(guò)程，可以持續向控制器提供修正誤差的方法。

有關(guān)積分的一些微妙問(wèn)題

我隱藏了一個(gè)小問(wèn)題（其實(shí)也不是小問(wèn)題，但是我們要把它變成小問(wèn)題）—— 時(shí)間。積分計算的其實(shí)是error×(單位時(shí)間） 的總和，單位時(shí)間(dT)是我們從上一次讀取光電傳感器值到這一次讀取光電傳感器值的時(shí)間間隔。
integral=integral+error*(dT)

因此，我們每次向integral中增加的應該是error×dT。測量機器人的dT是相當容易的事。在每次讀取光電傳感器值時(shí)，我們可以讀取計時(shí)器的值。如果我們從當前時(shí)間中減掉上一次的時(shí)間，就得到了從上一次讀值起的dT。如果不去測量這個(gè)dT，不做乘法計算，是不是會(huì )更好一些呢？如果這個(gè)dT總是相同值呢？如果我們每一次加入到integral中的部分，其dT值都是相同的，我們就能夠把因數dT從error×(dT)中提取出來(lái)，只做求和的運算。
integral=integral+error
實(shí)際上只有當我們需要用integral做另外的運算時(shí)，我們才需要去乘以dT。因此我們可以把這個(gè)時(shí)間因數藏起來(lái)。在PI控制器中，積分部分的表達式是Ki×(integral)×dT，其中Ki是一個(gè)需要我們進(jìn)行微調的系數（就像Kp一樣），所以為什么不用一個(gè)新的Ki來(lái)代替Ki×dT這一部分呢？這個(gè)新的Ki與原來(lái)的是不同的，但是因為我們哪個(gè)都不知道，所以用哪一個(gè)、叫什么，都沒(méi)有關(guān)系。無(wú)論它叫什么、代表什么含義，我們都需要通過(guò)反復試驗來(lái)找到相對準確的值。
所以，只要把所有的時(shí)間步進(jìn)值——dT設定成相同的（或大致相同的）值，我們就可以從積分控制部分中去除時(shí)間因素。

積分具有記憶性

關(guān)于integral（積分），還有最后一個(gè)要注意的細節。通常情況下，integral（積分）只能趨近于0，我們加入到integral（積分）中的error（誤差）值絕大多數都是符號相異的，在integral為0 時(shí)，它對控制器是不起任何作用的。例如，在經(jīng)過(guò)幾次循環(huán)后，error（誤差）值為1,2,2,3,2,1，相加后得到integral（積分）為11。在最后一點(diǎn)的error（誤差）僅為1，比在那一點(diǎn)的integral要小很多。讓integral趨近于0 的方法只有一個(gè)，就是加入負的error（誤差）來(lái)平衡早期加入的正的error（誤差），來(lái)逐漸減少integral。如，下面幾個(gè)error（誤差）是-2，-2，-3，則integral（積分）會(huì )從11降到4，還需要加入更多的負error（誤差），才會(huì )使積分降到0。此外，integral（積分）期望error（誤差）在正負誤差之間均勻分布。
如果巡線(xiàn)機器人在線(xiàn)邊緣的左側，而積分部分累積的誤差也在線(xiàn)的左側，那么積分控制部分不僅要把機器人送回線(xiàn)的邊緣，還要使機器人越過(guò)線(xiàn)，到線(xiàn)的右側。如果有大的誤差持續一段時(shí)間，積分會(huì )趨向于無(wú)窮。這在包含有積分控制的控制器里，會(huì )引起一些問(wèn)題。當積分部分設法修正的誤差很大時(shí)，積分的這種傾向會(huì )引起超調，我們必須在程序上做一些調整以避免出現問(wèn)題。解決integral（積分）趨向于無(wú)窮問(wèn)題，有兩個(gè)常見(jiàn)的解決方案：（1）將integral（積分）置零——每次error（誤差）為0，或error（誤差）改變符號，就將變量integral（積分）設置為0；（2）當計算一個(gè)新的integral時(shí)，對累積的integral乘以一個(gè)小于1的因數來(lái)抑制積分：
integral = (2/3)*integral + error
這樣，每次循環(huán)會(huì )把積分值降低1/3。如果你認為積分控制部分是一個(gè)控制器的“memory”（存儲器），那么這種抑制會(huì )在一段“較長(cháng)”的時(shí)間后強迫它變得健忘起來(lái)。

PI控制器的虛擬代碼

在控制器中加入積分控制部分，我們需要增加新的變量Ki和integral。別忘了，為了進(jìn)行整數運算，我們要把Ks乘以100。
Kp = 1000 !記住我們用 Kp*100，因此Kp實(shí)際為10
Ki = 100記住我們用Ki*100，因此Ki實(shí)際為1
offset = 45 ! 初始化變量
Tp = 50
integral = 0在這個(gè)變量里存儲積分值
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 ! 結束循環(huán)，返回，進(jìn)行下一次循環(huán) ! “ ! ! !

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

（“D”：接下來(lái)會(huì )發(fā)生什么？）

我們的控制器里有了比例控制部分，用于糾正當前的誤差；有了積分控制部分，用于糾正過(guò)去的誤差。有沒(méi)有一種辦法能讓我們及時(shí)預測未來(lái)，對還沒(méi)發(fā)生的誤差進(jìn)行糾正呢？
這就要用到高等數學(xué)里的另一個(gè)概念——導數，就是PID中的D。像積分一樣，導數也是高等數學(xué)中相當重要的數學(xué)方法。
假定誤差的下一個(gè)變化與當前最后一個(gè)變化是相同的，我們據此來(lái)預測將來(lái)。
這個(gè)意思是說(shuō)，下一個(gè)誤差的期望值是：當前誤差+前兩次傳感器采樣誤差的變化量。在兩個(gè)連續點(diǎn)之間的誤差變化量就叫做導數。導數是一條直線(xiàn)的斜率。

看上去，計算起來(lái)有些復雜。用數據舉例能幫助我們說(shuō)明這個(gè)問(wèn)題。讓我們假設當前誤差是2，前一個(gè)誤差是5，那么我們預測的下一個(gè)誤差會(huì )是多少呢？誤差的變化，也就是導數，是：

（當前誤差）-（前一個(gè)誤差）

按照我們的數值就是 2 - 5 = -3 。因此，當前的導數就是-3 。我們使用導數預測下一個(gè)誤差就是

（下一個(gè)誤差） = （當前誤差）+ （當前導數）

按照我們假定的數值就是2 + （-3） = -1 。因此，我們預測下一個(gè)誤差會(huì )是-1 。在實(shí)踐中，我們實(shí)際上并不是要完全一致地預測下一個(gè)誤差。相反地，我們只是在控制器的公式中直接使用導數。

導數控制部分，與積分控制部分一樣，實(shí)際上也包含時(shí)間因素，正式的導數控制部分是：

Kd(導數)/(dT)

與比例控制和微分控制一樣，我們需要對導數乘上一個(gè)常量。因為這個(gè)常量是與導數相關(guān)的，因此被稱(chēng)之為Kd。注意，在導數控制部分，我們是除以dT，而在積分控制部分，我們是乘以dT。我們會(huì )和在積分控制部分一樣，采用同樣的技巧從導數控制部分去掉這個(gè)dT。如果在每一個(gè)循環(huán)中dT的值相同，分數Kd/dT就是一個(gè)常量。我們可以用另外一個(gè)Kd來(lái)代替Kd/dT。同先前的Ks一樣，這個(gè)K值是未知的，需要通過(guò)反復試驗來(lái)確定，因此它是Kd/dT或是一個(gè)新的Kd，都沒(méi)有關(guān)系。

現在我們可以寫(xiě)出PID控制器的完整公式了：

Turn（轉向）= Kp*error(誤差) +Ki*integral(積分)+Kd*derivative(導數)

顯然，我們可以“預測將來(lái)”了，但是這么做有什么幫助？預測又能準確到什么程度呢？
如果當前誤差比前一個(gè)誤差更糟糕，導數控制部分就會(huì )糾正這一誤差。如果當前誤差比前一誤差要好一些，導數控制控制部分就會(huì )停止控制器去糾正這個(gè)誤差。第二個(gè)非常有用的作用是，誤差越接近于0，我們就越接近想正確停下的那個(gè)點(diǎn)。但是系統可能需要一段時(shí)間來(lái)響應馬達功率的變化，因此我們在誤差趨近于0之前，就要開(kāi)始降低馬達的功率，以防止過(guò)沖。不用擔心導數控制部分的方程式很復雜，你所要做的只有一件事，就是按照正確的順序做減法運算。所謂正確的順序，就是用“當前”減去“前一個(gè)”。因此在計算導數時(shí)，我們要用“當前誤差”減去“前一個(gè)誤差”。

PID控制器的虛擬代碼

在控制器中加上導數控制部分，我們需要為Kd增加一個(gè)新的變量，還需要增加一個(gè)變量來(lái)記錄最后一個(gè)誤差。同樣不要忘記，我們在Ks上乘以100，來(lái)進(jìn)行整數運算。
Kp = 1000 !記住我們用 Kp*100，因此Kp實(shí)際為10
Ki = 100 !記住我們用Ki*100，因此Ki實(shí)際為1
Kd = 10000 !記住我們用Kd*100，因此Kd實(shí)際為100
offset= 45 ! 初始化變量
Tp = 50
integral = 0 ! 用于存儲積分的變量
lastError =0 !用于存儲最后一個(gè)誤差值的變量
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中，作為下一次循環(huán)的最后一個(gè)誤差
end loop forever ! 結束循環(huán)，返回，進(jìn)行下一次循環(huán)
以上就是PID控制器用于巡線(xiàn)機器人的完整代碼。這其中最困難的部分，就是如何把Kp,Ki和Kd調整到最好的，至少是調整到還說(shuō)的過(guò)去。