旋转矩阵转化成四元数的三种算法

本文为博主“声时刻”原创文章，未经博主允许不得转载。

联系方式：[email protected]

旋转矩阵的两种表示

看到旋转矩阵有两种表现方式，左乘矩阵、右乘矩阵（两矩阵互为转置，本质是一样的）。在 Gait-Tracking-With-x-IMU 中，旋转矩阵为左乘矩阵。在秦永元的惯性导航中，旋转矩阵为右乘矩阵。

设 a = [ a x , a y , a z ] a=[a_x,a_y,a_z] a=[ax​,ay​,az​]为旋转前的世界坐标系下的向量， b = [ b x , b y , b z ] b=[b_x,b_y,b_z] b=[bx​,by​,bz​]为旋转后的世界坐标系下的向量。两向量与左乘矩阵、右乘矩阵的关系如下：

• 左乘矩阵：

  a ∗ R l e f t = b (1) a\ast R_{left}=b\tag{1} a∗Rleft​=b(1)

• 右乘矩阵：

  R r i g h t ∗ a ′ = b ′ (2) R_{right}\ast {a}'={b}'\tag{2} Rright​∗a′=b′(2)

• 旋转矩阵的性质：

R ∗ R ′ = I (3) R\ast {R}'=I \tag{3} R∗R′=I(3) R ∗ R − 1 = I (4) R\ast R^{-1}=I \tag{4} R∗R−1=I(4) R ′ = R − 1 (5) {R}'= R^{-1} \tag{5} R′=R−1(5) R r i g h t = R l e f t ′ (6) R_{right}=R_{left}'\tag{6} Rright​=Rleft′​(6)

由于旋转矩阵的逆矩阵与转置矩阵相同，在处理时一定要搞清楚是左乘矩阵，还是右乘矩阵。

本文算法操作的都为左乘旋转矩阵。

从四元数到左乘旋转矩阵

四元数 Q = cos ⁡ θ 2 + u R sin ⁡ θ 2 Q=\cos{\frac{\theta}{2}}+u^{R}\sin{\frac{\theta}{2}} Q=cos2θ​+uRsin2θ​， Q Q Q包含了旋转的全部信息： θ \theta θ 为转过的角度， u R u^{R} uR为旋转轴和旋转方向。

也看到两种四元数表达方式，旋转角度在前，和在后，本文的四元数旋转角度为第一项，也就是 q 0 = cos ⁡ θ 2 q_0=\cos{\frac{\theta}{2}} q0​=cos2θ​。

四元数 Q = [ q 0 , q 1 , q 2 , q 3 ] Q=[q_0,q_1,q_2,q_3] Q=[q0​,q1​,q2​,q3​]。

左乘旋转矩阵为：

R l e f t = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 + q 0 q 3 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 1 q 2 − q 0 q 3 ) q 0 2 + q 2 2 − q 1 2 − q 3 2 2 ( q 2 q 3 + q 0 q 1 ) 2 ( q 0 q 2 + q 1 q 3 ) 2 ( q 2 q 3 − q 0 q 1 ) q 0 2 + q 3 2 − q 1 2 − q 2 2 ] (7) R_{left}=\begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 &2(q_1q_2+q_0q_3) &2(q_1q_3-q_0q_2) \\ 2(q_1q_2-q_0q_3) &q_0^2+q_2^2-q_1^2-q_3^2 &2(q_2q_3+q_0q_1) \\ 2(q_0q_2+q_1q_3)&2(q_2q_3-q_0q_1) &q_0^2+q_3^2-q_1^2-q_2^2 \end{bmatrix}\tag{7} Rleft​=⎣⎡​q02​+q12​−q22​−q32​2(q1​q2​−q0​q3​)2(q0​q2​+q1​q3​)​2(q1​q2​+q0​q3​)q02​+q22​−q12​−q32​2(q2​q3​−q0​q1​)​2(q1​q3​−q0​q2​)2(q2​q3​+q0​q1​)q02​+q32​−q12​−q22​​⎦⎤​(7)

在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

           

