英文原文地址：http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html
中文翻譯地址：http://bbs.cmnxt.com/thread-5688-1-1.html

文章版權(quán)屬于原作者及譯者所有，本處為學(xué)習(xí)轉(zhuǎn)載

PID控制器是一種常用的控制技術(shù)，常用于多種機(jī)械裝置（如車輛、機(jī)器人、火箭）中。用數(shù)學(xué)方式來(lái)描述PID控制器是非常復(fù)雜的。本文描述了如何在使用NXT-G編程的LEGO機(jī)器人上創(chuàng)建PID控制器。文中將以實(shí)例來(lái)說(shuō)明如何創(chuàng)建PID來(lái)完成機(jī)器人巡線任務(wù)。PID創(chuàng)建完成后，經(jīng)過(guò)簡(jiǎn)單的修改就可以應(yīng)用到其他地方，如，讓機(jī)器人跑直線，做兩輪平衡機(jī)器人。其實(shí)學(xué)過(guò)微積分的人很容易理解PID的典型描述，本文是寫(xiě)給那些對(duì)PID幾乎沒(méi)有任何概念的讀者，比如參加FLL比賽的3~8年級(jí)的孩子們。考慮到大家可能不了解微積分，因此我盡量不使用微積分，從非常低的起點(diǎn)開(kāi)始建造整個(gè)概念。先來(lái)看看一個(gè)適于巡線的機(jī)器人是什么樣的結(jié)構(gòu)。看下圖，這個(gè)機(jī)器人用兩個(gè)馬達(dá)驅(qū)動(dòng)，分別與車輪A、C連接，前端裝有垂直向下的光電傳感器，紅圈標(biāo)出的部分就是光電傳感器能“看到”的部分。帶箭頭的大長(zhǎng)方形表示機(jī)器人的其余部分，箭頭指示機(jī)器人的運(yùn)動(dòng)方向。

巡線是機(jī)器人的基本技術(shù)，也是大家學(xué)習(xí)機(jī)器人時(shí)最先要做的。能夠巡線的自動(dòng)裝置具有機(jī)器人的全部特點(diǎn)：使用傳感器收集周圍環(huán)境的信息，并據(jù)此調(diào)整機(jī)器人的運(yùn)動(dòng)狀態(tài)。巡線機(jī)器人可以使用1個(gè)光電傳感器、2個(gè)光電傳感器、一打光電傳感器或者裝上你所有的光電傳感器。實(shí)際上，你使用的光電傳感器越多，巡線的效果越好。只使用1個(gè)光電傳感器也可以讓機(jī)器人精確的巡線（即使線條是有弧度的），但是機(jī)器人移動(dòng)過(guò)快時(shí)容易“飛線”（“飛線”——指機(jī)器人脫離線條，不能繼續(xù)沿著線移動(dòng)）。一般來(lái)說(shuō)，使用的傳感器越多，巡線的速度越快。現(xiàn)在我們來(lái)試驗(yàn)第一個(gè)方法（非PID方式）。巡線其實(shí)是讓機(jī)器人沿著線的邊緣走，因?yàn)槿绻刂诰€本身走，當(dāng)機(jī)器人偏離黑線，傳感器“看到白色”時(shí)，我們不知道機(jī)器人到底在線的哪一邊，是在線的右邊還是左邊？如果沿著線的邊緣走，當(dāng)光電傳感器“看到白色”，我們知道機(jī)器人在線邊緣（線）的左邊，當(dāng)光電傳感器“看到黑色”，我們知道機(jī)器人在線邊緣的右邊（在線上）。因?yàn)闄C(jī)器人跟隨的是線條的左邊，因此這種方式被稱為“左手法則”。我們需要知道當(dāng)光電傳感器“看到白色”和“看到黑色”時(shí)返回的讀數(shù)值。一個(gè)典型的非校準(zhǔn)傳感器（數(shù)值0~100）“看到白色”會(huì)返回50，“看到黑色”會(huì)返回40。我們可以在一條數(shù)據(jù)線段上標(biāo)出光電傳感器的讀值，來(lái)幫助我們理解如何將光電傳感器的讀值變化轉(zhuǎn)變?yōu)闄C(jī)器人的運(yùn)動(dòng)變化。以下是我們畫(huà)出的從“白”到“黑”的光電傳感器讀值。

我們把這個(gè)數(shù)值線段平分為兩部分：當(dāng)光電傳感器值小于45，讓機(jī)器人左轉(zhuǎn)；當(dāng)光電傳感器值大于45，讓機(jī)器人右轉(zhuǎn)。在這里，我們不考慮機(jī)器人的轉(zhuǎn)向動(dòng)作的精確性。在相對(duì)較直的線上，機(jī)器人的轉(zhuǎn)向動(dòng)作可以比較細(xì)小；在有很多彎的線上，機(jī)器人通常要有明顯的轉(zhuǎn)向動(dòng)作。做動(dòng)作細(xì)小的轉(zhuǎn)向時(shí)，你可以把速度快的輪子的馬力值設(shè)置為50%，速度慢的輪子的馬力值設(shè)置為20%。有很多彎的現(xiàn)上做明顯轉(zhuǎn)向動(dòng)作時(shí)，你可以在快的輪子上設(shè)置30%的馬力值，在慢的輪子上使用緩?fù)；蛲Ｖ埂o(wú)論你在輪子上設(shè)置什么數(shù)值的馬力值，在做左右不同轉(zhuǎn)向時(shí)，這個(gè)設(shè)置應(yīng)該是一樣的，即在一側(cè)的輪子上設(shè)置較大的馬力值，在另一側(cè)輪子上設(shè)置較小的馬力值（或設(shè)置為停止）。

這種巡線方式能夠完成巡線任務(wù)，但效果并不是很好。在比較直的線上完成巡線任務(wù)，在編程中設(shè)置動(dòng)作細(xì)小的轉(zhuǎn)彎方式，整體巡線效果看起來(lái)還算不錯(cuò)；但是如果線上有較大的彎度，你又采用明顯的轉(zhuǎn)向動(dòng)作讓機(jī)器人完成巡線，機(jī)器人就會(huì)來(lái)回?cái)[動(dòng)，橫向穿過(guò)線條。機(jī)器人只“知道”兩件事情：轉(zhuǎn)左和轉(zhuǎn)右。用這種方法巡線，通常機(jī)器人的速度不會(huì)很快，而且看起來(lái)很糟糕。

即使線是直的，這種方法也不能使機(jī)器人走直線，甚至不能完全對(duì)準(zhǔn)線的邊緣。如何使巡線更有效率呢？

讓我們來(lái)調(diào)整一下。把光電傳感器的讀值線段分成三部分。當(dāng)光電傳感器值低于43時(shí)，我們讓機(jī)器人轉(zhuǎn)左。光電傳感器值在44到47之間時(shí)，我們讓機(jī)器人直行。光電傳感器值大于47時(shí)，我們讓機(jī)器人轉(zhuǎn)右。這在NXT-G程序中，可以在判斷模塊中選擇yes/no來(lái)實(shí)現(xiàn)。你實(shí)際上只需做兩次判斷，而不是三次。第二種巡線方式效果比第一種方式好的多。至少機(jī)器人有時(shí)會(huì)直接向前走了。與第一種巡線方式一樣，你依然要根據(jù)線的曲直特點(diǎn)來(lái)決定使用哪種轉(zhuǎn)向方式（細(xì)小或者明顯的轉(zhuǎn)向動(dòng)作）。機(jī)器人依舊會(huì)有相當(dāng)數(shù)量的來(lái)回?cái)[動(dòng)。精明的讀者也許會(huì)想“如果使用3個(gè)光電傳感器是不會(huì)比2個(gè)光電傳感器要好些呢？在增加更多的光電傳感器會(huì)怎樣？”這就是PID的開(kāi)始了。

PID中的P比例控制是關(guān)鍵

如果我們把光電傳感器讀值的數(shù)據(jù)線段分成更多的段，會(huì)怎樣呢？我們要解決的第一件事情是，當(dāng)光電傳感器讀值的數(shù)據(jù)線段的分段數(shù)超過(guò)3段時(shí)，要如何確定“turn（轉(zhuǎn)向）”的取值。在我們的第一種巡線方式啊中，機(jī)器人只做兩件事情，轉(zhuǎn)左或轉(zhuǎn)右，“turn（轉(zhuǎn)向）”的數(shù)值是一樣的，只是方向不同。在第二種巡線方式中，我們?cè)谧笥覂蓚€(gè)轉(zhuǎn)向的基礎(chǔ)上加上了“直行”。在光電傳感器讀值的數(shù)據(jù)線段分段超過(guò)3個(gè)時(shí)，我們需要更多“種類”的“turn（轉(zhuǎn)向）”。為了幫助理解“更多種類的turn（轉(zhuǎn)向）”，我們重新畫(huà)出光電傳感器讀值的數(shù)據(jù)線段，并把它轉(zhuǎn)換為圖形。X軸（水平線）為光電傳感器讀值值，與上面的光電傳感器讀值的數(shù)據(jù)線段一樣。Y軸（垂直線)是“turn（轉(zhuǎn)向）”軸。

