一種教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng)的制作方法
【專利摘要】本發(fā)明公開(kāi)了一種教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng),所述的控制系統(tǒng)包括主站控制計(jì)算機(jī)和控制器,所述控制器包括DSP芯片控制器和ARM控制器,DSP芯片控制器和ARM控制器均與所述主站控制計(jì)算機(jī)通信連接,DSP芯片控制器與ARM控制器之間通信連接,四臺(tái)永磁直流伺服電機(jī)均與所述DSP芯片控制器通信連接,多個(gè)避障位移傳感器、定位傳感器S5、加速度傳感器A1、加速度傳感器A2和加速度傳感器A3均與所述DSP芯片控制器和ARM控制器通信連接。本發(fā)明的教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng)計(jì)算速度快,使關(guān)節(jié)機(jī)器人手臂轉(zhuǎn)向靈活、動(dòng)作穩(wěn)定精確、體積小巧,性能穩(wěn)定。
【專利說(shuō)明】
一種教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng)
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種教學(xué)用雙核常速關(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ù)率較 高的長(zhǎng)時(shí)間作業(yè),或是在危險(xiǎn)、惡劣環(huán)境下的作業(yè),一般用作搬取零件和裝配工作,在微電 子制造業(yè)、塑料工業(yè)、汽車工業(yè)、電子產(chǎn)品工業(yè)、藥品工業(yè)和食品工業(yè)等領(lǐng)域得到廣泛的應(yīng) 用,它對(duì)于提高生產(chǎn)自動(dòng)化水平、勞動(dòng)生產(chǎn)率和經(jīng)濟(jì)效益、保證產(chǎn)品質(zhì)量、保障人身安全、改 善勞動(dòng)環(huán)境,減輕勞動(dòng)強(qiáng)度、節(jié)約原材料消耗以及降低生產(chǎn)成本等有著十分重要的意義。 [0003] SCARA工業(yè)機(jī)器人即裝配作業(yè)的機(jī)器人手臂是一種圓柱坐標(biāo)型的工業(yè)機(jī)器人,它 依靠旋轉(zhuǎn)關(guān)節(jié)大臂和小臂來(lái)實(shí)現(xiàn)X-Y平面內(nèi)的快速定位,依靠一個(gè)手腕移動(dòng)關(guān)節(jié)和一個(gè)手 腕旋轉(zhuǎn)關(guān)節(jié)在Z方向上做伸縮和旋轉(zhuǎn)運(yùn)動(dòng),其具有四個(gè)運(yùn)動(dòng)自由度,該系列的操作手在其動(dòng) 作空間的四個(gè)方向具有有限剛度,而在剩下的其余兩個(gè)方向上具有無(wú)限大剛度。這種結(jié)構(gòu) 特性使得SCARA機(jī)器人擅長(zhǎng)從一點(diǎn)抓取物體,然后快速的安放到另一點(diǎn),因此SCARA機(jī)器人 在自動(dòng)裝配生產(chǎn)線上得到了廣泛的應(yīng)用。SCARA機(jī)器人結(jié)構(gòu)緊湊、動(dòng)作靈活,速度快、位置精 度高,它的使用大大提高了機(jī)器人對(duì)復(fù)雜裝配任務(wù)的適應(yīng)性,同時(shí)也降低了成本,提高了工 作空間利用率。
[0004] SCARA機(jī)器人結(jié)合了多學(xué)科知識(shí),對(duì)于提升在校學(xué)生的動(dòng)手能力、團(tuán)隊(duì)協(xié)作能力和 創(chuàng)新能力,促進(jìn)學(xué)生課堂知識(shí)的消化和擴(kuò)展學(xué)生的知識(shí)面都非常有幫助。教學(xué)用SCARA機(jī)器 人技術(shù)的開(kāi)展可以培養(yǎng)大批相關(guān)領(lǐng)域的人才,進(jìn)而促進(jìn)相關(guān)領(lǐng)域的技術(shù)發(fā)展和產(chǎn)業(yè)化進(jìn) 程。但是SCARA機(jī)器人在搬運(yùn)貨物過(guò)程中要時(shí)刻判斷主控制器輸入的位置參數(shù),并判斷周圍 的環(huán)境時(shí)刻避障,然后由運(yùn)動(dòng)控制器反復(fù)控制其精確的加速和減速進(jìn)行搬運(yùn)貨物,稍微的 誤差累計(jì)就有可能在多回合運(yùn)動(dòng)中導(dǎo)致運(yùn)輸失敗。國(guó)內(nèi)對(duì)SCARA機(jī)器人的使用雖然有幾十 年,但是由于國(guó)內(nèi)工業(yè)機(jī)器人發(fā)展起步比較晚,受較多關(guān)鍵技術(shù)的影響,SCARA機(jī)器人的發(fā) 展也受所影響,傳統(tǒng)的機(jī)器人原理如圖1所示,在長(zhǎng)期使用期間出現(xiàn)眾多問(wèn)題: (1)作為SCARA機(jī)器人的電源采用的是一般交流電源整流后的直流電源,當(dāng)突然停電時(shí) 會(huì)使整個(gè)搬運(yùn)運(yùn)動(dòng)失敗。
[0005] (2)受控制技術(shù)影響,所有的SCARA機(jī)器人均采用單核控制器,控制器的計(jì)算能力 較弱,SCARA無(wú)法快速處理實(shí)時(shí)環(huán)境。
[0006] (3)對(duì)于采用步進(jìn)電機(jī)驅(qū)動(dòng)的SCARA機(jī)器人來(lái)說(shuō),使得系統(tǒng)運(yùn)轉(zhuǎn)的機(jī)械噪聲大大增 加,不利于環(huán)境保護(hù)。
[0007] (4)對(duì)于采用步進(jìn)電機(jī)和直流電機(jī)驅(qū)動(dòng)的SCARA機(jī)器人來(lái)說(shuō),系統(tǒng)的轉(zhuǎn)動(dòng)慣量大, 不利于機(jī)器人的快速加速和減速。
[0008] (5)對(duì)于采用步進(jìn)電機(jī)和直流電機(jī)驅(qū)動(dòng)的SCARA機(jī)器人來(lái)說(shuō),系統(tǒng)的啟動(dòng)力矩較 小,不利于機(jī)器人的快速啟動(dòng)。
[0009] (6)在SCARA機(jī)器人的整個(gè)運(yùn)動(dòng)控制過(guò)程,旋轉(zhuǎn)的各個(gè)角度沒(méi)有實(shí)時(shí)反饋,時(shí)有造 成三軸不同步的現(xiàn)象發(fā)生。
[0010] (7)在SCARA機(jī)器人的整個(gè)運(yùn)動(dòng)控制過(guò)程,各個(gè)旋轉(zhuǎn)臂與外圍環(huán)境之間沒(méi)有進(jìn)行有 效的避障,有時(shí)會(huì)造成碰撞其他貨物的現(xiàn)象發(fā)生。
【發(fā)明內(nèi)容】
[0011] 本發(fā)明主要解決的技術(shù)問(wèn)題是提供一種教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng),該 教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng)計(jì)算速度快,使關(guān)節(jié)機(jī)器人手臂轉(zhuǎn)向靈活、動(dòng)作穩(wěn)定 精確、體積小巧,性能穩(wěn)定。
[0012] 為解決上述技術(shù)問(wèn)題,本發(fā)明采用的技術(shù)方案是:提供一種教學(xué)用雙核常速關(guān)節(jié) 機(jī)器人控制系統(tǒng),所述關(guān)節(jié)機(jī)器人采用永磁直流伺服電機(jī)X驅(qū)動(dòng)機(jī)器人大臂旋轉(zhuǎn)運(yùn)動(dòng)、采用 永磁直流伺服電機(jī)Y驅(qū)動(dòng)機(jī)器人小臂旋轉(zhuǎn)運(yùn)動(dòng)、采用永磁直流伺服電機(jī)Z驅(qū)動(dòng)機(jī)器人手腕旋 轉(zhuǎn)運(yùn)動(dòng)、采用永磁直流伺服電機(jī)R驅(qū)動(dòng)機(jī)器人手腕升降運(yùn)動(dòng),所述機(jī)器人大臂上安裝有避障 位移傳感器S1、避障位移傳感器S2和加速度傳感器A1,所述機(jī)器人小臂上安裝有避障位移 傳感器S3、避障位移傳感器S4和加速度傳感器A2,所述機(jī)器人手腕上安裝有定位傳感器S5 和加速度傳感器A3,所述的控制系統(tǒng)包括主站控制計(jì)算機(jī)和控制器,所述控制器包括DSPS 片控制器和ARM控制器,所述DSP芯片控制器和ARM控制器均與所述主站控制計(jì)算機(jī)通信連 接,所述DSP芯片控制器與ARM控制器之間通信連接,所述永磁直流伺服電機(jī)X、永磁直流伺 服電機(jī)Y、永磁直流伺服電機(jī)Z和永磁直流伺服電機(jī)R均與所述DSP芯片控制器通信連接,所 述避障位移傳感器S1、避障位移傳感器S2、避障位移傳感器S3、避障位移傳感器S4、定位傳 感器S5、加速度傳感器A1、加速度傳感器A2和加速度傳感器A3均同時(shí)與所述DSP芯片控制器 和ARM控制器通信連接。
[0013] 在本發(fā)明一個(gè)較佳實(shí)施例中,還包括為所述關(guān)節(jié)機(jī)器人和控制系統(tǒng)提供電源的主 電源和備用電源,所述關(guān)節(jié)機(jī)器人中的各個(gè)所述電機(jī)和傳感器以及控制系統(tǒng)中的控制器均 與所述主電源和/或備用電源電性連接。
[0014] 在本發(fā)明一個(gè)較佳實(shí)施例中,所述永磁直流伺服電機(jī)X、永磁直流伺服電機(jī)Y、永磁 直流伺服電機(jī)Z和永磁直流伺服電機(jī)R上勻設(shè)置有光電編碼器,所述光電編碼器與所述DSP 芯片控制器電性連接。
[0015] 在本發(fā)明一個(gè)較佳實(shí)施例中,所述DSP芯片控制器內(nèi)設(shè)置有記錄永磁直流伺服電 機(jī)位置的位置寄存器。
[0016] 本發(fā)明的有益效果是:本發(fā)明的教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng)計(jì)算速度 快,使關(guān)節(jié)機(jī)器人手臂轉(zhuǎn)向靈活、動(dòng)作穩(wěn)定精確、體積小巧,性能穩(wěn)定。
【附圖說(shuō)明】
[0017] 為了更清楚地說(shuō)明本發(fā)明實(shí)施例中的技術(shù)方案,下面將對(duì)實(shí)施例描述中所需要使 用的附圖作簡(jiǎn)單地介紹,顯而易見(jiàn)地,下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例,對(duì)于 本領(lǐng)域普通技術(shù)人員來(lái)講,在不付出創(chuàng)造性勞動(dòng)的前提下,還可以根據(jù)這些附圖獲得其它 的附圖,其中: 圖1為傳統(tǒng)SCARA機(jī)器人控制器原理圖; 圖2為基于四軸永磁直流伺服電機(jī)SCARA機(jī)器人運(yùn)動(dòng)副二維圖; 圖3為基于雙核四軸永磁直流伺服電機(jī)SCARA機(jī)器人控制器原理圖; 圖4為基于雙核四軸永磁直流伺服電機(jī)SCARA機(jī)器人程序框圖; 圖5為基于雙核四軸永磁直流伺服電機(jī)運(yùn)動(dòng)原理圖; 圖6為四軸永磁直流伺服電機(jī)加減速曲線圖; 圖7為本發(fā)明的教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng)結(jié)構(gòu)示意圖。
[0018] 附圖中各部件的標(biāo)記如下:1、主站控制計(jì)算機(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。
【具體實(shí)施方式】
[0019] 下面將對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述,顯然,所描述的實(shí)施 例僅是本發(fā)明的一部分實(shí)施例,而不是全部的實(shí)施例?;诒景l(fā)明中的實(shí)施例,本領(lǐng)域普通 技術(shù)人員在沒(méi)有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其它實(shí)施例,都屬于本發(fā)明保護(hù)的范 圍。
[0020] 請(qǐng)參閱圖2至圖7,本發(fā)明實(shí)施例包括:一種教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng), 該機(jī)器人為基于四軸永磁直流伺服電機(jī)的SCARA機(jī)器人,即所述關(guān)節(jié)機(jī)器人采用永磁直流 伺服電機(jī)X7驅(qū)動(dòng)機(jī)器人大臂旋轉(zhuǎn)運(yùn)動(dòng)、采用永磁直流伺服電機(jī)Y8驅(qū)動(dòng)機(jī)器人小臂旋轉(zhuǎn)運(yùn) 動(dòng)、采用永磁直流伺服電機(jī)Z9驅(qū)動(dòng)機(jī)器人手腕旋轉(zhuǎn)運(yùn)動(dòng)、采用永磁直流伺服電機(jī)R10驅(qū)動(dòng)機(jī) 器人手腕升降運(yùn)動(dòng),所述機(jī)器人大臂上安裝有避障位移傳感器S1 12、避障位移傳感器S2 13和加速度傳感器A1 17,所述機(jī)器人小臂上安裝有避障位移傳感器S3 14、避障位移傳感 器S4 15和加速度傳感器A2 18,所述機(jī)器人手腕上安裝有定位傳感器S5 16和加速度傳感 器A3 19〇
[0021] 所述的控制系統(tǒng)包括主站控制計(jì)算機(jī)1和控制器2,所述控制器2包括DSP芯片控制 器3和ARM控制器4,所述DSP芯片控制器3和ARM控制器4均與所述主站控制計(jì)算機(jī)1通信連 接,所述DSP芯片控制器3與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均同時(shí)與所述DSP芯片控制器3和ARM控制器4通信連接。
[0022]優(yōu)選地,本發(fā)明的教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng)還包括為所述關(guān)節(jié)機(jī)器人 和控制系統(tǒng)提供電源的主電源5和備用電源6,所述關(guān)節(jié)機(jī)器人中的各個(gè)所述電機(jī)和傳感器 以及控制系統(tǒng)中的控制器2均與所述主電源5和/或備用電源6電性連接。
[0023] 優(yōu)選地,所述永磁直流伺服電機(jī)X 7、永磁直流伺服電機(jī)Y 8、永磁直流伺服電機(jī)Z 9和永磁直流伺服電機(jī)R10上勻設(shè)置有光電編碼器11,所述光電編碼器11與所述DSP芯片控 制器3電性連接。
[0024] 優(yōu)選地,所述DSP芯片控制器3內(nèi)設(shè)置有記錄永磁直流伺服電機(jī)位置的位置寄存 器。
[0025] 本發(fā)明的DSP(TMS320F2812)+ ARM(STM32F746)雙核控制器,在電源打開(kāi)狀態(tài)下, ARM控制器4先對(duì)機(jī)器人備用電源S0C(荷電狀態(tài))和主電源進(jìn)行判斷,如果備用能源較低,控 制器會(huì)發(fā)出報(bào)警信號(hào);如果備用電源和主電源工作正常,先由PC機(jī)通過(guò)USB接口把搬運(yùn)貨物 大臂、小臂和手腕旋轉(zhuǎn)角度以及升降高度或者是搬用貨物的各個(gè)坐標(biāo)信息輸入給ARM,然后 由ARM計(jì)算出機(jī)器人伺服系統(tǒng)的參數(shù);然后SCARA機(jī)器人恢復(fù)到零位置,機(jī)器人先進(jìn)入自鎖 狀態(tài),一旦搬運(yùn)命令開(kāi)始后,機(jī)器人攜帶的避障傳感器、定位傳感器和加速度傳感器均開(kāi) 啟,SCARA機(jī)器人按照設(shè)定ARM優(yōu)化的搬運(yùn)路徑快速搬運(yùn),DSP按照伺服參數(shù)和傳感器反饋實(shí) 時(shí)調(diào)整SCARA機(jī)器人永磁直流伺服電機(jī)X、永磁直流伺服電機(jī)Y、永磁直流伺服電機(jī)Z和永磁 直流伺服電機(jī)R的PWM輸出,實(shí)現(xiàn)四臺(tái)永磁直流伺服電機(jī)的實(shí)時(shí)伺服控制,ARM實(shí)時(shí)采集機(jī)器 人運(yùn)動(dòng)信息并存儲(chǔ)貨物位置信息,如果對(duì)搬運(yùn)某一個(gè)位置有疑問(wèn),將與DSP通訊,DSP發(fā)出停 車指令使SCARA機(jī)器人停車,然后人工對(duì)搬運(yùn)信息進(jìn)行判斷,確認(rèn)無(wú)誤后二次人工啟動(dòng) SCARA機(jī)器人繼續(xù)未完成的任務(wù)。
[0026] 參照?qǐng)D1,圖2,圖3和圖4、圖5、圖6,其具體的功能實(shí)現(xiàn)如下: USCARA機(jī)器人電源打開(kāi)后,ARM會(huì)對(duì)備用電源S0C和主電源進(jìn)行判斷,如果備用電源 S0C較低時(shí),DSP將禁止四臺(tái)永磁直流伺服電機(jī)工作,電機(jī)輸入PWM波被封鎖,同時(shí)報(bào)警傳感 器將工作并發(fā)出報(bào)警信號(hào);如果電池S0C正常,SCARA機(jī)器人進(jìn)入待工作狀態(tài),等待工作命 令。
[0027] 2)人工通過(guò)PC機(jī)把大臂、小臂長(zhǎng)度和升降桿的長(zhǎng)度等信息通過(guò)USB接口傳入到ARM 控制器,然后人工引導(dǎo)SCARA機(jī)器人到設(shè)定工作的零位置位,此時(shí)設(shè)定各個(gè)旋轉(zhuǎn)角度 巧;=爲(wèi);0.,手腕上升尚度rfj. =.'〇.。
[0028] 3)為了滿足SCARA機(jī)器人的加減速需要,本發(fā)明采用如圖6的運(yùn)動(dòng)時(shí)間梯形圖,此 梯形圖包含的面積就是機(jī)器人大臂、小臂和手腕要旋轉(zhuǎn)的角度或者是手腕升降的高度,為 了方便控制,本發(fā)明采用單一加速度模式。
[0029] 4)SCARA機(jī)器人讀取其工作模式,如果是人工工作模式,主站開(kāi)始與ARM控制器通 訊,由主站輸入SCARA機(jī)器人大臂、小臂和手腕需要旋轉(zhuǎn)的角度0:,02,04以及SCARA機(jī)器人手 腕需要上升或者是下降的高度d3給ARM控制器,ARM控制器根據(jù)Denavit-Hartenberg算法開(kāi) 始機(jī)器人正向求解:ARM控制器首先根據(jù)SCARA機(jī)器人大臂、小臂和手腕需要旋轉(zhuǎn)的角度0:, 0 2,04以及SCARA機(jī)器人手腕需要上升的高度d3計(jì)算出相鄰兩桿件坐標(biāo)系之間的位姿矩陣 Moi、Ml2、M23、M34并用4*4二維數(shù)組標(biāo)志,Moi、Ml2、M23、M34分別表不如下:
[0030] 然后通過(guò)公式MQ4=MQ1M12M23M 34就可以求出手腕執(zhí)行器完成任務(wù)后的位置和姿態(tài), 然后ARM與DSP通訊,并傳輸人工輸入?yún)?shù)。
[0031] 5)SCARA機(jī)器人讀取其工作模式,如果是正常自動(dòng)搬運(yùn)工作模式,主站開(kāi)始與ARM 控制器通訊,由主站輸入SCARA機(jī)器人大臂、小臂和手腕所處的初始位置和給定位置三維坐 標(biāo)給ARM控制器,ARM控制器根據(jù)Denavit-Hartenberg算法開(kāi)始機(jī)器人逆向求解:ARM控制器 首先根據(jù)大臂、小臂長(zhǎng)度以及最終的三維坐標(biāo)中的X和Y坐標(biāo)求出大臂需要旋轉(zhuǎn)的角度0i, 并借助以的值求出9 2的值,并根據(jù)三維坐標(biāo)中的Z坐標(biāo)求出手腕上升或降低的高度d3,最終 求出旋轉(zhuǎn)角度94,由于在求解的時(shí)候方程具有雙解,所以SCARA機(jī)器人求出大臂、小臂和手 腕需要旋轉(zhuǎn)的角度kb 04以及SCARA機(jī)器人手腕需要上升的高度d3后,ARM控制器會(huì)對(duì)求解 結(jié)果進(jìn)行優(yōu)化,然后ARM控制器與DSP通訊,把機(jī)器人最有伺服運(yùn)動(dòng)參數(shù)傳輸給DSP控制器。 [0032] 6)DSP控制器接受SCARA機(jī)器人大臂、小臂和手腕需要旋轉(zhuǎn)的角度0K02.04以及 SCARA機(jī)器人手腕需要上升的高度d 3后,大臂、小臂和手腕上的傳感器S1~S5將開(kāi)啟,首先 SCARA機(jī)器人對(duì)各個(gè)旋轉(zhuǎn)臂旋轉(zhuǎn)角度內(nèi)的障礙物進(jìn)行判斷,如存在障礙物將向DSP發(fā)出中斷 請(qǐng)求,DSP會(huì)對(duì)中斷做第一時(shí)間響應(yīng),然后封鎖四軸P麗波控制信號(hào)輸出,禁止SCARA機(jī)器人 的永磁直流伺服電機(jī)X、永磁直流伺服電機(jī)Y、永磁直流伺服電機(jī)Z和電機(jī)永磁直流伺服電機(jī) R工作,機(jī)器人自鎖在原地,DSP控制器二次判斷運(yùn)動(dòng)范圍內(nèi)的障礙物信息,防止信息誤判; 7 )如果DSP控制器確定無(wú)障礙物進(jìn)入運(yùn)動(dòng)區(qū)域,DSP按照?qǐng)D6的速度時(shí)間曲線,DSP把三 個(gè)旋轉(zhuǎn)角度H04轉(zhuǎn)化為三臺(tái)永磁直流伺服電機(jī)的加速度、速度和位置初始指令值,然后 DSP結(jié)合電機(jī)X、電機(jī)Y和電機(jī)Z電機(jī)電流反饋、光電編碼器反饋和加速度傳感器的反饋,經(jīng)內(nèi) 部四閉環(huán)伺服控制程序?qū)崟r(shí)調(diào)整電機(jī)X、電機(jī)Y、電機(jī)Z的PWM波控制信號(hào),DSP控制器根據(jù)四 閉環(huán)輸入的偏差大小實(shí)時(shí)調(diào)整內(nèi)部伺服控制程序的PID參數(shù),通過(guò)調(diào)整永磁直流伺服電機(jī) 的驅(qū)動(dòng)脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度,通過(guò)調(diào)整永磁直流伺服電機(jī)控制信號(hào)的頻率實(shí)現(xiàn)角速度 快慢的變化,使四軸永磁直流伺服電機(jī)時(shí)刻同步工作,DSP時(shí)刻記錄機(jī)器人已經(jīng)運(yùn)動(dòng)的位置 參數(shù)。
[0033] 8)在SCARA機(jī)器人移動(dòng)過(guò)程中,傳感器S1~S4時(shí)刻對(duì)運(yùn)動(dòng)范圍內(nèi)的移動(dòng)障礙物進(jìn)行 判斷,如果有障礙物進(jìn)入運(yùn)動(dòng)范圍,SCARA機(jī)器人的大臂、小臂和手腕按照?qǐng)D6的運(yùn)動(dòng)曲線立 即停車,DSP控制器記錄下當(dāng)前旋轉(zhuǎn)角度0K02、04信息,等障礙物消失后,重新計(jì)算對(duì)新位置 的旋轉(zhuǎn)角度 01、02、04,然后按照?qǐng)D6的曲線重新經(jīng)過(guò)三段運(yùn)動(dòng)軌跡:加速運(yùn)動(dòng)、勻速運(yùn)動(dòng)和減 速運(yùn)動(dòng),最終到達(dá)設(shè)定點(diǎn)。
[0034] 9)在移動(dòng)過(guò)程中,加速度計(jì)時(shí)刻記錄大臂、小臂和手腕旋轉(zhuǎn)的角加速度,并通過(guò)二 次積分得到大臂、小臂和手腕的旋轉(zhuǎn)角度,并與設(shè)定的位置角度值相比較,如果偏差大于設(shè) 定閥值,在下一個(gè)采樣周期,DSP控制器根據(jù)內(nèi)部三閉環(huán)伺服控制器調(diào)整永磁直流伺服電機(jī) X、永磁直流伺服電機(jī)Y、永磁直流伺服電機(jī)Z的PWM控制信號(hào),DSP控制器根據(jù)三閉環(huán)輸入的 偏差大小實(shí)時(shí)調(diào)整內(nèi)部伺服控制程序的PID參數(shù),通過(guò)調(diào)整永磁直流伺服電機(jī)的驅(qū)動(dòng)脈沖 數(shù)目調(diào)整其旋轉(zhuǎn)角度,通過(guò)調(diào)整永磁直流伺服電機(jī)控制信號(hào)的頻率實(shí)現(xiàn)角速度快慢的變 化,進(jìn)而消除上一個(gè)采樣周期產(chǎn)生的誤差,使SCARA機(jī)器人按照設(shè)定軌跡完成任務(wù)。
[0035] 10)在SCARA機(jī)器人運(yùn)動(dòng)過(guò)程中,DSP會(huì)時(shí)刻儲(chǔ)存所經(jīng)過(guò)的SCARA機(jī)器人所處的位置 或者是經(jīng)過(guò)的參考點(diǎn),并根據(jù)這些距離信息由DSP計(jì)算得到相對(duì)下一個(gè)參考點(diǎn)SCARA機(jī)器人 永磁直流伺服電機(jī)X、永磁直流伺服電機(jī)Y、永磁直流伺服電機(jī)Z分別要運(yùn)行的角度、角度速 度和角加速度,DSP控制器再結(jié)合角速度傳感器反饋、光電編碼器反饋,根據(jù)其內(nèi)部三閉環(huán) 伺服控制程序得到四臺(tái)永磁直流伺服電機(jī)的PWM控制信號(hào)信號(hào),DSP控制器根據(jù)四閉環(huán)輸入 的偏差大小實(shí)時(shí)調(diào)整內(nèi)部伺服控制程序的PID參數(shù),通過(guò)調(diào)整永磁直流伺服電機(jī)的驅(qū)動(dòng)脈 沖數(shù)目調(diào)整其旋轉(zhuǎn)角度,通過(guò)調(diào)整永磁直流伺服電機(jī)控制信號(hào)的頻率實(shí)現(xiàn)角速度快慢的變 化,使SCARA機(jī)器人按照設(shè)定速度快速前行。
[0036] 11)在SCARA機(jī)器人完成四軸旋轉(zhuǎn)角度0^02、04的伺服控制后,DSP二次檢測(cè)加速度 傳感器的積分值,如果發(fā)現(xiàn)SCARA機(jī)器人在運(yùn)動(dòng)過(guò)程受到外界干擾后三個(gè)旋轉(zhuǎn)角度0k 02、04 與設(shè)定角度的誤差超過(guò)設(shè)定閥值時(shí),DSP把三個(gè)旋轉(zhuǎn)角度偏差A(yù)爲(wèi),A色,A爲(wèi)轉(zhuǎn)化為四臺(tái)永 磁直流伺服電機(jī)位置微調(diào)的加速度、速度和位置初始指令值,DSP再結(jié)合電機(jī)X、電機(jī)Y和電 機(jī)Z電流、光電編碼器反饋和加速度傳感器的反饋,經(jīng)DSP內(nèi)部四閉環(huán)伺服控制程序調(diào)整電 機(jī)X、電機(jī)Y和電機(jī)Z、R的PWM波控制信號(hào),DSP控制器根據(jù)三閉環(huán)輸入的偏差大小實(shí)時(shí)調(diào)整內(nèi) 部伺服控制程序的PID參數(shù),通過(guò)調(diào)整永磁直流伺服電機(jī)的驅(qū)動(dòng)脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度, 通過(guò)調(diào)整永磁直流伺服電機(jī)控制信號(hào)的頻率實(shí)現(xiàn)角速度快慢的變化,通過(guò)三軸永磁直流伺 服電機(jī)的再次工作使機(jī)器人大臂、小臂和手腕到達(dá)設(shè)定位置。
[0037] 12)當(dāng)SCARA機(jī)器人的大臂、小臂和手腕完成角度0^02、04的角度補(bǔ)償?shù)竭_(dá)設(shè)定位置 后,DSP按照?qǐng)D6的速度時(shí)間曲線,把手腕要升降的距離d 3轉(zhuǎn)化為永磁直流伺服電機(jī)R的加速 度、速度和位置初始指令值,然后DSP結(jié)合電機(jī)R的電流反饋、光電編碼器反饋和傳感器S5的 反饋,經(jīng)內(nèi)部三閉環(huán)伺服控制程序?qū)崟r(shí)調(diào)整電機(jī)R的PWM波控制信號(hào),DSP控制器根據(jù)三閉環(huán) 輸入的偏差大小實(shí)時(shí)調(diào)整內(nèi)部伺服控制程序的PID參數(shù),通過(guò)調(diào)整永磁直流伺服電機(jī)的驅(qū) 動(dòng)脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度,通過(guò)調(diào)整永磁直流伺服電機(jī)控制信號(hào)的頻率實(shí)現(xiàn)角速度快慢 的變化,使手腕平穩(wěn)在設(shè)定時(shí)間內(nèi)到達(dá)設(shè)定位置。
[0038] 13)在運(yùn)動(dòng)過(guò)程中如果SCARA機(jī)器人發(fā)現(xiàn)位置參數(shù)求解出現(xiàn)死循環(huán)將向ARM發(fā)出中 斷請(qǐng)求,ARM會(huì)對(duì)中斷做第一時(shí)間響應(yīng),ARM控制器將立即與DSP通訊,DSP立即封鎖四臺(tái)永磁 直流伺服電機(jī)的控制信號(hào),然后機(jī)器人原地自鎖,并放棄搬運(yùn)工作,然后由主站根據(jù)儲(chǔ)存信 息分析進(jìn)行分析。
[0039] 14 )裝在永磁直流伺服電機(jī)X、永磁直流伺服電機(jī)Y、永磁直流伺服電機(jī)Z、永磁直流 伺服電機(jī)R上的光電編碼器會(huì)輸出其位置信號(hào)A和位置信號(hào)B,光電編碼器的位置信號(hào)A脈沖 和B脈沖邏輯狀態(tài)每變化一次,DSP內(nèi)的位置寄存器會(huì)根據(jù)電機(jī)的運(yùn)行方向加 1或者是減1; 光電編碼器的位置信號(hào)A脈沖和B脈沖和Z脈沖同時(shí)為低電平時(shí),就產(chǎn)生一個(gè)INDEX信號(hào)給 DSP內(nèi)部寄存器,記錄永磁直流伺服電機(jī)的絕對(duì)位置,然后換算成SCARA機(jī)器人大臂、小臂或 者是手腕在三維坐標(biāo)系統(tǒng)中的具體位置。
[0040] 15)SCARA機(jī)器人在運(yùn)行過(guò)程ARM控制器時(shí)刻對(duì)AC交流主電源進(jìn)行監(jiān)控,如果控制 器發(fā)現(xiàn)主電源出現(xiàn)故障突然斷電時(shí),ARM會(huì)與DSP通訊,并開(kāi)通備用電源,由備用電源為四軸 永磁直流伺服電機(jī)提供能量,并通過(guò)DSP內(nèi)部三閉環(huán)伺服控制系統(tǒng)程序?qū)崟r(shí)調(diào)整四臺(tái)永磁 直流伺服電機(jī)的PWM輸出,通過(guò)調(diào)整永磁直流伺服電機(jī)的驅(qū)動(dòng)脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度,通 過(guò)調(diào)整永磁直流伺服電機(jī)控制信號(hào)的頻率實(shí)現(xiàn)角速度快慢的變化,使SCARA機(jī)器人完成這 次搬運(yùn)任務(wù),然后SCARA機(jī)器人與主站進(jìn)行通訊,并通知主站進(jìn)行檢修。
[0041] 16)在SCARA機(jī)器人多軸伺服系統(tǒng)工作過(guò)程中,如果DSP伺服控制器檢測(cè)到某一個(gè) 永磁直流伺服電機(jī)的轉(zhuǎn)矩出現(xiàn)脈動(dòng),由于本發(fā)明采用的永磁直流伺服電機(jī)力矩與電機(jī)電流 成正比例,因此控制器會(huì)很容易補(bǔ)償此干擾,并根據(jù)干擾大小DSP實(shí)時(shí)調(diào)整電流環(huán)的PID參 數(shù),減少了電機(jī)轉(zhuǎn)矩?cái)_動(dòng)對(duì)SCARA機(jī)器人運(yùn)動(dòng)過(guò)程的影響。
[0042] 17)當(dāng)SCARA機(jī)器人完成任務(wù),實(shí)現(xiàn)位置歸零時(shí),其攜帶的加速度傳感器A1~A3會(huì)時(shí) 刻檢測(cè)其加速度,當(dāng)加速度超過(guò)預(yù)設(shè)閥值時(shí),在下一個(gè)周期DSP控制器會(huì)修正上一個(gè)周期帶 來(lái)的誤差,確保機(jī)器人停止在設(shè)定零位置,SCARA機(jī)器人按照?qǐng)D6的速度-時(shí)間運(yùn)動(dòng)曲線實(shí)現(xiàn) 從某一點(diǎn)到零位的歸位,然后零位自鎖,等待下一個(gè)搬運(yùn)命令。
[0043]以上所述僅為本發(fā)明的實(shí)施例,并非因此限制本發(fā)明的專利范圍,凡是利用本發(fā) 明說(shuō)明書(shū)內(nèi)容所作的等效結(jié)構(gòu)或等效流程變換,或直接或間接運(yùn)用在其它相關(guān)的技術(shù)領(lǐng) 域,均同理包括在本發(fā)明的專利保護(hù)范圍內(nèi)。
【主權(quán)項(xiàng)】
1. 一種教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng),其特征在于,所述關(guān)節(jié)機(jī)器人采用永磁 直流伺服電機(jī)X驅(qū)動(dòng)機(jī)器人大臂旋轉(zhuǎn)運(yùn)動(dòng)、采用永磁直流伺服電機(jī)Y驅(qū)動(dòng)機(jī)器人小臂旋轉(zhuǎn)運(yùn) 動(dòng)、采用永磁直流伺服電機(jī)Z驅(qū)動(dòng)機(jī)器人手腕旋轉(zhuǎn)運(yùn)動(dòng)、采用永磁直流伺服電機(jī)R驅(qū)動(dòng)機(jī)器 人手腕升降運(yùn)動(dòng),所述機(jī)器人大臂上安裝有避障位移傳感器S1、避障位移傳感器S2和加速 度傳感器A1,所述機(jī)器人小臂上安裝有避障位移傳感器S3、避障位移傳感器S4和加速度傳 感器A2,所述機(jī)器人手腕上安裝有定位傳感器S5和加速度傳感器A3,所述的控制系統(tǒng)包括 主站控制計(jì)算機(jī)和控制器,所述控制器包括DSP芯片控制器和ARM控制器,所述DSP芯片控制 器和ARM控制器均與所述主站控制計(jì)算機(jī)通信連接,所述DSP芯片控制器與ARM控制器之間 通信連接,所述永磁直流伺服電機(jī)X、永磁直流伺服電機(jī)Y、永磁直流伺服電機(jī)Z和永磁直流 伺服電機(jī)R均與所述DSP芯片控制器通信連接,所述避障位移傳感器S1、避障位移傳感器S2、 避障位移傳感器S3、避障位移傳感器S4、定位傳感器S5、加速度傳感器A1、加速度傳感器A2 和加速度傳感器A3均同時(shí)與所述DSP芯片控制器和ARM控制器通信連接。2. 根據(jù)權(quán)利要求1所述的教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng),其特征在于,還包括為 所述關(guān)節(jié)機(jī)器人和控制系統(tǒng)提供電源的主電源和備用電源,所述關(guān)節(jié)機(jī)器人中的各個(gè)所述 電機(jī)和傳感器以及控制系統(tǒng)中的控制器均與所述主電源和/或備用電源電性連接。3. 根據(jù)權(quán)利要求1所述的教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng),其特征在于,所述永磁 直流伺服電機(jī)X、永磁直流伺服電機(jī)Y、永磁直流伺服電機(jī)Z和永磁直流伺服電機(jī)R上勻設(shè)置 有光電編碼器,所述光電編碼器與所述DSP芯片控制器電性連接。4. 根據(jù)權(quán)利要求1至3任一項(xiàng)所述的教學(xué)用雙核常速關(guān)節(jié)機(jī)器人控制系統(tǒng),其特征在 于,所述DSP芯片控制器內(nèi)設(shè)置有記錄永磁直流伺服電機(jī)位置的位置寄存器。
【文檔編號(hào)】B25J9/04GK106003030SQ201610409108
【公開(kāi)日】2016年10月12日
【申請(qǐng)日】2016年6月13日
【發(fā)明人】張好明
【申請(qǐng)人】江蘇若博機(jī)器人科技有限公司