//Logo Image
作者:謝佩均(2005-07-15);推薦:徐業良(2005-07-18)
附註:本文為九十四學年度元智大學機械工程研究所謝佩均碩士論文「具基礎自主行為能力之遠端臨場機器人之研發」第四章。

具基礎自主行為能力之遠端臨場機器人之研發—行控電腦程式設計

1為遠端臨場機器人之行控電腦與各元件間訊號傳輸概要圖。遠端使用者從網際網路端(PC Client程式)下達指令後,經過PIC_SERVER傳達到行控電腦處理,行控電腦程式即會依軌道辨識器、超音波模組、電池電量等元件回傳的訊息作判斷,驅動馬達控制器讓系統前進轉彎或停止、控制警示燈閃爍、閃光蜂鳴器開始警報、控制RC SERVO馬達切換充電端子位置、或調整IP CAM攝影機觀看遠端環境。本文將詳細說明行控電腦程式之設計。

1. 系統各元件與訊號概要圖

1.         行控電腦程式流程

如圖1所示,行控電腦程式共有「IP CAM控制程式」、「行控電腦對應PIC SERVER程式」、「警示燈程式」、「超音波測距程式」、「充電切換程式」、「馬達控制程式」、「圖騰辨識程式」、和「AGV行動程式」等8個副程式。圖2為行控電腦的程式流程圖。如圖所示,程式一開始關閉充電端子與初始化設定值後,會判斷遠端使用者是否從遠端輸入訊號,如沒有指令輸入,系統會進入「自主行為模式」,反之則進入「遠端操控模式」,依遠端使用者輸入的指令,由「行控電腦對應PIC_SERVER程式」處理做不同的動作。如果在遠端操控模式下太久沒有指令訊號輸入,即會回到自主行為模式。

在自主行為模式下,由「AGV行動程式」處理軌道辨識器回傳的訊號,配合「超音波測距程式」、「警示燈程式」、與「馬達控制程式」,讓機器人能穩定的沿著軌道自主前進、避障、與發出警示。而當「AGV行動程式」判別有T型圖騰時,即啟動「圖騰辨識程式」,由軌道辨識器傳回訊號辨識不同的圖騰,並透過「馬達控制程式」做出對應動作,讓機器人停止、前進、反向,或是啟動「充電切換程式」讓機器人充電、啟動「IP CAM控制程式」讓攝影機旋轉到固定的觀看點。程式序列執行完畢後,又回到圖中A點,等候遠端使用者輸入的指令。

而在遠端操控模式下,「行控電腦對應PIC_SERVER程式」依遠端使用者不同的輸入指令,啟動不同的「圖騰辨識程式」。機器人與自主模式相同,序列執行「AGV行動程式配合超音波測距程式」、「警示燈程式」、與「馬達控制程式」,讓機器人沿著軌道前進、避障、與發出警示。當「AGV行動程式」判別有T型圖騰時,啟動「圖騰辨識程式」,依不同的圖騰讓機器人有不同的動作流程。

例如當遠端使用者下達充電指令,系統會由「行控電腦對應PIC_SERVER程式」啟動充電的「圖騰辨識程式」。在充電圖騰辨識程式裡,軌道辨識器辨識到軌道端點的圖騰時,會要求「馬達控制程式」發出反向、前進指令;軌道辨識器辨識到充電站的圖騰時,會啟動「充電切換程式」進行充電程序;軌道辨識器辨識到其他圖騰時,都只是啟動「馬達控制程式」直接通過,而不做其他動作。

2. 行控電腦程式流程圖

3為行控電腦主程式程式碼。在主程式一開始利用pic_PROTO_init_peichun()對行控電腦的數位輸入輸出腳位做定義,接著利用PIN_C4關閉充電繼電器,初始化圖騰辨識程式之判斷值為自主行為模式,將系統初始定義為正向行走,使用door_close()副程式關閉充電端子,利用camstart()副程式讓IP CAM旋轉回觀看的初始定義點,利用m_stop()副程式讓機器人暫停一下讀取開機電壓後,才開始整個流程。而第一個while回圈即是圖2流程圖中的A點,且利用讀入PIN_A0的高低電位判定系統應執行遠端操控模式或是自主行為模式。

3. 系統主程式程式碼

行控電腦程式中8個副程式條列如下,以下各節即一一說明這些副程式的設計。

(1)   行控電腦對應PIC_SERVER程式:m_control()

