欧拉角、四元数和旋转矩阵

旋转变换

旋转变换最为直观的表示方法是“轴-角”：绕着某一个过原点轴，旋转某一角度。
轴可以用一个单位长度的点 [w1,w2,w3] 表示：原点到该点的射线即为此轴。
使用右手坐标系，拇指指向轴方向，四指方向即为旋转的方向。
一个旋转变换可以用用欧拉角、四元数或者旋转矩阵表示。以下讨论不同表示方法之间的关系，以及旋转变换的合成、取逆等操作。

旋转矩阵

旋转可以看做一种特殊的坐标变换，而坐标变换可以用用 3×3 矩阵 R 来表示。对一个坐标施加旋转的结果是 x′=Rx 。
旋转矩阵可以在不同坐标系之间进行变换，但不能进行“反演”，即不能在左手系和右手系之间进行变换。
旋转矩阵是正交矩阵，即 |R|=1 ，旋转变换不改变向量的长度。

欧拉角的物理意义

任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。
三个轴可以指固定的世界坐标系轴，也可以指被旋转的物体坐标系的轴。三个旋转轴次序不同，会导致结果不同。
本文中提到的欧拉角指：绕着世界坐标系的x,y,z轴，依次旋转的结果。其取值范围如下：

θx∈(−π,π),θy∈(−π2,π2),θz∈(−π,π)

欧拉角 → 旋转矩阵

单独绕一个轴旋转 θ 角度的旋转矩阵为：

如果依次绕x轴、y轴、z轴旋转，该变换的旋转矩阵为：

R=Rz⋅Ry⋅Rx

记三个轴欧拉角的正弦和余弦函数为 sx,cx,sy,cy,sz,cz 。使用matlab的syms功能可以轻松推导旋转矩阵 R ：

⎡⎣⎢cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czsxcxcy⎤⎦⎥

旋转矩阵 → 欧拉角

设旋转矩阵i行j列元素为 rij 。根据旋转矩阵的表达式，利用三角函数可以推导出欧拉角取值：

θx=atan2(r32,r33)

θy=atan2(−r31,r232+r233−−−−−−−√)

θz=atan2(r21,r11)

四元数的物理意义

设有一个通过原点 [0,0,0] 的旋转轴，该轴上单位长度的点为 [w1,w2,w3] 。绕此轴旋转 θ 角的变换可以用一个向量表示：

[cosθ2,w1sinθ2,w2sinθ2,w3sinθ2]

也记为 q=[q0,q1,q2,q3] ，或者 q=q0+q1i+q2j+q3k 。四元数的模长为1：

q20+q21+q22+q23=cos2θ2+sin2θ2(w21+w22+w33)=cos2θ2+sin2θ2=1

四元数 → 旋转矩阵

此段公式暂不确定，请您参看评论讨论

利用罗德里格旋转公式可以获得绕某过原点一轴 [w1,w2,w3] 旋转某一角度 θ 的旋转矩阵 R ：

⎡⎣⎢cosθ+w21(1−cosθ)w1w2(1−cosθ)−w2sinθw1w2(1−cosθ)cosθ+w22(1−cosθ)w1sinθw2sinθ−w1sinθcosθ⎤⎦⎥

代入四元组的表示方法，可得：

⎡⎣⎢⎢1−2q22−2q232q1q2+2q3q02q1q3−2q2q02q1q2−2q3q01−2q21−2q232q2q3+2q1q02q1q3+2q2q02q2q3−2q1q01−2q21−2q22⎤⎦⎥⎥

旋转矩阵 → 四元数

此段公式暂不确定，请您参看评论讨论

根据旋转矩阵的表达式，利用三角函数性质，可以由旋转矩阵得到四元数：

q0=1+r11+r22+r33−−−−−−−−−−−−−−√2

q1=r32−r234q0

q2=r13−r314q0

q3=r21−r124q0

开根号要求 1+Tr(R)>0 （其中 Tr 表示矩阵的迹，等于对角元素之和，也等于特征值之和），分式要求 q0≠0 。
某些情况下（例如 θ=π ）， q0 接近零， Tr(R) 接近-1，需要使用以下方式求解（来源于此处）。

如果 r11,r22,r33 中， r11 最大：

S=1+r11−r22−r33−−−−−−−−−−−−−−√

q0=r32−r23S

q1=S/4

q2=r12+r21S

q3=r21+r12S

如果 r11,r22,r33 中， r22 最大：

S=1−r11+r22−r33−−−−−−−−−−−−−−√

q0=r13−r31S

q1=r12+r21S

q2=S/4

q3=r23+r32S

如果 r11,r22,r33 中， r33 最大：

S=1−r11−r22+r33−−−−−−−−−−−−−−√

