PID非常好的光感巡線思路
加入技術交流群
如果線比較彎曲，尤其是有銳角彎時，要限制Tp的最大值。如果Tp超過了最大值，無論怎樣設置Kp，機器人在遇到曲線時，都會因為移動過快而“飛線”（機器人脫離線條）。如果Tp值很小，機器人移動速度就會很慢，此時無論將Kp設置成任何數(shù)值，機器人都會完成巡線任務。而我們的目標就是在保證機器人能夠完成巡線的情況下，讓它盡可能地跑的快一點。
我們推測出了Kp的一個初始值——Kp=10。對于Tp（目標功率值），你可以從一個比較低的值開始，比如15（機器人會移動的非常慢）。試一試，看看它的運行情況。當機器人因轉(zhuǎn)向過慢而出現(xiàn)“飛線”情況，就增大Kp，并繼續(xù)嘗試。如果機器人因來回擺動、過于活躍而出現(xiàn)“飛線”情況，就減小Kp。如果機器人巡線的狀態(tài)非常好，就提高Tp，觀察機器人在更快速度下的巡線情況。盡管Kp通常不會有太大的變化，對于每一個新的Tp值，你都需要確定新的Kp值。
沿著一條較直的線做巡線，通常比較簡單。沿著一條彎度不大的曲線巡線，有一點難。沿著一條有急遽彎度的曲線巡線，是最難的。如果機器人移動地很緩慢，那么即便是使用非常基本的控制器，機器人也幾乎可以完成任何巡線任務。我們想要的是機器人能夠以非常好的速度完成巡線，能夠處理有普通彎度的巡線任務（有著非常尖銳轉(zhuǎn)角的巡線任務，通常需要采用更專業(yè)的巡線機器人）。
對于各種不同類型的巡線任務（線的寬度不同，轉(zhuǎn)彎的尖銳程度不同等）來說，最好的P控制器很可能是不同的。換句話說，為一條特定的線和特定的機器人而調(diào)整出來的P控制器（或者PID控制器），對其他的線和機器人來說，不一定適用。程序代碼可以在很多機器人（和很多巡線任務）上使用，但是Kp,Tp和offset等參數(shù)必須要針對每一個機器人和每一種應用情況重新進行調(diào)整。
在一臺不認識小數(shù)點的計算機上做數(shù)學運算會有一些問題
注意：NXT-G 1.1 版本只支持整數(shù)運算，NXT-G 2.0 版本支持浮點運算。如果使用2.0及以上版本的NXT-G程序，你無需了解以下內(nèi)容，可以直接跳過這一部分。
在調(diào)整P控制器的過程中，你會對Kp的值做上下調(diào)整。 預期的Kp取值范圍可能完全取決于P控制器是如何計算的、輸入范圍有多大、輸出范圍有多大等因素。對于我們的巡線機器人的P控制器來說，輸入范圍是5個光值單位，輸出范圍是100個馬達功率單位，因此似乎Kp值在100/5=20 左右。在一些例子當中，預期的Kp值可能不會那么大。如果預期的Kp值為1 會怎樣？因為在NXT-G中變量只能使用整數(shù)，調(diào)整Kp值時，你可以嘗試使用的是...-2,-1, 0, 1, 2, 3, ...... 你不能輸入1.3，所以你不可能嘗試Kp=1.3，你不能使用帶小數(shù)點的數(shù)值！但是當你把Kp值做最小的調(diào)整，從1調(diào)整到2時，機器人的“反應”可能會有很大的不同。與Kp=1 相比，當Kp=2時，機器人修正的誤差會是兩倍，在光電傳感器值變化量相同時，馬達功率的變化量也會是兩倍。而我們需要Kp做更精細的控制。
其實解決這個問題很容易。我們要做的只是將Kp乘以10 ，增大整數(shù)范圍。當Kp接近1 時，乘以100 也是個好主意。實際上，在程序中直接使用100*Kp，可能是最好的選擇。當Kp乘以100 時，我們輸入的數(shù)值就從1.3 變?yōu)榱?30，沒有小數(shù)點，NXT-G會喜歡這個數(shù)的。
但是不要忘記，要對結果進行轉(zhuǎn)換。當完成P控制部分的計算時，要對結果除以100。還記得我們前面定義P控制器的表達式嗎？
在調(diào)整P控制器的過程中，你會對Kp的值做上下調(diào)整。 預期的Kp取值范圍可能完全取決于P控制器是如何計算的、輸入范圍有多大、輸出范圍有多大等因素。對于我們的巡線機器人的P控制器來說，輸入范圍是5個光值單位，輸出范圍是100個馬達功率單位，因此似乎Kp值在100/5=20 左右。在一些例子當中，預期的Kp值可能不會那么大。如果預期的Kp值為1 會怎樣？因為在NXT-G中變量只能使用整數(shù)，調(diào)整Kp值時，你可以嘗試使用的是...-2,-1, 0, 1, 2, 3, ...... 你不能輸入1.3，所以你不可能嘗試Kp=1.3，你不能使用帶小數(shù)點的數(shù)值！但是當你把Kp值做最小的調(diào)整，從1調(diào)整到2時，機器人的“反應”可能會有很大的不同。與Kp=1 相比，當Kp=2時，機器人修正的誤差會是兩倍，在光電傳感器值變化量相同時，馬達功率的變化量也會是兩倍。而我們需要Kp做更精細的控制。
其實解決這個問題很容易。我們要做的只是將Kp乘以10 ，增大整數(shù)范圍。當Kp接近1 時，乘以100 也是個好主意。實際上，在程序中直接使用100*Kp，可能是最好的選擇。當Kp乘以100 時，我們輸入的數(shù)值就從1.3 變?yōu)榱?30，沒有小數(shù)點，NXT-G會喜歡這個數(shù)的。
但是不要忘記，要對結果進行轉(zhuǎn)換。當完成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中因數(shù)100的影響!
powerA = Tp + Turn ! A馬達的功率值
powerC = Tp - Turn ! C馬達的功率值
MOTOR A direction=forward power=powerA! 在馬達模塊中設置這個功率值
MOTOR C direction=forward power=powerC! 設置另一個馬達的功率值
end loop forever ! 結束循環(huán)，返回，進行下一次循環(huán) !
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中因數(shù)100的影響!
powerA = Tp + Turn ! A馬達的功率值
powerC = Tp - Turn ! C馬達的功率值
MOTOR A direction=forward power=powerA! 在馬達模塊中設置這個功率值
MOTOR C direction=forward power=powerC! 設置另一個馬達的功率值
end loop forever ! 結束循環(huán)，返回，進行下一次循環(huán) !
等一下，在第一個版本的P控制器中還有一個“微妙的問題”，是什么呢？
在這個例子中，有這樣一個問題：在我們計算馬達功率值時（如powerC=Tp-Turn），可能會得到一個負的馬達功率值，這意味著馬達反向轉(zhuǎn)動，但是NXT-G程序中馬達模塊的數(shù)據(jù)接口無法“理解”這個數(shù)值。馬達的功率值是一個在0到100之間的數(shù)值，馬達的轉(zhuǎn)動方向是由另外一個不同的輸入接口控制的。當功率值為負數(shù)時，你需要在程序中設置馬達的運轉(zhuǎn)方向。方法如下：
If powerA > 0 ! 馬達功率值為正值時
MOTOR A direction=forwardpower=powerA
else
powerA = powerA * (-1) ! 馬達功率值為負值時，要做這一步運算
MOTOR A direction=reversepower=powerA!此時馬達功率值為正值，還需要在控制面板上顛倒馬達的轉(zhuǎn)向
end If
馬達模塊通過數(shù)據(jù)線接收功率值（powerA對應A馬達），在馬達的參數(shù)設置窗口，用復選框設置方向。
對C馬達也需要進行相似的程序代碼設置。這樣，當計算出的馬達功率值為負值時，就可以正確地控制馬達了，P控制器就能夠?qū)崿F(xiàn)“零轉(zhuǎn)彎半徑轉(zhuǎn)彎”，機器人可根據(jù)需要實現(xiàn)原地轉(zhuǎn)彎。
還有其他的“微妙問題”。如果出現(xiàn)計算出的馬達功率大于100的情況怎么辦？實際上馬達會將功率值認定為100。在P控制器（或PID控制器）中出現(xiàn)這種情況，并不十分太好。我們更希望控制器永遠不會讓馬達做超出能力范圍的事。如果計算出的馬達功率值比100大不了多少（或比-100小不了多少），機器人運行情況還算OK；如果這個計算出的馬達功率值比100大很多（或者比-100小很多），這就意味著控制器正經(jīng)常性的失去控制能力。你需要考慮一下如何處理這種情況！
If powerA > 0 ! 馬達功率值為正值時
MOTOR A direction=forwardpower=powerA
else
powerA = powerA * (-1) ! 馬達功率值為負值時，要做這一步運算
MOTOR A direction=reversepower=powerA!此時馬達功率值為正值，還需要在控制面板上顛倒馬達的轉(zhuǎn)向
end If
馬達模塊通過數(shù)據(jù)線接收功率值（powerA對應A馬達），在馬達的參數(shù)設置窗口，用復選框設置方向。
對C馬達也需要進行相似的程序代碼設置。這樣，當計算出的馬達功率值為負值時，就可以正確地控制馬達了，P控制器就能夠?qū)崿F(xiàn)“零轉(zhuǎn)彎半徑轉(zhuǎn)彎”，機器人可根據(jù)需要實現(xiàn)原地轉(zhuǎn)彎。
還有其他的“微妙問題”。如果出現(xiàn)計算出的馬達功率大于100的情況怎么辦？實際上馬達會將功率值認定為100。在P控制器（或PID控制器）中出現(xiàn)這種情況，并不十分太好。我們更希望控制器永遠不會讓馬達做超出能力范圍的事。如果計算出的馬達功率值比100大不了多少（或比-100小不了多少），機器人運行情況還算OK；如果這個計算出的馬達功率值比100大很多（或者比-100小很多），這就意味著控制器正經(jīng)常性的失去控制能力。你需要考慮一下如何處理這種情況！
P控制器概要
希望你已經(jīng)對P（比例）控制器有了足夠的了解，它還是相當簡單的。用傳感器測量你想控制的東西，將測量結果轉(zhuǎn)換為error（誤差）——對于巡線機器人來說，我們通過減掉黑和白光電傳感器讀值的平均值來實現(xiàn)，將error（誤差）乘以一個叫Kp的比例系數(shù)，就得到了系統(tǒng)的修正值。在我們的巡線機器人例子中，我們通過加大/減小馬達的功率值來應用這個修正值。這個叫Kp的比例系數(shù)要用有根據(jù)的推測來確定，并通過反復試驗進行調(diào)整。
P控制器能夠處理很多控制問題，不僅僅是用在樂高機器人巡線上。一般來說，在滿足條件的情況下，P控制器都能良好工作。
P控制器能夠處理很多控制問題，不僅僅是用在樂高機器人巡線上。一般來說，在滿足條件的情況下，P控制器都能良好工作。
1.傳感器需要有足夠?qū)挼膭討B(tài)范圍（不幸的是，我們的巡線機器人卻不是這樣）
2.被控制的東西（在我們的例子里是馬達）也需要有足夠?qū)挼膭討B(tài)范圍。每個馬達在功率值上的寬動態(tài)范圍應該很接近（NXT馬達在這一方面非常好）。
傳感器和被控制的東西必須響應迅速。“迅速”的意思是“比系統(tǒng)內(nèi)發(fā)生的任何變化都快”?？刂岂R達時，通常不太可能獲得馬達的“迅速”響應，因為馬達需要一定的時間來改變功率。就是說機器人的動作要比P控制器的命令滯后，這對P控制器的精確控制，會產(chǎn)生一定的困難。
在控制器中加入“I”：PI控制器
（“I”：會給我們帶來什么呢？）
為了提高P控制器的響應速度，我們在表達式中加入一個新的部分——積分，PID中的“I”。積分是高等數(shù)學中非常重要的內(nèi)容，在這里，我們只需要直截了當?shù)厥褂盟?br />積分用于計算誤差的動態(tài)求和。
每次我們讀取光電傳感器的值，并計算error（誤差）時，我們將error（誤差）加到一個變量中，這個變量我們稱之為integral（積分）。
integral（積分）=integral（積分）+error（誤差）
這個表達式不是普通的數(shù)學表達方式，它使用了將一系列數(shù)值累加的方法，這個方法在編程中經(jīng)常使用。在計算機程序里，這個表達式有著和數(shù)學不相同的含義。（在本文中，會用文字加重的方式來表明這是編程的方式，而不是普通的數(shù)學表達式。）這個“=”是賦值的意思，意味著將它右邊的計算結果賦值給左邊的那個變量名，就是計算機把原有的integral的值加上error的值，將結果賦值給integral。
每次我們讀取光電傳感器的值，并計算error（誤差）時，我們將error（誤差）加到一個變量中，這個變量我們稱之為integral（積分）。
integral（積分）=integral（積分）+error（誤差）
這個表達式不是普通的數(shù)學表達方式，它使用了將一系列數(shù)值累加的方法，這個方法在編程中經(jīng)常使用。在計算機程序里，這個表達式有著和數(shù)學不相同的含義。（在本文中，會用文字加重的方式來表明這是編程的方式，而不是普通的數(shù)學表達式。）這個“=”是賦值的意思，意味著將它右邊的計算結果賦值給左邊的那個變量名，就是計算機把原有的integral的值加上error的值，將結果賦值給integral。
接下來，同P的部分一樣，我們對integral乘以一個比例常數(shù)，這是另一個K。因為這個比例常數(shù)與積分部分有關，所以我們稱其為Ki。與P比例控制部分相同，我們把integral（積分）乘以一個常量，會得到一個修正值。我們要把這個修正值加到Turn變量中去。
Turn=Kp*(error) +Ki*(integral)
上面就是PI控制器的基本表達式。Turn是對馬達的修正，Kp*(error) 是比例控制部分，Ki*(integral)是積分控制部分。
積分控制部分有什么作用呢？如果誤差在幾次循環(huán)中都保持同樣的值，積分部分就會越來越大。例如，如果我們讀取光電傳感器值，計算出error為1，很短時間后，我們再次讀取光電傳感器值，這一次error為2，第三次的error還是為2，那么此時的integral將是1+2+2=5。Integral為5，但這一步的error只為2。在修正量中，積分部分能產(chǎn)生很大的影響，但通常來說，它需要比較長的時間才能發(fā)揮作用。
積分控制的另一個作用是能去除小的誤差。在巡線過程中，如果光電傳感器非常接近線的邊緣，但又不是正好在線的邊緣上，那么error會很小，只能產(chǎn)生一個很小的修正量。你可以通過改變比例控制中的Kp來修正這個小的error，但經(jīng)常會產(chǎn)生機器人的振蕩（來回搖擺）。積分控制部分就可以完美地修正小的誤差，因為integral（積分）是對errors（誤差）的累加，幾個連續(xù)的小誤差可以使integral（積分）大到足以發(fā)生作用。
我們可以把積分控制理解為控制器的"memory"（存儲器）。Integral（積分）表現(xiàn)的是error（誤差）累積的過程，可以持續(xù)向控制器提供修正誤差的方法。
積分控制部分有什么作用呢？如果誤差在幾次循環(huán)中都保持同樣的值，積分部分就會越來越大。例如，如果我們讀取光電傳感器值，計算出error為1，很短時間后，我們再次讀取光電傳感器值，這一次error為2，第三次的error還是為2，那么此時的integral將是1+2+2=5。Integral為5，但這一步的error只為2。在修正量中，積分部分能產(chǎn)生很大的影響，但通常來說，它需要比較長的時間才能發(fā)揮作用。
積分控制的另一個作用是能去除小的誤差。在巡線過程中，如果光電傳感器非常接近線的邊緣，但又不是正好在線的邊緣上，那么error會很小，只能產(chǎn)生一個很小的修正量。你可以通過改變比例控制中的Kp來修正這個小的error，但經(jīng)常會產(chǎn)生機器人的振蕩（來回搖擺）。積分控制部分就可以完美地修正小的誤差，因為integral（積分）是對errors（誤差）的累加，幾個連續(xù)的小誤差可以使integral（積分）大到足以發(fā)生作用。
我們可以把積分控制理解為控制器的"memory"（存儲器）。Integral（積分）表現(xiàn)的是error（誤差）累積的過程，可以持續(xù)向控制器提供修正誤差的方法。
有關積分的一些微妙問題
我隱藏了一個小問題（其實也不是小問題，但是我們要把它變成小問題）—— 時間。積分計算的其實是error×(單位時間） 的總和，單位時間(dT)是我們從上一次讀取光電傳感器值到這一次讀取光電傳感器值的時間間隔。
integral=integral+error*(dT)
因此，我們每次向integral中增加的應該是error×dT。測量機器人的dT是相當容易的事。在每次讀取光電傳感器值時，我們可以讀取計時器的值。如果我們從當前時間中減掉上一次的時間，就得到了從上一次讀值起的dT。如果不去測量這個dT，不做乘法計算，是不是會更好一些呢？如果這個dT總是相同值呢？如果我們每一次加入到integral中的部分，其dT值都是相同的，我們就能夠把因數(shù)dT從error×(dT)中提取出來，只做求和的運算。
integral=integral+error
實際上只有當我們需要用integral做另外的運算時，我們才需要去乘以dT。因此我們可以把這個時間因數(shù)藏起來。在PI控制器中，積分部分的表達式是Ki×(integral)×dT，其中Ki是一個需要我們進行微調(diào)的系數(shù)（就像Kp一樣），所以為什么不用一個新的Ki來代替Ki×dT這一部分呢？這個新的Ki與原來的是不同的，但是因為我們哪個都不知道，所以用哪一個、叫什么，都沒有關系。無論它叫什么、代表什么含義，我們都需要通過反復試驗來找到相對準確的值。
所以，只要把所有的時間步進值——dT設定成相同的（或大致相同的）值，我們就可以從積分控制部分中去除時間因素。
integral=integral+error*(dT)
因此，我們每次向integral中增加的應該是error×dT。測量機器人的dT是相當容易的事。在每次讀取光電傳感器值時，我們可以讀取計時器的值。如果我們從當前時間中減掉上一次的時間，就得到了從上一次讀值起的dT。如果不去測量這個dT，不做乘法計算，是不是會更好一些呢？如果這個dT總是相同值呢？如果我們每一次加入到integral中的部分，其dT值都是相同的，我們就能夠把因數(shù)dT從error×(dT)中提取出來，只做求和的運算。
integral=integral+error
實際上只有當我們需要用integral做另外的運算時，我們才需要去乘以dT。因此我們可以把這個時間因數(shù)藏起來。在PI控制器中，積分部分的表達式是Ki×(integral)×dT，其中Ki是一個需要我們進行微調(diào)的系數(shù)（就像Kp一樣），所以為什么不用一個新的Ki來代替Ki×dT這一部分呢？這個新的Ki與原來的是不同的，但是因為我們哪個都不知道，所以用哪一個、叫什么，都沒有關系。無論它叫什么、代表什么含義，我們都需要通過反復試驗來找到相對準確的值。
所以，只要把所有的時間步進值——dT設定成相同的（或大致相同的）值，我們就可以從積分控制部分中去除時間因素。
積分具有記憶性
關于integral（積分），還有最后一個要注意的細節(jié)。通常情況下，integral（積分）只能趨近于0，我們加入到integral（積分）中的error（誤差）值絕大多數(shù)都是符號相異的，在integral為0 時，它對控制器是不起任何作用的。例如，在經(jīng)過幾次循環(huán)后，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（誤差）在正負誤差之間均勻分布。
如果巡線機器人在線邊緣的左側(cè)，而積分部分累積的誤差也在線的左側(cè)，那么積分控制部分不僅要把機器人送回線的邊緣，還要使機器人越過線，到線的右側(cè)。如果有大的誤差持續(xù)一段時間，積分會趨向于無窮。這在包含有積分控制的控制器里，會引起一些問題。當積分部分設法修正的誤差很大時，積分的這種傾向會引起超調(diào)，我們必須在程序上做一些調(diào)整以避免出現(xiàn)問題。解決integral（積分）趨向于無窮問題，有兩個常見的解決方案：（1）將integral（積分）置零——每次error（誤差）為0，或error（誤差）改變符號，就將變量integral（積分）設置為0；（2）當計算一個新的integral時，對累積的integral乘以一個小于1的因數(shù)來抑制積分：
integral = (2/3)*integral + error
這樣，每次循環(huán)會把積分值降低1/3。如果你認為積分控制部分是一個控制器的“memory”（存儲器），那么這種抑制會在一段“較長”的時間后強迫它變得健忘起來。
如果巡線機器人在線邊緣的左側(cè)，而積分部分累積的誤差也在線的左側(cè)，那么積分控制部分不僅要把機器人送回線的邊緣，還要使機器人越過線，到線的右側(cè)。如果有大的誤差持續(xù)一段時間，積分會趨向于無窮。這在包含有積分控制的控制器里，會引起一些問題。當積分部分設法修正的誤差很大時，積分的這種傾向會引起超調(diào)，我們必須在程序上做一些調(diào)整以避免出現(xiàn)問題。解決integral（積分）趨向于無窮問題，有兩個常見的解決方案：（1）將integral（積分）置零——每次error（誤差）為0，或error（誤差）改變符號，就將變量integral（積分）設置為0；（2）當計算一個新的integral時，對累積的integral乘以一個小于1的因數(shù)來抑制積分：
integral = (2/3)*integral + error
這樣，每次循環(huán)會把積分值降低1/3。如果你認為積分控制部分是一個控制器的“memory”（存儲器），那么這種抑制會在一段“較長”的時間后強迫它變得健忘起來。
PI控制器的虛擬代碼
在控制器中加入積分控制部分，我們需要增加新的變量Ki和integral。別忘了，為了進行整數(shù)運算，我們要把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中因數(shù)100的影響 !
powerA = Tp + Turn ! A馬達的功率值
powerC = Tp - Turn ! C馬達的功率值
MOTOR A direction=forwardpower=powerA! 在馬達模塊里設置A馬達的功率值和轉(zhuǎn)向
MOTOR C direction=forwardpower=powerC! 在馬達模塊里設置C馬達的功率值和轉(zhuǎn)向
end loop forever ! 結束循環(huán)，返回，進行下一次循環(huán) ! “ ! ! !
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中因數(shù)100的影響 !
powerA = Tp + Turn ! A馬達的功率值
powerC = Tp - Turn ! C馬達的功率值
MOTOR A direction=forwardpower=powerA! 在馬達模塊里設置A馬達的功率值和轉(zhuǎn)向
MOTOR C direction=forwardpower=powerC! 在馬達模塊里設置C馬達的功率值和轉(zhuǎn)向
end loop forever ! 結束循環(huán)，返回，進行下一次循環(huán) ! “ ! ! !
在控制器中加入“D”：完整的PID控制器
（“D”：接下來會發(fā)生什么？）
我們的控制器里有了比例控制部分，用于糾正當前的誤差；有了積分控制部分，用于糾正過去的誤差。有沒有一種辦法能讓我們及時預測未來，對還沒發(fā)生的誤差進行糾正呢？
這就要用到高等數(shù)學里的另一個概念——導數(shù)，就是PID中的D。像積分一樣，導數(shù)也是高等數(shù)學中相當重要的數(shù)學方法。
假定誤差的下一個變化與當前最后一個變化是相同的，我們據(jù)此來預測將來。
這個意思是說，下一個誤差的期望值是：當前誤差+前兩次傳感器采樣誤差的變化量。在兩個連續(xù)點之間的誤差變化量就叫做導數(shù)。導數(shù)是一條直線的斜率。
這就要用到高等數(shù)學里的另一個概念——導數(shù)，就是PID中的D。像積分一樣，導數(shù)也是高等數(shù)學中相當重要的數(shù)學方法。
假定誤差的下一個變化與當前最后一個變化是相同的，我們據(jù)此來預測將來。
這個意思是說，下一個誤差的期望值是：當前誤差+前兩次傳感器采樣誤差的變化量。在兩個連續(xù)點之間的誤差變化量就叫做導數(shù)。導數(shù)是一條直線的斜率。
看上去，計算起來有些復雜。用數(shù)據(jù)舉例能幫助我們說明這個問題。讓我們假設當前誤差是2，前一個誤差是5，那么我們預測的下一個誤差會是多少呢？誤差的變化，也就是導數(shù)，是：
（當前誤差）-（前一個誤差）
按照我們的數(shù)值就是 2 - 5 = -3 。因此，當前的導數(shù)就是-3 。我們使用導數(shù)預測下一個誤差就是
（下一個誤差） = （當前誤差）+ （當前導數(shù)）
按照我們假定的數(shù)值就是2 + （-3） = -1 。因此，我們預測下一個誤差會是-1 。在實踐中，我們實際上并不是要完全一致地預測下一個誤差。相反地，我們只是在控制器的公式中直接使用導數(shù)。
導數(shù)控制部分，與積分控制部分一樣，實際上也包含時間因素，正式的導數(shù)控制部分是：
Kd(導數(shù))/(dT)
與比例控制和微分控制一樣，我們需要對導數(shù)乘上一個常量。因為這個常量是與導數(shù)相關的，因此被稱之為Kd。注意，在導數(shù)控制部分，我們是除以dT，而在積分控制部分，我們是乘以dT。我們會和在積分控制部分一樣，采用同樣的技巧從導數(shù)控制部分去掉這個dT。如果在每一個循環(huán)中dT的值相同，分數(shù)Kd/dT就是一個常量。我們可以用另外一個Kd來代替Kd/dT。同先前的Ks一樣，這個K值是未知的，需要通過反復試驗來確定，因此它是Kd/dT或是一個新的Kd，都沒有關系。
現(xiàn)在我們可以寫出PID控制器的完整公式了：
Turn（轉(zhuǎn)向）= Kp*error(誤差) +Ki*integral(積分)+Kd*derivative(導數(shù))
顯然，我們可以“預測將來”了，但是這么做有什么幫助？預測又能準確到什么程度呢？
如果當前誤差比前一個誤差更糟糕，導數(shù)控制部分就會糾正這一誤差。如果當前誤差比前一誤差要好一些，導數(shù)控制控制部分就會停止控制器去糾正這個誤差。第二個非常有用的作用是，誤差越接近于0，我們就越接近想正確停下的那個點。但是系統(tǒng)可能需要一段時間來響應馬達功率的變化，因此我們在誤差趨近于0之前，就要開始降低馬達的功率，以防止過沖。不用擔心導數(shù)控制部分的方程式很復雜，你所要做的只有一件事，就是按照正確的順序做減法運算。所謂正確的順序，就是用“當前”減去“前一個”。因此在計算導數(shù)時，我們要用“當前誤差”減去“前一個誤差”。
如果當前誤差比前一個誤差更糟糕，導數(shù)控制部分就會糾正這一誤差。如果當前誤差比前一誤差要好一些，導數(shù)控制控制部分就會停止控制器去糾正這個誤差。第二個非常有用的作用是，誤差越接近于0，我們就越接近想正確停下的那個點。但是系統(tǒng)可能需要一段時間來響應馬達功率的變化，因此我們在誤差趨近于0之前，就要開始降低馬達的功率，以防止過沖。不用擔心導數(shù)控制部分的方程式很復雜，你所要做的只有一件事，就是按照正確的順序做減法運算。所謂正確的順序，就是用“當前”減去“前一個”。因此在計算導數(shù)時，我們要用“當前誤差”減去“前一個誤差”。
PID控制器的虛擬代碼
在控制器中加上導數(shù)控制部分，我們需要為Kd增加一個新的變量，還需要增加一個變量來記錄最后一個誤差。同樣不要忘記，我們在Ks上乘以100，來進行整數(shù)運算。
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 ! 用于存儲導數(shù)的變量
Loop forever
LightValue = read light sensor ! 當前光電傳感器的讀值
error = LightValue - offset ! 減掉 offset（補償量），計算誤差值
integral = integral + error ! 計算積分值
derivative = error - lastError !計算導數(shù)值
Turn = Kp*error + Ki*integral +Kd*derivative! “比例控制部分”+“積分控制部分”+“導數(shù)控制部分”
Turn = Turn/100 ! 記住消除Kp，Ki和 Kd中因數(shù)100的影響!
powerA = Tp + Turn ! A馬達功率值
powerC = Tp - Turn ! C馬達功率值
MOTOR A direction=forwardpower=PowerA !在馬達模塊中設置A馬達的功率值和轉(zhuǎn)向
MOTOR C direction=forwardpower=PowerC ! 在馬達模塊中設置A馬達的功率值和轉(zhuǎn)向
lastError = error !把當前誤差存儲在變量lastError中，作為下一次循環(huán)的最后一個誤差
end loop forever ! 結束循環(huán)，返回，進行下一次循環(huán)
以上就是PID控制器用于巡線機器人的完整代碼。這其中最困難的部分，就是如何把Kp,Ki和Kd調(diào)整到最好的，至少是調(diào)整到還說的過去。
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 ! 用于存儲導數(shù)的變量
Loop forever
LightValue = read light sensor ! 當前光電傳感器的讀值
error = LightValue - offset ! 減掉 offset（補償量），計算誤差值
integral = integral + error ! 計算積分值
derivative = error - lastError !計算導數(shù)值
Turn = Kp*error + Ki*integral +Kd*derivative! “比例控制部分”+“積分控制部分”+“導數(shù)控制部分”
Turn = Turn/100 ! 記住消除Kp，Ki和 Kd中因數(shù)100的影響!
powerA = Tp + Turn ! A馬達功率值
powerC = Tp - Turn ! C馬達功率值
MOTOR A direction=forwardpower=PowerA !在馬達模塊中設置A馬達的功率值和轉(zhuǎn)向
MOTOR C direction=forwardpower=PowerC ! 在馬達模塊中設置A馬達的功率值和轉(zhuǎn)向
lastError = error !把當前誤差存儲在變量lastError中，作為下一次循環(huán)的最后一個誤差
end loop forever ! 結束循環(huán)，返回，進行下一次循環(huán)
以上就是PID控制器用于巡線機器人的完整代碼。這其中最困難的部分，就是如何把Kp,Ki和Kd調(diào)整到最好的，至少是調(diào)整到還說的過去。
評論