(2)   AGV行動程式:go(), back()

(3)   圖騰辨識程式:fgo(), fgo1(), fgo2(), fgo3(), fgo4(), cpower(), bgo(), bgo1(), bgo2(), bgo3(), bgo4()

(4)   IP CAM控制程式:camstart(), site1(), site2(), site3(), site4(), errorcam()

(5)   充電切換程式:power()

(6)   超音波測距程式與警示燈程式:us1(), us2()

(7)   與馬達控制程式:m_stop(), m_forward(), m_forwardl(), m_forwarda(), m_back(), m_backl(), m_backa(), m_right(), m_left(), m_b_right(), m_b_left())

2.         超音波測距與警示燈程式

超音波測距與警示燈程式流程如圖4所示。超音波感應器的基本工作原理為,給予超音波測距模組一觸發(Pulse Trigger)訊號後發射超音波,當超音波發射到物體而反射回來時,超音波測距模組會輸出一迴響(echo)訊號,使用者可以由觸發訊號和迴響訊號間的時間差,判定物體與超音波發射模組的距離。當時間差小於設定值時,接著會啟動警示燈閃爍,系統並停止前進,等待障礙物離開或是當時間差大於設定值,系統會繼續動作。

4. 超音波測距與警示燈程式流程

5為超音波測距與警示燈程式碼,在us1()超音波測距副程式中,利用行控電腦的PIN_D6當作觸發訊號,每次發射超音波訊號10msPIN_C0當作接收訊號,且在觸發的同時利用行控電腦內部的計時程式來計算這兩個訊號的間隔時間,其中程式的wait_for_low_to_high1()wait_for_low1()兩個小副程式,是讓行控電腦能夠判讀PIN_C0的脈波訊號,而當此時間計數差小於設定值時,執行警示燈閃爍副程式,即是啟動與關閉PIN_D7間隔250ms讓警示燈一閃一滅,並且系統等待警示燈閃爍完畢後才繼續執行後面的程式。

由程式計數推算,觸發訊號啟動發出聲波與接收到訊號的時間差為20msec(程式計數為15),此時實際量測障礙物距離系統約為15cm,而系統閃爍警示燈500ms

5. 超音波測距與警示燈程式

3.         AGV行動程式與馬達控制程式

機器人需要固定在軌道上來回巡邏,且能夠在不同停止點停下來巡視或是觀看遠端環境,以及在充電站時進入充電模式,這些沿著軌道行走的動作,都必須倚賴軌道辨識器。軌道辨識器是利用7個光感應器佈置成一H型,光感應器的配置如圖6所示來達成軌道辨識(AGV行動程式)與圖騰辨識(圖騰辨識程式)的功能。此7個光感應器是系統一直供電,且每100~120ms對行控電腦做一次訊號更新,讓系統能隨時接收軌道上不同情況的訊號。

6. 7個光感應器佈置成的H型軌道辨識器

而機器人的前進與轉彎訊號,是利用軌道辨識器上方或下方的3個光感應開關。如圖7所示,利用3個光感應開關排成ㄇ字形的幾何形狀,來迅速反應軌道的位置,當僅有中央感應器感應到軌道時,表示應筆直前進;當左(右)端感應器與中央感應器都感應到軌道時,即表示系統要向左(右)轉。依軌道辨識器的光感應器幾何配置推算,軌道最小的轉向角度約為,且軌道寬度要小於42mm

7. 上方∕下方ㄇ字型行動器光感應開關配置圖

而控制機器人的前進、轉彎是利用左右兩顆直流馬達驅動差速所達成,而此兩顆馬達分別獨立使用馬達控制器來驅動(TECEL公司編號D200的馬達驅動器)。

要達成上述之功能,系統的AGV行動程式流程如圖8,先由行走方向參數決定使用軌道辨識器上方或下方的ㄇ型光感應開關,如行走方向參數為正向,即利用圖7下方的3個ㄇ型光感應開關(PIN_B4,PIN_B6,PIN_B7)來辨識軌道位置,反之則是用圖7上方3個ㄇ型光感應開關(PIN_B1,PIN_B2,PIN_B4)。光感測器感測到軌道時為高電位,反之是低電位,此光感測器的高低電位即為數位輸入訊號,行控電腦讀入軌道辨識器的資料後由AGV行動程式判讀系統所要行走的動作,接著再送出資料到馬達控制程式,控制馬達驅動器讓系統前進、轉彎或停止。

