什么是 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 的结果: