旋转矩阵与四元数

点我进入原文

然后还看到一篇讲 旋转矩阵和 四元素之间转化的 http://blog.youkuaiyun.com/shenshikexmu/article/details/53608224?locationNum=8&fps=1

从四元数到左乘旋转矩阵

四元数 Q=cosθ2+uRsinθ2 ， Q 包含了旋转的全部信息： θ  为转过的角度， uR 为先旋转轴和旋转方向。 
也看到两种四元数表达方式，旋转角度在前，和在后，本文的四元数旋转角度为第一项，也就是 q0=cosθ2 。 
四元数 Q=[q0,q1,q2,q3] 。 
左乘旋转矩阵为： 

Rleft=⎡⎣⎢⎢q20+q21−q22−q232(q1q2−q0q3)2(q0q2+q1q3)2(q1q2+q0q3)q20+q22−q21−q232(q2q3−q0q1)2(q1q3−q0q2)2(q2q3+q0q1)q20+q23−q21−q22⎤⎦⎥⎥

在Gait-Tracking-With-x-IMU 中，四元数转旋转矩阵matlab代码，如下：

function R = quatern2rotMat(q)
    [rows cols] = size(q);
    R = zeros(3,3, rows);
    R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2;
    R(1,2,:) = 2.*(q(:,2).*q(:,3)+q(:,1).*q(:,4));
    R(1,3,:) = 2.*(q(:,2).*q(:,4)-q(:,1).*q(:,3));
    R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4));
    R(2,2,:) = 2.*q(:,1).^2-1+2.*q(:,3).^2;
    R(2,3,:) = 2.*(q(:,3).*q(:,4)+q(:,1).*q(:,2));
    R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3));
    R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2));
    R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2;
end

  
1
2
3
4
5
6
7
8
9
10
11
12
13
14
  
1
2
3
4
5
6
7
8
9
10
11
12
13
14

代码中的计算和旋转矩阵一样，其中q(:,1)代 q0 ，q(:,2)代 q1 ，q(:,3)代 q2 ，q(:,4)代 q3 ，对角元利用了四元数的性质 q20+q21+q22+q23=1 。

算法2

论文New Method for Extracting the Quaternion from a Rotation Matrix的算法。 
算法先把左乘旋转矩阵 Rleft 转化成 K3 矩阵，如下： 

K3=13⎡⎣⎢⎢⎢T11−T22−T33T21+T12T31+T13T23−T32T21+T12T22−T11−T33T32+T23T31−T13T31+T13T32+T23T33−T11−T22T12−T21T23−T32T31−T13T12−T21T11+T22+T33⎤⎦⎥⎥⎥

K3 矩阵用四元数的方式表示为： 

13⎡⎣⎢⎢⎢⎢4q21−14q1q24q1q34q1q04q1q24q22−14q2q34q2q04q1q34q2q34q23−14q3q04q1q04q2q04q3q04q20−1⎤⎦⎥⎥⎥⎥

矩阵可转化为 

43⎡⎣⎢⎢⎢⎢q1q2q3q0⎤⎦⎥⎥⎥⎥[q1q2q3q0]−13I4∗4

可以看出, K3 矩阵只有一个特征向量， [q1,q2,q3,q0] ，其特征值为1.

在Gait-Tracking-With-x-IMU 中，旋转矩阵转四元数matlab代码，如下：