左邊的圖形表示的是我們第一種巡線方式——將光電傳感器讀值分成兩段的情況，機(jī)器人只能做兩件事（用藍(lán)色的線表示），轉(zhuǎn)左或轉(zhuǎn)右，除了方向以外，轉(zhuǎn)向值是一樣的。中間的圖形是第二種巡線方式——將光電傳感器讀值分成三段的情況，中間增加的一段是機(jī)器人直行的部分（turn=0），轉(zhuǎn)向部分與前面的第一種巡線方式是一樣的。右側(cè)的圖形是一個(gè)比例控制的巡線機(jī)器人，在兩個(gè)極限點(diǎn)之間的轉(zhuǎn)向變化很平滑。如果光電傳感器讀取的光值表明機(jī)器人離線很近，機(jī)器人就做小的轉(zhuǎn)彎；如果讀取的光值表明機(jī)器人離線很遠(yuǎn)，機(jī)器人就做較大的轉(zhuǎn)彎。比例是一個(gè)重要的概念。比例的意思就是在兩個(gè)變量之間存在線性關(guān)系，簡(jiǎn)單的說(shuō)，就是變量之間的關(guān)系呈現(xiàn)為一條直線（如右側(cè)圖形所示）。

直線的表達(dá)式為：

y= mx + b

這里，x,y是指直線上任意一點(diǎn)的坐標(biāo)值（x,y），m是這條直線的斜率，b是直線在Y軸上的截距（當(dāng)x=0時(shí)，直線通過(guò)Y軸上的點(diǎn)，該點(diǎn)在Y軸上的坐標(biāo)值）。直線斜率的定義為直線上任意兩點(diǎn)y值的變化量除以x值的變化量。我來(lái)把圖形和表達(dá)式變得簡(jiǎn)單一些。首先，我們將光電傳感器讀值線段（X軸）的中心點(diǎn)定為0，因?yàn)槲覀兊墓怆妭鞲衅髯x值范圍是40到50，我們把所有光電傳感器讀數(shù)都減掉45（這是40和50的平均值，(40+50)/2），得到的結(jié)果稱為“error（誤差）”。當(dāng)光電傳感器讀數(shù)為47時(shí)，可得到error=47-45=2。這個(gè)error（誤差）表明了機(jī)器人的光電傳感器離線的邊緣有多遠(yuǎn)。當(dāng)光電傳感器正好在線的邊緣上，“error（誤差）”為0（因?yàn)榇藭r(shí)光電傳感器的讀值為45，而我們要從光電傳感器讀值中減掉45）。如果光電傳感器全部處在白色的地方，“error（誤差）”為 +5，如果光電傳感器全部處在黑色的地方，“error（誤差）”為 -5。

在上面的圖形中，我已經(jīng)用“error（誤差）”來(lái)表示X坐標(biāo)軸。因?yàn)檫@條直線正好在原點(diǎn)處通過(guò)Y軸，因此b的取值為0，這樣表達(dá)式會(huì)變得簡(jiǎn)單一些：

y = mx

