機器人技術(shù)基礎(chǔ)實驗報告2

1、機器人技術(shù)基礎(chǔ)實驗報告班級： 學(xué)號： 姓名： 臺號：2灤程：2機器人空間位姿描述 成績：批改日期： 教師簽字： 實驗?zāi)康模?、認(rèn)識機器人位置與姿態(tài)的描述方式2、了解多種姿態(tài)的描述方法實驗設(shè)備及軟件：1、珞石XB4機器人2、MATLAB實驗原理：位置描述：建立坐標(biāo)系后可以用一個3x1的位置矢量對坐標(biāo)系中的任何 點進(jìn)行定位。用三個相互正交的帶有箭頭的單位矢量來表示一個坐標(biāo)系A(chǔ)用 一個矢量來表示一個點PA ,并且可等價地被認(rèn)為是空間的一個位置矢量， 或者簡單地用一組有序的三個數(shù)字來表示。矢量的各個元素用下標(biāo)X, y和z 來標(biāo)明：姿態(tài)描述:為了描述物體的姿態(tài)，需要在物體上固定一個坐標(biāo)系并且給 出此 坐

2、標(biāo)系相對于參考系的表達(dá)。用心、和務(wù)來表示坐標(biāo)系B主軸 方向 的單位矢量。在用坐標(biāo)系A(chǔ)的坐標(biāo)表達(dá)時，寫成跆、滲、Z卷。這三個單位 矢量按照順序排列組成一個3x3的矩陣，稱之為旋轉(zhuǎn)矩陣。記為: 分別繞X軸，Y軸，Z軸的誡轉(zhuǎn)變換%: B100cO0sOcO-sO00cO-sO=010,Rz3)=sOcO0.0sOcO .-sO0cO001心(0)=坐標(biāo)系變換是一個坐標(biāo)系描述到另一個坐標(biāo)系描述的變換。被描述的空間 點本身沒有改變，只是它的描述改變了。一般情況下坐標(biāo)系A(chǔ)與坐標(biāo)系B既存在位置差異乂存在姿態(tài)差異。則相 對于坐標(biāo)系B描述的點PB在坐標(biāo)系A(chǔ)下的描述為：nA _ An pB I nArrBORG為

3、了簡化表達(dá)，可改寫為：pA-bRPborgPB=jTPB1011 -1其中笳彳張Pborg為4x4矩陣，稱為齊次變換矩陣。描述了坐標(biāo)系B相對0 1于坐標(biāo)系A(chǔ)的變換。坐標(biāo)系的半移和旋轉(zhuǎn)變換姿態(tài)其他描述：X-Y-Z固定角等效轉(zhuǎn)軸表示法X-Y-Z歐拉角四元素法1、X-Y-Z固定角:坐標(biāo)系B的方位規(guī)則如下：最初坐標(biāo)系B與A重合，轉(zhuǎn)動相對固定坐 標(biāo)系A(chǔ)來描述，先繞忑軸轉(zhuǎn)丫角，再繞紜軸轉(zhuǎn)卩角，最后繞乙軸轉(zhuǎn)a角。 按照從右向左原則，得到相應(yīng)旋轉(zhuǎn)矩陣R=rotz(a)*roty(p)*rotx(Y)= sacpcaspsy - sacysaspsy + cacycpsycascy + sasysascy 一

4、easyepey設(shè)姿態(tài)矩陣為了求其逆問題，需要求解旋轉(zhuǎn)矩陣到X-Y-Z固定角的描述表達(dá)式。rnr12r13-令心(y，B，a)=r21丫22曠23r31r32r33.將(1,1)與(2,1)平方相加再開方根,可得cos(B),再與(3,1)相比得:tan /?=-r31即：B= atan 2(-r3P 7rii + r2i)當(dāng) cos B 豐 0 時,利用r21/cos P 除以rn/cos 0 得:a = atan2(r21/cos P，7)/cos “)；利用r32/cos p 除以r33/cos p 得：y = atan2(r23/cos p, r33/cos /?)當(dāng)cos/?= 0時

5、，即/? = 90,僅能求出a與y的和或差，一般取a = 0.0,可解 得： = 90a = 0.0 , y = atan2(r12.r22)或 P =-90, a = 0.0, y = -atan2(r12,r22)o2、X-Y-Z歐拉角坐標(biāo)系B的方位規(guī)則如下：最初坐標(biāo)系B與A重合，轉(zhuǎn)動相對運動坐 標(biāo)系B來描述，先繞Xb軸轉(zhuǎn)丫角，再繞與軸轉(zhuǎn)p角，最后繞Zp軸轉(zhuǎn)a角。按照從左向右原則，得到相應(yīng)旋轉(zhuǎn)矩陣R=iotx(y)*roty(P)*rotz(a) =cpcac/?sasftCyS + SyCaSyy CyCSy SSp CpSySySq CyCaSp CaSy + CySSpCyC為了求其

