PID非常好的光感巡線思路

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

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

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

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

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

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

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

直線的表達式為：

y= mx + b

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

2011-8-28 21:39 上傳

下載附件(3.71 KB)

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

y = mx

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

從P到實際的馬達功率值

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

P控制器的虛擬代碼

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

MOTOR C direction=forward power=powerC !設(shè)置另一個馬達的功率值

end loop forever !結(jié)束這個循環(huán)，返回，進行下一次循環(huán)

復制代碼

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

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