中華大學資訊工程學系
系統開發專題報告
Lego NXT-藍芽足球機器人
Lego NXT-The Robot Plays the Soccer By Using Bluetooth Device
專題生:洪智賢
指導教授:周智勳 教授
專題編號 PRJ2011-CSIE-10000
執行時間:100 年七月至 101 年 1 月
目錄
page
背景與目的
2(一)專題系統架構
3(二)方法說明
5(三)實驗過程與結果
16(四)專題心得
23(五)總結與未來展望
25(六)參考文獻
25致謝
26背景與目的
背景:結合 Lego NXT 主機及超音波 SENSOR、視訊裝置、藍芽裝置，利用電腦上視訊裝置 所擷取之圖片，C#讀取之並進行影像處理，把運算結果轉換成機器人之動作命令，再透 過藍芽裝置，把要傳達給機器人之動作命令傳給 Lego NXT 機器人
目的:將上述的機器人動作命令傳給 Lego NXT 機器人,使之能夠找到足球,並且邊踢足球 邊移動還要能夠閃避障礙物,最後在距離球門一定之距離內,能夠成功把球踢到球門之 中。
Figure 1.1:LEGO NXT主機- Intelligent NXT Brick
Figure 1.2:超音波SENSOR- Ultrasonic Sensor
Figure 1.4:視訊裝置- Web Camera
Figure 1.5:組裝過後的 Lego NXT 足球機器人(主機+超音波 SENSOR)
Figure 1.6:Lego NXT 足球機器人與球場之全景圖
(一) 專題系統架構
1.1 實作前之規劃
a.使用 Web Camera 在每一個單位時間內對 Lego NXT 足球機器人、足球、球門同時進行 拍照動作。
b.利用C#來讀取 Web Camera 每一個單位時間所擷取之圖片。
c.再利用讀取之圖片進行影像處理，讀取相關Pixel之座標進行運算
,最後根據運算完之結果，並決定Lego NXT機器人下一次單位時間要做的動作。
d.透過電腦上藍芽裝置,把決定好的動作命令傳給Lego NXT主機。
e.當Lego NXT主機接受到命令時,會把命令轉換成自己能夠辨識的程式碼,並且執行這單 位時間所辨識之程式碼。
1.2 系統運作流程圖
(二)方法說明
說明:這裡將介紹系統架構流程圖中，各個流程要如何實作出來。
===========================================================
a.開啟 Webcam 並且拍照存檔到資料夾
說明：利用 C#的 MSDN 內建函式實作出來，以及利用自己寫的類別所創之物件來存檔
電腦端 a.開啟 Webcam 並且拍照存檔到 資料夾
b.到目的資料夾，讀取 此單位時間所照之圖 片
c.找圖片中代表球、球 門、車頭、車尾這四個 位置的座標
d.用這四個座標, 算出機器人下一秒 該做的動作，並且 把 output 轉成藍芽 訊息
藍芽裝置
e.電腦端算出 機器人要做之 動作的藍芽訊 息
Lego NXT 主機
f.順時針旋轉 g.逆時針旋轉 h.在能夠閃障 礙物的情況下 直走
i.射門
Figure 2.1:開啟/關閉 Webcam
Figure 2.2:用 Webcam 拍照
Figure 2.3:將拍照之圖存到目的資料夾
＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝
b.到目的資料夾，讀取此單位時間所照之圖片
說明：把照之圖片 show 出來，用Ｃ#內建函式實作在的主程式的類別
Figure 2.4:加在主程式的類別中
c.找圖片中代表球、球門、車頭、車尾這四個位置的座標 說明:
(1)先把 C#主程式的屬性改成 unsafe，因為讀圖片中之 pixel 座標會牽涉到記憶體修改。
（2）在主程式的類別中，先分析圖片中 RGB 資訊，利用 Bitmap 將 image 包起來，取 得 pixel 顏色資訊。
(3) 在主程式的類別中實作出以下功能，I.先鎖住存放圖片的記憶體 ，II.取得 pixel 資料的起始位址 ，III.計算每行的像點所佔據的 byte 總數，IV.直接利用指標, 把影像 資料取出來。
(4)先使用 C#工具箱的 timer1_Tick，再利用創到類別之物件把讀取之圖拿來分析 RGB 資 訊
(5)宣告要代表球、車頭、車尾、球門的四個座標，並且找出四個顏色的 RGB，利用兩個 FOR 迴圈找出此圖所有的座標之 RGB 資料，如果有符合有座標符合這四個顏色的 RGB 中其
中一個顏色的 RGB，既將此座標當作該位置的座標（EX:找到球的座標，球的顏色的 RGB 為 [255,0,0]）
Figure 2.5:如何把讀取之圖，將之分析其 RGB 資訊
Figure 2.6:用高效率的方式，把圖的資訊完整分析出來(pixel 數、定義所有 pixel 之 RGB)
Figure 2.7:宣告四個座標，並且定義四個顏色之 RGB，然後讀取此圖所有的 pixel 把符合球、車頭、車尾、球門代表這四個部份的顏色座標一一找出來
d.用這四個座標,算出機器人下一秒該做的動作，並且把 output 轉成藍芽訊息
說明：算出機器人的動作總共有四種 (1) 順/逆時針旋轉
利用球/球門、車頭、車尾這三個座標,用車尾當原點在利用剩下兩個座標所在 之象限的分布位置來觀察是否順/逆時針
先分成三種情形討論
I.球/球門跟車頭之象限差為 1/-1
由 I、II 象限來看，象限差為 1，所以就是車頭需逆時針找球,在看 III、IV 象限，象限差為-1，所以就 是車頭需順時針找球
II. 球/球門跟車頭之象限差為|2|
計算球跟原點(車尾)(X,Y)跟創的新座標(X,Y+5)或者(X,Y-5),這三點算出夾角，如果此角度>車尾車頭 X 軸之角度,則逆時針,否則順時針
III. 球/球門跟車頭之象限差為 0
再上圖之第一象限來看，計算球跟原點(車尾)(X,Y)跟創的新座標(X,Y-5),如果（新座標＋原點＋車頭 三點算兩個向量）的夾角＜（新座標＋原點＋球三點算兩個向量）的夾角，則順時針，反之逆時針。其 他向量亦可用此觀念推倒來求出，機器人如何順/逆時針轉到球之正面。
Figure 2.8:先定義象限位置
車尾
車頭
球 球
車頭 車頭 球
Figure 2.9:實作利用象限差判斷順/逆時針
Figure 2.10:最後把符合條件集中再逆時針,反之順時針
(2) 直走
當機器人跟球/球門成一直線，則直走找球/球門
Solve:利用科西不等式概念延伸之算式
I.先利用球/球門 車頭 車尾 三個座標，算出兩個向量 Ｕ1=(x1-x2,y1-y2)=(X1,Y1)
Ｕ2=(x3-x2,y3-y2)=(X2,Y2) II.在取兩個 NORM
NORM1=X1^2+Y1^2 NORM2=X2^2+Y2^2 III.取內積
內積=X1*X2+Y1*Y2
IV.取 COSΘ
球
車頭
車尾
Θ
180-Θ
內積
COSΘ= --- NORM1*NORM2
V.180-
Θ就是轉的角度，使車子能夠正面向球，達到直走找求目的
Figure 2.11:判斷機器人如何轉到正面
(3)射門
當機器人帶球找球門時,如果轉到跟球門適當的角度便會直走找球門,如果再射程範圍內 便會射門
Figure 2.12:如何判斷射門條件
==========================================================
e.電腦端算出機器人要做之動作的藍芽訊息
說明: 主程式會把要傳的訊息用 BYTE 的方式存在一個正確的通訊協定的陣列(長
度:2BYTE) 然後傳給機器人,其中這通訊協定包含了是否回傳訊息,傳到第幾個信箱,以 及要傳的訊息(EX:數字)轉成 16 進位 code
Figure 2.13:C#上要傳的訊息轉成認可的藍芽通訊協定的訊息
f、g、h、i：ＮＸＴ機器人收到訊息做的四個動作
(1) 用 BrixCC 內建的函式來接收訊息
(2) 再利用 switch 把收到的訊息一一對應該動作
Figure 2.14:NXT 接收訊息
Figure 2.15:利用 switch 把收到的訊息轉成動作
(三)實驗過程與結果
3-1 架構環境 i.先準備好材料
Webcam*1藍芽裝置*1 Lego NXT 主機*1
超音波 SENSOR*1 紅色塑膠球*1
用 Lego 材料做成之球門 用瓦楞紙做的圍牆*1
用四個白色瓦楞紙做的草皮*1
ii.場地製作流程
a. 先把 Webcame 跟藍芽裝置安裝再電腦上 b. 超音波 SENSOR 安裝再機器人身上
c. 把 Lego 材料安裝在機器人身上
-輪子*3(兩個大輪子當作左右胎,小輪子當輔助腳) -馬達*2(用來前進,後退,轉彎)
-綠色以及水藍色色紙+塑膠板(用來當作車頭.車尾之區分) -彎曲塑膠零件數個,直線塑膠零件數個(組裝控球把手)
d. 紅色塑膠球放置任意點
e. 用 Lego 零件組成的球門放置任意點,並放上紫色色紙(用來給 Webcam 擷取圖片用以判 別位置)
f.架上瓦楞紙做的圍牆跟鋪上四片瓦楞版做的草皮
Figure 3.1:Lego 足球機器人與場地全景圖*3
iii.開始主程式 C#,並利用 C#開啟視訊準備擷取照片並下達指令 Figure 3.2:執行主程式並開啟視訊準備要下達指令
3-2 為何進行實驗
目的:測試所寫之 Lego NXT 藍芽足球機器人是否可以執行下列功能 a. 能夠找球
b. 閃避障礙物
c. 當找到球把球控在自己身邊 d. 成功把球踢進球門
3-3 進行實驗與檢討
實驗 I:目的:希望機器人能夠閃避障礙物並且找足球,最後把球順利的踢進球門 靜態實驗圖:12 張 (觀看順序為由左到右,由上到下)
檢討與改進:
機器人雖然能夠順利把球踢進球門而且成功的閃避障礙物,可是筆者在圖片 12 時,是設定 射門的時候自動把程式關掉,停止指令,所以沒有成功拍到射門鏡頭,這是因為射門的指 令,所花的動作時間超過一秒,筆者設定的時間是 1 秒鐘擷取一張照片所以機器人是要在 1 秒鐘內接收完指令並且執行完動作,如果沒有辦法達成,會造成訊息無法傳進機器人,而 機器人會一直重複執行上一秒所作之動作
解決方法:
i. 因為射門動作超過１秒,所以可以讓擷取圖片的時間加長,或者減少射門動作的時 間,減少至 1 秒內。
機器人不能在接收到下一秒指令。
實驗 II
目的:希望機器能夠把球踢進左邊球門，並且能夠順利的拍到射門的畫面，因為射門的動 作仍超過一秒，所使用的方法為當機器人接受到射門的指令時，立刻把程式關掉，不會 讓機器人收到下一秒指令
靜態實驗圖 16 張
檢討與改進:
其實拍這部實驗影片,前 30 秒都卡在同一個點,之後才順利把球踢到球門 改進:下一個時間將加大機器人的馬力,並且加重球的重量使之不會卡點 實驗 III
目的:希望機器能夠把球踢進右邊球門，並且能夠順利的拍到射門的畫面 靜態實驗圖 16 張
檢討與改進:
機器人成功的找到球，並且能夠控球然後把球踢到球門，成功的地方在於轉彎的馬力增 強了約 15,而且利用紅色色紙加重了紅色塑膠球的重量，使之更穩定推進球門
缺點：如果球的重量加太多，射門會不容易成功，必須斟酌。
(四)專題心得 I.遇到的困難跟解決的方法
a.機器人如何知道要拿到球
困難:因為機器人再 360 度跟球之絕對距離=( (X1-X2)^2+(Y1-Y2)^2 ) ^ ½ ),幾乎都完 全不同
所以沒辦法用只有一個固定的絕對距離來讓機器人知道他拿到球
解決方法:用建表的方式把所有角度跟機器人與球的絕對距離一一記錄起來 Figure 4.1:計算車頭跟球之距離
Figure 4.2:判斷機器人是否拿到球
優點:當建表完整度夠,就可以讓機器人順利把球推進球門 缺點:i.時間長
ii.換場地跟視訊裝置拉高高度就要重新寫
b.場地會因為燈光的因素而使球,球門,車頭,車尾的 RGB 變化,嚴重可能會使運算結果錯誤,導致很難把球踢進球門 解決方法:利用 ColorPix 軟體,在現場調整 RGB 既可
優點:可以使運算恢復
缺點:換場地就要重新算 RGB,耗時
c.場地可能造成機器人卡點
解決方法:買平滑場地地板(EX.壓克力板) 優點:不容易卡點
缺點:球可能容易甩出去(調弱機器人馬力既可)
II.學習心得
心得:能夠學到藍芽通訊協定跟影像處理跟機器人的 AI 實作,很開心,未來希望能夠 繼續研究機器人 AI 的領域,雖然要計算的東西跟場地的狀況研究要思考,有時候 AI 並不 是只靠單純的演算法推倒,而還要考慮環境會不會影響變數運算
===========================================================
(五)總結與未來展望
寫完這次的樂高機器人踢足球,遇到的困難都要好好的思考,而不是土法鍊鋼的把條件寫 死,像機器人跟場地會受燈光的因素,而造成 RGB 混亂,使運算錯誤,這可以用觀察顏色跟 亮度的關係來寫出一套演算法來解決,可是因為時間問題無法完成,還有機器人跟球之絕 對距離也可以用演算法算出圖中紅色足球的中心跟車頭黃色的中心,使絕對距離可以固 定,這些未來應該好好思考。
(六)參考文獻
[1]樂高藍芽通訊協定: http://www.codeproject.com/KB/cs/nxtBluetooth.aspx
[2]視訊拍照影像處理 http://ppt.cc/ZWb0
[3]樂高所有運作之技巧 BLOG
http://www.blogger.com/profile/12376832463978326433
[4]OPENCV 安裝教學
http://www.dotblogs.com.tw/chou/category/2293.aspx?Show=All
[5]樂高論壇 1
http://note.tcc.edu.tw/515.html
[6]藍芽 BLOG
http://meyl0101.pixnet.net/blog/post/4978803
[7]專門討論藍芽的論壇
http://it360.tw/forum/forum_posts.asp?TID=7556&KW=%E8%97%8D%E8%8A%BD
[8]OPENCV 影像處理論壇
http://www.opencv.org.cn/index.php
[9] Robot fun 機器人論壇
http://www.robofun.net/forum/forumdisplay.php?fid=68
致謝
感謝周老師的指導,周老師提供的演算法以及解決方法都很實用,能夠碰到機器人 AI 實作 跟藍芽通訊協定以及影像處理這三個領域很充實。