6、逆問題，需要求解旋轉(zhuǎn)矩陣到X-Y-Z歐拉角的描述表達(dá)式。設(shè)姿態(tài)矩Hl廠12廠13陣$RxVz(Y，卩,a)=r21r22r23廠31 r32 g當(dāng)r2333不同時等于零時，山其旋轉(zhuǎn)矩陣可得:tan y=即：y = atan2(-r23,733)cosy* 73+siny * 7加二cos/?,與乃.3相比得tan B =mcos y* r33 +sin y * r22即：0= atari 2(r12f cos y r33 - sin y * r23)同樣山其旋轉(zhuǎn)矩陣可得:即：tan a=-rnY= atan2(-r12,rn)當(dāng)r23= 0Kr33 = 0時，cos0 = 0,即/?=90,可

7、解得: 90。, y = 0.0 , a = atan2(r2V r22)s = -90 , y = 0.0, a = atan2(r21, r22)。3、等效轉(zhuǎn)軸表示法坐標(biāo)系B的方位規(guī)則如下：首先將坐標(biāo)系B和一個已知參考坐標(biāo)系A(chǔ)重 合。將B繞矢量斤4按右手定則旋轉(zhuǎn)0角。矢量斤“被稱為有限旋轉(zhuǎn)的等效軸。kxkxv0 +c0Rk(0) = kxkyv0 + k2s0kxk7v0 一 kysOkxkyv0 一 kzs0 kxkzvO + kys0 kykyv3 + cO kykzv0 一 kxsdkykzv0 + kxs0 k2kzv0 + cO式中，cO = cosO. sO = sinO, v

8、O = 1-cosO,并且單位矢量KA=kx ky kzTW2-19等盤軸角坐標(biāo)杲表示法經(jīng)常用旋轉(zhuǎn)量e乘以單位方向矢量k形成一個簡單的3*1 的矢量來 描述姿態(tài)，用K表示。對于逆問題，給定的旋轉(zhuǎn)矩陣求出斤“和e,設(shè)：r13r33.rnBkW = %r31那么：門2 - r2z ri3r31 廠21 一幾2.4、四元數(shù)法單位四元數(shù)：可以被看作是繞單位向量元旋轉(zhuǎn)0角，該旋轉(zhuǎn)與四元數(shù)組 的 關(guān)系為：S=cos( 9 /2), v=sin( 9 /2) n參數(shù)念示為g =%,y,z,w ,其中：x2 +y2 +z2 +w2 = 1利用單位四元數(shù)參 數(shù)表示的旋轉(zhuǎn)矩陣為：1 一 2y2 2z22(xy z

9、w)2(xz + yvv)gR = 2(xy + zw)1 - 2x2 - 2z22(yz - xiv).2(xz - yw) 2(yz + xvv) 1 - 2x2 - 2y2單位四元數(shù)對姿態(tài)描述具有獨特的優(yōu)勢，其避免了歐拉角大角度旋轉(zhuǎn)時 奇 異性的問題，同時數(shù)據(jù)描述表達(dá)式簡單，可以被用于規(guī)劃高階連續(xù)姿態(tài) 運動以 及在多姿態(tài)間插值。為了求其逆問題，需要求解旋轉(zhuǎn)矩陣到單位四元數(shù)的描述表達(dá)式。設(shè)姿態(tài) 矩陣：門1廠21廠31r12r13r22r23r32r33.對應(yīng)的單位四元數(shù)描述為:W+ 曠22 +曠33加一廠234w4w實驗內(nèi)容：U已知坐標(biāo)系B相對于坐標(biāo)系A(chǔ)的描述及坐標(biāo)系B下的位置點= 1,2

