機器人學(xué)導(dǎo)論.doc

機器人學(xué)導(dǎo)論學(xué) 院： 工程機械學(xué)院 專 業(yè)： 機械工程 姓 名： 劉 敏 學(xué) 號： 2012225015 任課教師： 蔡宗琰 成 績： 目錄一、問題重述41.1、問題重述41.2 目標(biāo)任務(wù)4二、問題分析5三、模型的假設(shè)6四、符號說明6五、模型建立與求解75.1運動學(xué)模型建立與求解75.1.1機器人運動方程的建立75.1.2 利用逆運動學(xué)方法求解115.2、問題11的模型125.2.1搜索算法流程圖135.2.2、模型求解145.3、問題12、3的模型165.3.1、問題1的、175.3.2、問題2的195.3.3、問題2的215.4、問題324七、模型的評價247.1模型的優(yōu)點247.2模型的缺點24參考文獻(xiàn)25摘要本文探討了六自由度機械臂從一點到另一點沿任意軌跡移動路徑、一點到另一點沿著給定軌跡移動路徑、以及無碰撞路徑規(guī)劃問題，并討論了設(shè)計參數(shù)對機械臂靈活性和使用范圍的影響，同時給出了建議。問題一：（1）首先確定初始坐標(biāo)均為零時機械臂姿態(tài)，建立多級坐標(biāo)系，利用空間解析幾何的變換基本原理及相對坐標(biāo)系的齊次坐標(biāo)變換的矩陣解析方法，來建立機器人的運動系統(tǒng)的多級變換方程。通過逆運動學(xué)解法和構(gòu)建規(guī)劃，來求優(yōu)化指令（2）假定機械臂初始姿態(tài)為0，曲線離散化，每個離散點作為末端位置，通過得到的相鄰兩點的姿態(tài)，利用（1）中算法計算所有相鄰兩點間的增量指令，將滿足精度要求的指令序列記錄下來。（3）通過將障礙物理想化為球體，將躲避問題就轉(zhuǎn)化成保證機械手臂上的點與障礙球球心距離始終大于r的問題。進而通過迭代法和指令檢驗法，剔除不符合要求的指令，從而實現(xiàn)避障的目的 問題二：將問題二中的實例應(yīng)用到問題一中的相對應(yīng)的算法中。 問題三：靈活性與適用范圍相互制約，只能根據(jù)權(quán)重求得較優(yōu)連桿長度。關(guān)鍵詞：多級坐標(biāo)變換 逆運動學(xué)解法 遺傳搜索算法 優(yōu)化 一、問題重述 1.1、問題重述 某型號機器人（圖示和簡化圖略）一共有6個自由度，分別由六個旋轉(zhuǎn)軸（關(guān)節(jié)）實現(xiàn)，使機器人的末端可以靈活地在三維空間中運動。機器人關(guān)于六個自由度的每一個組合，表示機械臂的一個姿態(tài)，顯然每個姿態(tài)確定頂端指尖的空間位置X：。假定機器人控制系統(tǒng)只能夠接收改變各個關(guān)節(jié)的姿態(tài)的關(guān)于連桿角度的增量指令（機器指令），使得指尖（指尖具有夾工具、焊接、擰螺絲等多種功能，不過在這里不要求考慮這方面的控制細(xì)節(jié)）移動到空間點X，其中各個增量只能取到-2, -1.9, -1.8, ，1.8, 1.9, 2這41個離散值（即精度為0.1，絕對值不超過2）。通過一系列的指令序列可以將指尖依次到達(dá)位置X0，X1，Xn，則稱X0，X1，Xn為從指尖初始位置X0到達(dá)目標(biāo)位置Xn的一條路徑（運動軌跡）。根據(jù)具體的目標(biāo)和約束條件計算出合理、便捷、有效的指令序列是機器人控制中的一個重要問題。假設(shè)機器人的初始位置在y-z平面上，約定直角坐標(biāo)系的原點設(shè)在圖示的A點，z軸取為AB方向，x軸垂直紙面而y軸則在基座所固定的水平臺面上。 1.2 目標(biāo)任務(wù) 問題一：設(shè)計一個通用的算法，用來計算執(zhí)行下面指定動作所要求的指令序列，并要求對算法的適用范圍、計算效率以及近似算法所造成的誤差和增量離散取值所造成的誤差大小進行討論（不考慮其他原因造成的誤差）：（1）已知初始姿態(tài)0和一個可達(dá)目標(biāo)點的空間位置（Ox, Oy, Oz），計算指尖到達(dá)目標(biāo)點的指令序列。（2）要求指尖沿著預(yù)先指定的一條空間曲線x = x(s), y = y(s), z = z(s), a s b 移動，計算滿足要求的指令序列。（3）在第個問題中，假設(shè)在初始位置與目標(biāo)位置之間的區(qū)域中有若干個已知大小、形狀、方向和位置的障礙物，要求機械臂在運動中始終不能與障礙物相碰，否則會損壞機器。這個問題稱機械臂避碰問題，要求機械臂末端在誤差范圍內(nèi)到達(dá)目標(biāo)點并且整個機械臂不碰到障礙物（機械臂連桿的粗細(xì)自己設(shè)定）。問題二：應(yīng)用你的算法就下面具體的數(shù)據(jù)給出計算結(jié)果，并將計算結(jié)果以三組六維的指令序列（每行6個數(shù)據(jù)）形式存放在Excel文件里，文件名定為answer1.xls，answer2.xls和answer3.xls。假設(shè)在機械臂的旁邊有一個待加工的中空圓臺形工件，上部開口。工件高180mm，下底外半徑168mm，上底外半徑96mm，壁厚8mm。豎立地固定在xy平面的操作臺上，底部的中心在 (210, 0, 0)。要求機械臂（指尖）從初始位置移動到工具箱所在位置的 (20,200, 120) 處，以夾取要用的工具。如果圓臺形工件外表面與平面x =2z的交線是一條裂紋需要焊接，請你給出機械臂指尖繞這條曲線一周的指令序列。有一項任務(wù)是在工件內(nèi)壁點焊四個小零件，它們在內(nèi)表面上的位置到x-y平面的投影為（320,-104）、（120,106）、（190,-125）和（255,88）。要求機械臂從圓臺的上部開口處伸進去到達(dá)這些點進行加工，為簡捷起見，不妨不計焊條等的長度，只考慮指尖的軌跡問題三：制造廠家希望通過修改各條連桿的相對長度以及各關(guān)節(jié)最大旋轉(zhuǎn)角度等設(shè)計參數(shù)提高機械臂的靈活性和適用范圍。請根據(jù)你們的計算模型給他們提供合理的建議。二、問題分析機械臂運動路徑設(shè)計問題主要涉及到相對坐標(biāo)系坐標(biāo)變換、機器人正運動學(xué)分析、逆運動學(xué)求解、優(yōu)化以及機器人避碰問題。1運動方程的建立從機構(gòu)學(xué)觀點來看，機器人屬于空間機構(gòu)范疇。桿件每次轉(zhuǎn)動因此，采用空間解析幾何的變換基本原理及坐標(biāo)變換的矩陣解析方法，來建立機器人的運動系統(tǒng)的多級變換方程。由于旋轉(zhuǎn)軸涉及到平行連桿和垂直連桿兩類，因此對于各類旋轉(zhuǎn)變換，所使用的變換矩陣也不相同。同時，此題中涉及的機器人有6個自由度，則從工件的坐標(biāo)位置到固定坐標(biāo)系的變換要經(jīng)過多級坐標(biāo)變換。采用多級坐標(biāo)變換的方法。由上述三點，便可建立機器人運動系統(tǒng)的坐標(biāo)變換關(guān)系式。2尖端軌跡曲線模型的建立對于已給定一條空間曲線x=(s),y=(y),z=(z),可將其看成一個點的集合。因此機械臂實現(xiàn)一個空間軌跡的過程是實現(xiàn)軌跡離散的過程。如果這些離散點間距很大，機械臂軌跡與要求的軌跡就有較大誤差。只有這些離散點彼此很接近，才有可能使機械臂的軌跡以滿足要求精確度逼近要求的軌跡。連續(xù)軌跡的控制實際上就是在多次執(zhí)行離散點間的點位控制，離散點點越密集越能逼近要求的運動曲線。3避碰問題假設(shè)空間障礙物為半徑為r的球體，則這些球體空間便形成了機械手臂的約束。而躲避問題就轉(zhuǎn)化成保證機械手臂上的點與障礙球球心距離始終大于r的問題。根據(jù)運動規(guī)律，不難知道手臂相對于初始位置的姿態(tài)決定于之前執(zhí)行的所有指令的和。已知連桿上的點L在其所在的相對坐標(biāo)系中的坐標(biāo)及轉(zhuǎn)動的指令，根據(jù)齊次坐標(biāo)變換矩陣就可得到L在固定坐標(biāo)系中的坐標(biāo)，然后可計算距離。所以 可以用問題1-1算法產(chǎn)生點到點的指令，可以利用迭代法從初始位置開始提前檢驗每個指令，不滿足要求的無法執(zhí)行。4.自由度分析：由題中指出的：指尖E點，具有夾工具、焊接、擰螺絲等多種功能，不過在這里不要求考慮這方面的控制細(xì)節(jié)。則將情況理想化，不考慮第6個自由度對運動的影響。三、模型的假設(shè)1.各關(guān)節(jié)連桿在輸入指令后同時開始轉(zhuǎn)動，速度為低速，各桿件之間無摩擦，臂各旋轉(zhuǎn)軸最大運動速度相同2.在軌跡規(guī)劃中不考慮機械臂關(guān)節(jié)轉(zhuǎn)角的限制。3.不考慮機械臂結(jié)構(gòu)和裝配產(chǎn)生的誤差。4.連桿為剛體，不會發(fā)生形變5.設(shè)機器人的初始位置是在一個平面上的（y-z 平面）。6.關(guān)節(jié)視為質(zhì)點，它們所占的面積可忽略不計。四、符號說明：坐標(biāo)系，為固定坐標(biāo)系，其余為固定在桿上的相對坐標(biāo)系：到的坐標(biāo)系變換矩陣:尖端移動終點在固定坐標(biāo)系中的x方向坐標(biāo)：尖端移動終點在固定坐標(biāo)系中的y方向坐標(biāo) ：尖端移動終點在固定坐標(biāo)系中的z方向坐標(biāo) ：尖端轉(zhuǎn)動起點在固定坐標(biāo)系中初始x方向坐標(biāo)：尖端轉(zhuǎn)動起點在固定坐標(biāo)系中初始y方向坐標(biāo) ：尖端轉(zhuǎn)動起點在固定坐標(biāo)系中初始z方向坐標(biāo)-第i個自由度的相對于初始姿態(tài)轉(zhuǎn)角(i=1,2,3,4,5,6)-第i個自由度轉(zhuǎn)角的增量(i=1,2,3,4,5,6)si-sin ci-cos五、模型建立與求解5.1運動學(xué)模型建立與求解5.1.1機器人運動方程的建立 為了產(chǎn)生點到點的運動序列, 首先我們先作出初始角度均為0時的姿態(tài)，以每個節(jié)點為原點建立坐標(biāo)系到如圖1。 圖1 從圖2可以看出F點相當(dāng)于將繞Z軸旋轉(zhuǎn)得到，同理其他節(jié)點分別相當(dāng)于將前一坐標(biāo)系繞X,X,Z,X,Z軸轉(zhuǎn)動?？傻玫礁髯鴺?biāo)系相對于前一個的坐標(biāo)系的坐標(biāo)變換公式。從而得到機械臂各個關(guān)節(jié)的變換矩陣：旋轉(zhuǎn)矩陣：位置矢量：圖2 相乘,得 其中2確定固定坐標(biāo)系中E點位置由題設(shè)中，已知固定坐標(biāo)系原點，根據(jù)給定的連桿長度和角度，易計算得出，在固定坐標(biāo)系中E位置： Ex=-65(-cos(s1)sin(s4)+(-sin(s1)cos(s2)cos(s3)+sin(s1)sin(s2)sin(s3)cos(s4)sin(s5)+65(sin(s1)cos(s2)sin(s3)+sin(s1)sin(s2)cos(s3)cos(s5)+255sin(s1)cos(s2)sin(s3)+255sin(s1)sin(s2)cos(s3)+255sin(s1)sin(s2)Ey=-65(-sin(s1)sin(s4)+(cos(s1)cos(s2)cos(s3)-cos(s1)sin(s2)sin(s3)cos(s4)sin(s5)+65(-cos(s1)cos(s2)sin(s3)-cos(s1)sin(s2)cos(s3)cos(s5)-255cos(s1)cos(s2)*sin(s3)-255cos(s1)sin(s2)cos(s3)-255cos(s1)sin(s2)Ez=140-65(sin(s2)cos(s3)+cos(s2)sin(s3)cos(s4)sin(s5)+65(-sin(s2)sin(s3)+cos(s2)cos(s3)cos(s5)-255sin(s2)sin(s3)+255cos(s2)cos(s3)+255cos(s2)這樣便得到了運動學(xué)方程。5.1.2 利用逆運動學(xué)方法求解（1）求可用逆變換左乘運動方程式兩邊得：根據(jù)對應(yīng)元素相等可解得:（2）求由：解得:（3）求 （1） （2）解得：其中 （4）求 或（5）求由，可以解得：或（6）求；或5.2、問題11的模型在已有六自由度手臂運動方程和逆運動學(xué)解法的基礎(chǔ)上，若已知機械臂末端轉(zhuǎn)動終點坐標(biāo)和轉(zhuǎn)動起點坐標(biāo)已知，就可以得到各關(guān)節(jié)的坐標(biāo)，連桿的相對于初始狀態(tài)的角度變化及機械臂的姿態(tài)。針對問題21，根據(jù)題設(shè)，要求機械臂指針從所給基坐標(biāo)系初始點到目標(biāo)點的運動序列，初末狀態(tài)如圖：圖35.2.1確定目標(biāo)函數(shù)設(shè)機械臂指尖初始位置的坐標(biāo)為，目標(biāo)點的位置坐標(biāo)為，設(shè)為指尖由初始姿態(tài)經(jīng)過一系列指令變換后的各旋轉(zhuǎn)角的總增量，其對應(yīng)的位置為。為了使指尖實際到達(dá)點盡可能與目標(biāo)點接近，即實際指尖實際到達(dá)的點與目標(biāo)點的位置偏差盡可能小，可得到如下目標(biāo)函數(shù)：其中，即表示實際指尖到達(dá)點與目標(biāo)點的位置偏差。由于各個增量只能取到-2，-1.9，-1.8,1.8,1.9,2這41個離散值（即精度為，絕對值不超過），通過一系列的指令序列可以將指尖依次到達(dá)位置，則稱為從指尖初始位置到達(dá)目標(biāo)位置的移動路徑，故要達(dá)到目標(biāo)點需要進行多次移動，考慮到操作指令序列的復(fù)雜性，以移動的次數(shù)越少越好，即使指令步數(shù)盡量小。5.2.2確定約束條件(1)機器人的六個自由度轉(zhuǎn)角有一定的運動范圍， 即各個自由度角度總增量應(yīng)在對應(yīng)的變化范圍內(nèi)，即：又因為關(guān)節(jié)的角度改變不影響機械臂指針的坐標(biāo)，故將其指令恒視作0。(2)指尖所在的初始點為，初始姿態(tài)為，為由初始姿態(tài)經(jīng)過一系列指令變換后的各旋轉(zhuǎn)角的總增量，其對應(yīng)的位置為，這一系列指令序列為，其中相對于的角度增量為，因此，有：(3) 各個自由度角度單步增量只能取到-2，-1.9，-1.8,1.8,1.9,2這41個離散值（即精度為，絕對值不超過），即：5.2.3得到多目標(biāo)規(guī)劃模型5.2.4 模型的算法由于模型是一個雙目標(biāo)非線性規(guī)劃模型，基本思想是當(dāng)做兩個單目標(biāo)非線性規(guī)劃模型進行求解和篩選運算。模型首先采用遺傳算法，簡稱模仿自然界生物進化機制進行隨機全局搜索和優(yōu)化。重點針對第一目標(biāo)即機械臂指針?biāo)竭_(dá)目標(biāo)點的精確度進行優(yōu)化，具體算法如下：編碼個體評判隨機產(chǎn)生種群解集合選擇運算交叉運算解碼產(chǎn)生新種群變異運算利用遺傳算法求解時考慮到目標(biāo)二，即完成路徑的最少指令數(shù)，指令序列的生成分粗調(diào)和微調(diào)兩部分，粗調(diào)即讓機械臂各關(guān)節(jié)每次以最大幅度度動作，微調(diào)是讓機械臂各關(guān)節(jié)每次以最小幅度度動作進行精確調(diào)整，在保證精確度的同時盡量減少動作次數(shù)，按照以上算法編碼、根據(jù)已知的矩陣生成適應(yīng)度函數(shù)，隨機產(chǎn)生種群后進行選擇、交叉、變異運算，具體運行參數(shù)如下：群體大?。航徊娓怕剩河米赃m應(yīng)的方式，初始值取，在隨后的變動中.變異概率：取變異概率為0.1。隨著遺傳算法在線性能的下降可以減小變化，.遺傳代數(shù)：，即算法算至代時運算停止。用運行后得到相對最優(yōu)解及誤差L。為了進一步將機械臂指針點到點路徑優(yōu)化，利用搜索功能進行第二次篩選，在誤差精度基本保持一致的前提下減少動作次數(shù)。將得到的優(yōu)化解整理成指令序列。5.2.2、模型求解開始遺傳算法，得到“最佳適應(yīng)度”圖形和“最佳個體圖”圖形如下：圖5由圖中最佳值和平均值可以看出可以看出，采用該算法計算模型的收斂速度快，算法穩(wěn)定性好。由度機械臂最終位姿得各個關(guān)節(jié)得轉(zhuǎn)角見表1：表1圖6圖6為遺傳算法最優(yōu)解下機械臂指針運行的軌跡。指令序列如下：共步。表2122-2-220222-2-220322-2-220422-2-220522-2-220622-2-220722-2-220822-2-220922-2-2201022-2-2201122-2-2201222-2-2201322-2-2201422-2-2201522-2-2201622-2-2201722-2-2209900002010000002010100002010200001.60 實際到達(dá)點為：表3與理論到達(dá)點誤差為： 將初狀態(tài)帶入搜索算法模型，對最優(yōu)解進行篩選。得到相對最優(yōu)解如下：表4圖7上圖為優(yōu)化后機械臂指針軌跡圖，其中實際到達(dá)目標(biāo)點為表5指令共步，誤差為： 5.3、問題12、3的模型問題1的、和問題2的、5.3.1、問題1的、已給定一條空間曲線x = x(s), y = y(s), z = z(s), a s b 。由于代入計算機運算。我們將空間曲線離散化，取為若干個離散點，對離散點進行計算。即：將目標(biāo)坐標(biāo)由中的一固定點變化為若干個滿足空間曲線分布的離散點的集合。若根據(jù)問題1的的算法即逐步逼近的方法來計算，則會存在兩點之間逼近過程中出現(xiàn)迂回的現(xiàn)象?，F(xiàn)對此算法進行優(yōu)化。目標(biāo)坐標(biāo)系中存在的關(guān)系。為求得兩點的最短路徑，作如下計算：（，）是，的參數(shù)。則對，求偏導(dǎo)，有即： 即： 算法的流程圖：將曲線上的點離散化為n個（）計算出當(dāng)前點的坐標(biāo)和目標(biāo)點坐標(biāo)計算坐標(biāo)差向量計算A矩陣計算A的廣義逆陣計算調(diào)整值使指尖E從點處移動到判斷k是否等n是：結(jié)束 否：K=k+1 圖8同時，在中，考慮避碰問題。將機械臂進行拉伸，考慮機械臂的最大活動范圍。下面分析矩陣大小的選擇：因為連桿在x、y方向的極限長度為，設(shè)定一個區(qū)間為：400mm*400mm*400mm.設(shè)定單位小區(qū)間為：2mm*2mm*2mm.考慮與設(shè)定單位小區(qū)間為：1mm*1mm*1mm相比，用這個數(shù)據(jù)有利于節(jié)省計算機內(nèi)存。這也是我們將連桿橫截面積取為2mm*2mm*2mm的原因。將此空間用矩陣來描述。在障礙物存在的地方，設(shè)定取值為1，否則，設(shè)定取值為0。這樣建立一個Z空間來處理避碰問題。5.3.2、問題2的根據(jù)題設(shè)給定圓臺以及平面x = 2 z ,確定圓臺形工件外表面與平面 x = 2 z 的交線。圓臺形工件外表面公式為：，與平面的交線為?；蓞?shù)方程形式：下圖是所要求路徑的空間圖形：圖9 對路徑進行等分成分，后進行進行隨機的取點點（軌跡如圖10所示），共得到個指令點，坐標(biāo)如下：表6下圖為描點圖形：圖10按模型算法將個點構(gòu)成的軌跡向、 、面投影，分別得到相應(yīng)的值，以面為例：表7同理得到 、面的值：表8不存在根據(jù)遺傳搜索法，以為起點運動，得到其到達(dá)下一點的軌跡指令序列：表95.3.3、問題2的根據(jù)上述兩點的分析結(jié)果和算法，可得內(nèi)表面的曲面方程為： （）四個焊接點坐標(biāo)： （320，-104，21.5492）（120，-106，52.3651）（190，-125，83.5253）（255，88，152.9044） 算法流程圖如下：建立三維矩陣空間建立障礙物的Z空間（即將障礙物所在處賦值為1，其余賦為0）按問題1的中優(yōu)化搜索算法進行搜索并在此加以改進。每次移動E點前計算在該坐標(biāo)處，A,B,C,D的坐標(biāo)，判斷其連線AB,BC,CD,DE上是否有點處值為1（即與障礙物相碰）否：移動E點到目標(biāo)點排除該優(yōu)先級繼續(xù)搜索圖11 將數(shù)據(jù)代入計算，可得結(jié)果見表10： 表10 對結(jié)果進行分析，并用MATLAB對針尖運動路徑作圖如下：（由于是立體圖形，所以用兩個圖顯示兩個不同的方位示意）圖12圖135.4、問題3考慮修改各條連桿的相對長度以及各關(guān)節(jié)最大旋轉(zhuǎn)角度等設(shè)計參數(shù)提高機械臂的靈活性和適用范圍。（1）從靈活性角度考慮，連桿每作一次移動，產(chǎn)生的變化，則相對的角度也會出現(xiàn)變動。要提高靈活性即是要提高角度變化向量的變化范圍。這里我們考慮能量最省的優(yōu)化原則。則用角度變化向量的二范數(shù)來判定。（2）適用范圍：在所有桿的長度之和為固定值的情況下，指尖點E所能掃過的最大空間體積。靈活性和適用范圍存在的一些相互制約的關(guān)系，根據(jù)本文的分析結(jié)果，我們建議越靠近基座的值精度高一些，使得E點所能掃過的空間點數(shù)更多。七、模型的評價7.1模型的優(yōu)點1、基于齊次坐標(biāo)變換法的點位控制模型評價：該模型可以適用于所有已知目標(biāo)位置和姿態(tài)的點位控制問題。算法的計算效率非常的高，并且實時控制性好。算法誤差產(chǎn)生于多次三角變換求解所帶來的舍入誤差。2、連續(xù)軌跡控制模型評價：該模型適用于所有參數(shù)方程的空間曲線生成機器指令。適用于機器人在低速運動的情況。但離散點數(shù)量較少時，會產(chǎn)生誤差。3、通過求解整個Z空間，使得自由空間、障礙空間一目了然，因而，可按任何性能指標(biāo)搜索路徑且具有完備解。4、通過權(quán)重來判斷影響因素最大的方法來選擇搜索最優(yōu)路徑，有利于節(jié)省能量。5、通過曲線的參數(shù)方程直接得到值的增量，精度比較高，連續(xù)性比較好。7.2模型的缺點1、在焊接裂紋的時候沒有考慮焊接角度即桿DE與曲線法線方向所成的角度，實際上此夾角越大越容易造成對圓臺壁的損傷，相應(yīng)的焊接效果越差。2、障礙的Z空間只能通過離散化求得，而難獲得Z空間障礙的解析表達(dá)式，因而，為了提高精度要占用大量的內(nèi)存。3、對于關(guān)節(jié)敬較多的機械臂，Z空間自由路徑的搜索也將花費較多的時間。4、在搜索時，由于自由度的牽連關(guān)系，導(dǎo)致在搜索時可能會生成鋸齒形路徑。參考文獻(xiàn)1機械運動學(xué)教程2 機器人手臂下棋運動的逆運動學(xué)分析與仿真 趙艷云3 周遠(yuǎn)清等，只能機器人系統(tǒng)M,北京：清華大學(xué)出版社4 陳明生，沙威，謝瑩，六自由度機械臂運動路徑設(shè)計，數(shù)學(xué)的實踐與認(rèn)識，2008年7月5 龔純，王正林，精通MATLAB最優(yōu)化計算，電子工業(yè)出版社，2009機械臂參數(shù)關(guān)節(jié)變量符號初始位置變量范圍10 1802-90 12530 13840 2705-90-120 +133.560 270附件：問題1-1nx=T06(1,1);ny=T06(2,1);nz=T06(3,1);ox=T06(1,2);oy=T06(2,2);oz=T06(3,2); ax=T06(1,3);ay=T06(2,3);az=T06(3,3); px=T06(1,4);py=T06(2,4);pz=T06(3,4); n=nx ny nz; o=ox oy oz; a=ax ay az; p=px py pz;theta=zeros(6,100);for i=2:100 %theta1 theta11=atan(py-65*ay)/(px-65*ax); theta12=pi+atan(py-65*ay)/(px-65*ax); if theta11-theta(i-1,1)=theta12-theta(i-1,1) theta(i,1) =theta12; else theta(i,1) =theta11; end %theta3theta31=asin(1)+atan(1/6);theta32=pi-asin(1)+atan(1/6);if theta31-theta(i-1,3)=theta32-theta(i-1,3) theta(i,3) = theta32; else theta(i,3) = theta31;end %theta2 w=atan(c1(i,j)*px+s1(i,j)*py-65*(c1(i,j)*ax+s1(i,j)*ay)/(pz-65*az); B=(255*c3(i,j)/sqrt(c1(i,j)*px+s1(i,j)*py-65*c1(i,j)*ax-65*s1(i,j)*ay)2+(pz-p*az)2); theta21=asin(B-w-theta(i,3); theta22=pi-asin(B)-w-theta(i,3); if theta21-theta(i-1,2)=theta22-theta(i-1,2) theta(i,2) = theta22; else theta(i,2) = theta21; end %theta5theta51=acos(s231(i,j)*(c1(i,j)*ax+s1(i,j)*ay)-c231(i,j)*az);theta52=2*pi-acos(s232(i,j)*(c1(i,j)*ax+s1(i,j)*ay)-c232(i,j)*az);if theta51-theta(i-1,5)=b5-theta(i-1,5) theta(i,5) = theta52; else theta(i,5) = theta51;end %theta4 theta41=asin(s511(i,j)*ax-c1(i,j)*ay)/s511(i,j); theta42=pi-asin(s1(i,j)*ax-c1(i,j)*ay)/s511(i,j); if atheta41-theta(i-1,4)=theta42-theta(i-1,4) theta(i,4) = theta42; else theta(i,4) = theta41; end %theta6 theta61=asin(sin(c1(i,j)*ox+s1(i,j)*oy)-c231(i,j)*oz)/s511(i,j); theta62=pi-asin(s231(i,j)*(c1(i,j)*ox+s1(i,j)*oy)-c231(i,j)*oz)/s511(i,j); if theta61-theta(i-1,6)=theta62-theta(i-1,6) theta(i,6) = theta62; else theta(i,6) = theta61; end end問題2-1T06=transl(20,-200,120); nx=T06(1,1);ny=T06(2,1);nz=T06(3,1); ox=T06(1,2);oy=T06(2,2);oz=T06(3,2); ax=T06(1,3);ay=T06(2,3);az=T06(3,3); px=T06(1,4);py=T06(2,4);pz=T06(3,4); n=nx ny nz; o=ox oy oz; a=ax ay az; p=px py pz;theta=zeros(7,6);for i=2:100 %theta1 theta11=atan(py-65*ay)/(px-65*ax); theta12=pi+atan(py-65*ay)/(px-65*ax); if theta11-theta(i-1,1)=theta12-theta(i-1,1) theta(i,1) =theta12; else theta(i,1) =theta11; end %theta3 theta31=asin(1)+atan(1/6); theta32=pi-asin(1)+atan(1/6); if theta31-theta(i-1,3)=theta32-theta(i-1,3) theta(i,3) = theta32; else theta(i,3) = theta31; end c1(i)=cos(theta(i,1); s1(i)=sin(theta(i,1); c3(i)=cos(theta(i,3); s3(i)=sin(theta(i,3); c23(i)=cos(theta(i,1)+theta(i,3); s23(i)=sin(theta(i,1)+theta(i,3); %theta2 w(i)=atan(c1(i)*px+s1(i)*py-65*(c1(i)*ax+s1(i)*ay)/(pz-65*az); B(i)=(255*c3(i)/sqrt(c1(i)*px+s1(i)*py-65*c1(i)*ax-65*s1(i)*ay)2+(pz-1*az)2); theta21=asin(B(i)-w(i)-theta(i,3); theta22=pi-asin(B(i)-w(i)-theta(i,3); if theta21-theta(i-1,2)=theta22-theta(i-1,2) theta(i,2) = theta22; else theta(i