PID

Proportional Integral Derivative

"P": Proportion(al) is the key

"I": what have you done for me lately?

"D": what is going to happen next?

http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html

以前曾在舊版協作平台紀錄過PID的用法，現在因為手癢以及提供一個學生學習的地方，又重新整理了一遍。這篇文章適合國小五、六年級和國中學生，或是沒學過PID控制法的人閱讀，希望能幫助建立起樂高循跡的概念與方法，文章有點長，請耐心仔細閱讀，沒看懂就再回頭看一遍，可以幫助加深記憶與理解，加油吧。

PID是指Proportional Integral Derivative，可翻譯成「比例積分導數法」、「比例積分衍生法」、「比例積分預測法」等，其實是由三個項目構成，用來調整機器人的行為反應，例如直走、轉彎。PID的應用包括車輛、機器人甚至於火箭，PID的數學描述很複雜，一開始我也看不太懂，本文將用一般可以理解的方式來說明，不要像我一樣，被PID迷惑住了。

本文將會著重在樂高機器人的循跡應用，硬體使用45544盒裝EV3，軟體為Mindstorms，目標是讓機器人能盡可能順利循線不會偏離，甚至能控制到像賽格威Segway一樣，使用兩個輪子即可保持平衡，賽格威的寫法有空會分享出來，目前我還不會...，這裡有一個連結，超酷的！

賽格威影片：https://www.youtube.com/watch?v=P6b60zf1g18

有學過微積分的人，較容易明瞭PID的描述，礙於小學生、國中生未學過微積分，因此只從簡單的項目慢慢建構出PID循線方式，而簡單一點的線條，甚至不需要用到PID呢！

先來看一下底下組裝後機器人的構造。開始囉

圖1

「顏色感測器」Colour Sensor：紅點

「主控器」位於中間、綠色箭頭是前進方向

輪胎分別在左、右兩側，左輪接B連接埠、右輪接C連接埠

※機器人位置偏向黑線左側，第三輔助輪位於感應器稍後位置※

有一些名詞請先記住，「反射光強度」(Reflected Light Intensity)，簡稱為RLI，它是顏色感測器的其中一個作用，顏色感測器可提供『測量』Measure、『比較』Compare、『計算』Calibrate「顏色」Colour、「反射光強度」RLI、「周遭環境光線」Ambinet Light Intensity等功能，本文聚焦在使用RLI來進行循跡。

紅色圈圈所能看到地面的範圍其實相當小，「顏色感測器」必須垂直對準地面，且距離地面高度要適當，過高或過低都不可以，不恰當的高度會影響感測器讀到的黑色及白色RLI，進而影響到循線的準確性，組裝時務必多花一點時間確認高度，建議值0.8mm~1.2mm。這個時候黑線RLI約10左右，白色RLI約80左右，不要差太多都是可以被接受的，即使是6~68也沒關係。

【成功第一步】顏色感測器距離地面高度要裝好。

反射光強度RLI理論值0~100，實際10~80之間，地面再怎麼黑也會反射光線。

循線是學習機器人的基本功，透過顏色感測器收集地面RLI來改變機器人行動，循線可以利用一個感測器、兩個或更多個，越多感測器越有利於循線時的判斷和修正，也能以較快的速度循線，本文只討論使用單一一個「顏色感應器」Colour Sensor的情形。

如何循線？有方法嗎？這裡使用了沿著線「邊緣」走的技巧，這是很重要的概念。黑線兩旁都是白色區域，如果妳是機器人，沿著黑線的中間走，當妳偏離黑線測到白色時，妳該左轉還是右轉呢？所以，沿著黑線的中間行走並不是一個好策略。

若是沿著左側「邊緣」走，當妳測到白色時，就可以知道妳是在黑線的左邊，測到黑色時就是在黑線上或偏右邊，這種方式即為「左手循線」Left Hand Line Follower。

【成功第二步】沿著線的一邊行走。

現在假設一個未經校正的感測器回傳黑色40、白色50的數值，畫一個簡單的圖如下，妳就能了解反射值與機器人循線之間的關係。

圖2

上圖是將RLI分成兩個區域範圍，RLI<45機器人向左轉、RLI>45機器人向右轉，一條直線的巡線只需要慢慢轉彎即可，遇到很多轉角、彎曲、銳角時，就需要急轉彎了，記住一個原則『越快越易失控』。慢慢轉彎的方法很簡單，一輪快、另一輪慢就能轉彎，例如快速輪子50%、慢速輪子20%。急轉彎則快速輪30%、另一慢速輪不動或-30%(反方向轉動)。