10、,3,分別計算 旋轉(zhuǎn)軟 位置矢量器叱，齊次變換矩陣敎及位置點在坐標(biāo)系A(chǔ)下的位置PdCOSCXRz (a)= sina.0-sina 0cosa 00 1J繞Xb軸轉(zhuǎn)卩角的旋轉(zhuǎn)矩陣為cosp 0Ry (P)=01-sinp 0 繞與軸轉(zhuǎn)Y角的旋轉(zhuǎn)矩陣為sin仗0COSp.1 0 0Rx (y)= 0 cosy -siny .0 siny cosy. 按照從左向右原則，得到相應(yīng)旋轉(zhuǎn)矩陣Rg (a)心(P) Rx(Y)=cosacosp cosasinpsiny - sinacosy sinacosp sinasinpsiny + cosacosy .-sinpcospsinycosasinpcos

11、y + sinasinysinasinpcosy - cosasinycospcosy三次繞固定軸旋轉(zhuǎn)所得到的最終姿態(tài)與以相反順序繞運動坐標(biāo)軸(其 本身)旋轉(zhuǎn)的最終姿態(tài)相同。X-Y-Z固定角與Z-Y-X歐拉角相同。表21坐標(biāo)系描述變換P?ORGPA坐標(biāo)系B繞坐 標(biāo)系A(chǔ)的Z軸旋轉(zhuǎn)90度0一 101100001J(,() )0一10 010 0 0 0 010.0 0 0 1.-2 1 3坐標(biāo)系B繞坐 標(biāo)系A(chǔ)的Z軸 旋轉(zhuǎn)90度，之后 坐標(biāo)系A(chǔ)的X 軸旋轉(zhuǎn)45度0 -1 0 灼2 一呵2 灼2 0色.(,() )0-10Q灼2 -灼2 0 灼2 灼2 0 0 0 1-2y/22/21坐標(biāo)系B繞坐 標(biāo)

