四元数(Quaternion)是用于三维旋转和定向的四部分组成的超复数，超复数简单理解就是比a+bi这样的复数更复杂的复数，其中a+bi这样的复数我们也可以叫做二元数，表示复平面的一点，对于熟悉欧拉公式的朋友就知道，也可以看成是平面上的旋转。
由于二元数是在二维平面的旋转，于是哈密顿就在想是否可以找到一个表示三维空间旋转的复数呢？于是这个四元数就诞生了。
四元数的表示形式为：a +bi+cj+dk（或者w+xi+yj+zk），其中a、b、c、d是实数，i、j、k是三个虚数单位，满足 i² = j² = k² = ijk =−1，每个四元数都是a和i, j, k的线性组合。
因为四元数非常适合表示三维空间的旋转，跟旋转矩阵相比，四元数是没有奇异点的，也就是说两个不同的旋转是不可能存在同样的四元数，这样就避免了机器人中的“万向节死锁”的问题，万向节死锁的本质就是少了一个自由度，比如两个轴重合了，这样就无论怎么旋转都达不到预期了，而四元数不会出现这样的情况，所以特别好使。
1、概述
我们来看下i、j、k的旋转对应着XYZ的哪根轴旋转。
RPY分别表示翻滚角(Roll)、俯仰角(Pitch)、偏航角(Yaw)，这几个很好理解，比如开飞机的时候，战斗机在空中旋转，如果是绕X轴旋转，就叫做翻滚；如果是绕Y轴旋转，比如起飞降落这样的情况就叫做俯仰；偏航角就是绕Z轴旋转，这个很简单，就是左右打方向盘。
i旋转代表Z轴与Y轴相交平面中Z轴正向向Y轴正向的旋转。两条轴就是两条直线，就确定一个面，所以就是Z轴向Y轴旋转，那本质就是绕X轴旋转，对应着Roll翻滚角。
j旋转代表X轴与Z轴相交平面中X轴正向向Z轴正向的旋转，就是绕Y轴旋转，对应着Pitch俯仰角。
k旋转代表Y轴与X轴相交平面中Y轴正向向X轴正向的旋转，就是绕Z轴旋转，对应着Yaw偏航角。
简单来说ijk对应着rpy
2、四元数性质
我们先来熟悉四元数具备哪些性质：
i² = j² = k² = ijk =−1
ij = k
ji = -kjk = i,kj = -i
ki = j,ik = -j这个很好理解，跟刚学习时的虚数i的计算是一样的，但是请注意，四元数是没有乘法交换律的，所以上面的ij≠ji，这个跟矩阵类似，A*B和B*A是不一样的。
四元数的L2范数：a²+b²+c²+d²，四个平方和的平方根，也就是它的模。
加减乘除
我们来计算两个四元数的加减乘除：
% 定义四元数
x=quaternion(3,1,0,0);
% 3 + 1i + 0j + 0k
y=quaternion(0,5,1,-2);
% 0 + 5i + 1j - 2k% x取模（L2范数）
x.norm() 或者 norm(x)
% 3.1623
% x的共轭四元数
x.conj()
% 3 - 1i + 0j + 0k
y.conj()
% 0 - 5i - 1j + 2k共轭复数就是实部不变，虚部是相反数。
%加法
x+y
% 3 + 6i + 1j - 2k
%减法
x-y
% 3 - 4i - 1j + 2k%计算x*y
x*y
% -5 + 15i + 5j - 5k
% 乘法不遵循交换律
y*x
% -5 + 15i + 1j - 7k%除法分为左除和右除
%左除
x.\y
% 0.5 + 1.5i + 0.1j - 0.7k
%右除
x./y
% 0.16667 - 0.5i - 0.16667j + 0.16667k3、四元数构造
我们先来熟悉如何自己来构造四元数：
a=1;b=2;c=3,d=4;
quat = quaternion(a,b,c,d);
% 1 + 2i + 3j + 4k
%虽然这里我是用的是整数，但四元数的类型是double型
classUnderlying(quat)
% 'double'也可以使用四元数组来构造：
A = [1.1;1.2];
B = [2.1;2.2];
C = [3.1;3.2];
D = [4.1;4.2];
quaternion(A,B,C,D)
%得到2x1的四元数组
1.1 + 2.1i + 3.1j + 4.1k
1.2 + 2.2i + 3.2j + 4.2kA = [1.1,1.3;1.2,1.4];
B = [2.1,2.3; 2.2,2.4];
C = [3.1,3.3; 3.2,3.4];
D = [4.1,4.3; 4.2,4.4];
quatMatrix=quaternion(A,B,C,D)
%得到2x2的四元数矩阵
1.1 + 2.1i + 3.1j + 4.1k 1.3 + 2.3i + 3.3j + 4.3k
1.2 + 2.2i + 3.2j + 4.2k 1.4 + 2.4i + 3.4j + 4.4k
%转成Nx4的系数矩阵
compact(quatMatrix)
1.1000 2.1000 3.1000 4.1000
1.2000 2.2000 3.2000 4.2000
1.3000 2.3000 3.3000 4.3000
1.4000 2.4000 3.4000 4.4000还可以构造Nx1的随机四元数矩阵
quaternion(randn(5,4))
%结果类似
1.4384 - 0.10224i - 0.030051j - 0.86365k
0.32519 - 0.24145i - 0.16488j + 0.077359k
-0.75493 + 0.31921i + 0.62771j - 1.2141k
1.3703 + 0.31286i + 1.0933j - 1.1135k
-1.7115 - 0.86488i + 1.1093j - 0.0068493k
4、旋转向量与四元数
旋转向量包括旋转角度或者弧度，它们之间的相互转换如下：
% 绕X轴旋转60°
d1 = [60,0,0];
quat = quaternion(d1,'rotvecd')
%0.86603 + 0.5i + 0j + 0k
% 绕Y轴旋转60°
d2 = [0,60,0];
quat = quaternion(d2,'rotvecd')
%0.86603 + 0i + 0.5j + 0k
% 绕Z轴旋转60°
d3 = [0,0,60];
quat = quaternion(d3,'rotvecd')
%0.86603 + 0i + 0j + 0.5k
d4 = [60,60,60];
quat = quaternion(d4,'rotvecd')
%0.61619 + 0.45472i + 0.45472j + 0.45472k到d4的时候，这个结果是怎么来的，就有了疑问，我们来看下计算过程。
三维旋转向量为：v=[vx,vy,vz]，θ=||v||，于是就得到公式如下：
四元数=(cos(θ/2),sin(θ/2)*vx/θ,sin(θ/2)*vy/θ,sin(θ/2)*vz/θ)
还是画一张图来更直观的了解下其推导过程：
从图中我们通过复平面的欧拉公式，可以很容易地将旋转向量转换成为一个四元数，其中角度拆分成一半，这样就算是180度重合，也是没有问题的，可能也有基于这个原因的考虑吧。
示例：
%旋转角度(60°，30°，90°)，为便于计算，我们先转换成弧度：
v=[pi/3,pi/6,pi/2];
theta=norm(v); %1.9591
%四元数实部
a=cos(theta/2); %0.5574
%四元数虚部
b=sin(theta/2)*v(1)/theta %0.4438
c=sin(theta/2)*v(2)/theta %0.2219
d=sin(theta/2)*v(3)/theta %0.6657
或者
sin(theta/2)*[v/theta] %0.4438 0.2219 0.6657
所以这个四元数a+bi+cj+dk的结果为：0.5574+0.4438i+0.2219j+0.6657k
%四元数转换成旋转向量
rotvecd(quat)%弧度，rotvecd修改为rotvec即可
rotationVector = [pi/3,pi/6,pi/2];
quat = quaternion(rotationVector,'rotvec')
%0.55738 + 0.44379i + 0.22189j + 0.66568k
rotvec(quat)
%1.0472 0.5236 1.5708这里相当于也再次验证上面的推导，我们来看下旋转矩阵和欧拉角的转换
5、旋转矩阵与四元数
%旋转矩阵转换成四元数
M = [1 0 0; 0 sqrt(3)/2 0.5; 0 -0.5 sqrt(3)/2];
quat = quaternion(M,'rotmat','frame')
%0.96593 + 0.25882i + 0j + 0k
%quaternion(M,'rotmat','point')
rotmat(quat,'frame')
ans =1.0000 0 00 0.8660 0.50000 -0.5000 0.86606、欧拉角与四元数
%欧拉角转换成四元数
E = [pi/2,0,pi/4];
quat = quaternion(E,'euler','ZYX','frame')
%0.65328 + 0.2706i + 0.2706j + 0.65328k
%quaternion(E,'euler','ZYX','point')
%四元数转换成欧拉角
euler(quat,'ZYX','frame')
ans =1.5708 0 0.7854