代码中的计算和旋转矩阵一样，其中q(:,1)代 q 0 q_0 q0​，q(:,2)代 q 1 q_1 q1​，q(:,3)代 q 2 q_2 q2​，q(:,4)代 q 3 q_3 q3​，对角元利用了四元数的性质 q 0 2 + q 1 2 + q 2 2 + q 3 2 = 1 q_0^2+q_1^2+q_2^2+q_3^2=1 q02​+q12​+q22​+q32​=1。

旋转矩阵转四元数

算法1

算法出自秦永元的惯性导航，由于其书中是右乘矩阵，在此改成左乘矩阵。

设旋转矩阵 R l e f t R_{left} Rleft​为

[ T 11 T 12 T 13 T 21 T 22 T 23 T 31 T 32 T 33 ] (8) \begin{bmatrix} T_{11} &T_{12}&T_{13} \\ T_{21} &T_{22}&T_{23} \\ T_{31}&T_{32}&T_{33} \end{bmatrix}\tag{8} ⎣⎡​T11​T21​T31​​T12​T22​T32​​T13​T23​T33​​⎦⎤​(8)

根据四元数与旋转矩阵对应关系，可列方程：

{ q 0 2 + q 1 2 − q 2 2 − q 3 2 = T 11 q 0 2 − q 1 2 + q 2 2 − q 3 2 = T 22 q 0 2 − q 1 2 − q 2 2 + q 3 2 = T 33 q 0 2 + q 1 2 + q 2 2 + q 3 2 = 1 2 ( q 1 q 2 + q 0 q 3 ) = T 12 2 ( q 1 q 3 − q 0 q 2 ) = T 13 2 ( q 1 q 2 − q 0 q 3 ) = T 21 2 ( q 2 q 3 + q 0 q 1 ) = T 23 2 ( q 0 q 2 + q 1 q 3 ) = T 31 2 ( q 2 q 3 − q 0 q 1 ) = T 32 (9) \left\{\begin{matrix} q_0^2+q_1^2-q_2^2-q_3^2=T_{11}\\ q_0^2-q_1^2+q_2^2-q_3^2=T_{22}\\ q_0^2-q_1^2-q_2^2+q_3^2=T_{33}\\ q_0^2+q_1^2+q_2^2+q_3^2=1\\ 2(q_1q_2+q_0q_3)=T_{12}\\ 2(q_1q_3-q_0q_2)=T_{13}\\ 2(q_1q_2-q_0q_3)=T_{21}\\ 2(q_2q_3+q_0q_1)=T_{23}\\ 2(q_0q_2+q_1q_3)=T_{31}\\ 2(q_2q_3-q_0q_1)=T_{32} \end{matrix}\right.\tag{9} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​q02​+q12​−q22​−q32​=T11​q02​−q12​+q22​−q32​=T22​q02​−q12​−q22​+q32​=T33​q02​+q12​+q22​+q32​=12(q1​q2​+q0​q3​)=T12​2(q1​q3​−q0​q2​)=T13​2(q1​q2​−q0​q3​)=T21​2(q2​q3​+q0​q1​)=T23​2(q0​q2​+q1​q3​)=T31​2(q2​q3​−q0​q1​)=T32​​(9)

从上述方程可解得

{ ∣ q 0 ∣ = 1 2 1 + T 11 + T 22 + T 33 ∣ q 1 ∣ = 1 2 1 + T 11 − T 22 − T 33 ∣ q 2 ∣ = 1 2 1 − T 11 + T 22 − T 33 ∣ q 3 ∣ = 1 2 1 − T 11 − T 22 + T 33 (10) \left\{\begin{matrix} \left | q_0 \right |=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}}\\ \left | q_1 \right |=\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}\\ \left | q_2 \right |=\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}\\ \left | q_3 \right |=\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}} \end{matrix}\right.\tag{10} ⎩⎪⎪⎨⎪⎪⎧​∣q0​∣=21​1+T11​+T22​+T33​

​∣q1​∣=21​1+T11​−T22​−T33​

​∣q2​∣=21​1−T11​+T22​−T33​

​∣q3​∣=21​1−T11​−T22​+T33​

​​(10)

