四元数的使用

四元数可以理解成一个四维空间的量（其由四个变量组成），常用在三维旋转坐标下求取一个向量的坐标。四元数可以抽象地理解成对一种三维旋转方式的代表，这个旋转方式由两部分组成：一部分是旋转轴，这个可以用一个三维向量表示$v=(v_x,v_y,v_z)$，另一部分则是坐标系顺该旋转轴旋转角度，逆时针为正，例如$\theta$。

再已知这两部分的前提下，旋转操作是唯一的，我们用四元数来表示这种旋转操作，记为：
$$q=(cos\frac{\theta }{2},v_{x}sin\frac{\theta }{2},v_{y}sin\frac{\theta }{2},v_{z}sin\frac{\theta }{2})$$
现在假如我们已知一个三维向量$h=(x,y,z)$，我们想知道在$q$这种旋转操作下在新坐标系$h^{\prime }$的坐标，该怎么办？

下面解决方法我们不谈为什么，只讲怎么解决。

引入纯四元数这个概念，什么叫纯四元数，就是四元数有一个分量为0，然后将$h$扩充为纯四元数$$h_4=(0,x,y,z)$$也就是三维量变四维量，但事实上这个纯四元数也只是四维空间的一个三维超平面而已。

那么经过$q$旋转后在新坐标系下$h^{\prime }$的坐标可以用如下四元数表示：
$$h^{\prime }=q^{-1}vq$$
上面这个式子用到了四元数的乘法，下面引出四元数乘法的公式：($q_1=(w_1,x_1,y_1,z_1),q_2=(w_2,x_2,y_2,z_2)$)
$$\begin{gathered} q_1\ast q_2=(w_1{w}_2-x_1{x}_2-y_1{y}_2-z_1{z}_2,w_1{x}_2+x_1{w}_2+ \\ {y}_1{z}_2-z_1{y}_2,w_1{y}_2-x_1{z}_2+y_1{w}_2+z_1{x}_2,w_1{z}_2+x_1{y}_2 \\ -y_1{x}_2+z_1{w}_2) \end{gathered}$$

同样，一个四元数的共轭（逆运算）定义为：
$$q^{-1}=(w,-x,-y,-z)$$
最后旋转后向量的坐标为$h^{\prime }$的后三个分量。
下面先给出乘法和逆运算的MATLAB函数：

function result = quaternionMultiplication(q1,q2)
result(1) = q1(1)*q2(1) - q1(2)*q2(2) - q1(3)*q2(3) - q1(4)*q2(4);
result(2) = q1(1)*q2(2) + q1(2)*q2(1) + q1(3)*q2(4) - q1(4)*q2(3);
result(3) = q1(1)*q2(3) - q1(2)*q2(4) + q1(3)*q2(1) + q1(4)*q2(2);
result(4) = q1(1)*q2(4) + q1(2)*q2(3) - q1(3)*q2(2) + q1(4)*q2(1);
end

function result = quaternionInv(q1)
result(1) = q1(1);
result(2) = -q1(2);
result(3) = -q1(3);
result(4) = -q1(4);
end

四元数$q^{-1}vq$和$qv{q}^{-1}$区别

区别在于旋转方向发生改变：

psai = pi/4; phi = pi/6; theta = pi/3;

q1 = [cos(psai/2),0,0,sin(psai/2)];

v1 = [0,1,1,0]; sqrt(sum(v1.^2))
% temp1 = quaternionMultiplication(q1,v1);
% temp2 = quaternionInv(q1);
% vRotation1 = quaternionMultiplication(temp1,temp2);
% vRotation1 = vRotation1(2:4)
% sqrt(sum(vRotation1.^2))
temp1 = quaternionMultiplication(quaternionInv(q1),v1);
vRotation1 = quaternionMultiplication(temp1,q1);
vRotation1 = vRotation1(2:4)
sqrt(sum(vRotation1.^2))

figure();
v2 = [1,1,0];
plot3([0,vRotation1(1)],[0,vRotation1(2)],[0,vRotation1(3)],"r","LineWidth",3);
hold on;
plot3([0,v2(1)],[0,v2(2)],[0,v2(3)],"b","LineWidth",3);
grid on;

注释和非注释部分演示了两种计算方式，并绘图。
这是$q^{-1}vq$:

蓝色为旋转前矢量，红色为旋转后矢量，可以看到这个旋转操作是以z轴为旋转轴逆时针旋转45°。

这是$qv{q}^{-1}$:

对比得知，旋转方向相反！

欧拉角的劣势

欧拉角通过$\theta \varphi \psi$确定一个旋转后坐标系，当然，有一个明显缺点就是必须给定旋转顺序，先转$\theta$再转$\psi$和先转$\psi$再转$\theta$是不一样的，一般规定顺序为$\psi \theta \varphi$。对应角度如下：

而四元数是可以区分先后顺序的，例如:
$$v^{\prime }=q_3^{-1}{q}_2^{-1}{q}_1^{-1}v{q}_1{q}_2{q}_3$$
顺序是先$q_1$再$q_2$再$q_3$。

MATLAB验证欧拉角和四元数一一对应性

我们定义三个旋转操作，$q_1$为绕z轴45°，$q_2$为绕x轴30°，$q_3$为绕y轴60°。验证利用四元数计算结果和欧拉角旋转矩阵计算结果的一致性。
考虑到欧拉角旋转矩阵与旋转顺序有关，定义顺序为$\psi \theta \varphi$
待旋转的向量：
$$v=(1,2,3)$$

%-------------------四元数----------------------------%
psai = pi/4; phi = pi/6; theta = pi/3;

q1 = [cos(psai/2),0,0,sin(psai/2)];
q2 = [cos(theta/2),0,sin(theta/2),0];
q3 = [cos(phi/2),sin(phi/2),0,0];

v1 = [0,1,2,3]; sqrt(sum(v1.^2))

temp1 = quaternionMultiplication(q1,q2);
temp2 = quaternionMultiplication(temp1,q3);
temp3 = quaternionInv(temp2);
temp4 = quaternionMultiplication(temp3,v1);
vRotation1 = quaternionMultiplication(temp4,temp2);
vRotation1 = vRotation1(2:4)
sqrt(sum(vRotation1.^2))

v2 = [1,2,3];
Cbn = [cos(psai)*cos(theta),cos(psai)*sin(theta)*sin(phi)-sin(psai)*cos(phi),...
    sin(theta)*cos(phi)*cos(psai)+sin(phi)*sin(psai);...
    sin(psai)*cos(theta),cos(phi)*cos(psai)+sin(phi)*sin(theta)*sin(psai),...
    sin(theta)*cos(phi)*sin(psai)-sin(phi)*cos(psai);...
    -sin(theta),cos(theta)*sin(phi),cos(theta)*cos(phi)
    ]';

vRotation2 = Cbn * v2'
sqrt(sum(vRotation2.^2))

figure();
plot3([0,vRotation1(1)],[0,vRotation1(2)],[0,vRotation1(3)],"r","LineWidth",3);
hold on;
plot3([0,vRotation2(1)],[0,vRotation2(2)],[0,vRotation2(3)],"g","LineWidth",3);
plot3([0,v2(1)],[0,v2(2)],[0,v2(3)],"b","LineWidth",3);
grid on;

红色和绿色向量重合，蓝色为原始向量，可以看出结果一致。

值得一提的是，这两种运算都有保范性，模不变。