function q = rotMat2quatern(R)  
    %wiki URL: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#cite_note-5
    % paper URL: http://arc.aiaa.org/doi/pdf/10.2514/2.4654
    [row col numR] = size(R);
    q = zeros(numR, 4);
    K = zeros(4,4);    
    for i = 1:numR
        K(1,1) = (1/3) * (R(1,1,i) - R(2,2,i) - R(3,3,i));
        K(1,2) = (1/3) * (R(2,1,i) + R(1,2,i));
        K(1,3) = (1/3) * (R(3,1,i) + R(1,3,i));
        K(1,4) = (1/3) * (R(2,3,i) - R(3,2,i));  
        K(2,1) = (1/3) * (R(2,1,i) + R(1,2,i));
        K(2,2) = (1/3) * (R(2,2,i) - R(1,1,i) - R(3,3,i));
        K(2,3) = (1/3) * (R(3,2,i) + R(2,3,i));
        K(2,4) = (1/3) * (R(3,1,i) - R(1,3,i));   
        K(3,1) = (1/3) * (R(3,1,i) + R(1,3,i));
        K(3,2) = (1/3) * (R(3,2,i) + R(2,3,i));
        K(3,3) = (1/3) * (R(3,3,i) - R(1,1,i) - R(2,2,i));
        K(3,4) = (1/3) * (R(1,2,i) - R(2,1,i));    
        K(4,1) = (1/3) * (R(2,3,i) - R(3,2,i));
        K(4,2) = (1/3) * (R(3,1,i) - R(1,3,i));
        K(4,3) = (1/3) * (R(1,2,i) - R(2,1,i));
        K(4,4) = (1/3) * (R(1,1,i) + R(2,2,i) + R(3,3,i)); 
        [V,D] = eig(K);
        %p = find(max(D));
        %q = V(:,p)';    
        q(i,:) = V(:,4)';
        q(i,:) = [q(i,4) q(i,1) q(i,2) q(i,3)];
    end
end

其他的转化方式可以参考原文： http://blog.youkuaiyun.com/shenshikexmu/article/details/53608224?locationNum=8&fps=1

在计算机图形学的学习中，几何变换（Transformations）是一块重要的内容，我们使用齐次坐标（Homogeneous coordinates）描述点和向量，使用变换矩阵描述平移、旋转等变换。

而在平移、旋转、缩放这几种变换中，又以旋转的情况最为复杂。实际上，计算机图形学中三维空间的旋转不仅仅有旋转矩阵一种表达形式，欧拉角（Euler angles）和四元数（Quaternions）也是常用的方法。

---

旋转矩阵

三维空间中的一个点  P P ，我们用齐次坐标表示：

P=[x,y,z,w]T P=[x,y,z,w]T

我们首先考虑分别绕 X 轴、Y 轴、Z 轴旋转一定角度的情况。

绕坐标轴旋转

设  P P 绕 X 轴、Y 轴、Z 轴的旋转角度分别为  α α、 β β 和  θ θ。

我们使用右手坐标系，旋转角度的正向由右手定则确定。

点绕坐标轴旋转可以考虑点在相应坐标平面上投影的旋转。比如绕 Y 轴旋转，那么考虑点  P P 在 X-Z 平面上的投影的旋转，如下图所示：

绕Y轴旋转

假设点  P P 在 X-Z 平面上的投影点与坐标原点连成的向量长度为 L ，那么根据简单的平面几何知识，我们可以得到：

[x1z1]=[cosβ−sinβsinβcosβ][xz] [x1z1]=[cosβsinβ−sinβcosβ][xz]

用齐次坐标表示绕Y轴的旋转为：

⎡⎣⎢⎢⎢x1y1z1w1⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢cosβ0−sinβ00100sinβ0cosβ00001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢xyzw⎤⎦⎥⎥⎥ [x1y1z1w1]=[cosβ0sinβ00100−sinβ0cosβ00001][xyzw]

同理可分别得到绕X轴与绕Y轴的情况。

绕X轴旋转：

⎡⎣⎢⎢⎢x2y2z2w2⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢10000cosαsinα00−sinαcosα00001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢xyzw⎤⎦⎥⎥⎥ [x2y2z2w2]=[10000cosα−sinα00sinαcosα00001][xyzw]

绕Z轴旋转：

⎡⎣⎢⎢⎢x3y3z3w3⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢cosθsinθ00−sinθcosθ0000100001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢xyzw⎤⎦⎥⎥⎥ [x3y3z3w3]=[cosθ−sinθ00sinθcosθ0000100001][xyzw]

我们可以将绕X、Y和Z坐标轴的旋转矩阵分别记为  Rx(α),Ry(β),Rz(θ) Rx(α),Ry(β),Rz(θ)，则有：

Rx(α)=⎡⎣⎢⎢⎢10000cosαsinα00−sinαcosα00001⎤⎦⎥⎥⎥ Rx(α)=[10000cosα−sinα00sinαcosα00001]