或者使用我們的方法：Turn= merror我們還沒(méi)有對(duì)轉(zhuǎn)向軸做出定義，所以現(xiàn)在我們確定轉(zhuǎn)向的范圍是從-1（最大左轉(zhuǎn)）到+1（最大右轉(zhuǎn)），0轉(zhuǎn)向的意思就是直行。上面圖形中直線的斜率就可以用標(biāo)為紅色的兩個(gè)點(diǎn)計(jì)算出來(lái)（其實(shí)直線上任意兩點(diǎn)均可使用）。斜率= m = (y值的變化量)/(x值的變化量) = ( 1- (-1)) / (-5- 5 ) = -2/10 = -0.2斜率是一個(gè)比例常量，用它乘以（x值）就可得到“（轉(zhuǎn)向）”(y值）。請(qǐng)一定記住這一點(diǎn)。在各種PID文獻(xiàn)中，斜率（也叫做比例常數(shù)、直線表達(dá)式中的m）被稱作"K"。各式各樣的Ks出現(xiàn)在PID文獻(xiàn)中。你可以把K（或m，或斜率，或比例常數(shù)）看做是一個(gè)換算系數(shù)，用K把一個(gè)數(shù)字（光電傳感器讀值或我們例子中的error（誤差））轉(zhuǎn)換成另外一個(gè)數(shù)字（如Turn（轉(zhuǎn)向））。這就是K的作用，非常簡(jiǎn)單也非常強(qiáng)大。那么在我們的直線表達(dá)式中使用這些新的變量名字：Turn= K(error)用語(yǔ)言表達(dá)就是：將誤差值error乘以比例常數(shù)K得到所需的轉(zhuǎn)向值Turn。這個(gè)Turn 值就是P控制器的輸出結(jié)果，因?yàn)樗簧婕氨壤刂疲环Q為“比例控制部分”。“****error****”的取值范圍是由光電傳感器的設(shè)置、巡線測(cè)試紙的顏色等因素決定的。你可能已經(jīng)注意到了，在最后一個(gè)圖形里，直線沒(méi)有延伸到error（誤差）值-5 到 +5 的范圍以外。在-5 到 +5 的范圍以外，我們就不能判斷光電傳感器到底離線有多遠(yuǎn)了。當(dāng)光電傳感器完全看不到任何黑線時(shí)，它看到的所有“白色”都是一樣的。當(dāng)光電傳感器離線的邊緣太遠(yuǎn)時(shí)，光電傳感器讀取到的光值變成恒定的數(shù)值，這就意味著光電傳感器的讀與error（誤差）不再是比例關(guān)系。我們只能在光電傳感器相當(dāng)接近黑線時(shí)，判斷光電傳感器離線的邊緣有多遠(yuǎn)距離，在非常小的數(shù)值范圍內(nèi)，光電傳感器的讀值與這個(gè)距離是成比例的，因此，我們的光電傳感器值要設(shè)置在能給出比例關(guān)系的有限的范圍內(nèi)。超出這個(gè)范圍，就只能給出機(jī)器人調(diào)整的正確方向，但數(shù)量大小是錯(cuò)誤的，光電傳感器讀值或是誤差會(huì)小于實(shí)際情況，這樣在修正誤差時(shí)，就不會(huì)有很好的效果。在PID文獻(xiàn)中，把傳感器能給出比例響應(yīng)的范圍稱為“比例范圍”。在PID控制中，比例范圍是另一個(gè)非常重要的概念。在我們巡線機(jī)器人的應(yīng)用中，光電傳感器讀值的比例范圍是40到50，誤差的比例范圍是-5 到+5 ，馬達(dá)的比例范圍是-100（全馬力后退）到 +100（全馬力前進(jìn)）。以下是有關(guān)比例范圍的兩個(gè)重要內(nèi)容：（1）我們希望比例范圍盡可能的寬。光電傳感器的比例范圍是相當(dāng)小的，就是說(shuō)，光電傳感器必須很接近線的邊緣，才能獲得比例信息。比例范圍的寬度主要取決于光電傳感器距離巡線測(cè)試紙的高度有多少。如果光電傳感器非常靠近巡線測(cè)試紙，如1/16英寸（約0.16厘米），那么光電傳感器在巡線測(cè)試紙上看到范圍只是一個(gè)很小的圓圈。光電傳感器的一個(gè)很小的移動(dòng)就會(huì)產(chǎn)生-5到+5 范圍的error（誤差），也就是比例范圍。你也許會(huì)說(shuō)，光電傳感器的視野狹窄，只能看到巡線測(cè)試紙的很小的一部分，光電傳感器要非常接近線的邊緣，讀取的光電傳感器值既不是“黑”，也不是“白”。如果光電傳感器距離巡線測(cè)試紙的高度高一些，那么光電傳感器在巡線測(cè)試紙上看到的范圍就是一個(gè)大一些的圓圈。光電傳感器距離巡線測(cè)試紙的高度大約為1/2英寸（大約1.27厘米）時(shí)，在巡線測(cè)試紙上能看到的范圍是一個(gè)直徑大約1/2英寸的圓圈。光電傳感器處于這個(gè)高度上，比例范圍更大，因?yàn)楣怆妭鞲衅髟诰嚯x線的邊緣+/-1/2英寸寬度的范圍內(nèi)，就可以保持比例輸出。將光電傳感器位置提高有兩個(gè)缺點(diǎn)，光電傳感器位置提高后更容易對(duì)環(huán)境光做出錯(cuò)誤響應(yīng)；在區(qū)分黑和白時(shí)，也與位置較低的光電傳感器有些差異。光電傳感器距離巡線測(cè)試紙的高度足夠大時(shí)，對(duì)黑色和白色所讀取的值是一樣的。（2）在比例范圍之外，控制器只能把機(jī)器人向正確的方向移動(dòng)，但也只是趨向于正確。控制器的比例響應(yīng)是受比例范圍限制的。

從P到實(shí)際的馬達(dá)功率值

我們應(yīng)該如何設(shè)置轉(zhuǎn)向時(shí)的馬達(dá)功率值呢？做轉(zhuǎn)向的一個(gè)方法是：定義一個(gè)“目標(biāo)功率”，我稱之為"Tp"。Tp是當(dāng)error（誤差）=0時(shí)，機(jī)器人做直行得兩個(gè)馬達(dá)功率值。當(dāng)error（誤差）不為0時(shí)，我們用表達(dá)式Turn = K*(error)來(lái)計(jì)算如何改變兩個(gè)馬達(dá)的功率，一個(gè)馬達(dá)的功率為T(mén)p+Turn，另一個(gè)馬達(dá)的功率為T(mén)p-Turn。注意，因?yàn)槲覀兊膃rror（誤差）范圍是-5 到 +5，Turn（轉(zhuǎn)向）的值也會(huì)有正值和負(fù)值，相當(dāng)于做不同方向的轉(zhuǎn)向。這正是我們所需要的，它能自動(dòng)地正確設(shè)置馬達(dá)功率值，確定哪一個(gè)馬達(dá)速度快，哪一個(gè)馬達(dá)速度慢。我們假定左側(cè)的馬達(dá)接入端口A，其功率值為T(mén)p+Turn 的值；右側(cè)馬達(dá)接入端口C，其功率值為T(mén)p-Turn 的值。當(dāng)error 為正時(shí)，Turn值為正，Tp+Turn的值比Tp大，左側(cè)的馬達(dá)速度加快，右側(cè)的馬達(dá)速度減慢。當(dāng)error 改變符號(hào)變?yōu)樨?fù)值時(shí)（這就意味著機(jī)器人已經(jīng)越過(guò)線的邊緣，看到“黑色”了），此時(shí)Tp+Turn的值比Tp小，左側(cè)的馬達(dá)速度減慢，Tp-Turn的值比Tp大，右側(cè)的馬達(dá)速度加快。簡(jiǎn)單嗎？希望我們繼續(xù)往下進(jìn)行時(shí)，你會(huì)理解得更清楚一點(diǎn)。

P控制器的虛擬代碼

首先我們要測(cè)出光電傳感器讀取黑色和白色時(shí)的光電傳感器讀值。根據(jù)這兩個(gè)數(shù)值，我們能夠計(jì)算出offset（補(bǔ)償量），將光電傳感器讀值減掉這個(gè)數(shù)值就可轉(zhuǎn)換成 error（誤差）值。offset（補(bǔ)償量）是白色和黑色光電傳感器值的平均值。為簡(jiǎn)單起見(jiàn)，我假定offset（補(bǔ)償量）已經(jīng)測(cè)量完畢，并存儲(chǔ)在叫做offset的變量里。（讓機(jī)器人自己測(cè)量白和黑的光電傳感器讀值，并計(jì)算offset，會(huì)更好）

常數(shù)K被稱為Kp（比例控制器中恒量K）。要為Kp設(shè)定一個(gè)初始的推測(cè)值，然后通過(guò)反復(fù)試驗(yàn)來(lái)修正它。我們可以根據(jù)機(jī)器人和傳感器的特性估算出一個(gè)值：將Tp（目標(biāo)功率）設(shè)為50，當(dāng)誤差為0時(shí)，兩個(gè)馬達(dá)都以50的功率值轉(zhuǎn)動(dòng)；誤差范圍為-5 到 +5。我們期望當(dāng)誤差從0變化到-5時(shí)，馬達(dá)的功率值從50變化到0，就是說(shuō)Kp（斜率——y的變化量除以x的變化量)為：
Kp = (0 - 50)/(-5 - 0)= 10

