MATLAB---机器人学导论学习笔记（2）之 XYZ固定角与旋转矩阵的相互转换

什么是 XYZ固定角?
XYZ角又可以称作回转角、俯仰角、偏转角。
通俗点来说,有A和B两个坐标系，A和B坐标系都有X,Y,Z轴。
首先，将A和B两个坐标系完全重合，其中A作为参考坐标系不动，将B坐标系这一整个整体先绕A坐标系的X轴旋转角度a度，在绕Y轴旋转角度b度，最后绕Z轴角度c度。
！！每次旋转都是绕着固定参考系A的轴进行的！！这种姿态表示法就是“X-Y-Z固定角”

什么是 旋转矩阵？
首先我们得知道 “位置和姿态”这两个概念，位置和姿态又可以合称为位姿。

位置 ：我们可以用一个3*1的位置矢量描述一个xyz坐标系内任意一个点定位。

这是个3行1列的矩阵，第一行是x方向上的坐标，第二行是y方向的坐标，第三行是z方向上的坐标。

姿态 ：点的位置可以用矢量表示，而物体的姿态可以用固定在物体上的坐标系来表示。
如下图所示，固定在末端执行器上的B坐标系。
我们用
来表示坐标系B的主轴方向上的单位矢量。当以A坐标系为参考坐标系时，他们被写成以下形式

我们很容易将以上三个矢量用一个3*3的矩阵按顺序表示出来，这个矩阵就是旋转矩阵。

XYZ固定角 —>旋转矩阵

a = input('请输入角度a的值：');
b = input('请输入角度b的值：');
c = input('请输入角度c的值：');


Rz = [cosd(c) ,-sind(c) ,0;
      sind(c) ,cosd(c)  ,0;   
      0       ,0        ,1;];      %绕Z轴旋转

Ry = [cosd(b) ,0        ,sind(b);
      0       ,1        ,0      ;   
     -sind(b) ,0        ,cosd(b);];    %绕Y轴旋转

Rx = [1       ,0        ,0       ;
      0       ,cosd(a)  ,-sind(a);
      0       ,sind(a)  ,cosd(a) ;];     %绕X轴旋转


Rxyz = Rz *Ry *Rx;

disp(Rxyz);

在代码中，值得注意的是：
sind（） 括号里填写的是角度值，例如，30、45、60
而 sin（） 括号里填写的是弧度值，例如，30 * pi/180、45 * pi/180、60 * pi/180
（pi 是π的意思）

以下是结果代码：

当我们利用固定角求出了旋转矩阵，又怎么通过旋转矩阵转角度呢？

旋转矩阵 —> XYZ固定角

这里我为了方便，将上一个代码复制过来直接生成一个旋转矩阵

a = input('请输入角度a的值：');
b = input('请输入角度b的值：');
c = input('请输入角度c的值：');


Rz = [cosd(c) ,-sind(c)  , 0;
      sind(c) , cosd(c)  , 0;   
      0       ,0         , 1;];

Ry = [cosd(b) ,0        ,sind(b);
      0       ,1        ,0      ;   
     -sind(b) ,0        ,cosd(b);];

Rx = [1       ,0        ,0       ;
      0       ,cosd(a)  ,-sind(a);
      0       ,sind(a)  ,cosd(a) ;];


Rxyz = Rz *Ry *Rx;
disp(Rxyz);
 
 %以上都是上一个代码的内容，以下才是重点   

r11 = Rxyz(1,1);%将矩阵1行1列的数据赋值给变量
r12 = Rxyz(1,2);%将矩阵1行2列的数据赋值给变量
r13 = Rxyz(1,3);

r21 = Rxyz(2,1);
r22 = Rxyz(2,2);
r23 = Rxyz(2,3);

r31 = Rxyz(3,1);
r32 = Rxyz(3,2);
r33 = Rxyz(3,3);


if sind(b) == 1
   b = 90;
   a = 0;
   cc = atan2(r12,r22);
   c = rad2deg(cc);

   disp(a)
   disp(b)
   disp(c)
end

if sind(b) == -1
   b = -90;
   a = 0;
   cc = -atan2(r12,r22);
   c = rad2deg(cc);

   disp(a)
   disp(b)
   disp(c)
end

if sind(b) > -1 && sind(b) < 1
ee = atan2(r32/cosd(b),r33/cosd(b));
e =rad2deg(ee)


ddd = r11*r11+r21*r21;
dd = atan2(-r31,sqrt(ddd));
d =rad2deg(dd)

gg = atan2(r21/cosd(b),r11/cosd(b));
g =rad2deg(gg)
end

%x对应ee角，y对应dd角，z对应gg角
%sqrt函数表示根号，在命令行窗口中输入sqrt(9)，会得到3

主要掌握第三个 if 的一般算法，前面两个 if 是为了保证得出所有的解，当你不设条件语句，直接用第三个 if 中的代码计算：50、90、40，当y角为90时，x角永远是0，而且得不到正确结果。如下图：

第三个if中计算的情况必须保证 -90< y <90 计算结果才是正确的。

以下是三个 if 的结果：