1、-1- 機械臂運動路徑設計分析 摘要 本文探討了六白由度機械臂從一點到另一點沿任意軌跡移動路徑、 - 點到另一點沿著給定軌跡移動路徑、 以及無碰撞路徑規劃問題，并討論了 設計參數對機械臂靈活性和使用范圍的影響，同時給出了建議。 問題一 ：(1)首先確定初始坐標均為零時機械臂姿態， 建立多級坐標 系，利用空間解析幾何的變換基本原理及相對坐標系的齊次坐標變換的矩 陣解析方法，來建立機器人的運動系統的多級變換方程。 通過逆運動學解 法和構建規劃，來求優化指令 (2) 假定機械臂初始姿態為 0)0,曲線離散化，每個離散點作為末端 位置，通過得到的相鄰兩點的姿態，利用(1)中算法計算所有相鄰兩點 間的增

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

3、一 個組合0 =（訪,宛凡3凡4,缶,兔），表示機械臂的一個姿態，顯然每個姿態確定頂端指尖 的空間位置X： f（&）T X。假定機器人控制系統只能夠接收改變各個關節的姿態的 關丁連桿角度的增量指令（機器指令） P =（%, A82,A83，A84,缶,A%）,使得 指尖（指尖具有夾工具、焊接、擰螺絲等多種功能，不過在這里不要求考慮這方面的 控制細節）移動到空間點X,其中各個增量Aq只能取到-2, -1.9, -1.8,，1.8, 1.9, 2這41個離散值（即精度為0.1 ，絕對值不超過2 ）。通過一系列的指令序 列P,P2,P3,，Pn可以將指尖依次到達位置 X0, X,，X,則稱X

4、o, X1,，入為從 指尖初始位置Xo到達目標位置X的一條路徑（運動軌跡）。根據具體的目標和約束條 件計算出合理、便捷、有效的指令序列是機器人控制中的一個重要問題。假設機器人 的初始位置在y-z平面上，約定直角坐標系的原點設在圖示的 A點，z軸取為AB方向， x軸垂直紙面而y軸則在基座所固定的水平臺面上 1.2目標任務 問題一：設計一個通用的算法，用來計算執行下面指定動作所要求的指令序列， 并要求對算法的適用范圍、計算效率以及近似算法所造成的誤差和增量 離散取值 所造成的誤差大小進行討論（不考慮其他原因造成的誤差）： （1） 已知初始姿態 。和一個可達目標點的空間位置（Q, Q, O）,計算指

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

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

7、投影為(320,-104 )、(120,106 )、(190,-125 )和(255,88)。要求機械臂從圓臺 的上部開口處伸進去到達這些點進行加工，為簡捷起見，不妨不計焊條等的長度，只 考慮指尖的軌跡 問題三：制造廠家希望通過修改各條連桿的相對長度以及各關節最大旋轉角度等 設計參數提高機械臂的靈活性和適用范圍。請根據你們的計算模型給他們提供合理的 建議。 機械臂運動路徑設計問題主要涉及到相對坐標系坐標變換、機器人正運動學分 析、逆運動學求解、優化以及機器人避碰問題。 1. 運動方程的建立 從機構學觀點來看，機器人屆丁空間機構范疇。桿件每次轉動因此，采用空間解 析幾何的變換基本原理及坐標變換的

8、矩陣解析方法， 來建立機器人的運動系統的多級 變換方程。 由丁旋轉軸涉及到平行連桿和垂直連桿兩類，因此對丁各類旋轉變換，所使用的 變換矩陣也不相同。 同時，此題中涉及的機器人有 6個自由度，則從工件的坐標位置到固定坐標系的 變換要經過多級坐標變換。采用多級坐標變換的方法。 由上述三點，便可建立機器人運動系統的坐標變換關系式。 2. 尖端軌跡曲線模型的建立 對丁已給定一條空間曲線x=(s),y=(y),z=(z), 可將其看成一個點的集合。因此機 械臂實現一個空間軌跡的過程是實現軌跡離散的過程。如果這些離散點間距很大，機 械臂軌跡與要求的軌跡就有較大誤差。只有這些離散點彼此很接近，才有可能使機械

9、 臂的軌跡以滿足要求精確度逼近要求的軌跡。 連續軌跡的控制實際上就是在多次執行 離散點間的點位控制，離散點點越密集越能逼近要求的運動曲線 3. 避碰問題 假設空間障礙物為半徑為r的球體，則這些球體空間便形成了機械手臂的約束。 而躲避問題就轉化成保證機械手臂上的點與障礙球球心距離始終大丁 r的問題。根據 運動規律，不難知道手臂相對丁初始位置的姿態決定丁之前執行的所有指令的和。已 - 知連桿上的點L在其所在的相對坐標系中的坐標(x, y, z)及轉動的指令，根據齊次坐 標變換矩陣就可得到L在固定坐標系中的坐標（x,y,z）,然后可計算距離。所以 可以 用問題1-1算法產生點到點的指令，可以利用迭代

