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

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

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

我們把這個(gè)數值線(xiàn)段平分為兩部分：當光電傳感器值小于45，讓機器人左轉；當光電傳感器值大于45，讓機器人右轉。在這里，我們不考慮機器人的轉向動(dòng)作的精確性。在相對較直的線(xiàn)上，機器人的轉向動(dòng)作可以比較細??；在有很多彎的線(xiàn)上，機器人通常要有明顯的轉向動(dòng)作。做動(dòng)作細小的轉向時(shí)，你可以把速度快的輪子的馬力值設置為50%，速度慢的輪子的馬力值設置為20%。有很多彎的現上做明顯轉向動(dòng)作時(shí)，你可以在快的輪子上設置30%的馬力值，在慢的輪子上使用緩?；蛲Ｖ?。無(wú)論你在輪子上設置什么數值的馬力值，在做左右不同轉向時(shí)，這個(gè)設置應該是一樣的，即在一側的輪子上設置較大的馬力值，在另一側輪子上設置較小的馬力值（或設置為停止）。
這種巡線(xiàn)方式能夠完成巡線(xiàn)任務(wù)，但效果并不是很好。在比較直的線(xiàn)上完成巡線(xiàn)任務(wù)，在編程中設置動(dòng)作細小的轉彎方式，整體巡線(xiàn)效果看起來(lái)還算不錯；但是如果線(xiàn)上有較大的彎度，你又采用明顯的轉向動(dòng)作讓機器人完成巡線(xiàn)，機器人就會(huì )來(lái)回擺動(dòng)，橫向穿過(guò)線(xiàn)條。機器人只“知道”兩件事情：轉左和轉右。用這種方法巡線(xiàn)，通常機器人的速度不會(huì )很快，而且看起來(lái)很糟糕。即使線(xiàn)是直的，這種方法也不能使機器人走直線(xiàn)，甚至不能完全對準線(xiàn)的邊緣。如何使巡線(xiàn)更有效率呢？

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

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

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

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

直線(xiàn)的表達式為：

y= mx + b

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

2011-8-28 21:39 上傳

下載附件(3.71 KB)

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

y = mx

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

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

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

P控制器的虛擬代碼

首先我們要測出光電傳感器讀取黑色和白色時(shí)的光電傳感器讀值。根據這兩個(gè)數值，我們能夠計算出offset（補償量），將光電傳感器讀值減掉這個(gè)數值就可轉換成error（誤差）值。offset（補償量）是白色和黑色光電傳感器值的平均值。為簡(jiǎn)單起見(jiàn)，我假定offset（補償量）已經(jīng)測量完畢，并存儲在叫做offset的變量里。（讓機器人自己測量白和黑的光電傳感器讀值，并計算offset，會(huì )更好）
常數K被稱(chēng)為Kp（比例控制器中恒量K）。要為Kp設定一個(gè)初始的推測值，然后通過(guò)反復試驗來(lái)修正它。我們可以根據機器人和傳感器的特性估算出一個(gè)值：將Tp（目標功率）設為50，當誤差為0時(shí)，兩個(gè)馬達都以50的功率值轉動(dòng)；誤差范圍為-5 到 +5。我們期望當誤差從0變化到-5時(shí)，馬達的功率值從50變化到0，就是說(shuō)Kp（斜率——y的變化量除以x的變化量)為：
Kp= (0 - 50)/(-5 - 0)= 10
我們用Kp=10 將error (誤差）值轉換為turn(轉向）值。這句話(huà)中，轉換的意思是“error”（誤差）每發(fā)生1個(gè)單位的變化，我們就將一個(gè)馬達的功率值提高10，另一個(gè)馬達的功率降低10.
虛擬代碼如下：

Kp = 10 !初始化變量

offset = 45

Tp = 50

Loop forever

LightValue = read light sensor !當前光電傳感器的讀值

error = LightValue - offset !減去offset（補償量）計算error（誤差）

Turn = Kp * error ! “比例控制部分”, 我們希望馬達的功率值改變多少？

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 !結束這個(gè)循環(huán)，返回，進(jìn)行下一次循環(huán)

復制代碼

如果機器人在運行時(shí)，表現出的狀態(tài)是遠離線(xiàn)的邊緣，而不是尋找線(xiàn)的邊緣，你需要改變一下轉向的方向。把Kp的值變?yōu)?10，看看會(huì )怎樣。如果這樣做可以糾正機器人的轉向方向，就把Kp的值變回+10，將設置馬達功率的兩行代碼做如下改動(dòng)：
powerA = Tp - Turn
powerC = Tp + Turn
在這個(gè)P控制器里有兩個(gè)“可調參數”和一個(gè)恒量。恒量就是offset（補償量）（黑色和白色光電傳感器讀值的平均數）。你需要編寫(xiě)一小段程序，在巡線(xiàn)測試紙上用你的機器人來(lái)測量光電傳感器讀值。你需要測量出“black（黑）”和“white白”的光電傳感器讀值，然后計算平均值，并把平均值寫(xiě)入P控制器程序中的offset變量。幾乎所有的巡線(xiàn)機器人都要做這一步工作，你可以人工進(jìn)行，也可以通過(guò)編寫(xiě)程序代碼讓機器人自動(dòng)完成。

Kp值和Tp（目標功率）值是可調參數?？烧{參數必須經(jīng)過(guò)反復試驗才能確定。Kp決定了當機器人漸漸離開(kāi)線(xiàn)的邊緣時(shí)，控制器讓機器人返回線(xiàn)的邊緣的速度有多快；Tp決定了機器人沿著(zhù)線(xiàn)向前移動(dòng)的速度有多快。
如果線(xiàn)比較直，你可以將Tp的值設置的高一些，提高機器人的運行速度；將Kd的值設置的小一些，使機器人的轉向動(dòng)作（修正）更細小。