基于GSK-Link總線的模塊化機器人控制裝置

暫無

基于GSK-Link總線的模塊化機器人控制裝置\n技術(shù)領(lǐng)域\n[0001] 本實用新型涉及工業(yè)控制的技術(shù)領(lǐng)域，特別涉及基于GSK-Link總線的模塊化機器人控制裝置。\n背景技術(shù)\n[0002] 工業(yè)機器人是高新技術(shù)密集的機電一體化產(chǎn)品。它是提高生產(chǎn)過程自動化，改善勞動環(huán)境條件，提高產(chǎn)品質(zhì)量和生產(chǎn)效率的一種非常有效的手段，也是技術(shù)革命的重要內(nèi)容之一。機器人控制裝置作為機器人運動控制的核心裝置，對它的設(shè)計要求也不斷提高，為了使機器人控制更加靈活高效，目前越來越多的系統(tǒng)采用模塊化，具有總線功能的運動控制裝置。但是現(xiàn)在大多數(shù)機器人控制裝置采用PCI、CAN、DeviceNet總線傳輸，PCI是并行總線無法連接太多設(shè)備，總線擴展性比較差，線間干擾將導(dǎo)致系統(tǒng)無法正常工作；CAN和DeviceNet總線的傳輸速率最大只有1Mbits/s，不能滿足大數(shù)據(jù)量傳輸需要，總線上最多只能掛接110個設(shè)備。而基于PCI總線上位機大多仍采用普通計算機，體積大、成本較高。\n實用新型內(nèi)容\n[0003] 本實用新型的目的在于克服現(xiàn)有技術(shù)的缺點與不足，提供一種基于GSK-Link總線的模塊化機器人控制裝置。\n[0004] 為了達到上述目的，本實用新型采用以下技術(shù)方案：\n[0005] 本實用新型一種基于GSK-Link總線的模塊化機器人控制裝置，包括用于完成機器人示教、示教檢查、再現(xiàn)運動控制、示教編程、程序解釋、預(yù)處理及參數(shù)設(shè)置的主控制器；\n對機器人運動數(shù)據(jù)進行正逆解算法、插補算法、速度控制、軌跡規(guī)劃的算法控制器；完成對GSK-Link總線控制的GSK-Link總線板；顯示程序編輯和機器人狀態(tài)的顯示屏；及用來實現(xiàn)操作輸入和燈指示顯示按鍵板，所述主控制器分別和顯示屏、算法控制器以及GSK-Link總線板連接，所述GSK-Link總線板與按鍵板連接。\n[0006] 優(yōu)選的，所述主控制器包括中央處理芯片、顯示接口、鍵盤接口及通信接口，所述中央處理芯片運行嵌入式linux系統(tǒng)，構(gòu)成控制系統(tǒng)的上位機；所述顯示接口由LVDS連接到顯示屏；所述鍵盤接口由GSK-Link總線板的共享內(nèi)存?zhèn)鞯街醒胩幚硇酒?。\n[0007] 優(yōu)選的，所述上位機包括調(diào)度模塊、示教控制模塊、示教檢查控制模塊、再現(xiàn)控制模塊、解釋模塊、預(yù)處理模塊、通信模塊、監(jiān)控故障處理模塊、參數(shù)設(shè)置模塊以及顯示模塊，所述調(diào)度模塊用于在啟動機器人系統(tǒng)后，建立一個正常的工作環(huán)境，啟動調(diào)度線程、通信線程、解釋線程及界面線程，然后等待按鍵操作并監(jiān)控當(dāng)前機器人的狀態(tài)，協(xié)調(diào)上位機的各個模塊完成機器人控制；\n[0008] 示教控制模塊通過操作軸操作鍵選擇坐標(biāo)系及速度等級來完成機器人示教運動，達到用戶指定的位置；\n[0009] 示教檢查控制模塊用來檢查示教好的程序是否按預(yù)想的路徑或邏輯運行，通過按下“前進”或“后退”鍵進行逐行檢查，直到達到預(yù)想的編程要求；\n[0010] 再現(xiàn)控制模塊用于實現(xiàn)機器人的自動運行，機器人讀取示教檢查好的程序，根據(jù)程序的指令逐行自動運行；\n[0011] 解釋模塊由詞法分析、語法分析、語義分析及符號表組成，它的作用是提取程序文件里的指令、數(shù)據(jù)、符號信息；\n[0012] 預(yù)處理模塊用于對機器人的運動數(shù)據(jù)進行路徑規(guī)劃，使機器人按規(guī)劃路徑運行；\n[0013] 通信模塊是指與算法控制器的通信和與GSK-Link總線板的通信；\n[0014] 監(jiān)控故障處理模塊由監(jiān)控模塊和故障處理模塊組成，所述監(jiān)控模塊用于機器人狀態(tài)監(jiān)控、IO監(jiān)控、位姿監(jiān)控及變量監(jiān)控，所述故障處理模塊用于伺服報警錯誤、軌跡規(guī)劃錯誤、通信錯誤及總線連接錯誤現(xiàn)象的處理；\n[0015] 參數(shù)設(shè)置模塊用于運動參數(shù)設(shè)置、伺服參數(shù)設(shè)置、運用參數(shù)設(shè)置、軸最大速度及加速度設(shè)置、軟極限設(shè)置及干涉區(qū)設(shè)置；\n[0016] 顯示模塊用于顯示機器人狀態(tài)、示教編程、參數(shù)設(shè)置和界面監(jiān)控，為使用者顯示一個友好操作界面，所述顯示模塊使用minigui，minigui是一款面向嵌入式系統(tǒng)的高級窗口系統(tǒng)(Windowing System)和圖形用戶界面(Graphical User Interface，GUI)支持系統(tǒng)。\n[0017] 優(yōu)選的，所述算法控制器由DSP芯片和通信接口組成；所述DSP芯片包括初始化模塊、運動控制模塊、參數(shù)設(shè)置模塊以及狀態(tài)參數(shù)更新模塊；所述通信接口包括與主控制器通信的HPI及與GSK-Link總線板通信的共享內(nèi)存。\n[0018] 優(yōu)選的，所述初始化模塊用于對FPGA的初始化、DSP內(nèi)部初始化以及機器人相關(guān)參數(shù)的初始化；\n[0019] 運動控制模塊是算法控制器的核心模塊，包括手動模式和自動模式的運動控制方式；完成機器人運動數(shù)據(jù)的正逆解算法、插補算法、軌跡規(guī)劃；\n[0020] 參數(shù)設(shè)置模塊接收上位機的軸參數(shù)設(shè)置和笛卡爾參數(shù)設(shè)置，通過算法使用軸參數(shù)和笛卡爾參數(shù)來規(guī)劃機器人運動，包括各軸最大速度、加速度、加速時間、停機減速度參數(shù)；\n[0021] 狀態(tài)參數(shù)更新模塊用來更新全局狀態(tài)參數(shù)、軸狀態(tài)參數(shù)、笛卡爾狀態(tài)參數(shù)。\n[0022] 優(yōu)選的，所述GSK-Link總線板包括FPGA芯片和GSK-Link總線接口，所述FPGA芯片以每2ms時間間隔從DSP芯片讀取一次插補數(shù)據(jù)，F(xiàn)PGA芯片內(nèi)部再作一個六關(guān)節(jié)的數(shù)據(jù)同步處理，通過GSK-Link總線發(fā)送到機器人的六個伺服控制裝置，實現(xiàn)機器人六軸聯(lián)動控制；所述GSK-Link總線是一個基于工業(yè)以太網(wǎng)的總線，通信周期為微秒級，使總線數(shù)據(jù)達到快速傳輸和精準(zhǔn)同步；通過GSK-Link總線還具有設(shè)置伺服參數(shù)，監(jiān)控伺服狀態(tài)，連接IO裝置功能。\n[0023] 本實用新型相對于現(xiàn)有技術(shù)具有如下的優(yōu)點及效果：\n[0024] 1、本實用新型采用的GSK-Link總線是一種環(huán)形網(wǎng)絡(luò)拓撲結(jié)構(gòu)，GSK-Link通信協(xié)議規(guī)范是一種數(shù)字化、串行網(wǎng)絡(luò)的數(shù)據(jù)總線，通信速率最高達到100Mbits/s，掛接設(shè)備最多達255個，電氣傳輸采用平衡方式。\n[0025] 2、本實用新型GSK-Link總線具有精準(zhǔn)同步機制，通過總線連接機器人六個關(guān)節(jié)伺服裝置，實現(xiàn)同步控制。本實用新型采用高性能ARM核作為一個嵌入式上位機系統(tǒng)，采用DSP作為運動控制算法核，各個CPU通過高速接口和共享內(nèi)存方式進行通信，如圖1所示。\n[0026] 3、本實用新型由多模塊組成，具有較高的穩(wěn)定性、很好的移植性、體積小、成本低、實時控制性強的特點。\n[0027] 4、本實用新型由多個芯片組成，各個芯片根據(jù)各自性能完成相應(yīng)功能提高資源利用率，減少了處理的復(fù)雜度，增強了可靠性。\n[0028] 5、本實用新型顯示模塊使用minigui，具有較好的人機界面，采用LVDS連接控制器和顯示屏具有較好的顯示抗干擾性。\n附圖說明\n[0029] 圖1為本實用新型機器人控制裝置的結(jié)構(gòu)框圖；\n[0030] 圖2為本實用新型操作調(diào)度流程圖；\n[0031] 圖3為本實用新型示教控制流程；\n[0032] 圖4為本實用新型示教檢查控制流程圖；\n[0033] 圖5為本實用新型再現(xiàn)控制流程圖；\n[0034] 圖6為本實用新型解釋總體流程圖；\n[0035] 圖7為本實用新型詞法分析器圖；\n[0036] 圖8為本實用新型語法分析器圖；\n[0037] 圖9為本實用新型語義分析圖；\n[0038] 圖10為本實用新型預(yù)處理流程圖；\n[0039] 圖11為本實用新型DSP總體流程圖；\n[0040] 圖12為本實用新型手動模式運動控制流程圖；\n[0041] 圖13為本實用新型自動模式運動控制流程圖；\n[0042] 圖14為本實用新型總體控制流程圖；\n[0043] 圖15為本實用新型暫停處理流程圖；\n[0044] 圖16為本實用新型加速狀態(tài)時暫停處理效果圖；\n[0045] 圖17為本實用新型勻速狀態(tài)時暫停處理效果圖；\n[0046] 圖18為本實用新型減速狀態(tài)時暫停處理效果圖。\n具體實施方式\n[0047] 下面結(jié)合實施例及附圖對本實用新型作進一步詳細的描述，但本實用新型的實施方式不限于此。\n[0048] 實施例\n[0049] 本實施例一種基于GSK-Link總線的模塊化機器人控制裝置，如圖1所示，包括主控制器、算法控制器、GSK-Link總線板、顯示屏、按鍵板；所述主控制器主要完成機器人示教、示教檢查、再現(xiàn)運動控制、示教編程、程序解釋、預(yù)處理、參數(shù)設(shè)置功能；所述算法控制器主要對機器人運動數(shù)據(jù)進行正逆解算法、插補算法、速度控制、軌跡規(guī)劃運算；所述GSK-Link總線板作為GSK-Link總線控制器，主要完成對GSK-Link總線的控制；它把接收到的運動數(shù)據(jù)、參數(shù)數(shù)據(jù)發(fā)送到伺服裝置，并把伺服狀態(tài)數(shù)據(jù)和GSK-Link總線的狀態(tài)數(shù)據(jù)傳到上位機進行相應(yīng)的控制；所述顯示屏用來顯示程序編輯和機器人狀態(tài)狀況，便于操作者使用。按鍵板用來實現(xiàn)操作輸入和燈指示顯示。\n[0050] 主控制器與算法控制器通過HPI接口實現(xiàn)高速通信，發(fā)送運動控制指令和運動數(shù)據(jù)到算法控制器。算法控制器的狀態(tài)通過HPI反饋傳送到主控制器。主控制器與GSK-Link總線板通過共享內(nèi)存的方式，實現(xiàn)數(shù)據(jù)高速傳輸控制，進行伺服參數(shù)設(shè)置、伺服狀態(tài)監(jiān)控、按鍵板數(shù)據(jù)接收。算法控制器通過共享內(nèi)存方式把運算后的軌跡數(shù)據(jù)傳送到GSK-Link總線板實現(xiàn)機器人運動。\n[0051] 所述的主控制器是一個高性能ARM核芯片，運行嵌入式linux系統(tǒng)，構(gòu)成控制系統(tǒng)的上位機。上位機有調(diào)度模塊、示教控制模塊、示教檢查控制模塊、再現(xiàn)控制模塊、解釋模塊、預(yù)處理模塊、通信模塊、監(jiān)控故障處理模塊、參數(shù)設(shè)置模塊、顯示模塊。下面就主要模塊功能流程作詳細說明。\n[0052] 調(diào)度模塊不能獨立運行，必須同控制裝置的其他模塊一起完成操作。調(diào)度模塊在控制裝置中與其它模塊的關(guān)系如圖2操作調(diào)度流程所示，作為各個模塊的中間接口，實現(xiàn)各個模塊之間變量及狀態(tài)傳遞，實現(xiàn)協(xié)調(diào)控制。其他的各個模塊在不同線程中，調(diào)度模塊通過信號量、互斥量實現(xiàn)變量的保護或模塊之間的同步操作，使控制流程按設(shè)計的要求完成。\n如在示教運動過程中不能實現(xiàn)示教檢查操作，不然機器人就會出現(xiàn)運動軌跡錯誤。如機器人在報警狀態(tài)下是不能運動的，否則會出現(xiàn)預(yù)想不到的危險。\n[0053] 示教控制模塊是對工業(yè)機器人進行示教，以使工業(yè)機器人按需要移動到指定位置。示教鍵由12個鍵組成，分別為六個關(guān)節(jié)的正負方向。示教可以選擇使用關(guān)節(jié)坐標(biāo)、直角坐標(biāo)、工具坐標(biāo)或用戶坐標(biāo)來操作，使機器人移動到指定位置，可選擇微動、低速、中速或高速示教速度運行。編程時可以獲得當(dāng)前機器人的位姿值，以作為機器人運動的軌跡參考點。圖3為示教控制的流程圖，它首先等待示教鍵的輸入，如果有示教鍵按下，則下一步檢查機器人當(dāng)前的狀態(tài)，判定當(dāng)前的工作模式是否為示教模式、有沒有示教檢查在操作、是否有報警或急停、是否在極限或干涉區(qū)、外部信號是否異常等情況，如果條件都能滿足示教要求，則獲取示教需要的參數(shù)信息，如速度、坐標(biāo)系、按鍵軸方向等，示教控制模塊再調(diào)用HPI接口函數(shù)把示教相關(guān)信息發(fā)送到算法控制器，由算法控制器生成運動數(shù)據(jù)，使機器人進行示教運動。\n[0054] 示教檢查控制模塊在示教狀態(tài)下，接收到示教檢查鍵(前進鍵、后退鍵)時，就開始對打開的示教文件進行示教內(nèi)容檢查。在示教檢查中，可以選擇示教檢查速度(微動、低速、中速、高速)和檢查方式(連續(xù)、單步)，完成示教檢查。如果檢查中出現(xiàn)不理想的情況，則可以改變示教點、檢查速度、插補方式來達到理想的運動軌跡。在檢查過程中，單步時接收上一步或下一步信息后，按當(dāng)前所在程序行向前或向后檢查。如果連續(xù)檢查，則直接從頭到尾檢查示教程序。檢查完畢后，保存理想的示教程序，以備工作再現(xiàn)時調(diào)用。圖4是示教檢查控制流程圖，它首先等待示教檢查鍵的輸入，如果有示教檢查鍵按下，再檢查機器人當(dāng)前的狀態(tài)，除進行上述示教控制中狀態(tài)檢查外，還需進行文件是否打開、文件是否有語法錯誤檢查。如果狀態(tài)條件都能滿足示教檢查的要求，則判斷按下的示教檢查鍵是“前進”還是“后退”鍵，如果是前進鍵則要判斷單步檢查還是連續(xù)檢查，而后退鍵只有單步檢查。接著判斷是示教檢查開始還是暫停重啟，如果是示教檢查開始，則初始化相關(guān)示教檢查參數(shù)，獲取檢查速度，把當(dāng)前模式自動轉(zhuǎn)換示教檢查模式，調(diào)用解釋模塊解釋好的指令代碼，判斷當(dāng)前檢查行指令是否為運動指令，是運動指令則調(diào)用預(yù)處理模塊對運動數(shù)據(jù)進行路徑規(guī)劃，然后把規(guī)劃好的數(shù)據(jù)調(diào)用HPI接口函數(shù)發(fā)送到算法控制器，由算法控制器再對數(shù)據(jù)作進一步處理，非運動指令則調(diào)用解釋模塊對指令進行相應(yīng)的邏輯運算、跳轉(zhuǎn)等處理。如果是示教檢查暫停，一樣要判斷暫停行是運動指令還是非運動指令，是運動指令則要調(diào)用預(yù)處理的暫停重啟處理，再把處理好的數(shù)據(jù)調(diào)用HPI接口函數(shù)發(fā)送到算法控制器，而非運動指令暫停則主要是延時的暫停，在暫停過程中延時不會計數(shù)，暫停重啟則恢復(fù)計數(shù)。這樣示教檢查開始檢查程序，彈起示教檢查鍵則暫停當(dāng)前運行，重新按下則繼續(xù)運行，直到檢查結(jié)束。在示教檢查過程中可以隨時修改程序，如點數(shù)據(jù)、速度、過渡條件、邏輯等，直到符合設(shè)計要求。\n[0055] 再現(xiàn)控制模塊主要實現(xiàn)機器人的自動運行。在程序示教檢查并達到工作設(shè)計要求后，就可以進行工作再現(xiàn)了。在正常工作再現(xiàn)中，依次通過示教模式開關(guān)選擇再現(xiàn)模式，操作選擇示教再現(xiàn)程序、設(shè)置再現(xiàn)條件(低速啟動、限速運行、低速運行或機械鎖定)、設(shè)置連續(xù)或單步再現(xiàn)。伺服準(zhǔn)備燈點亮，按下啟動鍵，直接讀取并逐行執(zhí)行當(dāng)前打開的程序?？刂圃佻F(xiàn)的停止有兩種情況：異常中止程序和結(jié)束程序的運行中止程序。異常中止程序，把示教模式開關(guān)選擇示教模式，關(guān)掉伺服準(zhǔn)備，或者有錯誤報警都會出現(xiàn)異常停止。結(jié)束程序的運行中止程序，只需設(shè)置再現(xiàn)條件為單步，程序執(zhí)行完后自動停止。具體程序工作流程如圖5所示。機器人在再現(xiàn)模式下時，等待啟動鍵輸入，如果有啟動鍵按下，則要檢查機器人當(dāng)前的狀態(tài)，這個和示教檢查控制檢查的狀態(tài)差不多，如果狀態(tài)條件都滿足再現(xiàn)運動的要求，則判斷當(dāng)前機器人狀態(tài)是再現(xiàn)暫停狀態(tài)還是再現(xiàn)起始狀態(tài)。如果是再現(xiàn)起始狀態(tài)，則初始化再現(xiàn)起始參數(shù)，設(shè)置為再現(xiàn)狀態(tài)，如果是再現(xiàn)暫停狀態(tài)，則恢復(fù)暫停參數(shù)，獲取當(dāng)前再現(xiàn)行，調(diào)用解釋器模塊解釋當(dāng)前行的指令代碼，判斷當(dāng)前行指令是運動指令還是非運動指令，是運動指令則調(diào)用預(yù)處理模塊對運動數(shù)據(jù)進行路徑規(guī)劃，然后把規(guī)劃好的數(shù)據(jù)調(diào)用HPI接口函數(shù)發(fā)到算法控制器，由算法控制器再對數(shù)據(jù)作進一步處理，非運動指令則調(diào)用解釋模塊對指令進行相應(yīng)的邏輯運算、跳轉(zhuǎn)等處理。等待當(dāng)前行處理完成，如果一行處理完成，則自動處理下一行，直到程序最后一行完成，程序再現(xiàn)結(jié)束。如果在連續(xù)再現(xiàn)方式下，程序自動跳到第一行繼續(xù)運行。\n[0056] 解釋模塊的作用主要是提取程序文件里的指令、數(shù)據(jù)、符號等信息。它主要由詞法分析、語法分析、語義分析、符號表組成，其總體流程如圖6所示。下面分別對各組成塊作一個說明。\n[0057] 詞法分析的主要功能是掃描輸入串中的各個字符，并從中識別出一些具有獨立意義的基本語法單位-單詞(正則表達式提供記號的詞法結(jié)構(gòu))。它為語法分析模塊提供一個接口，它直接進行文件讀取操作，從文件中讀取字符流，并以字符為單位，進行分析，然后通過正則表達式提供的詞法結(jié)構(gòu)，按照一定的結(jié)合法則，把相鄰的有聯(lián)系的字符組成一個記號。因此，設(shè)計中主要考慮的就是通過一定的正則表達式把記號的詞法結(jié)構(gòu)通過程序描述出來。由于單個字符讀入之后直接就可以生成記號，接下來考慮的主要就是詞數(shù)大于一個字符的記號的考慮，本模塊中主要有數(shù)字(NUM)、字符串(ID)、等號(EQ)這三種情況是大于一個字符的，以后可能還有各種比較運算符會添加到本模塊中。然后用START、INNUM、INID、INEQ、DONE，來標(biāo)記當(dāng)前狀態(tài)，進行后續(xù)字符的讀取，最后生成一個記號。由于關(guān)鍵字由單獨的記號表示，如果是字符串的話，還要判斷是否是一個關(guān)鍵字。具體流程如圖7所示。\n[0058] 語法分析階段的任務(wù)是“組詞成句”，把TOKEN串按語法規(guī)則構(gòu)成更大的語單位，并識別出其中的語法錯誤。涉及到“組詞成句”，當(dāng)然少不了文法描述。下面給出的是機器人語言的EBNF文法描述。通過此文法，語法分析模塊根據(jù)所得到的關(guān)鍵字，對其后語句或者單詞進行相應(yīng)的匹配。具體看流程圖8所示。\n[0059] 機器人語言的EBNF文法描述\n[0060] \n[0061] \n[0062] 語義分析器完成類型檢查，主要是操作數(shù)的合法性檢查，和成對使用的關(guān)鍵字的匹配性檢查。把語義分析單獨運行于語法分析之后的主要原因是它可以完成語法分析不能完成的功能，如操作數(shù)合法性檢查、關(guān)鍵字匹配等。這是因為語法分析是按行檢查或者是按語句檢查，在一定的語法基礎(chǔ)上，它不能實現(xiàn)跨行的無關(guān)聯(lián)關(guān)鍵字匹配檢查，這就可以交給語義分析來執(zhí)行。語義分析器是通過頭結(jié)點完成對語法樹的遍歷，并向指定的文本文件輸出錯誤信息。具體如流程圖9所示。\n[0063] 預(yù)處理模塊作用根據(jù)程序的運動數(shù)據(jù)，對機器人的運動數(shù)據(jù)進行路徑規(guī)劃，使機器人按規(guī)劃路徑運行。如示教檢查控制模塊和再現(xiàn)控制模塊所描述的，在需要執(zhí)行運動指令時則需要調(diào)用預(yù)處理模塊來完成數(shù)據(jù)的處理。如圖10所示預(yù)處理是對運動指令進行處理，包括MOVJ、MOVL、MOVC指令。如果是單獨MOVJ、MOVL、MOVC指令則直接存儲對應(yīng)指令數(shù)據(jù)，它主要包括運動目標(biāo)點數(shù)據(jù)，期望速度數(shù)據(jù)。如果是三種指令的排列組合或相同指令連在一起處理，則要根據(jù)指令的過渡條件進行相關(guān)計算來規(guī)劃過渡路徑，在經(jīng)過計算后存儲預(yù)處理好的數(shù)據(jù)。等待示教檢查模塊或再現(xiàn)模塊進行調(diào)用。\n[0064] 所述算法控制器由DSP芯片、HPI通信接口和共享內(nèi)存組成。DSP芯片有DSP初始化模塊、運動控制模塊、參數(shù)設(shè)置模塊、狀態(tài)參數(shù)更新模塊，具體如圖11所示。DSP主要功能是對運動數(shù)據(jù)進行正逆解算法、插補算法、速度控制、軌跡規(guī)劃運算。運動控制模塊是算法控制器的核心模塊，它分為手動模式控制和自動模式控制，下面對兩個模式分別說明。\n[0065] 手動模式控制如圖12所示，它主要是手動示教的運動控制方式，在主控制器通過HPI把示教控制數(shù)據(jù)傳送到算法控制器后，手動模式控制首先判斷示教是需要在哪一個坐標(biāo)系下運動，如果是關(guān)節(jié)空間下的示教，則要調(diào)用關(guān)節(jié)空間下的插補算法，算得離散的關(guān)節(jié)角度值，通過共享內(nèi)存方式把關(guān)節(jié)角度數(shù)據(jù)傳送到GSK-Link總線板。如果是直角坐標(biāo)系、工具坐標(biāo)系或用戶坐標(biāo)系，則需要經(jīng)過相應(yīng)的插補算法得到離散的位姿點數(shù)據(jù)，通過逆解函數(shù)運算出對應(yīng)的關(guān)節(jié)角度數(shù)據(jù)，通過共享內(nèi)存方式把數(shù)據(jù)傳送到GSK-Link總線板。如果插補出現(xiàn)問題或逆解不能運算等問題，算法控制器通過共享內(nèi)存方式把相關(guān)狀態(tài)量發(fā)送給主控制器，再進行處理。\n[0066] 自動模式控制如圖13所示，它主要是處理在示教檢查和再現(xiàn)運動控制方式下的運動數(shù)據(jù)，在主控制器通過HPI把再現(xiàn)運動數(shù)據(jù)傳送到算法控制器后，自動模式控制首先判斷再現(xiàn)運動指令，如果是關(guān)節(jié)運動，則需要進行關(guān)節(jié)插補，首先作關(guān)節(jié)插補預(yù)規(guī)劃，再作關(guān)節(jié)插補運算，獲得離散的關(guān)節(jié)角度，再對離散的關(guān)節(jié)角度進行精插補，這樣所得數(shù)據(jù)就可以通過共享內(nèi)存?zhèn)魉偷紾SK-Link總線板，實現(xiàn)關(guān)節(jié)指令的運動。而直線指令和圓弧指令，因為示教點是關(guān)節(jié)角度數(shù)據(jù)，則要經(jīng)過正解函數(shù)運算獲得位姿數(shù)據(jù)，再進行直線插補的規(guī)劃和計算，圓弧插補的規(guī)劃和計算，獲得離散的位姿數(shù)據(jù)。對離散的位姿點數(shù)據(jù)再進行逆解運算，得到離散的關(guān)節(jié)角度數(shù)據(jù)，對離散的關(guān)節(jié)角度數(shù)據(jù)再進行精插補，把精插補數(shù)據(jù)發(fā)到GSK-Link總線板就可以控制機器人作相應(yīng)的直線運動和圓弧運動。\n[0067] 所述的GSK-Link總線板由FPGA芯片和GSK-Link總線接口組成。FPGA芯片以每2ms時間間隔從DSP芯片讀取一次插補數(shù)據(jù)，F(xiàn)PGA芯片內(nèi)部再作一個六關(guān)節(jié)的數(shù)據(jù)同步處理，通過GSK-Link總線發(fā)送到機器人的六個伺服控制裝置，實現(xiàn)機器人六軸聯(lián)動控制。\nGSK-Link總線是一個基于工業(yè)以太網(wǎng)的總線，通信周期可以達到微秒級，使總線數(shù)據(jù)達到快速傳輸和精準(zhǔn)同步。通過GSK-Link總線還具有設(shè)置伺服參數(shù)，監(jiān)控伺服狀態(tài)，連接IO裝置等功能。主控制器可對伺服裝置設(shè)置參數(shù)，它通過共享內(nèi)存方式，把伺服參數(shù)數(shù)據(jù)通過FPGA芯片傳送到GSK-Link總線，由總線傳送到各個伺服裝置，進行參數(shù)設(shè)置。主控制器還可以通過總線監(jiān)控伺服狀態(tài)，如速度、電流、位置等相關(guān)信息和編碼器值等，并對伺服的報警狀態(tài)實時監(jiān)控，增加控制的安全性。GSK-Link總線還可以連接IO裝置，對IO的輸入輸出狀態(tài)進行實時監(jiān)控，并能夠改變輸出IO的狀態(tài)，實現(xiàn)對外部信號的控制。\n[0068] 圖14為基于GSK-Link總線的模塊化機器人的控制方法的流程圖，需要模式開關(guān)輸入裝置和按鍵輸入裝置。開關(guān)輸入裝置用來切換機器人的工作模式，機器人再現(xiàn)啟動和暫停，機器人的急停操作。按鍵輸入用來控制機器人的示教運動，示教檢查運動，還有示教編程的界面操作。\n[0069] 機器人控制裝置上電后，主控制器開始初始化內(nèi)部參數(shù)，并對算法控制器的相關(guān)參數(shù)進行初始化，然后檢測GSK-Link總線是否工作正常，如果正常則通過共享內(nèi)存發(fā)送伺服參數(shù)到GSK-Link總線板，GSK-Link總線板通過GSK-Link總線發(fā)送伺服參數(shù)到六個伺服裝置，機器人控制系統(tǒng)初始化完成。\n[0070] 模式開關(guān)在示教模式下只能進行示教，示教檢查，示教編程和設(shè)置操作。在再現(xiàn)模式下，只能再現(xiàn)操作，機器人的再現(xiàn)啟動和暫停開關(guān)才有效。機器人在急?；驁缶癄顟B(tài)下任何運動操作都無效。\n[0071] 機器人在正常情況下，在示教模式下示教鍵按下，主控制器把示教數(shù)據(jù)通過HPI發(fā)送到算法控制器，算法控制器根據(jù)示教數(shù)據(jù)進行相關(guān)運算，得到機器人運動數(shù)據(jù)。算法控制器通過共享內(nèi)存把運動數(shù)據(jù)傳到GSK-Link總線板。GSK-Link總線板經(jīng)過同步處理通過GSK-Link總線把運動數(shù)據(jù)傳到伺服裝置實現(xiàn)機器人的運動。在這過程中主控制器會通過HPI實時監(jiān)控算法控制器的狀態(tài)，通過共享內(nèi)存實時監(jiān)控GSK-Link總線和伺服裝置的狀態(tài)，實現(xiàn)對機器人各種狀態(tài)的及時響應(yīng)處理。其中示教運動的具體步驟如下：\n[0072] S11、有示教按鍵輸入；\n[0073] S12、檢查當(dāng)前是不是示教模式，機器人有沒有報警、急停異常狀態(tài)；\n[0074] S13、獲取示教需要的參數(shù)信息，如速度、坐標(biāo)系、按鍵軸方向；\n[0075] S14、通過HPI發(fā)送示教數(shù)據(jù)到算法控制器；\n[0076] S15、算法控制器對示教數(shù)據(jù)信息進行計算處理，得到關(guān)節(jié)數(shù)據(jù)，通過共享內(nèi)存?zhèn)鞯紾SK-Link總線板；\n[0077] S16、GSK-Link總線板獲得機器人運動的關(guān)節(jié)數(shù)據(jù)，經(jīng)過六關(guān)節(jié)的同步處理得到同步運動數(shù)據(jù)，通過GSK-Link總線發(fā)到伺服裝置實現(xiàn)電機轉(zhuǎn)動。\n[0078] 機器人在正常情況下，示教檢查鍵按下或再現(xiàn)模式啟動鍵按下，都需要調(diào)用示教文件進行解釋處理，非運動指令由主控制器處理，運動指令先進行預(yù)處理操作，處理后的數(shù)據(jù)作為再現(xiàn)數(shù)據(jù)。其中示教檢查包括下述步驟：\n[0079] S21、有示教檢查鍵按下；\n[0080] S22、檢查當(dāng)前是不是示教模式，有沒有示教文件，機器人有沒有報警、急停異常狀態(tài)；\n[0081] S23、主控制器調(diào)用示教文件進行解釋處理，如果有錯誤要重新編輯；\n[0082] S24、非運動指令由主控制器處理，主控制器執(zhí)行完當(dāng)前指令后，根據(jù)是單步檢查還是連續(xù)檢查選擇是否執(zhí)行下一行程序；\n[0083] S25、運動指令經(jīng)過預(yù)處理提取再現(xiàn)數(shù)據(jù)，通過HPI發(fā)送再現(xiàn)數(shù)據(jù)到算法控制器；\n單步檢查只能發(fā)送一個運動指令，連續(xù)檢查可以發(fā)送連續(xù)的運動指令；\n[0084] S26、算法控制器從HPI讀取到再現(xiàn)數(shù)據(jù)，對再現(xiàn)數(shù)據(jù)進行相關(guān)運算，得到機器人能夠執(zhí)行S3中，節(jié)數(shù)據(jù)，通過共享內(nèi)存?zhèn)鞯紾SK-Link總線板；\n[0085] S27、GSK-Link總線板獲得機器人運動的關(guān)節(jié)數(shù)據(jù)，經(jīng)過六關(guān)節(jié)的同步處理得到同步運動數(shù)據(jù)，通過GSK-Link總線發(fā)到伺服裝置實現(xiàn)電機轉(zhuǎn)動；\n[0086] S28、主控制器同時監(jiān)控機器人的運動狀態(tài)，判斷發(fā)送到算法控制器的再現(xiàn)數(shù)據(jù)是否執(zhí)行完，執(zhí)行完了則執(zhí)行下一行程序；\n[0087] S29、循環(huán)到S24步，直到程序結(jié)束，在這過程通過示教檢查鍵的彈起操作可以暫停當(dāng)前指令的執(zhí)行。\n[0088] 步驟S29中，暫停處理包括下述步驟：\n[0089] S291、主控制器有暫停操作，主控制器暫停運動數(shù)據(jù)發(fā)送，對運動控制相關(guān)變量置位。\n[0090] S292、主控制器發(fā)送暫?？刂浦噶畹剿惴刂破?，等待算法控制器處理完成。\n[0091] S293、當(dāng)算法控制器在運動的過程中接收到暫停指令時需進行停止運動規(guī)劃，獲取暫停指令時的速度V0、加速度A0，要求在ta時間內(nèi)速度、加速度減為零，如果當(dāng)前段剩余距離不夠減速需要從內(nèi)存中讀取下一段指令信息繼續(xù)停止運動，當(dāng)停止結(jié)束時，自動清除停止運動標(biāo)志位，并清空FIFO中的運動數(shù)據(jù)。\n[0092] S294、當(dāng)主控制器獲取到算法控制器暫停處理完狀態(tài)，暫停處理完成。\n[0093] 再現(xiàn)運動包括下述具體步驟：\n[0094] S31、有再現(xiàn)啟動操作；\n[0095] S32、檢查當(dāng)前是不是再現(xiàn)模式，有沒有示教文件，機器人有沒有報警，急停異常狀態(tài)；\n[0096] S33、初始化再現(xiàn)參數(shù)，判斷當(dāng)前是再現(xiàn)暫停重啟還是再現(xiàn)啟動，如果是再現(xiàn)重啟則直接跳到S35步；\n[0097] S34、主控制器調(diào)用示教文件進行解釋處理，如果有錯誤要重新編輯；\n[0098] S35、非運動指令主控制器處理，處理完后接著執(zhí)行程序文件下一行；\n[0099] S36、運動指令經(jīng)過預(yù)處理提取再現(xiàn)數(shù)據(jù)，通過HPI發(fā)送再現(xiàn)數(shù)據(jù)到算法控制器，連續(xù)的運動指令可以多個發(fā)送下去；\n[0100] S37、算法控制器從HPI讀取到再現(xiàn)數(shù)據(jù)，對再現(xiàn)數(shù)據(jù)進行相關(guān)運算，得到機器人能夠執(zhí)行關(guān)節(jié)數(shù)據(jù)，通過共享內(nèi)存?zhèn)鞯紾SK-Link總線板；\n[0101] S38、GSK-Link總線板獲得機器人運動的關(guān)節(jié)數(shù)據(jù)，經(jīng)過六關(guān)節(jié)的同步處理得到同步運動數(shù)據(jù)，通過GSK-Link總線發(fā)到伺服裝置實現(xiàn)電機轉(zhuǎn)動；\n[0102] S39、主控制器同時監(jiān)控機器人的運動狀態(tài)，當(dāng)前運動指令是否執(zhí)行完，完成后接著執(zhí)行程序的下一行；\n[0103] S310、循環(huán)到S35步，如果是單步方式再現(xiàn)運行到最后一行程序結(jié)束，是連續(xù)方式再現(xiàn)在執(zhí)行完最后一行程序后，跳到第一行重新執(zhí)行。\n[0104] 主控制器通過HPI發(fā)送再現(xiàn)數(shù)據(jù)到算法控制器，算法控制器根據(jù)指令進行再現(xiàn)數(shù)據(jù)運算，得到機器人能夠運動的關(guān)節(jié)數(shù)據(jù)，算法控制器通過共享內(nèi)存把運動數(shù)據(jù)傳到GSK-Link總線板。GSK-Link總線板經(jīng)過同步處理通過GSK-Link總線把運動數(shù)據(jù)傳到伺服裝置實現(xiàn)機器人的運動。在這過程中主控制器會通過HPI實時監(jiān)控算法控制器的狀態(tài)，通過共享內(nèi)存實時監(jiān)控GSK-Link總線和伺服裝置的狀態(tài)，實現(xiàn)對機器人各種狀態(tài)的及時響應(yīng)處理。\n[0105] 如圖15所示，算法控制器再現(xiàn)暫停過程具體步驟如下：\n[0106] S41、接到請求停止的信號，機器人停止運動規(guī)劃；\n[0107] S42、進行停止運動插補計算；\n[0108] S43、判斷當(dāng)前段結(jié)束速度是否為0，若不為0，則讀取下一段運動數(shù)據(jù)，循環(huán)停止運動規(guī)劃；若為0，則停止運動結(jié)束；所述停止運動插補計算的計算公式如下：\n其中：V0為停止指令時其速度，A0\n為加速度，ta為停止時間，J1，J2為停止加速度。\n[0109] 假設(shè)V0＝123.238301，A0＝354.910619，ta＝0.25s(此時機器人處于加速運動過程中)停止運動時的速度、加速度波形效果如圖16所示；V0＝130.763885，A0＝0，ta＝\n0.25s(此時機器人處于勻速運動過程中)停止運動時的速度、加速度波形效果如圖17所示；V0＝107.635393，A0＝-622.189317，ta＝0.25s(此時機器人處于減速運動過程中)停止運動時的速度、加速度波形效果如圖18所示。\n[0110] 上述實施例為本實用新型較佳的實施方式，但本實用新型的實施方式并不受上述實施例的限制，其他的任何未背離本實用新型的精神實質(zhì)與原理下所作的改變、修飾、替代、組合、簡化，均應(yīng)為等效的置換方式，都包含在本實用新型的保護范圍之內(nèi)。