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

本文为博主“声时刻”原创文章，未经博主允许不得转载。
联系方式：shenshikexmu@163.com

旋转矩阵的两种表示

看到旋转矩阵有两种表现方式，左乘矩阵、右乘矩阵（两矩阵互为转置，本质是一样的）。在 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−q322(q1q2−q0q3)2(q0q2+q1q3)2(q1q2+q0q3)q02+q22−q12−q322(q2q3−q0q1)2(q1q3−q0q2)2(q2q3+q0q1)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} ⎣⎡T11T21T31T12T22T32T13T23T33⎦⎤(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=T11q02−q12+q22−q32=T22q02−q12−q22+q32=T33q02+q12+q22+q32=12(q1q2+q0q3)=T122(q1q3−q0q2)=T132(q1q2−q0q3)=T212(q2q3+q0q1)=T232(q0q2+q1q3)=T312(q2q3−q0q1)=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∣=211+T11+T22+T33 ∣q1∣=211+T11−T22−T33 ∣q2∣=211−T11+T22−T33 ∣q3∣=211−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} ⎩⎨⎧4q0q1=T23−T324q0q2=T31−T134q0q3=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−T33T21+T12T31+T13T23−T32T21+T12T22−T11−T33T32+T23T31−T13T31+T13T32+T23T33−T11−T22T12−T21T23−T32T31−T13T12−T21T11+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−14q1q24q1q34q1q04q1q24q22−14q2q34q2q04q1q34q2q34q32−14q3q04q1q04q2q04q3q04q02−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⎣⎢⎢⎡q1q2q3q0⎦⎥⎥⎤[q1q2q3q0]−31I4∗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:numRK(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)==0q = [0, [1,0,0]];
elsehalf = (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的旋转。