一種輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)的制作方法
【專利摘要】本發(fā)明公開了一種輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)，所述的控制系統(tǒng)包括主站控制計算機(jī)和控制器，所述控制器包括DSP芯片控制器、集成芯片處理器、PLC控制器和ARM控制器，DSP芯片控制器和ARM控制器均與所述主站控制計算機(jī)通信連接，DSP芯片控制器與ARM控制器之間通信連接，四臺稀土永磁無刷直流伺服電機(jī)均與所述DSP芯片控制器通信連接，多個避障位移傳感器、定位傳感器S5、加速度傳感器A1、加速度傳感器A2和加速度傳感器A3均與所述DSP芯片控制器和ARM控制器通信連接。本發(fā)明的輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)計算速度快，使關(guān)節(jié)機(jī)器人手臂轉(zhuǎn)向靈活、動作穩(wěn)定精確、體積小巧，性能穩(wěn)定，系統(tǒng)抗干擾能力強(qiáng)。
【專利說明】
一種輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)，屬于裝配作業(yè)的四關(guān)節(jié)機(jī) 器人手臂應(yīng)用領(lǐng)域。
【背景技術(shù)】
[0002] 在工業(yè)生產(chǎn)中，工業(yè)機(jī)器人可以代替人類做一些比較單調(diào)、比較頻繁和重復(fù)率較 高的長時間作業(yè)，或是在危險、惡劣環(huán)境下的作業(yè)，一般用作搬取零件和裝配工作，在微電 子制造業(yè)、塑料工業(yè)、汽車工業(yè)、電子產(chǎn)品工業(yè)、藥品工業(yè)和食品工業(yè)等領(lǐng)域得到廣泛的應(yīng) 用，它對于提高生產(chǎn)自動化水平、勞動生產(chǎn)率和經(jīng)濟(jì)效益、保證產(chǎn)品質(zhì)量、保障人身安全、改 善勞動環(huán)境，減輕勞動強(qiáng)度、節(jié)約原材料消耗以及降低生產(chǎn)成本等有著十分重要的意義。 [0003] SCARA工業(yè)機(jī)器人即裝配作業(yè)的機(jī)器人手臂是一種圓柱坐標(biāo)型的工業(yè)機(jī)器人，它 依靠旋轉(zhuǎn)關(guān)節(jié)大臂和小臂來實現(xiàn)X-Y平面內(nèi)的快速定位，依靠一個手腕移動關(guān)節(jié)和一個手 腕旋轉(zhuǎn)關(guān)節(jié)在Z方向上做伸縮和旋轉(zhuǎn)運動，其具有四個運動自由度，該系列的操作手在其動 作空間的四個方向具有有限剛度，而在剩下的其余兩個方向上具有無限大剛度。這種結(jié)構(gòu) 特性使得SCARA機(jī)器人擅長從一點抓取物體，然后快速的安放到另一點，因此SCARA機(jī)器人 在自動裝配生產(chǎn)線上得到了廣泛的應(yīng)用。SCARA機(jī)器人結(jié)構(gòu)緊湊、動作靈活，速度快、位置精 度高，它的使用大大提高了機(jī)器人對復(fù)雜裝配任務(wù)的適應(yīng)性，同時也降低了成本，提高了工 作空間利用率。
[0004] SCARA機(jī)器人在搬運貨物過程中要時刻判斷主控制器輸入的位置參數(shù)，并判斷周 圍的環(huán)境時刻避障，然后由運動控制器反復(fù)控制其精確的加速和減速進(jìn)行搬運貨物，稍微 的誤差累計就有可能在多回合運動中導(dǎo)致運輸失敗。國內(nèi)對SCARA機(jī)器人的使用雖然有幾 十年，但是由于國內(nèi)工業(yè)機(jī)器人發(fā)展起步比較晚，受較多關(guān)鍵技術(shù)的影響，SCARA機(jī)器人的 發(fā)展也受所影響，傳統(tǒng)的機(jī)器人原理如圖1所示，在長期使用期間出現(xiàn)眾多問題： (1)在SCARA機(jī)器人進(jìn)行零位置歸位時，或者是機(jī)器人復(fù)位時，均依靠人工目測的方式， 使得機(jī)器人的零位置設(shè)定有誤。
[0005] (2)雖然永磁直流伺服電機(jī)較直流電機(jī)、步進(jìn)電機(jī)功能有所提高，但是普通永磁直 流伺服電機(jī)為有刷結(jié)構(gòu)，因存在集電環(huán)炭刷結(jié)構(gòu)，在電機(jī)運行時產(chǎn)生火花，特別是在高速時 將產(chǎn)生嚴(yán)重的環(huán)火，產(chǎn)生無線電干擾，不適合高速旋轉(zhuǎn)，不適合運用于帶載運行的SCARA機(jī) 器人。
[0006] (3 )永磁直流伺服電機(jī)為有刷結(jié)構(gòu)，集電環(huán)和碳刷之間直接接觸且高速相對運動， 這種機(jī)械結(jié)構(gòu)使得有機(jī)械摩擦存在，產(chǎn)生較大的機(jī)械噪聲，使得SCARA機(jī)器人的噪聲污染增 大。
[0007] (4)永磁直流伺服電機(jī)為有刷結(jié)構(gòu)，需要定期更換炭刷，電機(jī)只能采用開啟式防護(hù) 形式，并且有刷直流電機(jī)效率低，不適合運用于帶載運行的SCARA機(jī)器人。
[0008] (5)在SCARA機(jī)器人多軸伺服控制系統(tǒng)中，基于單片機(jī)或者是DSP的控制系統(tǒng)都需 要編寫伺服控制軟件，使得系統(tǒng)開發(fā)周期較長，而且大大降低了系統(tǒng)的運算速度，并且程序 可移植能力較差。

