一種scara機器人分布式控制系統(tǒng)及其控制方法
【技術領域】
[0001]本發(fā)明涉及機器人控制系統(tǒng)，尤其涉及一種SCARA機器人分布式控制系統(tǒng)及其控制方法。
【背景技術】
[0002]SCARA機器人又稱為平面關節(jié)型機器人，有3個旋轉關節(jié)，其軸線相互平行，在平面內進行定位和定向，另一個關節(jié)是移動關節(jié)，用于完成末端件在垂直于平面的運動。SCARA機器人結構輕便、響應快，在X，Y方向上具有順從性，而在Z軸方向具有良好的剛度，特別適合于裝配工作，SCARA的另一個特點是其串接的兩桿結構，類似人的手臂，可以伸進有限空間中作業(yè)然后收回，適合于搬動和取放物件。由于SCARA機器人結構和動作原理的特殊性，電機的運行位置與機器人末端執(zhí)行器的位姿無法簡單直觀的進行對應，已知末端執(zhí)行器的位姿，求取各個關節(jié)電機的運行位置，稱之為反解，反解的計算比較復雜，因此控制機器人動作需要專用的控制器，目前主流的技術有以下兩種:
[0003]1、基于PC+運動控制卡的方案:邏輯控制和SCARA機器人相關的數(shù)學計算在PC平臺上進行，運動控制卡在每個控制周期將計算的結果通過脈沖或者通訊總線發(fā)送給伺服電機驅動器；
[0004]2、基于專用運動控制器的方案:在運動控制器中采用比較高端的處理器，完成邏輯控制和SCARA機器人相關的數(shù)學計算，在每個控制周期將計算的結果通過脈沖或者通訊總線發(fā)送給伺服電機驅動器。
[0005]然而，在上述現(xiàn)有技術中，存在以下問題:首先，現(xiàn)有技術屬于集中控制，控制器需要進行周期性的反解計算，對處理器的計算能力要求很高；其次，使用脈沖周期性通訊，控制器和每一臺電機驅動器都要連接脈沖線，在現(xiàn)場很容易受到干擾，出現(xiàn)干擾時排查和解決都比較困難，接線數(shù)量也較多；再次，使用高速現(xiàn)場總線周期性通訊，對總線的傳輸帶寬要求比較高，需要選擇高性能高成本的總線，實現(xiàn)方案復雜。
[0006]近年來，隨著半導體技術的飛速發(fā)展，嵌入式CPU處理能力越來越強，存儲空間越來越大，這使得電機驅動器在完成電機控制相關計算的前提下有富余的資源。但是目前電機驅動器富余的資源多用于增強電機的驅動性能，從系統(tǒng)的角度來看，驅動器CPU性能的提升并沒有降低控制器CPU的壓力。

【發(fā)明內容】

