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列的矩阵
这是个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);%将矩阵11列的数据赋值给变量
r12 = Rxyz(1,2);%将矩阵12列的数据赋值给变量
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 的结果:
在这里插入图片描述
在这里插入图片描述

在这里插入图片描述

  • 10
    点赞
  • 16
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值