實測RLI介於10~80，中間值剛好也是45，寫一隻簡單的循線程式如下，稱為「二段循跡法」2-Level Line Follower，因為走起來歪來歪去的，所以又稱為Zig-Zag Line Follower。

圖3

發現了嗎？它永遠不會以走直線的方式前進，一直在左轉或右轉，動作實在不夠漂亮，走一條直線還好，碰到銳角幾乎快失控，速度再調快一點真的就失控了。這台機器人只知道「左轉」、「右轉」，無法克服需要急轉彎的情況，那就來改良一下吧。

把圖2的兩區域改成三個區域，中間區域為「直走」區，如此循跡會更順一些，如下圖。

圖4

依圖4劃分動作，RLI<43左轉、44<=RLI<=47直走、RLI>47右轉，循線程式如下，稱為「三段循跡法」3-Level Line Follower。

圖5

這個「三段循跡」比起剛剛的「二段循跡」要好很多，因為機器人會走直線了，只有少部分時候需要轉彎，但是遇到複雜的曲線時，依然有失控的風險，循線通常都必須再考慮路線的特性，找出不會失控的方法。聰明的妳，應該也已經想到了吧！如何再把三個區域分成更多區域呢？這個想法就是PID的開端囉。

【成功第三步】將測量到的RLI分成不同區間。

PID中的P是關鍵循跡法，P是指Proportional，是「成比例」的意思。

第一種循線方法「二段法」，只能左轉、右轉，而且轉彎幅度還是一樣的，只是方向相反而已。第二種循線方法「三段法」，加了一個直走，同樣也是一樣的轉彎幅度。超過三個區域的劃分，意味著需要更多不同的轉彎幅度，也就是說「需要更多種類的轉彎」，轉彎半徑都不一樣。

為了能理解『更多種類的轉彎』，將圖形2,4轉換為XY座標圖，以RLI為X軸座標、Y軸座標是轉向，轉軸妳可以把它視為需要轉彎的種類或半徑，這樣就構成了以下三種座標圖模式。

由左至右：圖6、圖7、圖8

藍線表示機器人動作，紅線是直走，看出不同點了嗎？圖8的控制會在左轉和右轉之間平順的變化著，如果RLI值顯示接近黑線，那麼就做小轉彎，反之則偏離黑線較遠應做出急轉彎，『比例Proportional』是一個重要概念，意即兩個變量(RLI、轉彎)之間存在著線性關係，簡言之就是「RLI」和「轉軸」相互對比時的座標圖，會是一條直線的關係。

直線的數學方程式，列出來的恆等式「x」與「y」的關係圖是一條直線。

y = mx + b

y是Y軸上的位置(距離原點的距離)、x是X軸上的位置，m是直線的斜率slope，b是Y截距intercept。

斜率 = △y /△x (任取兩個點的座標，y值變化量 / x值變化量)

Y截距 = (x,y)這個點，y在Y軸上與原點的距離

取黑色、白色反射光值的中間值，本例為(40+50)/2=45，實際測試時可能為(10+80)/2=45，這個值的光感位置剛好就是位於黑、白界線，意即「左手循線」右圖綠色虛線(中央線)，、紅色箭頭為前進方向。

將每次讀取到的RLI值減去45，得到的稱為「誤差」error，若RLI=47則error=47-45=+2，假設黑色RLI=40、白色RLI=50，就能預測「誤差」error是介於-5~+5之間，當error=0就表示位於虛線上，直走就行了，其他不是0的情況就調整兩輪的馬力，讓兩個輪子以不同速度行動，就能達到轉彎的目的啦。

error>0、偏離綠色虛線左邊、應該右轉。(此時RLI>45)

error=0、在綠色虛線上，直走。(此時RLI=45)

error<0、偏離綠色虛線右邊、應該左轉。(此時RLI<45)

圖9

【成功第四步】學會PID修正轉彎主要原理：等比例控制P-Control。

接下來，把圖8X軸的值換成「誤差」error，轉換可以得到圖10。Y軸的+1、-1是任意取的，方便接下來的計算使用，妳也可以用其它數值取代。

圖10

直線方程式修改後變簡單一點了：y = mx

計算「誤差」時，是把0當中央點，等於是把y=mx+b的b削去了，就變成了y = mx。

公式：轉彎 = m * 誤差

上面這個公式就是PID的靈魂，主宰了需要轉彎的現況。