我們用 Kp=10 將error (誤差）值轉(zhuǎn)換為turn(轉(zhuǎn)向）值。這句話中，轉(zhuǎn)換的意思是“error”（誤差）每發(fā)生1個(gè)單位的變化，我們就將一個(gè)馬達(dá)的功率值提高10，另一個(gè)馬達(dá)的功率降低10.

虛擬代碼如下：

Kp = 10                                           ! 初始化變量
offset = 45
Tp = 50
Loop forever
   LightValue = read light sensor                     ! 當(dāng)前光電傳感器的讀值
   error = LightValue - offset                        ! 減去offset（補(bǔ)償量）計(jì)算error（誤差）
   Turn = Kp * error                        ! “比例控制部分”, 我們希望馬達(dá)的功率值改變多少？
   powerA = Tp + Turn                  ! A馬達(dá)的功率值
   powerC = Tp - Turn                  ! C馬達(dá)的功率值
   MOTOR A direction=forward power=powerA   ! 在馬達(dá)模塊中設(shè)置這個(gè)功率值
   MOTOR C direction=forward power=powerC   ! 設(shè)置另一個(gè)馬達(dá)的功率值
end loop forever                              ! 結(jié)束這個(gè)循環(huán)，返回，進(jìn)行下一次循環(huán)

如果機(jī)器人在運(yùn)行時(shí)，表現(xiàn)出的狀態(tài)是遠(yuǎn)離線的邊緣，而不是尋找線的邊緣，你需要改變一下轉(zhuǎn)向的方向。把Kp的值變?yōu)?10，看看會(huì)怎樣。如果這樣做可以糾正機(jī)器人的轉(zhuǎn)向方向，就把Kp的值變回+10，將設(shè)置馬達(dá)功率的兩行代碼做如下改動(dòng)：

powerA = Tp - Turn
powerC = Tp + Turn

在這個(gè)P控制器里有兩個(gè)“可調(diào)參數(shù)”和一個(gè)恒量。恒量就是offset（補(bǔ)償量）（黑色和白色光電傳感器讀值的平均數(shù)）。你需要編寫(xiě)一小段程序，在巡線測(cè)試紙上用你的機(jī)器人來(lái)測(cè)量光電傳感器讀值。你需要測(cè)量出“black（黑）”和“white白”的光電傳感器讀值，然后計(jì)算平均值，并把平均值寫(xiě)入P控制器程序中的offset變量。幾乎所有的巡線機(jī)器人都要做這一步工作，你可以人工進(jìn)行，也可以通過(guò)編寫(xiě)程序代碼讓機(jī)器人自動(dòng)完成。
Kp 值和Tp（目標(biāo)功率）值是可調(diào)參數(shù)。可調(diào)參數(shù)必須經(jīng)過(guò)反復(fù)試驗(yàn)才能確定。 Kp決定了當(dāng)機(jī)器人漸漸離開(kāi)線的邊緣時(shí)，控制器讓機(jī)器人返回線的邊緣的速度有多快；Tp決定了機(jī)器人沿著線向前移動(dòng)的速度有多快。

如果線比較直，你可以將Tp的值設(shè)置的高一些，提高機(jī)器人的運(yùn)行速度；將Kd 的值設(shè)置的小一些，使機(jī)器人的轉(zhuǎn)向動(dòng)作（修正）更細(xì)小。

如果線比較彎曲，尤其是有銳角彎時(shí)，要限制Tp的最大值。如果Tp超過(guò)了最大值，無(wú)論怎樣設(shè)置Kp，機(jī)器人在遇到曲線時(shí)，都會(huì)因?yàn)橐苿?dòng)過(guò)快而“飛線”（機(jī)器人脫離線條）。如果Tp值很小，機(jī)器人移動(dòng)速度就會(huì)很慢，此時(shí)無(wú)論將Kp設(shè)置成任何數(shù)值，機(jī)器人都會(huì)完成巡線任務(wù)。而我們的目標(biāo)就是在保證機(jī)器人能夠完成巡線的情況下，讓它盡可能地跑的快一點(diǎn)。

我們推測(cè)出了Kp的一個(gè)初始值——Kp=10。對(duì)于Tp（目標(biāo)功率值），你可以從一個(gè)比較低的值開(kāi)始，比如15（機(jī)器人會(huì)移動(dòng)的非常慢）。試一試，看看它的運(yùn)行情況。當(dāng)機(jī)器人因轉(zhuǎn)向過(guò)慢而出現(xiàn)“飛線”情況，就增大Kp，并繼續(xù)嘗試。如果機(jī)器人因來(lái)回?cái)[動(dòng)、過(guò)于活躍而出現(xiàn)“飛線”情況，就減小Kp。如果機(jī)器人巡線的狀態(tài)非常好，就提高Tp，觀察機(jī)器人在更快速度下的巡線情況。盡管Kp通常不會(huì)有太大的變化，對(duì)于每一個(gè)新的Tp值，你都需要確定新的Kp 值。

沿著一條較直的線做巡線，通常比較簡(jiǎn)單。沿著一條彎度不大的曲線巡線，有一點(diǎn)難。沿著一條有急遽彎度的曲線巡線，是最難的。如果機(jī)器人移動(dòng)地很緩慢，那么即便是使用非常基本的控制器，機(jī)器人也幾乎可以完成任何巡線任務(wù)。我們想要的是機(jī)器人能夠以非常好的速度完成巡線，能夠處理有普通彎度的巡線任務(wù)（有著非常尖銳轉(zhuǎn)角的巡線任務(wù)，通常需要采用更專業(yè)的巡線機(jī)器人）。

對(duì)于各種不同類型的巡線任務(wù)（線的寬度不同，轉(zhuǎn)彎的尖銳程度不同等）來(lái)說(shuō)，最好的P控制器很可能是不同的。換句話說(shuō)，為一條特定的線和特定的機(jī)器人而調(diào)整出來(lái)的P控制器（或者PID控制器），對(duì)其他的線和機(jī)器人來(lái)說(shuō)，不一定適用。程序代碼可以在很多機(jī)器人（和很多巡線任務(wù)）上使用，但是Kp, Tp 和offset等參數(shù)必須要針對(duì)每一個(gè)機(jī)器人和每一種應(yīng)用情況重新進(jìn)行調(diào)整。

在一臺(tái)不認(rèn)識(shí)小數(shù)點(diǎn)的計(jì)算機(jī)上做數(shù)學(xué)運(yùn)算會(huì)有一些問(wèn)題

注意：NXT-G 1.1 版本只支持整數(shù)運(yùn)算，NXT-G 2.0 版本支持浮點(diǎn)運(yùn)算。如果使用2.0及以上版本的NXT-G程序，你無(wú)需了解以下內(nèi)容，可以直接跳過(guò)這一部分。

在調(diào)整P控制器的過(guò)程中，你會(huì)對(duì)Kp 的值做上下調(diào)整。 預(yù)期的Kp取值范圍可能完全取決于P控制器是如何計(jì)算的、輸入范圍有多大、輸出范圍有多大等因素。對(duì)于我們的巡線機(jī)器人的P控制器來(lái)說(shuō)，輸入范圍是5個(gè)光值單位，輸出范圍是100個(gè)馬達(dá)功率單位，因此似乎Kp 值在100/5=20 左右。在一些例子當(dāng)中，預(yù)期的Kp 值可能不會(huì)那么大。如果預(yù)期的Kp 值為1 會(huì)怎樣？因?yàn)樵贜XT-G中變量只能使用整數(shù)，調(diào)整Kp 值時(shí)，你可以嘗試使用的是...-2,-1, 0, 1, 2, 3, ...... 你不能輸入1.3，所以你不可能嘗試 Kp=1.3，你不能使用帶小數(shù)點(diǎn)的數(shù)值！但是當(dāng)你把Kp值做最小的調(diào)整，從1調(diào)整到2時(shí)，機(jī)器人的“反應(yīng)”可能會(huì)有很大的不同。與 Kp=1 相比，當(dāng)Kp=2時(shí)，機(jī)器人修正的誤差會(huì)是兩倍，在光電傳感器值變化量相同時(shí)，馬達(dá)功率的變化量也會(huì)是兩倍。而我們需要Kp 做更精細(xì)的控制。

