MATLAB坐标系变换（2）---逃不过的四元数

1、前记：我是来恶补基础知识的。

       时隔写下人生中第一篇博客MATLAB下坐标系变换（1）已有十个月之久。发现自己到现在还不是真正的搞研究的样子。一方面看往期所留下的博文大多数可看成是搬砖的成分（我只是个老实本分的搬砖人 ^-^//），再有就是自己理论知识太欠缺了，特别是数学方面。苦于挣扎，但实际又做不出太大的改变，只能继往开来拿大神们的文章在这里站位‘作威作福’。

2、参看文章：四篇大同小异，看--理解

    （1）http://mini.eastday.com/mobile/180306210610472.html#

    （2）https://www.cnblogs.com/21207-iHome/p/6894128.html

    （3）https://blog.csdn.net/xiaoma_bk/article/details/79082629

    （4）https://blog.csdn.net/lql0716/article/details/72597719

3、以下函数方便运行时对照理解（函数来自Peter教授的机器人工具箱）

     ---使用过程----复制到脚本编辑器中，按小结运行。

%% 平移与旋转的组合 书---机器人学、视觉与控制
%% （1）rotx函数，关于x轴做旋转
R=rotx(0)%左手坐标系下，中指对着自己为x轴，z轴为拇指
%% 
%  角度
R1=rotx(45,'deg')
%  弧度
R2=rotx(pi/2)
%% 绘制 从R原坐标到R1绕x轴转45度，再到R2绕x轴转90度
trplot(R)
pause(3);
hold on
trplot(R1,'color','r')
pause(3);
trplot(R2,'color','g')
%% 或者如在
% 绘制所有动画过程
tranimate(R2) %初始坐标绕x轴旋转90度
%% （2）tr2eul函数，齐次变换矩阵--欧拉角；对应的ZYZ欧拉角（1x3）齐次变换T（4×4）的旋转部分。
%  These correspond to rotations about the Z,Y, Z axes respectively
T=transl(1,0,0)*trotx(pi/2)*transl(0,1,0)
tranimate(T)
%% 绘制对应坐标系
trplot(T)
Eul = tr2eul(T)
%% 提取矩阵T中的旋转部分、平移部分
t2r(T)
transl(T)'
%% （3）eul2tr函数：onvert Euler angles to homogeneous transform欧拉-齐次矩阵
%  These correspond to rotations about the Z,Y, Z axes respectively
T0=eul2tr(0,0,0) %左手坐标系下，中指对着自己为x轴，z轴为拇指
T1=eul2tr(pi/6,0,0) % z轴
T2=eul2tr(pi/4,0,0)
T3=eul2tr(pi/2,0,0)
trplot(T0)
pause(2);
hold on
trplot(T1,'color','r')
pause(2);
trplot(T2,'color','g')
pause(2);
trplot(T3,'color','r')
%% z---y---z ????绕z绕y绕z？？？
T01=eul2tr(pi/2,0,0)
T02=eul2tr(0,0,pi/2)
figure
trplot(T0)
hold on
pause(2);
trplot(T01,'color','r')%绕z
pause(2);
trplot(T02,'color','g')%绕z
%% （4）tr2rpy函数： Convert a homogeneous transform to roll-pitch-yaw angles齐次---rpy
% 格式 RPY = tr2rpy(T, options) 
%   Options::
%    'deg'   Compute angles in degrees (radians default)
%    'xyz'      Return solution for sequential rotations about X, Y, Z axes
%    'zyx'      Return solution for sequential rotations about Z, Y, X axes (default)
%    'yxz'      Return solution for sequential rotations about Y, X, Z axes
%    'arm'      Return solution for sequential rotations about X, Y, Z axes
%    'vehicle'  Return solution for sequential rotations about Z, Y, X axes
%    'camera'   Return solution for sequential rotations about Y, X, Z axes
rpy=tr2rpy(T0)%默认z，y，x
rpy1=tr2rpy(T2)
rpy2=tr2rpy(T3)
 figure
trplot(T0) %左手坐标系下，中指对着自己为x轴，z轴为拇指,作为初始坐标系
pause(2);
hold on
trplot(T2,'color','g')
pause(2);
trplot(T3,'color','r')
%% 
trplot(T0) %左手坐标系下，中指对着自己为x轴，z轴为拇指,作为初始坐标系

%% (5) rpy2tr Roll-pitch-yaw angles to homogeneous transform,rpy--齐次
 %格式：T = rpy2tr(ROLL, PITCH, YAW, OPTIONS) 
 %Options::同上
 Ta=rpy2tr(pi/6,0,0)%30度
 Tb=rpy2tr(pi/3,0,0) %60度
 Tc=rpy2tr(pi/2,0,0) %90度
  figure
 %T0开始--Ta--Tb--Tc,默认zyx则旋转次序为先X--Y---Z
