機械臂運動路徑設(shè)計問題

1、研究生試卷2016年 2017 年度第 一 學(xué)期 課 程 名 稱：機器人學(xué)導(dǎo)論 評分： 專 業(yè)： 機械工程 年級：2016級 研 究 生 姓 名: 學(xué)號： 任課教師姓名： 注意事項 1答題必須寫清題號； 2字跡要清楚，保持卷面清潔； 3試題隨試卷交回； 4考試課按百分制評分，考查課按5級分制評分； 5閱完卷后，一周內(nèi)將試卷、試題、成績單由任課教師簽名后， 送有關(guān)部門。機械臂運動路徑設(shè)計問題自1959年美國的英格伯格和德沃爾制造出世界上第一臺工業(yè)機器人“尤尼梅特”開始，近半個世紀以來，機器人的研制和應(yīng)用以驚人的速度發(fā)展并取得長足的進步。當今世界，機器人的應(yīng)用領(lǐng)域已十分廣泛，包括工業(yè)生產(chǎn)、?？仗剿?/p>

2、、醫(yī)療康復(fù)和軍事活動等，此外，機器人已逐漸在醫(yī)院、家庭和一些服務(wù)行業(yè)獲得應(yīng)用。從生產(chǎn)車間中的焊接機械手，到水下自治式機器人，從娛樂性的拳擊機器人，到伊拉克戰(zhàn)場上的無人駕駛機，機器人已經(jīng)與我們的日常生活息息相關(guān)。 機器人通常分為關(guān)節(jié)式機器人（或稱機械臂、機械手、機器人操作臂、工業(yè)機器人等）和移動式機器人。一般來說，前者具有更多的自由度，而后者的作業(yè)范圍則更大一些。以某型號機器人為例，其示意圖見圖1：第一個自由度 180度第二個自由度 125度第三個自由度 138度第四個自由度 270度第五個自由度-133.5 120度第六個自由度 270度圖1 機器人結(jié)構(gòu)圖這種機器人一共有6個自由度，分別由六個

3、旋轉(zhuǎn)軸（關(guān)節(jié)）實現(xiàn)，使機器人的末端可以靈活地在三維空間中運動。為了便于分析和計算，我們對機器人結(jié)構(gòu)進行簡化，簡化后的數(shù)據(jù)見圖2和參數(shù)表1。這里用七條直線段表示機器人的七個連桿，連桿之間用所謂的旋轉(zhuǎn)關(guān)節(jié)連接，已知AB140mm，BC255mm，CD255mm，DE65mm。根據(jù)旋轉(zhuǎn)的方向分成兩類關(guān)節(jié)，旋轉(zhuǎn)軸分平行連桿的（如圖1的自由度一、四、六，對應(yīng)于圖2中的F，G，H）和垂直連桿的（如圖1的自由度二、三、五，對應(yīng)于圖2中的B，C，D）兩種，前者如筆帽的轉(zhuǎn)動方向，后者如搖柄的轉(zhuǎn)動方向。每一個關(guān)節(jié)對應(yīng)一個角度，這個角度表示前一個連桿方向到后一個連桿方向轉(zhuǎn)角（對于B，C，D），連桿方向為AB、BC、

4、CD、DE，或者相對于初始位置的轉(zhuǎn)角（對于F，G，H），假設(shè)機器人的初始位置是在一個平面上的（y-z 平面）。為了使機器人運動得更加靈活允許關(guān)節(jié)的轉(zhuǎn)角超過360的。 圖2：機器人的尺寸圖 機器人關(guān)于六個自由度的每一個組合，表示機械臂的一個姿態(tài)，顯然每個姿態(tài)確定頂端指尖的空間位置X：。假定機器人控制系統(tǒng)只能夠接收改變各個關(guān)節(jié)的姿態(tài)的關(guān)于連桿角度的增量指令（機器指令），使得指尖（指尖圖2中的E點，具有夾工具、焊接、擰螺絲等多種功能，不過在這里不要求考慮這方面的控制細節(jié)）移動到空間點X，其中各個增量只能取到-2, -1.9, -1.8, ，1.8, 1.9, 2這41個離散值（即精度為0.1，絕對值

