Festo 六軸工業(yè)機(jī)械手結(jié)構(gòu)設(shè)計(jì)及控制系統(tǒng)二次開發(fā),Festo,六軸工業(yè)機(jī)械手結(jié)構(gòu)設(shè)計(jì)及控制系統(tǒng)二次開發(fā),工業(yè),機(jī)械手,結(jié)構(gòu)設(shè)計(jì),控制系統(tǒng),二次開發(fā) 畢業(yè)論文（設(shè)計(jì)） 題目名稱： Festo 六軸工業(yè)機(jī)械手結(jié)構(gòu)設(shè)計(jì) 及控制系統(tǒng)二次開發(fā) 所 在 學(xué) 院 ： 專業(yè)（ 班級(jí)）： 學(xué) 生 姓 名 ： 指 導(dǎo) 教 師 ： 評(píng) 閱 人 ： 院 長(zhǎng) ： Festo 六軸工業(yè)機(jī)械手結(jié)構(gòu)設(shè)計(jì)及控制系統(tǒng)二次開發(fā) 總計(jì)：畢業(yè)論文表 格 插 圖 摘 要 工業(yè)機(jī)器人是集機(jī)械、電子、控制、計(jì)算機(jī)、傳感器、人工智能等多學(xué)科先進(jìn)技術(shù)于一體的現(xiàn)代制造業(yè)重要的自動(dòng)化裝備。目前，工業(yè)機(jī)器人主要承擔(dān)著強(qiáng)干擾惡劣環(huán)境中重復(fù)性并且勞動(dòng)強(qiáng)度極大的工作。 本文通過對(duì)六軸工業(yè)機(jī)械手相關(guān)技術(shù)的學(xué)習(xí)和研究，完成六軸工業(yè)機(jī)械手的結(jié)構(gòu)設(shè)計(jì)及控制系統(tǒng)的二次開發(fā)。首先是擬定出六軸工業(yè)機(jī)械手整體結(jié)構(gòu)的設(shè)計(jì)方案并展開設(shè)計(jì)，具體是確定機(jī)械手各關(guān)節(jié)軸的軸長(zhǎng)尺寸、利用 SolidWorks 繪制機(jī)械手的關(guān)節(jié)軸零件及其裝配圖。其次重點(diǎn)研究伺服驅(qū)動(dòng)系統(tǒng)，具體完成 C 代碼的 PID 閉環(huán)控制傳遞函數(shù)。最后結(jié)合實(shí)驗(yàn)室 Festo 六軸工業(yè)機(jī)械手，在熟悉控制程序設(shè)計(jì)及調(diào)試操作的基礎(chǔ)上對(duì)其二次開發(fā)。具體開發(fā)內(nèi)容是結(jié)合不同末端執(zhí)行器完成分類搬運(yùn)、弧焊軌跡模仿和執(zhí)筆作畫等開發(fā)任務(wù)。 機(jī)器人技術(shù)屬于具有前瞻性、戰(zhàn)略性的高技術(shù)領(lǐng)域。機(jī)器人作為智能制造的重要體現(xiàn)，其技術(shù)水平直接反映工業(yè)自動(dòng)化程度。開展結(jié)構(gòu)設(shè)計(jì)和動(dòng)作的控制系統(tǒng)開發(fā)， 可培養(yǎng)分析問題、解決問題能力和機(jī)電綜合設(shè)計(jì)能力，有一定的創(chuàng)新性和設(shè)計(jì)實(shí)踐意義。 關(guān)鍵詞：六軸機(jī)械手；結(jié)構(gòu)設(shè)計(jì)；控制系統(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è)機(jī)械手結(jié)構(gòu)設(shè)計(jì)及控制系統(tǒng)二次開發(fā) 目 錄 摘 要 I ABSTRACT II 1 緒論 1 1.1 工業(yè)機(jī)器人國(guó)內(nèi)外發(fā)展現(xiàn)狀 1 1.2 工業(yè)機(jī)器人發(fā)展趨勢(shì) 2 1.3 課題設(shè)計(jì)的主要內(nèi)容 3 2 六軸工業(yè)機(jī)械手整體結(jié)構(gòu)設(shè)計(jì) 4 2.1 機(jī)械手的設(shè)計(jì)參數(shù) 4 2.2 機(jī)械手的傳動(dòng)方案 5 2.3 基本模型的參數(shù)化設(shè)計(jì) 6 2.4 伺服電機(jī)和減速器的選型計(jì)算 8 3 六軸工業(yè)機(jī)械手控制系統(tǒng)設(shè)計(jì) 13 3.1 控制系統(tǒng)的硬件結(jié)構(gòu) 13 3.2 機(jī)械手的伺服控制系統(tǒng) 14 3.3 基于 C 語言實(shí)現(xiàn)的閉環(huán)控制算法 16 4 基于 Festo 六軸工業(yè)機(jī)械手的二次開發(fā) 20 4.1 Festo 六軸工業(yè)機(jī)械手 20 4.2 工件夾持任務(wù)開發(fā) 22 4.3 焊槍弧焊軌跡模仿任務(wù)開發(fā) 30 4.4 執(zhí)筆作畫任務(wù)開發(fā) 35 5 結(jié)論 42 參 考 文 獻(xiàn) 43 致 謝 45 附錄Ⅰ：外文翻譯 46 附錄Ⅰ：外文原文 50 附錄Ⅲ：工件夾持代碼 56 附錄Ⅳ：焊槍弧焊軌跡代碼 60 附錄Ⅴ：執(zhí)筆作畫代碼 62 - I - 1 緒論 機(jī)械手在工業(yè)上應(yīng)用越來越廣泛，工業(yè) 4.0 的重要環(huán)節(jié)之一是確保加工前后序環(huán)節(jié)的自動(dòng)化，六軸工業(yè)機(jī)械手高度自動(dòng)化機(jī)器，在工業(yè)生產(chǎn)中能代替人做某些單調(diào)、頻繁和重復(fù)的長(zhǎng)時(shí)間作業(yè)，或是危險(xiǎn)、惡劣環(huán)境下的作業(yè)。由于工業(yè)機(jī)器人具有一定的通用性和適應(yīng)性，能適應(yīng)多品種中、小批量的生產(chǎn)，成為柔性制造單元或柔性制造系統(tǒng)的組成部分。 然而，國(guó)外已經(jīng)研制和生產(chǎn)了各種不同的標(biāo)準(zhǔn)組件，工業(yè)機(jī)器人技術(shù)遠(yuǎn)遠(yuǎn)超越我國(guó)，作為制造大國(guó)，應(yīng)在工業(yè)機(jī)器人領(lǐng)域自力更生，創(chuàng)新進(jìn)取。 1.1 工業(yè)機(jī)器人國(guó)內(nèi)外發(fā)展現(xiàn)狀 工業(yè)機(jī)器人自動(dòng)化生產(chǎn)線成套設(shè)備已成為自動(dòng)化裝備的主流及未來的發(fā)展方向。各國(guó)都對(duì)工業(yè)機(jī)器人的研究和開發(fā)做大量投入，工業(yè)機(jī)器人技術(shù)得到突飛猛進(jìn)。 國(guó)外概況：國(guó)外的工業(yè)機(jī)器人研究概況優(yōu)于我國(guó)。以智能化為主要方向，美國(guó)企 業(yè)一方面加大對(duì)新材料的研發(fā)力度，力爭(zhēng)大幅降低機(jī)器人自重與負(fù)載比，一方面加快 發(fā)展視覺、觸覺等人工智能技術(shù)，如視覺裝配的控制和導(dǎo)航。日本產(chǎn)業(yè)體系配套完備， 政府大力推動(dòng)應(yīng)用普及和技術(shù)突破。日本工業(yè)機(jī)器人完備的配套產(chǎn)業(yè)體系，在控制器、傳感器、減速機(jī)、伺服電機(jī)、數(shù)控系統(tǒng)等關(guān)鍵零部件方面，均具備較強(qiáng)的技術(shù)優(yōu)勢(shì)， 呈現(xiàn)出以工業(yè)機(jī)器人產(chǎn)業(yè)優(yōu)勢(shì)帶動(dòng)服務(wù)機(jī)器人產(chǎn)業(yè)發(fā)展的趨勢(shì)。德國(guó)帶動(dòng)傳統(tǒng)產(chǎn)業(yè)改 造升級(jí)，政府資助人機(jī)交互技術(shù)及軟件開發(fā)。通過智能人機(jī)交互傳感器，人類可借助物聯(lián)網(wǎng)對(duì)下一代工業(yè)機(jī)器人進(jìn)行遠(yuǎn)程管理，機(jī)器人還具備生產(chǎn)間隙的“網(wǎng)絡(luò)喚醒模式”，以解決使用中的高能耗問題，促進(jìn)制造業(yè)的綠色升級(jí)。 國(guó)內(nèi)概況：中國(guó)面臨核心技術(shù)被發(fā)達(dá)國(guó)家控制等挑戰(zhàn)，中國(guó)在機(jī)器人領(lǐng)域的部分技術(shù)已達(dá)到或接近國(guó)際先進(jìn)水平。機(jī)器人涉及的技術(shù)較多，大體可分為器件技術(shù)、系統(tǒng)技術(shù)和智能技術(shù)。中國(guó)在通用零部件、信息網(wǎng)絡(luò)等部分器件和系統(tǒng)技術(shù)領(lǐng)域與發(fā)達(dá)國(guó)家的差距在 10 年左右，而對(duì)智能化程度要求不高的焊接、搬運(yùn)、清潔、碼垛、包裝機(jī)器人的國(guó)產(chǎn)化率較高。近年來，中國(guó)在人工智能方面的研發(fā)也有所突破，中國(guó)科學(xué)院和多所著名高校都培育出專門從事人工智能研究的團(tuán)隊(duì)，機(jī)器人學(xué)習(xí)、仿生識(shí)別、數(shù)據(jù)挖掘以及模式、語言和圖像識(shí)別技術(shù)比較成熟。 45 1.2 工業(yè)機(jī)器人發(fā)展趨勢(shì) 隨著計(jì)算機(jī)技術(shù)的不斷向智能化方向發(fā)展，機(jī)器人應(yīng)用領(lǐng)域的不斷擴(kuò)展和深化以及在系統(tǒng)（FMS、CIMS）中的群體應(yīng)用，工業(yè)機(jī)器人也在不斷向智能化方向發(fā)展， 以適應(yīng)“敏捷制造”，滿足多樣化、個(gè)性化的需要，并適應(yīng)多變的非結(jié)構(gòu)環(huán)境作業(yè)， 向非制造領(lǐng)域進(jìn)軍。從優(yōu)化設(shè)計(jì)、材料優(yōu)選、加工工藝、裝配技術(shù)、專用制造裝備、產(chǎn)業(yè)化能力等多方面入手，全面提升高精密減速器、高性能機(jī)器人專用伺服電機(jī)和驅(qū)動(dòng)器、高速高性能控制器、傳感器、末端執(zhí)行器等五大關(guān)鍵零部件的質(zhì)量穩(wěn)定性和批量生產(chǎn)能力，突破技術(shù)壁壘，打破長(zhǎng)期依賴進(jìn)口的局面。機(jī)械結(jié)構(gòu)向模塊化、可重構(gòu)化發(fā)展。例如關(guān)節(jié)模塊中的伺服電機(jī)、減速機(jī)、檢測(cè)系統(tǒng)三位一體化；由關(guān)節(jié)模塊、連桿模塊用重組方式構(gòu)造機(jī)器人整機(jī)。工業(yè)機(jī)器人控制系統(tǒng)向基于 PC 機(jī)的開放型控制器方向發(fā)展，便于標(biāo)準(zhǔn)化、網(wǎng)絡(luò)化，控制器件集成度提高，控制柜日見小巧，且采用模塊化結(jié)構(gòu)，大大提高了系統(tǒng)的可控性、易操作性和可維修性。 （1） 感覺功能：感覺功能方面將實(shí)現(xiàn)多傳感器信息的融合，以檢測(cè)多變的外部環(huán)境，做出判斷和決策，其實(shí)質(zhì)類似于人的五官和身體的綜合感覺功能，包括視覺、觸覺、力覺、滑覺、接近覺、壓覺、聽覺、味覺、臭覺、溫覺等。研究包括各類傳感信息的采集及融合處理、傳感器與驅(qū)動(dòng)器一體化技術(shù)、感覺功能繼承模塊等。 （2） 控制智能化：由引導(dǎo)教向 NC，離線編程發(fā)展，進(jìn)而發(fā)展到進(jìn)一步應(yīng)用。隨著系統(tǒng)化、集成化生產(chǎn)的發(fā)展，基于 PC 的開放式控制系統(tǒng)將機(jī)器人控制和車間一級(jí)控制的發(fā)展方向，國(guó)外專家預(yù)測(cè)，2007 年它將占 30％。 （3） 移動(dòng)功能的智能化：為解決長(zhǎng)距離搬運(yùn)作業(yè)、大作業(yè)對(duì)象、多作業(yè)對(duì)象及極限作業(yè)等問題，需開發(fā)自主移動(dòng)系統(tǒng)（包括滑動(dòng)、滾動(dòng)、行走、爬行、跳躍、飛行等）。 （4） 系統(tǒng)應(yīng)用與集成化：支持以人為核心的生產(chǎn)系統(tǒng)，實(shí)現(xiàn)生產(chǎn)系統(tǒng)中機(jī)器人群體協(xié)調(diào)功能、群智能和多機(jī)通訊協(xié)議，開發(fā)能理解人的意志的“同事機(jī)器人”。國(guó)外專家預(yù)測(cè)，2020 以后有可能 IMS 要走向 MA(R)S（多智能體系統(tǒng)），而該系統(tǒng)中的“同事機(jī)器人”（Cobot）將成為操作人員不可或缺的伙伴。圍繞著各種機(jī)器人與人共存的諸多課題，正在興起一門新學(xué)科“軟機(jī)器人學(xué)”。 （5） 安全可靠性：由于大量不確定因素的存在，要實(shí)現(xiàn)智能化的安全可靠性， 機(jī)器人必須具有對(duì)各種意外情況的應(yīng)變能力，及時(shí)采取預(yù)防措施和安全對(duì)策，包括硬件級(jí)、軟件級(jí)、應(yīng)用級(jí)和人機(jī)系統(tǒng)級(jí)的自診斷和自修復(fù)故障。 （6） 微型化：向微型化發(fā)展，開發(fā)毫米級(jí)機(jī)器人，用于微加工、醫(yī)學(xué)、宇宙和海洋開發(fā)等領(lǐng)域。就使用性和成本來看，毫米級(jí)最可行。 （7） 多傳感器信息融合與配置技術(shù)：機(jī)器人的傳感器配置和融合技術(shù)在水泥生產(chǎn)過程控制和污水處理自動(dòng)控制系統(tǒng)中的應(yīng)用包括面向工藝過程的多傳感器融合和配置技術(shù)；采用智能傳感器的現(xiàn)場(chǎng)總線技術(shù)；面向工藝要求的新型傳感器研制。機(jī)電一體化智能傳感器：包括具有感知、自主運(yùn)動(dòng)、自清污（自調(diào)整、自適應(yīng)）的機(jī)電一體化傳感器研究；面向工藝要求的運(yùn)動(dòng)機(jī)構(gòu)設(shè)計(jì)、實(shí)現(xiàn)檢測(cè)和清污的自主運(yùn)動(dòng)；調(diào)節(jié)控制系統(tǒng)；機(jī)器人機(jī)構(gòu)和控制技術(shù)在傳感器設(shè)計(jì)中的應(yīng)用。 1.3 課題設(shè)計(jì)的主要內(nèi)容 （1） 了解工業(yè)機(jī)器人的功能和結(jié)構(gòu)，并提出六軸工業(yè)機(jī)械手結(jié)構(gòu)方案，為實(shí)現(xiàn)方案的可行化程度再進(jìn)行三維實(shí)體設(shè)計(jì)。機(jī)械結(jié)構(gòu)需模塊化、可重構(gòu)化，采用關(guān)節(jié)模塊中的伺服電機(jī)、減速機(jī)、檢測(cè)系統(tǒng)三位一體，由關(guān)節(jié)模塊、連桿模塊用重組方式構(gòu)造工業(yè)機(jī)械手整機(jī)。 （2） 了解工業(yè)機(jī)器人運(yùn)動(dòng)控制的原理，以用戶角度對(duì)工業(yè)機(jī)器人做控制分析， 學(xué)習(xí)機(jī)器人語言，其控制系統(tǒng)要基于 PC 機(jī)的開放型控制器，通過嵌入式的實(shí)操手柄和電腦無線通訊的仿真上位機(jī)進(jìn)行終端控制。本設(shè)計(jì)的研究重點(diǎn)在于控制系統(tǒng)的用戶開發(fā)，基于實(shí)驗(yàn)室現(xiàn)有的 Festo 六軸工業(yè)機(jī)械手，完成設(shè)計(jì)任務(wù)和具體實(shí)現(xiàn)。 2 六軸工業(yè)機(jī)械手整體結(jié)構(gòu)設(shè)計(jì) 工業(yè)機(jī)械手的整體結(jié)構(gòu)設(shè)計(jì)內(nèi)容是：基本設(shè)計(jì)參數(shù)，運(yùn)動(dòng)形式，整體構(gòu)型，驅(qū)動(dòng)系統(tǒng)。根據(jù)工業(yè)機(jī)械手的發(fā)展現(xiàn)狀及趨勢(shì)，對(duì)于六軸工業(yè)機(jī)械手關(guān)節(jié)軸的驅(qū)動(dòng)控制系統(tǒng)基本都采用伺服電動(dòng)機(jī)+增量編碼器，可形成反饋的閉環(huán)控制系統(tǒng)，達(dá)到高精度軌跡運(yùn)作當(dāng)然還有高精密的減速器，通常采用諧波減速器。六軸工業(yè)機(jī)械手的整體構(gòu)型包括腕部、小臂、大臂、腰部、臀部和基座等六個(gè)軸，大多數(shù)企業(yè)的機(jī)械手均相類似。本章主要描述工業(yè)機(jī)械手的參數(shù)化設(shè)計(jì)的方法和重要部件選型的方法。 2.1 機(jī)械手的設(shè)計(jì)參數(shù) 設(shè)計(jì)六軸工業(yè)機(jī)械手，首先根據(jù)工作任務(wù)確定機(jī)械手的工作空間，并規(guī)劃動(dòng)作， 制定各關(guān)節(jié)的節(jié)拍，分配各動(dòng)作時(shí)間，初步確定各個(gè)關(guān)鍵的運(yùn)動(dòng)速度。接著確定在工作空間內(nèi)的各個(gè)關(guān)節(jié)臂的運(yùn)動(dòng)行程和臂長(zhǎng)等參數(shù)。然后根據(jù)任務(wù)對(duì)機(jī)械手進(jìn)行粗略的受力分析確定末端的許用載荷等。本文以 FESTO 的六軸工業(yè)機(jī)械手為模板做參數(shù)設(shè)計(jì)。如機(jī)械手的側(cè)視圖和俯視圖可大致給出機(jī)械手手腕中心點(diǎn)的工作范圍，如圖 2.1 所示。 圖 2.1 六軸工業(yè)機(jī)械手工作空間示圖 參考 FESTO 六軸工業(yè)機(jī)械手的基本參數(shù)，擬定出機(jī)械手各關(guān)節(jié)的運(yùn)動(dòng)和物理特性等的基本參數(shù)，如下列表各所示。 表 2.1 六軸工業(yè)機(jī)械手運(yùn)動(dòng)參數(shù) 運(yùn)動(dòng)軸 工作范圍 最大速度 旋轉(zhuǎn)軸 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 旋轉(zhuǎn)軸 6 -400°~ +400° 420°/s 表 2.2 六軸工業(yè)機(jī)械手物理特性 基座尺寸 180 * 180 mm 高度 700 mm 工作范圍 580 mm 重量 25 kg 有效載荷 3 kg 2.2 機(jī)械手的傳動(dòng)方案 選用伺服電機(jī)作為各關(guān)節(jié)的動(dòng)力裝置，電機(jī)本身就就被高精度的優(yōu)點(diǎn)。但關(guān)鍵還是在于輸入軸和輸出軸之間的齒輪箱。齒輪箱本身就要有傳動(dòng)效率高體積小等等的要求，一般安裝在工業(yè)機(jī)械手的各個(gè)關(guān)節(jié)處。根據(jù)常見的六軸工業(yè)機(jī)械手都采用諧波減速器。六軸工業(yè)機(jī)械手的齒輪箱的總體設(shè)計(jì)位置圖，如圖 2.2 所示。 圖 2.2 六軸工業(yè)機(jī)械手齒輪箱的位置 注釋 1）A-齒輪箱，軸 1（機(jī)座內(nèi)）；B-齒輪箱，軸 2；C-齒輪箱，軸 3； D-齒輪箱，軸 4；E-齒輪箱，軸 5；F-齒輪箱，軸 6； 對(duì)于關(guān)節(jié)型工業(yè)機(jī)械手，越靠近起始端的的減速箱越為重要，相互比較，重要程度逐遠(yuǎn)遜次。始端軸 1 的減速箱輸出 1 度的轉(zhuǎn)動(dòng)，與末端軸 6 的減速箱輸出 1 度的轉(zhuǎn)動(dòng)，帶來的運(yùn)動(dòng)行程相差得有幾倍甚至幾十倍。 上一級(jí)電機(jī)作動(dòng)力源，經(jīng)減速器齒輪箱減速，將力矩放大，驅(qū)動(dòng)下一級(jí)軸臂。軸臂粗大的，臂內(nèi)的電機(jī)與齒輪箱放置比較靈活，齒輪箱可以設(shè)計(jì)的大小越大，則該齒輪箱的精度和傳動(dòng)比就可以設(shè)計(jì)得越高。對(duì)于較短的軸臂，其電機(jī)與齒輪箱緊挨著， 即電機(jī)的輸出軸直接傳入齒輪箱，如軸 1、2、4、6，其中 1、4、6 軸的輸出都為旋轉(zhuǎn)。而對(duì)于較長(zhǎng)的關(guān)節(jié)軸臂，電機(jī)可以安裝在離齒輪箱比較遠(yuǎn)的地方，選用帶傳動(dòng)或鏈傳動(dòng)形式，如軸 3、5。從圖中可以看出，越末端的電機(jī)和齒輪箱越小。 2.3 基本模型的參數(shù)化設(shè)計(jì) 根據(jù)六軸工業(yè)機(jī)械手的不同工作任務(wù)對(duì)其工作空間的不同要求，同一型號(hào)的工業(yè)機(jī)械手通過對(duì)各關(guān)節(jié)軸的伸縮擺動(dòng)，即可達(dá)到工作范圍的很大擴(kuò)展。為了在設(shè)計(jì)中減輕負(fù)擔(dān)，同型號(hào)的工業(yè)機(jī)械手的結(jié)構(gòu)，選擇參數(shù)化設(shè)計(jì)方法，以各關(guān)節(jié)的軸線在空間 中的位置關(guān)系構(gòu)成基礎(chǔ)參數(shù)。通過各關(guān)節(jié)軸線相對(duì)位置的平移旋轉(zhuǎn)進(jìn)行調(diào)整，實(shí)現(xiàn)設(shè)計(jì)中工業(yè)機(jī)械手各零部件的尺寸變化，從而擬定出六軸工業(yè)機(jī)械手各個(gè)關(guān)節(jié)軸線的位置關(guān)系，如圖 2.3 所示。 圖 2.3 六軸工業(yè)機(jī)械手各關(guān)節(jié)軸坐標(biāo)系 根據(jù)工業(yè)機(jī)械手的基本參數(shù)則能擬定出機(jī)械手各個(gè)關(guān)節(jié)軸線的位置尺寸，那么就可以在三維建模軟件上對(duì)六軸工業(yè)機(jī)械手進(jìn)行整體結(jié)構(gòu)建模，可利用 Solidworks 的測(cè)量功能，可得到模型的結(jié)構(gòu)和質(zhì)量屬性等參數(shù)，如圖 2.4 所示。 圖 2.4 六軸工業(yè)機(jī)械手的整體結(jié)構(gòu) 2.4 伺服電機(jī)和減速器的選型計(jì)算 伺服電機(jī)及減速器齒輪箱的選型尤為重要，所選型號(hào)要滿足機(jī)械手正常運(yùn)作的最大轉(zhuǎn)速 n 和穩(wěn)定工作的最大力矩 J 的需求。而電機(jī)和減速器在不同方式工作時(shí)，所選參數(shù)亦不同，當(dāng)周期性工作時(shí)，力矩的參數(shù)應(yīng)該采用機(jī)械手最大輸出的加速力矩。減速器齒輪箱的傳動(dòng)比也是重要的參數(shù)。故，一般對(duì)機(jī)械手的驅(qū)動(dòng)電機(jī)與關(guān)節(jié)軸的齒輪箱一起作選型。 2.4.1 選型方法 首先是轉(zhuǎn)速，為實(shí)現(xiàn)低功耗，額定轉(zhuǎn)速為重要參數(shù)，實(shí)際的轉(zhuǎn)速最好不超過額定轉(zhuǎn)速。如果電機(jī)正好處于額定轉(zhuǎn)速的工作時(shí)，那么就是機(jī)械手關(guān)節(jié)處于最大轉(zhuǎn)速的時(shí)候，其速度可由電機(jī)額定轉(zhuǎn)速比上齒輪箱的傳動(dòng)比求得。 其次是轉(zhuǎn)矩，額定轉(zhuǎn)矩也是重要的參數(shù)，實(shí)際的轉(zhuǎn)矩也最好不要超過額定轉(zhuǎn)矩。結(jié)合機(jī)械手的結(jié)構(gòu)尺寸和軌跡，受力分析，可計(jì)算出承載的最大力矩，應(yīng)小于減速器齒輪箱輸出的力矩，折算至電機(jī)輸出端，也應(yīng)小電機(jī)額定力矩。由于減速器齒輪箱的傳動(dòng)效率并不是 100%，因此它們所受力矩也存在些差異，它們承受的力矩關(guān)系如下式： （2.1） 式中： 電機(jī)的輸出力矩，N ? m； ---- 減速器的輸出力矩，N ? m； ---- 減速器的傳動(dòng)比； ---- 傳動(dòng)效率； 同理機(jī)械手在實(shí)際運(yùn)動(dòng)的過程中，其實(shí)際的加速力矩也不得超過齒輪箱的最大輸出力矩，再根據(jù)齒輪箱的傳動(dòng)效率，可算出電機(jī)的實(shí)際加速力矩，這個(gè)加速力矩也不得超過電機(jī)的額定加速力矩。 最后是慣量匹配，電機(jī)自身慣量與負(fù)載自身慣量符合一定比例稱之為慣量匹配。慣量是否符合匹配要求，通過慣量比的大小來看，一般電機(jī)轉(zhuǎn)子的慣量作為分母，負(fù)載的慣量作為分子，以 表示，如下式： （2.2） 式中： 電機(jī)的許用慣量比； ---- 電機(jī)轉(zhuǎn)子轉(zhuǎn)動(dòng)慣量， ； ---- 負(fù)載轉(zhuǎn)動(dòng)慣量， 。 根據(jù)設(shè)計(jì)經(jīng)驗(yàn) 的值應(yīng)盡量小于 10 則算是匹配，假設(shè) 與 不匹配，動(dòng)量傳遞會(huì)出現(xiàn)激烈的沖撞；根據(jù)下式分析。 ( ) （2.3） 式中： ---- 電機(jī)轉(zhuǎn)矩，N ? m； ---- 負(fù)載轉(zhuǎn)矩，N ? m； ---- 電機(jī)轉(zhuǎn)子轉(zhuǎn)動(dòng)慣量， ； ---- 負(fù)載轉(zhuǎn)動(dòng)慣量， 。 ---- 角加速度； 在一個(gè)工作任務(wù)中 電機(jī)轉(zhuǎn)矩和 負(fù)載轉(zhuǎn)矩通常不變，而電機(jī)一旦選定確認(rèn)后， 電機(jī)轉(zhuǎn)子轉(zhuǎn)動(dòng)慣量則也為定值。故，若想 負(fù)載轉(zhuǎn)動(dòng)慣量變小，則 角加速度就要加大。 2.4.2 六軸工業(yè)機(jī)械手末端軸 6 的電機(jī)及減速器的選型計(jì)算 末端軸所承受的外力主要由末端執(zhí)行器和工件產(chǎn)生，其中占主要的還是末端執(zhí)行器。當(dāng)末端軸帶動(dòng)末端執(zhí)行器和工件旋轉(zhuǎn)時(shí)，承受的阻力矩有負(fù)載轉(zhuǎn)矩和摩擦力矩， 而摩擦力矩過小可忽略不計(jì)。在此先計(jì)算工件的轉(zhuǎn)動(dòng)慣量，再計(jì)算末端執(zhí)行器的轉(zhuǎn)動(dòng)慣量。 首先計(jì)算工件的轉(zhuǎn)動(dòng)慣量，設(shè)工件為長(zhǎng) a、寬 b、高 h 的矩形體， 圖 2.5 方形工件 工件繞其質(zhì)心軸線旋轉(zhuǎn)時(shí)，根據(jù)轉(zhuǎn)動(dòng)慣量公式： ?? ?? { ??  ??(?? + ? ) 1 ??(?? + ?? ) （2.4） 1 ??(?? + ? ) 1 可根據(jù)實(shí)際任務(wù)，可給工件一個(gè)具體尺寸：a = 0.05m，b=0.05m，h=0.05m 即正方體，工件質(zhì)量 m=1kg，那么三個(gè)質(zhì)心軸線的轉(zhuǎn)動(dòng)慣量值一樣。故，公式（4）可簡(jiǎn)化為： ?? ?? （2.5） 6 式中的 i 為任意軸向。因?yàn)楣ぜ蛔ト∵^程中主要繞其質(zhì)心 y 軸旋轉(zhuǎn)，所以 X， Z 軸的轉(zhuǎn)動(dòng)慣量可以不計(jì)，將數(shù)據(jù)帶入（5）中計(jì)算得到： ??件  ?? 6 1 × 0.05 6  4.17 × 10?4 kg 接著計(jì)算末端執(zhí)行器的轉(zhuǎn)動(dòng)慣量，由于末端執(zhí)行器的結(jié)構(gòu)較為復(fù)雜，不像工件為 矩形體或圓柱體，具有公式計(jì)算。在此利用 Solidworks 軟件的測(cè)量功能得到末端執(zhí)行器的轉(zhuǎn)動(dòng)慣量，同理只考慮取末端執(zhí)行器 y 軸的轉(zhuǎn)動(dòng)慣量： ?? 具 13.93 × 10?4 kg 然后將末端執(zhí)行器的轉(zhuǎn)動(dòng)慣量與工件轉(zhuǎn)動(dòng)慣量相加可得末端軸的負(fù)載慣量： 末 ?? 件 ?? 具 18.1 × 10?4 kg 假設(shè)執(zhí)行的任務(wù)需要末端軸在 0.1 秒內(nèi)到達(dá)的最高轉(zhuǎn)速為： 7?? ?? ???? 末 ????????? 3  70 ??? ???? 則轉(zhuǎn)軸在 0.1 秒內(nèi)的瞬間角加速度為： ??末 ?? ???? 末 ??  7?? 3 0.1  70?? 3  ????????? 隨著加速度的產(chǎn)生，負(fù)載也會(huì)相對(duì)應(yīng)產(chǎn)生一個(gè)慣性力矩，并與??成正比： 末 末  ??末  18.1 × 10?4 × 70?? 3 ≈ 0.14 N m 最后將上訴所計(jì)算的值帶入電機(jī)及減速箱的選型方法的折算公式中，根據(jù)所選減速器齒輪箱的傳動(dòng)比返求驗(yàn)算，則可簡(jiǎn)完成末端軸的電機(jī)和減速器的選型。 2.4.3 六軸工業(yè)機(jī)械手軸 1、4 的電機(jī)及減速器的選型計(jì)算 軸 1 的電機(jī)及減速器輸出轉(zhuǎn)矩為其他各個(gè)軸再加上末端執(zhí)行器及工件的轉(zhuǎn)矩總和。軸 4 的電機(jī)及減速器輸出轉(zhuǎn)矩為軸 5、6 再加上末端執(zhí)行器及工件的轉(zhuǎn)矩總和。它們結(jié)構(gòu)復(fù)雜，難以精確計(jì)算出轉(zhuǎn)動(dòng)慣量。但可直接利用三維建模軟件的測(cè)量功能， 將三維模型分別擬動(dòng)為對(duì)軸 1、4 產(chǎn)生最大的轉(zhuǎn)動(dòng)慣量位置，展開測(cè)量得： 軸 1 0.837 kg 軸 4 0.023 kg 假設(shè)執(zhí)行的任務(wù)需要軸 1、4 在 0.1 秒內(nèi)到達(dá)的最高轉(zhuǎn)速為： 4?? ?? ???? 軸 1 ?? ???? 軸 4 ????????? 3 5?? ????????? 3  40 ??? ???? 50 ??? ???? 則兩轉(zhuǎn)軸在 0.1 秒內(nèi)的瞬間角加速度為： ??軸 1 ??軸 4 ?? ???? 軸 1 ?? ?? ???? 軸 4 ??  4?? 3 0.1 5?? 3 0.1  40?? 3 50?? 3  ????????? ????????? 隨著角加速度的產(chǎn)生，負(fù)載也會(huì)相對(duì)應(yīng)產(chǎn)生一個(gè)慣性力矩，并與角加速度??成正比，可求出慣性力矩： 軸 1 軸 1 ?? 軸 1 0.837 × 軸 4 軸 4 ?? 軸 4 0.023 × 40?? ≈ 35.06 N m 3 50?? ≈ 1.21 N m 3 最后將上訴所計(jì)算的值帶入電機(jī)及減速箱的選型方法的折算公式中，根據(jù)所選減速器齒輪箱的傳動(dòng)比返求驗(yàn)算，則可簡(jiǎn)完成軸 1 和軸 4 的電機(jī)及減速器的選型。 2.4.4 六軸工業(yè)機(jī)械手 2、3、5 軸的電機(jī)及減速器的選型計(jì)算 首先結(jié)合工業(yè)機(jī)械手的設(shè)計(jì)參數(shù)，將機(jī)械手至于各軸關(guān)節(jié)受靜態(tài)力最大的位置及姿態(tài)，如下圖 2.5 所示： 圖 2.6 末端執(zhí)行器伸直最遠(yuǎn)處 圖 2.7 平面平衡分析 計(jì)算軸 2 的輸出靜態(tài)轉(zhuǎn)矩，將六軸機(jī)械手分成兩個(gè)部分，其中軸 1、2 為固定不變的第一整體，軸 3、4、5、6 和末端執(zhí)行器 7 為另一個(gè)固定的第二整體。第二整體的重心大致位于 的中點(diǎn)處，利用三維建模軟件的測(cè)量功能，測(cè)得第二整體的質(zhì)量為17.74 kg。要想機(jī)械手實(shí)現(xiàn)靜態(tài)力矩平衡，則可算出關(guān)節(jié)的輸出力矩如下： 2 × 4 6 754 × 10? × 17.74 × 9.8 65.54 N m 2 計(jì)算軸 3 的輸出靜態(tài)轉(zhuǎn)矩，將軸 1、2、3 視為固定不變的第一整體，軸 4、5、6 和末端執(zhí)行器 7 視為另一個(gè)固定的第二整體。第二整體的重心大致位于 的中點(diǎn)處， 利用三維建模軟件的測(cè)量功能，測(cè)得第二整體的質(zhì)量為 10.04 kg。要想機(jī)械手實(shí)現(xiàn)靜態(tài)力矩平衡，則可算出關(guān)節(jié)的輸出力矩如下： 2 × 4 6 484 × 10? × 10.04 × 9.8 23.81 N m 2 計(jì)算軸 5 的輸出靜態(tài)轉(zhuǎn)矩，將軸 1、2、3、4、5 視為固定不變的第一整體，軸 6 和末端執(zhí)行器 7 視為另一個(gè)固定的第二整體。第二整體的重心大致位于 的中點(diǎn)處， 利用三維建模軟件的測(cè)量功能，測(cè)得第二整體的質(zhì)量為 2.87 kg。要想機(jī)械手實(shí)現(xiàn)靜態(tài)力矩平衡，則可算出關(guān)節(jié)的輸出力矩如下： 2 × 6 182 × 10? × 2.87 × 9.8 2.56 N m 2 最后將上訴所計(jì)算的值帶入電機(jī)及減速箱的選型方法的折算公式中，根據(jù)所選減速器齒輪箱的傳動(dòng)比返求驗(yàn)算，則可簡(jiǎn)完成軸 2、軸 3 和軸 4 的電機(jī)及減速器的選型。 I/O 3 六軸工業(yè)機(jī)械手控制系統(tǒng)設(shè)計(jì) 本章對(duì)六軸工業(yè)機(jī)械手的通用控制系統(tǒng)展開設(shè)計(jì)，主要是機(jī)械手的伺服驅(qū)動(dòng)系統(tǒng)的設(shè)計(jì)。通過理論分析，將驅(qū)動(dòng)控制系統(tǒng)理論算法轉(zhuǎn)換為具體的實(shí)用算法，根據(jù)設(shè)計(jì)框圖，利用 C 語言簡(jiǎn)單易懂的特性，設(shè)計(jì)出 PID 閉環(huán)控制傳遞函數(shù)。 3.1 控制系統(tǒng)的硬件結(jié)構(gòu) 工業(yè)機(jī)械手控制器其核心大腦，運(yùn)行著各種復(fù)雜運(yùn)算算法，其內(nèi)核時(shí)鐘基本要上 G 級(jí)別，處理著各種輸入信號(hào)的同時(shí)，也輸出各種控制信號(hào)。首先是編程示教器將各程序命令傳入主機(jī)中，與主機(jī)直接通訊，構(gòu)成人機(jī)交互。其次主機(jī)在于控制總線上的各個(gè)模塊進(jìn)行通訊，各模塊包括傳感器、伺服驅(qū)動(dòng)模塊和其他 I/O 設(shè)備，它們各自都有獨(dú)一無二的總線通訊地址。最后終結(jié)于伺服驅(qū)動(dòng)系統(tǒng)對(duì)伺服電機(jī)的閉環(huán)控制，實(shí)現(xiàn)機(jī)械手的運(yùn)作，它們共同構(gòu)成了工業(yè)機(jī)械手控制系統(tǒng)的硬件結(jié)構(gòu)，如下圖 3.1 所示： I/O 圖 3.1 工業(yè)機(jī)械手控制系統(tǒng)的硬件結(jié)構(gòu) CPU 主機(jī)配備 RTOS，具有多線程、實(shí)時(shí)性和穩(wěn)定性。數(shù)據(jù)存儲(chǔ)、信息打印、信息處理顯示、監(jiān)控信息顯示，數(shù)據(jù)二次利用等等，關(guān)鍵在于機(jī)械手運(yùn)動(dòng)學(xué)的實(shí)時(shí)計(jì)算， 規(guī)劃軌跡。機(jī)械手的末端執(zhí)行器的位置及姿態(tài)由多個(gè)關(guān)節(jié)的運(yùn)動(dòng)合成，比如末端執(zhí)行器走一個(gè)直線，需要各個(gè)軸的運(yùn)動(dòng)配合，有的軸加速，有的軸不用動(dòng)，這些都有建立數(shù)學(xué)模型和智能算法來實(shí)現(xiàn)。 示教器編寫程序時(shí)，要用到機(jī)器人語言，其示教方式主要分為實(shí)際示教法和不實(shí)際示教法。機(jī)械手運(yùn)作的示教信息大概可分為三部分：順序信息、姿態(tài)與位置信息和作業(yè)條件及動(dòng)作信息。 3.2 機(jī)械手的伺服控制系統(tǒng) 對(duì)機(jī)械手實(shí)際運(yùn)作中的末端位姿進(jìn)行實(shí)時(shí)監(jiān)測(cè)是很困難的。為提高控制精度，機(jī)械手的電機(jī)控制必須選擇閉環(huán)的方式去控制，而常用的閉環(huán)控制為 PID。有些機(jī)械手的控制器將驅(qū)動(dòng)集成在伺服電機(jī)中，控制效果好，制造成本低。還有些控制器利用電機(jī)驅(qū)動(dòng)實(shí)現(xiàn)電流、電壓和速度閉環(huán)，完成機(jī)械手位置控制，如下圖 3.2 所示： 圖 3.2 機(jī)械手閉環(huán)伺服控制系統(tǒng) 假設(shè)電機(jī)驅(qū)動(dòng)與伺服電機(jī)的動(dòng)力學(xué)函數(shù)為： ( ) ( ?? ??) (??) (??) （3.1） 式中： ( )----電機(jī)控制電壓； (??) 電機(jī)轉(zhuǎn)角； (??) 干擾信號(hào)； ----電機(jī)轉(zhuǎn)動(dòng)慣量； ----有效阻尼。 3.2.1 PD 控制系統(tǒng) 在位置式閉環(huán)控制系統(tǒng)中常用 PD 控制器作為控制系統(tǒng)，其系統(tǒng)框圖如下： 圖 3.3 PD 控制器 這時(shí)，電機(jī)的輸入控制量是： ( ) ( )( ( ) ( )) （3.2） 式中： ( )----目標(biāo)角度值。 ----比例項(xiàng)系數(shù)。 ----微分項(xiàng)系數(shù)。 由于干擾信號(hào) (??)的存在，則閉環(huán)系統(tǒng)可寫為： ( ) + ( ) 1  (??) （3.3） ( ) 式中的 1(??)為特征多項(xiàng)式：  ( ) 1(??) ?? ( ) ?? （3.4） 式（3.4）為二階系統(tǒng)，主要參數(shù)有 阻尼系數(shù)以及 固有頻率： +  2 {  2  （3.5） 一般 阻尼系數(shù)的值為 1， 和 作為 PD 閉環(huán)的重要參數(shù)，調(diào)節(jié)參數(shù)使之穩(wěn)定。那么 PD 系統(tǒng)的跟隨誤差為： ( ) ( ) ( ) + ( ) 1  (??) （3.6） ( ) ( ) 如果系統(tǒng)產(chǎn)生階躍信號(hào) ( ) 和一個(gè)固定干擾信號(hào) ，利用終值定理得到控制環(huán)的穩(wěn)態(tài)誤差為： m ?? (??) （3.7） 可見當(dāng) 增大時(shí)，穩(wěn)態(tài)誤差則會(huì)變小。 3.2.2 PID 控制系統(tǒng) 為消除靜態(tài)溫差，對(duì) PD 系統(tǒng)進(jìn)行改良，增加積分項(xiàng)，即 PID 控制系統(tǒng)，其控制框圖如下： 圖 3.4 PID 控制器 則閉環(huán)系統(tǒng)可寫為： ( ) + + ( )  (??) （3.8） ( ) ( ) 式中 為積分項(xiàng)系數(shù)，那么其特征多項(xiàng)式： (??) ?? ( ) ?? ?? （3.9） 利用勞斯判據(jù)來判斷系統(tǒng)是否處于穩(wěn)定狀態(tài)，系統(tǒng)增益不為負(fù)數(shù)，并且符合下式 條件： ( + ) （3.10） 3.3 基于C 語言實(shí)現(xiàn)的閉環(huán)控制算法 對(duì)于電機(jī)而言，只有扭矩、轉(zhuǎn)速和轉(zhuǎn)動(dòng)方向等基本參數(shù)。如果給電機(jī)的轉(zhuǎn)軸加上一個(gè)指針，那么指針的方向可以是任意的，在經(jīng)過一段時(shí)間的轉(zhuǎn)動(dòng)后電機(jī)停止，根本就不知道指針的所指的方向。從這個(gè)角度分析，將電機(jī)的轉(zhuǎn)動(dòng)分為大于一周以上和在一周之內(nèi)。 圖 3.5 單關(guān)節(jié)控制反饋框圖 3.3.1 增量式 PID 控制 增量式 PID 控制對(duì)應(yīng)的是增量式編碼器作為轉(zhuǎn)軸的狀態(tài)反饋傳感器。增量式編碼器并不能確定轉(zhuǎn)軸的位置，當(dāng)轉(zhuǎn)軸轉(zhuǎn)動(dòng)一個(gè)小角度時(shí)，編碼器會(huì)反饋一個(gè)脈沖信號(hào)， 當(dāng)轉(zhuǎn)軸轉(zhuǎn)動(dòng)速度越快則編碼器單位時(shí)間內(nèi)反饋的脈沖數(shù)越多。一般增量式編碼器安裝在電機(jī)的輸出端，因?yàn)槟抢镛D(zhuǎn)軸基本上都運(yùn)行在很多周以上，專門用于檢測(cè)電機(jī)的轉(zhuǎn)速。 根據(jù)增量式 PID 控制控制原理，設(shè)計(jì)實(shí)際運(yùn)用算法，在此利用 C 語言程序進(jìn)行PID 閉環(huán)控制傳遞函數(shù)的算法設(shè)計(jì)，程序如下： //定義 PID 的比例項(xiàng)系數(shù) P 及參數(shù)賦值int speedP = 21; //定義 PID 的積分項(xiàng)系數(shù) I 及參數(shù)賦值int speedI = 16; //定義 PID 的微分項(xiàng)系數(shù) D 及參數(shù)賦值int speedD = 0; //目標(biāo)速度 int My_speed = 0; //全局變量，上一次的偏差 long int ierror1; //全局變量，上上一次的偏差 long int ierror2; //全局變量，上一次的速度控制量 long int oldspeedPWM; //增量式 PID 閉環(huán)控制系統(tǒng)的傳遞函數(shù)long int speedPID(int speed) { //定義計(jì)算的中間變量 long int ierror,P,I,D, speedPWM; //計(jì)算當(dāng)前偏差ierror=My_speed-speed; //計(jì)算積分項(xiàng)I=speedI*ierror; //計(jì)算比例項(xiàng)P=speedP*(ierror-ierror1); //計(jì)算微分銷 D=speedD*(ierror-ierror1-ierror1+ierror2); //上次偏差存為上上次偏差 ierror2=ierror1; //當(dāng)前偏差存為上次偏差 ierror1=ierror; //計(jì)算出當(dāng)前速度控制量 speedPWM=oldspeedPWM+P+I+D; //當(dāng)前控制量存為上次控制量 oldspeedPWM=speedPWM; //對(duì)當(dāng)前速度控制量限幅 if(speedPWM>7500) speedPWM=7500; if(speedPWM<(-7500)) speedPWM=(-7500); //返回當(dāng)前控制量return speedPWM; } 通過調(diào)節(jié) PID 的三個(gè)項(xiàng)系數(shù)，實(shí)現(xiàn)對(duì)電機(jī)的最佳控制，可以通過 PID 調(diào)節(jié)上位機(jī) 來模擬實(shí)際參數(shù)的確定。 3.3.2 位置式 PD 控制 位置式 PD 控制對(duì)應(yīng)的是位置式編碼器作為轉(zhuǎn)軸的狀態(tài)反饋傳感器。位置式編碼器通常用在轉(zhuǎn)軸的轉(zhuǎn)動(dòng)范圍在一周之內(nèi)，轉(zhuǎn)軸有唯一對(duì)應(yīng)的位置。而工業(yè)機(jī)械手關(guān)節(jié)的擺動(dòng)范圍就算在一定角度之內(nèi)，將位置式編碼器安裝在減速器的輸出端，能實(shí)時(shí)反饋機(jī)械手關(guān)節(jié)臂的絕對(duì)位置。 根據(jù)位置式 PD 控制控制原理，設(shè)計(jì)實(shí)際運(yùn)用算法，在此利用 C 語言程序進(jìn)行 PID 閉環(huán)控制傳遞函數(shù)的算法設(shè)計(jì)，程序如下： //定義 PD 的比例項(xiàng)系數(shù) P 及參數(shù)賦值float placeP=33.0; //定義 PD 的比例項(xiàng)系數(shù) D 及參數(shù)賦值float placeD=20.0; //全局變量，上一次的位置偏差float oldplace=0.0; //位置式 PD 閉環(huán)控制系統(tǒng)的傳遞函數(shù)float placePD(float place) { //定義計(jì)算的中間變量 float ierror,P,D,newplace; //計(jì)算當(dāng)前位置偏差 ierror=place-oldplace; //計(jì)算比例項(xiàng) P=placeP*place; //計(jì)算微分項(xiàng) D=placeP*placeD*ierror; //計(jì)算當(dāng)前位置控制量 newplace=midle_servo+P+D; //當(dāng)前位置偏差存為上次位置偏差 oldplace=place; //當(dāng)前位置控制量限幅 if(newplace>youjixian_servo) newplace=youjixian_servo; if(newplace

收藏