机器人技术基础实验报告班级：学号：姓名：台号： 2 课程：2.机器人空间位姿描述成绩：批改日期：教师签字：实验目的：1、认识机器人位置与姿态的描述方式2、了解多种姿态的描述方法实验设备及软件:1、珞石XB4机器人2、MATLAB实验原理:位置描述：建立坐标系后可以用一个3×1的位置矢量对坐标系中的任何点进行定位。

用三个相互正交的带有箭头的单位矢量来表示一个坐标系{A}.用一个矢量来表示一个点P A ，并且可等价地被认为是空间的一个位置矢量，或者简单地用一组有序的三个数字来表示。

矢量的各个元素用下标x，y和z 来标明：姿态描述：为了描述物体的姿态，需要在物体上固定一个坐标系并且给出此坐标系相对于参考系的表达。

用X B 、Y B和Z B来表示坐标系{B}主轴方向的单位矢量。

在用坐标系{A}的坐标表达时，写成X B A、Y B A、Z B A。

这三个单位矢量按照顺序排列组成一个3×3的矩阵，称之为旋转矩阵。

记为：R B A= [X B A Y B A Z B A]分别绕X 轴，Y轴，Z轴的旋转变换为：坐标系变换是一个坐标系描述到另一个坐标系描述的变换。

被描述的空间点本身没有改变，只是它的描述改变了。

一般情况下坐标系{A}与坐标系{B}既存在位置差异又存在姿态差异。

则相对于坐标系{B}描述的点PB在坐标系{A}下的描述为：AP A=R B A P B+P BORG为了简化表达，可改写为：[P A1]=[R B A P BORG A 01][P B 1]=T B A[P B 1] 其中T B A =[RB A P BORGA01]为4×4矩阵，称为齐次变换矩阵。

描述了坐标系{B} 相对于坐标系{A}的变换。

姿态其他描述： X-Y-Z 固定角 等效转轴表示法 X-Y-Z 欧拉角 四元素法 1、X-Y-Z 固定角:坐标系{B}的方位规则如下：最初坐标系{B}与{A}重合，转动相对固定坐标系{A}来描述，先绕X A 轴转γ 角 ，再绕Y A 轴转β角，最后绕Z A 轴转α角。

按照从右向左原则，得到相应旋转矩阵R=rotz(α)*roty(β)*rotx(γ)= [cαcβcαsβsγ−sαcγcαsβcγ+sαsγsαcβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ]为了求其逆问题，需要求解旋转矩阵到X-Y-Z 固定角的描述表达式。

设姿态矩阵将(1,1)与(2,1)平方相加再开方根，可得cos(β) , 再与 (3,1)相比得：tan β= 31√r 11+r 21即：β= atan 2(−r 31,√r 112+r 212)当cos β ≠ 0时，利用r 21/cos β 除以r 11/cos β 得：α = atan 2(r 21/cos β, r 11/cosβ)；利用r 32/cos β 除以r 33/cos β 得：γ = atan 2(r 23/cos β, r 33/cos β) 当cos β = 0时，即β = ±90°，仅能求出 α与γ的和或差，一般取α = 0.0， 可解得： β = 90°α = 0.0 ，γ = atan 2(r 12, r 22) 或 β = −90°， α = 0.0， γ = −atan 2(r 12, r 22)。

2、X-Y-Z欧拉角坐标系{B}的方位规则如下：最初坐标系{B}与{A}重合，转动相对运动坐标系{B}来描述，先绕X B 轴转γ角，再绕Y B 轴转β 角，最后绕Z B 轴转α角。

按照从左向右原则，得到相应旋转矩阵R=rotx(γ)*roty(β)*rotz(α) =为了求其逆问题，需要求解旋转矩阵到X-Y-Z 欧拉角的描述表达式。

设姿态矩阵当r23,r33不同时等于零时，由其旋转矩阵可得：tanγ=−r23r33即：γ = atan 2(−r23,r33)cosγ∗ r33+sinγ * r23=cosβ，与r13相比得tanβ =r13cos γ∗ r33+sin γ ∗ r23即：β= atan 2(r13,cos γ∗ r33−sin γ ∗ r23)同样由其旋转矩阵可得：tanα=−r12r11即：γ = atan 2(−r12,r11)当r23= 0且r33=0时，cos β = 0，即β = ±90°，可解得：β = 90°，γ = 0.0 ，α = atan2(r21, r22)或β = −90°，γ = 0.0，α = atan2(r21, r22)。

3、等效转轴表示法坐标系{B}的方位规则如下：首先将坐标系{B}和一个已知参考坐标系{A}重合。

将{B}绕矢量K̂A按右手定则旋转θ角。

矢量K̂A被称为有限旋转的等效轴。

式中，cθ = cosθ，sθ = sinθ，vθ= 1−cosθ，并且单位矢量K̂A=[k x k y k z]T经常用旋转量θ乘以单位方向矢量K̂形成一个简单的3*1的矢量来 描述姿态，用K 表示。

