一種教學(xué)用無線傳輸三核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)的制作方法
【專利摘要】本發(fā)明公開了一種教學(xué)用無線傳輸三核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)，所述的控制系統(tǒng)包括主站控制計(jì)算機(jī)和控制器，所述控制器包括DSP芯片控制器、集成芯片處理器、圖像采集模塊、無線傳輸模塊和ARM控制器，DSP芯片控制器和ARM控制器均與主站控制計(jì)算機(jī)通信連接，DSP芯片控制器、圖像采集模塊、無線傳輸模塊與ARM控制器之間通信連接，四臺(tái)永磁直流伺服電機(jī)均與DSP芯片控制器通信連接，多個(gè)避障位移傳感器、定位傳感器、加速度傳感器均與DSP芯片控制器和ARM控制器通信連接。本發(fā)明的教學(xué)用無線傳輸三核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)計(jì)算速度快，使關(guān)節(jié)機(jī)器人手臂轉(zhuǎn)向靈活、動(dòng)作穩(wěn)定精確、體積小巧，性能穩(wěn)定。
【專利說明】
一種教學(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ù)率較 高的長時(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é)大臂和小臂來實(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è)方向上具有無限大剛度。這種結(jié)構(gòu) 特性使得SCARA機(jī)器人擅長從一點(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ù)的開展可以培養(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)貨物過程中要時(shí)刻判斷主控制器輸入的位置參數(shù)，并判斷周圍 的環(huán)境時(shí)刻避障，然后由運(yùn)動(dòng)控制器反復(fù)控制其精確的加速和減速進(jìn)行搬運(yùn)貨物，稍微的 誤差累計(jì)就有可能在多回合運(yùn)動(dòng)中導(dǎo)致運(yùn)輸失敗。國內(nèi)對(duì)SCARA機(jī)器人的使用雖然有幾十 年，但是由于國內(nèi)工業(yè)機(jī)器人發(fā)展起步比較晚，受較多關(guān)鍵技術(shù)的影響，SCARA機(jī)器人的發(fā) 展也受所影響，傳統(tǒng)的機(jī)器人原理如圖1所示，在長期使用期間出現(xiàn)眾多問題： (1)在SCARA機(jī)器人多軸伺服控制系統(tǒng)中，基于單片機(jī)或者是DSP的控制系統(tǒng)都需要編 寫伺服控制軟件，使得系統(tǒng)開發(fā)周期較長，而且大大降低了系統(tǒng)的運(yùn)算速度，并且程序可移 植能力較差。
[0005] (2)在大多數(shù)情況下，隨著時(shí)間的積累，搬運(yùn)會(huì)存在一定的位置誤差，依靠人工引 導(dǎo)到零位置的方法，不僅精確度不高，而且也不利于高密度搬運(yùn)等工作。
[0006] (3)在SCARA大批量搬運(yùn)過程中，搬運(yùn)誤差和質(zhì)量問題時(shí)有發(fā)生，如果不將類似問 題檢查出來，會(huì)為以后的運(yùn)動(dòng)過程留下隱患。目前對(duì)于SCARA工作的檢測，主要依靠人工目 測等檢驗(yàn)方法實(shí)現(xiàn)，這不僅存在主觀因素影響大、勞動(dòng)強(qiáng)度高、工作效率低等弊端，而目在 短時(shí)間內(nèi)人工大批量檢驗(yàn)SCARA工作結(jié)果也不現(xiàn)實(shí)。
[0007] (4)在SCARA大批量搬運(yùn)生產(chǎn)中，有時(shí)會(huì)有重要的臨時(shí)任務(wù)加入，普通的SCARA往往 通過停機(jī)的方式完成，造成效率較低。

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

