一種教學用無線傳輸雙核常速關節(jié)機器人控制系統(tǒng)的制作方法
【專利摘要】本發(fā)明公開了一種教學用無線傳輸雙核常速關節(jié)機器人控制系統(tǒng)，所述的控制系統(tǒng)包括主站控制計算機和控制器，所述控制器包括DSP芯片控制器、圖像采集模塊、無線傳輸模塊和ARM控制器，DSP芯片控制器和ARM控制器均與主站控制計算機通信連接，DSP芯片控制器、圖像采集模塊、無線傳輸模塊與ARM控制器之間通信連接，四臺永磁直流伺服電機均與DSP芯片控制器通信連接，多個避障位移傳感器、定位傳感器、加速度傳感器均與所述DSP芯片控制器和ARM控制器通信連接。本發(fā)明的教學用無線傳輸雙核常速關節(jié)機器人控制系統(tǒng)計算速度快，使關節(jié)機器人手臂轉向靈活、動作穩(wěn)定精確、體積小巧，性能穩(wěn)定，可以遠距離無線監(jiān)控機器人操作。
【專利說明】
一種教學用無線傳輸雙核常速關節(jié)機器人控制系統(tǒng)
技術領域
[0001] 本發(fā)明涉及一種教學用無線傳輸雙核常速關節(jié)機器人控制系統(tǒng)，屬于裝配作業(yè) 的四關節(jié)機器人手臂應用領域。
【背景技術】
[0002] 在工業(yè)生產(chǎn)中，工業(yè)機器人可以代替人類做一些比較單調、比較頻繁和重復率較 高的長時間作業(yè)，或是在危險、惡劣環(huán)境下的作業(yè)，一般用作搬取零件和裝配工作，在微電 子制造業(yè)、塑料工業(yè)、汽車工業(yè)、電子產(chǎn)品工業(yè)、藥品工業(yè)和食品工業(yè)等領域得到廣泛的應 用，它對于提高生產(chǎn)自動化水平、勞動生產(chǎn)率和經(jīng)濟效益、保證產(chǎn)品質量、保障人身安全、改 善勞動環(huán)境，減輕勞動強度、節(jié)約原材料消耗以及降低生產(chǎn)成本等有著十分重要的意義。 [0003] SCARA工業(yè)機器人即裝配作業(yè)的機器人手臂是一種圓柱坐標型的工業(yè)機器人，它 依靠旋轉關節(jié)大臂和小臂來實現(xiàn)X-Y平面內的快速定位，依靠一個手腕移動關節(jié)和一個手 腕旋轉關節(jié)在Z方向上做伸縮和旋轉運動，其具有四個運動自由度，該系列的操作手在其動 作空間的四個方向具有有限剛度，而在剩下的其余兩個方向上具有無限大剛度。這種結構 特性使得SCARA機器人擅長從一點抓取物體，然后快速的安放到另一點，因此SCARA機器人 在自動裝配生產(chǎn)線上得到了廣泛的應用。SCARA機器人結構緊湊、動作靈活，速度快、位置精 度高，它的使用大大提高了機器人對復雜裝配任務的適應性，同時也降低了成本，提高了工 作空間利用率。
[0004] SCARA機器人結合了多學科知識，對于提升在校學生的動手能力、團隊協(xié)作能力和 創(chuàng)新能力，促進學生課堂知識的消化和擴展學生的知識面都非常有幫助。教學用SCARA機器 人技術的開展可以培養(yǎng)大批相關領域的人才，進而促進相關領域的技術發(fā)展和產(chǎn)業(yè)化進 程。但是SCARA機器人在搬運貨物過程中要時刻判斷主控制器輸入的位置參數(shù)，并判斷周圍 的環(huán)境時刻避障，然后由運動控制器反復控制其精確的加速和減速進行搬運貨物，稍微的 誤差累計就有可能在多回合運動中導致運輸失敗。國內對SCARA機器人的使用雖然有幾十 年，但是由于國內工業(yè)機器人發(fā)展起步比較晚，受較多關鍵技術的影響，SCARA機器人的發(fā) 展也受所影響，傳統(tǒng)的機器人原理如圖1所示，在長期使用期間出現(xiàn)眾多問題： (1)對于采用步進電機和直流電機驅動的SCARA機器人來說，系統(tǒng)的轉動慣量大，不利 于機器人的快速加速和減速。
[0005] (2)對于采用步進電機和直流電機驅動的SCARA機器人來說，系統(tǒng)的啟動力矩較 小，不利于機器人的快速啟動。
[0006] (3)在SCARA機器人的整個運動控制過程，旋轉的各個角度沒有實時反饋，時有造 成三軸不同步的現(xiàn)象發(fā)生。
[0007] (4)在SCARA機器人的整個運動控制過程，各個旋轉臂與外圍環(huán)境之間沒有進行有 效的避障，有時會造成碰撞其他貨物的現(xiàn)象發(fā)生。
[0008] (5)在大多數(shù)情況下，隨著時間的積累，機器人工作會存在一定的位置誤差，依靠 人工引導到零位置的方法，不僅精確度不高，而且也不利于高密度搬運等工作。
[0009] (6)在SCARA大批量搬運過程中，搬運誤差和質量問題時有發(fā)生，如果不將類似問 題檢查出來，會為以后的運動過程留下隱患。目前對于SCARA工作的檢測，主要依靠人工目 測等檢驗方法實現(xiàn)，這不僅存在主觀因素影響大、勞動強度高、工作效率低等弊端，而目在 短時間內人工大批量檢驗SCARA工作結果也不現(xiàn)實。
[0010] (7)在SCARA大批量搬運生產(chǎn)中，有時會有重要的臨時任務加入，普通的SCARA往往 通過停機的方式完成，造成效率較低。

【發(fā)明內容】

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