机器人技术基础实验报告2

机器人技术基础实验报告班级： 学号： 姓名： 台号：2滦程：2机器人空间位姿描述 成绩：批改日期： 教师签字： 实验目的：1、认识机器人位置与姿态的描述方式2、了解多种姿态的描述方法实验设备及软件：1、珞石XB4机器人2、MATLAB实验原理：位置描述：建立坐标系后可以用一个3x1的位置矢量对坐标系中的任何 点进行定位。用三个相互正交的带有箭头的单位矢量来表示一个坐标系A用 一个矢量来表示一个点PA ,并且可等价地被认为是空间的一个位置矢量， 或者简单地用一组有序的三个数字来表示。矢量的各个元素用下标X, y和z 来标明：姿态描述:为了描述物体的姿态，需要在物体上固定一个坐标系并且给 出此 坐标系相对于参考系的表达。用心、和务来表示坐标系B主轴 方向 的单位矢量。在用坐标系A的坐标表达时，写成跆、滲、Z卷。这三个单位 矢量按照顺序排列组成一个3x3的矩阵，称之为旋转矩阵。记为: 分别绕X轴，Y轴，Z轴的诫转变换%: B100cO0sOcO-sO00cO-sO=010,Rz3)=sOcO0.0sOcO .-sO0cO001心(0)=坐标系变换是一个坐标系描述到另一个坐标系描述的变换。被描述的空间 点本身没有改变，只是它的描述改变了。一般情况下坐标系A与坐标系B既存在位置差异乂存在姿态差异。则相 对于坐标系B描述的点PB在坐标系A下的描述为：nA _ An pB I nArrBORG为了简化表达，可改写为：pA-bRPborgPB=jTPB1011 -1其中笳彳张Pborg为4x4矩阵，称为齐次变换矩阵。描述了坐标系B相对0 1于坐标系A的变换。坐标系的半移和旋转变换姿态其他描述：X-Y-Z固定角等效转轴表示法X-Y-Z欧拉角四元素法1、X-Y-Z固定角:坐标系B的方位规则如下：最初坐标系B与A重合，转动相对固定坐 标系A来描述，先绕忑轴转丫角，再绕纭轴转卩角，最后绕乙轴转a角。 按照从右向左原则，得到相应旋转矩阵R=rotz(a)*roty(p)*rotx(Y)= sacpcaspsy - sacysaspsy + cacycpsycascy + sasysascy 一 easyepey设姿态矩阵为了求其逆问题，需要求解旋转矩阵到X-Y-Z固定角的描述表达式。rnr12r13-令心(y，B，a)=r21丫22旷23r31r32r33.将(1,1)与(2,1)平方相加再开方根,可得cos(B),再与(3,1)相比得:tan /?=-r31即：B= atan 2(-r3P 7rii + r2i)当 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 /?)当cos/?= 0时，即/? = 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欧拉角坐标系B的方位规则如下：最初坐标系B与A重合，转动相对运动坐 标系B来描述，先绕Xb轴转丫角，再绕与轴转p角，最后绕Zp轴转a角。按照从左向右原则，得到相应旋转矩阵R=iotx(y)*roty(P)*rotz(a) =cpcac/?sasftCyS + SyCaSyy CyCSy SSp CpSySySq CyCaSp CaSy + CySSpCyC为了求其逆问题，需要求解旋转矩阵到X-Y-Z欧拉角的描述表达式。设姿态矩Hl厂12厂13阵$RxVz(Y，卩,a)=r21r22r23厂31 r32 g当r2333不同时等于零时，山其旋转矩阵可得: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)同样山其旋转矩阵可得:即：tan a=-rnY= atan2(-r12,rn)当r23= 0Kr33 = 0时，cos0 = 0,即/?=90,可解得: 90。, y = 0.0 , a = atan2(r2V r22)s = -90 , y = 0.0, a = atan2(r21, r22)。3、等效转轴表示法坐标系B的方位规则如下：首先将坐标系B和一个已知参考坐标系A重 合。将B绕矢量斤4按右手定则旋转0角。矢量斤“被称为有限旋转的等效轴。kxkxv0 +c0Rk(0) = kxkyv0 + k2s0kxk7v0 一 kysOkxkyv0 一 kzs0 kxkzvO + kys0 kykyv3 + cO kykzv0 一 kxsdkykzv0 + kxs0 k2kzv0 + cO式中，cO = cosO. sO = sinO, vO = 1-cosO,并且单位矢量KA=kx ky kzTW2-19等盘轴角坐标杲表示法经常用旋转量e乘以单位方向矢量k形成一个简单的3*1 的矢量来 描述姿态，用K表示。对于逆问题，给定的旋转矩阵求出斤“和e,设：r13r33.rnBkW = %r31那么：门2 - r2z ri3r31 厂21 一几2.4、四元数法单位四元数：可以被看作是绕单位向量元旋转0角，该旋转与四元数组 的 关系为：S=cos( 9 /2), v=sin( 9 /2) n参数念示为g =%,y,z,w ,其中：x2 +y2 +z2 +w2 = 1利用单位四元数参 数表示的旋转矩阵为：1 一 2y2 2z22(xy zw)2(xz + yvv)gR = 2(xy + zw)1 - 2x2 - 2z22(yz - xiv).2(xz - yw) 2(yz + xvv) 1 - 2x2 - 2y2单位四元数对姿态描述具有独特的优势，其避免了欧拉角大角度旋转时 奇 异性的问题，同时数据描述表达式简单，可以被用于规划高阶连续姿态 运动以 及在多姿态间插值。为了求其逆问题，需要求解旋转矩阵到单位四元数的描述表达式。设姿态 矩阵：门1厂21厂31r12r13r22r23r32r33.对应的单位四元数描述为:W+ 旷22 +旷33加一厂234w4w实验内容：U已知坐标系B相对于坐标系A的描述及坐标系B下的位置点= 1,2,3,分别计算 旋转软 位置矢量器叱，齐次变换矩阵敎及位置点在坐标系A下的位置PdCOSCXRz (a)= sina.0-sina 0cosa 00 1J绕Xb轴转卩角的旋转矩阵为cosp 0Ry (P)=01-sinp 0 绕与轴转Y角的旋转矩阵为sin仗0COSp.1 0 0Rx (y)= 0 cosy -siny .0 siny cosy. 按照从左向右原则，得到相应旋转矩阵Rg (a)心(P) Rx(Y)=cosacosp cosasinpsiny - sinacosy sinacosp sinasinpsiny + cosacosy .-sinpcospsinycosasinpcosy + sinasinysinasinpcosy - cosasinycospcosy三次绕固定轴旋转所得到的最终姿态与以相反顺序绕运动坐标轴(其 本身)旋转的最终姿态相同。X-Y-Z固定角与Z-Y-X欧拉角相同。表21坐标系描述变换P?ORGPA坐标系B绕坐 标系A的Z轴旋转90度0一 101100001J(,() )0一10 010 0 0 0 010.0 0 0 1.-2 1 3坐标系B绕坐 标系A的Z轴 旋转90度，之后 坐标系A的X 轴旋转45度0 -1 0 灼2 一呵2 灼2 0色.(,() )0-10Q灼2 -灼2 0 灼2 灼2 0 0 0 1-2y/22/21坐标系B绕坐 标系A的Z轴 旋转90度之后 坐标系(A的X 轴旋转45度，再 沿着坐标系A 的X轴负向移动 10个爪位.坐标 系A的Y轴移 动5个单位0 -1 0 灼2 -冋2 化0化.-105.00-10 一10 叫2 灼2 5 % 0 灼20 0 0 1 -12 5-彥 2個2、推导Z-Y-X欧拉角的姿态描述表达式，并比较与X-Y-Z固定角的区别。 最初坐标系B与A重合，转动相对运动坐标系B来描述，先绕 轴转a角，再绕与轴转卩角,最后绕/轴转丫角。绕為轴转a角的旋转矩阵为X-Y-Z固定角的旋转矩阵为： cosacosp cosasinpsiny 一 sinacosy sinacosp sinasinpsiny + cosacosy .-sinpcospsinycosasinpcosy + sinasinysinasinpcosy - cosasinycospcosy3、推导旋转矩阵到X-Y-Z欧拉角的变换表达式 其旋转矩阵：R=iotx(y)*roty(p)*rotz(a)=cpcaCySa + Sy CaSpsysa CyCaSp-CpSuCyCa SySSp caSY + cysas(iSfi一卬SyCyCp为了求其逆问题，需要求解旋转矩阵到X-Y-Z欧拉角的描述表达式。 设姿态矩阵Hi r12 r13-眛 xYz(Y0a)=厂21r22厂23 1 心 203.当r2333不同时等于零时，山其旋转矩阵可得：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)同样山其旋转矩阵可得:tan (a)=rny = atanl-rr当匕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、编写姿态变化代码，使用MATLAB软件打开东大机器人实验程序,机器 人空间位姿描述sia002.slx文件，编写所有转换模块。参见附件5、利用上述推动的姿态转换表达进行不同姿态描述的求取，并填表。并利用机 器人工具箱求出的结果进行比较。表2-2姿态描述转换旋转矩阵X-Y-Z欧拉 角X-Y-Z固定角 描述等效转 轴单位四 元数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 = 900 = 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.12770.23930.9515注：等效轴描述为e与单位方向矢量的乘积形势，e的单位为“度”。 真机实验步骤：1、操作珞石XB6机器人观察其位置与姿态的数据变化。读取儿组位置和姿态 数据并在MATLAB中进行绘图，并于实际机器人末端进行对照。依据表23中不同的构型值，修改模型输入，单击运行按钮，观看输出的旋 转矩阵值，并填入到表23中。表23机器人姿态计算构型旋转矩阵描述X-Y-Z 固定角 描述等效转 轴描述单位四元 数10,10,10,10,10,100.53360.02010.8455 -0.2512-0.95080.1812.0.8076-0.30910.5023.-2.59-0.94-0.44-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.0782610,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与单位方向矢量的乘积形势，e的单位为“度”。注意事项：1、旋转矩阵描述法结果唯一，欧拉角、固泄角、等效转轴、单位四元数等解不唯一2、手动示教机器人时无法精确到指左关右角，接近指定关节角即可。具体程序： XYZ欧拉角至旋转矩阵：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;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 旋转矩阵到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; %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欧拉角到旋转矩阵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);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使用系统库函数function R = ITF(rpy)% outputR = zeros(3,3);% function descriptioneul=rpy(1)*pi/180 rpy(2)*pi/180 rpy(3)*pi/180;R=eul2rotm(eulz1ZYX1);end 旋转矩阵到ZYX欧拉角function rpy = TF (R)% outputrpy = zeros (lr 3);% function description%rpy=(rotm2eul(R, 1 ZYX1) ) *180/pi; 使用库函数进行计算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(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固定角到旋转矩阵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*cx; 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 旋转矩阵到XYZ固定角function rpy = TF (R)% outputrpy = zeros (1,3);% function descriptionrpy(2)=atan2(-R(3/1)fsqrt(R(lz1)八2+R(2Z1)A2);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 等效转轴到旋转矩阵function R = ITF(rpy)% outputR = zeros (3Z 3);% function descriptionth=norm(rpy*pi/180);ct=cos(th);st=sin(th);vt=l-cos(th);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 旋转矩阵到等效转轴function rpy = TF (R)% outputrpy = zeros (1,3);% function descriptionth= acosd(R(l,1)+R(2Z2)+R(3Z 3)-1)/2);rpy=(R(3,2)-R(2,3) (R(1,3)-R(3,1) (R(2,1)-R(lz2);if sind(th) = 0rpy=rpy/(2*sind(th)*th;endend 四元素到旋转矩阵 function R = ITF(rpy) % outputR = zeros(3Z 3);% function description x=rpy(1)/norm(rpy); y=rpy(2)/norm(rpy); z=rpy(3)/norm(rpy); w=rpy(4)/norm(rpy);R(l,1)=l-2*y*y-2*z*z;R(lf 2)=2*(x*y-z*w);R(1A 3)=2*(x*z+y*w);R(2,l)=2*(x*y+z*w);R(2Z2)=l-2*x*x-2*z*z;R(2Z 3)=2*(z*y-x*w);R(3Z1)=2*(x*z-y*w);R(3,3)=l-2*x*x-2*y*y;R(3,2)=2*(z*y+x*w);end 旋转矩阵到四元素function rpy = TF (R)% outputrpy = zeros (1 4);rpy(4)=sqrt(1+R(lr1)+R(2,2)+R(3,3)*05; rpy(l) = (R(3z2)-R(2z3) )/(4*rpy(4);rpy(2) = (R(l,3)-R(3zl) )/(4*rpy(4);rpy(3) = (R(2/l)-R(lf2) )/(4*rpy(4);end第一组数据matlab绘图一10897054D13920 一9894伊171039 .一1114099.14.27.20 矗1O.1OW101O10一 ：； fe1:2-3 4 5 60.0000000.0000000.0000000.000000 0.000000 0.000000第二组数据matlab绘图xI1041981010.200540 10.099470 -10.180028 1071013 40.326961第三组数据matlab绘图-1-0.5X第四组数据matlab绘图1 -0.5N 0-0.5 第五组数据matlab绘图