12、系A(chǔ)的Z軸 旋轉(zhuǎn)90度之后 坐標(biāo)系(A的X 軸旋轉(zhuǎn)45度，再 沿著坐標(biāo)系A(chǔ) 的X軸負(fù)向移動 10個爪位.坐標(biāo) 系A(chǔ)的Y軸移 動5個單位0 -1 0 灼2 -冋2 化0化.-105.00-10 一10 叫2 灼2 5 % 0 灼20 0 0 1 -12 5-彥 2個2、推導(dǎo)Z-Y-X歐拉角的姿態(tài)描述表達(dá)式，并比較與X-Y-Z固定角的區(qū)別。 最初坐標(biāo)系B與A重合，轉(zhuǎn)動相對運動坐標(biāo)系B來描述，先繞 軸轉(zhuǎn)a角，再繞與軸轉(zhuǎn)卩角,最后繞/軸轉(zhuǎn)丫角。繞為軸轉(zhuǎn)a角的旋轉(zhuǎn)矩陣為X-Y-Z固定角的旋轉(zhuǎn)矩陣為： cosacosp cosasinpsiny 一 sinacosy sinacosp sinasinpsi

13、ny + cosacosy .-sinpcospsinycosasinpcosy + sinasinysinasinpcosy - cosasinycospcosy3、推導(dǎo)旋轉(zhuǎn)矩陣到X-Y-Z歐拉角的變換表達(dá)式 其旋轉(zhuǎn)矩陣：R=iotx(y)*roty(p)*rotz(a)=cpcaCySa + Sy CaSpsysa CyCaSp-CpSuCyCa SySSp caSY + cysas(iSfi一卬SyCyCp為了求其逆問題，需要求解旋轉(zhuǎn)矩陣到X-Y-Z歐拉角的描述表達(dá)式。 設(shè)姿態(tài)矩陣Hi r12 r13-眛 xYz(Y0a)=廠21r22廠23 1 心 203.當(dāng)r2333不同時等于零時，

14、山其旋轉(zhuǎn)矩陣可得：tan (y)= 33即：y = atan2(-3,r33)cosy* 73+siny * 血3二COS0,與乃.3相比得tan =COS Y* 3 3+s,n Y * r23B= atari 2(如 cos y r33 + sin y r23)同樣山其旋轉(zhuǎn)矩陣可得:tan (a)=rny = atanl-rr當(dāng)匕3=0且忌3 = 0時，cos 0 = 0, B|J/? = 90,可解得：0 = 90。, y = 0.0 , a = atan2(r2P r22)/? = -90 , y = 0.0, a = atan2(r2P r22) 4、編寫姿態(tài)變化代碼，使用MATLAB

15、軟件打開東大機器人實驗程序,機器 人空間位姿描述sia002.slx文件，編寫所有轉(zhuǎn)換模塊。參見附件5、利用上述推動的姿態(tài)轉(zhuǎn)換表達(dá)進(jìn)行不同姿態(tài)描述的求取，并填表。并利用機 器人工具箱求出的結(jié)果進(jìn)行比較。表2-2姿態(tài)描述轉(zhuǎn)換旋轉(zhuǎn)矩陣X-Y-Z歐拉 角X-Y-Z固定角 描述等效轉(zhuǎn) 軸單位四 元數(shù)0 -1 0 V2 n V2 T o -T V2 n V2 k o tJa = 900 = 0y = 45a = 900 = 45y = 035.18 -35.1884.92.0.2706-0.27060.65330.65330 -1 01 10 0lo 011a = 900 = 90y = 0a = 90

16、0 = 0y = 000 bo000.70710.70710.8529 -0.49240.17360.52130.7841-0.33680.02970.37780.9524a = 300 = 10y =20a = 31.430 = -1.70y = 22.2122.14.45431.36.0.18930.038120.26850.94370.8529-0.41840.31230.49240.8435-0.2146-0.17360.33680.9254a = 26.13卩=18.65y = 13.06a = 30 p = 10 y = 2016.8714.87.27.87.0.14490.127

17、70.23930.9515注：等效軸描述為e與單位方向矢量的乘積形勢，e的單位為“度”。 真機實驗步驟：1、操作珞石XB6機器人觀察其位置與姿態(tài)的數(shù)據(jù)變化。讀取兒組位置和姿態(tài) 數(shù)據(jù)并在MATLAB中進(jìn)行繪圖，并于實際機器人末端進(jìn)行對照。依據(jù)表23中不同的構(gòu)型值，修改模型輸入，單擊運行按鈕，觀看輸出的旋 轉(zhuǎn)矩陣值，并填入到表23中。表23機器人姿態(tài)計算構(gòu)型旋轉(zhuǎn)矩陣描述X-Y-Z 固定角 描述等效轉(zhuǎn) 軸描述單位四元 數(shù)10,10,10,10,10,100.53360.02010.8455 -0.2512-0.95080.1812.0.8076-0.30910.5023.-2.59-0.94-0.4

18、4-142.911.04-79.06-0.8640.06679-0.47810.141910,-10,10廠10,10廠100.10920.19670.9744 0.3649-0.91970.1448092460.3398-0.1722.2.041.2812&132.72110.50.74130.18930.63940.0657610,30,30,10,-20 J0-0.6432-0.098520.7594-0.2275-0.97150.066660.7311-0.2156-0.6472-2.82-0.82-0.34-154.715.51-70.68-0.90160.0904-0.4120.0

19、782610,10廠30,20,10,-10-0.5174-0.036450.855-0.07903-0.9928-0.0901.0.8521-0.11420.51085-0.22-1.02-2.99-87.6810.57-155.2-0.49090.0592-0.86920.0122510,30,-30,20,-90,10-0.94050.33470.05845-0.3344-0.8816-0.3332.0.05996-0.33290.9411.-0.34-0.06-2.800.0706927.9-157.7-0.00.1716-0.96980.1725注：等效軸描述為e與單位方向矢量的乘積

20、形勢，e的單位為“度”。注意事項：1、旋轉(zhuǎn)矩陣描述法結(jié)果唯一，歐拉角、固泄角、等效轉(zhuǎn)軸、單位四元數(shù)等解不唯一2、手動示教機器人時無法精確到指左關(guān)右角，接近指定關(guān)節(jié)角即可。具體程序： XYZ歐拉角至旋轉(zhuǎn)矩陣：function R = ITF(rpy)R = zeros(3Z 3);rpy = rpy/180*pi;sx = sin(rpy(1);ex = cos (rpy(1);sy = sin(rpy(2);cy = cos(rpy(2);sz = sin(rpy(3);cz = cos(rpy(3);R(lz1) = cy*cz; R(lz 2) = -cy*sz; R(lz 3) = sy

21、;R(2/1) = cx*sz + cz*sx*sy; R(2,2) = cx*cz - sx*sy*sz; R(2Z 3) -cy*sx;R (3Z1) = sx*sz - cx*cz*sy; R(3,2) = cz*sx + cx*sy*sz; R(3,3) cx*cy;end 旋轉(zhuǎn)矩陣到XYZ歐拉角function rpy = TF (R)% outputrpy = zeros (1,3);% function descriptionif (R(2z3)=0) | | ( R(3,3) = 0 )rpy(3)=atan2(R(2f1)ZR(2Z 2)*180/pi;rpy(2)=90;

22、%rpy(2)=-90;rpy(1)=0;elserpy(1)=atan2(-R(2*3)z R(3r 3)*180/pi;rpy(2)=atan2(R(lr 3)z (cos(rpy(3)*R(3Z3)-sin(rpy(3)*R(2,3)*180/pi;rpy(3)=atan2(-R(lf 2)f R(lz1)*180/pi;endend ZYX歐拉角到旋轉(zhuǎn)矩陣function R = ITF(rpy)R = zeros(3Z 3);rpy=rpy/180*pisx = sin (rpy(1);ex = cos(rpy(1);sy = sin (rpy(2);cy = cos(rpy(2);

23、sz = sin (rpy(3)；cz = cos(rpy(3);R(l,1) = cy*cz; R(lz 2) =cz*sy*sx-sz*cx ; R(1,3) cz*sy*cx+sz*sx;R (2,1) = cy*sz; R(2Z 2) =sz*sy*sx+cz*cy; R(2Z 3) =sz*sy*cx-cz*sx;R (3,1) = -sy; R(3,2) =cy*sx; R(3,3) = cy*cx;end使用系統(tǒng)庫函數(shù)function R = ITF(rpy)% outputR = zeros(3,3);% function descriptioneul=rpy(1)*pi/18

24、0 rpy(2)*pi/180 rpy(3)*pi/180;R=eul2rotm(eulz1ZYX1);end 旋轉(zhuǎn)矩陣到ZYX歐拉角function rpy = TF (R)% outputrpy = zeros (lr 3);% function description%rpy=(rotm2eul(R, 1 ZYX1) ) *180/pi; 使用庫函數(shù)進(jìn)行計算rpy(2)=atan2(-R(3/1)/sqrt(R(lz1)A2+R(2r1)A2);if cos(rpy(2)=0rpy(1)=0;rpy(3)=atan2(R(lr2),R(2Z2);rpy(2)=90;elsecb=cos(

25、rpy(2);rpy(1)an2(R(2r1)/cb,R(l,l)/cb);rpy(3)=atan2(R(3f2)/cbfR(3z3)/cb);endrpy=rpy*180/pi;end XYZ固定角到旋轉(zhuǎn)矩陣function R = ITF(rpy)% outputR = zeros(3Z 3);% function descriptionrpy = rpy/180*pi;sx = sin (rpy(1);ex = cos(rpy(1);sy = sin(rpy(2);cy = cos(rpy(2);sz = sin (rpy(3);cz = cos(rpy(3);R(lz1) = cy*c

26、x; R(lf 2) =cx*sy*sz-cz*sx; R(lr 3) = sz*sx+cz*cx*sy;R (2r1) = cy*sx; R(2Z 2) = cx*cz + sx*sy*sz; R(2Z 3) =cz*sx*sy - cx*sz;R(3f1) = -sy; R(3Z 2) = cy*sz; R(3Z 3) = cy*cz;end 旋轉(zhuǎn)矩陣到XYZ固定角function rpy = TF (R)% outputrpy = zeros (1,3);% function descriptionrpy(2)=atan2(-R(3/1)fsqrt(R(lz1)八2+R(2Z1)A2);

27、if cos(rpy(2)=0rpy(1)=0;rpy(3)=atan2(R(lf 2)ZR(2Z 2);rpy(2)=90;elsecb=cos(rpy(2);rpy(1)=atan2(R(2rl)/cb/R(lzl)/cb);rpy(3)=atan2(R(3r2)/cb/R(3z3)/cb);endrpy=rpy*180/pi;end 等效轉(zhuǎn)軸到旋轉(zhuǎn)矩陣function R = ITF(rpy)% outputR = zeros (3Z 3);% function descriptionth=norm(rpy*pi/180);ct=cos(th);st=sin(th);vt=l-cos(t

28、h);kx=rpy(1)/norm(rpy);ky=rpy(2)/norm(rpy);kz=rpy(3)/norm(rpy);R(1,1)=kx*kx*vt+ct;R(lf 2)=kx*ky*vR(lz 3)=kx*kz*vt+ky*st;R(2Z1)=kx*ky*vt+ky*st;R(2Z 2)=ky*ky*vt+ct;R(2/3)=ky*kz*vt-kx*st;R(3r1)=kx*kz*vR(3,2)=kz*ky*vt+kx*st;R(3,3)=kz*kz*vt+ct;end 旋轉(zhuǎn)矩陣到等效轉(zhuǎn)軸function rpy = TF (R)% outputrpy = zeros (1,3);% function descriptionth= acosd(R(l,