10、法從初始位置開始提前檢驗每個指 令，不滿足要求的問題分析 -4- 無法執行。 4.自由度分析： 由題中指出的：指尖 -E點，具有夾工具、焊接、擰螺絲等多種功能，不過 在這里不要求考慮這方面的控制細節。則將情況理想化，不考慮第 6個自由度對運動 的影響。 三、模型的假設 1. 各關節連桿在輸入指令后同時開始轉動，速度為低速，各桿件之間無摩擦, 臂各旋轉軸最大運動速度相同 2. 在軌跡規劃中不考慮機械臂關節轉角的限制。 3. 不考慮機械臂結構和裝配產生的誤差。 4. 連桿為剛體，不會發生形變 5. 設機器人的初始位置是在一個平面上的（y-z平面）。 6. 關節視為質點，它們所占的面積可忽略不計。

11、四、符號說明 Ui：坐標系，Uo為固定坐標系，其余為固定在桿上的相對坐標系 i-*T : Ui到U-的坐標系變換矩陣 X :尖端移動終點在固定坐標系中的x方向坐標 y :尖端移動終點在固定坐標系中的y萬向坐標 z :尖端移動終點在固定坐標系中的 z方向坐標 xo :尖端轉動起點在固定坐標系中初始 x方向坐標 y0 ：尖端轉動起點在固定坐標系中初始 y方向坐標 % :尖端轉動起點在固定坐標系中初始 z方向坐標 -5- 饑-第i個自由度的相對丁初始姿態轉角(i=1,2,3,4,5,6) 睥-第i個自由度轉角的增量(i=1,2,3,4,5,6) si - sin 刁 ci - cos a 五、模型建

12、立與求解 5.1運動學模型建立與求解 5.1.1機器人運動方程的建立 為了產生點到點的運動序列,首先我們先作出初始角度均為0時的姿態，以 每個節點為原點建立坐標系Uo到化如圖5.1.1。-6- 圖-5.1.1 從圖-5.1.2可以看出F點相當丁將UA繞Z軸旋轉得到0T,同理其他節點分別相當于 將前一坐標系繞X,X,Z,X,Z軸轉動。可得到各坐標系相對丁前一個的坐標系的坐標變 換公式。從而得到機械臂各個關節的變換矩陣 ri1 ri2 ri3 旋轉矩陣：AR= &1如歸 31 r32 r33 _E D G C B F A -7- 位置矢量: px! |Py ：.Pz i* AR T = Z

13、t yA yA Z B X :機器人連桿參數 （見表格1） 0T = cos仲) sin(1) 0 _0 -sinG) cosG) 0 0 0 0 1 0 0 1 0 140 1 2T = 一1 0 I。 _0 圖-5.1.2 0 sinC1?) -cos2) 0 0 cos sin。?) 0 01 0 0 1 -8- 0 cos3) sin(03)0 0 3T - A 1 sin(04) 0cos(84) 0 0 0 sin(63)0 cos(q)1 255 0 0 1 255 p 0 0 1 0 0 0 1 1 0 0 0 -cos&) -sin傀)0 0 0 sin(缶) cos

14、(缶)0 4T: 0sin(86) 0cos(e6) 0 0 0 -cos但)sin(e5) 0 0 0 1 0 0 0 0 1 _ - 0 0 0 1- 1 0 0 -sin(u4) 0 3T = 5T = 0 0 COS(u4) T6 =凡（叫治2（口2）&（口3）丹（。4治5（口5）偶（口6）= n x ax Px 1 ny oy ay Py nz z az Pz 0 0 1 - 其中 nx = GC23C4C5C6 - CQ23S4& S&C5C6 SCqSg -。1,23,5,6 ny = C23C4C5C6 - C23S4S6 -C1S4C5C6 C1C4S

15、 S1S23S5S6 4 =，23。4。5。6 C23SA Ox = -CIC23C4C5& - CIC23S4C SIS4C5S5 SC4C6 C1S23S5S6 Oy = -SICZBCICSSG - SC23S4& C1S4C5S3 - C1C4C6 S1S23S5& Oz = -S23C4C5S6 - &3S4C6 - C23S5 S3 ax =GC23C4S5 SIS4S5 CIS23C5 ay - S1C23C4S5 -CIS4S5 SIS23C5 az = S23C4S5 - C23C5 Px = 65CQ23C4S5 65SS4S5 65C1S2

