(完整word版)機械臂運動路徑設(shè)計分析(word文檔良心出品)

1、-1- 機械臂運動路徑設(shè)計分析 摘要 本文探討了六自由度機械臂從一點到另一點沿任意軌跡移動路徑、 一 點到另一點沿著給定軌跡移動路徑、 以及無碰撞路徑規(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)為 ,曲線離散化，每個離散點作為末端 位置，通過得到的相鄰兩點的姿態(tài)，利用( 1)中算法計算所有相鄰兩點 間的增量

2、指令,將滿足精度要求的指令序列記錄下來。 ( 3)通過將障礙物理想化為球體, 將躲避問題就轉(zhuǎn)化成保證機械手臂 上的點與障礙球球心距離始終大于 r 的問題。進而通過迭代法和指令檢驗 法,剔除不符合要求的指令,從而實現(xiàn)避障的目的 問題二：將問題二中的實例應(yīng)用到問題一中的相對應(yīng)的算法中,部 分結(jié)果見附錄 問題三：靈活性與適用范圍相互制約,只能根據(jù)權(quán)重求得較優(yōu)連桿 長度。 -2- 關(guān)鍵詞 ： 多級坐標(biāo)變換 逆運動學(xué)解法 優(yōu)化 指令檢驗 一、問題重述 1.1 問題背景 某型號機器人（圖示和簡化圖略）一共有 6 個自由度，分別由六個旋轉(zhuǎn)軸（關(guān)節(jié)） 實現(xiàn)，使機器人的末端可以靈活地在三維空間中運動。機器人關(guān)于

3、六個自由度的每一 個組合心-（九，2宀3宀4門5門6）,表示機械臂的一個姿態(tài)，顯然每個姿態(tài)確定頂端指尖 的空間位置 X： f（0） X。假定機器人控制系統(tǒng)只能夠接收改變各個關(guān)節(jié)的姿態(tài)的 關(guān)于連桿角度的增量指令（機器指令） P - 2 -（.鮎厶如厶如厶如厶丸），使得 指尖（指尖具有夾工具、焊接、擰螺絲等多種功能，不過在這里不要求考慮這方面的 控制細節(jié)）移動到空間點 X，其中各個增量厶可只能取到-2, -1.9, -1.8, ,1.8, 1.9, 2這41個離散值 （即精度為0.1 ， 絕對值不超過2） 。 通過一系列的指令序 列R,P2,P3,，Pn可以將指尖依次到達位置 Xo, X1，, X