Ry(β)=⎡⎣⎢⎢⎢cosβ0−sinβ00100sinβ0cosβ00001⎤⎦⎥⎥⎥ Ry(β)=[cosβ0sinβ00100−sinβ0cosβ00001]

Rz(θ)=⎡⎣⎢⎢⎢cosθsinθ00−sinθcosθ0000100001⎤⎦⎥⎥⎥ Rz(θ)=[cosθ−sinθ00sinθcosθ0000100001]

旋转矩阵可以通过其他旋转矩阵复合得到（矩阵乘法）。

---

欧拉角

上面讨论了绕三条坐标轴旋转的旋转矩阵，旋转矩阵的一般形式（这里没有用齐次坐标）为：

C=⎡⎣⎢c11c21c31c12c22c32c13c23c33⎤⎦⎥ C=[c11c12c13c21c22c23c31c32c33]

物体在三维空间中的旋转可以从坐标系的旋转来考虑（三维空间中坐标轴，即三维线性空间中基的变换）。那么矩阵  C C 的三个列向量实际对应着原坐标系三个坐标轴方向的单位向量在旋转后的新坐标系下的坐标。

我们知道直角坐标系的三个坐标轴方向的单位向量实际上是一组标准正交基，于是矩阵  C C 是一个正交矩阵。所以旋转矩阵表面上看起来依赖于 9 个参数，实际上只有三个是独立的。

为了更直接地指出这三个独立参数，欧拉（Euler）证明了如下事实：任何一个旋转都可以由连续施行的三次绕轴旋转来实现，这三次绕轴旋转的旋转角就是三个独立参数，称为欧拉角。

根据绕轴旋转的顺序不同，欧拉角的表示也不同。常见的欧拉角表示有 Yaw-Pitch-Roll (Y-X-Z顺序)，通过下面的图片可以形象地进行理解。

Yaw-Pitch-Roll

偏航(Yaw)：

airplane Yaw

仰俯(Pitch)：

airplane Pitch

侧偏(Roll)：

airplane Roll

设 Yaw 、Pitch 、Roll 三个角度分别为  θ,φ,ψ θ,φ,ψ，那么利用欧拉角进行旋转对应的旋转变换矩阵为：

⎡⎣⎢cosψ cosθ−sinψ cosφ sinθcosψ sinθ+sinψ cosφ cosθsinψ sinφ−sinψ cosθ−cosψ cosφ sinθ−sinψ sinθ+cosψ cosφ cosθcosψ sinφsinφ sinθ−sinφ cosθcosφ⎤⎦⎥ [cosψ cosθ−sinψ cosφ sinθ−sinψ cosθ−cosψ cosφ sinθsinφ sinθcosψ sinθ+sinψ cosφ cosθ−sinψ sinθ+cosψ cosφ cosθ−sinφ cosθsinψ sinφcosψ sinφcosφ]

实际上 Yaw 、Pitch 、Roll 的旋转就分别对应着前面我们给出的旋转矩阵  Rx(θ),Ry(φ),Rz(ψ) Rx(θ),Ry(φ),Rz(ψ)，上面的矩阵就是这三个矩阵的复合。

欧拉角的好处是简单、容易理解，但使用它作为旋转的工具有严重的缺陷—万向节死锁（Gimbal Lock）。

万向节死锁是指物体的两个旋转轴指向同一个方向。实际上，当两个旋转轴平行时，我们就说万向节锁现象发生了，换句话说，绕一个轴旋转可能会覆盖住另一个轴的旋转，从而失去一维自由度。

例如，三维空间中有一个平行于 X 轴的向量，我们将它绕 Y 轴旋转直到它平行于 Z 轴，这时，我们会发现任何绕 Z 轴的旋转都改变不了该向量的方向，即出现了万向节死锁。

由于万向节死锁的存在，使用欧拉角也无法很好地处理旋转的插值（以实现“平滑”旋转）。

---

四元数

从前面的讨论我们发现三角度系统（three-angle system）无法很好地处理旋转的插值。下面介绍四元数（Quaternions）以及如何利用四元数描述旋转。

四元数的定义

