一、根据公式，自己编写函数

S表示旋转顺序，我习惯上将‘XYZ’定义为“先旋转X，再旋转Y，最后旋转Z”，所以最后的R=Rz*Ry*Rx(特别注意这里！不同的地方表示的含义不一样，有些工具(比如Matlab)里面'XYZ'，表示的是“先旋转Z，再旋转Y，最后旋转X”,所以最后的R=Rx*Ry*Rz)

注意：区分角度的单位是度还是弧度！deg2rad()是Matlab自带的角度转弧度的函数！

二、Matlab自带的函数eul2rotm()

官方文档：https://ww2.mathworks.cn/help/robotics/ref/eul2rotm.html

特别注意红框里面的两处(之前想当然的认为Matlab里面的XYZ跟自己习惯的一样，然而并不是，英语好点的话也不会折腾这么久了......)

第一处：Matlab中S='XYZ'的意义是指“先旋转Z，再旋转Y，最后旋转X”；

第二处：调用函数的时候，参数1也就是eul(1*3的行向量)里面的三个数，顺序要跟S对应上，

S='XYZ'时，eul=[angleX angleY angleZ]

S='ZYX'时，eul=[angleZ angleY angleX]

......

三、关于Direction Cosine Matrix函数angle2dcm()

参考：https://en.wikiversity.org/wiki/PlanetPhysics/Direction_Cosine_Matrix

R_dcm=angle2dcm(angleZ, angleY ,angleX,'ZYX');

R_rotm=eul2rotm(eul,'ZYX');(其中eul=[angleZ angleY angleX])

运行结果：

可以发现：二者是一个逆(正交单位阵，转置=逆)的关系，因此，如果eul2rotm结果描述的旋转是A->B的旋转，那么angle2dcm结果描述的是B->A的旋转！注意这一点即可！

参考：

[1]https://blog..net/zzcoming/article/details/52065276

[2]https://blog..net/u012423865/article/details/78219787

学习过程中涉及欧拉角和旋转矩阵的转换，索性整理学习一下欧拉角四元数和旋转矩阵的概念以及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

clear

clc

vartheta_c=0.1;

fea_c=0.1;

gamma_c=0;

q1=sin(fea_c/2)*sin(vartheta_c/2)*cos(gamma_c/2)+cos(fea_c/2)*cos(vartheta_c/2)*sin(gamma_c/2);

q2=sin(fea_c/2)*cos(vartheta_c/2)*cos(gamma_c/2)+cos(fea_c/2)*sin(vartheta_c/2)*sin(gamma_c/2);

q3=cos(fea_c/2)*sin(vartheta_c/2)*cos(gamma_c/2)-sin(fea_c/2)*cos(vartheta_c/2)*sin(gamma_c/2);

q4=cos(fea_c/2)*cos(vartheta_c/2)*cos(gamma_c/2)-sin(fea_c/2)*sin(vartheta_c/2)*sin(gamma_c/2);

Dt=0.001;

n=1;

t=0;

for i=1:5000

if t<1.5

vartheta_c=vartheta_c;

fea_c=fea_c;

gamma_c=gamma_c;

else

vartheta_c=vartheta_c-Dt*pi/180;

fea_c=fea_c+Dt*pi/180;

gamma_c=gamma_c+Dt*pi/180;

end

q1_c=sin(fea_c/2)*sin(vartheta_c/2)*cos(gamma_c/2)+cos(fea_c/2)*cos(vartheta_c/2)*sin(gamma_c/2);

q2_c=sin(fea_c/2)*cos(vartheta_c/2)*cos(gamma_c/2)+cos(fea_c/2)*sin(vartheta_c/2)*sin(gamma_c/2);

q3_c=cos(fea_c/2)*sin(vartheta_c/2)*cos(gamma_c/2)-sin(fea_c/2)*cos(vartheta_c/2)*sin(gamma_c/2);

q4_c=cos(fea_c/2)*cos(vartheta_c/2)*cos(gamma_c/2)-sin(fea_c/2)*sin(vartheta_c/2)*sin(gamma_c/2);

vartheta=asin(2*(q1_c*q2_c+q3_c*q4_c));

fea=-atan(2*(q1_c*q3_c-q4_c*q2_c)/(q1_c^2-q2_c^2-q3_c^2+q4_c^2));

gamma=-atan(2*(q2_c*q3_c-q4_c*q1_c)/(-q1_c^2+q2_c^2-q3_c^2+q4_c^2));

q1=sin(fea/2)*sin(vartheta/2)*cos(gamma/2)+cos(fea/2)*cos(vartheta/2)*sin(gamma/2);

q2=sin(fea/2)*cos(vartheta/2)*cos(gamma/2)+cos(fea/2)*sin(vartheta/2)*sin(gamma/2);

q3=cos(fea/2)*sin(vartheta/2)*cos(gamma/2)-sin(fea/2)*cos(vartheta/2)*sin(gamma/2);

q4=cos(fea/2)*cos(vartheta/2)*cos(gamma/2)-sin(fea/2)*sin(vartheta/2)*sin(gamma/2);

q=[q1;q2;q3;q4];

vartheta_store(:,n)=[vartheta_c;vartheta];

fea_store(:,n)=[fea_c;fea];

gamma_store(:,n)=[gamma_c;gamma];

q_store(:,n)=[q1_c;q2_c;q3_c;q4_c;q];

n=n+1;

t=t+Dt;

end

figure(1)

plot((1:n-1)*Dt,gamma_store(1,:)*180/pi,(1:300:n-1)*Dt,gamma_store(2,1:300:end)*180/pi,'r+')

legend('Original','Transformed')

xlabel('time[s]')

ylabel('\gamma[Deg]')

figure(2)

plot((1:n-1)*Dt,vartheta_store(1,:)*180/pi,(1:300:n-1)*Dt,vartheta_store(2,1:300:end)*180/pi,'r+')

legend('Original','Transformed')

xlabel('time[s]')

ylabel('\theta[Deg]')

figure(3)

plot((1:n-1)*Dt,fea_store(1,:)*180/pi,(1:300:n-1)*Dt,fea_store(2,1:300:end)*180/pi,'r+')

legend('Original','Transformed')

xlabel('time[s]')

ylabel('\psi[Deg]')

figure(4)

plot((1:n-1)*Dt,q_store(1,:),(1:300:n-1)*Dt,q_store(5,1:300:end),'r+')

legend('q_1_Ori','q_1_Tran')

xlabel('时间/s')

ylabel('q_1')

figure(5)

plot((1:n-1)*Dt,q_store(2,:),(1:300:n-1)*Dt,q_store(6,1:300:end),'r+')

legend('q_2_Ori','q_2_Tran')

xlabel('时间/s')

ylabel('q_2')

figure(6)

plot((1:n-1)*Dt,q_store(3,:),(1:300:n-1)*Dt,q_store(7,1:300:end),'r+')

legend('q_3__Ori','q_3_Tran')

xlabel('时间/s')

ylabel('q_3')

figure(7)

plot((1:n-1)*Dt,q_store(4,:),(1:300:n-1)*Dt,q_store(8,1:300:end),'r+')

legend('q_4__Ori','q_4_Tran')

xlabel('时间/s')

ylabel('q_4')