[0008] 本發(fā)明主要解決的技術(shù)問題是提供一種教學(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)定精確、體積小巧，系統(tǒng)性能穩(wěn)定，抗干擾能力強(qiáng)，可以遠(yuǎn)距離無線監(jiān)控機(jī) 器人操作。
[0009] 為解決上述技術(shù)問題，本發(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ī)器人大臂上 安裝有避障位移傳感器Sl、避障位移傳感器S2和加速度傳感器Al，所述機(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均與所述集成芯片處理器通信連接， 所述避障位移傳感器Sl、避障位移傳感器S2、避障位移傳感器S3、避障位移傳感器S4、定位 傳感器S5、加速度傳感器Al、加速度傳感器A2和加速度傳感器A3均同時(shí)與所述DSP芯片控制 器、集成芯片處理器和ARM控制器通信連接。
[0010] 在本發(fā)明一個(gè)較佳實(shí)施例中，還包括為所述關(guān)節(jié)機(jī)器人和控制系統(tǒng)提供電源的主 電源和備用電源，所述關(guān)節(jié)機(jī)器人中的各個(gè)所述電機(jī)和傳感器以及控制系統(tǒng)中的控制器均 與所述主電源和/或備用電源電性連接。
[0011] 在本發(fā)明一個(gè)較佳實(shí)施例中，所述永磁直流伺服電機(jī)X、永磁直流伺服電機(jī)Y、永磁 直流伺服電機(jī)Z和永磁直流伺服電機(jī)R上勻設(shè)置有光電編碼器，所述光電編碼器與所述集成 芯片處理器電性連接。
[0012] 在本發(fā)明一個(gè)較佳實(shí)施例中，所述圖像采集模塊為CCD攝像頭組件。
[0013] 在本發(fā)明一個(gè)較佳實(shí)施例中，所述圖像采集模塊內(nèi)設(shè)置有解碼芯片。
[0014] 本發(fā)明的有益效果是:本發(fā)明的教學(xué)用無線傳輸三核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)計(jì) 算速度快，使關(guān)節(jié)機(jī)器人手臂轉(zhuǎn)向靈活、動(dòng)作穩(wěn)定精確、體積小巧，性能穩(wěn)定，抗干擾能力 強(qiáng)，可以遠(yuǎn)距離無線監(jiān)控機(jī)器人操作。
【附圖說明】
[0015] 為了更清楚地說明本發(fā)明實(shí)施例中的技術(shù)方案，下面將對(duì)實(shí)施例描述中所需要使 用的附圖作簡單地介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于 本領(lǐng)域普通技術(shù)人員來講，在不付出創(chuàng)造性勞動(dò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ī)運(yùn)動(dòng)原理圖； 圖7為四軸電機(jī)加減速曲線圖； 圖8為本發(fā)明的教學(xué)用無線傳輸三核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)結(jié)構(gòu)示意圖。
[0016] 附圖中各部件的標(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、避障位移傳感器Sl，13、避障 位移傳感器S2，14、避障位移傳感器S3，15、避障位移傳感器S4，16、定位傳感器S5，17、加速 度傳感器Al，18、加速度傳感器A2，19、加速度傳感器A3，20、無線傳輸模塊，21、圖像采集模 塊，22、解碼芯片，23、集成芯片處理器。
【具體實(shí)施方式】
[0017] 下面將對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施 例僅是本發(fā)明的一部分實(shí)施例，而不是全部的實(shí)施例?；诒景l(fā)明中的實(shí)施例，本領(lǐng)域普通 技術(shù)人員在沒有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其它實(shí)施例，都屬于本發(fā)明保護(hù)的范 圍。
[0018] 請(qǐng)參閱圖2至圖8,本發(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ī)RlO 驅(qū)動(dòng)機(jī)器人手腕升降運(yùn)動(dòng)，所述機(jī)器人大臂上安裝有避障位移傳感器Sl 12、避障位移傳感 器S2 13和加速度傳感器Al 17,所述機(jī)器人小臂上安裝有避障位移傳感器S3 14、避障位移 傳感器S4 15和加速度傳感器A2 18,所述機(jī)器人手腕上安裝有定位傳感器S5 16和加速度 傳感器A3 19。
[0019] 所述的控制系統(tǒng)包括主站控制計(jì)算機(jī)1和控制器2,所述控制器2包括DSP芯片控制 器3、集成芯片處理器23、圖像采集模塊21、無線傳輸模塊20和ARM控制器4,所述圖像采集模 塊21與無線傳輸模塊20之間通信連接，所述DSP芯片控制器3、集成芯片處理器、無線傳輸模 塊20和ARM控制器4均與所述主站控制計(jì)算機(jī)1通信連接，所述DSP芯片控制器3、集成芯片處 理器23、無線傳輸模塊20和ARM控制器4彼此之間通信連接。所述永磁直流伺服電機(jī)X7、永磁 直流伺服電機(jī)Y8、永磁直流伺服電機(jī)Z9和永磁直流伺服電機(jī)RlO均與集成芯片處理器23通 信連接，所述避障位移傳感器Sl 12、避障位移傳感器S2 13、避障位移傳感器S3 14、避障位 移傳感器S4 15、定位傳感器S5 16、加速度傳感器Al 17、加速度傳感器A2 18和加速度傳感 器A3 19均同時(shí)與所述DSP芯片控制器3、集成芯片處理器23和ARM控制器4通信連接。
[0020] 優(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電性連接。
[0021] 優(yōu)選地，所述永磁直流伺服電機(jī)X 7、永磁直流伺服電機(jī)Y 8、永磁直流伺服電機(jī)Z 9和永磁直流伺服電機(jī)RlO上勻設(shè)置有光電編碼器11，所述光電編碼器11與所述集成芯片處 理器23電性連接。
[0022]優(yōu)選地，所述圖像采集模塊21為CCD攝像頭組件，所述圖像采集模塊21內(nèi)設(shè)置有解 碼芯片22。
[0023] CCD是一種光電轉(zhuǎn)換式圖像傳感器，它利用光電轉(zhuǎn)換原理把圖像信息直接轉(zhuǎn)換成 電信號(hào)，實(shí)現(xiàn)非電量的電測量，同時(shí)它還具有體積小、重量輕、噪聲低、自掃描、工作速度快、 測量精度高、壽命長等諸多優(yōu)點(diǎn)。為了及時(shí)處理搬運(yùn)結(jié)果，本系統(tǒng)加入了基于CCD的實(shí)時(shí)圖 像采集和無線傳輸系統(tǒng)。CCD攝像頭輸出模擬信號(hào)，通過解碼芯片22對(duì)其進(jìn)行解碼，變成可 編程的數(shù)字信號(hào)，便于ARM對(duì)數(shù)字圖像的處理和存儲(chǔ)，然后通過無線系統(tǒng)與主站進(jìn)行通訊和 傳輸。
[0024]本發(fā)明的控制系統(tǒng)采用DSP芯片控制器3+集成芯片處理器23(即LM628芯片，一種 美國國家半導(dǎo)體公司生產(chǎn)的大規(guī)模集成芯片）+ARM控制器4三核控制器，在電源打開狀態(tài) 下，ARM控制器先對(duì)機(jī)器人備用電源SOC(荷電狀態(tài))和主電源進(jìn)行判斷，如果備用能源較低， 控制器將會(huì)通過無線裝置向總站發(fā)出報(bào)警信號(hào)；如果備用電源和主電源工作正常，先由主 站控制計(jì)算機(jī)1通過無線傳輸模塊20把搬運(yùn)貨物位置信息輸入給ARM，由ARM計(jì)算出機(jī)器人 伺服系統(tǒng)的參數(shù);SCARA機(jī)器人依靠圖像采集系統(tǒng)自動(dòng)引導(dǎo)到零位置，機(jī)器人先進(jìn)入自鎖狀 態(tài)，一旦主站控制計(jì)算機(jī)1發(fā)出搬運(yùn)命令，機(jī)器人攜帶的避障傳感器、定位傳感器和加速度 傳感器均開啟，SCARA機(jī)器人按照設(shè)定ARM優(yōu)化的搬運(yùn)路徑快速搬運(yùn)，DSP與LM628通訊， LM628按照伺服參數(shù)和傳感器反饋實(shí)時(shí)調(diào)整SCARA機(jī)器人永磁直流伺服電機(jī)X、永磁直流伺 服電機(jī)Y、永磁直流伺服電機(jī)Z和永磁直流伺服電機(jī)R的PWM輸出，實(shí)現(xiàn)四臺(tái)永磁直流伺服電 機(jī)的實(shí)時(shí)伺服控制，DSP實(shí)時(shí)采集機(jī)器人運(yùn)動(dòng)信息并通過無線裝置向總站傳輸;CCD攝像頭 實(shí)時(shí)檢測搬運(yùn)結(jié)果，ARM對(duì)數(shù)字圖像的處理和存儲(chǔ)并通過無線系統(tǒng)傳輸，主站通過圖像對(duì)比 技術(shù)自動(dòng)檢測搬運(yùn)結(jié)果。如果ARM對(duì)搬運(yùn)某一個(gè)位置有疑問，將與DSP通訊，DSP通過LM628使 SCARA機(jī)器人停車，然后主站通過圖像對(duì)比對(duì)搬運(yùn)信息進(jìn)行判斷，確認(rèn)無誤后通過無線裝置 二次啟動(dòng)SCARA機(jī)器人繼續(xù)未完成的任務(wù);主站通過無線裝置實(shí)時(shí)與ARM通訊傳輸新加入搬 運(yùn)貨物位置信息，由ARM更新機(jī)器人伺服系統(tǒng)參數(shù)，并與DSP通訊。
[0025] 參照?qǐng)D1，圖2,圖3和圖4、圖5、圖6、圖7,其具體的功能實(shí)現(xiàn)如下： DSCARA機(jī)器人電源打開后，ARM會(huì)對(duì)備用電源SOC和主電源進(jìn)行判斷，如果備用電源 SOC較低時(shí)，DSP將禁止控制四臺(tái)永磁直流伺服電機(jī)的LM628工作，電機(jī)輸入PffM波控制信號(hào) 被封鎖，同時(shí)報(bào)警傳感器將工作并通過無線裝置向總站發(fā)出報(bào)警信號(hào)；如果電池 SOC正常， SCARA機(jī)器人進(jìn)入待工作狀態(tài)，等待工作命令。
[0026] 2)主站通過無線裝置把大臂、小臂長度和升降桿長度等信息傳入到ARM控制器， ARM控制器開啟CCD圖像采集系統(tǒng)，SCARA機(jī)器人依靠圖像采集通過DSP開通LM628自動(dòng)引導(dǎo) SCARA機(jī)器人到設(shè)定工作的零位置位，SCARA機(jī)器人設(shè)定各個(gè)旋轉(zhuǎn)角度螂=焉==?*，手 腕上升高度Wf= ?。
[0027] 3 )為了滿足SCARA機(jī)器人的加減速運(yùn)動(dòng)需要，本發(fā)明采用如圖7的運(yùn)動(dòng)時(shí)間梯形 圖，此梯形圖包含的面積就是機(jī)器人大臂、小臂和手腕要旋轉(zhuǎn)的角度或者是手腕升降的高 度，為了方便控制，本發(fā)明采用單一加速度模式。
[0028] 4)無線裝置發(fā)出運(yùn)動(dòng)模式，SCARA機(jī)器人通過ARM讀取其工作模式，如果是人工工 作模式，主站開始與ARM控制器通訊，由主站輸入SCARA機(jī)器人大臂、小臂和手腕需要旋轉(zhuǎn)的 角度1?,?以及SCARA機(jī)器人手腕需要上升或者是下降的高度衊給ARM控制器，ARM控制 器根據(jù)Denavit-Hartenberg算法開始機(jī)器人位置和姿態(tài)正向求解:ARM控制器首先根據(jù) SCARA機(jī)器人大臂、小臂和手腕需要旋轉(zhuǎn)的角度巧，咚，巧以及3041^機(jī)器人手腕需要上升的 高度*計(jì)算出相鄰兩桿件坐標(biāo)系之間的位姿矩陣I 01:，M12，，Mw，并用各自用4*4二 維數(shù)組標(biāo)志，碼《，M k，M25，Mw分別表示如下：
[0029] ARM控制器然后通過公式Mtw 就可以求出手腕執(zhí)行器完成任務(wù) 后的位置和姿態(tài)，并與DSP通訊，傳輸人工輸入?yún)?shù)給DSP。
[0030] 5)無線裝置發(fā)出運(yùn)動(dòng)模式，SCARA機(jī)器人讀取其工作模式，主站開始與ARM控制器 通訊，由主站輸入SCARA機(jī)器人所處的初始位置和給定位置三維坐標(biāo)給ARM控制器，ARM控制 器根據(jù)Denavit-Hartenberg算法開始機(jī)器人逆向求解:ARM控制器首先根據(jù)大臂、小臂長度 以及最終的三維坐標(biāo)中的X和Y坐標(biāo)求出大臂需要旋轉(zhuǎn)的角度巧，并借助咚的值求出蜷的值， 并根據(jù)三維坐標(biāo)中的Z坐標(biāo)求出手腕上升或降低的高度禽，最終求出旋轉(zhuǎn)角度武，由于在求 解各的時(shí)候方程具有雙解，所以SCARA機(jī)器人求出大臂、小臂和手腕需要旋轉(zhuǎn)的角度磉，咚， 咚以及SCARA機(jī)器人手腕需要上升的高度:rf 3后，ARM控制器會(huì)對(duì)求解結(jié)果進(jìn)行優(yōu)化，然后ARM 控制器與DSP通訊，把機(jī)器人最有伺服運(yùn)動(dòng)參數(shù)傳輸給DSP控制器。
[0031] 6)DSP控制器接受SCARA機(jī)器人大臂、小臂和手腕需要旋轉(zhuǎn)的角度峪，咚，芎以及 SCARA機(jī)器人手腕需要上升的高度A后，大臂、小臂和手腕上的傳感器S1~S5將開啟，首先 SCARA機(jī)器人對(duì)各個(gè)旋轉(zhuǎn)臂旋轉(zhuǎn)角度內(nèi)的障礙物進(jìn)行判斷，如存在障礙物將向DSP發(fā)出中斷 請(qǐng)求，DSP會(huì)對(duì)中斷做第一時(shí)間響應(yīng)，然后DSP禁止四軸LM628工作，四軸永磁直流伺服電機(jī) PWM波控制信號(hào)被封鎖，SCARA機(jī)器人禁止永磁直流伺服電機(jī)X、永磁直流伺服電機(jī)Y、永磁 直流伺服電機(jī)Z和電機(jī)永磁直流伺服電機(jī)R工作，機(jī)器人自鎖在原地;然后ARM通過無線裝置 向總站發(fā)出中斷請(qǐng)求，為了防止信息誤判，總站二次判斷運(yùn)動(dòng)范圍內(nèi)的障礙物信息，由總站 人為處理障礙物，防止障礙物影響搬運(yùn)工作。
[0032] 7)如果總站二次通過圖像采集結(jié)果確定無障礙物進(jìn)入運(yùn)動(dòng)區(qū)域，將通過無線裝置 與DSP通訊，DSP控制器按照?qǐng)D7的速度時(shí)間曲線對(duì)運(yùn)動(dòng)位置進(jìn)行分解，DSP首先把三個(gè)旋轉(zhuǎn) 角度咚，琿，吸轉(zhuǎn)化為三臺(tái)永磁直流伺服電機(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的PffM波控制信號(hào)，PWM波控制信號(hào)經(jīng)驅(qū)動(dòng)橋放大后驅(qū)動(dòng)各個(gè)電機(jī) 運(yùn)動(dòng)。DSP控制器根據(jù)輸入偏差大小實(shí)時(shí)調(diào)整LM628內(nèi)部伺服控制程序的PID參數(shù)，LM628控 制器通過調(diào)整永磁直流伺服電機(jī)的驅(qū)動(dòng)脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度，通過調(diào)整永磁直流伺服 電機(jī)控制信號(hào)的頻率實(shí)現(xiàn)角速度快慢的變化，使三軸永磁直流伺服電機(jī)時(shí)刻同步工作，DSP 時(shí)刻記錄機(jī)器人已經(jīng)運(yùn)動(dòng)的位置參數(shù)，并通過無線實(shí)時(shí)向總站傳輸。
[0033] 8)在SCARA機(jī)器人移動(dòng)過程中，傳感器S1~S4時(shí)刻對(duì)運(yùn)動(dòng)范圍內(nèi)的移動(dòng)障礙物進(jìn)行 判斷，如果有障礙物進(jìn)入運(yùn)動(dòng)范圍，DSP立即通過LM628使SCARA機(jī)器人的大臂、小臂和手腕 按照?qǐng)D7的減速運(yùn)動(dòng)曲線立即停車，DSP控制器記錄下當(dāng)前旋轉(zhuǎn)角度和，線，咚信息，并通過 無線與主站通訊;等障礙物消失后，主站通過無線與ARM通訊，重新計(jì)算對(duì)新位置的旋轉(zhuǎn)角 度H劣，然后DSP控制LM628按照?qǐng)D7的運(yùn)動(dòng)曲線重新經(jīng)過三段運(yùn)動(dòng)軌跡:加速運(yùn)動(dòng)、勻 速運(yùn)動(dòng)和減速運(yùn)動(dòng)，最終到達(dá)設(shè)定點(diǎn)。
[0034] 9)在移動(dòng)過程中，DSP時(shí)刻記錄加速度計(jì)反饋的大臂、小臂和手腕旋轉(zhuǎn)的角加速 度，控制器通過二次積分得到大臂、小臂和手腕的旋轉(zhuǎn)角度，并與設(shè)定的位置角度值相比 較，如果偏差大于設(shè)定閥值，DSP把這個(gè)偏差轉(zhuǎn)化為三臺(tái)永磁直流伺服電機(jī)新的加速度、速 度和位置初始指令值，在下一個(gè)采樣周期，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 波控制信號(hào)，PWM波控制信號(hào)經(jīng)驅(qū)動(dòng)橋放大后驅(qū)動(dòng)各個(gè)電機(jī)運(yùn)動(dòng)。DSP控制器根據(jù)輸入偏差 大小實(shí)時(shí)調(diào)整LM628內(nèi)部伺服控制程序的PID參數(shù)，LM628控制器通過調(diào)整永磁直流伺服電 機(jī)的驅(qū)動(dòng)脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度，通過調(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)過程中，DSP會(huì)時(shí)刻儲(chǔ)存所經(jīng)過的SCARA機(jī)器人所處的位置 或者是經(jīng)過的參考點(diǎn)，并根據(jù)這些距離信息由DSP計(jì)算得到相對(duì)下一個(gè)參考點(diǎn)SCARA機(jī)器人 永磁直流伺服電機(jī)X、永磁直流伺服電機(jī)Y、永磁直流伺服電機(jī)Z分別要運(yùn)行的角度、角度速 度和角加速度，LM628結(jié)合電機(jī)X、電機(jī)Y和電機(jī)Z電機(jī)光電編碼器反饋的反饋，經(jīng)內(nèi)部伺服調(diào) 節(jié)程序生成電機(jī)X、電機(jī)Y和電機(jī)Z的PffM波控制信號(hào)，PffM波控制信號(hào)經(jīng)驅(qū)動(dòng)橋放大后驅(qū)動(dòng)各 個(gè)電機(jī)運(yùn)動(dòng)。DSP控制器根據(jù)輸入偏差大小實(shí)時(shí)調(diào)整LM628內(nèi)部伺服控制程序的PID參數(shù)，通 過調(diào)整永磁直流伺服電機(jī)的驅(qū)動(dòng)脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度，通過調(diào)整永磁直流伺服電機(jī)控 制信號(hào)的頻率實(shí)現(xiàn)角速度快慢的變化，使SCARA機(jī)器人按照設(shè)定速度快速前行。
[0036] 11)在SCARA機(jī)器人完成三軸旋轉(zhuǎn)角度H今的伺服控制后，DSP二次檢測加速 度傳感器的積分值，如果發(fā)現(xiàn)SCARA機(jī)器人在運(yùn)動(dòng)過程受到外界干擾后三個(gè)旋轉(zhuǎn)角度^，咚， 砵與設(shè)定角度的誤差超過設(shè)定閥值時(shí)，CXD圖像采集系統(tǒng)實(shí)時(shí)采集搬運(yùn)信息，經(jīng)ARM處理后 通過無線裝置向總站發(fā)出圖像傳輸請(qǐng)求，總站通過圖像對(duì)比技術(shù)自動(dòng)檢測搬運(yùn)結(jié)果，如果 確實(shí)存在一定誤差，總站通過無線裝置開始與DSP通訊，DSP把三個(gè)旋轉(zhuǎn)角度偏差，Ag， 轉(zhuǎn)化為三臺(tái)永磁直流伺服電機(jī)位置微調(diào)的加速度、速度和位置初始指令值，然后DSP與 LM6 28通訊，LM6 28結(jié)合電機(jī)X、電機(jī)Y和電機(jī)Z電機(jī)光電編碼器反饋的反饋，經(jīng)LM6 28內(nèi)部伺服 調(diào)節(jié)程序生成電機(jī)X、電機(jī)Y和電機(jī)Z的PffM波控制信號(hào)，PWM波控制信號(hào)經(jīng)驅(qū)動(dòng)橋放大后驅(qū)動(dòng) 各個(gè)電機(jī)運(yùn)動(dòng)。DSP控制器根據(jù)輸入偏差大小實(shí)時(shí)調(diào)整LM628內(nèi)部伺服控制程序的PID參數(shù)， LM628控制器通過調(diào)整永磁直流伺服電機(jī)的驅(qū)動(dòng)脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度，通過調(diào)整永磁 直流伺服電機(jī)控制信號(hào)的頻率實(shí)現(xiàn)角速度快慢的變化，通過三軸永磁直流伺服電機(jī)的再次 工作使機(jī)器人大臂、小臂和手腕到達(dá)設(shè)定位置。
[0037] 12)當(dāng)SCARA機(jī)器人的大臂、小臂和手腕完成角度螂，蜱，咚的角度補(bǔ)償?shù)竭_(dá)設(shè)定位 置后，DSP按照?qǐng)D7的速度時(shí)間曲線，把手腕要升降的距離七轉(zhuǎn)化為永磁直流伺服電機(jī)R的加 速度、速度和位置初始指令值，然后DSP與LM628通訊，LM628結(jié)合電機(jī)R光電編碼器的反饋， 經(jīng)內(nèi)部伺服調(diào)節(jié)程序生成電機(jī)R的ΠΜ波控制信號(hào)，PBi波控制信號(hào)經(jīng)驅(qū)動(dòng)橋放大后驅(qū)動(dòng)各 個(gè)電機(jī)運(yùn)動(dòng)。DSP控制器根據(jù)輸入偏差大小實(shí)時(shí)調(diào)整LM6 28內(nèi)部伺服控制程序的PID參數(shù)， DSP控制器通過調(diào)整永磁直流伺服電機(jī)的驅(qū)動(dòng)脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角度，通過調(diào)整永磁直 流伺服電機(jī)控制信號(hào)的頻率實(shí)現(xiàn)角速度快慢的變化，使手腕平穩(wěn)在設(shè)定時(shí)間內(nèi)到達(dá)設(shè)定位 置。
[0038] 13)在運(yùn)動(dòng)過程中如果SCARA機(jī)器人發(fā)現(xiàn)位置參數(shù)求解出現(xiàn)死循環(huán)將向ARM發(fā)出中 斷請(qǐng)求，ARM會(huì)對(duì)中斷做第一時(shí)間響應(yīng)，ARM控制器將立即與DSP通訊，DSP立即通過LM628封 鎖四臺(tái)永磁直流伺服電機(jī)的控制信號(hào)，然后機(jī)器人原地自鎖，ARM控制器通過無線裝置與主 站進(jìn)行通訊，CCD時(shí)刻采集周圍信息并通過無線傳輸，主站對(duì)圖像采集信息進(jìn)行分析，并根 據(jù)采集結(jié)果二次重新啟動(dòng)SCARA機(jī)器人，通過無線裝置二次向SCARA機(jī)器人重新輸入搬運(yù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)中的具體位置，DSP通過無線裝置實(shí)時(shí)向總站發(fā)出數(shù)據(jù)傳輸信息。
[0040] 15)SCARA機(jī)器人在運(yùn)行過程ARM控制器時(shí)刻對(duì)AC交流主電源進(jìn)行監(jiān)控，如果控制 器發(fā)現(xiàn)主電源出現(xiàn)故障突然斷電時(shí)，ARM會(huì)與DSP通訊，并開通備用電源，由備用電源為四軸 永磁直流伺服電機(jī)提供能量，DSP通過LM628內(nèi)部伺服調(diào)節(jié)程序?qū)崟r(shí)調(diào)整四臺(tái)永磁直流伺服 電機(jī)的Pmi輸出，LM628控制器通過調(diào)整永磁直流伺服電機(jī)的驅(qū)動(dòng)脈沖數(shù)目調(diào)整其旋轉(zhuǎn)角 度，通過調(diào)整永磁直流伺服電機(jī)控制信號(hào)的頻率實(shí)現(xiàn)角速度快慢的變化，使SCARA機(jī)器人完 成這次搬運(yùn)任務(wù)，然后DSP控制LM628封鎖四路永磁直流伺服電機(jī)的PffM控制信號(hào)，機(jī)器人原 地自鎖;SCARA機(jī)器人通過ARM與主站通過無線進(jìn)行通訊，通知主站進(jìn)行檢修。
[0041] 16)在SCARA機(jī)器人在搬運(yùn)過程中，如果主站有新的任務(wù)輸入，主站先通過無線裝 置與ARM通訊并傳輸新的工作任務(wù)，ARM控制器計(jì)算出新的伺服控制位置參數(shù)并與DSP通訊， DSP首先完成當(dāng)前的任務(wù)，并更新下一個(gè)伺服控制的位置參數(shù)。
[0042] 17)在SCARA機(jī)器人在搬運(yùn)過程中，主站會(huì)對(duì)基于CCD的采集圖像進(jìn)行自動(dòng)分析對(duì) 比，如發(fā)現(xiàn)問題某一位置存在問題時(shí)，主站先通過無線裝置與DSP通訊，DSP首先通過LM628 完成當(dāng)前任務(wù)并在設(shè)定零位置停車，有主站自動(dòng)檢測搬運(yùn)結(jié)果，自動(dòng)檢測完畢后通過ARM更 新DSP下一個(gè)伺服控制的位置參數(shù)。
[0043] 18)在SCARA機(jī)器人多軸伺服系統(tǒng)工作過程中，如果DSP伺服控制器檢測到某一個(gè) 永磁直流伺服電機(jī)的轉(zhuǎn)矩出現(xiàn)脈動(dòng)，由于本發(fā)明采用的永磁直流伺服電機(jī)力矩與電機(jī)電流 成正比例，因此控制器會(huì)很容易補(bǔ)償此干擾，并根據(jù)干擾大小DSP實(shí)時(shí)調(diào)整LM628內(nèi)部的PID 參數(shù)，減少了電機(jī)轉(zhuǎn)矩?cái)_動(dòng)對(duì)SCARA機(jī)器人運(yùn)動(dòng)過程的影響。
[0044] 19)當(dāng)SCARA機(jī)器人完成任務(wù)，實(shí)現(xiàn)位置歸零時(shí)，其攜帶的加速度傳感器A1~A3會(huì)時(shí) 刻檢測其加速度，當(dāng)加速度超過預(yù)設(shè)閥值時(shí)，在下一個(gè)周期DSP控制器會(huì)通過LM628修正上 一個(gè)周期帶來的誤差，同時(shí)基于CCD圖像采集系統(tǒng)實(shí)時(shí)工作，并引導(dǎo)機(jī)器人自動(dòng)導(dǎo)引到機(jī)器 人設(shè)定零位置，SCARA機(jī)器人按照?qǐng)D7的速度-時(shí)間運(yùn)動(dòng)曲線實(shí)現(xiàn)從某一點(diǎn)到零位的歸位，然 后零位自鎖，等待下一個(gè)搬運(yùn)命令。
[0045] 以上所述僅為本發(fā)明的實(shí)施例，并非因此限制本發(fā)明的專利范圍，凡是利用本發(fā) 明說明書內(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和加速度傳感器Al，所述機(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均與所 述集成芯片處理器通信連接，所述避障位移傳感器S1、避障位移傳感器S2、避障位移傳感器 S3、避障位移傳感器S4、定位傳感器S5、加速度傳感器Al、加速度傳感器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è)置有光電編碼器，所述光電編碼器與所述集成芯片處理器電性連接。4. 根據(jù)權(quán)利要求1至3任一項(xiàng)所述的教學(xué)用無線傳輸三核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)，其 特征在于，所述圖像采集模塊為CCD攝像頭組件。5. 根據(jù)權(quán)利要求4所述的教學(xué)用無線傳輸三核快速關(guān)節(jié)機(jī)器人控制系統(tǒng)，其特征在于， 所述圖像采集模塊內(nèi)設(shè)置有解碼芯片。
【文檔編號(hào)】B25J9/18GK105922270SQ201610409059
【公開日】2016年9月7日
【申請(qǐng)日】2016年6月13日
【發(fā)明人】張好明
【申請(qǐng)人】江蘇若博機(jī)器人科技有限公司