q0=r21−r12S

q1=r13+r31S

q2=r23−r32S

q3=S/4

变换的逆

使用欧拉角表示时，必须颠倒三个旋转轴的顺序，同时对旋转角度取反。
使用旋转矩阵表示时，求矩阵的逆即可： R∗=R−1 。
使用四元组表示时，考虑其物理意义，对后三位取反即可： q−1=[q0,−q1,−q2,−q3] 。

向量的叉乘

接下去讨论之前，需要先复习向量的叉乘。

三维空间中的一个点可以表示为向量 [xi,yj,zk] ，其中 i,j,k 是三个坐标轴： [1,0,0],[0,1,0],[0,0,1] 。
两个向量 a,b 叉乘的结果是一个向量，其长度为 |a||b|sinθ 。 θ 表示从向量 a 到 b 的小于180°的角度。其方向垂直于 a,b 所在平面，遵循右手定则：四指从 a 转向 b ，拇指方向为叉乘结果方向。

四元组作为一种向量，其叉乘涉及到虚数单位的乘法，遵循以下原则：

i×i=−1

i×j=−j×i

i×j=k,j×k=i,k×i=j

叉乘满足反交换律：

a×b=−b×a

联想叉乘的物理意义：交换了 a,b 顺序，则拇指方向反转。

叉乘满足加法分配律：

(a+b)×c=a×c+b×c

特别要注意，叉乘不满足结合律：

(a×b)×c≠a×(b×c)

四元数的叉乘

四元数既不是矢量也不是标量。可以看做一个标量 q0 和一个三维矢量 [q1,q2,q3] 的结合体： q=q0+q1i+q2j+q3k 。
两个四元数 a1+a2i+a3j+a4k ， b1+b2i+b3j+b4k ，相乘的结果 q=a×b ：

q0=a1b1−a2b2−a3b3−a4b4

q1=a1b2+a2b1+a3b4−a4b3

q2=a1b3−a2b4+a3b1+a4b2

q3=a1b4+a2b3−a3b2+a4b1

四元数叉乘不满足交换律、结合律。

变换的组合

使用欧拉角表示时，很难直接组合两个变换。
使用旋转矩阵表示时，依次做矩阵相乘即可： R=R2R1 。
使用四元组表示时，需要对两个四元组叉乘： q=q2×q1 。
注意，先发生的变换要放在乘号右侧。
由于叉乘不满足结合律，当有一系列变化 q1,q2,q3... 陆续发生时，要用括号保证乘法发生的顺序：

q=q4×(q3×(q2×q1))

使用四元数

首先把待旋转的点 (x,y,z) 表示成四元数形式： p=[0,x,y,z] 。这里的四元数不再表示一个旋转，所以模长不一定为1。
对该点 p 施加旋转变换 q ，通过四元组的两次叉乘实现： p′=q×p×q−1=[0,x′,y′,z′] 。

利用matlab的syms功能（参看附录），可以得到对三维点 [x,y,z] 施加变换 [q0,q1,q2,q3] 的结果：

x′=x(q20+q21−q22−q23)+2y(q1q2−q0q3)+2z(q0q2+q1q3)

y′=2x(q0q3+q1q2)+y(q20−q21+q22−q23)+2z(−q0q1+q2q3)

z′=2x(−q0q2+q1q3)+2y(q0q1+q2q3)+z(q20−q21−q22+q23)

附录

以下代码推导对三维点 [x,y,z]
施加变换 [q0,q1,q2,q3] 的结果。

clear;clc;close all;
syms a1 a2 a3 a4 b1 b2 b3 b4
syms q0 q1 q2 q3
syms x y z

% a: rotation, b: 3D vector
a1 = q0;
a2 = q1;
a3 = q2;
a4 = q3;

b1 = 0;
b2 = x;
b3 = y;
b4 = z;

% c = a (*) b
c1 = a1*b1-a2*b2-a3*b3-a4*b4;
c2 = a1*b2+a2*b1+a3*b4-a4*b3;
c3 = a1*b3-a2*b4+a3*b1+a4*b2;
c4 = a1*b4+a2*b3-a3*b2+a4*b1;

% d = conj(a)
d1 = a1;
d2 = -a2;
d3 = -a3;
d4 = -a4;

% e = a (*) b (*) conj(a) = c (*) d
e1 = c1*d1-c2*d2-c3*d3-c4*d4;
e2 = c1*d2+c2*d1+c3*d4-c4*d3;
e3 = c1*d3-c2*d4+c3*d1+c4*d2;
e4 = c1*d4+c2*d3-c3*d2+c4*d1;

expand(e1)
expand(e2)
expand(e3)
expand(e4)