還沒哦，不要急，繼續看下去。

轉彎軸還沒有定義，先以+1(向右急轉彎)、-1(向左急轉彎)來代表、0正在直走。斜率的計算是取線上的任意兩點來計算的，就取那兩個紅點的座標吧！

斜率 = m = (y變化量) / (x變化量) = ( 1- (-1)) / (-5 - 5 ) = 2/-10 = -0.2

斜率是一個『比例常數』，必須再乘上「誤差」error值，就能將其換算為Y軸的「轉彎Turn」值。

斜率、m、比例常數在PID裡指的都是同一件事情，通常會使用K為代號。它的功用就是將x值換算成y值，簡單的數學比例換算而已。

舉例來說，若m=-0.2=K且x=3，則y=K*3=-0.2*3=-0.6=Turn，是不是很簡單呢！

公式變化成：Turn = K * error

Turn是P-Control的輸出，稱之為「P Term」，這是一個控制項，還要再加工一下才能用於馬達馬力上面。圖10的x值範圍是-5~+5，稱之為「比例範圍Proportiona Range」，循線時，RLI值範圍是40~50(這個顏色感測器高度一定沒裝好！！10~80是比較適當的裝法！)，誤差範圍-5~+5，但是、但是，馬達的馬力範圍卻是-100~+100，這該怎麼處理呢？

先提醒兩個注意事項：

一、「比例範圍Proportiona Range」越大越好

顏色感測器要距離地面0.8mm~1.2mm。

感覺到了嗎？機器人是沿著一條很窄的黑線在循跡，顏色感測器自然就會被限縮在一定範圍內，超出了這個範圍，顏色感測器看到的就全都是白色，而且還無法得知離黑線有多遠！方法是『擴大白黑反射值差距』，這就與先前提過的安裝高度有絕對關係。建議安裝高度距離地面0.8mm~1.2mm，這時候的反射光數值範圍會介於10~80之間，組裝循跡機器人時必須多加注意。距離地面過高的顏色感測器，測到的RLI會偏向黑色RLI值10，試著想一下結果會如何呢？循跡公式還能發揮作用嗎？(應該只會直走不會轉彎了)

二、超出比例範圍

將範圍控制在「比例範圍Proportiona Range」內。

必須將範圍控制在「比例範圍」內，循跡才能平順，超出範圍後循跡變成不可靠，可能發生轉錯邊，造成機器人循線時迴轉了。

【成功第五步】知道比例控制P-Control轉彎公式。

從 P 到實際馬達功率

『比例控制』P-Control來囉！

該怎麼轉彎呢？實際的馬達功率如何給？先定義一個目標數字Tp，這個Tp就是error=0時必須直走的兩輪最低馬力功率(Tp=50%)，左右輪馬力值都是Tp、直走；error不等於0時，就套用公式『Turn = K* ( error ) 』，其中一輪得到「Tp + Turn 」、另一輪得到「Tp - Turn 」。

設一個變數Kp來儲存「比例常數K」，Kp可以用猜的、反覆不斷測試修改、或是透過計算來取得。假設Tp=50、比例範圍-5~+5，就能猜測：當error從0變成-5時，希望控制功率從50變成0，計算Kp= (0 - 50)/(-5 - 0) = 10。使用這個Kp值來將「誤差值error」轉換為「轉彎值」，以變數公式為例，就會變為底下的樣子：

powerB = Tp + Kp*error

powerC = Tp - Kp*error

黑線左手循線口訣『左加右減』，於是得到以下結果。

左輪馬力功率 = Tp + Kp * error

右輪馬力功率 = Tp - Kp * error

※Kp控制轉彎、Tp控制巡線速度，兩者關係如下。※

一、Tp越大、機器人循線越快，遇到銳角時，Tp會有一個極限值，Tp超過該極限值會失控，這時候無論Kp值怎麼改變都無法改變失控現象。反過來說，Tp值越小，循線速度越慢，Kp怎麼給都會有作用。目標是找出Tp的最大值，同時又能順利轉彎(調整Kp值)。

二、Kp越小、轉彎（修正）越溫和。對於每一個Tp的改變，可能都需要再調整一下Kp，Kp越大越會做出急轉彎。

【成功第六步】學會了P-Control左右輪馬力公式。

※已經可以拿來循跡囉，覺得不夠好就繼續往下學習。※

要注意Mindstroms能否處理小數點？

如果不能處理小數點，那麼Tp、Kp就只能設成整數，任何大於100以上的馬達功率數字，都被視為最大馬達功率100%。