16、3C5 255C1S23 255C1C2 Pz -65S23C4S5 Y5C23C5 -25523 255S2 Py =65S1C23C4S5 -65C1S4S5 65S1S23C5 255S1S23 255S1C2 2.確定固定坐標系中E點位置 -9- 由題設中，已知固定坐標系原點，根據給定的連桿長度和 們角度，易計算得出, 在固定坐標系中E位置： K=-65(-cos(s1)sin(s4)+(-sin(s1)cos(s2)cos(s3)+sin(s1)sin(s2)sin(s3)cos(s 4)sin(s5)+65(sin(s1)cos(s2)sin(s3)+sin(s1)sin(s2)c

17、os(s3)cos(s5)+255s in(s1)cos(s2)sin(s3)+255sin(s1)sin(s2)cos(s3)+255sin(s1)sin(s2) E/=-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)-255 cos(s1)cos(s2)*sin(s3)-255cos(s1)sin(s2)cos(s3)-255cos(s1)sin(s2) Ez=140-65

18、(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) 這樣便得到了運動學方程。 5.1.2利用逆運動學方法求解 （1）求 01 可用逆變換AI ）左乘運動方程式兩邊得： 凡”（叫）十6 =入2（口2）任。3）丹（口4）位（口5治6。6） 根據對性元素相等可解的 解得： Py _ 65ay q = arctan - 或訪 Px - 65a （2）求 03 由： 解得: Py _ 65

19、ay arctan - 我-65ax nx ny nz -0 ox oy oz 0 ax ay az 0 Px Py Pz 1 = A4(3)A5 05)A(%) 1 . 3 = arcsin a arctan 一廣3 6 1 =二一 arcsin a arctan - 6 (3)求 c23(c1ax &ay) &az = c4s5 c23(GPx Py) &Pz-(255%) =65c4& (1) (2) 解得: sin(623 + w) = B= 623 =arcsinB w;或日23 =冗一arcsinB w 七嚀仃林6) 其中 Pz -65az J B

20、=(255cJ- (qpx s1Py-65qax-65ay)2 (Pz-paz)2 -10- e2 =e23 e3 =arcsinB w 務;或 e2 =兀arcsinB we3 或)5 =2 虹 一 arccoss23(c1ax say) q3az (5)求 04 由ax -Gay =S4S ,可以解得: sax-Gay . . sax-Gay . s4 =- n S =arcsin - ;或 & =兀-arcsin - s5 ss ss (6)求 e6 s23(ciOx SOy) -c23Oz sin 5 5.2問題1 1的模型 在已有六自由度手臂運動方程和逆運動學解法的基礎上，若

21、已知機械臂末端轉 動終點坐標和轉動起點坐標已知，就可以得到各關節的坐標，連桿的相對丁初始狀態 的角度變化中A=（q，4，q,e4，e5,4）及機械臂的姿態 5.2.1機械臂最佳姿態的確定 對丁一般六自由度機械臂，帶入末端坐標（x,y,z ）會得到16組 A=（qe,q,4,息,4），考慮用機械臂指尖實際到達的空間位置（ x , y , z ）到理想位 置（x,y,z ）的偏差距離與移動距離的比值最少來衡量機械臂是否是最佳姿態，所以 我們通過定義一個參量-偏移系數來尋找最佳指令， 其數學模型可表示為： ,：(x-x)2 (y-y)2 (z-z)2 .(x-xo)2 (y-yo)2 (z-zb)2

22、 x :尖端移動終點在固定坐標系中的 x方向坐標 . y :尖端移動終點在固定坐標系中的y萬向坐標 z :尖端移動終點在固定坐標系中的 z方向坐標 xo ：尖端轉動起點在固定坐標系中初始 x方向坐標 Vo :尖端轉動起點在固定坐標系中初始 y方向坐標 % :尖端轉動起點在固定坐標系中初始 z方向坐標 (4)求 e5 S23(cax ay) %3az =G= * =arccoss23(c1ax 私)-c23az s23(GOx 0y) gOz =技= sinu6 arcsinGOx 一海 或寸-arcsinWMx 一蒙 sin % Min M = -11- 當尖端移動終點坐標（x, y, z）已

23、知情況下，利用 MATLAB Robotics Tool可 得到T矩陣，利用逆運動學解法，可得到（&,成&,&,&R），因指令中各個增量AS只能 取到-2, -1.9, -1.8, ，1.8, 1.9, 2 這41個離散值（即精度為0.1 ，絕對值不 超過2 ）,所以最終轉動的 0,&島由禺是均只保留小數點后一位。所以對得到的 （q凡40用,）取位處理，然后回代入正方向運動學方程，解得的（ x , y , z）才為 實際位置。通過對M的比較得到最佳（。同恩忠尺息）*。 5.2.2 指令生成 在已知。（i=1,2,3,4,5 , 6）后，只需要設計一種方式

24、使機械臂從 61轉到020 這里根據點位機械臂運動特點選用加速一一勻速一一減速三段式控制方式。 如圖5-2 中的方式二。 （速度） 規定一個加速時間T（T=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) =

25、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)A2); 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) = the

26、ta22; else theta(i,2) = theta21; end %theta5 theta51=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); -18- 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,

27、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)

28、-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);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=T0

29、6(3,4); n=nx ny nz; o=ox oy oz; a=ax ay az;%theta5 -19- 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)+a

30、tan(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); %theta

31、2 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,2) = theta21; end-20- 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 theta51-theta(i-