学习过程中涉及欧拉角和旋转矩阵的转换，索性整理学习一下欧拉角四元数和旋转矩阵的概念以及matlab中的互相转换 本文摘自各大课本，博客，自己学习整理使用，侵删 MATLAB矩阵乘法从左到右依次相乘 用R表示旋转矩阵。 yaw（偏航） pitch（俯仰） roll（横滚）分别表示Z Ｙ Ｘ轴的转角。 q=[q0,q1,q2,q3]’表示单位四元数。
1旋转矩阵（方向余弦矩阵）
当确定一个点在空间中的位置后，要确定其姿态才能完全定义该点的位姿。所以采用坐标系{B}相对于坐标系{A}的描述来表示物体姿态，
通俗的讲，是坐标系{B}的各个轴分别与参考坐标系{A}的各个轴的余弦值构成的3×3矩阵，称为旋转矩阵。
欧拉角
1、内旋和外旋
内在旋转与外在旋转的转换关系：互换第一次和第三次旋转的位置则两者结果相同。例如Z-Y-X旋转的内部旋转和X-Y-Z旋转的外部旋转的旋转矩阵相同。
一、绕定轴X-Y-Z旋转（RPY角）（外旋）
假设两个坐标系A和B，二者初始时完全重合。 过程如下：B绕A的X轴旋转γ角，再绕A的Y轴旋转β角，最后绕A的Z轴旋转α角，完成旋转。整个过程，A不动B动。
旋转矩阵的计算方法如下：R = Rz * Ry *Rx，乘法顺序：从右向左，依次旋转X轴Y轴Z轴
其中，cα = cosα，sα = sinα，矩阵相乘，结果如下：
二、绕动轴Z-Y-X旋转（Euler角）（内旋）
过程如下：B绕B的Z轴旋转α角，再绕B的Y轴旋转β角，最后绕B的X轴旋转γ角，完成旋转。整个过程，A不动B动。 旋转矩阵的计算方法如下：R = Rz * Ry *Rx。乘法顺序：从左向右
欧拉角的表示方式比较直观，但是有几个缺点：
(1) 欧拉角的表示方式不唯一。给定某个起始朝向和目标朝向，即使给定yaw、pitch、roll的顺序，也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。比如，同样的yaw-pitch-roll顺序，(0,90,0)和(90,90,90)会将刚体转到相同的位置。这其实主要是由于万向锁(Gimbal Lock)引起的 (2) 欧拉角的插值比较难。 (3) 计算旋转变换时，一般需要转换成旋转矩阵，这时候需要计算很多sin, cos，计算量较大。
欧拉角转旋转矩阵
在计算坐标变换时，旋转更方便的表示形式是旋转矩阵(Rotation Matrix)。三维空间的旋转矩阵可以表示成3×3的矩阵，将欧拉角转换为旋转矩阵的计算方式如下，假设欧拉角yaw、pitch、roll的角度为alpha, beta, gamma，则旋转矩阵可以计算如下：
eul2rotm(eul)中，默认旋转顺序是ZYX,而矩阵相乘顺序是从右到左，先X再Y后Z， 即：若eul(Z Y X)=eul(angle1 angle2 angle3)，则eul2rotm(eul,sequence)=Rot(Z,angle1)*Rot(Y,angle2)*Rot(X,angle3).
rotm2eul() 计算单位为弧度制，计算结果的顺序对应欧拉角旋转轴顺序
deg2rad()%MATLAB角度转弧度函数
% Z Y X
eul=[0 pi/2 0];
yaw=0;
pitch=pi/2'
roll=0;
R_x=[1 0 0;
0 cos(roll) -sin(roll);
0 sin(roll) cos(roll)];
R_y=[cos(pitch) 0 sin(pitch);
0 1 0;
-sin(pitch) 0 cos(pitch)];
R_z=[cos(yaw) -sin(yaw) 0;
sin(yaw) cos(yaw) 0;
0 0 1];
R=R_z*R_y*R_x;%欧拉角转旋转矩阵
R2=angle2dcm(eul,'ZYX');%由欧拉角转方向余弦矩阵
R2=eul2rotm(eul,'ZYX');%欧拉角转旋转矩阵
四元数（后续补充）
一篇介绍四元数的文章 在Matlab里，可以用quatmultiply计算四元数乘法，用quatinv来计算四元数的逆，用quatconj来计算四元数的共轭。四元数的旋转和旋转矩阵的旋转可以由以下matlab代码验证：
% Matlab code by MulinB, Aerospace Toolbox is needed
pt = [10,20,30]; % point coordinate
yaw = 45;
pitch = 30;
roll = 60;
q = angle2quat(yaw/180*pi,pitch/180*pi,roll/180*pi);
R = angle2dcm(yaw/180*pi,pitch/180*pi,roll/180*pi);
pt1 = R*pt';
pt2 = quatmultiply(quatconj(q), quatmultiply([0,pt],q)); % NOTE the order
disp(pt1');disp(pt2(2:4));
从上述代码里也可以看到四元数和欧拉角和dcm的转换，在matlab里可以很方便的用quat, dcm, angle之间的转换来任意互转。另外，从四元数计算axis和angle，可以用以下代码计算：
% Matlab code by MulinB, Compute the axis and angle from a quaternion
function [axis, theta] = quat2axisangle(q)
theta = acos(q(1)) * 2;
axis = q(2:4)/sin(theta/2);
总结：
转欧拉角：
旋转矩阵转欧拉角 [r2,r2,r3]=dcm2angle(R, S) eul=rotm2rul(R,S) 注：得到的结果为弧度，若需要角度需进一步转化 四元数转欧拉角 [r1,r2,r3]=quat2angle([q0 q1 q2 q3]，S) 注：S 的选择有12种，【‘ZYX’,‘ZYZ’,‘ZXY’,‘ZXZ’,‘YXZ’,‘YXY’,‘YZX’,‘YZY’,‘XYZ’,‘XYX’,‘XZY’,‘XZX’】 S 默认 ‘ZYX’
转旋转矩阵
四元数转旋转矩阵 R=quat2dcm([q0 q1 q2 q3]) 欧拉角转旋转矩阵 R=angle2dcm(r1,r2,r3,S); R=angle2dcm(yaw/180pi,pitch/180pi,roll/180*pi) 注：根据欧拉角是弧度/角度，选择以上操作
转四元数
旋转矩阵转四元数 q =dcm2quat®； 欧拉角转四元数 q=angle2quat(r1,r2,r3，S)；
clear all;
close all;
clc;
%欧拉角
x = 0.5;
y = 0.6;
z = 0.7;
Ang1 = [x y z];
%欧拉角转旋转矩阵
Rx = [1 0 0;
cos(x) -sin(x);
sin(x) cos(x)];
Ry = [cos(y) 0 sin(y);
1 0;
-sin(y) 0 cos(y)];
Rz = [cos(z) -sin(z) 0;
sin(z) cos(z) 0;
0 1];
R = Rz*Ry*Rx;
R1 = R;
%旋转矩阵转欧拉角
x = atan2(R(3,2),R(3,3));
y = atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2));
z = atan2(R(2,1),R(1,1));
Ang2 = [x y z];
%旋转矩阵转四元数
t=sqrt(1+R(1,1)+R(2,2)+R(3,3))/2;
q=[t (R(3,2)-R(2,3))/(4*t) (R(1,3)-R(3,1))/(4*t) (R(2,1)-R(1,2))/(4*t)];
Q1 = q;
%四元数转旋转矩阵
R=[ 2*q(1).^2-1+2*q(2)^2 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3));
2*(q(2)*q(3)+q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2 2*(q(3)*q(4)-q(1)*q(2));
2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2];
R2 = R;
%欧拉角转四元数
q = [cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2) ...
sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2) ...
cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2) ...
cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)];
Q2 = q;
%四元数转欧拉角
x = atan2(2*(q(1)*q(2)+q(3)*q(4)),1 - 2*(q(2)^2+q(3)^2));
y = asin(2*(q(1)*q(3) - q(2)*q(4)));
z = atan2(2*(q(1)*q(4)+q(2)*q(3)),1 - 2*(q(3)^2+q(4)^2));
Ang3 = [x y z];
Ang1
Ang2
Ang3
R1
R2
Q1
Q2