四元数是由数学家 William Rowan Hamilton 于1843年所发明的数学概念，是复数的推广，可以说是“三维的复数”，形式为  w+x i+y j+z k w+x i+y j+z k ，其中  i,j,k i,j,k 的关系如下：

i2=j2=k2=−1i⋅j=−j⋅i=ki⋅k=−k⋅i=jj⋅k=−k⋅j=i i2=j2=k2=−1i⋅j=−j⋅i=ki⋅k=−k⋅i=jj⋅k=−k⋅j=i

假设有两个四元数：

q1=w1+x1 i+y1 j+z1 kq2=w2+x2 i+y2 j+z2 k q1=w1+x1 i+y1 j+z1 kq2=w2+x2 i+y2 j+z2 k

四元数的加法定义如下：

q1+q2=(w1+w2)+(x1+x2) i+(y1+y2) j+(z1+z2) k q1+q2=(w1+w2)+(x1+x2) i+(y1+y2) j+(z1+z2) k

四元数的乘法定义，利用简单的分配律定义如下：

q1⋅q2=(w1⋅w2−x1⋅x2−y1⋅y2−z1⋅z2)+(w1⋅x2+x1⋅w2+y1⋅z2−z1⋅y2) i+(w1⋅y2−x1⋅z2+y1⋅w2+z1⋅x1) j+(w1⋅z2+x1⋅y2−y1⋅x2+z1⋅w2) k q1⋅q2=(w1⋅w2−x1⋅x2−y1⋅y2−z1⋅z2)+(w1⋅x2+x1⋅w2+y1⋅z2−z1⋅y2) i+(w1⋅y2−x1⋅z2+y1⋅w2+z1⋅x1) j+(w1⋅z2+x1⋅y2−y1⋅x2+z1⋅w2) k

为了方便表示，我们将四元数记为：

q=(x,y,z,w)=(v⃗ ,w) q=(x,y,z,w)=(v→,w)

注意，这里四元数的表示形式和“齐次坐标”长得一样，但是它们之间没什么关系！

四元数常常用来表示旋转，很多人将其理解为“w表示旋转角度，v表示旋转轴”，也是错误的！

正确的理解应为：“w与旋转角度有关，v与旋转轴有关”。

四元数的模（norm）定义为

|q|=x2+y2+z2+w2 |q|=x2+y2+z2+w2

模为1的四元数称为单位四元数(Unit quaternions)。

四元数的共轭（conjugate）定义为:

q∗=(−v⃗ ,w) q∗=(−v→,w)

四元数的倒数定义为：

1/q=q∗/|q|2 1/q=q∗/|q|2

四元数与旋转

这里直接给出结论：如果把单位四元数表示为：

q=(n⃗ ⋅sinθ2, cosθ2) q=(n→⋅sinθ2, cosθ2)

的形式，那么该单位四元数可以表示绕轴  n⃗  n→
进行  θ θ 角的旋转。

该单位四元数对应的旋转矩阵为

R(q)=⎡⎣⎢1−2(y2+z2)2(xy+zw)2(xz−yw)2(xy−zw)1−2(x2+z2)2(yz+xw)2(xz+yw)2(yz−xw)1−2(x2+y2)⎤⎦⎥ R(q)=[1−2(y2+z2)2(xy−zw)2(xz+yw)2(xy+zw)1−2(x2+z2)2(yz−xw)2(xz−yw)2(yz+xw)1−2(x2+y2)]

这里的推导用到了轴-角旋转表示中的Rodrigues’ rotation formula，具体证明这里不展开了，有兴趣的可以查阅相关资料。

我们发现用四元数描述旋转需要的存储空间很小，更为关键的是可以使用被称为球面线性插值（Slerp Algorithm）的方法对四元数进行插值运算，从而解决了平滑旋转的插值问题。

在 OpenGL 或者 DirectX 中我们通常使用模型视图矩阵来进行几何变换，当我们希望实现光滑旋转、对旋转进行插值时，就可以利用四元数这一工具。处理过程为：

• 模型视图矩阵 -> 四元数
• 使用四元数进行运算
• 四元数 -> 模型视图矩阵