5、不超過2）。通過一系列的指令序列可以將指尖依次到達位置X0，X1，Xn，則稱X0，X1，Xn為從指尖初始位置X0到達目標位置Xn的一條路徑（運動軌跡）。根據(jù)具體的目標和約束條件計算出合理、便捷、有效的指令序列是機器人控制中的一個重要問題。我們約定直角坐標系的原點設(shè)在圖2的A點，z軸取為AB方向，x軸垂直紙面而y軸則在基座所固定的水平臺面上。1根據(jù)市場需求，機械臂制造廠打算為他們的產(chǎn)品研發(fā)一個軟件系統(tǒng)，能夠直接將用戶的運動命令自動轉(zhuǎn)換成機器指令序列。即為這類機器人設(shè)計一個通用的算法，用來計算執(zhí)行下面指定動作所要求的指令序列，并要求對你們算法的適用范圍、計算效率以及你們的近似算法所造成的誤差和增量

6、離散取值所造成的誤差大小進行討論（不考慮其他原因造成的誤差）：已知初始姿態(tài)0和一個可達目標點的空間位置（Ox, Oy, Oz），計算指尖到達目標點的指令序列。要求指尖沿著預(yù)先指定的一條空間曲線x = x(s), y = y(s), z = z(s), a s b 移動，計算滿足要求的指令序列。在第個問題中，假設(shè)在初始位置與目標位置之間的區(qū)域中有若干個已知大小、形狀、方向和位置的障礙物，要求機械臂在運動中始終不能與障礙物相碰，否則會損壞機器。這個問題稱機械臂避碰問題，要求機械臂末端在誤差范圍內(nèi)到達目標點并且整個機械臂不碰到障礙物（機械臂連桿的粗細自己設(shè)定）。2. 應(yīng)用你的算法就下面具體的數(shù)據(jù)給出