8. AGV行走程式與馬達控制程式流程圖

9AGV行動程式碼。在正向時(go())依據光感應開關PIN_B4PIN_B6PIN_B7的高低電位訊號,判斷系統應直行(m_forward)、左轉(m_left)、或右轉(m_right);同理,在反向時(back())依據光感應開關PIN_B1PIN_B2PIN_B4的高低電位訊號,判斷系統應直行(m_back)、左轉(m_b_left)、或右轉(m_b_right)

9. AGV行動程式

馬達控制程式應用波寬調制(Pulse Width Modulation, PWM)的訊號去控制馬達,即可簡單又方便控制直流馬達的轉速。首先將左方馬達定義為motor2,右方馬達定義為motoe3,而且把轉速定義成數字0~1516個階段的轉速,以0表示馬達停止,15表示馬達轉速最高,而數字前面加個負號如(0~-15)表示馬達反向旋轉16個階段的轉速。此設定是引用PIC_Protomotor()的副程式,有此方便的定義,可控制機器人作前進、後退、轉彎、變速度等動作。

10為馬達控制程式的程式碼,各副程式功能說明如下:

(1)   m_stop():左右馬達皆停止轉動,也就是讓機器人停止前進。

(2)   m_forward():左右馬達皆以+12階段的轉速前進,此轉速下機器人前進速度可達每秒15cm。此速度設定為在當遠端使用者下達指令,要機器人觀察某停止點或是強制充電時,機器人會加快速度來達成指定動作。

(3)   m_forwardl():左右馬達皆以+9階段的轉速前進,此轉速下機器人前進速度約為每秒8cm。此速度設定為機器人在自主行為模式下行走的速度。

(4)   m_right():左邊馬達停止,右邊馬達以+5階段轉速前進,如此可以讓機器人以差速的方式向右轉。+5階段轉速約是每秒前進4cm,以此較慢速度設定進行左右轉,可增加機器人沿著軌道行走的穩定性。

(5)   (m_back()):機器人反向行走的馬達控制程式指令。

10. 馬達控制程式程式碼

實際測試執行整個程式流程,「AGV行動程式」每100~120ms被執行一次,也就是光感應器每100~120ms對行控電腦做一次訊號更新。反推如果機器人行走速度為每秒8cm時,軌道辨識的解析度約為1cm

4.         圖騰辨識程式

機器人的軌道辨識器佈置成H型的7個光感應器,可以讀入軌道上不同幾何圖形的訊號後,利用行控電腦8個腳位(PIN_B0~PIN_B7)組成的數位輸入阜接收這8個字元的二進位數據,將此訊號轉換成十進位後,會有256種不同的CASE代碼組合,讓系統針對特定的圖騰(也就是不同的CASE代碼)做相對應的動作,而沒有設定的CASE代碼就繼續執行原程式。

在實際應用過程中,發現圖騰若無特殊設計,圖騰辨識容易與軌道左右轉辨識混淆,降低圖騰的辨識率,因此軌道圖騰的前後方設計了T型圖騰,以與單純左右轉作區分。如圖11所示,以CASE 151為例,一開始系統在AGV行動程式裡,PIN_B4(軌道辨識器正中的光感應器)感應到軌道而直線前進,直到PIN_B1PIN_B2(上方2個光感應器)也感應到軌道,也就是辨識到T型圖騰,此時機器人降低速度,以每秒4cm的速度筆直前進,直到軌道辨識器下方PIN_B6PIN_B7同時辨識到軌道時停止前進,啟動圖騰辨識程式辨識圖騰,判別對應的CASE

11. 圖騰辨識的流程

12為對應圖11圖騰辨識流程,啟動圖騰辨識程式之主程式碼。在AGV行走程式中,當系統為正向行走、且PIN_B6PIN_B7PIN_B4辨識到T型圖騰時,由do後面的forwarda()讓機器人減速筆直前進直到while成立,也就是直到軌道辨識器上方PIN_B1PIN_B2同時辨識到軌道時,系統即會啟動圖騰辨識程式辨識對應的CASE。在自主行為模式下的圖騰辨識程式是fgo()bgo(),但是當有遠端訊號輸入時會對應不同的圖騰辨識程式fgo1()fgo2()fgo3()fgo4()cpower()bgo1()bgo2()bgo3()、和bgo4()

12. 啟動圖騰辨識程式之主程式碼