[0007]本發(fā)明要解決的技術問題在于，針對現(xiàn)有技術的不足，提供一種降低對主站計算能力的要求、降低對總線帶寬需求、降低布線和排查難度、提高抗干擾能力的SCARA機器人分布式控制系統(tǒng)及其控制方法。
[0008]為解決上述技術問題，本發(fā)明采用如下技術方案。
[0009]—種SCARA機器人分布式控制系統(tǒng)，所述SCARA機器人包括有依次設置的基座旋轉軸、小臂旋轉軸、升降軸和末端執(zhí)行器旋轉軸，所述SCARA機器人分布式控制系統(tǒng)包括有主站和從站，其中:所述主站內設有用戶程序模塊、PTP命令執(zhí)行模塊、SCARA反解計算模塊、通訊控制模塊、虛擬主軸模塊、電子凸輪模塊和電機控制模塊，其中，所述用戶程序模塊用于執(zhí)行用戶編寫的邏輯程序，該邏輯程序包含定位指令，在執(zhí)行定位指令時，PTP命令執(zhí)行模塊獲取指令中的目標點坐標，并發(fā)送到SCARA反解計算模塊，SCARA反解計算模塊對目標點的坐標進行一次反解計算，得出目標點對應的各個軸的位置，本軸位置直接發(fā)送給本軸的電子凸輪模塊，其他軸的位置通過通訊控制模塊發(fā)送給各個從站，之后虛擬主軸模塊持續(xù)發(fā)出虛擬主軸，驅動各軸的電子凸輪運行；所述從站內設有電機控制模塊、電子凸輪模塊和通訊模塊，所述通訊模塊接收到本軸目標位置后，將參數(shù)傳遞給電子凸輪模塊，電子凸輪模塊根據(jù)這些參數(shù)，計算對應的電子凸輪曲線并保存到凸輪表，接收到虛擬主軸時，根據(jù)凸輪表周期性的計算本周期應該發(fā)出的周期脈沖量，電機控制模塊每個周期接收電子凸輪模塊的輸出信號，控制電機運行。
[0010]優(yōu)選地，所述主站中:所述用戶程序模塊供于用戶編程，在程序的控制下調用SCARA機器人定位指令，定位指令包括直接坐標系下末端執(zhí)行器的X、Y、Z坐標以及末端執(zhí)行器的旋轉角度等參數(shù)，用戶程序模塊在定位指令生效后將這些參數(shù)傳遞給PTP命令執(zhí)行模塊；所述通訊控制模塊用于將參數(shù)打包發(fā)送到對應的電機驅動器；所述虛擬主軸模塊用于在插補運行中周期性的發(fā)出數(shù)量和速度可控的虛擬主軸。
[0011]優(yōu)選地，所述從站中:通訊控制模塊用于接收控制器發(fā)送給本機的參數(shù)，對通訊數(shù)據(jù)進行校驗，校驗通過后將參數(shù)發(fā)送到電子凸輪模塊；電子凸輪模塊用于根據(jù)設定的曲線類型、目標點位置、當前位置等參數(shù)自動計算生產凸輪曲線，運行時根據(jù)虛擬主軸周期計算得到每個周期的周期脈沖量，發(fā)送到電機控制模塊；電機控制模塊用于實現(xiàn)電機的控制算法，根據(jù)周期脈沖量驅動電機運行，同時反饋電機的運行狀態(tài)。
[0012]優(yōu)選地，所述電子凸輪模塊包括:參數(shù)緩沖區(qū)，保存通訊控制模塊發(fā)來的定位控制參數(shù)；電子凸輪曲線計算模塊，根據(jù)參數(shù)計算對應的電子凸輪曲線并保存到凸輪表；凸輪表，用來保存電子凸輪曲線；虛擬主軸接收模塊，每個周期接收控制器傳遞過來的虛擬主軸，計算主軸增量；電子凸輪運行控制模塊，每個計算周期根據(jù)虛擬主軸和凸輪表計算本周期應該發(fā)出的周期脈沖量，發(fā)送給電機控制模塊。
[0013]優(yōu)選地，電子凸輪模塊包括參數(shù)緩沖區(qū)、電子凸輪曲線計算模塊、凸輪表、虛擬主軸接收模塊及電子凸輪運行控制模塊，其中:參數(shù)緩沖區(qū)用來保存通訊控制模塊發(fā)送的相關參數(shù)，該相關參數(shù)包括曲線類型、目標點位置，定位啟動時，電子凸輪曲線計算模塊根據(jù)參數(shù)緩沖區(qū)內容計算相應的電子凸輪曲線，保存到凸輪表中，之后虛擬主軸接收模塊開始接收虛擬主軸數(shù)據(jù)，電子凸輪運行控制模塊根據(jù)虛擬主軸的位置和凸輪表計算得到周期脈沖量。
[0014]—種SCARA機器人分布式控制方法，該控制方法包括:主站將用戶編寫程序中的SCARA機器人定位指令解析成對應的參數(shù)，將參數(shù)通過總線發(fā)送到從站的電機驅動器中，電機驅動器接收到有效的數(shù)據(jù)后將其中的定位運行參數(shù)解析出來，發(fā)送到電子凸輪模塊，電子凸輪模塊在虛擬主軸的驅動下周期性的計算，每個周期計算出周期脈沖量，電機控制模塊根據(jù)周期脈沖量驅動電機運行。
[0015]優(yōu)選地，控制系統(tǒng)由一個主站和三臺從站構成，一個主站和三臺從站分別對應SCARA機器人的基座旋轉軸、小臂旋轉軸、升降軸和末端執(zhí)行器旋轉軸，主站電機驅動器基于ARM等嵌入式處理器，處理器上運行的用戶程序模塊用于處理用戶編寫的程序，用戶編寫程序可控制SCARA機器人的定位運行，程序判斷定位指令滿足執(zhí)行條件時，將參數(shù)送到PTP命令執(zhí)行模塊，PTP命令執(zhí)行模塊根據(jù)SCARA反解算法計算出目標位置對應的各關節(jié)電機位置。
[0016]優(yōu)選地，SCARA反解的算法為:Θ I = atan2 (A, 土 sqrt (1-A2)) - Φ ； θ 2 =acos ((r*sin ( θ 1+ φ) -LI) /L2) ；d3 = -Pz ; θ 4 = θ 2-asin (-sin θ l*nx+cos θ l*ny) ；Α =(L12_L22+Px2+Py2)/(2*Ll*sqrt(Px2+Py2)) ；r = sqrt(Px2+Py2) ； Φ = atan2 (Px, Py) ；nx=cos ( Θ 1+ Θ 2+ Θ 4) ；ny = sin ( Θ 1+ Θ 2+ Θ 4);其中，Px、Py、Pz 為目標點直角坐標系下 X、Y、Z坐標，LI為第一臂長度，L2為小臂長度，Θ I為基座關節(jié)角度，Θ 2為小臂關節(jié)角度，d3為升降高度，0 4為末端執(zhí)行器旋轉角度。
[0017]優(yōu)選地，計算出Θ 1、Θ 2、d3、Θ 4后，SCARA反解計算模塊根據(jù)用戶預設的機械參數(shù)，該機械參數(shù)為減速機減速比、臂長、螺距，根據(jù)機械參數(shù)計算出對應各軸電機的目標位置，之后主站將這些數(shù)據(jù)發(fā)送到本機電子凸輪模塊和通訊控制模塊，通訊控制模塊由通訊的處理程序及相關通訊接口硬件組成，在接收到SCARA反解計算模塊發(fā)送過來的數(shù)據(jù)后，將數(shù)據(jù)轉換成通訊需要的格式，通過接口硬件發(fā)送到對應的電機驅動器，在各從站接收完定位運行參數(shù)并生成凸輪曲線后，PTP命令執(zhí)行模塊啟動虛擬主軸，開始驅動定位運行。
[0018]本發(fā)明公開的SCARA機器人分布式控制系統(tǒng)及控制方法中，主站將用戶編寫程序中的SCAR