P-Control結論

1.決定Tp、Kp。

2.利用顏色感測器讀取地面反射光強度數值(RLI)。

3.將RLI換算成「誤差值error」。(減去黑、白反射光的平均值)

4.計算「轉彎Turm」= Kp * 「誤差值error」

5.計算兩輪馬力，「Tp+Turn」、「Tp-Turn」。

6.大部分數形況下，P-Control足以應付循跡。

P-Control公式：

powerB = Tp + Kp*error

powerC = Tp - Kp*error

切記，這是『黑線』、『左手線』循線公式，黑色背景、白線的巡線公式必須對調。

圖11.P-Control

圖12.P-Control 校正版

將「積分」I 添加到P-Control

從P-Control變成PI-Control

Integral：what have you done for me lately?

最近都幫我做了甚麼？(錯誤的累積不能一直錯下去，用積分I來修正吧！)

機器人循線時，有可能一直偏向黑線的一邊，妳會發現一件好玩的事情，即便只是下達「直走」指令，機器人往往還是偏向一邊，這是一個無法改變的事實，這個現象是否與PID循跡有關，尚待觀察，此處只是提醒玩機器人的您，機器人不會一直走直線，因此循線時必須不斷修正，讓機器人可以維持在線上。

好啦，既然會歪掉，那就必須再考慮「修正率不足」的情況，不要讓「誤差」持續下去。為了改進P-Control的效能表現，不會把錯誤一直「累積」下去，再添加一個「積分」項目，「積分」就是多次「誤差error」的總和。

累加指令：積分 = 積分 + 誤差

將「積分」乘上一個常數Ki，Ki *(積分) 稱為積分項，得到新的PI-Control公式：

轉彎Turn = Kp *(誤差error) + Ki *(積分)

「積分」主要是在累積小錯誤，舉個例子，機器人很靠近綠色虛線(中央假想線)，這些小錯誤基本上是不需要做出馬上修正的，那就透過積分把這些小錯誤累加起來，「積分」是PI-Control的記憶，累積歷史錯誤，一段時間之後，「積分項」就可以對PI-Contrl做出修正的貢獻。

另一種修正小錯誤方法是修改Kp值，但是過多的修正卻會造成機器人「左右搖擺」，如果希望循跡能平順地進行，修改Kp並不是好方法。

PI-Control 馬力公式

Turn = Kp * error + Ki * 積分

powerB = Tp + Turn

powerC = Tp - Turn

【成功第七步】學會加入積分Integral到PI-Control。

將『導數』D添加到PI-Control

從PI-Control蛻變成PID-Control

Derivative：what is going to happen next?

預測一下，接下來會發生什麼事？

PI-Control現在已經具有了糾正當前錯誤的「比例項目」以及嚐試修正過去錯誤的「積分項目」了，可不可先預測下一次將會發生的情況，及早做回應呢？

解決方案是來自數學的另一個概念，稱為導數Derivative。

兩個連續點之間的誤差變化稱為導數，導數的計算很簡單，只需要計算的是「當前誤差」與「上次誤差」間的差異值、加上「當前誤差」，實際來算一下吧。

假設「當前誤差」= 2、「上次誤差」= 5，則：

「導數」= 「當前誤差」-「上次誤差」= 2 - 5 = -3

預測『下次誤差』=「當前誤差」+「導數」= 2 + (-3) = -1

把導數項加入Turn，轉彎的修正就會變成：Kp*error + Ki*積分 + Kd*導數，至此可以寫出轉彎公式了：

Turn = Kp *（誤差）+ Ki *（積分）+ Kd *（導數）

【成功第八步】學會加入導數Derivative到PI-Control。

完整PID-Control 馬力公式

Turn = Kp*error + Ki*積分 + Kd*導數

powerB = Tp + Turn

powerC = Tp - Turn

好啦，接下來也是跟Kp一樣，需要訂出、測出Ki、Kd，因此有必要列一張表格，針對各項目紀錄一下影響了那些事情。

Kp = (0.60)(Kc) = (0.60)(300) = 180

Ki = 2( Kp )( dT ) / ( Pc ) = 2(180)(0.014) / (0.8) = 6.3（四捨五入為 6 )

Kd = ( Kp )( Pc ) / ((8)( dT )) = (180)(0.8) / ((8)(0.014)) = 1286

先拿以上的數字是是看吧！他們是用Ziegler-Nichols方法算出來的。