如表1,本系統設計8個不同的圖騰,而不同的圖騰也對應不同的CASE。圖騰12表系統走到軌道的兩端,系統會依軌道辨識器判定執行CASE 7CASE 193,讓系統更改行走的方向。圖騰3~7則分別表示停止點IIV和充電區,注意正向行走或反向行走在相同的停止點會對應不同的CASE,如停止點I在正向行走時對應為CASE 151,而反向行走時對應為CASE 241。圖騰8留作系統擴充時使用(CASE 95,CASE 219)

1. 圖騰形狀與CASE對照表

編號

行走狀態

圖騰形狀

正向行走CASE代號

反向行走CASE代號

1

盡頭行走轉換(正行轉反行)

7

 

2

盡頭行走轉換(反行轉正行)

 

193

3

停止 I

151

241

4

停止 II

87

217

5

停止 III

55

213

6

停止 IV

31

211

7

充電區

183

245(保留)

8

預備

95(保留)

219(保留)

13是圖騰辨識程式fgo()bgo()程式碼,也就是當系統在自主行為模式下正向行走(fgo())與反向行走(bgo())時啟動的軌道辨識器程式碼。例如當正向行走時,在CASE 151時程式會停止,並且執行控制IP CAM到第一個停止點的副程式(site1());在CASE 183時會對應充電副程式(power());當辨識為CASE 95時,只會執行(fb())的副程式(不理會AGV行動程式控制,慢速向前行走1.5秒,離開圖騰);在辨識為CASE 7時表示機器人行走到端點,將行走方向參數變成1(direct=1),讓系統執行反向行走的程式。

13. 圖騰辨識程式

5.         充電切換程式

機器人在電源不足時能自動在充電站停下充足電力後自動離開,如尚未充足電力但遠端使用者下達要機器人要離開充電站到別的停止點時,機器人也要停止充電離開充電站。圖14為充電流程圖,當機器人進入充電區時會利用行控電腦的類比輸入阜(PIN_E0)讀入電池電壓,當電壓在設定電壓之下,而且行走方向為正向時,系統即會停止前進開始進行充電。首先行控電腦驅動RC SERVO馬達將充電端子接觸到充電站進行充電,接著開啟充電繼電器讓系統電源切換到充電站的電源供應器上,確保機器人在充電時仍然可以正常運作,而電池不再供電、讓電池獨立充電。充電繼電器作動後即開始計時,當充電150分鐘、或是有外部指令介入時,充電繼電器關閉、回復到電池供電,機器人並將充電端子收回,最後離開充電站讓系統繼續執行巡邏模式。圖15為充電端子與充電站接點充電示意圖,利用RC SERVO控制轉向角度,讓系統的充電端子能穩固且正確的接觸到充電站接點,進行充電。

14. 系統充電流程

15. 充電端子與充電站接點充電示意圖

16為充電切換程式碼。當機器人為正向行走時且軌道辨識器讀入資訊為CASE 183,系統即會執行power()充電切換程式。首先行控電腦會利用類比輸入PIN_E0讀取電池電壓,當電池電壓不滿設定值(9.8V)時,系統會執行充電端子接觸充電站的副程式door_open(),也就是利用PIN_C3傳送脈衝訊號控制RC_SERVO馬達帶動充電端子,讓充電端子接觸充電站以讓系統進行充電。接著由PIN_C4送訊號開啟充電繼電器,讓電源切換到充電站的電源供應器提供電源。充電過程中行控電腦會計時150分鐘後或有外界指令介入時,會再由PIN_C4關閉充電繼電器,讓機器人電源切回電池供電,接著執行充電端子離開充電站的副程式door_close(),也就是再次利用PIN_ C3送出脈衝訊號將充電端子收回系統本體,讓機器人回到軌道繼續在作動。

程式中for迴圈執行一次約需1秒,以變數a值來作150分鐘計時(a值大於1800時停止充電),但充電過程中程式仍不斷檢查是否有PIN_A0~PINA5傳來之外界指令,如收到外界指令for迴圈裡的a值都會提早大於1800,跳離計數程式,讓充電提前完成。

16. 充電切換程式

6.         IP CAM控制程式

本系統使用SONY公司編號SNC-RZ25NIP CAM,其Pan/Tilt角度為340/120度,Zoom放大倍率為18倍,可自動對焦。IP CAM控制程式流程如圖17,行控電腦會因軌道辨識程式對應不同的CASE,而當這些CASE有對應到IP CAM控制程式site1()site2()site3()site4()時,即可控制IP CAM旋轉到預設拍攝位置,讓遠端使用者可以觀看定點環境資訊。

