SPIKE 樂高機器人

我的網頁

2011-04-01

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


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




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




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




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


 


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


 



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





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




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






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


 


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


 


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


 


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






程式流程如下:(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: 1884 (傳統二分法)








2.      LF-Prop1: 1697 (一般比例控制)








3.      LF-Prop2: 1325 (改良比例控制,模糊控制)








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


 


歡迎大家一起討論!




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




參考資料


A PID Controller For Lego Mindstorms Robots


Fuzzy Line Following NXT Robot







3 則留言:

  1. CAVE教育團隊12:32 上午

    很豐富實用的說明, 超棒!!

    [版主回覆04/02/2011 17:03:57]謝謝鼓勵!

    回覆刪除
  2. 老師您好:
    想請問地2個範例,光源直是放在B嗎?那A的值是多少,有點想不出來,所以我就把值放在A-(白+黑/2),不知概念相同嗎
    [版主回覆04/03/2011 20:22:51]您好:

    第二個範例是LF_Prop2嗎?

    我們必須先測量光源感應器在黑線與白地板的讀值(假設為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

    如果還是不清楚,歡迎來信,我再將程式寄給你。

    回覆刪除
  3. 老師您好我也有問題
    那個第3個範例程式中,迴圈裡面的第4個指令,有一個位移10(x=+10-+70),
    請問這個位移值10是什麼意思?如果沒有這10這程式會怎樣嗎?
     
    [版主回覆10/30/2011 16:08:46]你好:

    原本計算出來的數值是0~+60,為了調整馬達的馬力值不要為零,所以我加了一個位移10,你可以將他改為零,或是變為20,看看實際變化及影響程度。

    祝你成功!

    回覆刪除

探奇歡迎大家留言討論!謝謝分享你的意見。