其實(shí)解決這個(gè)問(wèn)題很容易。我們要做的只是將Kp乘以10 ，增大整數(shù)范圍。當(dāng)Kp接近1 時(shí)，乘以100 也是個(gè)好主意。實(shí)際上，在程序中直接使用100*Kp ，可能是最好的選擇。當(dāng)Kp 乘以100 時(shí)，我們輸入的數(shù)值就從1.3 變?yōu)榱?30，沒(méi)有小數(shù)點(diǎn)，NXT-G會(huì)喜歡這個(gè)數(shù)的。

但是不要忘記，要對(duì)結(jié)果進(jìn)行轉(zhuǎn)換。當(dāng)完成P控制部分的計(jì)算時(shí)，要對(duì)結(jié)果除以100。還記得我們前面定義P控制器的表達(dá)式嗎？

Turn= Kp*(error)

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

因此，我們新的、改進(jìn)過(guò)的巡線P控制器虛擬代碼如下：

Kp = 1000             !記住，我們用 Kp*100 ，因此這個(gè)Kp實(shí)際只有10！
offset = 45               ! 初始化其它兩個(gè)變量
Tp = 50
Loop forever
   LightValue = read light sensor         ! 當(dāng)前光電傳感器的讀值  
   error = LightValue - offset            ! 減去offset（補(bǔ)償量）計(jì)算error（誤差）
   Turn = Kp * error                  ! “比例控制部分”, 我們希望馬達(dá)的功率值改變多少？
Turn = Turn/100               ! 記住消除Kp中因數(shù)100的影響!
   powerA = Tp + Turn                     ! A馬達(dá)的功率值
   powerC = Tp - Turn                     ! C馬達(dá)的功率值
   MOTOR A direction=forward power=powerA! 在馬達(dá)模塊中設(shè)置這個(gè)功率值
   MOTOR C direction=forward power=powerC! 設(shè)置另一個(gè)馬達(dá)的功率值
end loop forever                         ! 結(jié)束循環(huán)，返回，進(jìn)行下一次循環(huán)

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

在這個(gè)例子中，有這樣一個(gè)問(wèn)題：在我們計(jì)算馬達(dá)功率值時(shí)（如powerC=Tp-Turn），可能會(huì)得到一個(gè)負(fù)的馬達(dá)功率值，這意味著馬達(dá)反向轉(zhuǎn)動(dòng)，但是NXT-G程序中馬達(dá)模塊的數(shù)據(jù)接口無(wú)法“理解”這個(gè)數(shù)值。馬達(dá)的功率值是一個(gè)在0到100之間的數(shù)值，馬達(dá)的轉(zhuǎn)動(dòng)方向是由另外一個(gè)不同的輸入接口控制的。當(dāng)功率值為負(fù)數(shù)時(shí)，你需要在程序中設(shè)置馬達(dá)的運(yùn)轉(zhuǎn)方向。方法如下：

If powerA > 0 ! 馬達(dá)功率值為正值時(shí)
MOTOR A direction=forwardpower=powerA
else
powerA = powerA * (-1) ! 馬達(dá)功率值為負(fù)值時(shí)，要做這一步運(yùn)算
MOTOR A direction=reverse power=powerA !此時(shí)馬達(dá)功率值為正值，還需要在控制面板上顛倒馬達(dá)的轉(zhuǎn)向
end If

馬達(dá)模塊通過(guò)數(shù)據(jù)線接收功率值（powerA對(duì)應(yīng)A馬達(dá)），在馬達(dá)的參數(shù)設(shè)置窗口，用復(fù)選框設(shè)置方向。

對(duì)C馬達(dá)也需要進(jìn)行相似的程序代碼設(shè)置。這樣，當(dāng)計(jì)算出的馬達(dá)功率值為負(fù)值時(shí)，就可以正確地控制馬達(dá)了，P控制器就能夠?qū)崿F(xiàn)“零轉(zhuǎn)彎半徑轉(zhuǎn)彎”，機(jī)器人可根據(jù)需要實(shí)現(xiàn)原地轉(zhuǎn)彎。

還有其他的“微妙問(wèn)題”。如果出現(xiàn)計(jì)算出的馬達(dá)功率大于100的情況怎么辦？實(shí)際上馬達(dá)會(huì)將功率值認(rèn)定為100。在P控制器（或PID控制器）中出現(xiàn)這種情況，并不十分太好。我們更希望控制器永遠(yuǎn)不會(huì)讓馬達(dá)做超出能力范圍的事。如果計(jì)算出的馬達(dá)功率值比100大不了多少（或比-100小不了多少），機(jī)器人運(yùn)行情況還算OK；如果這個(gè)計(jì)算出的馬達(dá)功率值比100大很多（或者比-100小很多），這就意味著控制器正經(jīng)常性的失去控制能力。你需要考慮一下如何處理這種情況！

P控制器概要

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

• 傳感器需要有足夠?qū)挼膭?dòng)態(tài)范圍（不幸的是，我們的巡線機(jī)器人卻不是這樣）
• 被控制的東西（在我們的例子里是馬達(dá)）也需要有足夠?qū)挼膭?dòng)態(tài)范圍。每個(gè)馬達(dá)在功率值上的寬動(dòng)態(tài)范圍應(yīng)該很接近（NXT馬達(dá)在這一方面非常好）。
• 傳感器和被控制的東西必須響應(yīng)迅速。“迅速”的意思是“比系統(tǒng)內(nèi)發(fā)生的任何變化都快”。控制馬達(dá)時(shí)，通常不太可能獲得馬達(dá)的“迅速”響應(yīng)，因?yàn)轳R達(dá)需要一定的時(shí)間來(lái)改變功率。就是說(shuō)機(jī)器人的動(dòng)作要比P控制器的命令滯后，這對(duì)P控制器的精確控制，會(huì)產(chǎn)生一定的困難。

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

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

為了提高P控制器的響應(yīng)速度，我們?cè)诒磉_(dá)式中加入一個(gè)新的部分——積分，PID中的“I”。積分是高等數(shù)學(xué)中非常重要的內(nèi)容，在這里，我們只需要直截了當(dāng)?shù)厥褂盟?/p>

積分用于計(jì)算誤差的動(dòng)態(tài)求和。

每次我們讀取光電傳感器的值，并計(jì)算error（誤差）時(shí)，我們將error（誤差）加到一個(gè)變量中，這個(gè)變量我們稱之為integral（積分）。

integral（積分）= integral（積分）+ error（誤差）

這個(gè)表達(dá)式不是普通的數(shù)學(xué)表達(dá)方式，它使用了將一系列數(shù)值累加的方法，這個(gè)方法在編程中經(jīng)常使用。在計(jì)算機(jī)程序里，這個(gè)表達(dá)式有著和數(shù)學(xué)不相同的含義。（在本文中，會(huì)用文字加重的方式來(lái)表明這是編程的方式，而不是普通的數(shù)學(xué)表達(dá)式。）這個(gè)“=”是賦值的意思，意味著將它右邊的計(jì)算結(jié)果賦值給左邊的那個(gè)變量名，就是計(jì)算機(jī)把原有的integral的值加上error的值，將結(jié)果賦值給integral。

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

Turn= Kp(error) + Ki(integral)

上面就是PI控制器的基本表達(dá)式。Turn是對(duì)馬達(dá)的修正，Kp(error) 是比例控制部分， Ki(integral)是積分控制部分。

