Festo 六軸工業(yè)機械手結構設計及控制系統(tǒng)二次開發(fā),Festo,六軸工業(yè)機械手結構設計及控制系統(tǒng)二次開發(fā),工業(yè),機械手,結構設計,控制系統(tǒng),二次開發(fā) 畢業(yè)論文（設計）題目名稱： Festo 六軸工業(yè)機械手結構設計 及控制系統(tǒng)二次開發(fā)所 在 學 院 ： 專業(yè)（ 班級）： 學 生 姓 名 ： 指 導 教 師 ： 評閱人： 院 長 ： Festo 六軸工業(yè)機械手結構設計及控制系統(tǒng)二次開發(fā)總計：畢業(yè)論文表格插圖摘要工業(yè)機器人是集機械、電子、控制、計算機、傳感器、人工智能等多學科先進技術于一體的現(xiàn)代制造業(yè)重要的自動化裝備。目前，工業(yè)機器人主要承擔著強干擾惡劣環(huán)境中重復性并且勞動強度極大的工作。本文通過對六軸工業(yè)機械手相關技術的學習和研究，完成六軸工業(yè)機械手的結構設計及控制系統(tǒng)的二次開發(fā)。首先是擬定出六軸工業(yè)機械手整體結構的設計方案并展開設計，具體是確定機械手各關節(jié)軸的軸長尺寸、利用 SolidWorks 繪制機械手的關節(jié)軸零件及其裝配圖。其次重點研究伺服驅動系統(tǒng)，具體完成 C 代碼的 PID 閉環(huán)控制傳遞函數(shù)。最后結合實驗室 Festo 六軸工業(yè)機械手，在熟悉控制程序設計及調試操作的基礎上對其二次開發(fā)。具體開發(fā)內容是結合不同末端執(zhí)行器完成分類搬運、弧焊軌跡模仿和執(zhí)筆作畫等開發(fā)任務。機器人技術屬于具有前瞻性、戰(zhàn)略性的高技術領域。機器人作為智能制造的重要體現(xiàn)，其技術水平直接反映工業(yè)自動化程度。開展結構設計和動作的控制系統(tǒng)開發(fā)， 可培養(yǎng)分析問題、解決問題能力和機電綜合設計能力，有一定的創(chuàng)新性和設計實踐意義。關鍵詞：六軸機械手；結構設計；控制系統(tǒng)；二次開發(fā)IIABSTRACTIndustrial robots are an important automated equipment for modern manufacturing, integrating mechanical, electronic, control, computer, sensors, artificial intelligence and other multi-disciplinary advanced technologies. At present, industrial robots are mainly responsible for strong interference with harsh and repetitive tasks and labor-intensive work.In this paper, through the study and research on the relevant technology of the six-axis industrial manipulator, the structural design of the six-axis industrial manipulator and the secondary development of the control system are completed. The first is to develop a design plan for the overall structure of the six-axis industrial robot and to develop a design, specifically to determine the axial length of each joint axis of the manipulator, use SolidWorks to draw the joint axis parts of the manipulator and its assembly drawings. Secondly, we will focus on the servo drive system and complete the PID closed-loop control transfer function of the C code. Finally, in combination with the laboratory Festo six-axis industrial robot, it is secondarily developed after familiar with the control program design and debugging operations. The specific development content is the combination of different end-effectors to accomplish the tasks of classification and handling, arc welding track simulation and pen painting.Robotics is a forward-looking and strategic high-tech field. As an important embodiment of intelligent manufacturing, the robots technical level directly reflects the degree of industrial automation. The development of control systems for the development of structural designs and actions can foster analytical problems, problem solving capabilities, and comprehensive electromechanical design capabilities, with certain innovation and design practical significance.Keywords: six-axis manipulator ； structure design ； control system ； secondarily developedFesto 六軸工業(yè)機械手結構設計及控制系統(tǒng)二次開發(fā)目錄摘要IABSTRACTII1 緒論11.1 工業(yè)機器人國內外發(fā)展現(xiàn)狀11.2 工業(yè)機器人發(fā)展趨勢21.3 課題設計的主要內容32 六軸工業(yè)機械手整體結構設計42.1 機械手的設計參數(shù)42.2 機械手的傳動方案52.3 基本模型的參數(shù)化設計62.4 伺服電機和減速器的選型計算83 六軸工業(yè)機械手控制系統(tǒng)設計133.1 控制系統(tǒng)的硬件結構133.2 機械手的伺服控制系統(tǒng)143.3 基于 C 語言實現(xiàn)的閉環(huán)控制算法164 基于 Festo 六軸工業(yè)機械手的二次開發(fā)204.1 Festo 六軸工業(yè)機械手204.2 工件夾持任務開發(fā)224.3 焊槍弧焊軌跡模仿任務開發(fā)304.4 執(zhí)筆作畫任務開發(fā)355結論42參 考 文 獻43致謝45附錄：外文翻譯46附錄：外文原文50附錄：工件夾持代碼56附錄：焊槍弧焊軌跡代碼60附錄：執(zhí)筆作畫代碼62 - I - 1 緒論機械手在工業(yè)上應用越來越廣泛，工業(yè) 4.0 的重要環(huán)節(jié)之一是確保加工前后序環(huán)節(jié)的自動化，六軸工業(yè)機械手高度自動化機器，在工業(yè)生產中能代替人做某些單調、頻繁和重復的長時間作業(yè)，或是危險、惡劣環(huán)境下的作業(yè)。由于工業(yè)機器人具有一定的通用性和適應性，能適應多品種中、小批量的生產，成為柔性制造單元或柔性制造系統(tǒng)的組成部分。 然而，國外已經研制和生產了各種不同的標準組件，工業(yè)機器人技術遠遠超越我國，作為制造大國，應在工業(yè)機器人領域自力更生，創(chuàng)新進取。1.1 工業(yè)機器人國內外發(fā)展現(xiàn)狀工業(yè)機器人自動化生產線成套設備已成為自動化裝備的主流及未來的發(fā)展方向。各國都對工業(yè)機器人的研究和開發(fā)做大量投入，工業(yè)機器人技術得到突飛猛進。國外概況：國外的工業(yè)機器人研究概況優(yōu)于我國。以智能化為主要方向，美國企 業(yè)一方面加大對新材料的研發(fā)力度，力爭大幅降低機器人自重與負載比，一方面加快 發(fā)展視覺、觸覺等人工智能技術，如視覺裝配的控制和導航。日本產業(yè)體系配套完備， 政府大力推動應用普及和技術突破。日本工業(yè)機器人完備的配套產業(yè)體系，在控制器、傳感器、減速機、伺服電機、數(shù)控系統(tǒng)等關鍵零部件方面，均具備較強的技術優(yōu)勢， 呈現(xiàn)出以工業(yè)機器人產業(yè)優(yōu)勢帶動服務機器人產業(yè)發(fā)展的趨勢。德國帶動傳統(tǒng)產業(yè)改 造升級，政府資助人機交互技術及軟件開發(fā)。通過智能人機交互傳感器，人類可借助物聯(lián)網對下一代工業(yè)機器人進行遠程管理，機器人還具備生產間隙的“網絡喚醒模式”，以解決使用中的高能耗問題，促進制造業(yè)的綠色升級。國內概況：中國面臨核心技術被發(fā)達國家控制等挑戰(zhàn)，中國在機器人領域的部分技術已達到或接近國際先進水平。機器人涉及的技術較多，大體可分為器件技術、系統(tǒng)技術和智能技術。中國在通用零部件、信息網絡等部分器件和系統(tǒng)技術領域與發(fā)達國家的差距在 10 年左右，而對智能化程度要求不高的焊接、搬運、清潔、碼垛、包裝機器人的國產化率較高。近年來，中國在人工智能方面的研發(fā)也有所突破，中國科學院和多所著名高校都培育出專門從事人工智能研究的團隊，機器人學習、仿生識別、數(shù)據(jù)挖掘以及模式、語言和圖像識別技術比較成熟。 45 1.2 工業(yè)機器人發(fā)展趨勢隨著計算機技術的不斷向智能化方向發(fā)展，機器人應用領域的不斷擴展和深化以及在系統(tǒng)（FMS、CIMS）中的群體應用，工業(yè)機器人也在不斷向智能化方向發(fā)展， 以適應“敏捷制造”，滿足多樣化、個性化的需要，并適應多變的非結構環(huán)境作業(yè)， 向非制造領域進軍。從優(yōu)化設計、材料優(yōu)選、加工工藝、裝配技術、專用制造裝備、產業(yè)化能力等多方面入手，全面提升高精密減速器、高性能機器人專用伺服電機和驅動器、高速高性能控制器、傳感器、末端執(zhí)行器等五大關鍵零部件的質量穩(wěn)定性和批量生產能力，突破技術壁壘，打破長期依賴進口的局面。機械結構向模塊化、可重構化發(fā)展。例如關節(jié)模塊中的伺服電機、減速機、檢測系統(tǒng)三位一體化；由關節(jié)模塊、連桿模塊用重組方式構造機器人整機。工業(yè)機器人控制系統(tǒng)向基于 PC 機的開放型控制器方向發(fā)展，便于標準化、網絡化，控制器件集成度提高，控制柜日見小巧，且采用模塊化結構，大大提高了系統(tǒng)的可控性、易操作性和可維修性。（1） 感覺功能：感覺功能方面將實現(xiàn)多傳感器信息的融合，以檢測多變的外部環(huán)境，做出判斷和決策，其實質類似于人的五官和身體的綜合感覺功能，包括視覺、觸覺、力覺、滑覺、接近覺、壓覺、聽覺、味覺、臭覺、溫覺等。研究包括各類傳感信息的采集及融合處理、傳感器與驅動器一體化技術、感覺功能繼承模塊等。（2） 控制智能化：由引導教向 NC，離線編程發(fā)展，進而發(fā)展到進一步應用。隨著系統(tǒng)化、集成化生產的發(fā)展，基于 PC 的開放式控制系統(tǒng)將機器人控制和車間一級控制的發(fā)展方向，國外專家預測，2007 年它將占 30。（3） 移動功能的智能化：為解決長距離搬運作業(yè)、大作業(yè)對象、多作業(yè)對象及極限作業(yè)等問題，需開發(fā)自主移動系統(tǒng)（包括滑動、滾動、行走、爬行、跳躍、飛行等）。（4） 系統(tǒng)應用與集成化：支持以人為核心的生產系統(tǒng)，實現(xiàn)生產系統(tǒng)中機器人群體協(xié)調功能、群智能和多機通訊協(xié)議，開發(fā)能理解人的意志的“同事機器人”。國外專家預測，2020 以后有可能 IMS 要走向 MA(R)S（多智能體系統(tǒng)），而該系統(tǒng)中的“同事機器人”（Cobot）將成為操作人員不可或缺的伙伴。圍繞著各種機器人與人共存的諸多課題，正在興起一門新學科“軟機器人學”。（5） 安全可靠性：由于大量不確定因素的存在，要實現(xiàn)智能化的安全可靠性， 機器人必須具有對各種意外情況的應變能力，及時采取預防措施和安全對策，包括硬件級、軟件級、應用級和人機系統(tǒng)級的自診斷和自修復故障。（6） 微型化：向微型化發(fā)展，開發(fā)毫米級機器人，用于微加工、醫(yī)學、宇宙和海洋開發(fā)等領域。就使用性和成本來看，毫米級最可行。（7） 多傳感器信息融合與配置技術：機器人的傳感器配置和融合技術在水泥生產過程控制和污水處理自動控制系統(tǒng)中的應用包括面向工藝過程的多傳感器融合和配置技術；采用智能傳感器的現(xiàn)場總線技術；面向工藝要求的新型傳感器研制。機電一體化智能傳感器：包括具有感知、自主運動、自清污（自調整、自適應）的機電一體化傳感器研究；面向工藝要求的運動機構設計、實現(xiàn)檢測和清污的自主運動；調節(jié)控制系統(tǒng)；機器人機構和控制技術在傳感器設計中的應用。1.3 課題設計的主要內容（1） 了解工業(yè)機器人的功能和結構，并提出六軸工業(yè)機械手結構方案，為實現(xiàn)方案的可行化程度再進行三維實體設計。機械結構需模塊化、可重構化，采用關節(jié)模塊中的伺服電機、減速機、檢測系統(tǒng)三位一體，由關節(jié)模塊、連桿模塊用重組方式構造工業(yè)機械手整機。（2） 了解工業(yè)機器人運動控制的原理，以用戶角度對工業(yè)機器人做控制分析， 學習機器人語言，其控制系統(tǒng)要基于 PC 機的開放型控制器，通過嵌入式的實操手柄和電腦無線通訊的仿真上位機進行終端控制。本設計的研究重點在于控制系統(tǒng)的用戶開發(fā)，基于實驗室現(xiàn)有的 Festo 六軸工業(yè)機械手，完成設計任務和具體實現(xiàn)。2 六軸工業(yè)機械手整體結構設計工業(yè)機械手的整體結構設計內容是：基本設計參數(shù)，運動形式，整體構型，驅動系統(tǒng)。根據(jù)工業(yè)機械手的發(fā)展現(xiàn)狀及趨勢，對于六軸工業(yè)機械手關節(jié)軸的驅動控制系統(tǒng)基本都采用伺服電動機+增量編碼器，可形成反饋的閉環(huán)控制系統(tǒng)，達到高精度軌跡運作當然還有高精密的減速器，通常采用諧波減速器。六軸工業(yè)機械手的整體構型包括腕部、小臂、大臂、腰部、臀部和基座等六個軸，大多數(shù)企業(yè)的機械手均相類似。本章主要描述工業(yè)機械手的參數(shù)化設計的方法和重要部件選型的方法。2.1 機械手的設計參數(shù)設計六軸工業(yè)機械手，首先根據(jù)工作任務確定機械手的工作空間，并規(guī)劃動作， 制定各關節(jié)的節(jié)拍，分配各動作時間，初步確定各個關鍵的運動速度。接著確定在工作空間內的各個關節(jié)臂的運動行程和臂長等參數(shù)。然后根據(jù)任務對機械手進行粗略的受力分析確定末端的許用載荷等。本文以 FESTO 的六軸工業(yè)機械手為模板做參數(shù)設計。如機械手的側視圖和俯視圖可大致給出機械手手腕中心點的工作范圍，如圖 2.1 所示。圖 2.1六軸工業(yè)機械手工作空間示圖參考 FESTO 六軸工業(yè)機械手的基本參數(shù)，擬定出機械手各關節(jié)的運動和物理特性等的基本參數(shù)，如下列表各所示。表 2.1 六軸工業(yè)機械手運動參數(shù) 運動軸 工作范圍 最大速度 旋轉軸 1 -165 +165 250/s 手臂軸 2 -110 +110 250/s 手臂軸 3 -90 +70 250/s 手腕軸 4 -160 +160 320/s 彎曲軸 5 -120 +120 320/s 旋轉軸 6 -400 +400 420/s 表 2.2 六軸工業(yè)機械手物理特性 基座尺寸 180 * 180 mm 高度 700 mm 工作范圍 580 mm 重量 25 kg 有效載荷 3 kg 2.2 機械手的傳動方案選用伺服電機作為各關節(jié)的動力裝置，電機本身就就被高精度的優(yōu)點。但關鍵還是在于輸入軸和輸出軸之間的齒輪箱。齒輪箱本身就要有傳動效率高體積小等等的要求，一般安裝在工業(yè)機械手的各個關節(jié)處。根據(jù)常見的六軸工業(yè)機械手都采用諧波減速器。六軸工業(yè)機械手的齒輪箱的總體設計位置圖，如圖 2.2 所示。圖 2.2 六軸工業(yè)機械手齒輪箱的位置注釋 1）A-齒輪箱，軸 1（機座內）；B-齒輪箱，軸 2；C-齒輪箱，軸 3； D-齒輪箱，軸 4；E-齒輪箱，軸 5；F-齒輪箱，軸 6；對于關節(jié)型工業(yè)機械手，越靠近起始端的的減速箱越為重要，相互比較，重要程度逐遠遜次。始端軸 1 的減速箱輸出 1 度的轉動，與末端軸 6 的減速箱輸出 1 度的轉動，帶來的運動行程相差得有幾倍甚至幾十倍。上一級電機作動力源，經減速器齒輪箱減速，將力矩放大，驅動下一級軸臂。軸臂粗大的，臂內的電機與齒輪箱放置比較靈活，齒輪箱可以設計的大小越大，則該齒輪箱的精度和傳動比就可以設計得越高。對于較短的軸臂，其電機與齒輪箱緊挨著， 即電機的輸出軸直接傳入齒輪箱，如軸 1、2、4、6，其中 1、4、6 軸的輸出都為旋轉。而對于較長的關節(jié)軸臂，電機可以安裝在離齒輪箱比較遠的地方，選用帶傳動或鏈傳動形式，如軸 3、5。從圖中可以看出，越末端的電機和齒輪箱越小。2.3 基本模型的參數(shù)化設計根據(jù)六軸工業(yè)機械手的不同工作任務對其工作空間的不同要求，同一型號的工業(yè)機械手通過對各關節(jié)軸的伸縮擺動，即可達到工作范圍的很大擴展。為了在設計中減輕負擔，同型號的工業(yè)機械手的結構，選擇參數(shù)化設計方法，以各關節(jié)的軸線在空間中的位置關系構成基礎參數(shù)。通過各關節(jié)軸線相對位置的平移旋轉進行調整，實現(xiàn)設計中工業(yè)機械手各零部件的尺寸變化，從而擬定出六軸工業(yè)機械手各個關節(jié)軸線的位置關系，如圖 2.3 所示。圖 2.3六軸工業(yè)機械手各關節(jié)軸坐標系根據(jù)工業(yè)機械手的基本參數(shù)則能擬定出機械手各個關節(jié)軸線的位置尺寸，那么就可以在三維建模軟件上對六軸工業(yè)機械手進行整體結構建模，可利用 Solidworks 的測量功能，可得到模型的結構和質量屬性等參數(shù)，如圖 2.4 所示。圖 2.4六軸工業(yè)機械手的整體結構2.4 伺服電機和減速器的選型計算伺服電機及減速器齒輪箱的選型尤為重要，所選型號要滿足機械手正常運作的最大轉速 n 和穩(wěn)定工作的最大力矩 J 的需求。而電機和減速器在不同方式工作時，所選參數(shù)亦不同，當周期性工作時，力矩的參數(shù)應該采用機械手最大輸出的加速力矩。減速器齒輪箱的傳動比也是重要的參數(shù)。故，一般對機械手的驅動電機與關節(jié)軸的齒輪箱一起作選型。 2.4.1 選型方法 首先是轉速，為實現(xiàn)低功耗，額定轉速為重要參數(shù)，實際的轉速最好不超過額定轉速。如果電機正好處于額定轉速的工作時，那么就是機械手關節(jié)處于最大轉速的時候，其速度可由電機額定轉速比上齒輪箱的傳動比求得。 其次是轉矩，額定轉矩也是重要的參數(shù)，實際的轉矩也最好不要超過額定轉矩。結合機械手的結構尺寸和軌跡，受力分析，可計算出承載的最大力矩，應小于減速器齒輪箱輸出的力矩，折算至電機輸出端，也應小電機額定力矩。由于減速器齒輪箱的傳動效率并不是 100%，因此它們所受力矩也存在些差異，它們承受的力矩關系如下式： （2.1） 式中：電機的輸出力矩，N m； - 減速器的輸出力矩，N m； - 減速器的傳動比； - 傳動效率； 同理機械手在實際運動的過程中，其實際的加速力矩也不得超過齒輪箱的最大輸出力矩，再根據(jù)齒輪箱的傳動效率，可算出電機的實際加速力矩，這個加速力矩也不得超過電機的額定加速力矩。 最后是慣量匹配，電機自身慣量與負載自身慣量符合一定比例稱之為慣量匹配。慣量是否符合匹配要求，通過慣量比的大小來看，一般電機轉子的慣量作為分母，負載的慣量作為分子，以 表示，如下式： （2.2） 式中：電機的許用慣量比； - 電機轉子轉動慣量， ； - 負載轉動慣量， 。 根據(jù)設計經驗 的值應盡量小于 10 則算是匹配，假設 與 不匹配，動量傳遞會出現(xiàn)激烈的沖撞；根據(jù)下式分析。 ( ) （2.3） 式中： - 電機轉矩，N m； - 負載轉矩，N m； - 電機轉子轉動慣量， ； - 負載轉動慣量， 。 - 角加速度； 在一個工作任務中 電機轉矩和 負載轉矩通常不變，而電機一旦選定確認后， 電機轉子轉動慣量則也為定值。故，若想 負載轉動慣量變小，則 角加速度就要加大。 2.4.2 六軸工業(yè)機械手末端軸 6 的電機及減速器的選型計算 末端軸所承受的外力主要由末端執(zhí)行器和工件產生，其中占主要的還是末端執(zhí)行器。當末端軸帶動末端執(zhí)行器和工件旋轉時，承受的阻力矩有負載轉矩和摩擦力矩， 而摩擦力矩過小可忽略不計。在此先計算工件的轉動慣量，再計算末端執(zhí)行器的轉動慣量。 首先計算工件的轉動慣量，設工件為長 a、寬 b、高 h 的矩形體， 圖 2.5 方形工件 工件繞其質心軸線旋轉時，根據(jù)轉動慣量公式： ( + )1 ( + ) （2.4） 1 ( + )1 可根據(jù)實際任務，可給工件一個具體尺寸：a = 0.05m，b=0.05m，h=0.05m 即正方體，工件質量 m=1kg，那么三個質心軸線的轉動慣量值一樣。故，公式（4）可簡化為： （2.5） 6式中的 i 為任意軸向。因為工件被抓取過程中主要繞其質心 y 軸旋轉，所以 X， Z 軸的轉動慣量可以不計，將數(shù)據(jù)帶入（5）中計算得到： 件 61 0.05 6 4.17 104 kg 接著計算末端執(zhí)行器的轉動慣量，由于末端執(zhí)行器的結構較為復雜，不像工件為矩形體或圓柱體，具有公式計算。在此利用 Solidworks 軟件的測量功能得到末端執(zhí)行器的轉動慣量，同理只考慮取末端執(zhí)行器 y 軸的轉動慣量： 具 13.93 104 kg 然后將末端執(zhí)行器的轉動慣量與工件轉動慣量相加可得末端軸的負載慣量： 末 件 具 18.1 104 kg 假設執(zhí)行的任務需要末端軸在 0.1 秒內到達的最高轉速為： 7 末 3 70 則轉軸在 0.1 秒內的瞬間角加速度為： 末 末 7 30.1703 隨著加速度的產生，負載也會相對應產生一個慣性力矩，并與成正比： 末 末 末 18.1 104 703 0.14 N m 最后將上訴所計算的值帶入電機及減速箱的選型方法的折算公式中，根據(jù)所選減速器齒輪箱的傳動比返求驗算，則可簡完成末端軸的電機和減速器的選型。 2.4.3 六軸工業(yè)機械手軸 1、4 的電機及減速器的選型計算 軸 1 的電機及減速器輸出轉矩為其他各個軸再加上末端執(zhí)行器及工件的轉矩總和。軸 4 的電機及減速器輸出轉矩為軸 5、6 再加上末端執(zhí)行器及工件的轉矩總和。它們結構復雜，難以精確計算出轉動慣量。但可直接利用三維建模軟件的測量功能， 將三維模型分別擬動為對軸 1、4 產生最大的轉動慣量位置，展開測量得： 軸 1 0.837 kg 軸 4 0.023 kg 假設執(zhí)行的任務需要軸 1、4 在 0.1 秒內到達的最高轉速為： 4 軸 1 軸 4 353 40 50 則兩轉軸在 0.1 秒內的瞬間角加速度為： 軸 1軸 4 軸 1 軸 4 4 30.1 5 30.1403503 隨著角加速度的產生，負載也會相對應產生一個慣性力矩，并與角加速度成正比，可求出慣性力矩： 軸 1 軸 1 軸 1 0.837 軸 4 軸 4 軸 4 0.023 40 35.06 N m 350 1.21 N m 3最后將上訴所計算的值帶入電機及減速箱的選型方法的折算公式中，根據(jù)所選減速器齒輪箱的傳動比返求驗算，則可簡完成軸 1 和軸 4 的電機及減速器的選型。 2.4.4 六軸工業(yè)機械手 2、3、5 軸的電機及減速器的選型計算 首先結合工業(yè)機械手的設計參數(shù)，將機械手至于各軸關節(jié)受靜態(tài)力最大的位置及姿態(tài)，如下圖 2.5 所示： 圖 2.6末端執(zhí)行器伸直最遠處圖 2.7平面平衡分析計算軸 2 的輸出靜態(tài)轉矩，將六軸機械手分成兩個部分，其中軸 1、2 為固定不變的第一整體，軸 3、4、5、6 和末端執(zhí)行器 7 為另一個固定的第二整體。第二整體的重心大致位于 的中點處，利用三維建模軟件的測量功能，測得第二整體的質量為17.74 kg。要想機械手實現(xiàn)靜態(tài)力矩平衡，則可算出關節(jié)的輸出力矩如下： 2 4 6 754 10 17.74 9.8 65.54 N m 2計算軸 3 的輸出靜態(tài)轉矩，將軸 1、2、3 視為固定不變的第一整體，軸 4、5、6和末端執(zhí)行器 7 視為另一個固定的第二整體。第二整體的重心大致位于 的中點處， 利用三維建模軟件的測量功能，測得第二整體的質量為 10.04 kg。要想機械手實現(xiàn)靜態(tài)力矩平衡，則可算出關節(jié)的輸出力矩如下： 2 4 6 484 10 10.04 9.8 23.81 N m 2計算軸 5 的輸出靜態(tài)轉矩，將軸 1、2、3、4、5 視為固定不變的第一整體，軸 6和末端執(zhí)行器 7 視為另一個固定的第二整體。第二整體的重心大致位于 的中點處， 利用三維建模軟件的測量功能，測得第二整體的質量為 2.87 kg。要想機械手實現(xiàn)靜態(tài)力矩平衡，則可算出關節(jié)的輸出力矩如下： 2 6 182 10 2.87 9.8 2.56 N m 2最后將上訴所計算的值帶入電機及減速箱的選型方法的折算公式中，根據(jù)所選減速器齒輪箱的傳動比返求驗算，則可簡完成軸 2、軸 3 和軸 4 的電機及減速器的選型。 I/O3 六軸工業(yè)機械手控制系統(tǒng)設計本章對六軸工業(yè)機械手的通用控制系統(tǒng)展開設計，主要是機械手的伺服驅動系統(tǒng)的設計。通過理論分析，將驅動控制系統(tǒng)理論算法轉換為具體的實用算法，根據(jù)設計框圖，利用 C 語言簡單易懂的特性，設計出 PID 閉環(huán)控制傳遞函數(shù)。 3.1 控制系統(tǒng)的硬件結構工業(yè)機械手控制器其核心大腦，運行著各種復雜運算算法，其內核時鐘基本要上 G 級別，處理著各種輸入信號的同時，也輸出各種控制信號。首先是編程示教器將各程序命令傳入主機中，與主機直接通訊，構成人機交互。其次主機在于控制總線上的各個模塊進行通訊，各模塊包括傳感器、伺服驅動模塊和其他 I/O 設備，它們各自都有獨一無二的總線通訊地址。最后終結于伺服驅動系統(tǒng)對伺服電機的閉環(huán)控制，實現(xiàn)機械手的運作，它們共同構成了工業(yè)機械手控制系統(tǒng)的硬件結構，如下圖 3.1 所示： I/O圖 3.1 工業(yè)機械手控制系統(tǒng)的硬件結構 CPU 主機配備 RTOS，具有多線程、實時性和穩(wěn)定性。數(shù)據(jù)存儲、信息打印、信息處理顯示、監(jiān)控信息顯示，數(shù)據(jù)二次利用等等，關鍵在于機械手運動學的實時計算， 規(guī)劃軌跡。機械手的末端執(zhí)行器的位置及姿態(tài)由多個關節(jié)的運動合成，比如末端執(zhí)行器走一個直線，需要各個軸的運動配合，有的軸加速，有的軸不用動，這些都有建立數(shù)學模型和智能算法來實現(xiàn)。 示教器編寫程序時，要用到機器人語言，其示教方式主要分為實際示教法和不實際示教法。機械手運作的示教信息大概可分為三部分：順序信息、姿態(tài)與位置信息和作業(yè)條件及動作信息。 3.2 機械手的伺服控制系統(tǒng)對機械手實際運作中的末端位姿進行實時監(jiān)測是很困難的。為提高控制精度，機械手的電機控制必須選擇閉環(huán)的方式去控制，而常用的閉環(huán)控制為 PID。有些機械手的控制器將驅動集成在伺服電機中，控制效果好，制造成本低。還有些控制器利用電機驅動實現(xiàn)電流、電壓和速度閉環(huán)，完成機械手位置控制，如下圖 3.2 所示： 圖 3.2 機械手閉環(huán)伺服控制系統(tǒng) 假設電機驅動與伺服電機的動力學函數(shù)為： ( ) ( ) () () （3.1） 式中： ( )-電機控制電壓； ()電機轉角； ()干擾信號； -電機轉動慣量； -有效阻尼。 3.2.1 PD 控制系統(tǒng) 在位置式閉環(huán)控制系統(tǒng)中常用 PD 控制器作為控制系統(tǒng)，其系統(tǒng)框圖如下：圖 3.3 PD 控制器 這時，電機的輸入控制量是： ( ) ( )( ( ) ( ) （3.2） 式中： ( )-目標角度值。 -比例項系數(shù)。 -微分項系數(shù)。 由于干擾信號 ()的存在，則閉環(huán)系統(tǒng)可寫為： ( ) + ( )1 () （3.3） ( )式中的 1()為特征多項式： ( ) 1() ( ) （3.4） 式（3.4）為二階系統(tǒng)，主要參數(shù)有 阻尼系數(shù)以及 固有頻率： + 2 2 （3.5） 一般 阻尼系數(shù)的值為 1， 和 作為 PD 閉環(huán)的重要參數(shù)，調節(jié)參數(shù)使之穩(wěn)定。那么 PD 系統(tǒng)的跟隨誤差為： ( ) ( ) ( ) + ( )1 () （3.6） ( )( )如果系統(tǒng)產生階躍信號 ( ) 和一個固定干擾信號 ，利用終值定理得到控制環(huán)的穩(wěn)態(tài)誤差為： m () （3.7） 可見當 增大時，穩(wěn)態(tài)誤差則會變小。 3.2.2 PID 控制系統(tǒng) 為消除靜態(tài)溫差，對 PD 系統(tǒng)進行改良，增加積分項，即 PID 控制系統(tǒng)，其控制框圖如下：圖 3.4 PID 控制器 則閉環(huán)系統(tǒng)可寫為： ( ) + + ( ) () （3.8） ( )( )式中 為積分項系數(shù)，那么其特征多項式： () ( ) （3.9） 利用勞斯判據(jù)來判斷系統(tǒng)是否處于穩(wěn)定狀態(tài)，系統(tǒng)增益不為負數(shù)，并且符合下式條件： ( + ) （3.10） 3.3 基于C 語言實現(xiàn)的閉環(huán)控制算法對于電機而言，只有扭矩、轉速和轉動方向等基本參數(shù)。如果給電機的轉軸加上一個指針，那么指針的方向可以是任意的，在經過一段時間的轉動后電機停止，根本就不知道指針的所指的方向。從這個角度分析，將電機的轉動分為大于一周以上和在一周之內。 圖 3.5 單關節(jié)控制反饋框圖 3.3.1 增量式 PID 控制 增量式 PID 控制對應的是增量式編碼器作為轉軸的狀態(tài)反饋傳感器。增量式編碼器并不能確定轉軸的位置，當轉軸轉動一個小角度時，編碼器會反饋一個脈沖信號，當轉軸轉動速度越快則編碼器單位時間內反饋的脈沖數(shù)越多。一般增量式編碼器安裝在電機的輸出端，因為那里轉軸基本上都運行在很多周以上，專門用于檢測電機的轉速。 根據(jù)增量式 PID 控制控制原理，設計實際運用算法，在此利用 C 語言程序進行PID 閉環(huán)控制傳遞函數(shù)的算法設計，程序如下： /定義 PID 的比例項系數(shù) P 及參數(shù)賦值int speedP = 21; /定義 PID 的積分項系數(shù) I 及參數(shù)賦值int speedI = 16; /定義 PID 的微分項系數(shù) D 及參數(shù)賦值int speedD = 0; /目標速度 int My_speed = 0; /全局變量，上一次的偏差 long int ierror1; /全局變量，上上一次的偏差 long int ierror2; /全局變量，上一次的速度控制量 long int oldspeedPWM; /增量式 PID 閉環(huán)控制系統(tǒng)的傳遞函數(shù)long int speedPID(int speed) /定義計算的中間變量 long int ierror,P,I,D, speedPWM; /計算當前偏差ierror=My_speed-speed; /計算積分項I=speedI*ierror; /計算比例項P=speedP*(ierror-ierror1); /計算微分銷 D=speedD*(ierror-ierror1-ierror1+ierror2); /上次偏差存為上上次偏差 ierror2=ierror1; /當前偏差存為上次偏差 ierror1=ierror; /計算出當前速度控制量 speedPWM=oldspeedPWM+P+I+D; /當前控制量存為上次控制量 oldspeedPWM=speedPWM; /對當前速度控制量限幅 if(speedPWM7500) speedPWM=7500; if(speedPWMyoujixian_servo) newplace=youjixian_servo; if(newplacezuojixian_servo) newplace=zuojixian_servo; /返回位置控制量 return newplace; 參數(shù)調節(jié)原理同增量式 PID 一樣。 4 基于 Festo 六軸工業(yè)機械手的二次開發(fā)利用控制柜 IRC5 和機械手留有的用戶接口，在示教器上編寫用戶代碼，就是對機械手控制系統(tǒng)的二次開發(fā)。接口的使用根據(jù)不同設計任務選用，配套的編寫對應開發(fā)程序。 末端執(zhí)行器的機械接口尤為重要，而我們通常說的工業(yè)機械手其實是不包含末端執(zhí)行器的，六軸工業(yè)機械手的六個軸是其機械結構的最基本形式，而這個機械結構在隨著機械手的不斷發(fā)展，各家的設計基本都大同小異，機械手的基礎參數(shù)性能逐漸模塊化，全球化。 對機械手進行開發(fā)就是安裝在手腕上的不同末端執(zhí)行器。目前所出現(xiàn)的末端執(zhí)行器可謂是五花八門，千型百態(tài)。工業(yè)機械手除了提供安裝接口外，還提供了電子接口和液氣壓源接口，目的就是供給需要電氣控制的末端執(zhí)行器。 根據(jù)工業(yè)任務的不同，同一型號的機械手可能換接著不同末端執(zhí)行器。接下來本章將對六軸工業(yè)機械手使用到不同的末端執(zhí)行器做不同的任務做簡單的開發(fā)。 4.1 Festo 六軸工業(yè)機械手根據(jù)設備條件，以 Festo 六軸工業(yè)機械手的控制系統(tǒng)為平臺開發(fā)。其控制系統(tǒng)硬件主要由 IRC5 控制器、一個六軸工業(yè)機械手和手持示教器 Flex Pendent 三部分組成。如下圖 4.1 所示： 圖 4.1 六軸工業(yè)機械手系統(tǒng)圖 手持示教器 Flex Penden 是集離線編程、監(jiān)控、手動操縱、參數(shù)配置等于一身。是整個六軸工業(yè)機械手系統(tǒng)軟件運行的平臺，可用其進行編程調試。大部分的操作均由手持示教器的點觸屏幕上實現(xiàn)的，手持示教器上留有常用的快捷按鍵開關，其中有4 個按鍵供任務開發(fā)。如下圖 4.2 所示： 圖 4.2 手持示教器 圖中：1-連接器；2-點觸屏；3-緊急停止按鈕；4-控制遙感；5-USB 端口；6-三位使動裝置；7-觸摸筆；8-重置按鈕；A-D-預設按鍵；E-機械單元；F-運動模式 1， 重定向或線性；G-運動模式 2，軸 1-3 或軸 4-6；H-增量模式；J-上一步按鈕。按下此按鈕，可使程序后退至上一條指令；K-運行按鈕，開始執(zhí)行程序；L-下一步按鈕， 按下此按鈕，可使程序前進至下一條指令；M-停止按鈕，停止程序執(zhí)行。 控制器 IRC5 分驅動模塊和控制模塊，兩者裝在一個柜中。驅動模塊包括機械手伺服電機供電的全部電子設備，最多附有 9 個單元驅動。除了能處理六個電機外，還可以再附加三個電機?？刂颇K執(zhí)行操作機械手所需的全部軟件，即 Robot Ware 操作系統(tǒng)?？刂乒裆贤庖鲈S多通訊接口和控制端子供開發(fā)者使用，如下圖 4.3 所示： 圖 4.3 控制柜 IRC5 部分接口圖 六軸機械手作為控制對象，實體內包含六個伺服電機，同時也帶有六個編碼器、阻尼開關和類似陀螺儀的角度姿態(tài)傳感器等等傳感器。為滿足控制要求，機械手都引出它們的接口，與控制柜的接口通過電纜相連，實現(xiàn)機器人的基本運作。為了使機器人的末端執(zhí)行器種類增加，機械手還留有供末端執(zhí)行使用的若干電氣接口，其中包含10 路電信號接入接出引腳和 4 路氣壓管接入接出口。如圖 4.4 所示： 圖 4.4 機械手部分接口圖 注釋 1）A-10 路電信號接口；B-4 路氣壓管接口； C-機械手內傳感器讀寫接口；D-伺服電機及編碼器接口。 4.2 工件夾持任務開發(fā)任務：六軸工業(yè)機械手夾取 A 位置高度不同的黑色或白色端蓋，并按顏色分類， 白色夾至 B 位置，黑色夾至 C 位置。使用的末端執(zhí)行如下圖 4.5 所示： 圖 4.5 末端工件夾具 控制分析：首先判斷 A 位置是否有端蓋，安裝光電對管判斷，當有端蓋時，傳感器反饋高電平接入 I/O 引腳，則讀取值為 1，反之，讀取值為 0；夾取動作由一 I/O 口引腳的輸出功能控制電磁閥，再由電磁閥控制末端執(zhí)行器氣缸夾具。不同高度的端蓋需要利用測距傳感器，將距離轉化為電壓模擬量，經 AD 采集判斷機械手每次下降的高度，實現(xiàn)靈活改變。端蓋的黑白，由安裝在夾具上的光電傳感器的判斷，當端蓋為白色時，傳感器反饋高電平接入 I/O 引腳，則讀取值為 1，反之，讀取值為 0； 4.2.1 夾具的氣路驅動控制設計及氣壓計算 此任務所用的末端執(zhí)行器是以氣缸為執(zhí)行元件的工件夾具，通過此夾具可實現(xiàn)工件的位移和旋轉，同時夾具上還要安裝一些光電管和紅外傳感器可作為測距和判斷黑白顏色等。那么就需要用到機械手的用戶接口，在此選用兩路氣壓管接口和四路信號接口。以氣缸為動力原件的夾具，驅動原理很簡單，氣路簡圖如下所示： 圖 4.6 末端執(zhí)行器氣路驅動簡圖 利用兩個反向的氣缸，推桿上固定著夾子的半邊，當兩氣缸推桿同時伸出時，夾子即打開。當兩氣缸推桿同時縮回時，夾子即關閉。這兩種狀態(tài)的切換，是通過改變氣缸通氣口內氣體的流向，電磁閥的右位工作和左位工作，分別對應夾具的兩種狀態(tài)。 圖 4.7 工件受力分析圖 計算氣壓源壓強的最小值 P_min，擬定氣缸內推桿的受力橫截面積為 A = 10mm * 10mm 的正方形，要夾持的方塊質量為 m = 1 kg?？刹榉綁K表面摩擦系數(shù) f = 0.15。如右圖受力分析得到方程組如下式： 2 夾 夾 簡化計算，可得氣壓源壓強的最小值為： 2 1 9.82 0.15 1 104 0.327 通常在實驗室里的氣泵基本額定輸出 7 MPa 的壓強。 4.2.2 控制柜 IRC5 選擇外設及其接口 控制柜 IRC5 僅提供通信接口和對工業(yè)機械手的控制接口，對于任務中外加的傳感器并沒有留出接口。在實際生產制造中，往往給控制器增加外設，如單片機，PLC 和 PC 端電腦等。尤其是遇到信息量特別大的傳感器是，外設的處理速度采要求極高， 這樣不僅減輕了控制器的負擔，而且更有利于系統(tǒng)的開發(fā)設計。 圖 4.8 外設的開發(fā)思路 本次采用 DSQC651 標準 I/O 開發(fā)板最為控制器 IRC5 的外設 I/O 單元，DSQC651 微處理器內有一個 DeviceNet 網絡總線通訊模塊，控制柜與 DSQC651 微處理器之間利用 DeviceNet 網絡總線進行通信，將用到的傳感器接在 DSQC651 開發(fā)板上，通過DSQC651 處理傳感器信號，實現(xiàn)復雜算法，將計算的結果，通過 DeviceNet 網絡總線發(fā)送到 IRC5 控制柜，比如發(fā)送的信息直接是機械手下一步要移動至的坐標信息。開發(fā)板和傳感器所需要用到的電源由控制柜提供，并且開發(fā)板留有固定裝置，可固定在控制柜 IRC5 的附近。其中 DSQC651 標準 I/O 開發(fā)板的端口使用參數(shù)如下： 圖 4.9 外接 651 標準 I/O 開發(fā)板 圖中：A-數(shù)字輸出信號指示燈；B- X1 數(shù)字輸出接口；C- X6 數(shù)字輸出接口；D- X5 是 DeviceNet 接口；E- X3 數(shù)字輸入接口；F-模塊狀態(tài)指示燈；G-數(shù)字輸入信號指示燈。 表 4.1 板上普通端子引腳信息參數(shù) X1 端子編號 使用定義 地址分配 X3 端子編號 使用定義 地址分配 1 OUTPUT_CH1 32 1 INPUT_CH1 0 2 OUTPUT_CH2 33 2 INPUT_CH2 1 3 OUTPUT_CH3 34 3 INPUT_CH3 2 4 OUTPUT_CH4 35 4 INPUT_CH4 3 5 OUTPUT_CH5 36 5 INPUT_CH5 4 6 OUTPUT_CH6 37 6 INPUT_CH6 5 7