PID非常好的光感巡線思路

PID控制器是一種常用的控制技術(shù)，常用于多種機(jī)械裝置（如車(chē)輛、機(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)?？聪聢D，這個(gè)機(jī)器人用兩個(gè)馬達(dá)驅(qū)動(dòng)，分別與車(chē)輪A、C連接，前端裝有垂直向下的光電傳感器，紅圈標(biāo)出的部分就是光電傳感器能“看到”的部分。帶箭頭的大長(zhǎng)方形表示機(jī)器人的其余部分，箭頭指示機(jī)器人的運(yùn)動(dòng)方向。

巡線是機(jī)器人的基本技術(shù)，也是大家學(xué)習(xí)機(jī)器人時(shí)最先要做的。能夠巡線的自動(dòng)裝置具有機(jī)器人的全部特點(diǎn)：使用傳感器收集周?chē)h(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ō)，使用的傳感器越多，巡線的速度越快?，F(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ù)；蛲Ｖ?。無(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。

2011-8-28 21:39 上傳

下載附件(3.71 KB)

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

y = mx

或者使用我們的方法：
Turn= m*error我們還沒(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)樗簧婕氨壤刂?，被稱為“比例控制部分”。
“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)

復(fù)制代碼

如果機(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ì)小。