積分控制部分有什么作用呢？如果誤差在幾次循環(huán)中都保持同樣的值，積分部分就會(huì)越來(lái)越大。例如，如果我們讀取光電傳感器值，計(jì)算出error為1，很短時(shí)間后，我們?cè)俅巫x取光電傳感器值，這一次error為2，第三次的error還是為2，那么此時(shí)的integral將是1+2+2=5。Integral為5，但這一步的error只為2。在修正量中，積分部分能產(chǎn)生很大的影響，但通常來(lái)說(shuō)，它需要比較長(zhǎng)的時(shí)間才能發(fā)揮作用。

積分控制的另一個(gè)作用是能去除小的誤差。在巡線過(guò)程中，如果光電傳感器非常接近線的邊緣，但又不是正好在線的邊緣上，那么error會(huì)很小，只能產(chǎn)生一個(gè)很小的修正量。你可以通過(guò)改變比例控制中的 Kp來(lái)修正這個(gè)小的error，但經(jīng)常會(huì)產(chǎn)生機(jī)器人的振蕩（來(lái)回?fù)u擺）。積分控制部分就可以完美地修正小的誤差，因?yàn)閕ntegral（積分）是對(duì)errors（誤差）的累加，幾個(gè)連續(xù)的小誤差可以使integral（積分）大到足以發(fā)生作用。

我們可以把積分控制理解為控制器的"memory"（存儲(chǔ)器）。Integral（積分）表現(xiàn)的是error（誤差）累積的過(guò)程，可以持續(xù)向控制器提供修正誤差的方法。

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

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

integral= integral+ error*(dT)

因此，我們每次向integral 中增加的應(yīng)該是error×dT。測(cè)量機(jī)器人的dT是相當(dāng)容易的事。在每次讀取光電傳感器值時(shí)，我們可以讀取計(jì)時(shí)器的值。如果我們從當(dāng)前時(shí)間中減掉上一次的時(shí)間，就得到了從上一次讀值起的dT。如果不去測(cè)量這個(gè)dT，不做乘法計(jì)算，是不是會(huì)更好一些呢？如果這個(gè)dT總是相同值呢？如果我們每一次加入到integral中的部分，其dT值都是相同的，我們就能夠把因數(shù)dT從 error×(dT)中提取出來(lái)，只做求和的運(yùn)算。

integral= integral+ error

實(shí)際上只有當(dāng)我們需要用integral做另外的運(yùn)算時(shí)，我們才需要去乘以dT。因此我們可以把這個(gè)時(shí)間因數(shù)藏起來(lái)。在PI控制器中，積分部分的表達(dá)式是Ki ×(integral)×dT，其中Ki是一個(gè)需要我們進(jìn)行微調(diào)的系數(shù)（就像Kp一樣），所以為什么不用一個(gè)新的Ki來(lái)代替Ki ×dT這一部分呢？這個(gè)新的Ki與原來(lái)的是不同的，但是因?yàn)槲覀兡膫€(gè)都不知道，所以用哪一個(gè)、叫什么，都沒(méi)有關(guān)系。無(wú)論它叫什么、代表什么含義，我們都需要通過(guò)反復(fù)試驗(yàn)來(lái)找到相對(duì)準(zhǔn)確的值。

所以，只要把所有的時(shí)間步進(jìn)值——dT 設(shè)定成相同的（或大致相同的）值，我們就可以從積分控制部分中去除時(shí)間因素。

積分具有記憶性

關(guān)于integral（積分），還有最后一個(gè)要注意的細(xì)節(jié)。通常情況下，integral（積分）只能趨近于0，我們加入到integral（積分）中的error（誤差）值絕大多數(shù)都是符號(hào)相異的，在integral為0 時(shí)，它對(duì)控制器是不起任何作用的。例如，在經(jīng)過(guò)幾次循環(huán)后，error（誤差）值為1,2,2,3,2,1，相加后得到integral（積分）為11。在最后一點(diǎn)的 error（誤差）僅為1，比在那一點(diǎn)的 integral 要小很多。讓integral趨近于0 的方法只有一個(gè)，就是加入負(fù)的error（誤差）來(lái)平衡早期加入的正的error（誤差），來(lái)逐漸減少integral。如，下面幾個(gè)error（誤差）是-2，-2，-3，則integral（積分）會(huì)從11降到4，還需要加入更多的負(fù) error（誤差），才會(huì)使積分降到0。此外，integral（積分）期望error（誤差）在正負(fù)誤差之間均勻分布。

如果巡線機(jī)器人在線邊緣的左側(cè)，而積分部分累積的誤差也在線的左側(cè)，那么積分控制部分不僅要把機(jī)器人送回線的邊緣，還要使機(jī)器人越過(guò)線，到線的右側(cè)。如果有大的誤差持續(xù)一段時(shí)間，積分會(huì)趨向于無(wú)窮。這在包含有積分控制的控制器里，會(huì)引起一些問(wèn)題。當(dāng)積分部分設(shè)法修正的誤差很大時(shí)，積分的這種傾向會(huì)引起超調(diào)，我們必須在程序上做一些調(diào)整以避免出現(xiàn)問(wèn)題。解決integral（積分）趨向于無(wú)窮問(wèn)題，有兩個(gè)常見(jiàn)的解決方案：（1）將integral（積分）置零——每次error（誤差）為0，或error（誤差）改變符號(hào)，就將變量integral（積分）設(shè)置為0；（2）當(dāng)計(jì)算一個(gè)新的integral時(shí)，對(duì)累積的integral乘以一個(gè)小于1的因數(shù)來(lái)抑制積分：

integral = (2/3)*integral + error

這樣，每次循環(huán)會(huì)把積分值降低1/3。如果你認(rèn)為積分控制部分是一個(gè)控制器的“memory”（存儲(chǔ)器），那么這種抑制會(huì)在一段“較長(zhǎng)”的時(shí)間后強(qiáng)迫它變得健忘起來(lái)。

PI控制器的虛擬代碼

在控制器中加入積分控制部分，我們需要增加新的變量Ki 和integral。別忘了，為了進(jìn)行整數(shù)運(yùn)算，我們要把Ks乘以100。

Kp = 1000                 ! 記住我們用 Kp*100，因此Kp實(shí)際為10
Ki = 100               !記住我們用 Ki*100，因此Ki實(shí)際為1
offset = 45                 ! 初始化變量
Tp = 50
integral = 0           ! 在這個(gè)變量里存儲(chǔ)積分值
Loopforever
   LightValue = read light sensor    ! 當(dāng)前光電傳感器讀值
   error = LightValue - offset       !減去offset（補(bǔ)償量）計(jì)算error（誤差）
   integral = integral + error       ! 新增的積分控制部分
   Turn = Kp*error + Ki*integral     ! “比例控制部分”+“積分控制部分”
   Turn = Turn/100                   ! 記住消除Kp，Ki中因數(shù)100的影響 !
   powerA = Tp + Turn                ! A馬達(dá)的功率值
   powerC = Tp - Turn                ! C馬達(dá)的功率值
   MOTOR A direction=forwardpower=powerA  ! 在馬達(dá)模塊里設(shè)置A馬達(dá)的功率值和轉(zhuǎn)向
   MOTOR C direction=forwardpower=powerC  ! 在馬達(dá)模塊里設(shè)置C馬達(dá)的功率值和轉(zhuǎn)向
end loop forever                    ! 結(jié)束循環(huán)，返回，進(jìn)行下一次循環(huán)

在控制器中加入“D”：完整的PID控制器
（“D”：接下來(lái)會(huì)發(fā)生什么？）

我們的控制器里有了比例控制部分，用于糾正當(dāng)前的誤差；有了積分控制部分，用于糾正過(guò)去的誤差。有沒(méi)有一種辦法能讓我們及時(shí)預(yù)測(cè)未來(lái)，對(duì)還沒(méi)發(fā)生的誤差進(jìn)行糾正呢？