trplot(T0) %左手坐标系下，中指对着自己为x轴，z轴为拇指,作为初始坐标系
pause(2);
hold on
trplot(Ta,'color','g')
pause(2);
trplot(Tb,'color','r') 
pause(2);
trplot(Tc,'color','g') 
% tranimate(Ta) %整个动画过程
% tranimate(Tb)
% tranimate(Tc)
%%
Tz1=Ta% 因为T0是单位矩阵所以T0*Ta就是Ta
Tz2=Tz1*Tb
Tz3=Tz2*Tc
figure
trplot(T0)%从初始坐标开始
hold on
pause(2);
trplot(Tz1,'color','r')
pause(3);
trplot(Tz2,'color','g')
pause(2);
trplot(Tz3,'color','r') 
tranimate(Tz1)
tranimate(Tz2)
tranimate(Tz3)
%% 默认zyx则旋转次序为先X--Y---Z
 Ta1=rpy2tr(pi/2,0,0)%绕x90度
 Tb1=rpy2tr(0,pi/2,0) %绕y
 Tc1=rpy2tr(0,0,pi/2)%raoz
 %T0开始--Ta--Tb--Tc
trplot(T0) %左手坐标系下，中指对着自己为x轴，z轴为拇指,作为初始坐标系
figure
trplot(Ta1,'color','g')%初始坐标绕x轴旋转90度
figure
trplot(Tb1,'color','r')%初始坐标绕y轴旋转90度
figure
trplot(Tc1,'color','g')%初始坐标绕z轴旋转90度  
tranimate(Ta1)
tranimate(Tb1)
tranimate(Tc1)
%% 选项为xyz则旋转次序为先z--Y---x
 Ta2=rpy2tr(pi/2,0,0,'xyz')
 Tb2=rpy2tr(0,pi/2,0,'xyz') 
 Tc2=rpy2tr(0,0,pi/2,'xyz')
 %T0开始--Ta--Tb--Tc
trplot(T0) %左手坐标系下，中指对着自己为x轴，z轴为拇指,作为初始坐标系
figure
trplot(Ta2,'color','g')%初始坐标绕x轴旋转90度
figure
trplot(Tb2,'color','r')%初始坐标绕y轴旋转90度
figure
trplot(Tc2,'color','g')%初始坐标绕z轴旋转90度  
%% (6)quaternion - quaternion array四元数组
% 四元数是四部分超复数，可用于表示三维旋转。
% 四元数是一个数字
% a + b * i + c * j + d * k
% 其中i * i = j * j = k * k = i * j * k = -1
%       格式：
%        Q = quaternion(A,B,C,D)创建一个四元数组四元数部分取自阵列A，B，C和D.
%        所有的输入必须具有相同的大小并且属于同一类
%        Q = quaternion(M)从N-by-4创建N-by-1四元数阵列矩阵M，其中每列成为四元数的一部分。
%        Q = quaternion(RV, 'rotvec') 从中创建一个N乘1的四元数阵列N-by-3矩阵RV。 
%        RV的每一行代表[X Y Z]元素旋转矢量。旋转矢量表示三维轴旋转，其中幅度对应于旋转角度弧度。
%        Q = quaternion(R, 'rotmat', PF) 创建N×1四元数阵列来自旋转矩阵的数组R. R可以是3乘3或3乘3N
%        如果欧拉角代表点，PF可以是“point”用于框架旋转的旋转或“frame”。
%        Q = quaternion(E, 'euler', CV, PF) 
Q=[0.1461 0.9889 0.30125 0.4026];%分别表示四元数的实部与虚部
Rq= quaternion(Q)  
Rq1= quaternion([0 0 1 0])  
%% （7）rotmat函数：将四元数转换为旋转矩阵或方向余弦矩阵
% %格式：
%         R = rotmat(Q,'frame')将标量四元数Q转换为3乘3
% ???? 的等效旋转矩阵，适用于框架旋转。
% ?
% ???? R = rotmat（Q，'point'）将标量四元数Q转换为3乘3
% ???? 的等效旋转矩阵，适用于点旋转。
Q2R= rotmat(Rq,'frame')
Q2R1= rotmat(Rq1,'point')
tranimate(Q2R)
figure
tranimate(Q2R1)
%% 将四元数转换成欧拉角？？？
% 有工具箱函数可调用quat2angle等
               %-----己亥年二月初五

4、在Robotics System Toolbox中的有关坐标转换的函数参看一下：

       https://au.mathworks.com/help/robotics/coordinate-system-transformations.html