樂高NXT機器人單光源軌跡車的實驗(4/8增補)比例控制

機器人使用光源感應器尋跡是非常有趣的應用，許多機器人競賽也利用這個特性設計比賽項目。

NXT樂高機器人套件就附有一顆光源感應器，大部分使用一顆光源感應器，都是利用『Zigzag』之字型走法，也就是讓機器人沿著軌跡線的一邊前進。

當光源感應器偵測到黑線或白邊時，機器人進行偏轉，我們可以使用Switch Block完成下圖的程式。

執行後機器人會擺頭前進，光源感應器的紅色光點也會在黑色軌跡線與白色地板來回跳動。

 

因為只有兩極化的判斷（不是黑就是白），機器人擺頭的動作很大，實際前進的路徑變成彎路，前進的速度就變慢。

 

為了讓機器人擺頭幅度較小，前進速度加快，我們可以想個辦法讓機器人判別黑與白的中間地帶。

這也就是最近很流行的PID Control的理論，這篇文章中先導入P Control (比例控制)。

將軌跡黑線放大，我們以紅色圈圈表示光源感應器的紅光投射到地面的範圍，假設當圓圈全部在黑線上，得到的感應值是30；相對的，當圓圈全部在白色地板時得到的感應值是60。理想狀態中，機器人可以直接沿著黑線與白地板的交接處前進，所以在交接處的紅色圈圈恰好是一半黑一半白，感應值為45（請參考下圖上半部）。當紅色圓圈內黑色與白色比例不同時，光源感應器會得到不同的數值（參考下圖下半部）。

為了讓機器人的光源感應器盡量維持在黑白交接處，機器人必須根據黑色與白色的比例來進行控制。先將黑白交接處的中間數值當作基準0，如果光源感應器偏向左側，數值的差異以負數表示；當光源感應器偏向右側，數值的差異以正數表示。

 

我們可以直接使用這個差異值控制機器人的轉向，感應器數值差異愈大，轉向就愈大，這樣當機器人偏離時，就能控制機器人愈快回到黑白交接處。

 

在Move Block可以利用Steering控制機器人的轉向大小，也可以利用資料線動態地改變轉向的幅度，Move 命令方塊的Steering接頭的設定為-100~+100，0表示兩顆馬達轉的一樣快，也就是直走；負值表示向左轉，正值表示向右轉。

 

根據這樣的原理，我們可以將程式改寫如下：

程式流程如下：（2011.4.8增補）

 1.      讀取光源感應器數值

2.      計算A-B(光源感應器讀值)，A設定為黑白中間值（(黑＋白)÷2），這樣的設定是為了讓光源感應器看到黑色時，產生正值，讓機器人向右轉。

3.      將2的計算值乘上一個係數。

4.      將計算結果透過資料線送入Move命令方塊的Steering接頭。

 

由於光源感應器的黑白讀值差異不大，將差異值直接輸入Steering時，可能會造成機器人轉彎幅度不足，所以可以視需要將數值乘上一個係數，增大數值，讓轉向的幅度符合需求。

 

這個係數需要依據軌跡線調整，請自行嘗試。

 

關於單光源軌跡車，還有另外一種方式，參考Stefan's Robots的文章：Fuzzy Line Following （http://stefans-robots.net/en/fuzzy-line-follwing-nxt-robot.php），我做了一些修改，使用Motor指令分別控制BC兩顆馬達，如下圖：

我們必須先測量光源感應器在黑線與白地板的讀值（假設為B, W）

下面我將程式流程寫出來：

1. 讀取光源感應器數值L
2. 將光源感應器數值減去測量黑線值(D=L-B)
3. 乘上係數Kp （D*Kp）
4. 計算馬力T，加上調整速率S   （T=S+D*Kp）S根據B, W調整
5. 計算相反值馬力R  (取T值的最大值及最小值相加再減去T，R=(Tmax+Tmin)-T）
6. C馬達設定為馬力T
7. B馬達設定為馬力R

寫了一些東西，當然需要實測，一般測試的軌跡都是圓邊長方形（或是使用8547附的測試軌跡圖），探奇認為這樣太沒挑戰了，在地板上貼出S型的曲線，這樣的軌跡就有一定的難度。

 

實測的結果為：

 

1.      LF-Switch: 18秒84 (傳統二分法)

2.      LF-Prop1: 16秒97 (一般比例控制)

3.      LF-Prop2: 13秒25 (改良比例控制，模糊控制)

其實軌跡車的設計有許多需要注意的地方，例如：軌跡形狀，車體輪距，感應器的位置，機器人的速度等。

 

歡迎大家一起討論！

補充一下：我使用探奇『NXT樂高機器人：創意樂趣，隨心所欲！』書中的軌跡車程式跑了18秒84，改版時再更新程式。

參考資料

A PID Controller For Lego Mindstorms Robots

Fuzzy Line Following NXT Robot