7、計算結(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 = 2 z 的交線是一條裂紋需要焊接，請你給出機械臂指尖繞這條曲線一周的指令序列。有一項任務(wù)是在工

8、件內(nèi)壁點焊四個小零件，它們在內(nèi)表面上的位置到xy平面的投影為（320,-104）、（120,106）、（190,-125）和（255,88）。要求機械臂從圓臺的上部開口處伸進去到達這些點進行加工，為簡捷起見，不妨不計焊條等的長度，只考慮指尖的軌跡。3制造廠家希望通過修改各條連桿的相對長度以及各關(guān)節(jié)最大旋轉(zhuǎn)角度等設(shè)計參數(shù)提高機械臂的靈活性和適用范圍。請根據(jù)你們的計算模型給他們提供合理的建議。關(guān)節(jié)變量符號初始位置變量范圍10 1802-90 12530 13840 2705-90-120 +133.560 270參數(shù)表1 初始姿態(tài)0和動作范圍機器人學(xué)導(dǎo)論-大作業(yè)答案12. 機械臂運動路徑設(shè)計問題1

9、2.1初始姿態(tài)0的確定關(guān)節(jié)轉(zhuǎn)角按照逆時針旋轉(zhuǎn)選擇為正，按順時針旋轉(zhuǎn)為負。約定機器人的初始位置是在一個平面上的（y-z 平面），直角坐標系的原點設(shè)在A點，z軸取為AB方向，x軸垂直紙面而y軸則在基座所固定的水平臺面上。初始位置是指的初始值,其中關(guān)節(jié)1,4,6的初始值就是使機器人的各個連桿在y-z平面,關(guān)節(jié)1的初始值就是使連桿BC等在y-z平面的角度,關(guān)節(jié)4初始值就是使機器人的連桿DE在連桿AB、BC、CD所生成的平面內(nèi)，從而可確定機械臂的初始姿態(tài)，如圖12.1所示。圖12.1 機械臂的初始姿態(tài)圖12.2指尖在無障礙的情形下移動到指定點,我們建立如下模型12.2.1模型一設(shè)初始點的位置為其姿態(tài)各個

10、角度是目標點的位置為, 設(shè)為由初始姿態(tài),經(jīng)過一些列指令變換后的各旋轉(zhuǎn)角的總增量,其對應(yīng)的位置為.由于連桿角度的增量指令是,而且只能取到-2,-1.9,-1.8,1.8,1.9,2 這 41個離散值(即精度為0.1, 絕對值不超過2),因此為了使機器指尖與目標點能夠盡可能地接近,我們有如下的目標函數(shù): 12.2.2問題1的算法:考慮到有效的控制角度有 5個而要完成從當前點移動到目標點,在精度要求滿足的情況下僅僅需要改變 3個角度就可達到目標點,所以本文提出的算法是首先固定和,令它們同時為0給出控制序列即可達到目標點.由于不考慮障礙物所以機械臂的轉(zhuǎn)動很自由為了使得兩點之間的運動時間最短操作指令最少

11、應(yīng)該使得所有關(guān)節(jié)同時轉(zhuǎn)動。這樣指令的長度就僅僅是中轉(zhuǎn)動角度最大的關(guān)節(jié)的轉(zhuǎn)動所對應(yīng)得操作序列長度。設(shè)的轉(zhuǎn)動角度對應(yīng)得指令序列長度是 L1 L2 L3 L4 L5 總的操作序列長度 L=max(L1、 L2、 L3、 L4、 L5)。設(shè)每個關(guān)節(jié)的目標角度與當前角度差為（i =1,2,3,4,5 ）因為角度的增量是(2, 1.9, 1.8, ,0,-0.1,-0.2 -1.8,-1.9,-2.0),所以如果, 首先應(yīng)該考慮取大的轉(zhuǎn)角,然后依次取較小的角度增量. 直至分解完畢.如果,則角度增量(負值)從小到大分解。詳細算法可通過MatLab編程實現(xiàn)。12.2.3問題的求解下圖為從初始點運動到給定點(2

12、0, -200,120)的各臂的運動軌跡:圖12.2上圖中加星粗線表示的是初始狀態(tài)粗線表示的是目標狀態(tài)，細線是控制指令下完成全過程的運動軌跡。12.3 指尖沿指定曲線運動12.3.1問題2的算法算法一要求指尖沿著預(yù)先指定的一條空間曲線迅速x= x(s ), y= y( s), z= z( s), asb移動。設(shè)曲線的端點為 P0和Pn，不妨規(guī)定P0為起始點，Pn為運動終點，從起始點的位姿出發(fā)，以=0 或 0.1 的微小變化逐步搜尋相鄰位姿，并從相鄰位姿所對應(yīng)的指尖實際位置中選取與給定曲線最接近的作為第 1 個近似插點記為 p1 ,同時記錄對應(yīng)指令為p1，然后再從p1出發(fā)搜尋合適的p2和P2,依

13、次類推直至指尖距離曲線終點位置在一定的精確度范圍內(nèi).這樣通過一系列的指令 p1、p2、p3、p n,，將指尖依次到達曲線上或鄰近位置，從而最終實現(xiàn)指尖沿著預(yù)先指定的空間曲線移動。算法二首先對曲線抽樣離散化這樣指尖沿曲線運動就可以轉(zhuǎn)化成點到點的運動通過分析和計算機模擬結(jié)果可知，由于控制角度的離散化，指尖在三維空間的運動間隔為 1mm 數(shù)量級可以說本系統(tǒng)指尖的分辨率是 1mm 數(shù)量級，所以曲線的抽樣間隔點空間距離應(yīng)該同樣為1mm 的數(shù)量級。本文的算法是首先用上文提出的點到點算法到達線段的端點，然后固定改變，指尖可以在一定的空間范圍內(nèi)沿著空間曲線運動，由于固定指尖的運動范圍比較有限，超出這個范圍后要

14、計時改變，每次在超出運動范圍之前對做一次大的調(diào)整反復(fù)的進行以上的步驟即可達到相當好的精確度沿著空間曲線運動12.3.2問題2的求解根據(jù)算法二，通過Matlab編程仿真得到給定曲線以及指尖實際運動軌跡的圖像可以看出精確度是很高的也就是說本算法得出的仿真結(jié)果誤差非常小。在計算出的一系列控制指令的作用下機械臂指尖實際運動的軌跡與給定曲線幾乎完全重合以致較難分辨，故同時給出局部放大圖圖12.3是三維視圖，圖12.4是x z面的放大圖。 圖12.3圖12.4給定曲線以及指尖運動軌跡放大圖經(jīng)過計算，給定曲線與指尖實際運動軌跡的誤差僅為 0.1223mm。12.4避障運動問題12.4.1避障問題的運動規(guī)劃對

15、于機械臂來說，運動避障問題可以描述為機械臂在障礙物環(huán)境中計算一條安全高效的路徑。首先說明一下，本文研究的方法是基于機械臂的位姿空間，即機械臂的關(guān)節(jié)空間的機械臂運動規(guī)劃問題的輸入為機械臂和障礙物的幾何要素比如機械臂的尺寸關(guān)節(jié)數(shù)量等，以及障礙物的形狀、大小等，機械臂運動規(guī)劃問題的輸出為連接初始和目標位姿的無碰的位姿序列。下面將就運動規(guī)劃問題中的幾個關(guān)鍵要素進行具體的闡述。對于靜態(tài)運動規(guī)劃障礙物的所有信息均已知運動規(guī)劃在此基礎(chǔ)上進行有限制的運動規(guī)劃如機器人機械臂的關(guān)節(jié)角度有一定的限制范圍。本文所研究的機械臂是有一定的自身限制的根據(jù)完備性來分類運動規(guī)劃算法有精確的和啟發(fā)式的兩種前者可以看作是用窮盡的方

16、法找出符合要求的路徑或者得出無法找到路徑的結(jié)論。通常是很耗費計算量的后者則著眼于怎樣快速的找到一條路經(jīng)，通常加了一些啟發(fā)性的因素，有可能會失敗，也有可能找到的路徑并不是最優(yōu)的工程上一般采用基于啟發(fā)式的算法而精確算法則決定了問題本身算法的復(fù)雜度。12.4.2位姿空間的計算方法對于機械臂來說，一般只能在位姿空間中來考慮規(guī)劃問題。所以，怎么有效地將物理空間轉(zhuǎn)化為位姿空間就很重要了。這里考慮用雅可比矩陣法計算位姿空間，該方法通過求得給定位姿點 q 的 Jacobi 行列式絕對值的一個上界，來得出 q 點的某個鄰域為自由空間區(qū)域或者為障礙物區(qū)域。模型的建立在有障礙物的情況下，我們假設(shè)組成障礙物的所有點的

17、集合為A。設(shè)指尖所在位置為( x1, y1, z1),其姿態(tài)各個角度是,在旋轉(zhuǎn)了任意小的角度之后，機械臂的位置以及姿態(tài)發(fā)生了變化，不妨設(shè)位置為( x2, y2, z2)，其相應(yīng)的姿態(tài)為。對此我們寫出各個機械臂的關(guān)節(jié)點以及機械臂各段的點集。如果在旋轉(zhuǎn)的任意小的角度內(nèi)，機械臂和關(guān)節(jié)都與集合A不相交，則我們避開了障礙物。12.5模型的分析與討論：本文設(shè)計的算法和模型具有以下特點1 在不影響精度的基礎(chǔ)上將多自由度運動問題合理分解成若干低自由度運動的合成，從而減少了變量控制，簡化了運動方程的求解。2 拋開傳統(tǒng)方法，提出一種新的實時運動規(guī)劃方法，大大縮小了搜索空間，從而有效地提高了系統(tǒng)效率。3 采用先整體

18、定位后局部精調(diào)的交替控制方法，使機械臂達到更加合理、便捷、高效的運動。附錄1Answer1: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=nxnynz; o=ox oyoz; a=ax ay az; p=pxpypz;theta=zeros(6,100);fori=2:100%theta1 theta11=atan(py-65*ay)/(px-65

19、*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;elsetheta(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;elsetheta(i,3) = theta31;end%theta2 w=a

20、tan(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;elsetheta(i,2) = theta21

21、;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;elsetheta(i,5) = theta51;end%theta4 theta41=asin(s511(i,j)*ax-c1(i,j)*ay)/s511(i,j); theta42=pi-asin(s

22、1(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;elsetheta(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)=the

23、ta62-theta(i-1,6)theta(i,6) = theta62;elsetheta(i,6) = theta61;endendAnswer2: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=T06(3,4); n=nxnynz; o=ox oyoz; a=ax ay az; p=pxpypz;theta=ze

24、ros(7,6);fori=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;elsetheta(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)

25、theta(i,3) = theta32;elsetheta(i,3) = theta31;endc1(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+

26、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;elsetheta(i,2) = theta21;end%theta5theta51=acos(s23(i)*(c1(i)*ax+s1(i)*ay)-c23(i)*az);theta52=2*pi-acos(s23(i)*(c1(

27、i)*ax+s1(i)*ay)-c23(i)*az);if theta51-theta(i-1,5)=theta52-theta(i-1,5)theta(i,5) = theta52;elsetheta(i,5) = theta51;endc5(i)=cos(theta(i,5);s5(i)=sin(theta(i,5);%theta4 theta41=asin(s5(i)*ax-c1(i)*ay)/s5(i); theta42=pi-asin(s1(i)*ax-c1(i)*ay)/s5(i);if theta41-theta(i-1,4)=theta42-theta(i-1,4)theta(

28、i,4) = theta42;elsetheta(i,4) = theta41;end%theta6 theta61=asin(sin(c1(i)*ox+s1(i)*oy)-c23(i)*oz)/s5(i); theta62=pi-asin(s23(i)*(c1(i)*ox+s1(i)*oy)-c23(i)*oz)/s5(i);if theta61-theta(i-1,6)=theta62-theta(i-1,6)theta(i,6) = theta62;elsetheta(i,6) = theta61;endendAnswer3:L1 = link( pi/2 0 0 140 0, stan

29、dard);L2 = link( 0 225 0 0 0, standard);L3 = link(-pi/2 0 0 0 0, standard);L4 = link(pi/2 0 0 225 0, standard);L5 = link(-pi/2 0 0 0 0, standard);L6 = link(0 0 0 65 0, standard);r=robot(L1 L2 L3 L4 L5 L6);=robot;drivebot(r);q=0 0 -pi/2 0 -pi/2 0;q1=-1.4713 1.0647 -0.7557 0 -0.3072 0;qi=q1+q;t=

30、0:0.056:15; q=jtraj(q,qi,t); holdonT=fkine(r,q); sizeT=size(T); fori=1:sizeT(3) x=T(1,4,i); y=T(2,4,i); z=T(3,4,i);holdon plot3(x,y,z)endplot(r,q)附錄：指令序列附錄1 answer1的部分結(jié)果指令序列關(guān)節(jié)1關(guān)節(jié)2 關(guān)節(jié)3關(guān)節(jié)4關(guān)節(jié)5關(guān)節(jié)61-2.0 2.0 -2.0 0.0 2.0 0.0 2-2.0 2.0 -2.0 0.0 2.0 0.0 30-2.0 2.0 -2.0 0.0 2.0 0.0 31-2.0 1.0 -2.0 0.0 2.0 0.0 36-2.0 0.0 -2.0 0.0 2.0 0.0 37-2.0 0.0 -2.0 0.0 0.4 0.0 66-2.0 0.0 -2.0 0.0 0.0 0.0 67-2.0 0.0 -1.4 0.0 0.0 0.0 87-2.0 0.0 0.0 0.0 0.0 0.0 88-0.3 0.0 0.0 0.0 0.0 0.0 備注：由于關(guān)節(jié)6的轉(zhuǎn)角對機械臂指尖的位置沒有影響，本文不予考慮，其轉(zhuǎn)角值認定為0，省略號表示中間指令序列與上一個指令相同，具體指令序列參見answer1.xls。附錄2 answer2的部分結(jié)果指尖從最初的E點移動到閉合曲線上點(5