1、旋转矩阵转欧拉角

        旋转矩阵转换为欧拉角在机器人运动学中，刚体坐标系的变换通常都是通过旋转矩阵进行表示，而在用户窗口则是通过欧拉角显示，而欧拉角存在旋转顺序不同、万向锁等问题。目前KUKA、ABB机器人欧拉角旋转顺序均为ZYX，因此默认欧拉角旋转顺序为ZYX。当B=±90°时，万向锁会导致失去一个自由度，即A和C的角度无法唯一确定，在机器人运动控制过程中会发生突变。

        因此，通过阈值判定处理，当B接近±90°时，即|cos(B)|<10^(-6)时,对欧拉角进行特殊处理，代码如下。

% 定义末端位姿矩阵
% 机器人关节角度为[60°,-25°,40°,45°,-20°,60°]
T06=[-0.6992     0.1693    0.6946    385
      0.6781    -0.1505    0.7194    625.7  
      0.2263     0.9740   -0.0096    983.9
      0          0         0         1   ];

% 机器人关节角度为[0°,0°,0°,0°,0°,0°]
% T06=[0     0     1     1030
%     0     1     0     0
%    -1     0     0     1260  
%     0     0     0     1  ];

r11=T06(1,1);r12=T06(1,2);r13=T06(1,3);
r21=T06(2,1);r22=T06(2,2);r23=T06(2,3);
r31=T06(3,1);r32=T06(3,2);r33=T06(3,3);

% 计算B
B=atan2(-r31,sqrt(r32*r32+r33*r33));
% 判定abs(cos(B))的阈值
if((abs(cos(B))<0.000001))
    A=0;
    C=atan2(r32, r33);
else
    A=atan2(r21, r11);
    C=atan2(r32, r33);
end
Euler=[A B C]*57.3;

        机器人关节角度分别为[60°, -25°, 40°, 45°, -20°, 60°] 和[0°, 0°, 0°, 0°, 0°, 0°]时，matlab计算数值与ABB机器人对比如下：

2、旋转矩阵转四元数

        旋转矩阵到四元数的转换公式如下，在转换过程中，分母为0或者值非常小，会导致四元数求解不稳定，因此需要先比较筛选出最大值，再进行计算。

% 定义末端位姿矩阵
% 机器人关节角度为[60°,-25°,40°,45°,-20°,60°]
T06=[-0.6992     0.1693    0.6946    385
      0.6781    -0.1505    0.7194    625.7  
      0.2263     0.9740   -0.0096    983.9
      0          0         0         1   ];

% 机器人关节角度为[0°,0°,0°,0°,0°,0°]
%T06=[0     0     1     1030
%     0     1     0     0
%    -1     0     0     1260  
%     0     0     0     1  ];

r11=T06(1,1);r12=T06(1,2);r13=T06(1,3);
r21=T06(2,1);r22=T06(2,2);r23=T06(2,3);
r31=T06(3,1);r32=T06(3,2);r33=T06(3,3);

% 筛选四个元素最大值
W=r11+r22+r33;
X=r11-r22-r33;
Y=r22-r11-r33;
Z=r33-r11-r22;
k=0; 
value=W;
if(X>value)
    value=X;
    k=1;
end
if(Y>value)
    value=Y;
    k=2;
end
if(Z>value)
    value=Z;
    k=3;
end

% 以最大值为分母计算四元数
max=sqrt((value+1))/2;
switch(k)
    case 0
        q1=max;
        q2=(r32-r23)/4/max;
        q3=(r13-r31)/4/max;
        q4=(r21-r12)/4/max;
    case 1
        q1=(r32-r23)/4/max;
        q2=max;
        q3=(r21+r12)/4/max;
        q4=(r13+r31)/4/max;
     case 2
        q1=(r13-r31)/4/max;
        q2=(r12+r21)/4/max;
        q3=max;
        q4=(r23+r32)/4/max;
     case 3
        q1=(r21-r12)/4/max;
        q2=(r31+r13)/4/max;
        q3=(r23+r32)/4/max;
        q4=max;      
end
q=[q1 q2 q3 q4];