這就要用到高等數(shù)學(xué)里的另一個(gè)概念——導(dǎo)數(shù)，就是PID中的D。像積分一樣，導(dǎo)數(shù)也是高等數(shù)學(xué)中相當(dāng)重要的數(shù)學(xué)方法。

假定誤差的下一個(gè)變化與當(dāng)前最后一個(gè)變化是相同的，我們據(jù)此來(lái)預(yù)測(cè)將來(lái)。

這個(gè)意思是說(shuō)，下一個(gè)誤差的期望值是：當(dāng)前誤差+前兩次傳感器采樣誤差的變化量。在兩個(gè)連續(xù)點(diǎn)之間的誤差變化量就叫做導(dǎo)數(shù)。導(dǎo)數(shù)是一條直線的斜率。

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

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

按照我們的數(shù)值就是 2 - 5 = -3 。因此，當(dāng)前的導(dǎo)數(shù)就是-3 。我們使用導(dǎo)數(shù)預(yù)測(cè)下一個(gè)誤差就是

  （下一個(gè)誤差） = （當(dāng)前誤差）+ （當(dāng)前導(dǎo)數(shù)）

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

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

   Kd(導(dǎo)數(shù))/(dT)

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

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

  Turn （轉(zhuǎn)向）= Kp*error(誤差) + Ki*integral(積分)+ Kd*derivative(導(dǎo)數(shù))

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

PID控制器的虛擬代碼

在控制器中加上導(dǎo)數(shù)控制部分，我們需要為Kd增加一個(gè)新的變量，還需要增加一個(gè)變量來(lái)記錄最后一個(gè)誤差。同樣不要忘記，我們?cè)贙s上乘以100，來(lái)進(jìn)行整數(shù)運(yù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                          ! 用于存儲(chǔ)積分的變量
lastError =0                         ! 用于存儲(chǔ)最后一個(gè)誤差值的變量
derivative = 0                        ! 用于存儲(chǔ)導(dǎo)數(shù)的變量
Loop forever
   LightValue = read light sensor     ! 當(dāng)前光電傳感器的讀值
   error = LightValue - offset        ! 減掉 offset（補(bǔ)償量），計(jì)算誤差值
   integral = integral + error        ! 計(jì)算積分值
   derivative = error - lastError     ! 計(jì)算導(dǎo)數(shù)值
   Turn = Kp*error + Ki*integral + Kd*derivative  ! “比例控制部分”+“積分控制部分”+“導(dǎo)數(shù)控制部分”
   Turn = Turn/100                      ! 記住消除Kp，Ki和 Kd中因數(shù)100的影響!
   powerA = Tp + Turn                 ! A馬達(dá)功率值
   powerC = Tp - Turn                 ! C馬達(dá)功率值
   MOTOR A direction=forwardpower=PowerA   !在馬達(dá)模塊中設(shè)置A馬達(dá)的功率值和轉(zhuǎn)向
   MOTOR C direction=forwardpower=PowerC   ! 在馬達(dá)模塊中設(shè)置A馬達(dá)的功率值和轉(zhuǎn)向
   lastError = error                  ! 把當(dāng)前誤差存儲(chǔ)在變量lastError中，作為下一次循環(huán)的最后一個(gè)誤差
end loop forever                      ! 結(jié)束循環(huán)，返回，進(jìn)行下一次循環(huán)

以上就是PID控制器用于巡線機(jī)器人的完整代碼。這其中最困難的部分，就是如何把 Kp, Ki 和 Kd調(diào)整到最好的，至少是調(diào)整到還說(shuō)的過(guò)去。

調(diào)整PID控制器，不使用復(fù)雜的數(shù)學(xué)方法

（但是我們還是要做一些計(jì)算）

在本文中，我使用了其他人總結(jié)出來(lái)的PID控制器調(diào)整的方法，測(cè)量幾個(gè)系統(tǒng)參數(shù)就可以讓你非常好地計(jì)算出 Kp,Ki 和 Kd的值。有幾種技術(shù)可用于計(jì)算Ks，其中之一就叫做 "Ziegler–Nichols方法" 。通過(guò)谷歌搜索可以找到很多講述這種技術(shù)的網(wǎng)頁(yè)。我所使用的版本幾乎是直接使用了維基網(wǎng)頁(yè)——PID控制器中的內(nèi)容（在很多其他的地方也可以找到相同的內(nèi)容），我只做了一點(diǎn)小小的改動(dòng)，包括下表中所示計(jì)算過(guò)程中的循環(huán)時(shí)間。
按以下步驟調(diào)整PID控制器：

將 Ki 和 Kd 的值置為0，即關(guān)閉控制器中的這些部分，將控制器作為一個(gè)簡(jiǎn)單的比例控制器。
把Tp（目標(biāo)功率值）設(shè)置的小一點(diǎn)。對(duì)于我們使用的馬達(dá)來(lái)說(shuō)，可以設(shè)為25.
將 Kp 設(shè)置為一個(gè)“合理”的值，什么是合理的？

1）用我們想讓馬達(dá)功率達(dá)到的最大值（100）除以能使用的最大誤差值。對(duì)于我們的巡線機(jī)器人，我們假定這個(gè)最大誤差是5，所以推測(cè)出Kp值為 100/5=20。當(dāng)誤差為+5，馬達(dá)的功率將達(dá)到100。當(dāng)誤差為0，馬達(dá)的功率會(huì)在 Tp （目標(biāo)功率值）上。

2）或者，將Kp 值設(shè)為 1 （或100），看看會(huì)發(fā)生什么。

3）如果你要把 K's的值乘以100，在這里，1就要記成100，20記成2000，100記成10000.

運(yùn)行機(jī)器人，觀察運(yùn)行狀態(tài)。如果它不能巡線，從線上脫離開(kāi)，就提高Kp值；如果它劇烈擺動(dòng)，就降低Kp 值。調(diào)整Kp值，直到機(jī)器人能夠巡線，并且沒(méi)有明顯的擺動(dòng)為止。我們稱這時(shí)的Kp 值為"Kc" （在PID文獻(xiàn)中，被稱為臨界值）
使用Kc值作為Kp，運(yùn)行機(jī)器人，試著找出機(jī)器人運(yùn)行時(shí)的“振蕩周期”是多少。這個(gè)測(cè)試不需要非常準(zhǔn)確。振蕩周期（Pc)是指機(jī)器人從線的一側(cè)開(kāi)始，擺動(dòng)到另一側(cè)，再回到開(kāi)始點(diǎn)的時(shí)間長(zhǎng)短。對(duì)于典型的樂(lè)高機(jī)器人來(lái)說(shuō)，Pc 大約是在0.5秒到1或2秒之間。
你還需要知道，機(jī)器人控制系統(tǒng)的循環(huán)周期是多少。我將循環(huán)設(shè)置為一個(gè)固定的次數(shù)（如10,000），測(cè)量機(jī)器人完成全部循環(huán)次數(shù)的總時(shí)間（從開(kāi)始到結(jié)束的時(shí)間，或機(jī)器人顯示出結(jié)果的時(shí)間），每個(gè)循環(huán)的周期是測(cè)量時(shí)間除以循環(huán)次數(shù)。對(duì)于一個(gè)完整的PID控制器來(lái)說(shuō)，使用NXT-G編程（在程序中不要使用發(fā)聲、顯示等模塊，這些模塊的使用會(huì)占用程序運(yùn)行時(shí)間，影響測(cè)試結(jié)果），dT值應(yīng)該在每個(gè)循環(huán)0.015秒到0.020秒之間。
使用下表計(jì)算 Kp, Ki, 和 Kc 的值。如果你只想要一個(gè)P控制器，使用表中標(biāo)注了P的那一行來(lái)計(jì)算Kp (Ki' 和 Kd' 均為0)。如果你想要一個(gè)PI控制器，就使用第二行來(lái)計(jì)算。如果你想要一個(gè)完整的PID控制器，就使用最后一行來(lái)計(jì)算。
在實(shí)際操作時(shí)，那些K值都要用100乘以它們實(shí)際的值，但是在計(jì)算中你不需要考慮這個(gè)問(wèn)題。這個(gè)因數(shù)100 ，在確定Kp = Kc 臨界值時(shí)，就已經(jīng)考慮在內(nèi)了。
運(yùn)行機(jī)器人，看看它的表現(xiàn)。
你可以調(diào)整Kp, Ki 和 Kd 的值直到獲得最佳的性能。你可以從相當(dāng)大的調(diào)整開(kāi)始，如30%，然后嘗試較小的調(diào)整，以獲得最佳的（或者至少是可以接受的）效果。
一旦你確定了一組好的K值，提高TP值，提高機(jī)器人的直線速度。
對(duì)于新的TP值，要重新的調(diào)整K值，也許甚至要回到第1步，重復(fù)整個(gè)過(guò)程，
不斷地重復(fù)，直到機(jī)器人的表現(xiàn)是可以接受的。

