Festo六軸工業(yè)機械手結構設計及控制系統(tǒng)二次開發(fā)含SW三維圖,festo,工業(yè),機械手,結構設計,控制系統(tǒng),二次開發(fā),sw,三維
Festo 六軸工業(yè)機械手結構設計及控制系統(tǒng)二次開發(fā)
摘 要
工業(yè)機器人是集機械、電子、控制、計算機、傳感器、人工智能等多學科先進技術于一體的現(xiàn)代制造業(yè)重要的自動化裝備。目前，工業(yè)機器人主要承擔著強干擾惡劣環(huán)境中重復性并且勞動強度極大的工作。
本文通過對六軸工業(yè)機械手相關技術的學習和研究，完成六軸工業(yè)機械手的結構設計及控制系統(tǒng)的二次開發(fā)。首先是擬定出六軸工業(yè)機械手整體結構的設計方案并展開設計，具體是確定機械手各關節(jié)軸的軸長尺寸、利用 SolidWorks 繪制機械手的關節(jié)軸零件及其裝配圖。其次重點研究伺服驅動系統(tǒng)，具體完成 C 代碼的 PID 閉環(huán)控制傳遞函數。最后結合實驗室 Festo 六軸工業(yè)機械手，在熟悉控制程序設計及調試操作的基礎上對其二次開發(fā)。具體開發(fā)內容是結合不同末端執(zhí)行器完成分類搬運、弧焊軌跡模仿和執(zhí)筆作畫等開發(fā)任務。
機器人技術屬于具有前瞻性、戰(zhàn)略性的高技術領域。機器人作為智能制造的重要體現(xiàn)，其技術水平直接反映工業(yè)自動化程度。開展結構設計和動作的控制系統(tǒng)開發(fā)， 可培養(yǎng)分析問題、解決問題能力和機電綜合設計能力，有一定的創(chuàng)新性和設計實踐意義。
關鍵詞：六軸機械手；結構設計；控制系統(tǒng)；二次開發(fā)
II
ABSTRACT
Industrial 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 robot's 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 developed
Festo 六軸工業(yè)機械手結構設計及控制系統(tǒng)二次開發(fā)
目 錄
摘 要 I
ABSTRACT II
1 緒論 1
1.1 工業(yè)機器人國內外發(fā)展現(xiàn)狀 1
1.2 工業(yè)機器人發(fā)展趨勢 2
1.3 課題設計的主要內容 3
2 六軸工業(yè)機械手整體結構設計 4
2.1 機械手的設計參數 4
2.2 機械手的傳動方案 5
2.3 基本模型的參數化設計 6
2.4 伺服電機和減速器的選型計算 8
3 六軸工業(yè)機械手控制系統(tǒng)設計 13
3.1 控制系統(tǒng)的硬件結構 13
3.2 機械手的伺服控制系統(tǒng) 14
3.3 基于 C 語言實現(xiàn)的閉環(huán)控制算法 16
4 基于 Festo 六軸工業(yè)機械手的二次開發(fā) 20
4.1 Festo 六軸工業(yè)機械手 20
4.2 工件夾持任務開發(fā) 22
4.3 焊槍弧焊軌跡模仿任務開發(fā) 30
4.4 執(zhí)筆作畫任務開發(fā) 35
5 結論 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è)體系，在控制器、傳感器、減速機、伺服電機、數控系統(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ā)也有所突破，中國科學院和多所著名高校都培育出專門從事人工智能研究的團隊，機器人學習、仿生識別、數據挖掘以及模式、語言和圖像識別技術比較成熟。
46
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è)機械手的整體結構設計內容是：基本設計參數，運動形式，整體構型，驅動系統(tǒng)。根據工業(yè)機械手的發(fā)展現(xiàn)狀及趨勢，對于六軸工業(yè)機械手關節(jié)軸的驅動控制系統(tǒng)基本都采用伺服電動機+增量編碼器，可形成反饋的閉環(huán)控制系統(tǒng)，達到高精度軌跡運作當然還有高精密的減速器，通常采用諧波減速器。六軸工業(yè)機械手的整體構型包括腕部、小臂、大臂、腰部、臀部和基座等六個軸，大多數企業(yè)的機械手均相類似。本章主要描述工業(yè)機械手的參數化設計的方法和重要部件選型的方法。
2.1 機械手的設計參數
設計六軸工業(yè)機械手，首先根據工作任務確定機械手的工作空間，并規(guī)劃動作， 制定各關節(jié)的節(jié)拍，分配各動作時間，初步確定各個關鍵的運動速度。接著確定在工作空間內的各個關節(jié)臂的運動行程和臂長等參數。然后根據任務對機械手進行粗略的受力分析確定末端的許用載荷等。本文以 FESTO 的六軸工業(yè)機械手為模板做參數設計。如機械手的側視圖和俯視圖可大致給出機械手手腕中心點的工作范圍，如圖 2.1 所示。
圖 2.1 六軸工業(yè)機械手工作空間示圖
參考 FESTO 六軸工業(yè)機械手的基本參數，擬定出機械手各關節(jié)的運動和物理特性等的基本參數，如下列表各所示。
表 2.1 六軸工業(yè)機械手運動參數
運動軸
工作范圍
最大速度
旋轉軸 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é)處。根據常見的六軸工業(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 基本模型的參數化設計
根據六軸工業(yè)機械手的不同工作任務對其工作空間的不同要求，同一型號的工業(yè)機械手通過對各關節(jié)軸的伸縮擺動，即可達到工作范圍的很大擴展。為了在設計中減輕負擔，同型號的工業(yè)機械手的結構，選擇參數化設計方法，以各關節(jié)的軸線在空間
中的位置關系構成基礎參數。通過各關節(jié)軸線相對位置的平移旋轉進行調整，實現(xiàn)設計中工業(yè)機械手各零部件的尺寸變化，從而擬定出六軸工業(yè)機械手各個關節(jié)軸線的位置關系，如圖 2.3 所示。
圖 2.3 六軸工業(yè)機械手各關節(jié)軸坐標系
根據工業(yè)機械手的基本參數則能擬定出機械手各個關節(jié)軸線的位置尺寸，那么就可以在三維建模軟件上對六軸工業(yè)機械手進行整體結構建模，可利用 Solidworks 的測量功能，可得到模型的結構和質量屬性等參數，如圖 2.4 所示。
圖 2.4 六軸工業(yè)機械手的整體結構
2.4 伺服電機和減速器的選型計算
伺服電機及減速器齒輪箱的選型尤為重要，所選型號要滿足機械手正常運作的最大轉速 n 和穩(wěn)定工作的最大力矩 J 的需求。而電機和減速器在不同方式工作時，所選參數亦不同，當周期性工作時，力矩的參數應該采用機械手最大輸出的加速力矩。減速器齒輪箱的傳動比也是重要的參數。故，一般對機械手的驅動電機與關節(jié)軸的齒輪箱一起作選型。
2.4.1 選型方法
首先是轉速，為實現(xiàn)低功耗，額定轉速為重要參數，實際的轉速最好不超過額定轉速。如果電機正好處于額定轉速的工作時，那么就是機械手關節(jié)處于最大轉速的時候，其速度可由電機額定轉速比上齒輪箱的傳動比求得。
其次是轉矩，額定轉矩也是重要的參數，實際的轉矩也最好不要超過額定轉矩。結合機械手的結構尺寸和軌跡，受力分析，可計算出承載的最大力矩，應小于減速器齒輪箱輸出的力矩，折算至電機輸出端，也應小電機額定力矩。由于減速器齒輪箱的傳動效率并不是 100%，因此它們所受力矩也存在些差異，它們承受的力矩關系如下式：
（2.1）
式中： ---- 電機的輸出力矩，N ? m；
---- 減速器的輸出力矩，N ? m；
---- 減速器的傳動比；
---- 傳動效率；
同理機械手在實際運動的過程中，其實際的加速力矩也不得超過齒輪箱的最大輸出力矩，再根據齒輪箱的傳動效率，可算出電機的實際加速力矩，這個加速力矩也不得超過電機的額定加速力矩。
最后是慣量匹配，電機自身慣量與負載自身慣量符合一定比例稱之為慣量匹配。慣量是否符合匹配要求，通過慣量比的大小來看，一般電機轉子的慣量作為分母，負載的慣量作為分子，以 表示，如下式：
（2.2）
式中： ---- 電機的許用慣量比；
---- 電機轉子轉動慣量， ；
---- 負載轉動慣量， 。
根據設計經驗 的值應盡量小于 10 則算是匹配，假設 與 不匹配，動量傳遞會出現(xiàn)激烈的沖撞；根據下式分析。
( ) （2.3） 式中： ---- 電機轉矩，N ? m；
---- 負載轉矩，N ? m；
---- 電機轉子轉動慣量， ；
---- 負載轉動慣量， 。
---- 角加速度；
在一個工作任務中 電機轉矩和 負載轉矩通常不變，而電機一旦選定確認后，
電機轉子轉動慣量則也為定值。故，若想 負載轉動慣量變小，則 角加速度就要加大。
2.4.2 六軸工業(yè)機械手末端軸 6 的電機及減速器的選型計算
末端軸所承受的外力主要由末端執(zhí)行器和工件產生，其中占主要的還是末端執(zhí)行器。當末端軸帶動末端執(zhí)行器和工件旋轉時，承受的阻力矩有負載轉矩和摩擦力矩， 而摩擦力矩過小可忽略不計。在此先計算工件的轉動慣量，再計算末端執(zhí)行器的轉動慣量。
首先計算工件的轉動慣量，設工件為長 a、寬 b、高 h 的矩形體，
圖 2.5 方形工件
工件繞其質心軸線旋轉時，根據轉動慣量公式：
??
??
{ ??
??(?? + ? )
1
??(?? + ?? ) （2.4） 1
??(?? + ? )
1
可根據實際任務，可給工件一個具體尺寸：a = 0.05m，b=0.05m，h=0.05m 即正方體，工件質量 m=1kg，那么三個質心軸線的轉動慣量值一樣。故，公式（4）可簡化為：
?? ?? （2.5） 6
式中的 i 為任意軸向。因為工件被抓取過程中主要繞其質心 y 軸旋轉，所以 X， Z 軸的轉動慣量可以不計，將數據帶入（5）中計算得到：
??件
?? 6
1 × 0.05
6
4.17 × 10?4 kg
接著計算末端執(zhí)行器的轉動慣量，由于末端執(zhí)行器的結構較為復雜，不像工件為
矩形體或圓柱體，具有公式計算。在此利用 Solidworks 軟件的測量功能得到末端執(zhí)行器的轉動慣量，同理只考慮取末端執(zhí)行器 y 軸的轉動慣量：
?? 具 13.93 × 10?4 kg
然后將末端執(zhí)行器的轉動慣量與工件轉動慣量相加可得末端軸的負載慣量：
末 ?? 件 ?? 具 18.1 × 10?4 kg 假設執(zhí)行的任務需要末端軸在 0.1 秒內到達的最高轉速為：
7??
?? ???? 末
?????????
3
70 ??? ????
則轉軸在 0.1 秒內的瞬間角加速度為：
??末
?? ???? 末
??
7?? 3
0.1
70??
3
?????????
隨著加速度的產生，負載也會相對應產生一個慣性力矩，并與??成正比：
末 末
??末
18.1 × 10?4 × 70??
3
≈ 0.14 N m
最后將上訴所計算的值帶入電機及減速箱的選型方法的折算公式中，根據所選減速器齒輪箱的傳動比返求驗算，則可簡完成末端軸的電機和減速器的選型。
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
?????????
3
5??
?????????
3
40 ??? ????
50 ??? ????
則兩轉軸在 0.1 秒內的瞬間角加速度為：
??軸 1
??軸 4
?? ???? 軸 1
??
?? ???? 軸 4
??
4?? 3
0.1
5?? 3
0.1
40??
3
50??
3
?????????
?????????
隨著角加速度的產生，負載也會相對應產生一個慣性力矩，并與角加速度??成正比，可求出慣性力矩：
軸 1 軸 1 ?? 軸 1 0.837 ×
軸 4 軸 4 ?? 軸 4 0.023 ×
40??
≈ 35.06 N m
3
50??
≈ 1.21 N m
3
最后將上訴所計算的值帶入電機及減速箱的選型方法的折算公式中，根據所選減速器齒輪箱的傳動比返求驗算，則可簡完成軸 1 和軸 4 的電機及減速器的選型。
2.4.4 六軸工業(yè)機械手 2、3、5 軸的電機及減速器的選型計算
首先結合工業(yè)機械手的設計參數，將機械手至于各軸關節(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
最后將上訴所計算的值帶入電機及減速箱的選型方法的折算公式中，根據所選減速器齒輪箱的傳動比返求驗算，則可簡完成軸 2、軸 3 和軸 4 的電機及減速器的選型。
I/O
3 六軸工業(yè)機械手控制系統(tǒng)設計
本章對六軸工業(yè)機械手的通用控制系統(tǒng)展開設計，主要是機械手的伺服驅動系統(tǒng)的設計。通過理論分析，將驅動控制系統(tǒng)理論算法轉換為具體的實用算法，根據設計框圖，利用 C 語言簡單易懂的特性，設計出 PID 閉環(huán)控制傳遞函數。
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)定性。數據存儲、信息打印、信息處理顯示、監(jiān)控信息顯示，數據二次利用等等，關鍵在于機械手運動學的實時計算， 規(guī)劃軌跡。機械手的末端執(zhí)行器的位置及姿態(tài)由多個關節(jié)的運動合成，比如末端執(zhí)行器走一個直線，需要各個軸的運動配合，有的軸加速，有的軸不用動，這些都有建立數學模型和智能算法來實現(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)
假設電機驅動與伺服電機的動力學函數為：
( ) ( ?? ??) (??) (??) （3.1） 式中： ( )----電機控制電壓；
(??)----電機轉角；
(??)----干擾信號；
----電機轉動慣量；
----有效阻尼。
3.2.1 PD 控制系統(tǒng)
在位置式閉環(huán)控制系統(tǒng)中常用 PD 控制器作為控制系統(tǒng)，其系統(tǒng)框圖如下：
圖 3.3 PD 控制器
這時，電機的輸入控制量是：
( ) ( )( ( ) ( )) （3.2） 式中： ( )----目標角度值。
----比例項系數。
----微分項系數。
由于干擾信號 (??)的存在，則閉環(huán)系統(tǒng)可寫為：
( ) +
( ) 1
(??) （3.3）
( )
式中的 1(??)為特征多項式：
( )
1(??) ?? ( ) ?? （3.4） 式（3.4）為二階系統(tǒng)，主要參數有 阻尼系數以及 固有頻率：
+
2
{
2
（3.5）
一般 阻尼系數的值為 1， 和 作為 PD 閉環(huán)的重要參數，調節(jié)參數使之穩(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）
( ) ( )
式中 為積分項系數，那么其特征多項式：
(??) ?? ( ) ?? ?? （3.9） 利用勞斯判據來判斷系統(tǒng)是否處于穩(wěn)定狀態(tài)，系統(tǒng)增益不為負數，并且符合下式
條件：
( + ) （3.10）
3.3 基于 C 語言實現(xiàn)的閉環(huán)控制算法
對于電機而言，只有扭矩、轉速和轉動方向等基本參數。如果給電機的轉軸加上一個指針，那么指針的方向可以是任意的，在經過一段時間的轉動后電機停止，根本就不知道指針的所指的方向。從這個角度分析，將電機的轉動分為大于一周以上和在一周之內。
圖 3.5 單關節(jié)控制反饋框圖
3.3.1 增量式 PID 控制
增量式 PID 控制對應的是增量式編碼器作為轉軸的狀態(tài)反饋傳感器。增量式編碼器并不能確定轉軸的位置，當轉軸轉動一個小角度時，編碼器會反饋一個脈沖信號，
當轉軸轉動速度越快則編碼器單位時間內反饋的脈沖數越多。一般增量式編碼器安裝在電機的輸出端，因為那里轉軸基本上都運行在很多周以上，專門用于檢測電機的轉速。
根據增量式 PID 控制控制原理，設計實際運用算法，在此利用 C 語言程序進行
PID 閉環(huán)控制傳遞函數的算法設計，程序如下：
//定義 PID 的比例項系數 P 及參數賦值int speedP = 21;
//定義 PID 的積分項系數 I 及參數賦值int speedI = 16;
//定義 PID 的微分項系數 D 及參數賦值int speedD = 0;
//目標速度
int My_speed = 0;
//全局變量，上一次的偏差
long int ierror1;
//全局變量，上上一次的偏差
long int ierror2;
//全局變量，上一次的速度控制量
long int oldspeedPWM;
//增量式 PID 閉環(huán)控制系統(tǒng)的傳遞函數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(speedPWM>7500) speedPWM=7500;
if(speedPWM<(-7500)) speedPWM=(-7500);
//返回當前控制量return speedPWM;
}
通過調節(jié) PID 的三個項系數，實現(xiàn)對電機的最佳控制，可以通過 PID 調節(jié)上位機
來模擬實際參數的確定。
3.3.2 位置式 PD 控制
位置式 PD 控制對應的是位置式編碼器作為轉軸的狀態(tài)反饋傳感器。位置式編碼器通常用在轉軸的轉動范圍在一周之內，轉軸有唯一對應的位置。而工業(yè)機械手關節(jié)的擺動范圍就算在一定角度之內，將位置式編碼器安裝在減速器的輸出端，能實時反饋機械手關節(jié)臂的絕對位置。
根據位置式 PD 控制控制原理，設計實際運用算法，在此利用 C 語言程序進行 PID 閉環(huán)控制傳遞函數的算法設計，程序如下：
//定義 PD 的比例項系數 P 及參數賦值float placeP=33.0;
//定義 PD 的比例項系數 D 及參數賦值float placeD=20.0;
//全局變量，上一次的位置偏差float oldplace=0.0;
//位置式 PD 閉環(huán)控制系統(tǒng)的傳遞函數float placePD(float place)
{
//定義計算的中間變量
float ierror,P,D,newplace;
//計算當前位置偏差
ierror=place-oldplace;
//計算比例項
P=placeP*place;
//計算微分項
D=placeP*placeD*ierror;
//計算當前位置控制量
newplace=midle_servo+P+D;
//當前位置偏差存為上次位置偏差
oldplace=place;
//當前位置控制量限幅
if(newplace>youjixian_servo)
newplace=youjixian_servo;
if(newplace
收藏