PUMA560機(jī)器人運(yùn)動(dòng)學(xué)分析

山東大學(xué)威海分校2012年上學(xué)期工業(yè)機(jī)器人學(xué)作業(yè)論文第12頁DATE\@"M/d/yyyy"12/29/2012作者：李安洲PUMA560機(jī)器人運(yùn)動(dòng)學(xué)論文姓名:學(xué)號(hào):專業(yè)及班級(jí):目錄摘要 2關(guān)鍵詞 21.引言 22．PUMA560機(jī)器人數(shù)學(xué)模型的建立 22.1.確定D-H坐標(biāo)系并獲取D-參數(shù) 22.2建立運(yùn)動(dòng)學(xué)方程 33.位姿的正﹑逆解 33.1正解 33.2逆解 33.2.1求，， 33.2.2求，， 44.PUMA560雅克比矩陣 54.1矢量積法 64.2微分變換法 65．Matlab編程對(duì)其正﹑逆解和雅克比矩陣的求解 75.1正解﹑逆解 75.2雅克比矩陣 76.總結(jié) 7參考文獻(xiàn) 7附件： 8正解程序 8逆解程序 8雅克比矩陣的矢量法程序 10雅克比矩陣的微分變換法程序 11摘要：采用D-H坐標(biāo)系對(duì)機(jī)器人Puma560建立個(gè)關(guān)節(jié)的坐標(biāo)系并獲取D-H參數(shù)，并對(duì)其運(yùn)動(dòng)建立數(shù)學(xué)模型用matlab編程對(duì)其求位姿正逆解及雅克比矩陣，catia對(duì)Puma560建模三維模型。關(guān)鍵詞：Puma560正逆解；雅克比矩陣；Matlab引言機(jī)器人運(yùn)動(dòng)學(xué)包括正向運(yùn)動(dòng)學(xué)，即給定機(jī)器人各關(guān)節(jié)變量，計(jì)算機(jī)器人末端的位置姿態(tài)；逆向運(yùn)動(dòng)學(xué)即已知機(jī)器人末端的位置姿態(tài)，計(jì)算機(jī)器人對(duì)應(yīng)位姿的全部關(guān)節(jié)變量。一般正向運(yùn)動(dòng)學(xué)的解是唯一和容易獲得的，而逆向運(yùn)動(dòng)學(xué)往往有多個(gè)解而且分析更為復(fù)雜。機(jī)器人逆運(yùn)動(dòng)分析是運(yùn)動(dòng)規(guī)劃與控制中的重要問題，但由于機(jī)器人逆運(yùn)動(dòng)問題的復(fù)雜性和多樣性，無法建立通用的解析算法。機(jī)構(gòu)逆運(yùn)動(dòng)學(xué)問題實(shí)際上是一個(gè)非線性超越方程組的求解問題，其中包括解的存在性、唯一性及求解的方法等一系復(fù)雜問題。本文主要通過最基本分析方法對(duì)Puma560機(jī)器人的運(yùn)動(dòng)進(jìn)行分析，包括運(yùn)動(dòng)的位置和姿態(tài)的正、逆解，及其運(yùn)動(dòng)的雅克比矩陣的求解。2．PUMA560機(jī)器人數(shù)學(xué)模型的建立PUMA機(jī)器人操作臂可以看作一個(gè)開式運(yùn)動(dòng)鏈，它由一系列連桿通過轉(zhuǎn)動(dòng)關(guān)節(jié)串聯(lián)而成，關(guān)節(jié)的相對(duì)轉(zhuǎn)動(dòng)導(dǎo)致連桿的運(yùn)動(dòng)。為了研究機(jī)器人各連桿的運(yùn)動(dòng)，建立如圖1所示坐標(biāo)系根據(jù)Denavil和Hartenberg提出的齊次變換矩陣法，建立機(jī)器人的運(yùn)動(dòng)學(xué)方程。2.1.確定D-H坐標(biāo)系并獲取D-參數(shù)PUMA560的6個(gè)關(guān)節(jié)全為轉(zhuǎn)動(dòng)關(guān)節(jié):坐標(biāo)軸:沿著i+1關(guān)節(jié)的運(yùn)動(dòng)軸;坐標(biāo)軸:沿著和的公法線,指向離開軸的方向;坐標(biāo)軸:按右手直角坐標(biāo)系法則制定;連桿長(zhǎng)度;和兩軸心線的公法線長(zhǎng)度;連桿扭角:和兩軸心線的夾角;兩連桿距離:和兩坐標(biāo)軸的公法線距離;兩桿夾角:和兩坐標(biāo)軸的夾角;D-H參數(shù)表連桿變量變量范圍10020149.093431.80420.32433.075006002.2建立運(yùn)動(dòng)學(xué)方程PUMA560機(jī)器人具有六個(gè)自由度，而且六個(gè)關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)，前三個(gè)關(guān)節(jié)主要影響末端執(zhí)行器的位置，后三個(gè)關(guān)節(jié)決定末端執(zhí)行器的姿態(tài)，將機(jī)器人位置結(jié)構(gòu)和姿態(tài)結(jié)構(gòu)末端執(zhí)行器的位置矢量和姿態(tài)轉(zhuǎn)換矩陣并通過齊次變換得到：兩桿間的位姿矩陣其中：末端執(zhí)行器位姿矩陣即PUMA560機(jī)器人的運(yùn)動(dòng)學(xué)方程：3.位姿的正﹑逆解3.1正解已知各關(guān)節(jié)的變量，求末端執(zhí)行器的位姿矩陣即正解。把帶入（2.1）求得各兩連桿間的位姿矩陣，再由（2.2）即可求得末端執(zhí)行器的位姿矩陣：3.2逆解將PUMA560的運(yùn)動(dòng)方程（3.1）寫為:若末端連桿的位姿已經(jīng)給定,即和為已知,則求關(guān)節(jié)變量的值稱為運(yùn)動(dòng)反解.3.2.1求，，式中,正、負(fù)號(hào)對(duì)應(yīng)于的兩個(gè)可能解.再令矩陣方程(3.3)兩端的元素(1,4)和(3,4)分別對(duì)應(yīng)相等,則得兩方程:式(3.68)與式(3.71)的平方和為:式中,正、負(fù)號(hào)對(duì)應(yīng)于的兩個(gè)可能解.令矩陣方程(3.8)兩端的元素(1,4)和(2,4)分別對(duì)應(yīng)相等,則得兩方程:根據(jù)和解的四種可能組合可以得到相應(yīng)的四種可能值,于是可得到的四種可能解.3.2.2求，，設(shè)令矩陣方程(3.8)兩端的元素(1,3)和(3,3)分別對(duì)應(yīng)相等,則得兩方程:令矩陣方程(3.12)兩端的元素(1,3)和(3,3)分別對(duì)應(yīng)相等,則得兩方程:令矩陣方程(3.13)兩端的元素(3,1)和(1,1)分別對(duì)應(yīng)相等可得到：以上求得PUMA560的運(yùn)動(dòng)反解可能存在8種解.但是,由于結(jié)構(gòu)的限制,例如各關(guān)節(jié)變量不能在全部360度范圍內(nèi)運(yùn)動(dòng),有些解不能實(shí)現(xiàn).在機(jī)器人存在多種解的情況下,應(yīng)選取其中最滿意的一組解,以滿足機(jī)器人的工作要求.4.PUMA560雅克比矩陣機(jī)器人雅可比矩陣(簡(jiǎn)稱雅可比)揭示了操作空間與關(guān)節(jié)空間的映射關(guān)系。雅可比不僅表示操作空間與關(guān)節(jié)空間的速度映射關(guān)系，也表示二者之間力的傳遞關(guān)系，為確定機(jī)器人的靜態(tài)關(guān)節(jié)力矩以及不同坐標(biāo)系間速度、加速度和靜力的變換提供了便捷的方法。在機(jī)器人學(xué)中，雅可比是一個(gè)把關(guān)節(jié)速度向量變換為手爪相對(duì)基坐標(biāo)的廣義速度向量v的變換矩陣。機(jī)械手的操作速度與關(guān)節(jié)速度間的線性變換定義為機(jī)械手的雅可比矩陣.4.1矢量積法PUMA560的6個(gè)關(guān)節(jié)都是轉(zhuǎn)動(dòng)關(guān)節(jié),因而其雅克比矩陣具有下列形式:由式（2.1）求得各關(guān)節(jié)相對(duì)于基坐標(biāo)系的位姿矩陣即獲得相應(yīng)的和再由式（4.2）求得對(duì)應(yīng)。得到的數(shù)據(jù)帶入式（4.1）求得各關(guān)節(jié)，最后帶入式（4.3）求得其末端執(zhí)行器相對(duì)基坐標(biāo)系的雅克比矩陣。4.2微分變換法PUMA560機(jī)器人末端執(zhí)行器的變換矩陣根據(jù)文獻(xiàn)的微分變換法，相對(duì)雅克比矩陣的第列元素由決定，對(duì)于PUMA560機(jī)器人來說，根據(jù)式（2.1）和（2.2）求得則可求得雅克比矩陣與相對(duì)雅克比矩陣之間的關(guān)系為式中是的轉(zhuǎn)置矩陣，可通過式（4.4）求出。將PUMA560機(jī)器人的相對(duì)雅克比矩陣和帶入式（4.6），即可得到機(jī)器人雅克比矩陣。5.Matlab編程對(duì)其正﹑逆解和雅克比矩陣的求解利用Matlab軟件根據(jù)以上的數(shù)學(xué)模型對(duì)其求解，以下均是以正解給的數(shù)據(jù)和結(jié)果對(duì)其求解的結(jié)果。5.1正解﹑逆解初始數(shù)據(jù)（六個(gè)關(guān)節(jié)的）運(yùn)行matlab程序求得末端執(zhí)行器位姿矩陣為：0.5322-0.1137-0.83900.0833-0.4697-0.8641-0.18090.1890-0.70440.4903-0.5133-0.49250001.0000利用以上求得的末端執(zhí)行器位姿矩陣數(shù)據(jù)運(yùn)行matlab程序求得其八個(gè)解為：解1-67.5924-200.0000165.3728116.2894-70.3888178.42842-67.5924-200.0000165.3728-63.710670.3888-1.57163-67.592452.372120.000062.3243-72.4909-65.60514-67.592452.372120.0000-117.675772.4909114.3949520.0000-232.3721165.3728-171.7287-125.5969-136.2815620.0000-232.3721165.37288.2713125.596943.7185720.000020.000020.0000-160.0000-20.0000-160.0000820.000020.000020.000020.000020.000020.00005.2雅克比矩陣用正解中的數(shù)據(jù)運(yùn)行程序結(jié)果為：-0.1890-0.4628-0.32400000.0833-0.1684-0.11790000.0000-0.14300.26280000-0.3420-0.3420-0.6040-0.0752-0.83900.00000.93970.9397-0.21980.9726-0.18091.00000.00000.0000-0.7660-0.2198-0.51336.總結(jié)本文介紹了對(duì)PUMA560機(jī)器人運(yùn)動(dòng)學(xué)的正問題和逆問題的，及雅克比矩陣的基本數(shù)學(xué)模型和算法，對(duì)于機(jī)器人的動(dòng)力學(xué)沒有加以分析，對(duì)于機(jī)器人的分析不僅這些，軌跡優(yōu)化機(jī)器人的控制，程序設(shè)計(jì)等，隨著科技的發(fā)展機(jī)器人的為了滿足人們的要求，機(jī)器人發(fā)展擴(kuò)展越來越廣，對(duì)其的研究也越來越復(fù)雜。參考文獻(xiàn)［1］蔡自興.機(jī)器人學(xué)基礎(chǔ).北京：機(jī)械工業(yè)出版社．2009.5［2］蔡自興.機(jī)器人學(xué)．第二版．清華大學(xué)出版社［3］熊有倫．機(jī)器人學(xué)［Ｍ］．北京：機(jī)械工業(yè)出版社，1993.87.94［4］葛哲學(xué)．精通MATLAB．北京：電子工業(yè)出版社．2008．2附件：正解程序%Puma560位姿正解functionT=Positive(angle)a1=[0-pi/20-pi/2pi/2-pi/2];l=[000.43180.0203200];d=[00.1490900.4330700];A=zeros(4);T=eye(4);A(4,4)=1;fori=1:6c=cos(angle(i)/180*pi);s=sin(angle(i)/180*pi);ca=cos(a1(i));sa=sin(a1(i));A(1,1)=c;A(1,2)=-s;A(1,4)=l(i);A(2,1)=s*ca;A(2,2)=c*ca;A(2,3)=-sa;A(2,4)=-sa*d(i);A(3,1)=s*sa;A(3,2)=c*sa;A(3,3)=ca;A(3,4)=ca*d(i);T=T*A;end逆解程序%Puma560逆解functionangle=nijie(T)d2=0.14909;d4=0.43307;a2=0.4318;a3=0.02032;nx=T(1,1);ny=T(2,1);nz=T(3,1);ox=T(1,2);oy=T(2,2);oz=T(3,2);ax=T(1,3);ay=T(2,3);az=T(3,3);px=T(1,4);py=T(2,4);pz=T(3,4);angle=[];form=1:2%angle1有兩個(gè)解angle1=atan2(py,px)-atan2(d2,((-1)^m)*sqrt(px^2+py^2-d2^2));forn=1:2%angle3有兩種可能k=(px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2)/(2*a2);angle3=atan2(a3,d4)-atan2(k,(-1)^n*sqrt(a3^2+d4^2-k^2));c1=cos(angle1);s1=sin(angle1);c3=cos(angle3);s3=sin(angle3);angle23=atan2(-(a3+a2*c3)*pz+(c1*px+s1*py)*(a2*s3-d4),(-d4+a2*s3)*pz+(c1*px+s1*py)*(a2*c3+a3));angle2=angle23-angle3;c2=cos(angle2);s2=sin(angle2);fort=1:2%angle4有兩種情況A1=[c1-s100;s1c100;0010;0001];A2=[c2-s200;001d2;-s2-c200;0001];A3=[c3-s30a2;s3c300;0010;0001];T3=A1*A2*A3;u=inv(T3)*T;angle4=atan2((-1)^t*u(3,3),(-1)^(t+1)*u(1,3));s4=sin(angle4);c4=cos(angle4);A4=[c4-s40a3;001d4;-s4-c400;0001];%求angle5T4=T3*A4;u1=inv(T4)*T;angle5=atan2(-u1(1,3),u1(3,3));s5=sin(angle5);c5=cos(angle5);A5=[c5-s500;00-10;s5c500;0001];%求angle6T5=T4*A5;u2=inv(T5)*T;angle6=-atan2(u2(3,1),u2(1,1));Row=[angle1,angle2,angle3,angle4,angle5,angle6]*180/pi;angle=[angle;Row];endendend雅克比矩陣的矢量法程序%雅克比矩陣矢量積法求解functionJa=vep(angle)a1=[0-pi/20-pi/2pi/2-pi/2];l=[000.43180.0203200];d=[00.1490900.4330700];A=zeros(4);T=eye(4);A(4,4)=1;fork=1:6c=cos(angle(k)/180*pi);s=sin(angle(k)/180*pi);ca=cos(a1(k));sa=sin(a1(k));A(1,1)=c;A(1,2)=-s;A(1,4)=l(k);A(2,1)=s*ca;A(2,2)=c*ca;A(2,3)=-sa;A(2,4)=-sa*d(k);A(3,1)=s*sa;A(3,2)=c*sa;A(3,3)=ca;A(3,4)=ca*d(k);T=T*A;Tn(:,:,k)=T;endfori=1:6z=Tn