Ki' 和 Kd' 上的符號(hào)只是要提醒你——Ki' 和 Kd'已經(jīng)考慮了時(shí)間的因素，即ki'= ki*dt，kd’=kd/dt （假定dT為恒定值）。

這里有一個(gè)我自己做機(jī)器人測(cè)試的測(cè)量數(shù)據(jù)。Kc為300，當(dāng)Kp=Kc時(shí)，機(jī)器人的擺動(dòng)周期大約為0.8秒，因此Pc為0.8。我測(cè)量Pc的方法是，每當(dāng)機(jī)器人擺動(dòng)到一個(gè)特定的方向，就大聲數(shù)出來(lái)。循環(huán)時(shí)間dT為0.014秒/每個(gè)循環(huán)，用程序運(yùn)行10,000次循環(huán)時(shí)，NXT上顯示的程序運(yùn)行時(shí)間和循環(huán)次數(shù)相除所獲得。使用上表中PID控制器的各計(jì)算公式，我們得到：

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

在進(jìn)一步的反復(fù)試驗(yàn)后，最終的Kp, Ki 和Kd值分別為220,7 和500。別忘了所有這些K值均已乘以100，因此，它們的實(shí)際值為 2.2 ，0.07和5 。

改變Kp, Ki, 和 Kd的值對(duì)機(jī)器人運(yùn)行情況的影響

在優(yōu)化PID的過(guò)程中，上面說(shuō)明的方法和表格是一個(gè)好的開(kāi)始。有時(shí)，了解一下增加（或降低）三個(gè)K值中的一個(gè)會(huì)有怎樣的結(jié)果，也是非常有幫助的。下表在很多網(wǎng)頁(yè)上都能找到，這個(gè)版本來(lái)源于wiki——PID控制器的網(wǎng)頁(yè)。

增加參數(shù)值的影響

“響應(yīng)時(shí)間”是指機(jī)器人確定誤差的時(shí)間，在我們的例子中，是指機(jī)器人在離線以后，需要多少時(shí)間能回到線的邊緣。響應(yīng)時(shí)間主要由Kp控制。Kp值變大，機(jī)器人返回線的速度變快，響應(yīng)時(shí)間就減少。Kp過(guò)大，會(huì)造成機(jī)器人超調(diào)。

“超調(diào)”是指機(jī)器人在響應(yīng)誤差時(shí)，會(huì)越過(guò)線的邊緣多遠(yuǎn)。例如，如果超調(diào)較小，當(dāng)機(jī)器人想回到線的左邊時(shí)，就不會(huì)擺動(dòng)到線的右邊去。如果超調(diào)較大，機(jī)器人在糾正誤差時(shí)，就會(huì)擺動(dòng)過(guò)大，超過(guò)線的邊緣。超調(diào)受 Kd影響最大，但 Ki 和Kp對(duì)它的影響也頗強(qiáng)。通常情況下，糾正很大的超調(diào)，你需要增大Kd值。還記得我們第一個(gè)非常簡(jiǎn)單的巡線機(jī)器人嗎，除了左轉(zhuǎn)和右轉(zhuǎn)，它不會(huì)做任何事，這個(gè)巡線機(jī)器人就會(huì)產(chǎn)生非常大的超調(diào)現(xiàn)象。

“穩(wěn)定時(shí)間”是指機(jī)器人在發(fā)生一個(gè)大的變化時(shí)，需要多長(zhǎng)時(shí)間才能穩(wěn)定下來(lái)。在我們巡線的例子中，機(jī)器人遇到一個(gè)轉(zhuǎn)彎就會(huì)發(fā)生較大的變化。當(dāng)機(jī)器人對(duì)曲線做出響應(yīng)，它會(huì)糾正誤差，并產(chǎn)生一些超調(diào)，然后機(jī)器人會(huì)以另一個(gè)方向的超調(diào)來(lái)糾正當(dāng)前的超調(diào)，然后再糾正這個(gè)超調(diào)......你明白了吧。當(dāng)機(jī)器人對(duì)誤差進(jìn)行響應(yīng)時(shí)，它會(huì)圍繞期望位置進(jìn)行擺動(dòng)。“穩(wěn)定時(shí)間”就是這個(gè)擺動(dòng)被抑制到0的時(shí)間。Ki 和 Kd都對(duì)穩(wěn)定時(shí)間有很強(qiáng)的影響，Ki越大，穩(wěn)定時(shí)間越長(zhǎng)；Kd越大，穩(wěn)定時(shí)間越短。

“靜態(tài)誤差”是指系統(tǒng)在不受干擾的情況下運(yùn)行所保持的誤差。對(duì)于我們的巡線機(jī)器人來(lái)說(shuō)，當(dāng)機(jī)器人走了很長(zhǎng)一段直線后，這個(gè)誤差會(huì)被抵消掉。P控制器和PD控制器經(jīng)常會(huì)被這種誤差搞垮。增加Kp 值會(huì)降低它的影響，但會(huì)加大機(jī)器人的擺動(dòng)。P控制器和PD控制器在平衡狀態(tài)下會(huì)有一個(gè)恒定的誤差，因此經(jīng)常會(huì)在其中增加I控制，和加大Ki的值。（這是假定，當(dāng)機(jī)器人巡線時(shí)，你更關(guān)注小的系統(tǒng)誤差。這就意味著，機(jī)器人會(huì)稍微向一邊或另一邊偏移）

關(guān)于PID的一些認(rèn)識(shí)

當(dāng)誤差超出比例范圍時(shí)，導(dǎo)數(shù)會(huì)變成0.
導(dǎo)數(shù)對(duì)噪聲很敏感。
誤差精度高時(shí)，導(dǎo)數(shù)的作用就好。對(duì)于我們的巡線機(jī)器人來(lái)說(shuō)，這個(gè)誤差是在+5到-5之間的整數(shù)。這是相當(dāng)?shù)偷木取Ｒ苍S可以使用馬達(dá)軸速率的導(dǎo)數(shù)來(lái)替代？