一種重載無線傳輸五核高速關節機器人控制系統的制作方法

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

【發明內容】

[0009]本發明主要解決的技術問題是提供一種重載無線傳輸五核高速關節機器人控制 系統，該重載無線傳輸五核高速關節機器人控制系統計算速度快，使關節機器人手臂轉向 靈活、動作穩定精確、承載力大，性能穩定，便于無線監控，機器人的智能程度高。
[0010]為解決上述技術問題，本發明采用的技術方案是：提供一種重載無線傳輸五核高 速關節機器人控制系統，所述關節機器人采用稀土永磁同步伺服電機X驅動機器人大臂旋 轉運動、采用稀土永磁同步伺服電機Y驅動機器人小臂旋轉運動、采用稀土永磁同步伺服電 機Z驅動機器人手腕旋轉運動、采用稀土永磁同步伺服電機R驅動機器人手腕升降運動，所 述機器人大臂上安裝有避障位移傳感器S1、避障位移傳感器S2、陀螺儀和加速度傳感器A1， 所述機器人小臂上安裝有避障位移傳感器S3、避障位移傳感器S4、陀螺儀和加速度傳感器 A2,所述機器人手腕上安裝有定位傳感器S5、陀螺儀和加速度傳感器A3,所述的控制系統包 括主站控制計算機和控制器，所述控制器包括DSP芯片控制器、P1C控制器、可編程邏輯芯片 控制器、語音識別處理器、圖像采集模塊、無線傳輸模塊和ARM控制器，所述圖像采集模塊與 無線傳輸模塊之間通信連接，所述DSP芯片控制器、可編程邏輯芯片控制器、語音識別處理 器和P1C控制器均與所述主站控制計算機通信連接，所述DSP芯片控制器、可編程邏輯芯片 控制器、語音識別處理器和ARM控制器彼此通信連接，所述無線傳輸模塊和ARM控制器通信 連接，所述P1C控制器與ARM控制器通信連接，所述稀土永磁同步伺服電機X、稀土永磁同步 伺服電機Y、稀土永磁同步伺服電機Z和稀土永磁同步伺服電機R均與所述DSP芯片控制器通 信連接，所述避障位移傳感器S1、避障位移傳感器S2、避障位移傳感器S3、避障位移傳感器 S4、定位傳感器S5、各陀螺儀、加速度傳感器A1、加速度傳感器A2和加速度傳感器A3均同時 與所述DSP芯片控制器和ARM控制器通信連接。
[0011]在本發明一個較佳實施例中，還包括為所述關節機器人和控制系統提供電源的主 電源和備用電源，所述關節機器人中的各個所述電機和傳感器以及控制系統中的控制器均 與所述主電源和/或備用電源電性連接。
[0012]在本發明一個較佳實施例中，所述稀土永磁同步伺服電機X、稀土永磁同步伺服電 機Y、稀土永磁同步伺服電機Z和稀土永磁同步伺服電機R上勻設置有光電編碼器，所述光電 編碼器與所述DSP芯片控制器電性連接。
[0013] 在本發明一個較佳實施例中，所述圖像采集模塊為CCD攝像頭組件。
[0014] 在本發明一個較佳實施例中，所述圖像采集模塊內設置有解碼芯片。
[0015] 在本發明一個較佳實施例中，所述機器人大臂上安裝有磁電傳感器EM1，所述機器 人小臂上安裝有磁電傳感器EM2,所述機器人手腕上安裝有磁電傳感器EM3和EM4,所述磁電 傳感器EM1、磁電傳感器EM2、磁電傳感器EM3和EM4均與所述DSP芯片控制器和ARM控制器通 信連接。
[0016] 本發明的有益效果是:本發明的重載無線傳輸五核高速關節機器人控制系統計算 速度快，使關節機器人手臂轉向靈活、動作穩定精確、承載力大，性能穩定，便于無線監控， 機器人的智能程度高，無線傳輸系統的加入使得主站可以實時了解工作結果，同時主站可 以隨時根據實際需要實時更改SCARA機器人工作任務。
【附圖說明】
[0017] 為了更清楚地說明本發明實施例中的技術方案，下面將對實施例描述中所需要使 用的附圖作簡單地介紹，顯而易見地，下面描述中的附圖僅僅是本發明的一些實施例，對于 本領域普通技術人員來講，在不付出創造性勞動的前提下，還可以根據這些附圖獲得其它 的附圖，其中： 圖1為傳統SCARA機器人控制器原理圖； 圖2為基于四軸稀土永磁同步伺服電機SCARA機器人二維結構圖； 圖3為可編程邏輯芯片控制器內部三閉環伺服控制原理圖； 圖4為基于五核四軸稀土永磁同步伺服電機SCARA機器人控制器原理圖； 圖5為基于五核四軸稀土永磁同步伺服電機SCARA機器人程序框圖； 圖6為基于五核四軸稀土永磁同步伺服電機運動原理圖； 圖7為四軸電機加減速曲線圖； 圖8為本發明的重載無線傳輸五核高速關節機器人控制系統結構示意圖。
[0018] 附圖中各部件的標記如下：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、加速度傳感器Α1，18、加速度傳感器Α2，19、加速度傳感器A3，20、PLC控制器， 21、磁電傳感器ΕΜ1，22、磁電傳感器ΕΜ2，23、磁電傳感器ΕΜ3，24、磁電傳感器ΕΜ4，25、無線傳 輸模塊，26、圖像采集模塊，27、解碼芯片，28、語音識別處理器，29、陀螺儀，30、可編程邏輯 芯片控制器。
【具體實施方式】
[0019] 下面將對本發明實施例中的技術方案進行清楚、完整地描述，顯然，所描述的實施 例僅是本發明的一部分實施例，而不是全部的實施例。基于本發明中的實施例，本領域普通 技術人員在沒有做出創造性勞動前提下所獲得的所有其它實施例，都屬于本發明保護的范 圍。
[0020] 請參閱圖2至圖8,本發明實施例包括:一種重載無線傳輸五核高速關節機器人控 制系統，該機器人為基于四軸稀土永磁同步伺服電機的SCARA機器人，即所述關節機器人采 用稀土永磁同步伺服電機X7驅動機器人大臂旋轉運動、采用稀土永磁同步伺服電機Y8驅動 機器人小臂旋轉運動、采用稀土永磁同步伺服電機Z9驅動機器人手腕旋轉運動、采用稀土 永磁同步伺服電機R10驅動機器人手腕升降運動，所述機器人大臂上安裝有避障位移傳感 器S1 12、避障位移傳感器S2 13、陀螺儀29和加速度傳感器A1 17,所述機器人小臂上安裝 有避障位移傳感器S3 14、避障位移傳感器S4 15、陀螺儀29和加速度傳感器A2 18,所述機 器人手腕上安裝有定位傳感器S5 16、陀螺儀29和加速度傳感器A3 19。
[0021] 所述的控制系統包括主站控制計算機1和控制器2,所述控制器2包括DSP芯片控制 器3、語音識別處理器28、可編程邏輯芯片控制器30、P1C控制器20、圖像采集模塊26、無線傳 輸模塊25和ARM控制器4,所述圖像采集模塊26與無線傳輸模塊25之間通信連接，所述DSPS 片控制器3、可編程邏輯芯片控制器30、語音識別處理器28和P1C控制器20均與所述主站控 制計算機1通信連接，所述DSP芯片控制器3、可編程邏輯芯片控制器30、語音識別處理器28 和ARM控制器4彼此通信連接，所述無線傳輸模塊25和ARM控制器4通信連接，所述P1C控制器 20與ARM控制器4通信連接，所述圖像采集模塊26內設置有解碼芯片27。
[0022] 所述稀土永磁同步伺服電機X7、稀土永磁同步伺服電機Y8、稀土永磁同步伺服電 機Z9和稀土永磁同步伺服電機R10均與所述DSP芯片控制器3通信連接，所述避障位移傳感 器S1 12、避障位移傳感器S2 13、避障位移傳感器S3 14、避障位移傳感器S4 15、定位傳感 器S5 16、陀螺儀29、加速度傳感器A1 17、加速度傳感器A2 18和加速度傳感器A3 19均同時 與所述DSP芯片控制器3、PLC控制器20和ARM控制器4通信連接。
[0023]優選地，本發明的重載無線傳輸五核高速關節機器人控制系統還包括為所述關節 機器人和控制系統提供電源的主電源5和備用電源6,所述關節機器人中的各個所述電機和 傳感器以及控制系統中的控制器2均與所述主電源5和/或備用電源6電性連接。
[0024] 優選地，所述稀土永磁同步伺服電機X 7、稀土永磁同步伺服電機Y 8、稀土永磁同 步伺服電機Z 9和稀土永磁同步伺服電機R10上勻設置有光電編碼器11，所述光電編碼器11 與所述DSP芯片控制器3電性連接。
[0025]優選地，所述機器人大臂上安裝有磁電傳感器EM1 21，所述機器人小臂上安裝有 磁電傳感器EM2 22,所述機器人手腕上安裝有磁電傳感器EM3 23和EM4 24,所述磁電傳感 器EM1 21、磁電傳感器EM2 22、磁電傳感器EM3 23和EM4 24均與所述DSP芯片控制器3和ARM 控制器4通信連接。這些磁電傳感器分別讀取各自的零位設置標志，當四者均探測到信號 時，SCARA機器人實現精確復位，提高了復位的精確度。
[0026] 本發明采用DSP控制器3(TMS320F2812)+PLC控制器20+語音識別處理器28(芯片型 號LD3320H可編程邏輯芯片控制器30(即CPLD控制器)+ARM控制器4(STM32F746)五核進行 系統控制。在電源打開狀態下，ARM控制器先動態編輯LD3320的關鍵詞語列表，增加機器人 語音識別率，然后對機器人備用電源S0C(荷電狀態）和主電源進行判斷，如果備用能源較 低，控制器將會通過無線裝置向總站發出報警信號;如果備用電源和主電源工作正常，先由 總站通過PLC控制器或者基于LD3320語音識別系統把搬運貨物大臂、小臂和手腕旋轉角度 以及升降高度等信息輸入給ARM，由ARM計算出機器人伺服系統的參數隊列;SCARA機器人攜 帶的零位置傳感器EM1、EM2、EM3、EM4和基于C⑶的圖像采集系統開始工作，使機器人復位到 設定零位置，機器人進入自鎖狀態;一旦搬運命令開始后，機器人攜帶的避障傳感器、定位 傳感器和加速度傳感器均開啟，SCARA機器人按照設定的優化搬運路徑快速搬運，CPLD按照 伺服參數和傳感器反饋實時調整SCARA機器人稀土永磁同步伺服電機X、稀土永磁同步伺服 電機Y、稀土永磁同步伺服電機Z和稀土永磁同步伺服電機R的PWM輸出，實現四臺稀土永磁 同步伺服電機的實時伺服控制，CCD攝像頭實時檢測搬運結果，ARM對數字圖像的處理和存 儲并通過無線系統傳輸，主站控制計算機1通過圖像對比技術自動檢測搬運結果;如果ARM 對搬運某一個位置有疑問，將與DSP通訊，DSP禁止CPLD工作，四軸SCARA機器人的PWM控制信 號被封鎖，機器人原地自鎖，然后主站通過圖像對比對搬運信息進行判斷，確認無誤后通過 無線裝置二次啟動SCARA機器人繼續未完成的任務;在機器人運動過程中，如果遇到緊急情 況，主站將通過麥克風裝置向控制器輸入控制命令，語音識別器LD3320識別后與ARM通訊， 有ARM處理后與DSP通訊，DSP通過CPLD把主站命令轉化為各個稀土永磁同步伺服電機的控 制信號。
[0027] 參照圖1，圖2,圖3和圖4、圖5、圖6、圖7,其具體的功能實現如下： 1)SCARA機器人電源打開后，為了提高機器人語音識別效果，進一步降低誤識別率，ARM 控制器先向LD3320輸入SCARA機器人常用的一些命令和指令，用來吸收錯誤識別，從而達到 降低LD3320誤識別率的目的。
[0028] 2)ARM先對備用電源S0C和主電源狀態進行判斷，如果備用電源S0C較低時，DSP將 禁止四臺稀土永磁同步伺服電機工作，電機輸入PWM波被封鎖，同時報警傳感器將工作并發 出報警信號;如果電池 S0C正常，SCARA機器人進入待工作狀態，等待工作命令。
[0029] 3)-旦總站工作命令開始，總站可以選擇基于LD3320語音識別系統或者是PLC控 制器與ARM通訊，任何一個控制器都可以把大臂、小臂長度和升降桿的長度等信息傳入到 ARM控制器，然后機器人開始校正零位置，SCARA機器人攜帶的磁電傳感器EMI、EM2、EM3、EM4 和基于CCD的圖像采集系統開始工作，并各自尋找設定的零位置標志，當磁電傳感器EMI、 EM2、EM3和EM4均有信號輸出時，ARM通過無線系統實時傳輸(XD采集圖像，總站通過圖像對 比技術確定位置無誤后，通過無線裝置與DSP通訊，DSP通過CPLD封鎖四路稀土永磁同步伺 服電機的P麗波控制信號，SCARA機器人自動引導零位置復位完成，ARM和DSP控制器此時設 定各個旋轉角度咚= ? ，手腕上升高度A := ?。
[0030] 4)為了滿足SCARA機器人的加減速需要，本發明采用如圖7的運動時間梯形圖，此 梯形圖包含的面積就是機器人大臂、小臂和手腕要旋轉的角度或者是手腕升降的高度，為 了方便控制，本發明采用單一加速度模式。
[0031] 5) SCARA機器人讀取其工作模式，如果是人工工作模式，主站開始通過PLC或者是 基于LD3320的語音識別系統與ARM控制器通訊，由主站輸入SCARA機器人大臂、小臂和手腕 需要旋轉的角度成肩，巧以及SCARA機器人手腕需要上升或者是下降的高度七給ARM控制 器，ARM控制器根據Denavi t-Hartenberg算法開始機器人正向求解:ARM控制器首先根據 SCARA機器人大臂、小臂和手腕需要旋轉的角度4，咚以及SCARA機器人手腕需要上升 的高度，計算出相鄰兩桿件坐標系之間的位姿矩陣~2并用4*4二維數 組標志，辦Β? ，財33..，^^51...分別表不如下：
[0032] 然后通過公式就可以求出手腕執行器完成任務后的位置和 姿態，然后ARM與DSP通訊，并傳輸人工輸入參數。
[0033] 6)SCARA機器人讀取其工作模式，如果是正常自動搬運工作模式，主站開始通過 PLC或者是基于LD3320的語音識別系統與ARM控制器通訊，由主站輸入SCARA機器人大臂、小 臂和手腕所處的初始位置和給定位置三維坐標給ARM控制器，ARM控制器根據Denavit-Hartenberg算法開始機器人逆向求解:ARM控制器首先根據大臂、小臂長度以及最終的三維 坐標中的X和Y坐標求出大臂需要旋轉的角度蜷，并借助蜱的值求出％的值，并根據三維坐 標中的Z坐標求出手腕上升或降低的高度最終求出旋轉角度，由于在求解咚的時候 方程具有雙解，所以SCARA機器人求出大臂、小臂和手腕需要旋轉的角度蜱，咚，砵以及 SCARA機器人手腕需要上升的高度4后，ARM控制器會對求解結果進行優化，然后ARM控制 器與DSP通訊，把機器人最有伺服運動參數傳輸給DSP控制器。
[0034] 7)DSP控制器接受SCARA機器人大臂、小臂和手腕需要旋轉的角度蜷，蠔，巧以及 SCARA機器人手腕需要上升的高度，后，大臂、小臂和手腕上的傳感器S1~S5、EM1、EM2和 EM3將開啟，首先SCARA機器人要對零位置進行判斷，確認初始位置無誤后，SCARA機器人對 各個旋轉臂旋轉角度內的障礙物進行判斷，如存在障礙物將向DSP發出中斷請求，DSP會對 中斷做第一時間響應，然后DSP禁止CPLD工作，四軸稀土永磁同步伺服電機P麗波控制信號 被封鎖，SCARA機器人禁止稀土永磁同步伺服電機X、稀土永磁同步伺服電機Y、稀土永磁同 步伺服電機Z和電機稀土永磁同步伺服電機R工作，機器人原地自鎖，基于CCD的圖像采集系 統開啟，ARM處理后通過無線裝置向總站發出中斷請求，為了防止信息誤判，總站通過無線 傳輸圖像二次判斷運動范圍內的障礙物信息，由總站人為處理障礙物，防止障礙物影響搬 運工作。
[0035] 8)如果總站二次通過圖像采集結果確定無障礙物進入運動區域，將通過無線裝置 與DSP通訊，按照圖7的速度時間曲線，DSP把三個旋轉角度成，蜷，巧轉化為三臺稀土永磁 同步伺服電機的加速度、速度和位置初始指令值，然后DSP與CPLD通訊，CPLD結合電機X、電 機Y和電機Z電流反饋、加速度傳感器、陀螺儀和光電編碼器的反饋，經CPLD內部伺服調節程 序生成電機X、電機Y和電機Z的PWM波控制信號，經驅動橋驅動各個電機運動。CPLD控制器通 過調整稀土永磁同步伺服電機的驅動脈沖數目調整其旋轉角度，通過調整稀土永磁同步伺 服電機控制信號的頻率實現角速度快慢的變化，CPLD根據三閉環輸入的偏差大小實時調整 內部伺服控制程序的PID參數，使三軸稀土永磁同步伺服電機時刻同步工作，DSP時刻記錄 機器人已經運動的位置參數。
[0036] 9)在SCARA機器人移動過程中，傳感器S1~S4時刻對運動范圍內的移動障礙物進行 判斷，如果有障礙物進入運動范圍，DSP立即通過CPLD使SCARA機器人的大臂、小臂和手腕按 照圖7的減速運動曲線立即停車，DSP控制器記錄下當前旋轉角度今，信息，主站通過 無線裝置向DSP輸入新位置的旋轉角度成，名，g，然后DSP控制CPLD按照圖7的運動曲線重 新經過三段運動軌跡:加速運動、勾速運動和減速運動，最終到達設定點。
[0037] 10)在移動過程中，DSP時刻記錄加速度計反饋的大臂、小臂和手腕旋轉的角加速 度，并通過二次積分得到大臂、小臂和手腕的旋轉角度，并與設定的位置角度值相比較，如 果偏差大于設定閥值，DSP把這個偏差轉化為三臺稀土永磁同步伺服電機新的加速度、速 度和位置初始指令值，在下一個采樣周期，DSP與CPLD通訊，CPLD結合電機X、電機Y和電機Z 電機電流反饋、加速度傳感器、陀螺儀和光電編碼器的反饋，經CPLD內部伺服調節程序生成 電機X、電機Y和電機Z的PWM波控制信號，經驅動橋驅動各個電機運動。CPLD控制器根據三閉 環輸入的偏差大小實時調整內部伺服控制程序的PID參數，CPLD控制器通過調整稀土永磁 同步伺服電機的驅動脈沖數目調整其旋轉角度，通過調整稀土永磁同步伺服電機控制信號 的頻率實現角速度快慢的變化，進而消除上一個采樣周期產生的誤差，使SCARA機器人按照 設定軌跡完成任務。
[0038] 11)在SCARA機器人運動過程中，DSP會時刻儲存所經過的SCARA機器人所處的位置 或者是經過的參考點，并根據這些距離信息由DSP計算得到相對下一個參考點SCARA機器人 稀土永磁同步伺服電機X、稀土永磁同步伺服電機Y、稀土永磁同步伺服電機Z分別要運行的 角度、角度速度和角加速度，CPLD結合電機X、電機Y和電機Z電流反饋、加速度傳感器、陀螺 儀和光電編碼器的反饋，經CPLD內部伺服調節程序生成電機X、電機Y和電機Z的ΠΜ波控制 信號，經驅動橋驅動各個電機運動。CPLD控制器根據三閉環輸入的偏差大小實時調整內部 伺服控制程序的PID參數，CPLD通過調整稀土永磁同步伺服電機的驅動脈沖數目調整其旋 轉角度，通過調整稀土永磁同步伺服電機控制信號的頻率實現角速度快慢的變化，使SCARA 機器人按照設定速度快速前行。
[0039] 12)在SCARA機器人運動過程中，ARM和DSP實時記錄機器人大臂、小臂和手腕旋轉 的角度，如果總站發現機器人運行速度較慢，可以通過LD3320語音識別系統向ARM輸入加速 命令，ARM處理后與DSP通訊，CPLD根據加速時間要求，把剩余的角度轉化為稀土永磁同步伺 服電機X、稀土永磁同步伺服電機Y、稀土永磁同步伺服電機Z分別要運行的角度、角度速度 和角加速度指令，CPLD控制器再結合電機電流反饋、加速度傳感器、陀螺儀和光電編碼器的 反饋，根據其內部三閉環基于％ = ?的矢量控制伺服程序重新調整三臺稀土永磁同步伺服 電機的PWM控制信號信號，使機器人快速完成任務。
[0040] 13)在SCARA機器人運動過程中，ARM和DSP實時記錄機器人大臂、小臂和手腕旋轉 的角度，如果總站發現機器人需要緊急檢修，可以通過LD3320語音識別系統向ARM輸入停車 命令，ARM處理后與DSP通訊，CPLD根據停車時間要求，再結合電機電流反饋、加速度傳感器、 陀螺儀和光電編碼器的反饋，根據其內部三閉環基于的矢量控制伺服程序重新調整 三臺稀土永磁同步伺服電機的PWM控制信號信號，使機器人迅速完成停車，DSP控制器通過 加速度和陀螺儀傳感器實時記錄機器人旋轉的角度并儲存。主站通過圖像對比檢測完畢 后，通過LD3320語音識別系統向ARM輸入啟動命令，ARM處理后與DSP通訊，CPLD根據加速時 間要求，把剩余的角度轉化為稀土永磁同步伺服電機X、稀土永磁同步伺服電機Y、稀土永磁 同步伺服電機Z分別要運行的角度、角度速度和角加速度指令，CPLD控制器再結合電機電流 反饋、加速度傳感器、陀螺儀和光電編碼器的反饋，根據其內部三閉環基于％ = ?的矢量控 制伺服程序重新調整三臺稀土永磁同步伺服電機的PWM控制信號信號，使機器人快速完成 剩余任務。
[0041 ] 14)在SCARA機器人完成三軸旋轉角度和，咚，砵的伺服控制后，DSP二次檢測加速 度傳感器的積分值，如果發現SCARA機器人在運動過程受到外界干擾后三個旋轉角度咚，龜 ，或與設定角度的誤差超過設定閥值時，C⑶圖像采集系統實時采集搬運信息，經ARM處理后 通過無線裝置向總站發出圖像傳輸請求，總站通過圖像對比技術自動檢測搬運結果，DSP把 三個旋轉角度偏差Δ內，轉化為三臺稀土永磁同步伺服電機位置微調的加速度、 速度和位置初始指令值，然后DSP與CPLD通訊，CPLD結合電機X、電機Y和電機Z電流反饋、加 速度傳感器、陀螺儀和光電編碼器的反饋，經CPLD內部伺服調節程序生成電機X、電機Y和電 機Z的PWM波控制信號，經驅動橋驅動各個電機運動。CPLD控制器根據三閉環輸入的偏差大 小實時調整內部伺服控制程序的PID參數，CPLD控制器通過調整稀土永磁同步伺服電機的 驅動脈沖數目調整其旋轉角度，通過調整稀土永磁同步伺服電機控制信號的頻率實現角速 度快慢的變化，通過三軸稀土永磁同步伺服電機的再次工作使機器人大臂、小臂和手腕到 達設定位置。
[0042] 15)當SCARA機器人的大臂、小臂和手腕完成角度咚，爲，咚的角度補償到達設定 位置后，手腕上的傳感器EM4將再次開啟，首先SCARA機器人要對零位置進行判斷，確認初始 位置無誤后，DSP按照圖7的速度時間曲線，把手腕要升降的距離七轉化為稀土永磁同步伺 服電機R的加速度、速度和位置初始指令值，然后DSP與CPLD通訊，CPLD結合電機R電流反饋、 加速度傳感器、陀螺儀和光電編碼器的反饋，經CPLD內部伺服調節程序生成電機R的PWM波 控制信號，經驅動橋驅動電機R運動。CPLD控制器根據三閉環輸入的偏差大小實時調整內部 伺服控制程序的PID參數，CPLD控制器通過調整稀土永磁同步伺服電機的驅動脈沖數目調 整其旋轉角度，通過調整稀土永磁同步伺服電機控制信號的頻率實現角速度快慢的變化， 使手腕平穩在設定時間內到達設定位置。
[0043] 16)在運動過程中如果SCARA機器人發現位置參數求解出現死循環將向ARM發出中 斷請求，ARM會對中斷做第一時間響應，ARM控制器將立即與DSP通訊，DSP立即通過CPLD封鎖 四臺稀土永磁同步伺服電機的控制信號，機器人原地自鎖，ARM控制器通過無線裝置與主站 進行通訊，CCD時刻采集周圍信息并通過無線傳輸，主站對圖像采集信息進行分析，并根據 采集結果二次重新啟動SCARA機器人，通過PLC控制器或者是LD3320語音識別系統二次向 SCARA機器人輸入搬運信息。
[0044] 17)在SCARA機器人多次反復運動過程中，如果主站發現有臨時重要任務需要加入 工作隊列中，主站將開啟LD3320控制器，由主站通過LD3320控制器與ARM通訊，向ARM輸入重 要任務的位置伺服參數，ARM與DSP通訊，首先由DSP完成當前未完成的任務，同時ARM更新 DSP原有的伺服運動控制參數隊列，并傳輸新的伺服位置參數給DSP，使DSP下一個伺服運動 控制完成重要臨時任務。
[0045] 18)在SCARA機器人多次反復運動過程中，如果當磁電傳感器EM1、EM2、EM3和EM4讀 到傳感信號時，DSP控制器將以零位置參數代替現有的位置參數進行新的位置伺服控制，并 計算出誤差，在下一個采樣周期，DSP通過CPLD內部三閉環稀土永磁同步伺服電機控制程序 對誤差進行補償，及時消除累計誤差。
[0046] 19)在SCARA機器人搬運過程中，主站會對基于CCD的采集圖像進行自動分析對比， 如發現問題某一位置存在問題時，主站先通過無線裝置與DSP通訊，DSP首先控制CPLD完成 當前任務并在設定零位置停車，由主站自動檢測搬運結果，自動檢測完畢后通過ARM更新 DSP下一個伺服控制的位置參數，由DSP控制CPLD實現新位置的伺服控制。
[0047] 20)裝在稀土永磁同步伺服電機X、稀土永磁同步伺服電機Y、稀土永磁同步伺服電 機Ζ、稀土永磁同步伺服電機R上的光電編碼器會輸出其位置信號Α和位置信號Β和脈沖Ζ，光 電編碼器的位置信號A脈沖和B脈沖邏輯狀態每變化一次，DSP和ARM內的位置寄存器會根據 電機的運行方向加1或者是減1;光電編碼器的位置信號A脈沖和B脈沖和Z脈沖同時為低電 平時，就產生一個INDEX信號給DSP和ARM內部寄存器，記錄稀土永磁同步伺服電機的絕對位 置，然后換算成SCARA機器人大臂、小臂或者是手腕在三維坐標系統中的具體位置，ARM控制 器實時通過PLC與主站通訊，把重要的位置參數傳輸給主站。當磁電傳感器EM1、EM2和EM3讀 到傳感信號時，DSP控制器將以零位置參數代替現有的位置參數，及時消除累計誤差。
[0048] 21 )SCARA機器人在運行過程ARM控制器時刻對AC交流主電源進行監控，如果控制 器發現主電源出現故障突然斷電時，ARM會與DSP通訊，并開通備用電源，由備用電源為四軸 稀土永磁同步伺服電機提供能量，DSP通過CPLD內部伺服調節程序實時調整四臺稀土永磁 同步伺服電機的PWM輸出，DSP控制器通過調整稀土永磁同步伺服電機的驅動脈沖數目調整 其旋轉角度，通過調整稀土永磁同步伺服電機控制信號的頻率實現角速度快慢的變化，使 SCARA機器人完成這次搬運任務，然后SCARA機器人通過PLC與主站進行通訊，并通知主站進 行檢修。
[0049] 22)在SCARA機器人多軸伺服系統工作過程中，如果DSP伺服控制器檢測到某一個 稀土永磁同步伺服電機的轉矩出現脈動，由于本發明采用的是的矢量控制方式，因此 控制器會很容易補償此干擾，并根據干擾大小CPLD實時調整內部三閉環伺服控制程序的 PID參數，減少了電機轉矩擾動對SCARA機器人運動過程的影響。
[0050] 20)當SCARA機器人完成任務，實現位置歸零時，其攜帶的加速度傳感器A1~A3、陀 螺儀30、磁電傳感器EM1、EM2、EM3、EM4和圖像采集是系統會時刻工作，當加速度和速度超過 預設閥值時，在下一個周期DSP控制器會通過CPLD修正上一個周期帶來的誤差，當磁電傳感 器EM1、EM2、EM3、EM4均有信號輸出時，C⑶圖像采集系統實時采集復位信息，經ARM處理后通 過無線裝置向總站發出圖像傳輸請求，總站通過圖像對比技術自動復位結果，確定無誤后， DSP通過CPLD封鎖四路稀土永磁同步伺服電機的ΠΜ波控制信號，SCARA機器人自動引導零 位置復位完成，SCARA機器人按照圖7的速度-時間運動曲線實現從某一點到零位的歸位，然 后零位自鎖，等待下一批搬運命令。
[0051] 以上所述僅為本發明的實施例，并非因此限制本發明的專利范圍，凡是利用本發 明說明書內容所作的等效結構或等效流程變換，或直接或間接運用在其它相關的技術領 域，均同理包括在本發明的專利保護范圍內。
【主權項】
1. 一種重載無線傳輸五核高速關節機器人控制系統，其特征在于，所述關節機器人采 用稀土永磁同步伺服電機X驅動機器人大臂旋轉運動、采用稀土永磁同步伺服電機Y驅動機 器人小臂旋轉運動、采用稀土永磁同步伺服電機Z驅動機器人手腕旋轉運動、采用稀土永磁 同步伺服電機R驅動機器人手腕升降運動，所述機器人大臂上安裝有避障位移傳感器S1、避 障位移傳感器S2、陀螺儀和加速度傳感器A1，所述機器人小臂上安裝有避障位移傳感器S3、 避障位移傳感器S4、陀螺儀和加速度傳感器A2，所述機器人手腕上安裝有定位傳感器S5、陀 螺儀和加速度傳感器A3,所述的控制系統包括主站控制計算機和控制器，所述控制器包括 DSP芯片控制器、P1C控制器、可編程邏輯芯片控制器、語音識別處理器、圖像采集模塊、無線 傳輸模塊和ARM控制器，所述圖像采集模塊與無線傳輸模塊之間通信連接，所述DSP芯片控 制器、可編程邏輯芯片控制器、語音識別處理器和P1C控制器均與所述主站控制計算機通信 連接，所述DSP芯片控制器、可編程邏輯芯片控制器、語音識別處理器和ARM控制器彼此通信 連接，所述無線傳輸模塊和ARM控制器通信連接，所述P1C控制器與ARM控制器通信連接，所 述稀土永磁同步伺服電機X、稀土永磁同步伺服電機Y、稀土永磁同步伺服電機Z和稀土永磁 同步伺服電機R均與所述DSP芯片控制器通信連接，所述避障位移傳感器S1、避障位移傳感 器S2、避障位移傳感器S3、避障位移傳感器S4、定位傳感器S5、各陀螺儀、加速度傳感器A1、 加速度傳感器A2和加速度傳感器A3均同時與所述DSP芯片控制器和ARM控制器通信連接。2. 根據權利要求1所述的重載無線傳輸五核高速關節機器人控制系統，其特征在于，還 包括為所述關節機器人和控制系統提供電源的主電源和備用電源，所述關節機器人中的各 個所述電機和傳感器以及控制系統中的控制器均與所述主電源和/或備用電源電性連接。3. 根據權利要求1所述的重載無線傳輸五核高速關節機器人控制系統，其特征在于，所 述稀土永磁同步伺服電機X、稀土永磁同步伺服電機Y、稀土永磁同步伺服電機Z和稀土永磁 同步伺服電機R上勻設置有光電編碼器，所述光電編碼器與所述DSP芯片控制器電性連接。4. 根據權利要求1至3中任一項所述的教學用無線傳輸三核快速關節機器人控制系統， 其特征在于，所述圖像采集模塊為CCD攝像頭組件。5. 根據權利要求4所述的教學用無線傳輸三核快速關節機器人控制系統，其特征在于， 所述圖像采集模塊內設置有解碼芯片。6. 根據權利要求1至3中任一項所述的重載無線傳輸五核高速關節機器人控制系統，其 特征在于，所述機器人大臂上安裝有磁電傳感器EM1，所述機器人小臂上安裝有磁電傳感器 EM2,所述機器人手腕上安裝有磁電傳感器EM3和EM4,所述磁電傳感器EM1、磁電傳感器EM2、 磁電傳感器EM3和EM4均與所述DSP芯片控制器和ARM控制器通信連接。7. 根據權利要求4所述的重載無線傳輸五核高速關節機器人控制系統，其特征在于，所 述機器人大臂上安裝有磁電傳感器EM1，所述機器人小臂上安裝有磁電傳感器EM2,所述機 器人手腕上安裝有磁電傳感器EM3和EM4,所述磁電傳感器EM1、磁電傳感器EM2、磁電傳感器 EM3和EM4均與所述DSP芯片控制器和ARM控制器通信連接。
【文檔編號】B25J9/18GK105856244SQ201610412014
【公開日】2016年8月17日
【申請日】2016年6月14日
【發明人】張好明
【申請人】江蘇若博機器人科技有限公司