对于逆问题，给定的旋转矩阵求出K ̂A 和θ，设：那么：且K ̂A =12sinθ[r 32−r 23r 13−r 31r 21−r12]4、四元数法单位四元数：可以被看作是绕单位向量n ̂旋转θ角，该旋转与四元数组 的关系为：S=cos(θ/2)，v=sin(θ/2) n ̂参数表示为q = x ,y ,z ,w ，其中：x 2 +y 2 +z 2 +w 2 = 1 利用单位四元数参数表示的旋转矩阵为：单位四元数对姿态描述具有独特的优势，其避免了欧拉角大角度旋转时 奇异性的问题，同时数据描述表达式简单，可以被用于规划高阶连续姿态 运动以及在多姿态间插值。

为了求其逆问题，需要求解旋转矩阵到单位四元数的描述表达式。

设姿态矩阵:对应的单位四元数描述为： w =√1+r 11 +r 22 +r 332，x =r 32−r 234w，y =r 13−r 314w，z =r 21−r 124w，实验内容：1、 已知坐标系{B}相对于坐标系{A}的描述及坐标系{B}下的位置点P B =[1,2,3]，分别计算旋转R B A ，位置矢量P BORG A ，齐次变换矩阵T B A 及位置点P B 在坐标系{A}下的位置P A 。

最初坐标系{B}与{A}重合，转动相对运动坐标系{B}来描述， 先绕Z B 轴转α角，再绕Y B 轴转β 角，最后绕X B 轴转γ角。

绕Z B 轴转α角的旋转矩阵为R Z (α)= [cosα−sinα0sinαcosα0001]绕X B 轴转β 角的旋转矩阵为R Y (β)= [cosβ0sinβ10−sinβ0cosβ] 绕Y B 轴转γ角的旋转矩阵为R X (γ)= [1000cosγ−sinγ0sinγcosγ]按照从左向右原则，得到相应旋转矩阵 R=R Z (α) R Y (β) R X (γ)=[cosαcosβcosαsinβsinγ−sinαcosγcosαsinβcosγ+sinαsinγsinαcosβsinαsinβsinγ+cosαcosγsinαsinβcosγ−cosαsinγ−sinβcosβsinγcosβcosγ] 三次绕固定轴旋转所得到的最终姿态与以相反顺序绕运动坐标轴（其本身）旋转的最终姿态相同。

X-Y-Z 固定角与Z-Y-X 欧拉角相同。

X-Y-Z固定角的旋转矩阵为：[cosαcosβcosαsinβsinγ−sinαcosγcosαsinβcosγ+sinαsinγsinαcosβsinαsinβsinγ+cosαcosγsinαsinβcosγ−cosαsinγ−sinβcosβsinγcosβcosγ]3、推导旋转矩阵到X-Y-Z欧拉角的变换表达式其旋转矩阵：R=rotx(γ)*roty(β)*rotz(α) =为了求其逆问题，需要求解旋转矩阵到X-Y-Z 欧拉角的描述表达式。

设姿态矩阵当r23,r33不同时等于零时，由其旋转矩阵可得：tan (γ)=−r23r33即：γ = atan 2(−r23,r33)cosγ∗ r33+sinγ * r23=cosβ，与r13相比得tan (β) =r13cos γ∗ r33+sin γ ∗ r23β= atan 2(r13,cos γ∗ r33+sin γ ∗ r23)同样由其旋转矩阵可得：tan (α)=−r12r11γ = atan 2(−r12,r11)当r23= 0且r33=0时，cos β = 0，即β = ±90°，可解得：β = 90°，γ = 0.0 ，α = atan2(r21, r22)或β = −90°，γ = 0.0，α = atan2(r21, r22)。

4、编写姿态变化代码，使用MATLAB软件打开\\东大机器人实验程序\，机器人空间位姿描述\sia002.slx文件，编写所有转换模块。

参见附件5、利用上述推动的姿态转换表达进行不同姿态描述的求取，并填表。

并利用机器人工具箱求出的结果进行比较。

真机实验步骤：1、操作珞石XB6机器人观察其位置与姿态的数据变化。

读取几组位置和姿态数据并在MATLAB中进行绘图，并于实际机器人末端进行对照。

依据表2-3中不同的构型值，修改模型输入，单击运行按钮，观看输出的旋转矩阵值，并填入到表2-3中。

注意事项：1、旋转矩阵描述法结果唯一，欧拉角、固定角、等效转轴、单位四元数等解不唯一2、手动示教机器人时无法精确到指定关节角，接近指定关节角即可。