【發(fā)明內(nèi)容】

[0009] 本發(fā)明主要解決的技術(shù)問題是提供一種輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)，該輕 載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)計算速度快，使關(guān)節(jié)機(jī)器人手臂轉(zhuǎn)向靈活、動作穩(wěn)定精確、 體積小巧，性能穩(wěn)定，噪聲低，方便機(jī)器人插入臨時動作，系統(tǒng)開發(fā)周期短，程序可移植能力 強(qiáng)。
[0010] 為解決上述技術(shù)問題，本發(fā)明采用的技術(shù)方案是：提供一種輕載四核快速關(guān)節(jié)機(jī) 器人控制系統(tǒng)，所述關(guān)節(jié)機(jī)器人采用稀土永磁無刷直流伺服電機(jī)X驅(qū)動機(jī)器人大臂旋轉(zhuǎn)運 動、采用稀土永磁無刷直流伺服電機(jī)Y驅(qū)動機(jī)器人小臂旋轉(zhuǎn)運動、采用稀土永磁無刷直流伺 服電機(jī)Z驅(qū)動機(jī)器人手腕旋轉(zhuǎn)運動、采用稀土永磁無刷直流伺服電機(jī)R驅(qū)動機(jī)器人手腕升降 運動，所述機(jī)器人大臂上安裝有避障位移傳感器S1、避障位移傳感器S2和加速度傳感器A1， 所述機(jī)器人小臂上安裝有避障位移傳感器S3、避障位移傳感器S4和加速度傳感器A2,所述 機(jī)器人手腕上安裝有定位傳感器S5和加速度傳感器A3,所述的控制系統(tǒng)包括主站控制計算 機(jī)和控制器，所述控制器包括DSP芯片控制器、集成芯片處理器、P1C控制器和ARM控制器，所 述DSP芯片控制器、P1C控制器和集成芯片處理器均與所述主站控制計算機(jī)通信連接，所述 DSP芯片控制器、集成芯片處理器和ARM控制器彼此通信連接，所述P1C控制器與ARM控制器 通信連接，所述稀土永磁無刷直流伺服電機(jī)X、稀土永磁無刷直流伺服電機(jī)Y、稀土永磁無刷 直流伺服電機(jī)Z和稀土永磁無刷直流伺服電機(jī)R均與所述DSP芯片控制器通信連接，所述避 障位移傳感器S1、避障位移傳感器S2、避障位移傳感器S3、避障位移傳感器S4、定位傳感器 S5、加速度傳感器A1、加速度傳感器A2和加速度傳感器A3均同時與所述DSP芯片控制器和 ARM控制器通信連接。
[0011] 在本發(fā)明一個較佳實施例中，還包括為所述關(guān)節(jié)機(jī)器人和控制系統(tǒng)提供電源的主 電源和備用電源，所述關(guān)節(jié)機(jī)器人中的各個所述電機(jī)和傳感器以及控制系統(tǒng)中的控制器均 與所述主電源和/或備用電源電性連接。
[0012]在本發(fā)明一個較佳實施例中，所述稀土永磁無刷直流伺服電機(jī)X、稀土永磁無刷直 流伺服電機(jī)Y、稀土永磁無刷直流伺服電機(jī)Z和稀土永磁無刷直流伺服電機(jī)R上勻設(shè)置有光 電編碼器，所述光電編碼器與所述DSP芯片控制器電性連接。
[0013] 在本發(fā)明一個較佳實施例中，所述機(jī)器人大臂上安裝有磁電傳感器EM1，所述機(jī)器 人小臂上安裝有磁電傳感器EM2,所述機(jī)器人手腕上安裝有磁電傳感器EM3和EM4,所述磁電 傳感器EM1、磁電傳感器EM2、磁電傳感器EM3和EM4均與所述DSP芯片控制器和ARM控制器通 信連接。
[0014] 本發(fā)明的有益效果是:本發(fā)明的輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)計算速度快， 使關(guān)節(jié)機(jī)器人手臂轉(zhuǎn)向靈活、動作穩(wěn)定精確、體積小巧，性能穩(wěn)定，系統(tǒng)抗干擾能力強(qiáng)，噪聲 低，方便機(jī)器人插入臨時動作，系統(tǒng)開發(fā)周期短，程序可移植能力強(qiáng)。
【附圖說明】
[0015] 為了更清楚地說明本發(fā)明實施例中的技術(shù)方案，下面將對實施例描述中所需要使 用的附圖作簡單地介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實施例，對于 本領(lǐng)域普通技術(shù)人員來講，在不付出創(chuàng)造性勞動的前提下，還可以根據(jù)這些附圖獲得其它 的附圖，其中： 圖1為傳統(tǒng)SCARA機(jī)器人控制器原理圖； 圖2為基于四軸稀土永磁無刷直流伺服電機(jī)SCARA機(jī)器人二維結(jié)構(gòu)圖； 圖3為DSP控制器與集成芯片處理器連接示意圖； 圖4為基于四核四軸稀土永磁無刷直流伺服電機(jī)SCARA機(jī)器人控制器原理圖； 圖5為基于四核四軸稀土永磁無刷直流伺服電機(jī)SCARA機(jī)器人程序框圖； 圖6為基于四核四軸稀土永磁無刷直流伺服電機(jī)運動原理圖； 圖7為四軸電機(jī)加減速曲線圖； 圖8為本發(fā)明的輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)結(jié)構(gòu)示意圖。
[0016] 附圖中各部件的標(biāo)記如下：1、主站控制計算機(jī)，2、控制器，3、DSP芯片控制器，4、 ARM控制器，5、主電源，6、備用電源，7、稀土永磁無刷直流伺服電機(jī)X，8、稀土永磁無刷直流 伺服電機(jī)Y，9、稀土永磁無刷直流伺服電機(jī)Z，10、稀土永磁無刷直流伺服電機(jī)R，11、光電編 碼器，12、避障位移傳感器S1，13、避障位移傳感器S2,14、避障位移傳感器S3,15、避障位移 傳感器S4，16、定位傳感器S5，17、加速度傳感器Al，18、加速度傳感器A2，19、加速度傳感器 A3，20、PLC控制器，21、磁電傳感器EM1，22、磁電傳感器EM2，23、磁電傳感器EM3，24、磁電傳 感器EM4,25、集成芯片處理器。
【具體實施方式】
[0017] 下面將對本發(fā)明實施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實施 例僅是本發(fā)明的一部分實施例，而不是全部的實施例?；诒景l(fā)明中的實施例，本領(lǐng)域普通 技術(shù)人員在沒有做出創(chuàng)造性勞動前提下所獲得的所有其它實施例，都屬于本發(fā)明保護(hù)的范 圍。
[0018] 請參閱圖2至圖8,本發(fā)明實施例包括:一種輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)，該 機(jī)器人為基于四軸稀土永磁無刷直流伺服電機(jī)的SCARA機(jī)器人，即所述關(guān)節(jié)機(jī)器人采用稀 土永磁無刷直流伺服電機(jī)X7驅(qū)動機(jī)器人大臂旋轉(zhuǎn)運動、采用稀土永磁無刷直流伺服電機(jī)Y8 驅(qū)動機(jī)器人小臂旋轉(zhuǎn)運動、采用稀土永磁無刷直流伺服電機(jī)Z9驅(qū)動機(jī)器人手腕旋轉(zhuǎn)運動、 采用稀土永磁無刷直流伺服電機(jī)R10驅(qū)動機(jī)器人手腕升降運動，所述機(jī)器人大臂上安裝有 避障位移傳感器S1 12、避障位移傳感器S2 13和加速度傳感器A1 17,所述機(jī)器人小臂上安 裝有避障位移傳感器S3 14、避障位移傳感器S4 15和加速度傳感器A2 18,所述機(jī)器人手腕 上安裝有定位傳感器S5 16和加速度傳感器A3 19。
[0019] 所述的控制系統(tǒng)包括主站控制計算機(jī)1和控制器2,所述控制器2包括DSP芯片控制 器3、集成芯片處理器25、P1C控制器20和ARM控制器4,所述DSP芯片控制器3、P1C控制器20和 集成芯片處理器25均與所述主站控制計算機(jī)1通信連接，所述DSP芯片控制器3、集成芯片處 理器25和ARM控制器4彼此通信連接，所述P1C控制器20與ARM控制器4通信連接。所述稀土永 磁無刷直流伺服電機(jī)X7、稀土永磁無刷直流伺服電機(jī)Y8、稀土永磁無刷直流伺服電機(jī)Z9和 稀土永磁無刷直流伺服電機(jī)R10均與所述DSP芯片控制器3通信連接，所述避障位移傳感器 S1 12、避障位移傳感器S2 13、避障位移傳感器S3 14、避障位移傳感器S4 15、定位傳感器 S5 16、加速度傳感器A1 17、加速度傳感器A2 18和加速度傳感器A3 19均同時與所述DSPS 片控制器3、集成芯片處理器25、PLC控制器20和ARM控制器4通信連接。
[0020] 優(yōu)選地，本發(fā)明的輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)還包括為所述關(guān)節(jié)機(jī)器人和 控制系統(tǒng)提供電源的主電源5和備用電源6,所述關(guān)節(jié)機(jī)器人中的各個所述電機(jī)和傳感器以 及控制系統(tǒng)中的控制器2均與所述主電源5和/或備用電源6電性連接。
[0021] 優(yōu)選地，所述稀土永磁無刷直流伺服電機(jī)X 7、稀土永磁無刷直流伺服電機(jī)Y 8、稀 土永磁無刷直流伺服電機(jī)Z 9和稀土永磁無刷直流伺服電機(jī)R10上勻設(shè)置有光電編碼器11， 所述光電編碼器11與所述DSP芯片控制器3電性連接。
[0022]優(yōu)選地，所述機(jī)器人大臂上安裝有磁電傳感器EM1 21，所述機(jī)器人小臂上安裝有 磁電傳感器EM2 22,所述機(jī)器人手腕上安裝有磁電傳感器EM3 23和EM4 24,所述磁電傳感 器EM1 21、磁電傳感器EM2 22、磁電傳感器EM3 23和EM4 24均與所述DSP芯片控制器3和ARM 控制器4通信連接。這些磁電傳感器分別讀取各自的零位設(shè)置標(biāo)志，當(dāng)四者均探測到信號 時，SCARA機(jī)器人實現(xiàn)精確復(fù)位，提高了復(fù)位的精確度。
[0023] 本發(fā)明采用DSP控制器(TMS320F2812H集成芯片處理器（即LM628芯片，一種美國 國家半導(dǎo)體公司生產(chǎn)的大規(guī)模集成芯片)+PLC控制器20+ ARM控制器(STM32F746)四核進(jìn)行 系統(tǒng)控制。本發(fā)明的控制器原理圖如圖4:控制板以DSP(TMS320F28335)和LM628為處理器核 心，ARM( STM32F746)實現(xiàn)貨物搬運從一點到另外一點的大臂、小臂、手腕旋轉(zhuǎn)角度和手腕上 升高度的計算，并通過PLC控制器實時響應(yīng)總站人為搬運貨物的中斷，并與DSP (TMS320F28335)通訊，把05?(了]\^32(^28335)從復(fù)雜的工作當(dāng)中解脫出來，05? (TMS320F28335)配合LM628實現(xiàn)四軸稀土永磁無刷直流伺服電機(jī)的實時控制，并響應(yīng)ARM中 斷，實現(xiàn)數(shù)據(jù)通信和存儲實時信號。
[0024]本系統(tǒng)用效率高、功率密度高、轉(zhuǎn)動慣量較小、啟動轉(zhuǎn)矩大的稀土永磁無刷直流伺 服電機(jī)替代了步進(jìn)電機(jī)和普通直流電機(jī);為了提高運算速度，保證SCARA機(jī)器人系統(tǒng)的穩(wěn)定 性和可靠性，本發(fā)明在基于DSP的控制器中引入精密運動控制器LM628、嵌入式控制器ARM、 可編程邏輯控制器PLC和多種傳感器，形成基于DSP+LM628+ARM+PLC的全新四核控制器，此 控制器充分考慮電源在這個系統(tǒng)的作用，雙電源的使用保證了能源的供給；四核系統(tǒng)把工 作量最大的四軸稀土永磁無刷直流伺服電機(jī)控制系統(tǒng)交給LM628處理，充分發(fā)揮LM628作為 電機(jī)伺服控制處理器的作用，而位置讀取、路徑規(guī)劃、信息存儲、電源監(jiān)控與切換等交給DSP 處理，總站通過PLC實時向ARM輸入位置數(shù)據(jù)，基于Denavit-Hartenberg算法的大臂、小臂和 手腕旋轉(zhuǎn)角度以及手腕上升高度計算、數(shù)據(jù)存儲等功能交給ARM完成，充分發(fā)揮ARM數(shù)據(jù)處 理速度相對較快的特點，這樣就實現(xiàn)了ARM、DSP、LM628和PLC的分工，同時ARM通過RS485與 PLC通訊，與主站進(jìn)行數(shù)據(jù)交換和調(diào)用。
[0025] 在電源打開狀態(tài)下，ARM控制器先對機(jī)器人備用電源S0C(荷電狀態(tài))和主電源進(jìn)行 判斷，如果備用能源較低，控制器將會通過PLC向總站發(fā)出報警信號；如果備用電源和主電 源工作正常，先由總站通過PLC控制器把搬運貨物大臂、小臂和手腕旋轉(zhuǎn)角度以及升降高度 或者是搬用貨物的各個坐標(biāo)信息輸入給ARM，然后由ARM計算出機(jī)器人伺服系統(tǒng)的參數(shù)； SCARA機(jī)器人攜帶的零位置傳感器EM1、EM2、EM3和EM4開始工作，使機(jī)器人復(fù)位到設(shè)定零位 置，機(jī)器人進(jìn)入自鎖狀態(tài)；一旦搬運命令開始后，機(jī)器人攜帶的避障傳感器、定位傳感器和 加速度傳感器均開啟，SCARA機(jī)器人按照設(shè)定ARM優(yōu)化的搬運路徑快速搬運，DSP與LM628通 訊，LM628按照伺服參數(shù)和傳感器反饋實時調(diào)整SCARA機(jī)器人稀土永磁無刷直流伺服電機(jī)X、 稀土永磁無刷直流伺服電機(jī)Y、稀土永磁無刷直流伺服電機(jī)Z和稀土永磁無刷直流伺服電機(jī) R的PWM輸出，實現(xiàn)四臺稀土永磁無刷直流伺服電機(jī)的實時伺服控制，DSP實時采集機(jī)器人運 動信息并存儲貨物位置信息；如果ARM對搬運某一個位置有疑問，將與DSP通訊，DSP通過 LM628使SCARA機(jī)器人停車，然后人工對DSP存儲信息進(jìn)行判斷，確認(rèn)無誤后總站通過PLC控 制器二次人工啟動SCARA機(jī)器人繼續(xù)未完成的任務(wù)。
[0026] 參照圖1，圖2,圖3和圖4、圖5、圖6、圖7,其具體的功能實現(xiàn)如下： USCARA機(jī)器人電源打開后，ARM會對備用電源S0C和主電源進(jìn)行判斷，如果備用電源 S0C較低時，DSP將禁止控制四臺稀土永磁無刷直流伺服電機(jī)的LM628工作，電機(jī)輸入PWM波 控制信號被封鎖，同時報警傳感器將工作通過PLC控制器向總站并發(fā)出報警信號;如果電池 S0C正常，SCARA機(jī)器人進(jìn)入待工作狀態(tài)，等待工作命令。
[0027] 2)-旦總站工作命令開始，PLC開始與ARM通訊，PLC控制器把大臂、小臂長度和升 降桿的長度等信息通過RS484傳入到ARM控制器，然后機(jī)器人開始校正零位置，SCARA機(jī)器人 攜帶的磁電傳感器EMI、EM2、EM3和EM4開始工作，并各自尋找設(shè)定的零位置標(biāo)志，當(dāng)磁電傳 感器EM1、EM2、EM3和EM4均有信號輸出時，DSP通過LM628封鎖四路稀土永磁無刷直流伺服電 機(jī)的HVM波控制信號，SCARA機(jī)器人自動引導(dǎo)零位置復(fù)位完成，，此時ARM控制器設(shè)定各個旋 轉(zhuǎn)角度爲(wèi)=為=A.=見手腕上升高度
[0028] 3 )為了滿足SCARA機(jī)器人的加減速運動需要，本發(fā)明采用如圖7的運動時間梯形 圖，此梯形圖包含的面積就是機(jī)器人大臂、小臂和手腕要旋轉(zhuǎn)的角度或者是手腕升降的高 度，為了方便控制，本發(fā)明采用單一加速度模式。
[0029] 4)SCARA機(jī)器人讀取其工作模式如果是人工工作模式，主站開始通過PLC與ARM控 制器通訊，由主站輸入SCARA機(jī)器人大臂、小臂和手腕需要旋轉(zhuǎn)的角度咚，巧，巧以及SCARA 機(jī)器人手腕需要上升或者是下降的高度禹給ARM控制器，ARM控制器根據(jù)Denavit-Hartenberg算法開始機(jī)器人位置和姿態(tài)正向求解:ARM控制器首先根據(jù)SCARA機(jī)器人大臂、 小臂和手腕需要旋轉(zhuǎn)的角度咚，各，咚以及SCARA機(jī)器人手腕需要上升的高度計算出相鄰 兩桿件坐標(biāo)系之間的位姿矩陣，Ms，-5*，并用各自用4*4二維數(shù)組標(biāo)志，， 分別表不如下：
[0030] ARM控制器然后通過公式就可以求出手腕執(zhí)行器完成任務(wù) 后的位置和姿態(tài)，并與DSP通訊，傳輸人工輸入?yún)?shù)給DSP。
[0031] 5) SCARA機(jī)器人讀取工作模式如果是正常自動搬運工作模式，主站開始通過PLC 與ARM控制器通訊，由主站輸入SCARA機(jī)器人所處的初始位置和給定位置三維坐標(biāo)給ARM控 制器，ARM控制器根據(jù)Denavi t-Hartenberg算法開始機(jī)器人逆向求解:ARM控制器首先根據(jù) 大臂、小臂長度以及最終的三維坐標(biāo)中的X和Y坐標(biāo)求出大臂需要旋轉(zhuǎn)的角度咚，并借助今 的值求出巧的值，并根據(jù)三維坐標(biāo)中的Z坐標(biāo)求出手腕上升或降低的高度最終求出旋轉(zhuǎn) 角度硌，由于在求解珥的時候方程具有雙解，所以SCARA機(jī)器人求出大臂、小臂和手腕需要 旋轉(zhuǎn)的角度咚，咚，巧以及SCARA機(jī)器人手腕需要上升的高度名后，ARM控制器會對求解結(jié)果 進(jìn)行優(yōu)化，然后ARM控制器與DSP通訊，把機(jī)器人最有伺服運動參數(shù)傳輸給DSP控制器。
[0032] 6)DSP控制器接受SCARA機(jī)器人大臂、小臂和手腕需要旋轉(zhuǎn)的角度色，巧，私以及 SCARA機(jī)器人手腕需要上升的高度后，大臂、小臂和手腕上的傳感器S1~S5、EM1、EM2和EM3 將開啟，首先SCARA機(jī)器人要對零位置進(jìn)行判斷，確認(rèn)初始位置無誤后，SCARA機(jī)器人對各個 旋轉(zhuǎn)臂旋轉(zhuǎn)角度內(nèi)的障礙物進(jìn)行判斷，如存在障礙物將向DSP發(fā)出中斷請求，DSP會對中斷 做第一時間響應(yīng)，然后DSP禁止四軸LM628工作，四軸稀土永磁無刷直流伺服電機(jī)PWM波控制 信號被封鎖，SCARA機(jī)器人禁止稀土永磁無刷直流伺服電機(jī)X、稀土永磁無刷直流伺服電機(jī) Y、稀土永磁無刷直流伺服電機(jī)Z和電機(jī)稀土永磁無刷直流伺服電機(jī)R工作，機(jī)器人自鎖在原 地，DSP控制器二次判斷運動范圍內(nèi)的障礙物信息，防止信息誤判。
[0033] 7)如果DSP控制器確定無障礙物進(jìn)入運動區(qū)域，DSP控制器按照圖7的速度時間曲 線對運動位置進(jìn)行分解，DSP首先把三個旋轉(zhuǎn)角度咚，咚，咚轉(zhuǎn)化為三臺稀土永磁無刷直流 伺服電機(jī)的加速度、速度和位置初始指令值，然后DSP與LM628通訊，LM628結(jié)合電機(jī)X、電機(jī)Y 和電機(jī)Z的光電編碼器反饋，經(jīng)LM628內(nèi)部伺服調(diào)節(jié)程序生成電機(jī)X、電機(jī)Y和電機(jī)Z的PWM波 控制信號，PWM波控制信號經(jīng)驅(qū)動橋放大后驅(qū)動各個電機(jī)運動。DSP控制器根據(jù)輸入偏差大 小實時調(diào)整LM628內(nèi)部伺服控制程序的PID參數(shù)，LM628控制器通過調(diào)整稀土永磁無刷直流 伺服電機(jī)的驅(qū)動脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度，通過調(diào)整稀土永磁無刷直流伺服電機(jī)控制信號 的頻率實現(xiàn)角速度快慢的變化，使三軸稀土永磁無刷直流伺服電機(jī)時刻同步工作，DSP時刻 記錄機(jī)器人已經(jīng)運動的位置參數(shù)。
[0034] 8)在SCARA機(jī)器人移動過程中，傳感器S1~S4時刻對運動范圍內(nèi)的移動障礙物進(jìn)行 判斷，如果有障礙物進(jìn)入運動范圍，DSP立即通過LM628使SCARA機(jī)器人的大臂、小臂和手腕 按照圖7的減速運動曲線立即停車，DSP控制器記錄下當(dāng)前旋轉(zhuǎn)角度涔，垮，咚信息。等障礙 物消失后，重新計算對新位置的旋轉(zhuǎn)角度成，劣，劣，然后DSP控制LM628按照圖7的運動曲線 重新經(jīng)過三段運動軌跡:加速運動、勾速運動和減速運動，最終到達(dá)設(shè)定點。
[0035] 9)在移動過程中，DSP時刻記錄加速度計反饋的大臂、小臂和手腕旋轉(zhuǎn)的角加速 度，控制器通過二次積分得到大臂、小臂和手腕的旋轉(zhuǎn)角度，并與設(shè)定的位置角度值相比 較，如果偏差大于設(shè)定閥值，DSP把這個偏差轉(zhuǎn)化為三臺稀土永磁無刷直流伺服電機(jī)新的 加速度、速度和位置初始指令值，在下一個采樣周期，DSP與LM628通訊，LM628結(jié)合電機(jī)X、電 機(jī)Y和電機(jī)Z電機(jī)光電編碼器反饋的反饋，經(jīng)LM628內(nèi)部伺服調(diào)節(jié)程序生成電機(jī)X、電機(jī)Y和電 機(jī)Z的PWM波控制信號，PWM波控制信號經(jīng)驅(qū)動橋放大后驅(qū)動各個電機(jī)運動。DSP控制器根據(jù) 輸入偏差大小實時調(diào)整LM628內(nèi)部伺服控制程序的PID參數(shù)，LM628控制器通過調(diào)整稀土永 磁無刷直流伺服電機(jī)的驅(qū)動脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度，通過調(diào)整稀土永磁無刷直流伺服電 機(jī)控制信號的頻率實現(xiàn)角速度快慢的變化，進(jìn)而消除上一個采樣周期產(chǎn)生的誤差，使SCARA 機(jī)器人按照設(shè)定軌跡完成任務(wù)。
[0036] 10)在SCARA機(jī)器人運動過程中，DSP會時刻儲存所經(jīng)過的SCARA機(jī)器人所處的位置 或者是經(jīng)過的參考點，并根據(jù)這些距離信息由DSP計算得到相對下一個參考點SCARA機(jī)器人 稀土永磁無刷直流伺服電機(jī)X、稀土永磁無刷直流伺服電機(jī)Y、稀土永磁無刷直流伺服電機(jī)Z 分別要運行的角度、角度速度和角加速度，LM628結(jié)合電機(jī)X、電機(jī)Y和電機(jī)Z電機(jī)光電編碼器 反饋的反饋，經(jīng)內(nèi)部伺服調(diào)節(jié)程序生成電機(jī)X、電機(jī)Y和電機(jī)2的1^麗波控制信號，P麗波控制 信號經(jīng)驅(qū)動橋放大后驅(qū)動各個電機(jī)運動。DSP控制器根據(jù)輸入偏差大小實時調(diào)整LM628內(nèi)部 伺服控制程序的PID參數(shù)，通過調(diào)整稀土永磁無刷直流伺服電機(jī)的驅(qū)動脈沖數(shù)目調(diào)整其旋 轉(zhuǎn)角度，通過調(diào)整稀土永磁無刷直流伺服電機(jī)控制信號的頻率實現(xiàn)角速度快慢的變化，使 SCARA機(jī)器人按照設(shè)定速度快速前行。
[0037] 11)在SCARA機(jī)器人完成三軸旋轉(zhuǎn)角度咚，咚，芎的伺服控制后，DSP二次檢測加速 度傳感器的積分值，如果發(fā)現(xiàn)SCARA機(jī)器人在運動過程受到外界干擾后三個旋轉(zhuǎn)角度巧，咚 ，負(fù)與設(shè)定角度的誤差超過設(shè)定閥值時，DSP把三個旋轉(zhuǎn)角度偏差A(yù)巧，A咚3磉轉(zhuǎn)化為三臺 稀土永磁無刷直流伺服電機(jī)位置微調(diào)的加速度、速度和位置初始指令值，然后DSP與LM628 通訊，LM628結(jié)合電機(jī)X、電機(jī)Y和電機(jī)Z電機(jī)光電編碼器反饋的反饋，經(jīng)LM628內(nèi)部伺服調(diào)節(jié) 程序生成電機(jī)X、電機(jī)Y和電機(jī)Z的PWM波控制信號，PWM波控制信號經(jīng)驅(qū)動橋放大后驅(qū)動各個 電機(jī)運動。DSP控制器根據(jù)輸入偏差大小實時調(diào)整LM628內(nèi)部伺服控制程序的PID參數(shù)， LM628控制器通過調(diào)整稀土永磁無刷直流伺服電機(jī)的驅(qū)動脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度，通過 調(diào)整稀土永磁無刷直流伺服電機(jī)控制信號的頻率實現(xiàn)角速度快慢的變化，通過三軸稀土永 磁無刷直流伺服電機(jī)的再次工作使機(jī)器人大臂、小臂和手腕到達(dá)設(shè)定位置。
[0038] 12)當(dāng)SCARA機(jī)器人的大臂、小臂和手腕完成角度巧，龜，私的角度補(bǔ)償?shù)竭_(dá)設(shè)定位 置后，手腕上的傳感器EM4將再次開啟，首先SCARA機(jī)器人要對零位置進(jìn)行判斷，確認(rèn)初始位 置無誤后，DSP按照圖7的速度時間曲線，把手腕要升降的距離溝轉(zhuǎn)化為稀土永磁無刷直流 伺服電機(jī)R的加速度、速度和位置初始指令值，然后DSP與LM628通訊，LM628結(jié)合電機(jī)R光電 編碼器的反饋，經(jīng)內(nèi)部伺服調(diào)節(jié)程序生成電機(jī)R的PWM波控制信號，PWM波控制信號經(jīng)驅(qū)動橋 放大后驅(qū)動各個電機(jī)運動。DSP控制器根據(jù)輸入偏差大小實時調(diào)整LM628內(nèi)部伺服控制程序 的PID參數(shù)，DSP控制器通過調(diào)整稀土永磁無刷直流伺服電機(jī)的驅(qū)動脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角 度，通過調(diào)整稀土永磁無刷直流伺服電機(jī)控制信號的頻率實現(xiàn)角速度快慢的變化，使手腕 平穩(wěn)在設(shè)定時間內(nèi)到達(dá)設(shè)定位置。
[0039] 13)在運動過程中如果SCARA機(jī)器人發(fā)現(xiàn)位置參數(shù)求解出現(xiàn)死循環(huán)將向ARM發(fā)出中 斷請求，ARM會對中斷做第一時間響應(yīng)，ARM控制器將立即與DSP通訊，DSP立即通過LM628封 鎖四臺稀土永磁無刷直流伺服電機(jī)的控制信號，機(jī)器人原地自鎖，然后由主站根據(jù)儲存信 息進(jìn)行分析，分析完畢后，主站通過PLC向ARM控制器輸入新的工作位置信息。
[0040] 14)在SCARA機(jī)器人多次反復(fù)運動過程中，如果主站發(fā)現(xiàn)有臨時重要任務(wù)加入工作 隊列中，主站將開啟PLC控制器，由主站通過PLC控制器與ARM通訊，PLC通過RS485向ARM輸入 重要任務(wù)的位置伺服參數(shù)，ARM與DSP通訊，首先由DSP控制LM628完成當(dāng)前未完成的任務(wù)，同 時ARM更新DSP原有的伺服運動控制參數(shù)隊列，并傳輸新的伺服位置參數(shù)給DSP，使DSP控制 LM628完成插入隊列中的重要臨時任務(wù)。
[0041 ] 15)在SCARA機(jī)器人多次反復(fù)運動過程中，如果當(dāng)磁電傳感器EM1、EM2、EM3和EM4讀 到傳感信號時，DSP控制器將以零位置參數(shù)代替現(xiàn)有的位置參數(shù)進(jìn)行新的位置伺服控制，并 計算出誤差，在下一個采樣周期，DSP通過LM628內(nèi)部三閉環(huán)稀土永磁無刷直流伺服電機(jī)控 制程序?qū)φ`差進(jìn)行補(bǔ)償，及時消除累計誤差。
[0042] 16)裝在稀土永磁無刷直流伺服電機(jī)X、稀土永磁無刷直流伺服電機(jī)Y、稀土永磁無 刷直流伺服電機(jī)Z、稀土永磁無刷直流伺服電機(jī)R上的光電編碼器會輸出其位置信號A和位 置信號B，光電編碼器的位置信號A脈沖和B脈沖邏輯狀態(tài)每變化一次，DSP和ARM內(nèi)的位置寄 存器會根據(jù)電機(jī)的運行方向加1或者是減1;光電編碼器的位置信號A脈沖和B脈沖和Z脈沖 同時為低電平時，就產(chǎn)生一個INDEX信號給DSP和ARM內(nèi)部寄存器，記錄稀土永磁無刷直流伺 服電機(jī)的絕對位置，然后換算成SCARA機(jī)器人大臂、小臂或者是手腕在三維坐標(biāo)系統(tǒng)中的具 體位置，ARM控制器實時通過PLC與主站通訊，把重要的位置參數(shù)傳輸給主站。當(dāng)磁電傳感器 EMI、EM2和EM3讀到傳感信號時，DSP控制器將以零位置參數(shù)代替現(xiàn)有的位置參數(shù)，及時消除 累計誤差。
[0043] 17)SCARA機(jī)器人在運行過程ARM控制器時刻對AC交流主電源進(jìn)行監(jiān)控，如果控制 器發(fā)現(xiàn)主電源出現(xiàn)故障突然斷電時，ARM會與DSP通訊，并開通備用電源，由備用電源為四軸 稀土永磁無刷直流伺服電機(jī)提供能量，DSP通過LM628內(nèi)部伺服調(diào)節(jié)程序?qū)崟r調(diào)整四臺稀土 永磁無刷直流伺服電機(jī)的PWM輸出，LM628控制器通過調(diào)整稀土永磁無刷直流伺服電機(jī)的驅(qū) 動脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度，通過調(diào)整稀土永磁無刷直流伺服電機(jī)控制信號的頻率實現(xiàn)角 速度快慢的變化，使SCARA機(jī)器人完成這次搬運任務(wù)，然后SCARA機(jī)器人通過PLC與主站進(jìn)行 通訊，并通知主站進(jìn)行檢修。
[0044] 18)在SCARA機(jī)器人多軸伺服系統(tǒng)工作過程中，如果DSP伺服控制器檢測到某一個 稀土永磁無刷直流伺服電機(jī)的轉(zhuǎn)矩出現(xiàn)脈動，由于本發(fā)明采用的稀土永磁無刷直流伺服電 機(jī)力矩與電機(jī)電流成正比例，因此控制器會很容易補(bǔ)償此干擾，并根據(jù)干擾大小DSP實時調(diào) 整LM628內(nèi)部的PID參數(shù)，減少了電機(jī)轉(zhuǎn)矩擾動對SCARA機(jī)器人運動過程的影響。
[0045] 19)當(dāng)SCARA機(jī)器人完成任務(wù)，實現(xiàn)位置歸零時，其攜帶的加速度傳感器A1~A3、磁 電傳感器EMI、EM2、EM3和EM4會時刻工作，先有加速度傳感器檢測其加速度，當(dāng)加速度超過 預(yù)設(shè)閥值時，在下一個周期DSP控制器會通過LM628修正上一個周期帶來的誤差，當(dāng)磁電傳 感器EM1、￡112、￡1〇、￡114均有信號輸出時，03?通過111628封鎖四路稀土永磁無刷直流伺服電 機(jī)的PWM波控制信號，SCARA機(jī)器人自動引導(dǎo)零位置復(fù)位完成，SCARA機(jī)器人按照圖7的速度-時間運動曲線實現(xiàn)從某一點到零位的歸位，然后零位自鎖，等待下一批工作命令。
[0046] 以上所述僅為本發(fā)明的實施例，并非因此限制本發(fā)明的專利范圍，凡是利用本發(fā) 明說明書內(nèi)容所作的等效結(jié)構(gòu)或等效流程變換，或直接或間接運用在其它相關(guān)的技術(shù)領(lǐng) 域，均同理包括在本發(fā)明的專利保護(hù)范圍內(nèi)。
【主權(quán)項】
1. 一種輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)，其特征在于，所述關(guān)節(jié)機(jī)器人采用稀土永 磁無刷直流伺服電機(jī)X驅(qū)動機(jī)器人大臂旋轉(zhuǎn)運動、采用稀土永磁無刷直流伺服電機(jī)Y驅(qū)動機(jī) 器人小臂旋轉(zhuǎn)運動、采用稀土永磁無刷直流伺服電機(jī)Z驅(qū)動機(jī)器人手腕旋轉(zhuǎn)運動、采用稀土 永磁無刷直流伺服電機(jī)R驅(qū)動機(jī)器人手腕升降運動，所述機(jī)器人大臂上安裝有避障位移傳 感器S1、避障位移傳感器S2和加速度傳感器A1，所述機(jī)器人小臂上安裝有避障位移傳感器 S3、避障位移傳感器S4和加速度傳感器A2,所述機(jī)器人手腕上安裝有定位傳感器S5和加速 度傳感器A3,所述的控制系統(tǒng)包括主站控制計算機(jī)和控制器，所述控制器包括DSP芯片控制 器、集成芯片處理器、P1C控制器和ARM控制器，所述DSP芯片控制器、P1C控制器和集成芯片 處理器均與所述主站控制計算機(jī)通信連接，所述DSP芯片控制器、集成芯片處理器和ARM控 制器彼此通信連接，所述P1C控制器與ARM控制器通信連接，所述稀土永磁無刷直流伺服電 機(jī)X、稀土永磁無刷直流伺服電機(jī)Y、稀土永磁無刷直流伺服電機(jī)Z和稀土永磁無刷直流伺服 電機(jī)R均與所述DSP芯片控制器通信連接，所述避障位移傳感器S1、避障位移傳感器S2、避障 位移傳感器S3、避障位移傳感器S4、定位傳感器S5、加速度傳感器A1、加速度傳感器A2和加 速度傳感器A3均同時與所述DSP芯片控制器和ARM控制器通信連接。2. 根據(jù)權(quán)利要求1所述的輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)，其特征在于，還包括為所 述關(guān)節(jié)機(jī)器人和控制系統(tǒng)提供電源的主電源和備用電源，所述關(guān)節(jié)機(jī)器人中的各個所述電 機(jī)和傳感器以及控制系統(tǒng)中的控制器均與所述主電源和/或備用電源電性連接。3. 根據(jù)權(quán)利要求1所述的輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)，其特征在于，所述稀土永 磁無刷直流伺服電機(jī)X、稀土永磁無刷直流伺服電機(jī)Y、稀土永磁無刷直流伺服電機(jī)Z和稀土 永磁無刷直流伺服電機(jī)R上勻設(shè)置有光電編碼器，所述光電編碼器與所述DSP芯片控制器電 性連接。4. 根據(jù)權(quán)利要求1至3任一項所述的輕載四核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)，其特征在于， 所述機(jī)器人大臂上安裝有磁電傳感器EM1，所述機(jī)器人小臂上安裝有磁電傳感器EM2,所述 機(jī)器人手腕上安裝有磁電傳感器EM3和EM4,所述磁電傳感器EM1、磁電傳感器EM2、磁電傳感 器EM3和EM4均與所述DSP芯片控制器和ARM控制器通信連接。
【文檔編號】B25J9/18GK106003062SQ201610413567
【公開日】2016年10月12日
【申請日】2016年6月14日
【發(fā)明人】張好明
【申請人】江蘇若博機(jī)器人科技有限公司