{ 4 q 0 q 1 = T 23 − T 32 4 q 0 q 2 = T 31 − T 13 4 q 0 q 3 = T 12 − T 21 (11) \left\{\begin{matrix} 4q_0q_1=T_{23}-T_{32}\\ 4q_0q_2=T_{31}-T_{13}\\ 4q_0q_3=T_{12}-T_{21} \end{matrix}\right.\tag{11} ⎩⎨⎧​4q0​q1​=T23​−T32​4q0​q2​=T31​−T13​4q0​q3​=T12​−T21​​(11)

确定 q 0 q_0 q0​、 q 1 q_1 q1​、 q 2 q_2 q2​、 q 3 q_3 q3​的符号，可按下式确定

{ s i g n ( q 1 ) = s i g n ( q 0 ) s i g n ( T 23 − T 32 ) s i g n ( q 2 ) = s i g n ( q 0 ) s i g n ( T 31 − T 13 ) s i g n ( q 3 ) = s i g n ( q 0 ) s i g n ( T 12 − T 21 ) (12) \left\{\begin{matrix} sign(q_1)=sign(q_0)sign(T_{23}-T_{32})\\ sign(q_2)=sign(q_0)sign(T_{31}-T_{13})\\ sign(q_3)=sign(q_0)sign(T_{12}-T_{21}) \end{matrix}\right.\tag{12} ⎩⎨⎧​sign(q1​)=sign(q0​)sign(T23​−T32​)sign(q2​)=sign(q0​)sign(T31​−T13​)sign(q3​)=sign(q0​)sign(T12​−T21​)​(12)

上式中， s i g n ( q 0 ) sign(q_0) sign(q0​)的符号可以任选，在表示旋转上 Q Q Q与 − Q -Q −Q是等价的, − Q -Q −Q的旋转轴与 Q Q Q完全相反， − Q -Q −Q在反向旋转轴上，旋转了 − q 0 -q_0 −q0​,等价于正向旋转轴上旋转 q 0 q_0 q0​。

算法2

论文New Method for Extracting the Quaternion from a Rotation Matrix的算法。

算法先把左乘旋转矩阵 R l e f t R_{left} Rleft​转化成 K 3 K_3 K3​矩阵，如下：

K 3 = 1 3 [ T 11 − T 22 − T 33 T 21 + T 12 T 31 + T 13 T 23 − T 32 T 21 + T 12 T 22 − T 11 − T 33 T 32 + T 23 T 31 − T 13 T 31 + T 13 T 32 + T 23 T 33 − T 11 − T 22 T 12 − T 21 T 23 − T 32 T 31 − T 13 T 12 − T 21 T 11 + T 22 + T 33 ] (13) K_{3}=\frac{1}{3}\begin{bmatrix} T_{11}-T_{22}-T_{33} & T_{21}+T_{12} & T_{31}+T_{13} & T_{23}-T_{32}\\ T_{21}+T_{12}& T_{22}-T_{11}-T_{33} & T_{32}+T_{23} & T_{31}-T_{13}\\ T_{31}+T_{13}& T_{32}+T_{23} &T_{33}-T_{11}-T_{22} & T_{12}-T_{21}\\ T_{23}-T_{32}& T_{31}-T_{13} & T_{12}-T_{21} &T_{11}+T_{22}+T_{33} \end{bmatrix}\tag{13} K3​=31​⎣⎢⎢⎡​T11​−T22​−T33​T21​+T12​T31​+T13​T23​−T32​​T21​+T12​T22​−T11​−T33​T32​+T23​T31​−T13​​T31​+T13​T32​+T23​T33​−T11​−T22​T12​−T21​​T23​−T32​T31​−T13​T12​−T21​T11​+T22​+T33​​⎦⎥⎥⎤​(13)

K 3 K_3 K3​矩阵用四元数的方式表示为：

1 3 [ 4 q 1 2 − 1 4 q 1 q 2 4 q 1 q 3 4 q 1 q 0 4 q 1 q 2 4 q 2 2 − 1 4 q 2 q 3 4 q 2 q 0 4 q 1 q 3 4 q 2 q 3 4 q 3 2 − 1 4 q 3 q 0 4 q 1 q 0 4 q 2 q 0 4 q 3 q 0 4 q 0 2 − 1 ] (14) \frac{1}{3}\begin{bmatrix} 4q_{1}^2-1& 4q_1q_2 & 4q_1q_3 & 4q_1q_0 \\ 4q_1q_2 & 4q_{2}^2-1 & 4q_2q_3 & 4q_2q_0 \\ 4q_1q_3 & 4q_2q_3 & 4q_{3}^2-1 & 4q_3q_0 \\ 4q_1q_0 & 4q_2q_0 & 4q_3q_0 & 4q_{0}^2-1 \end{bmatrix}\tag{14} 31​⎣⎢⎢⎡​4q12​−14q1​q2​4q1​q3​4q1​q0​​4q1​q2​4q22​−14q2​q3​4q2​q0​​4q1​q3​4q2​q3​4q32​−14q3​q0​​4q1​q0​4q2​q0​4q3​q0​4q02​−1​⎦⎥⎥⎤​(14)

矩阵可转化为

4 3 [ q 1 q 2 q 3 q 0 ] [ q 1 q 2 q 3 q 0 ] − 1 3 I 4 ∗ 4 (15) \frac{4}{3}\begin{bmatrix} q_1\\ q_2\\ q_3\\ q_0 \end{bmatrix} \begin{bmatrix} q_1 & q_2 & q_3 & q_0 \end{bmatrix}-\frac{1}{3}I_{4*4}\tag{15} 34​⎣⎢⎢⎡​q1​q2​q3​q0​​⎦⎥⎥⎤​[q1​​q2​​q3​​q0​​]−31​I4∗4​(15)

可以看出, K 3 K_3 K3​矩阵只有一个特征向量， [ q 1 , q 2 , q 3 , q 0 ] [q_1,q_2,q_3,q_0] [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

           

算法3

终于要到这篇博客要讲的算法了，这算法在我接触四元数1年左右才知道，让我甚是羞愧，但理解了算法的思想却让我叫好。由于是同事没有说明从哪里找到的，出处不详。他只是把这算法调成和算法2的结果一样，所以我判断他并不知道算法具体操作方式（思想他可能理解）。

算法3旋转矩阵转四元数matlab代码，如下：

function q = rotMat2qRichard(R)
vX=R(1,:);
vY=R(2,:);

qX = qUtoV(vX,[1,0,0]);

y= qMultiVec(vY, qX);
qY = qUtoV(y,[0,1,0]);

qx=[-qX(1),qX(2:4)];
qy=[-qY(1),qY(2:4)];

q =qMultiQ(qx,qy);

end
           

function [qq]=qMultiQ(p,q)   %p*q
qq=[...
        p(1) * q(1) - p(2) * q(2) - p(3) * q(3) - p(4) * q(4)...
       ,p(2) * q(1) + p(1) * q(2) - p(4) * q(3) + p(3) * q(4)...
       ,p(3) * q(1) + p(4) * q(2) + p(1) * q(3) - p(2) * q(4)...
       ,p(4) * q(1) - p(3) * q(2) + p(2) * q(3) + p(1) * q(4)  ];

end
           

function q = qUtoV(v1, v2)        %two vetor rotation to quaternions
nv1 = v1/norm(v1);
nv2 = v2/norm(v2);

if norm(nv1+nv2)==0
    q = [0, [1,0,0]];
else
    half = (nv1 + nv2)/norm(nv1 + nv2);
    q = [nv1*half',cross(nv1, half)];
end
end
           

function [vector]=qMultiVec(vec,q)  %sensor frame to world frame
x = q(2);
y = q(3);
z = q(4);
w = q(1);

vecx = vec(1);
vecy = vec(2);
vecz = vec(3);

x_ =  w * vecx  +  y * vecz  -  z * vecy;
y_ =  w * vecy  +  z * vecx  -  x * vecz;
z_ =  w * vecz  +  x * vecy  -  y * vecx;
w_ = -x * vecx  -  y * vecy  -  z * vecz;

vector = [x_ * w  +  w_ * -x  +  y_ * -z  -  z_ * -y ...
    , y_ * w  +  w_ * -y  +  z_ * -x  -  x_ * -z ...
    , z_ * w  +  w_ * -z  +  x_ * -y  -  y_ * -x ...
    ];

end

           

算法思想

1 左乘旋转矩阵，把旋转前x轴向量[1,0,0]，转化到旋转后x轴 向量 [ T 11 , T 12 , T 13 ] [T_{11},T_{12},T_{13}] [T11​,T12​,T13​]，如此类推，y轴[0,1,0]转到 [ T 21 , T 22 , T 23 ] [T_{21},T_{22},T_{23}] [T21​,T22​,T23​],z轴[0,0,1]转到 [ T 31 , T 32 , T 33 ] [T_{31},T_{32},T_{33}] [T31​,T32​,T33​]。

2 先计算一个四元数 q 1 q_1 q1​，使得在 q 1 q_1 q1​的旋转变化下，向量[1,0,0]转到向量 [ T 11 , T 12 , T 13 ] [T_{11},T_{12},T_{13}] [T11​,T12​,T13​]的位置，也就是原x轴与旋转后x轴重合，这时旋转后的y轴、z轴不一定与 [ T 21 , T 22 , T 23 ] [T_{21},T_{22},T_{23}] [T21​,T22​,T23​]、 [ T 31 , T 32 , T 33 ] [T_{31},T_{32},T_{33}] [T31​,T32​,T33​]重合，但可以知道，旋转后y、z轴已经和 [ T 21 , T 22 , T 23 ] [T_{21},T_{22},T_{23}] [T21​,T22​,T23​]、 [ T 31 , T 32 , T 33 ] [T_{31},T_{32},T_{33}] [T31​,T32​,T33​]在一个平面上，因为他们都垂直于 [ T 11 , T 12 , T 13 ] [T_{11},T_{12},T_{13}] [T11​,T12​,T13​]。

3 四元数 q 1 q_1 q1​作用下，旋转后的y、z轴向量记作 [ y x ′ , y y ′ , y z ′ ] [{y_x}',{y_y}',{y_z}'] [yx​′,yy​′,yz​′]和 [ z x ′ , z y ′ , z z ′ ] [{z_x}',{z_y}',{z_z}'] [zx​′,zy​′,zz​′]。 [ y x ′ , y y ′ , y z ′ ] [{y_x}',{y_y}',{y_z}'] [yx​′,yy​′,yz​′]与 [ T 21 , T 22 , T 23 ] [T_{21},T_{22},T_{23}] [T21​,T22​,T23​]之间的角度，和 [ z x ′ , z y ′ , z z ′ ] [{z_x}',{z_y}',{z_z}'] [zx​′,zy​′,zz​′]与 [ T 31 , T 32 , T 33 ] [T_{31},T_{32},T_{33}] [T31​,T32​,T33​]之间的角度相同。所以再加一个四元数 q 2 q_2 q2​，使 [ y x ′ , y y ′ , y z ′ ] [{y_x}',{y_y}',{y_z}'] [yx​′,yy​′,yz​′]转到 [ T 21 , T 22 , T 23 ] [T_{21},T_{22},T_{23}] [T21​,T22​,T23​]，就可以完成原坐标系到旋转后坐标系的转化。

4 从形式上看， q 1 q_1 q1​与 q 2 q_2 q2​做四元数乘法，就可以得到想要的四元数。但是这里还是有个坑的。四元数更新都是原坐标系建立的四元数，也就是x轴总是[1,0,0],y轴总是[0,1,0],z轴总是[0,0,1]。所以要先逆向思维，旋转后的x轴先转到原x轴[1,0,0]，四元数为 q X qX qX，在四元数 q X qX qX作用后的旋转后y轴 [ T 21 , T 22 , T 23 ] [T_{21},T_{22},T_{23}] [T21​,T22​,T23​]转换为向量 [ y q X , y q X , y q X ] [{y_{qX}},{y_{qX}},{y_{qX}}] [yqX​,yqX​,yqX​]。然后 [ y q X , y q X , y q X ] [{y_{qX}},{y_{qX}},{y_{qX}}] [yqX​,yqX​,yqX​]转到原y轴[0,1,0],四元数为 q Y qY qY。两四元数 q X qX qX、 q Y qY qY反，相乘就得到所要的四元数了。

5关于四元数表示向量旋转算法，请看本人博客四元数表示向量V1到V2的旋转。