032]S02:選擇具備以太網(wǎng)接口且支持EtherCAT協(xié)議的標準型或嵌入式工控機等工業(yè)領域通用計算機作為本控制器2硬件平臺，控制器2的軟件構架采用分層結構，如圖3所示，由底層到頂層分別劃分為設備層、中間層、應用層和用戶層。這種層次化設計，旨在高效的分配系統(tǒng)的資源，使資源利用更加合理，確?？刂葡到y(tǒng)各功能模塊的重用性，各層的擴展性，提高系統(tǒng)的開放程度。
[0033]S03:用戶層程序由組態(tài)軟件或通用可視化編程工具開發(fā)為用戶提供人機交互界面，如Labview、組態(tài)王、紫金橋等。用戶層程序由機器人配置界面Config模塊、主選擇界面Select模塊和用戶指令代碼輸入窗口 Window(i)模塊組成。Config模塊用于配置每一臺機器人的固定參數(shù)，包括:關節(jié)數(shù)k，機器人構型f、各關節(jié)運動范圍等。Select模塊用于選擇確定對哪一臺機器人Ri進行示教操作，選擇后即調用Window(i)模塊并將所選擇的機器人Ri的機器人固定參數(shù)傳輸給Window(i)模塊。由于本發(fā)明可以兼容控制的不同種類的機器人，這就允許在同一條產(chǎn)線上可以共存多臺構型不同，關節(jié)數(shù)量不同的機器人，如通用6自由度工業(yè)機器人和各種專用機器人，并且統(tǒng)一控制所有機器人，系統(tǒng)兼容性強，可擴展性強。Window(i)模塊為用戶提供了規(guī)范化用戶指令代碼格式，并對指令代碼進行編碼和功能定義。用戶指令代碼是控制機器人運動和作業(yè)的指令及指令參數(shù)的組合，如用戶指令代碼MoveL(x，y，z，01，Θ2，Θ3)由指令MoveL和指令參數(shù)(x，y，z，01，Θ2，Θ3)組成，是指以直線插補運動方式從當前位姿運動到空間目標位姿口(1，7，2，91，02，03)。用戶指令代碼的編制方法包括:示教編程方法和離線編程方法。示教編程方法包括示教、編輯和軌跡再現(xiàn)，可以通過示教器示教和導引式示教兩種途徑實現(xiàn)。離線編程方法是利用計算機圖形學成果，借助圖形處理工具建立幾何模型，通過一些規(guī)劃算法來獲取作業(yè)規(guī)劃軌跡。與示教編程不同，離線編程不與機器人發(fā)生關系，在編程過程中機器人可以照常工作。由于示教方式實用性更強，操作更簡便，因此本實施例采用示教編程為主，離線編程為輔的方式編制用戶指令代碼。通過用戶層程序Select模塊和Window(i)模塊的配合使用，為每臺機器人編寫用戶指令代碼。為提高用戶編寫指令代碼的效率，用戶可編寫并導出機器人指令代碼模板，并根據(jù)實際需要重復導入、修改并創(chuàng)建每臺機器人的指令代碼。
[0034]S04:如圖4所示，在控制器2內(nèi)存區(qū)域開辟一存儲區(qū)域，記為M，用于實現(xiàn)在用戶層程序與應用層程序之間進行信息交換。存儲區(qū)_或根據(jù)機器人數(shù)量進行η細分，第i個細分區(qū)域對應第i臺機器人，記為Mi ;存儲區(qū)Mi的最大用戶指令代碼存儲數(shù)量記為m;存儲區(qū)Mi中指令代碼存儲區(qū)按照最大用戶指令代碼數(shù)量m進行細分，第i臺機器人第k條指令代碼存儲區(qū)，記為MiCk;第i臺機器當前待執(zhí)行的指令代碼對應存儲區(qū)Mi的位置指針，記為Qi;用戶層程序負責把每臺機器人固定參數(shù)和用戶編寫的各機器人指令代碼存于存儲區(qū)M的相應位置，即第I臺機器人Rl的機器人固定參數(shù)和用戶指令代碼存入Ml，第η臺機器人Rn的指令代碼和機器人參數(shù)存入Mn。
[0035]S05:如圖4所示，應用層程序由實時運動控制軟件開發(fā)，如TwinCAT軟件、Codesys軟件等。主要功能為讀取各個細分存儲區(qū)域Mi中的機器人固定參數(shù)，選擇并讀取各機器人當前待執(zhí)行的指令代碼。第i臺機器人當前待執(zhí)行的指令代碼對應存儲區(qū)Mi的位置指針，記為Qi;應用層程序通過按順序對各個位置指針Qi進行控制，從而選擇存儲區(qū)M中各機器人當前待執(zhí)行的指令代碼的存儲地址，然后讀取所有機器人的當前待執(zhí)行的指令代碼，采用定周期掃描方式執(zhí)行，即每個周期遍歷掃描控制并讀取一次每一臺機器人當前掃描周期內(nèi)待執(zhí)行的一條指令代碼。
[0036]S06:中間層程序由實時運動控制軟件開發(fā)，如TwinCAT軟件、Codesys軟件等。主要功能為實施用戶指令功能算法。在每個掃描周期內(nèi)，通過基于所述應用層程序提供的機器人固定參數(shù)和當前用戶指令代碼參數(shù)以及基于所述存儲區(qū)E提供的每臺機器人對應的每個關節(jié)中的狀態(tài)數(shù)據(jù)，計算在當前掃描周期內(nèi)的所有機器人且所有關節(jié)的運動數(shù)據(jù);計算方法為:為不同構型，不同關節(jié)數(shù)量的機器人編寫統(tǒng)一的且健全的用戶指令代碼執(zhí)行函數(shù)庫，每臺機器人在運動過程執(zhí)行用戶指令代碼時，均調函數(shù)庫中相應的庫函數(shù)，庫函數(shù)實現(xiàn)機器人正運動學計算，運動所必需的中間點插補、逆運動學計算，得到各關節(jié)實時運動數(shù)據(jù)，并轉換成相應關節(jié)驅動模塊可接受的具體控制數(shù)據(jù)。如圖5所示，如當執(zhí)行用戶指令代碼1(^4(^7，2，01，02，03)時，中間層程序根據(jù)機器人固定參數(shù)和包含在用戶指令代碼的目標位姿p(X，y，z，θl，θ2，θ3)，在每個掃描周期內(nèi)，首先結合所述存儲區(qū)E提供的所有機器人且所有關節(jié)返回的狀態(tài)數(shù)據(jù)，如:當前位置，當前速度，狀態(tài)字等信息，進行機器人正運動學計算，求出所有機器人當前空間位姿P，運行速度V等;接著調用庫函數(shù)進行可達性分析，比較所有機器人的當前位姿和目標位姿，篩選出可以以直線插補方式運動到達的機器人，接著調用庫函數(shù)對這些機器人分別進行當前周期位姿插補點計算;然后調用庫函數(shù)分別求得的當前周期位姿插補點逆運動學計算;最后輸出求得的所有機器人且所有關節(jié)的當前周期實時運動控制數(shù)據(jù)。
[0037]S07:如圖6所示，在控制器2內(nèi)存區(qū)域開辟另一片存儲區(qū)域，記為E，用于實現(xiàn)在中間程序與硬件層程序之間進行信息交換。存儲區(qū)E域根據(jù)機器人數(shù)量進行η細分，第i個細分區(qū)域對應第i臺機器人，記為Ei;存儲區(qū)Ei按照關節(jié)數(shù)k再進行細分，第i臺機器人的第j個關節(jié)記為EiJj;存儲區(qū)EiJj按照數(shù)據(jù)傳輸方向再進行細分，第i臺機器人的第j個關節(jié)接收的控制數(shù)據(jù)輸入?yún)^(qū)，記為EiJjIn;第i臺機器人的第j個關節(jié)返回的狀態(tài)數(shù)據(jù)輸出區(qū)，記為EiJjOuU中間層程序負責把計算所得的當前掃描周期內(nèi)的所有機器人且所有關節(jié)的控制數(shù)據(jù)存于存儲區(qū)E的相應位置，即第I臺機器人的第I個關節(jié)RlJl的控制數(shù)據(jù)存入Eljlln，第η臺機器人的第k個關節(jié)RnJk的運動數(shù)據(jù)存入EnJkIn。
[0038]S08:如圖6所示，設備層程序在每個掃描周期實時把存儲區(qū)E中所有控制數(shù)據(jù)輸入?yún)^(qū)中的數(shù)據(jù)，通過以太網(wǎng)按照EtherCAT協(xié)議以報文的形式傳送到所有機器人且所有關節(jié)驅動模塊中去，并將所有機器人且所有關節(jié)中的狀態(tài)數(shù)據(jù)送回存儲區(qū)E中的狀態(tài)數(shù)據(jù)輸出區(qū)中。報文中每個關節(jié)所接收的控制數(shù)據(jù)，如:目標位置，控制字等，記為Date In;每個關節(jié)所返回的狀態(tài)數(shù)據(jù)，如當前位置，當前速度，狀態(tài)字等，記為Date Out;每個關節(jié)驅動模塊5作為EtherCAT從站，利用以太網(wǎng)全雙工特性，依序讀取報文相應位置的Date In數(shù)據(jù)，且將Date Out數(shù)據(jù)寫入報文相應位置，每個從站處理完報文后都將報文傳輸至下一個從站，由最后一個從站發(fā)回經(jīng)過完全處理的報文，并由第一個從站將其作為響應報文送回控制器中，從而實現(xiàn)主-從-主之間的循環(huán)通信。在每個掃描周期，所有機器人且所有關節(jié)的目標位置、當前位置、當前速度、控制字、狀態(tài)字都會更新一次。機器人連接順序確定后，EtherCAT的從站順序就相應確定，EtherCAT報文依序經(jīng)過各個從站，EtherCAT報文的處理是由EtherCAT從站設備硬件實現(xiàn)，延時短，不占用控制器CPU，因此可提高控制系統(tǒng)實時性高。
[0039]進一步的，為了讓本領域技術人員進一步了解本發(fā)明所提出的機器人群控系統(tǒng)及方法，下面結合具體實例進行說明。在本實施例中，控制機器人Rl和機器人R2進行直線插補運動的步驟如下:
在用戶層程序Config模塊中配置機器人Rl和機器人R2的固定參數(shù)，用戶層程序分別將機器人Rl和機器人R2的固定參數(shù)存入細分存儲區(qū)Ml和M2;在Select模塊中選擇機器人Rl進AWindow( I)模塊，用戶可通過示教編程方法或離線編程方法選擇機器人Rl目標位姿，記為ple(xl，yl，zl，011，021，Θ31)，并輸入第一條用戶指令代碼MoveL(xl，yl，ζ1，Θ11，Θ21，Θ31)，用戶層程序將此用戶指令代碼存入細分存儲區(qū)MlCl;返回Select模塊中選擇機器人R2進入Window(2)模塊，選擇機器人Rl目標位姿，記為p2e(x2，y2，ζ2，Θ12，Θ22，Θ32)，并輸入第一條用戶指令代碼此代1^(12，72，22，012，022，032)，用戶層