具体程序：①XYZ欧拉角至旋转矩阵：function R = ITF(rpy)R = zeros(3,3);rpy = rpy/180*pi;sx = sin(rpy(1));cx = cos(rpy(1));sy = sin(rpy(2));cy = cos(rpy(2));sz = sin(rpy(3));cz = cos(rpy(3));R(1,1) = cy*cz; R(1,2) = -cy*sz; R(1,3) = sy;R(2,1) = cx*sz + cz*sx*sy; R(2,2) = cx*cz - sx*sy*sz; R(2,3) = -cy*sx;R(3,1) = 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(2,3)==0)||( R(3,3) == 0 )rpy(3)=atan2(R(2,1),R(2,2))*180/pi;rpy(2)=90; %rpy(2)=-90;rpy(1)=0;elserpy(1)=atan2(-R(2,3),R(3,3))*180/pi;rpy(2)=atan2(R(1,3),(cos(rpy(3))*R(3,3)-sin(rpy(3))*R(2,3)))*180/pi;rpy(3)=atan2(-R(1,2),R(1,1))*180/pi;endend③ ZYX欧拉角到旋转矩阵function R = ITF(rpy)R = zeros(3,3);rpy=rpy/180*pisx = sin(rpy(1));cx = cos(rpy(1));sy = sin(rpy(2));cy = cos(rpy(2));sz = sin(rpy(3));cz = cos(rpy(3));R(1,1) = cy*cz; R(1,2) =cz*sy*sx-sz*cx ; R(1,3) = cz*sy*cx+sz*sx;R(2,1) = cy*sz; R(2,2) =sz*sy*sx+cz*cy; R(2,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(eul,'ZYX');end④旋转矩阵到ZYX欧拉角function rpy = TF(R)%% outputrpy = zeros(1,3);%% function description%rpy=(rotm2eul(R,'ZYX'))*180/pi; 使用库函数进行计算rpy(2)=atan2(-R(3,1),sqrt(R(1,1)^2+R(2,1)^2));if cos(rpy(2))==0rpy(1)=0;rpy(3)=atan2(R(1,2),R(2,2));rpy(2)=90;elsecb=cos(rpy(2));rpy(1)=atan2(R(2,1)/cb,R(1,1)/cb);rpy(3)=atan2(R(3,2)/cb,R(3,3)/cb);endrpy=rpy*180/pi;end⑤XYZ固定角到旋转矩阵function R = ITF(rpy)%% outputR = zeros(3,3);%% function descriptionrpy = rpy/180*pi;sx = sin(rpy(1));cx = cos(rpy(1));sy = sin(rpy(2));cy = cos(rpy(2));sz = sin(rpy(3));cz = cos(rpy(3));R(1,1) = cy*cx; R(1,2) =cx*sy*sz-cz*sx; R(1,3) = sz*sx+cz*cx*sy; R(2,1) = cy*sx; R(2,2) = cx*cz + sx*sy*sz; R(2,3) =cz*sx*sy -cx*sz;R(3,1) = -sy; R(3,2) = cy*sz; R(3,3) = cy*cz;end⑥旋转矩阵到XYZ固定角function rpy = TF(R)%% outputrpy = zeros(1,3);%% function descriptionrpy(2)=atan2(-R(3,1),sqrt(R(1,1)^2+R(2,1)^2));if cos(rpy(2))==0rpy(1)=0;rpy(3)=atan2(R(1,2),R(2,2));rpy(2)=90;elsecb=cos(rpy(2));rpy(1)=atan2(R(2,1)/cb,R(1,1)/cb);rpy(3)=atan2(R(3,2)/cb,R(3,3)/cb);endrpy=rpy*180/pi;end⑦等效转轴到旋转矩阵function R = ITF(rpy)%% outputR = zeros(3,3);%% function descriptionth=norm(rpy*pi/180);ct=cos(th);st=sin(th);vt=1-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(1,2)=kx*ky*vt-kz*st;R(1,3)=kx*kz*vt+ky*st;R(2,1)=kx*ky*vt+ky*st;R(2,2)=ky*ky*vt+ct;R(2,3)=ky*kz*vt-kx*st;R(3,1)=kx*kz*vt-ky*st;R(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(1,1)+R(2,2)+R(3,3)-1)/2);rpy=[(R(3,2)-R(2,3)) (R(1,3)-R(3,1)) (R(2,1)-R(1,2))]; if sind(th) ~= 0rpy=rpy/(2*sind(th))*th;endend⑨四元素到旋转矩阵function R = ITF(rpy)%% outputR = zeros(3,3);%% function descriptionx=rpy(1)/norm(rpy);y=rpy(2)/norm(rpy);z=rpy(3)/norm(rpy);w=rpy(4)/norm(rpy);R(1,1)=1-2*y*y-2*z*z;R(1,2)=2*(x*y-z*w);R(1,3)=2*(x*z+y*w);R(2,1)=2*(x*y+z*w);R(2,2)=1-2*x*x-2*z*z;R(2,3)=2*(z*y-x*w);R(3,1)=2*(x*z-y*w);R(3,3)=1-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(1,1)+R(2,2)+R(3,3))*0.5;rpy(1)=(R(3,2)-R(2,3))/(4*rpy(4));rpy(2)=(R(1,3)-R(3,1))/(4*rpy(4));rpy(3)=(R(2,1)-R(1,2))/(4*rpy(4)); end。