17. IP CAM控制程式流程

SONY這款IP CAM可透過RS232接收VISCA控制訊號,進而控制IP CAM的動作。IP CAM內部有6組預設觀看儲存點的記憶位置,可以記憶6組不同的轉動角度及縮放大小,控制IP CAM設定或呼叫預設觀看點的VISCA控制指令如圖18所示。這些控制碼是由1416進位的字元組成,控制碼第10個字元為1時(例如“Set”(8101043F010pFF)),是設定IP CAM記憶目前設定的旋轉角度及縮放大小為預設觀看點,控制碼第10個字元為2時(例如“Recall”(8101043F020pFF)),是呼叫設定好的預設觀看點。控制碼第12個字元p值是記憶編號(memory number),總共是0~5,也就是有6組可以記憶的編號。

18. IP CAM內部記憶控制碼

IP CAM控制程式碼,主要在利用行控電腦上的RS232端子將上述14個字元構成的VISCA控制碼依序傳送給IP CAMRS232端子,進而呼叫IP CAM中已記憶的預設觀看點。圖19即為IP CAM控制程式(site1())的程式碼,其事先定義72字元16進位的矩陣,當行走方向為正向時,程式會依序傳送這個矩陣的值(8101043F0200FF)此VISCA指令也就是要求IP CAM旋轉到第一個預設拍攝位置SONY公司的IP CAM中型號SNC RZ30N16個預設觀看儲存點可以設定,而SNC RZ25N只有6個。

19. IP CAM控制程式

7.         行控電腦對應PIC_SERVER程式

遠端使用者經由PC Client端程式下達網路指令給PIC_SERVER端程式解碼後,再由PIC_SERVER透過5(PIN_A1PIN_A2PIN_A3PIN_B2PIN_B3)腳位的數位輸入(DI)傳達給行控電腦,而機器人利用行控電腦對應PIC_SERVER程式,將網路下達的指令輾轉變成機器人應對應的動作。以下分別敘述PIC_SERVER端解碼程式與行控電腦對應PIC_SERVER程式之設計。

(1)   PIC_SERVER端解碼程式

遠端使用者透過PC Client端程式下達指令給PIC_SERVERPIC_SERVER端程式碼如圖20所示,主要是利用callback_HTTPclientCmdHandler副程式處理網路下達的指令。例如當網路下達指令的第一個字元http.clientCmd[0]A時,即會由DI輸入一組11010的控制指令,其中字元為0(1)時,表輸出訊號為低電位(高電位)。

20. PIC_SERVER端程式

21PIC_SERVER輸出訊號與行控電腦接收訊號的溝通示意圖,PIC_SERVER利用其之PIN_A1PIN_A2PIN_A3PIN_B2PIN_B3輸出訊號,而行控電腦利用PIN_A0PIN_A1PIN_A2PIN_A3PIN_A5接收訊號。PIC_SERVER輸出的這組DI訊號,最多可有32種指令字串與行控電腦溝通,如圖21所示,本系統目前使用了7種控制狀態(PC Client端指令),分別是停止點I(A)、停止點II(B)、停止點III(C)、停止點IV(D)、選擇恢復為自主行為模式(H)、選擇強制充電模式(F)、與停止系統(G)

21. PIC_SERVER輸出訊號與行控電腦接收訊號的溝通示意圖

(2)   行控電腦對應PIC_SERVER程式

22為行控電腦對應PIC_SERVER程式m_control()之程式碼,其中利用PIN_A1PIN_A2PIN_A3PIN_A5接收訊號,接著判斷與翻譯PC Client端所下達的指令的意思後,改變對應的圖騰辨識程式。例如當PC Client端程式指令下達停止點I(A),透過PIC_SERVER的解碼成一組11010的訊號,而行控電腦利用PIN_A0PIN_A1PIN_A2PIN_A3PIN_A5接收訊號當這組訊號為11010,判斷翻譯後,會啟用停止點I軌道辨識程式,在這個停止點I軌道辨識程式下,機器人會沿著軌道自動行駛到停止點I的位置,停下來讓攝影機觀看停止點I的環境,讓遠端使用者透過機器人了解停止點I的資訊,同理可以對應其他狀態。

22. 行控電腦對應PIC_SERVER程式