機(jī)械手-搬運(yùn)機(jī)械手控制系統(tǒng)的設(shè)計(jì)(不含CAD圖紙)

機(jī)械手-搬運(yùn)機(jī)械手控制系統(tǒng)的設(shè)計(jì)(不含CAD圖紙),機(jī)械手,搬運(yùn),控制系統(tǒng),設(shè)計(jì),cad,圖紙 畢業(yè)論文（設(shè)計(jì)）任務(wù)書論文（設(shè)計(jì)）題目：搬運(yùn)機(jī)械手控制系統(tǒng)的設(shè)計(jì) 一、主要內(nèi)容及基本要求在本次搬運(yùn)機(jī)械手控制系統(tǒng)的設(shè)計(jì)中，要完成的任務(wù)有如下幾個(gè)方面： 1、用機(jī)器人運(yùn)動(dòng)學(xué)求得搬運(yùn)機(jī)械手的正解； 2、由運(yùn)動(dòng)學(xué)方程解得搬運(yùn)機(jī)械手各關(guān)節(jié)變量，并求出軌跡方程； 3、用 VB 建立人機(jī)交互界面，并根據(jù)軌跡方程畫出軌跡曲線； 4、實(shí)現(xiàn) VB 界面與 PLC 的通信，以及對(duì)搬運(yùn)機(jī)械手的控制； 5、寫一份 8000 字以上的畢業(yè)設(shè)計(jì)說明書； 6、一篇不少于 3000 單詞的英文原文及中文翻譯。 二、重點(diǎn)研究的問題1、搬運(yùn)機(jī)械手運(yùn)動(dòng)學(xué)方程正解的求解； 2、建立 VB 界面，實(shí)現(xiàn)計(jì)算以及畫出軌跡曲線； 3、實(shí)現(xiàn)搬運(yùn)機(jī)械手的自動(dòng)控制。 三、應(yīng)收集的資料及主要參考文獻(xiàn)1、劉極峰，易際明.機(jī)器人技術(shù)基礎(chǔ)[M].北京：高等教育出版社，2006 年. 2、張鐵.機(jī)器人學(xué)[M].廣州：華南理工大學(xué)出版社，2000 年. 3、從爽.實(shí)用運(yùn)動(dòng)控制技術(shù)[M].北京：電子工業(yè)出版社，2006 年. 4、鐘肇新，范建東.可編程控制器原理及應(yīng)用[M].廣州：華南理工大學(xué)出版社，2003年 5、俞建家.Visual Basic 6.0 程序設(shè)計(jì)與應(yīng)用教程[M].福建：廈門大學(xué)出版社，2003 年 6、呂偉臣.Visual Basic 6.0 初級(jí)編程教程[M]. 北京：北京大學(xué)出版社， 2002 年. 7、大熊.機(jī)器人控制[M].北京：科學(xué)出版社，2002 年. 8、張海根.機(jī)電傳動(dòng)控制[M].北京：高等教育出版社，2005 年. 9、丹尼斯.機(jī)器人設(shè)計(jì)與控制[M].北京：科學(xué)出版社，2004 年. 10、劉極峰.計(jì)算機(jī)輔助設(shè)計(jì)與制造[M].北京：高等教育出版社，2004 年. 目 錄摘要 1第 1 章 引言 .21. 1 課題研究趨勢(shì)與意義 21.2 串聯(lián)關(guān)節(jié)機(jī)器人的發(fā)展和現(xiàn)狀 .31.3 本文的研究?jī)?nèi)容和主要工作 .3第 2 章 PLC 控制系統(tǒng)及實(shí)現(xiàn) 52.1 機(jī)械手及控制器主要參數(shù) .52.2 簡(jiǎn)述機(jī)械手動(dòng)作的實(shí)現(xiàn) .62.3 控制軟件的設(shè)計(jì) .62.3.1 PLC 的選擇 .62.3.2 光電編碼器 62.3.3 接口電路 72.3.4 控制原理及程序 73.1 引言 .93. 2 機(jī)械手運(yùn)動(dòng)學(xué)數(shù)學(xué)基礎(chǔ) 93.2.1 機(jī)器人位置與姿態(tài)的描述 [12].93.3 空間齊次坐標(biāo)變換 .113.3.1 坐標(biāo)變換 .113.3.2 齊次坐標(biāo)變換 .123.3 點(diǎn)在空間直角坐標(biāo)系中繞過原點(diǎn)任意軸的一般旋轉(zhuǎn)變換 133.4 Denavt-Hartenberg(D-H)表示法 .153.4.1 坐標(biāo)系的建立 153.4.2 幾何參數(shù)定義 163.4.3 建立坐標(biāo)系和坐標(biāo)系的齊次變換矩陣 16第 4 章 機(jī)器人運(yùn)動(dòng)學(xué)方程的求解 184.1 機(jī)器人正向運(yùn)動(dòng)學(xué) .184.2 機(jī)器人逆向運(yùn)動(dòng)學(xué) .20第 5 章 機(jī)械手軌跡規(guī)劃 225.1 機(jī)器人軌跡的概念 225.2 軌跡的生成方式 225.2.1 軌跡規(guī)劃涉及的主要問題 225.3 插補(bǔ)方式分類 235.4 機(jī)器人軌跡插值計(jì)算 235.4.1 三次多項(xiàng)式插值 245.4.2 機(jī)械手軌跡規(guī)劃。 .25第 6 章 系統(tǒng)交流界面設(shè)計(jì) 276.1Visual Basic6.0 簡(jiǎn)介 276.2 VB 設(shè)計(jì)任務(wù) .286.3 窗體設(shè)計(jì) 286.4 窗體運(yùn)行 .29參考文獻(xiàn) .22致謝 .33附錄：程序清單 .34附錄：程序清單 .50附錄：外文原文 .55如需聯(lián)系作者 QQ 4013398281搬運(yùn)機(jī)械手控制系統(tǒng)的設(shè)計(jì)摘 要:本文運(yùn)用運(yùn)動(dòng)學(xué)的研究方法建立了串聯(lián)機(jī)械手運(yùn)動(dòng)模型，并用 Visual Basic 6.0通過逆運(yùn)動(dòng)學(xué)對(duì)其運(yùn)動(dòng)軌跡的規(guī)劃做分析。首先，本文建立了參考坐標(biāo)系和運(yùn)動(dòng)坐標(biāo)系，給出串聯(lián)機(jī)械手在這兩個(gè)坐標(biāo)系中的描述方法，并且定義了串聯(lián)機(jī)械手的機(jī)構(gòu)參數(shù)，以及相關(guān)數(shù)學(xué)知識(shí)的介紹。在此基礎(chǔ)上，建立起了串聯(lián)機(jī)械手的正解模型。機(jī)械手的動(dòng)作過程由三菱公司生產(chǎn)的PLC實(shí)行控制。其次，通過利用串聯(lián)機(jī)械手的逆解模型結(jié)論，對(duì)機(jī)械手的軌跡進(jìn)行分析。此過程中 ，應(yīng)用了功能強(qiáng)大的Visual Basic 6.0，進(jìn)行自動(dòng)運(yùn)算，并利用其可視化功能做出簡(jiǎn)單人機(jī)交流界面，計(jì)算并顯示出機(jī)械手的軌跡曲線，使得機(jī)械手的軌跡規(guī)劃更為直觀、易懂。本文的研究工作對(duì)串聯(lián)關(guān)節(jié)型機(jī)械手的設(shè)計(jì)具有一定的指導(dǎo)意義。運(yùn)用本文的研究方法，可以有效提高機(jī)械手的設(shè)計(jì)質(zhì)量和縮短設(shè)計(jì)周期。關(guān)鍵詞：機(jī)械手，正解模型，Visual Basic 6.0，PLC，軌跡規(guī)劃2Design of Handing Manipulator Control System Abstract:This paper studies the use of kinematic method of Tandem manipulator motion model, using Visual Basic 6.0 inverse kinematics through its trajectory planning to do the analysis.First, this paper set up a reference coordinate system and coordinates movement, given the description of tandem manipulator in this two coordinates, and the definition of the series manipulator body parameters and the associated mathematical knowledge presentation. On this basis, established a series of positive manipulator model. Mechanical movement of the hands from the process of production of Mitsubishi Corporation PLC control. Secondly, through the use of serial manipulator inverse kinematics model concludes that the manipulator trajectory analysis. During this process, the application of the powerful Visual Basic 6.0, automatic operation, and the use of visualization functions made simple man-machine interface exchange, calculated and shown manipulator curves, make manipulator trajectory planning more intuitive, understandable.The research of this paper is very useful for the chain-structured robot. it willimprove robot design quality and shorten the design time.Key words:Manipulator, positive solution model, Visual Basic 6.0, PLC, trajectory planning 3第 1 章 引言1.1 工業(yè)機(jī)器人的發(fā)展和現(xiàn)狀機(jī)器人是一種被設(shè)計(jì)用來移動(dòng)物體、部件、工具或者特定設(shè)備的，可以重復(fù)編程、具有多種功能的操作器.它通過一系列可變的程控動(dòng)作來完成各種各樣的任務(wù)。通常工業(yè)機(jī)器人大致分為以下三類:1) 串聯(lián)結(jié)構(gòu)機(jī)器人2) 框架結(jié)構(gòu)機(jī)器人3) 并行機(jī)構(gòu)機(jī)器人從結(jié)構(gòu)上看，并行結(jié)構(gòu)機(jī)器人的運(yùn)動(dòng)平臺(tái)通過相互關(guān)聯(lián)的多個(gè)運(yùn)動(dòng)鏈與下平臺(tái)相連，這使并行結(jié)構(gòu)機(jī)器人具有剛性高、承載能力大和精度好等特點(diǎn)。它的主要缺點(diǎn)是控制復(fù)雜，使得它很難應(yīng)用高階控制方法，從而妨礙了這種機(jī)器人的應(yīng)用?？蚣芙Y(jié)構(gòu)機(jī)器人工作空間比較小，操作靈活性受到限制。串聯(lián)結(jié)構(gòu)機(jī)器人具有較大的工作空間和較高的運(yùn)動(dòng)靈巧度，常用的結(jié)構(gòu)是串聯(lián)關(guān)節(jié)形式，它采用串聯(lián)鏈?zhǔn)浇Y(jié)構(gòu)和全關(guān)節(jié)驅(qū)動(dòng)，容易建立運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型，可以采用一些先進(jìn)的、實(shí)時(shí)的控制算法，其動(dòng)作靈活，避障性好。由于串聯(lián)結(jié)構(gòu)機(jī)器人采用懸臂梁式的手臂結(jié)構(gòu)和關(guān)聯(lián)的驅(qū)動(dòng)系統(tǒng)，使得該類機(jī)器人存在著一些固有缺陷如操作精度性欠佳，難以完全平衡，且運(yùn)動(dòng)存在耦合等問題，但總體數(shù)量目前應(yīng)用越來越多。在本文中研究的機(jī)器人是串聯(lián)機(jī)構(gòu)機(jī)器人的一類一一串聯(lián)關(guān)節(jié)機(jī)器人，如圖 1.1 所示。圖 1-1 垂直關(guān)節(jié)機(jī)器人示意圖這類機(jī)器人的基本結(jié)構(gòu)是串聯(lián)的六自由度開鏈?zhǔn)?，關(guān)節(jié)相互垂直或平行，每個(gè)關(guān)4節(jié)都有單獨(dú)的驅(qū)動(dòng)機(jī)構(gòu)。前三個(gè)關(guān)節(jié)(包括基座、腰部和臂部)具有三個(gè)轉(zhuǎn)動(dòng)自由度，確定手部在空間的位置，這三個(gè)關(guān)節(jié)和聯(lián)接他們的桿件所構(gòu)成的機(jī)構(gòu)稱為機(jī)械手的位置機(jī)構(gòu);后三個(gè)關(guān)節(jié)(腕部)的主要功能是確定手部在空間的姿勢(shì)，這三個(gè)關(guān)節(jié)和聯(lián)接他們的桿件所構(gòu)成的機(jī)構(gòu)稱作姿勢(shì)機(jī)構(gòu)。位置機(jī)構(gòu)可基本確定機(jī)械手的工作空間范圍，前三個(gè)關(guān)節(jié)的運(yùn)動(dòng)稱作機(jī)械手的主運(yùn)動(dòng);后三個(gè)關(guān)節(jié)的運(yùn)動(dòng)稱作姿態(tài)運(yùn)動(dòng)，姿態(tài)機(jī)構(gòu)主要確定機(jī)械手的工作姿態(tài)。所有的關(guān)節(jié)驅(qū)動(dòng)器共同參與控制，來完成機(jī)械手的六自由度運(yùn)動(dòng)，以實(shí)現(xiàn)終端執(zhí)行器按一定姿態(tài)到達(dá)工作空間的每個(gè)工作點(diǎn)，本文主要討論除手指關(guān)節(jié)外的其它五個(gè)自由度。串聯(lián)關(guān)節(jié)機(jī)器人一般可用一個(gè)開環(huán)關(guān)節(jié)鏈來建模，此鏈由數(shù)個(gè)剛體(桿件)用驅(qū)動(dòng)器驅(qū)動(dòng)的移動(dòng)或轉(zhuǎn)動(dòng)關(guān)節(jié)串聯(lián)而成，開鏈的一端固接在基座上，另一端是自由的，安裝著工具(終端執(zhí)行器)，用以操縱物體，或完成裝配作業(yè)。這類機(jī)械手屬于空間機(jī)構(gòu)，運(yùn)動(dòng)通常只用轉(zhuǎn)動(dòng)和移動(dòng)兩類。用轉(zhuǎn)動(dòng)相聯(lián)的關(guān)節(jié)成為轉(zhuǎn)動(dòng)關(guān)節(jié);以移動(dòng)相聯(lián)的關(guān)節(jié)成為移動(dòng)關(guān)節(jié)。單獨(dú)驅(qū)動(dòng)的主動(dòng)關(guān)節(jié)數(shù)目成為機(jī)械手的自由度數(shù)。本文要研究串聯(lián)關(guān)節(jié)機(jī)器人的五個(gè)關(guān)節(jié)都是轉(zhuǎn)動(dòng)關(guān)節(jié)。隨著技術(shù)的進(jìn)步，串聯(lián)結(jié)構(gòu)機(jī)器人本體結(jié)構(gòu)近 10 年來發(fā)展變化很快，從開始的含有局部閉鏈的平行四邊形機(jī)構(gòu)，到改變?yōu)榇髢?chǔ)采用新型的非平行四邊形的單連桿機(jī)構(gòu)，工作空間有所增加，本體自重進(jìn)一步減小，變得更加輕巧，新材料的采用如輕質(zhì)鋁合金材料的應(yīng)用，大大提高了機(jī)器人的性能，并逐漸成為串聯(lián)結(jié)構(gòu)機(jī)器人普遍采用的結(jié)構(gòu)之一。1.2 課題的提出及意義進(jìn)入 20 世紀(jì) 90 年代以來，由于具有一般功能的傳統(tǒng)工業(yè)機(jī)器人的應(yīng)用趨向飽和，而許多高級(jí)生產(chǎn)和特種應(yīng)用則需要具有各種智能的機(jī)器人參與，因而促使智能機(jī)器人獲得較為迅速的發(fā)展。無論從國際或國內(nèi)的角度來看，復(fù)蘇和繼續(xù)發(fā)展機(jī)器人產(chǎn)業(yè)的一條重要途徑就是開發(fā)各種智能機(jī)器人，以求提高機(jī)器人的性能，擴(kuò)大其功能和應(yīng)用領(lǐng)域。回顧近 10 多年來國內(nèi)外機(jī)器人技術(shù)的發(fā)展歷程，可以歸納出下列趨勢(shì):1)傳感型智能機(jī)器人發(fā)展較快2)開發(fā)新型智能技術(shù)3)采用模塊化設(shè)計(jì)技術(shù)4)機(jī)器人工程系統(tǒng)呈上升趨勢(shì)5)微型機(jī)器人的研究有所突破6)應(yīng)用領(lǐng)域向非制造業(yè)和服務(wù)業(yè)擴(kuò)展從整個(gè)工業(yè)領(lǐng)域來看，對(duì)工業(yè)機(jī)器人需求越來越大，性能指標(biāo)越來越高。運(yùn)動(dòng)學(xué)5系統(tǒng)是工業(yè)機(jī)器人的底層核心部分，對(duì)其關(guān)鍵技術(shù)，如運(yùn)動(dòng)學(xué)建模、運(yùn)動(dòng)學(xué)方程的求解、運(yùn)動(dòng)空間插值算法等的研究，將從很大程度上決定著一個(gè)機(jī)器人系統(tǒng)的基本性能。隨著科技的發(fā)展，機(jī)器人已成為工業(yè)的現(xiàn)代化程度的標(biāo)志，機(jī)器人學(xué)是一門高度交叉的前沿學(xué)科，與機(jī)械學(xué)、生物學(xué)、人類學(xué)、計(jì)算機(jī)科學(xué)與工程、控制論與控制工程學(xué)、人工智能、社會(huì)學(xué)等。機(jī)器人學(xué)包含機(jī)器人運(yùn)動(dòng)學(xué)、機(jī)器人動(dòng)力學(xué)、機(jī)器人控制、機(jī)器人智能化等領(lǐng)域有著不同層次意義的聯(lián)系，本文以對(duì)機(jī)器手的控制及運(yùn)動(dòng)學(xué)系統(tǒng)為主線，對(duì)如何實(shí)現(xiàn)機(jī)器手動(dòng)作作了較深入的研究與討論。總的說來，對(duì)于機(jī)器人控制及運(yùn)動(dòng)系統(tǒng)的研究在理論和應(yīng)用上都具有重要的意義。本文基于上述的串聯(lián)關(guān)節(jié)機(jī)器人為研究對(duì)象，對(duì) GR-1 型教學(xué)機(jī)械手的運(yùn)動(dòng)及控制進(jìn)行研究。1.3 本文的研究?jī)?nèi)容和主要工作本文以 GR-1 型教學(xué)機(jī)械手為研究對(duì)象，對(duì)其進(jìn)行運(yùn)動(dòng)學(xué)求解；并利用其結(jié)果對(duì)機(jī)械手的軌跡進(jìn)行規(guī)劃；最后利用其軌跡規(guī)劃的結(jié)果，對(duì)機(jī)械手的運(yùn)動(dòng)進(jìn)行自動(dòng)控制。該內(nèi)容由兩人共同完成，本人具體完成以下工作：1、對(duì)機(jī)械手進(jìn)行位姿描述，應(yīng)用 D-H 參數(shù)法建立串聯(lián)關(guān)節(jié)機(jī)器人桿件坐標(biāo)系，求解位姿正解。2、對(duì)機(jī)械手的軌跡進(jìn)行規(guī)劃，采用三次多項(xiàng)式插值運(yùn)算的方法，利用由機(jī)器人運(yùn)動(dòng)學(xué)解得的關(guān)節(jié)變量求得運(yùn)動(dòng)軌跡方程。3、用 Visual Basic 6.0 軟件做出人機(jī)交互界面，在界面上顯示計(jì)算結(jié)果和軌跡曲線圖。4、編寫機(jī)械手控制程序，通過 Visual Basic 6.0 界面將結(jié)果輸出并將控制數(shù)據(jù)輸入 PLC，實(shí)現(xiàn)對(duì)機(jī)械手的自動(dòng)控制。6第 2 章 PLC 控制系統(tǒng)及實(shí)現(xiàn)2.1 機(jī)械手及控制器主要參數(shù)1 機(jī)械手a、自由度：5（不包括手指開閉控制）b、各關(guān)節(jié)活動(dòng)范圍：腰： ≥ ?180?肩： ≥ 2肘： ≥270。手俯仰： ≥ 7?手指旋轉(zhuǎn)：≥ 360c、最大活動(dòng)區(qū)域：垂直方向：850 mm水平方向：600 mmd、基本尺寸：上、下臂長(zhǎng)：228 mm手后部：73 mme、各軸分辨率如表 2-1：表 2-1 各軸分辨率參數(shù)軸 電機(jī)代號(hào) 分辨率（度/脈沖）軀干 M1 0.14上臂 M2 0.11下臂 M3 0.11手俯仰 M4 0.11手指旋轉(zhuǎn) M5 0.242 電機(jī)工作電壓： U = 12 V?空載轉(zhuǎn)速： n = 3000 r/min空載電流： I = 0.15 A72.2 簡(jiǎn)述機(jī)械手動(dòng)作的實(shí)現(xiàn)GR—1 型教學(xué)機(jī)械手主要由機(jī)械和控制兩大部分組成，本論文主要對(duì)其控制部分進(jìn)行研究。該機(jī)械手采用的是串聯(lián)式關(guān)節(jié)機(jī)械手，全部采用開鏈?zhǔn)剑Y(jié)構(gòu)簡(jiǎn)單，整個(gè)機(jī)械手分 5 個(gè)自由度：腰、肩關(guān)節(jié)、肘關(guān)節(jié)、腕關(guān)節(jié)和手指，全部有直流電機(jī)驅(qū)動(dòng)。控制功能的實(shí)現(xiàn)主要是通過對(duì)控制器的編程，由光電編碼器檢測(cè)各電機(jī)的旋轉(zhuǎn)角位移，再結(jié)合 PLC 來控制各個(gè)電機(jī)的角位移，從而實(shí)現(xiàn)對(duì)機(jī)械手各個(gè)關(guān)節(jié)較精確的運(yùn)動(dòng)以實(shí)現(xiàn)機(jī)械手預(yù)期的動(dòng)作。光電編碼器起控制直流電機(jī)角位移精度作用，固定于電機(jī)轉(zhuǎn)軸上與電機(jī)同步運(yùn)轉(zhuǎn)。系統(tǒng)工作時(shí)，各光電編碼器輸出一系列與直流電機(jī)運(yùn)轉(zhuǎn)角度相對(duì)應(yīng)的脈沖信號(hào)，經(jīng)轉(zhuǎn)換電路將序列脈沖電壓信號(hào)轉(zhuǎn)換成 PLC 可以接收的開關(guān)信號(hào)，在 PLC 內(nèi)部通過計(jì)數(shù)器計(jì)數(shù)后得到一個(gè)電機(jī)實(shí)際的角位移信號(hào)，運(yùn)行 PLC 控制程序，獲得確定輸出，再通過外電路由 PLC 的輸出信號(hào)來控制電機(jī)的啟動(dòng)停止，以實(shí)現(xiàn)機(jī)械手的預(yù)期動(dòng)作。下面將對(duì) PLC 如何控制直流電機(jī)來實(shí)現(xiàn)機(jī)械手動(dòng)作實(shí)現(xiàn)的方案來做說明。2.3 控制軟件的設(shè)計(jì)實(shí)現(xiàn) PLC 對(duì)機(jī)械手的控制，要將各桿件的運(yùn)動(dòng)控制轉(zhuǎn)換成對(duì)機(jī)械手各電機(jī)角位移的控制。要使機(jī)械手手抓到達(dá)預(yù)期的位置就要計(jì)算出每個(gè)關(guān)節(jié)運(yùn)動(dòng)的角度，即每個(gè)電機(jī)所應(yīng)轉(zhuǎn)的角度，這可通過對(duì)各空間坐標(biāo)系的相對(duì)運(yùn)動(dòng)在運(yùn)動(dòng)學(xué)基礎(chǔ)上計(jì)算得到，最后，將其轉(zhuǎn)換成 PLC 內(nèi)部控制指令即可實(shí)現(xiàn)軟件控制。2.3.1 PLC 的選擇PLC 類型的選擇主要考慮兩個(gè)方面：輸入輸出點(diǎn)的數(shù)目和存儲(chǔ)容量。容量有計(jì)算公式：指令條數(shù)=（輸入點(diǎn)數(shù)+輸出點(diǎn)數(shù)） (10--12)?再留有 15%左右的備用量，根據(jù)外部電路的特點(diǎn)得到輸入、輸出點(diǎn)的數(shù)目，最后選定用三菱公司生產(chǎn)的 FX2N—64MR 型 PLC。具體計(jì)算和分析過程參照同組同學(xué)畢業(yè)設(shè)計(jì)說明書。2.3.2 光電編碼器機(jī)械手各個(gè)電機(jī)角位移的準(zhǔn)確控制是通過光電編碼器反饋的信息來實(shí)現(xiàn)的，每個(gè)直流電機(jī)輸出軸上都裝有光電編碼器，通過它實(shí)現(xiàn)光電脈沖轉(zhuǎn)換及對(duì)電機(jī)轉(zhuǎn)角的檢測(cè)。光電編碼器主要是由控制電路板、光電耦合器及遮光盤組成。隨碼盤旋轉(zhuǎn)，輸出一系列計(jì)數(shù)脈沖。增量式編碼器需要預(yù)先指定一個(gè)基數(shù)：零位。輸出脈沖相對(duì)于基數(shù)進(jìn)行加減，從而測(cè)量出碼盤位移，即電機(jī)的角位移。碼盤固定在電機(jī)轉(zhuǎn)軸上，隨電機(jī)的旋8轉(zhuǎn)，編碼器產(chǎn)生代表角位移的脈沖。光電編碼器輸出包括 A、 B 兩個(gè)頻道的信號(hào)，碼盤為 6 孔均勻分布的金屬盤。當(dāng)總線通電時(shí)，光電耦合器的發(fā)光二極管發(fā)出不可見紅外線光，通過旋轉(zhuǎn)的碼盤通光孔而被光敏器接收，光敏器件兩端由截止變?yōu)閷?dǎo)通，兩端電壓發(fā)生變化，從 A、 B 輸出信號(hào)，碼盤每旋轉(zhuǎn)一周發(fā)送 12 次信號(hào)2.3.3 接口電路1、輸入接口輸入接口有位置傳感器信號(hào)的輸入，限位信號(hào)輸入以及手動(dòng)開關(guān)的接入。FX2N—64MR 提供 21 個(gè)高速計(jì)數(shù)器，即 C235 至 C255，但它們共享同一個(gè) PLC 上的6 個(gè)高速計(jì)數(shù)器輸入端（X0～X5）。即如果輸入已經(jīng)被某個(gè)計(jì)數(shù)器占用，它就不能再用于另一個(gè)高速計(jì)數(shù)器（或其他用途）。也就是說，由于只有 6 個(gè)高速計(jì)數(shù)器的輸入，因此，最多同時(shí)用 6 個(gè)高速計(jì)數(shù)器。在本文中將用到的是沒有啟動(dòng)/復(fù)位的單相高速計(jì)數(shù)器 C235～ C239。計(jì)數(shù)器端口分配如表 2-2。表 2-2 高計(jì)數(shù)器表輸入 C235 C236 C237 C238 C239X0 U/DX1 U/DX2 U/DX3 U/DX4 U/D注： U—增計(jì)數(shù)輸入； D—減計(jì)數(shù)輸入。2、輸出接口輸出接口主要接電機(jī)，由于五個(gè)電機(jī)都要實(shí)現(xiàn)正反轉(zhuǎn)，即能實(shí)現(xiàn)正反向供電，每個(gè)電機(jī)分配 4 個(gè)輸出端口。接線電路及其設(shè)計(jì)參考同組同學(xué)設(shè)計(jì)說明書。2.3.4 控制原理及程序1、機(jī)械手動(dòng)作控制原理程序啟動(dòng)后，各個(gè)電機(jī)按照程序要求逐個(gè)開始運(yùn)動(dòng)使得機(jī)械手開始動(dòng)作。首先，腰部電機(jī) M1 按程序指定要求正向轉(zhuǎn)動(dòng)，相應(yīng)計(jì)數(shù)器開始對(duì)光電編碼器返回的信號(hào)進(jìn)行計(jì)數(shù)，計(jì)數(shù)器計(jì)滿數(shù)據(jù)后， PLC 輸出控制指令，使 M1 停止轉(zhuǎn)動(dòng)，同時(shí)依次逐個(gè)啟動(dòng)控制電機(jī) M2、M3、M4、M5 開始轉(zhuǎn)動(dòng)，各個(gè)電機(jī)控制方式的實(shí)現(xiàn)同電機(jī) M1。當(dāng)電機(jī) M5 動(dòng)作完畢后，整個(gè)機(jī)械手完成了其全部動(dòng)作，由規(guī)定的起點(diǎn)到達(dá)終點(diǎn)。流程圖如圖 2-1所示。9PLC電機(jī) M1 啟動(dòng)M1 停，M2 啟動(dòng)發(fā)出指令腰部轉(zhuǎn)動(dòng)計(jì)數(shù)器C235滿上臂轉(zhuǎn)動(dòng)計(jì)數(shù)器C236滿M2 停，M3 啟動(dòng)前臂轉(zhuǎn)動(dòng)計(jì)數(shù)器C237滿M3 停，M4 啟動(dòng)手腕轉(zhuǎn)動(dòng)計(jì)數(shù)器C238滿M4 停，M5 啟動(dòng)M5 停止，整個(gè)動(dòng)作完成手指轉(zhuǎn)動(dòng)計(jì)數(shù)器C239滿圖 2-1 機(jī)械手動(dòng)作流程圖2 編寫控制程序搬運(yùn)機(jī)械手動(dòng)作由電機(jī)驅(qū)動(dòng)各個(gè)關(guān)節(jié)實(shí)現(xiàn)，轉(zhuǎn)動(dòng)角度由計(jì)數(shù)器計(jì)數(shù)控制，計(jì)數(shù)器值滿后斷電使電機(jī)停止運(yùn)轉(zhuǎn)。各計(jì)數(shù)器的值由表 2-1 和軌跡規(guī)劃的結(jié)果表 5-4 確定如表 2-3。表 2-3 各計(jì)數(shù)器數(shù)值計(jì)數(shù)器 C235 C236 C237 C238 C239數(shù)值 4 147 118 31 16梯形圖見附錄。10第 3 章 機(jī)械手運(yùn)動(dòng)學(xué)系統(tǒng)3.1 引言機(jī)器人運(yùn)動(dòng)學(xué)主要有以下兩個(gè)基本問題：（1）對(duì)一給定機(jī)器人，已知桿件幾何參數(shù)和關(guān)節(jié)變量，求末端執(zhí)行器相對(duì)給定坐標(biāo)系的位置和姿態(tài)。給定坐標(biāo)系為固定在大地上的笛卡兒坐標(biāo)系，作為機(jī)器人的總坐標(biāo)系，也稱為世界坐標(biāo)系（World Coordinate）。（2）已知機(jī)器人的桿件參數(shù)，給定末端執(zhí)行器相對(duì)于總體坐標(biāo)系的位置和姿態(tài)，確定關(guān)節(jié)變量的大小。第一個(gè)問題常稱為運(yùn)動(dòng)學(xué)正問題（ DKP-Direct Kinematic Problems），第二個(gè)問題通常稱為運(yùn)動(dòng)學(xué)逆問題（ IKP-Inverse Kinematic Problems）。機(jī)器人手臂的關(guān)節(jié)變量是獨(dú)立變量，而末端執(zhí)行器的作業(yè)通常在總體坐標(biāo)系中說明。根據(jù)末端執(zhí)行器在總體坐標(biāo)系中的位姿來確定相應(yīng)各關(guān)節(jié)變量要進(jìn)行運(yùn)動(dòng)學(xué)逆題的求解。機(jī)器人運(yùn)動(dòng)學(xué)逆問題是編制機(jī)器人運(yùn)動(dòng)控制系統(tǒng)軟件所必備的知識(shí)。3.2 機(jī)械手運(yùn)動(dòng)學(xué)數(shù)學(xué)基礎(chǔ)機(jī)械手是機(jī)器人系統(tǒng)的機(jī)械運(yùn)動(dòng)部分. 為了描述機(jī)械手的操作，必須建立機(jī)械手各連桿間以及機(jī)械手與周圍環(huán)境間的運(yùn)動(dòng)關(guān)系，研究機(jī)械手的運(yùn)動(dòng)，不僅涉及機(jī)械手本身，而且涉及各物體間以及物體與機(jī)械手的關(guān)系，齊次坐標(biāo)及其變換就是用來表達(dá)這些關(guān)系的.齊次坐標(biāo)交換不僅能夠表示運(yùn)動(dòng)學(xué)問題，而且能夠表達(dá)機(jī)械手控制算法、計(jì)算機(jī)視覺和計(jì)算機(jī)圖形學(xué)等問題。3.2.1 機(jī)器人位置與姿態(tài)的描述在描述物體，如零件、工具或機(jī)械手間的關(guān)系時(shí)，要用到位置矢量、平面和坐標(biāo)系等概念，對(duì)于工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)系統(tǒng)的描述就是建立在這些概念的基礎(chǔ)上的。1.位置描述描述物體(如零件、工具或機(jī)械手)間關(guān)系時(shí)，一旦建立了一個(gè)坐標(biāo)系，我們就能夠用某個(gè) 3 1 位置矢量來確定該空間內(nèi)任一點(diǎn)的位置.對(duì)于直角坐標(biāo)系{ A}，空間任意?一點(diǎn)的 P 位置可用 3 1 的列矢量 AP11AyAxOPAAz圖 3-1 位姿表示(3.1)xAyzpP???????表示。其中， 、 、 是點(diǎn) P 在坐標(biāo)系{A}中的三個(gè)坐標(biāo)分量 的上標(biāo) A 代表xypz AP參考坐標(biāo)系{ A}。我們稱 為位置矢量，如圖 3-1 所示。A2.位姿描述為了研究機(jī)器人的運(yùn)動(dòng)與操作，往往不僅要表示空間某個(gè)點(diǎn)的位置，而且需要表示物體的方位。物體的方位可由某個(gè)固接于此物體的坐標(biāo)系描述。為了規(guī)定空間某剛體 B 的方位，設(shè)置一個(gè)直角坐標(biāo)系{ B}與此剛體固接。用坐標(biāo)系{ B}的三個(gè)單位主矢量相對(duì)于參考坐標(biāo)系{ A}方向余弦組成的 3×3 矩陣：,xyz（3.3）12133AABBrRxyz????????????來表示剛體 B 相對(duì)于坐標(biāo)系{ A}的方位。 稱為旋轉(zhuǎn)矩陣。式中，上標(biāo) A 表參考ABR坐標(biāo)系{ A}，下標(biāo) B 代表被描述的坐標(biāo)系{ B}. 共有 9 個(gè)元素，但只有 3 個(gè)是獨(dú)立的.由于 的三個(gè)列矢量 和 ，都是單位矢量，且雙雙相互垂直，因而它的 9BR,ABxyz個(gè)元素滿足 6 個(gè)約束條件(正交條件)。（3.3）.1ABxy??（3.4）0AABBz對(duì)應(yīng)于軸 x,y 或 z 作轉(zhuǎn)角為 θ 的旋轉(zhuǎn)變換，其旋轉(zhuǎn)矩陣分別為:(3.5)10(,)Rotcs??????????12(3.6)0(,)1csRoty?????????(3.7)(,)01cstz???????AxAyBz{B}{A} BOAzA圖 3-2 方位描述式中， 表示 , 表示 。圖 3-2 表示一個(gè)物體的方位，此物體與坐標(biāo)系{B}sincos固接，并相對(duì)于參考坐標(biāo)系{A}運(yùn)動(dòng)。3.3 空間齊次坐標(biāo)變換3.3.1 坐標(biāo)變換空間中任意點(diǎn) P 在不同坐標(biāo)系中的描述是不同的。為了闡明從一個(gè)坐標(biāo)系的描到另一個(gè)坐標(biāo)系的描述關(guān)系，需要討論這種變換的數(shù)學(xué)問題。坐標(biāo)變換一般有兩種形式:平移坐標(biāo)變換和旋轉(zhuǎn)坐標(biāo)變換。設(shè)坐標(biāo)系{ B}與{ A}具有相同的方位，但{ B}坐標(biāo)系的原點(diǎn)與{ A}的原點(diǎn)不重合。用位置矢量 ，描述它相對(duì)于{ A}的位置，如圖 3-3 所示。AP稱 為{ B}相對(duì)于{ A}的平移矢量。如果點(diǎn) P 在坐標(biāo)系{ B}中的位置為 ，那么它相對(duì)P P于坐標(biāo)系{ A}的位置矢量 可由矢量相加得出，即= + (3.8)式(3.8)被稱為坐標(biāo)平移方程。13{A}zAyAxAoAApApBxRoR yRzR{B}Bp圖 3-3 平移變換設(shè)坐標(biāo)系{ B}與{ A}有共同的坐標(biāo)原點(diǎn)，但兩者的方位不同，如圖 3-4 所示.用旋轉(zhuǎn)矩陣 描述{ B}對(duì)于{ A}的方位。同一點(diǎn) P 在兩個(gè)坐標(biāo)系{ A}和{ B}中的描述 和 具ABR BPA有如下變換關(guān)系:= (3.9)P稱(3.9)式為坐標(biāo)旋轉(zhuǎn)方程。我們可以類似地用穿 R 描述坐標(biāo)系{A}相對(duì)于{B}的方位。 和 都是正交矩陣，兩者互逆。ABR對(duì)于最一般的情形:坐標(biāo)系{ B}的原點(diǎn)與{ A}的原點(diǎn)既不重合，{ B}的方位與{ A}的方位也不相同。用位置矢量 描述{ B}的坐標(biāo)原點(diǎn)相對(duì)于{ A}的位置:用旋轉(zhuǎn)矩陣 描述AP BR{B}相對(duì)于{ A}的方位，如圖 3-4 所示。{A}zAyAxAoAApApRzC zRoRxRxCyCyR {B}B圖 3-4 平移變換與旋轉(zhuǎn)變換對(duì)于任一點(diǎn) P 在兩坐標(biāo)系{ A}和{ B}中的描述 和 具有以下變換關(guān)系:APB（3.10）ABAR??3.3.2 齊次坐標(biāo)變換己知一直角坐標(biāo)系中的某點(diǎn)坐標(biāo)，那么該點(diǎn)在另一直角坐標(biāo)系中的坐標(biāo)可通過齊14次坐標(biāo)變換求得。變換式(3.10)對(duì)于點(diǎn) 而言是非齊次的，但是可以將其表示為等價(jià)的齊次變換形BP式（3.11）101AABPR?????????????其中，4x1 的列矢量表示三維空間的點(diǎn)，稱為點(diǎn)的齊次坐標(biāo)，仍然記為 或 。APB可把式(3.11)寫成矩陣形式= （3.12）APBT式中，齊次坐標(biāo) 或 是 4x1 的列矢量。齊次變換矩陣 是 4x4 的方陣，具有APB ABT如下形式（3.13）01ABRT???????綜合地表示了平移變換和旋轉(zhuǎn)變換。B3.3 點(diǎn)在空間直角坐標(biāo)系中繞過原點(diǎn)任意軸的一般旋轉(zhuǎn)變換1.RPY 角（繞固定軸 X-Y-Z 旋轉(zhuǎn)）RPY 角是描述船舶在海中航行時(shí)的姿態(tài)的一種方法。將船的行駛方向取為 Z 軸，則繞 Z 軸的旋轉(zhuǎn)（ 角）稱為滾動(dòng)（Roll）；把繞 Y 軸的旋轉(zhuǎn)（ 角）稱為俯仰??（Pitch）；而把垂直方向取為 X 軸，將繞 X 軸的旋轉(zhuǎn)（ 角）稱為偏轉(zhuǎn)（Yaw），操?作臂手抓姿態(tài)的規(guī)定方法類似，故習(xí)慣上稱為 RPY 角方法。2.描述活動(dòng)坐標(biāo)系方位的法則如下：活動(dòng)系的初始方位與參考系重合，首先將活動(dòng)系繞參考系的 X 軸旋轉(zhuǎn) 角，再繞?參考系的 Y 軸轉(zhuǎn) 角，最后繞參考系的 Z 軸轉(zhuǎn) 角，??因三次旋轉(zhuǎn)都是相對(duì)于參考系的，所以得相應(yīng)的旋轉(zhuǎn)矩陣（3.14）(,)(,)(,)(,)RPotzRtyotx?????0010(,)1 1cscscsY????????????????????????其中 。將矩陣相乘得cos,in???15（3.15）cs0(,)0 1csssRPYc??????????? ?? ?????? ?它表示繞固定坐標(biāo)系的三個(gè)軸依次旋轉(zhuǎn)得到的旋轉(zhuǎn)矩陣，因此稱為繞固定軸 X-Y-Z旋轉(zhuǎn)的 RPY 角法。令：（3.16）0(,)01xxyyzznoaRPY??????????（1）由給定的旋轉(zhuǎn)矩陣求出等價(jià)的繞固定軸 X-Y-Z 的旋轉(zhuǎn)角 。???、 、式（3.16）中有 3 個(gè)未知數(shù)，共 9 個(gè)方程，其中 6 個(gè)方程不獨(dú)立，因此可以利用其中 3 個(gè)方程解出未知數(shù)。由式（3.15）、（3.16）可以看出：（3.17）2cosxyn???如果 ，則得到各個(gè)角的反正切表達(dá)式：0?（3.18）2ta(,)nt,zxyyzAo????????式中， 是雙變量反正切函數(shù)，用其計(jì)算 arctan(y/x)的優(yōu)點(diǎn)在于利用a2()x了 X 和 Y 的符號(hào)能夠確定所得角度所在的象限這一現(xiàn)象。式（3.17）中的根式一般有兩個(gè)解，我們總是取 中的一個(gè)解。90?????（2）從給定的繞固定軸 X-Y-Z 的旋轉(zhuǎn)角 ，陣求出等價(jià)的旋轉(zhuǎn)矩陣。??、 、將給定的角 代入式（3.15）與式（3.16）中元素對(duì)應(yīng)相等即可以求得所???、 、求矩陣。163.4 Denavt-Hartenberg(D-H)表示法圖 3-4 關(guān)節(jié)參數(shù)規(guī)定機(jī)械手由一串用轉(zhuǎn)動(dòng)或平移關(guān)節(jié)連接的剛體（桿件）組成。每一對(duì)關(guān)節(jié)-桿構(gòu)成一個(gè)自由度。桿件編號(hào)由手臂的固定機(jī)座開始，固定機(jī)座可看成桿件 0，第一個(gè)運(yùn)動(dòng)體是桿件 1，依次類推，最后一個(gè)桿件與工具相連；關(guān)節(jié) 1 處于連接桿件 1 和基座之間，每個(gè)桿件至多與另外兩個(gè)桿件相聯(lián)，而不構(gòu)成閉環(huán)。任何桿件 i 都可以用兩個(gè)尺度表征，如圖 2.5 所示，桿件 i 的長(zhǎng)度 ，是桿件上ia兩個(gè)關(guān)節(jié)軸線的最短距離；桿件 i 的扭轉(zhuǎn)角 ，是兩個(gè)關(guān)節(jié)軸線的夾角。i?通常，在每個(gè)關(guān)節(jié)軸線上連接有兩根桿件，每個(gè)桿件各有一根和軸線垂直的法線。兩個(gè)桿件的相對(duì)位置由兩桿間的距離 （關(guān)節(jié)軸上兩軸間法線的距離）和夾角 （關(guān)id i?節(jié)軸上兩個(gè)法線的夾角）確定。為描述相鄰桿件間平移和轉(zhuǎn)動(dòng)的關(guān)系。Denavit 和 Hatenberg（1955）提出了一種為關(guān)節(jié)鏈中的每一桿件建立附體坐標(biāo)系的矩陣方法。 D-H 方法是為每個(gè)關(guān)節(jié)處的桿件坐標(biāo)系建立 4 4 齊次變換矩陣，表示它與前一桿件坐標(biāo)系的關(guān)系。這樣逐次變換，有?“手部坐標(biāo)”表示的末端執(zhí)行器可被變換并用機(jī)座坐標(biāo)表示。3.4.1 坐標(biāo)系的建立N 關(guān)節(jié)機(jī)器人需建立 n+1 個(gè)坐標(biāo)系，其中參考（機(jī)座）坐標(biāo)系為 ，機(jī)械手0Oxyz末端的坐標(biāo)系為 ，第 i 關(guān)節(jié)上的坐標(biāo)系為 。確定和建立每個(gè)坐標(biāo)nOxyz 11iixyz??系應(yīng)根據(jù)下面 3 條規(guī)則：（1） 軸沿著第 關(guān)節(jié)軸的運(yùn)動(dòng)軸；1iz?i（2） 軸垂直于 和 軸并指向離開 軸的方向；ix1iz?i 1iz?17（3） 按右手坐標(biāo)系的要求建立。iy按照這些規(guī)則，第 0 號(hào)坐標(biāo)系在機(jī)座上的位置和方向可任選，只要 軸沿著第 10z關(guān)節(jié)運(yùn)動(dòng)軸。第 n 坐標(biāo)系可防在手的任何部位，只要 軸與 軸垂直。nx1z?3.4.2 幾何參數(shù)定義根據(jù)上述對(duì)桿件參數(shù)及坐標(biāo)系的定義，描述串聯(lián)機(jī)器人相鄰坐標(biāo)系之間的關(guān)系可歸結(jié)如下 4 個(gè)參數(shù)： '2iO?1i? ix1ix?圖3-5 桿件的參數(shù)和坐標(biāo)系2iz?第 關(guān)節(jié)i1iz?：繞軸（右手規(guī)則）由 軸向 軸的關(guān)節(jié)角；i?1iz?1i?：從第 坐標(biāo)系的原點(diǎn)到 軸和 軸的交點(diǎn)沿 軸的距離；d1i?ix1iz?：從 和 的交點(diǎn)到第 坐標(biāo)系原點(diǎn)沿 軸的偏置距離（或者說，是 和 兩iazixii 1iz?i軸間的最小距離）；：繞 軸（右手規(guī)則）由 軸轉(zhuǎn)向 軸的偏角。i?i 1iz?iz對(duì)于轉(zhuǎn)動(dòng)關(guān)節(jié) 、 和 是關(guān)節(jié)參數(shù)， 是關(guān)節(jié)變量。移動(dòng)關(guān)節(jié)的關(guān)節(jié)參數(shù)是 、idi?ia? ia和 ， 是關(guān)節(jié)變量。ii?i3.4.3 建立 坐標(biāo)系和 坐標(biāo)系的齊次變換矩陣1i將第 個(gè)坐標(biāo)系的點(diǎn) 在 坐標(biāo)系表示，需建立 坐標(biāo)系和 坐標(biāo)系的齊次變換iir?i1i?矩陣，因而需經(jīng)過以下變換：1、將坐標(biāo)系 繞 軸轉(zhuǎn) 角，使 軸與 軸平行并指向同一方向；11iiOxyz?ii?1ix?i2、將坐標(biāo)系 沿 軸平移距離 ，使 軸與 的 軸重合；?diiOxyzi3、將坐標(biāo)系 沿 軸平移距離 ，使兩坐標(biāo)系的原點(diǎn)重合；11iiixiaid'1iO?i?ia iO第 關(guān)節(jié)1?i?iz184、將坐標(biāo)系 繞 軸轉(zhuǎn) 角，使兩坐標(biāo)系完全重合。11iiOxyz??ixi?從而， 坐標(biāo)系和 坐標(biāo)系的齊次變換矩陣 可以根據(jù)矩陣的合成規(guī)則得到，i1iA?稱為相鄰坐標(biāo)系 和 的變換矩陣。即（由算子左、右乘規(guī)則得）1iA?i=i,,zdxRT??01010101ii iiiics acsd???????????????????????????（3.19）001iiiiiiicsacsd???????????式中， 表示 , 表示 。sincos對(duì)于在第 個(gè)坐標(biāo)系的點(diǎn) 在 坐標(biāo)系表示為：ir?1iiirA??確定第 坐標(biāo)系相對(duì)于機(jī)座坐標(biāo)系的位置的齊次變換矩陣 是各齊次變換矩陣i0iT的連乘積，可表示成1iA? 0112,ijiT????i-1i…=0iiiiiinoaPR????????????式中， 是固連桿在桿件 上的第 個(gè)坐標(biāo)系的姿態(tài)矩陣， 是由機(jī)座坐??iiinoaii iP標(biāo)系原點(diǎn)指向第 個(gè)坐標(biāo)系原點(diǎn)的位置矢量。特別當(dāng) i=6 時(shí)，求得 T 矩陣， ，06A?它確定了機(jī)械手的末端相對(duì)于機(jī)座坐標(biāo)系的位置和狀態(tài)?？梢园?T 矩陣寫成0666010101xxyyzznoapnoaPnoaPRT ???????????????????????????式中， 為手的法向矢量， 為手的滑動(dòng)矢量， 為手的接近矢量， 為手的位置o P矢量。19第 4 章 機(jī)器人運(yùn)動(dòng)學(xué)方程的求解4.1 機(jī)器人正向運(yùn)動(dòng)學(xué)根據(jù)前面介紹的方法，機(jī)器人末端執(zhí)行器相對(duì)于機(jī)身坐標(biāo)系的齊次變換矩陣為：0T5 = A1A2…A5式中： 0T5 常寫成 T5 。運(yùn)動(dòng)學(xué)正解方程的建立機(jī)械手對(duì)應(yīng)的連桿參數(shù)如表 4-1 所示表 4-1 機(jī)械手參數(shù)關(guān)節(jié)i 變量 ?ad ?cosin1 θ190 0 0 0 12 θ20 2 0 1 03 θ30 3a0 1 04 θ4-90 4 0 0 -15 θ50 0 0 1 0將表 4-1 中的參數(shù)分別代入式（3.19）中可得如下變換矩陣。1100csA????????22101csacA????????3301csacA????????44301csacs????????5401csA???????20由手端坐標(biāo)逐一向基礎(chǔ)坐標(biāo)變換，其過程如下： 54501csTA????????44534550011cscasscT????????????????45401csca????????3345425350101csaccsacsTA????????????????34534534534534434300001csscscacsac????? ?? ???? ?=345345343001csacss??? ?? ?? ?22152510csacTA?????????345345343001csacss??? ?? ?? ?=2345234523423423001csacacsss??? ?? ?? ?1123452345234234230515 00001cscsacacssTA????? ???? ??????? ?21??1234512345123412342300cscscsacacs?????? ????? ?（4.1）01xxyyzznoap???????式中： ??12345123451152345123413242323xyzxyzxyzxyzncsoscascpacss????????式中 、 和 分別表示 、 和 ，其它類4c4c234cos()??23cos()??34cos()??推，下同。4.2 機(jī)器人逆向運(yùn)動(dòng)學(xué)參考同組同學(xué)論文逆向運(yùn)動(dòng)學(xué)部分可得如下結(jié)果：，1arctnyxp????????180?????，15rtyxsco???51??，2341artnzxycs???23480?????222221343433()()xyzpcsapsa??????233s?3arctns??2234312342 22()()()zxypaspcsasc????3213432342 223()()()xyzcaasa?343123422 321243()()()rtnrctzxyxyzpspcsasca?????因 均已知，所以可進(jìn)而求得 。2343、 、 ?2???以上結(jié)果，給出了機(jī)械手置于任何期望位姿時(shí)所需的關(guān)節(jié)值，為下一章軌跡規(guī)劃所用。23第 5 章 機(jī)械手軌跡規(guī)劃5.1 機(jī)器人軌跡的概念機(jī)器人軌跡泛指工業(yè)機(jī)器人在運(yùn)動(dòng)過程中的運(yùn)動(dòng)軌跡，即運(yùn)動(dòng)點(diǎn)的位移、速度和加速度。機(jī)器人在作業(yè)空間要完成給定的任務(wù)，其手部運(yùn)動(dòng)必須按一定的軌跡(trajectory)進(jìn)行。軌跡的生成一般是先給定軌跡上的若干個(gè)點(diǎn)，將其經(jīng)運(yùn)動(dòng)學(xué)反解映射到關(guān)節(jié)空間，對(duì)關(guān)節(jié)空間中的相應(yīng)點(diǎn)建立運(yùn)動(dòng)方程，然后按這些運(yùn)動(dòng)方程對(duì)關(guān)節(jié)進(jìn)行插值，從而實(shí)現(xiàn)作業(yè)空間的運(yùn)動(dòng)要求，這一過程通常稱為軌跡規(guī)劃。工業(yè)機(jī)器人軌跡規(guī)劃屬于機(jī)器人低層規(guī)劃，基本上不涉及人工智能的問題，本章僅討論在關(guān)節(jié)空間或笛卡爾空間中工業(yè)機(jī)器人運(yùn)動(dòng)的軌跡規(guī)劃和軌跡生成方法。機(jī)器人運(yùn)動(dòng)軌跡的描述一般是對(duì)其手部位姿的描述，此位姿值可與關(guān)節(jié)變量相互轉(zhuǎn)換?？刂栖壽E也就是按時(shí)間控制手部或工具中心走過的空間路徑。5.2 軌跡的生成方式運(yùn)動(dòng)軌跡的描述或生成有以下幾種方式：(1) 示教-再現(xiàn)運(yùn)動(dòng)。這種運(yùn)動(dòng)由人手把手示教機(jī)器人，定時(shí)記錄各關(guān)節(jié)變量，得到沿路徑運(yùn)動(dòng)時(shí)各關(guān)節(jié)的位移時(shí)間函數(shù) q(t)；再現(xiàn)時(shí)，按內(nèi)存中記錄的各點(diǎn)的值產(chǎn)生序列動(dòng)作。(2) 關(guān)節(jié)空間運(yùn)動(dòng)。這種運(yùn)動(dòng)直接在關(guān)節(jié)空間里進(jìn)行。由于動(dòng)力學(xué)參數(shù)及其極限值直接在關(guān)節(jié)空間里描述，所以用這種方式求最短時(shí)間運(yùn)動(dòng)很方便。(3) 空間直線運(yùn)動(dòng)。這是一種直角空間里的運(yùn)動(dòng)，它便于描述空間操作，計(jì)算量小，適宜簡(jiǎn)單的作業(yè)。(4) 空間曲線運(yùn)動(dòng)。這是一種在描述空間中用明確的函數(shù)表達(dá)的運(yùn)動(dòng)，如圓周運(yùn)動(dòng)、螺旋運(yùn)動(dòng)等。5.2.1 軌跡規(guī)劃涉及的主要問題為了描述一個(gè)完整的作業(yè)，往往需要將上述運(yùn)動(dòng)進(jìn)行組合。通常這種規(guī)劃涉及到以下幾方面的問題：(1) 對(duì)工作對(duì)象及作業(yè)進(jìn)行描述，用示教方法給出軌跡上的若干個(gè)結(jié)點(diǎn)(knot)。(2) 用一條軌跡通過或逼近結(jié)點(diǎn)，此軌跡可按一定的原則優(yōu)化，如加速度平滑得到直角空間的位移時(shí)間函數(shù) X(t)或關(guān)節(jié)空間的位移時(shí)間函數(shù) q(t)；在結(jié) 點(diǎn)之間如何24進(jìn)行插補(bǔ)，即根據(jù)軌跡表達(dá)式在每一個(gè)采樣周期實(shí)時(shí)計(jì)算軌跡上點(diǎn)的位姿和各關(guān)節(jié)變量值。(3) 以上生成的軌跡是機(jī)器人位置控制的給定值，可以據(jù)此并根據(jù)機(jī)器人的動(dòng)態(tài)參數(shù)設(shè)計(jì)一定的控制規(guī)律。(4) 規(guī)劃?rùn)C(jī)器人的運(yùn)動(dòng)軌跡時(shí)，尚需明確其路徑上是否存在障礙約束的組合。一般將機(jī)器人的規(guī)劃與控制方式分為四種情況，如表 5-1 所示。表 5-1 機(jī)器人的規(guī)劃與控制方式障 礙 約 束有 無有 離線無碰撞路徑規(guī)則+在線路徑跟蹤離線路徑規(guī)劃+在線路徑跟蹤路徑約束無 位置控制+在線障礙探測(cè)和避障位置控制本章主要討論連續(xù)路徑的無障礙軌跡規(guī)劃方法。5.3 插補(bǔ)方式分類點(diǎn)位控制( PTP 控制)通常沒有路徑約束，多以關(guān)節(jié)坐標(biāo)運(yùn)動(dòng)表示。點(diǎn)位控制只要求滿足起終點(diǎn)位姿，在軌跡中間只有關(guān)節(jié)的幾何限制、最大速度和加速度約束；為了保證運(yùn)動(dòng)的連續(xù)性，要求速度連續(xù)，各軸協(xié)調(diào)。連續(xù)軌跡控制( CP 控制)有路徑約束，因此要對(duì)路徑進(jìn)行設(shè)計(jì)。路徑控制與插補(bǔ)方式分類如表 5-2 所示。表 5-2 路徑控制與插補(bǔ)方式分類路 徑 控 制 不 插 補(bǔ) 關(guān) 節(jié) 插 補(bǔ) (平 滑 ) 空 間 插 補(bǔ)點(diǎn) 位 控 制PTP(1) 各 軸 獨(dú) 立 快速 到 達(dá) 。(2) 各 關(guān) 節(jié) 最 大加 速 度 限 制(1) 各 軸 協(xié) 調(diào) 運(yùn) 動(dòng) 定 時(shí) 插 補(bǔ) 。(2) 各 關(guān) 節(jié) 最 大 加 速 度 限 制連 續(xù) 路 徑控 制 CP(1) 在 空 間 插 補(bǔ) 點(diǎn) 間 進(jìn) 行 關(guān)節(jié) 定 時(shí) 插 補(bǔ) 。(2) 用 關(guān) 節(jié) 的 低 階 多 項(xiàng) 式 擬合 空 間 直 線 使 各 軸 協(xié) 調(diào) 運(yùn) 動(dòng) 。(3) 各 關(guān) 節(jié) 最 大 加 速 度 限 制(1) 直 線 、 圓 弧 、 曲 線 等 距 插補(bǔ) 。(2) 起 停 線 速 度 、 線 加 速 度 給定 ， 各 關(guān) 節(jié) 速 度 、 加 速 度 限 制255.4 機(jī)器人軌跡插值計(jì)算給出各個(gè)路徑結(jié)點(diǎn)后，軌跡規(guī)劃的任務(wù)包含解變換方程，進(jìn)行運(yùn)動(dòng)學(xué)反解和插值計(jì)算。在關(guān)節(jié)空間進(jìn)行規(guī)劃時(shí)，需進(jìn)行的大量工作是對(duì)關(guān)節(jié)變量的插值計(jì)算。插補(bǔ)分為：直線插補(bǔ)、圓弧插補(bǔ)、定時(shí)插補(bǔ)、定距插補(bǔ)和關(guān)節(jié)空間插補(bǔ)等。在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃，需要給定機(jī)器人在起始點(diǎn)和終止點(diǎn)手臂的位形。對(duì)關(guān)節(jié)進(jìn)行插值時(shí)應(yīng)滿足一系列的約束條件，例如抓取物體時(shí)手部的運(yùn)動(dòng)方向(初始點(diǎn))、提升物體離開的方向(提升點(diǎn))、放下物體(下放點(diǎn))和停止點(diǎn)等結(jié)點(diǎn)上的位姿、速度和加速度的要求；與此相應(yīng)的各個(gè)關(guān)節(jié)位移、速度、加速度在整個(gè)時(shí)間間隔內(nèi)的連續(xù)性要求以及其極值必須在各個(gè)關(guān)節(jié)變量的容許范圍之內(nèi)等。滿足所要求的約束條件之后，可以選取不同類型的關(guān)節(jié)插值函數(shù)，生成不同的軌跡。常用的關(guān)節(jié)空間插補(bǔ)有：三次多項(xiàng)式插值、高階多項(xiàng)式插值、用拋物線過渡的線性插值等方法，本章重點(diǎn)對(duì)關(guān)節(jié)空間插補(bǔ)中的三次多項(xiàng)式插值進(jìn)行討論。5.4.1 三次多項(xiàng)式插值在機(jī)器人運(yùn)動(dòng)過程中，若末端執(zhí)行器的起始和終止位姿已知，由逆向運(yùn)動(dòng)學(xué)即可求出對(duì)應(yīng)于兩位姿的各個(gè)關(guān)節(jié)角度。末端執(zhí)行器實(shí)現(xiàn)兩位姿的運(yùn)動(dòng)軌跡描述可在關(guān)節(jié)空間中用通過起始點(diǎn)和終止點(diǎn)關(guān)節(jié)角的一個(gè)平滑軌跡函數(shù) ?(t)來表示。為實(shí)現(xiàn)系統(tǒng)的平穩(wěn)運(yùn)動(dòng)，每個(gè)關(guān)節(jié)的軌跡函數(shù) ?(t)至少需要滿足四個(gè)約束條件，即兩端點(diǎn)位置約束和兩端點(diǎn)速度約束。端點(diǎn)位置約束是指起始位姿和終止位姿分別所對(duì)應(yīng)的關(guān)節(jié)角度。 ?(t)在時(shí)刻 =00t時(shí)的值是起始關(guān)節(jié)角度 ，在終端時(shí)刻 時(shí)的值是終止關(guān)節(jié)角度 ，即0?ft f(5.1)????0)(ft?為滿足關(guān)節(jié)運(yùn)動(dòng)速度的連續(xù)性要求，起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)速度可簡(jiǎn)單地設(shè)定為零，即(5.2)???????0)(ft?上面給出的四個(gè)約束條件可以惟一地確定一個(gè)三次多項(xiàng)式（5.3)??3210tatat?運(yùn)動(dòng)過程中的關(guān)節(jié)速度和加速度則為(5.4)?????tt32.216)(??為求得三次多項(xiàng)式的系數(shù) a0，a1，a2 和 a3，將式(5.1)和式(5.2)代以給定的約束26條件，有方程組(5.5)??????2321300ffffff tattta??求解該方程組，可得(5.6)??????)(230031??fftaa對(duì)于起始速度及終止速度為零的關(guān)節(jié)運(yùn)動(dòng)，滿足連續(xù)平穩(wěn)運(yùn)動(dòng)要求的三次多項(xiàng)式插值函數(shù)為(5.7)3032020 )()(3)( tttt ff ???????由式(3.9)可得關(guān)節(jié)角速度和角加速度的表達(dá)式為(5.8)2002366())()f fttt?（5.9）. 1f ft tt?????三次多項(xiàng)式插值的關(guān)節(jié)運(yùn)動(dòng)軌跡曲線如圖 5.1 所示。由圖可知，其速度曲線為拋物線，相應(yīng)的加速度曲線為直線。圖 5.1 三次多項(xiàng)式插值的關(guān)節(jié)運(yùn)動(dòng)軌跡5.4.2 機(jī)械手軌跡規(guī)劃。本文所研究的是五自由度機(jī)械手的正運(yùn)動(dòng)問題。在起點(diǎn)和終點(diǎn)，末端關(guān)節(jié)的位姿是已知的，故可通過前文所討論的求得兩位置時(shí)各關(guān)節(jié)角度。因此，可以用平滑的插27值函數(shù)來描述機(jī)械手的起始位置和目標(biāo)位置之間的運(yùn)動(dòng)軌跡。另外，為了使實(shí)際生成的軌跡平滑，在機(jī)械手的整個(gè)工作期間，除了保證其運(yùn)動(dòng)和速度的連續(xù)之外，還應(yīng)該保證加速度不發(fā)生突變，使運(yùn)動(dòng)平穩(wěn)。根據(jù)已知條件和前文的討論，選用三次多項(xiàng)式軌跡規(guī)劃方法進(jìn)行規(guī)劃。起點(diǎn)和終點(diǎn)位姿如表 5-3 所示表 5-3 機(jī)械手起點(diǎn)、終點(diǎn)給定值控制點(diǎn) xpyzp???起點(diǎn) 151 231 250 32 45 60終點(diǎn) 200 300 178 20 50 80由機(jī)械手運(yùn)動(dòng)學(xué)方程的逆解可以求得控制點(diǎn)各個(gè)關(guān)節(jié)變量值，如表 5-4 所示：表 5-4 機(jī)械手起點(diǎn)、終點(diǎn)關(guān)節(jié)變量值控制點(diǎn) 1?23?45?起點(diǎn) 56.31 80.922 -45.244 -52.738 -24.683終點(diǎn) 56.828 -66.006 73.282 -83.861 -9.074各關(guān)節(jié)軌跡的生成由于在三次多項(xiàng)式的插值計(jì)算中，為實(shí)現(xiàn)系統(tǒng)的平穩(wěn)運(yùn)動(dòng)，每個(gè)關(guān)節(jié)的軌跡函數(shù)至少需要滿足四個(gè)約束條件，即兩端點(diǎn)的位置約束和兩端點(diǎn)的速度約束。在機(jī)器)(t?人正運(yùn)動(dòng)過程中，末端執(zhí)行器的起始和終止位姿已經(jīng)給出，由逆向運(yùn)動(dòng)學(xué)即可求得對(duì)應(yīng)于兩位姿的各個(gè)關(guān)節(jié)角度，如表（5-4）所示；再假設(shè)在初始位置和目標(biāo)位置的速度為零，即在這兩個(gè)位置的位姿函數(shù)的導(dǎo)數(shù)為零，即有四個(gè)約束條件，滿足了解得各個(gè)關(guān)節(jié)的三次多項(xiàng)式軌跡的要求。最后，根據(jù)位移表達(dá)式（5.7）代入已知數(shù)據(jù)求得位移，再分別求一階導(dǎo)數(shù)，二階導(dǎo)數(shù)即式（5.8）和（5.9）便可求得速度和加速度表達(dá)式。下一章將用 Visual Basic 6.0 對(duì)機(jī)械手各關(guān)節(jié)的運(yùn)動(dòng)軌跡作介紹。