4、.，則稱 Xo, X1，Xn為從 指尖初始位置Xo到達目標(biāo)位置X的一條路徑（運動軌跡）。根據(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和一個可達目標(biāo)點

5、的空間位置（Q, Q, Q）,計算指尖到 達目標(biāo)點的指令序列。 （2） 要求指尖沿著預(yù)先指定的一條空間曲線 x = x（s）, y = y（s）, z = z（s）, a 三s 三 b 移動，計算滿足要求的指令序列。 （3） 在第個問題中，假設(shè)在初始位置與目標(biāo)位置之間的區(qū)域中有若干個已知大 小、形狀、方向和位置的障礙物，要求機械臂在運動中始終不能與障礙物相碰，否則 會損壞機器。這個問題稱機械臂避碰問題，要求機械臂末端在誤差范圍內(nèi)到達目標(biāo)點 并且整個-3- 機械臂不碰到障礙物（機械臂連桿的粗細自己設(shè)定） 。 問題二：應(yīng)用你的算法就下面具體的數(shù)據(jù)給出計算結(jié)果，并將計算結(jié)果以三組六維的 指令序列(每

6、行 6 個數(shù)據(jù))形式存放在 Excel 文件里，文件名定為 answer1.xls , answer2.xls 和 answer3.xls。 假設(shè)在機械臂的旁邊有一個待加工的中空圓臺形工件， 上部開口。工件高 180mm 下底外半徑 168mm 上底外半徑 96mm 壁厚 8mm 豎立地固定在 x y 平面的操作臺上， 底部的中心在(210, 0, 0)。 要求機械臂(指尖)從初始位置移動到工具箱所在位置的 (20, 200, 120) 處，以夾取要用的工具。 如果圓臺形工件外表面與平面 x =2z 的交線是一條裂紋需要焊接，請你給出 機械臂指尖繞這條曲線一周的指令序列。 有一項任務(wù)是在工件內(nèi)

7、壁點焊四個小零件，它們在內(nèi)表面上的位置到 x-y 平 面的投影為(320,-104 )、(120,106 )、( 190,-125 )和(255,88 )。要求機械臂從圓臺 的上部開口處伸進去到達這些點進行加工，為簡捷起見，不妨不計焊條等的長度，只 考慮指尖的軌跡 問題三：制造廠家希望通過修改各條連桿的相對長度以及各關(guān)節(jié)最大旋轉(zhuǎn)角度等 設(shè)計參數(shù)提高機械臂的靈活性和適用范圍。請根據(jù)你們的計算模型給他們提供合理的 建議。 二、問題分析 機械臂運動路徑設(shè)計問題主要涉及到相對坐標(biāo)系坐標(biāo)變換、機器人正運動學(xué)分 析、逆運動學(xué)求解、優(yōu)化以及機器人避碰問題。 1. 運動方程的建立 從機構(gòu)學(xué)觀點來看，機器人屬于

8、空間機構(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)軌跡離散的過程。如果這些離散點間距很大

9、，機 械臂軌跡與要求的軌跡就有較大誤差。只有這些離散點彼此很接近，才有可能使機械 臂的軌跡以滿足要求精確度逼近要求的軌跡。 連續(xù)軌跡的控制實際上就是在多次執(zhí)行 離散點間的點位控制，離散點點越密集越能逼近要求的運動曲線 3. 避碰問題 假設(shè)空間障礙物為半徑為 r 的球體，則這些球體空間便形成了機械手臂的約束。 而躲避問題就轉(zhuǎn)化成保證機械手臂上的點與障礙球球心距離始終大于 r 的問題。根據(jù) 運動規(guī)律，不難知道手臂相對于初始位置的姿態(tài)決定于之前執(zhí)行的所有指令的和。已 知連-4- 桿上的點 L 在其所在的相對坐標(biāo)系中的坐標(biāo)(x,y,zj 及轉(zhuǎn)動的指令，根據(jù)齊次坐-5- 標(biāo)變換矩陣就可得到 L 在固定坐

10、標(biāo)系中的坐標(biāo) x,y,z ,然后可計算距離。所以 可以 用問題1-1 算法產(chǎn)生點到點的指令，可以利用迭代法從初始位置開始提前檢驗每個指 令，不滿足要求的無法執(zhí)行。 4自由度分析： 由題中指出的：指尖 - E 點，具有夾工具、焊接、擰螺絲等多種功能，不過 在這里不要求考慮這方面的控制細節(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è)機器人的

11、初始位置是在一個平面上的（y-z 平面）。 6. 關(guān)節(jié)視為質(zhì)點，它們所占的面積可忽略不計。 四、符號說明 U i :坐標(biāo)系，u o為固定坐標(biāo)系，其余為固定在桿上的相對坐標(biāo)系 i : Ui到Ujj的坐標(biāo)系變換矩陣 I x :尖端移動終點在固定坐標(biāo)系中的 x 方向坐標(biāo) I y :尖端移動終點在固定坐標(biāo)系中的 y 方向坐標(biāo) -6- x 0 :尖端轉(zhuǎn)動起點在固定坐標(biāo)系中初始 x 方向坐標(biāo) yo :尖端轉(zhuǎn)動起點在固定坐標(biāo)系中初始 y 方向坐標(biāo) Zo :尖端轉(zhuǎn)動起點在固定坐標(biāo)系中初始 z 方向坐標(biāo) -第 i 個自由度的相對于初始姿態(tài)轉(zhuǎn)角(i=1,2,3,4,5,6) 宀-第 i 個自由度轉(zhuǎn)角的增量(i=1

12、,2,3,4,5,6) si -sin r ci - cos R 五、模型建立與求解 5.1 運動學(xué)模型建立與求解 5.1.1 機器人運動方程的建立 為了產(chǎn)生點到點的運動序列，首先我們先作出初始角度均為 0 時的姿態(tài)，以 每個節(jié)點為原點建立坐標(biāo)系Uo到U6如圖 5.1.1 oz :尖端移動終點在固定坐標(biāo)系中的 z 方向坐標(biāo) -7- 圖-5.1.1 從圖-5.1.2 可以看出 F 點相當(dāng)于將UA繞 Z 軸旋轉(zhuǎn)得到0T ,同理其他節(jié)點分別相當(dāng)于 將前一坐標(biāo)系繞 X,X,Z,X,Z 軸轉(zhuǎn)動?？傻玫礁髯鴺?biāo)系相對于前一個的坐標(biāo)系的坐標(biāo)變 換公式。從而得到機械臂各個關(guān)節(jié)的變換矩陣 ri1 ri2 ri3

13、I 旋轉(zhuǎn)矩陣：AR= % % 丿31 r32 r33 _-8- AR i _ 0 圖-5.1.2 :機器人連桿參數(shù)（見表格 1） Px! Py 位置矢量: -Pz cos（ ） -si n（q） 0 0 j sin（q） cos（q） 0 0 ；T 0 0 0 1 14-0 0 0 1 _ 衛(wèi) 0 sin（） _C0S（V2） 0 0 0 COS但 2） 0 sin 但 2） 0 0 1 -9- .0 0 0 px 65C1C23C4S5 65S| S4 S5 65C1S23C5 255C1S23 255C1C2 p 65S23C4S -65C23C255C23 255S2 2確定固定坐標(biāo)系中

14、 E 點位置1 0 0 0 1 _cos4； -s in （日 ）0 0 1 0 cos（日3） -s in （日 3）0 0 sin （04） 0cos（ e4） 0 0 0 sin （夷）。 COS（83） 1 255 0 0 1 255 衛(wèi) 0 0 1 0 0 0 1 1 1 0 0 0 -cos但 6 ） -si nd） 0 0 0 sin （85） cos（日5） 0 4 0si n（B6） 0cos（日6） 0 0 0 -cos） sin （日0 0 0 1 0 0 0 1 0 0 0 1 nx Ox ax ny nz Oy Oz ay az Px Py Pz Py =65S1C2

15、3C4S5 _65C1S4S5 65S|S23C5 2550S23 255$C2 其中 nx =C1C23C4C5C - C1C23S4S6 S1S4C5C6 S1C4S _ C1S23S5S6 ny -SIC23C4C5C _ S1C23S4S6 _ C1S4C5C _ C1C4S6 _ S1S23S5S6 nz -S23C4C5C _ S23S4S3 C23S5S6 Ox =_C1C23C4C5S6 _ C1C23S4C _ S1S4C5 S3 S1C4C6 C1S23S5S3 Oy = -S1C23C4C5S6 -$1。23&56 C1S4C5S6 - C1C4C6 S1S23S

16、5S3 Oz = _S23C4C5S6 _ S23S4C6 _C23S5S3 ax =C1C23C4S5 S1S4S5 C1 SCay -S1C23C4 S5 1C1S4S5 * S1S23C5 az -S23C4S5 _C23C5 -10- 由題設(shè)中，已知固定坐標(biāo)系原點，根據(jù)給定的連桿長度和 M 角度，易計算得出, 在固定坐標(biāo)系中 E 位置： &=_65(-cos(s1)si n( s4)+(-si n(s1)cos(s2)cos(s3)+si n( s1)si n( s2)si n(s3)cos(s 4)si n( s5)+65(si n(s1)cos(s2)si n(s3)+si

17、 n( s1)si n(s2)cos(s3)cos(s5)+255s in (s1)cos(s2)si n( s3)+255si n( s1)si n(s2)cos(s3)+255si n( s1)si n( s2) E/=-65(-si n( s1)si n(s4)+(cos(s1)cos(s2)cos(s3)-cos(s1)si n( s2)si n( s3)cos(s4 )si n(s5)+65(-cos(s1)cos(s2)si n(s3)-cos(s1)si n(s2)cos(s3)cos(s5)-255 cos(s1)cos(s2)*si n(s3)-255cos(s1)si n(

18、 s2)cos(s3)-255cos(s1)si n(s2) Ez=140-65(si n( s2)cos(s3)+cos(s2)si n(s3)cos(s4)s in (s5)+65(-si n( s2)s in( s3)+cos(s2)cos(s3)cos(s5)-255si n( s2)si n( s3)+255cos(s2)cos(s3)+255cos (s2) 這樣便得到了運動學(xué)方程。 5.1.2 利用逆運動學(xué)方法求解 （1）求 韋 可用逆變換A（K）左乘運動方程式兩邊得: Al佝）6 =A2（日 2）Aa（日 3）幾（日4於5（%爪6但6） 根據(jù)對性元素相等可解的 解得: K =a

19、rctan卩廠鴉或*arctan Py噸 Px _6 5Px - 65ax (2)求 6 由： - nx Ox ax Px A/（ej A2 （82）A， （q） ny Oy ay Py =A4但4）人但5）人（日6） nz Oz az Pz 0 0 0 1 解得: 1 =一 arcs in a arcta n 6 二3 = arcsin a arcta n1 戶 3 6 (3) 求二 2 c23(c1ax S1ay) s23az = c4s5 C23(C1Px $Py) sPz -(255 03)=65c4s 解得: si n23 w) = B = 咕 二 arcs in B - w; 或

20、23 二二 - arcsin B - w ci +$Py -65(鬼 +阿) tanw =- 其中 Pz -65az B =(255Q)十 J(q Px sy 65qax 65sy)2 +(Pz paz)2 (1) (2) -11- R - % -乜 二 arcs in B - w - J3； 或- 2 =二-arcs in B - w - J3 (4) 求屯 S23(Gax say) C23az = C5 = k =arccoss23(ciax say)C23az 或 k =2二-arccosS23(qax sy)-辭乙 (5) 求二 4 由sax -ciay =S4S，可以解得： Sax

21、-Gay a . SaxGRy 卡 r . sia - ciay s4 4 =arcs in ;或二 4 - - arcs in s5 s5 S5 (6) 求二 6 / 丄 . 口 s23(qOx +0y)C23z S23(GOx SOy) -Xz 二 sin 出： sin 日 5 a . s23(qx +$Oy)qOz 十 a . (qOx + $Oy) 一 Oz r6 二 arcs in ；或 二6 -二-arcs in sin屯 sin也 5.2 問題 1 1 的模型 在已有六自由度手臂運動方程和逆運動學(xué)解法的基礎(chǔ)上， 若已知機械臂末端轉(zhuǎn) 動終點坐標(biāo)和轉(zhuǎn)動起點坐標(biāo)已知，就可以得到各關(guān)節(jié)的

22、坐標(biāo)，連桿的相對于初始狀態(tài) 的角度變化門.廣行,3,忑九忑對及機械臂的姿態(tài) 5.2.1 機械臂最佳姿態(tài)的確定 對于一般六自由度機械臂，帶入末端坐標(biāo)（x,y,z）會得到 16 組 總忌斥），考慮用機械臂指尖實際到達的空間位置（ x , y , z ）到理想位 置（x,y,z ）的偏差距離與移動距離的比值最少來衡量機械臂是否是最佳姿態(tài)，所以 我們通過定義一個參量-偏移系數(shù)來尋找最佳指令， 其數(shù)學(xué)模型可表示為： (x-x)2 (y-y)2 (z-z)2 ,(x-滄)2 (y- y。)2 (z- %)2 x :尖端移動終點在固定坐標(biāo)系中的 x 方向坐標(biāo) I y :尖端移動終點在固定坐標(biāo)系中的 y 方向

23、坐標(biāo) I z :尖端移動終點在固定坐標(biāo)系中的 z 方向坐標(biāo) X。：尖端轉(zhuǎn)動起點在固定坐標(biāo)系中初始 x 方向坐標(biāo) yo :尖端轉(zhuǎn)動起點在固定坐標(biāo)系中初始 y 方向坐標(biāo) Min M -12- z。：尖端轉(zhuǎn)動起點在固定坐標(biāo)系中初始 z 方向坐標(biāo) 當(dāng)尖端移動終點坐標(biāo)(x, y, z)已知情況下，利用 MATLAB Robotics Tool 可 得到 T矩陣，利用逆運動學(xué)解法，可得到因指令中各個增量心i只能 取到-2, -1.9, -1.8, , 1.8, 1.9, 2 這 41 個離散值(即精度為 0.1 ，絕對值不 超過 2 )，所以最終轉(zhuǎn)動的 an總九話氏均只保留小數(shù)點后一位。所以對得到的 (q

24、屯息,Q,Q)取位處理，然后回代入正方向運動學(xué)方程，解得的(x , y , z)才為 實際位置。通過對M的比較得到最佳 龍衛(wèi)衛(wèi)宀衛(wèi))*。 5.2.2 指令生成 在已知 K (i=1,2,3,4,5 ，6)后，只需要設(shè)計一種方式使機械臂從 于轉(zhuǎn)到于。 這里根據(jù)點位機械臂運動特點選用加速一一勻速一一減速三段式控制方式。 如圖 5-2 中的方式二。 薩(速度) 方式1 規(guī)定一個加速時間 T(T=theta12-theta(i-1,1) theta(i,1) =theta12; else theta(i,1) =theta11; end %theta3 theta31=asin(1)+atan(1/6

25、); 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)A 2+(pz-p*azF2); theta2

26、1=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 %theta5 theta51=acos(s231(i,j)*(c1(i,j)*ax+s1(i,j)*ay)-c231(i,j)*az); -19- theta52=2*pi-acos(s232(i,j)*(c1(i,j)*ax+s1(i,j)*ay)-c232(i,j)*az); if t

27、heta51-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 %thet

28、a6 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-1 T06=transl(20,-200,120); nx=T06(1,1);ny=T06(2,1

29、);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-thet

30、a(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(th

31、eta(i,3); -20- 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)*ayF2+(pz-1 *az)A2); 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,2) = theta21; end-21- %theta5 theta51=acos(s23(i)*(c1(i)*ax+s1(i)*ay)-c23(i)*az); theta52=2*pi-acos(s23(i)*(c1(i)*ax+s1(i)*ay)-c23(i)*az); if theta5