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

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

旋转矩阵的两种表示

看到旋转矩阵有两种表现方式，左乘矩阵、右乘矩阵（两矩阵互为转置，本质是一样的）。在 Gait-Tracking-With-x-IMU 中，旋转矩阵为左乘矩阵。在秦永元的惯性导航中，旋转矩阵为右乘矩阵。
设$a=[a_x,a_y,a_z]$为旋转前的世界坐标系下的向量，$b=[b_x,b_y,b_z]$为旋转后的世界坐标系下的向量。两向量与左乘矩阵、右乘矩阵的关系如下：

• 左乘矩阵：
  $$a\ast R_{left}=b\text{\hspace{1em}(1)}$$

• 右乘矩阵：
  $$R_{right}\ast a^{\prime }=b^{\prime }\text{\hspace{1em}(2)}$$

• 旋转矩阵的性质：

$$R\ast R^{\prime }=I\text{\hspace{1em}(3)}$$ $$R\ast R^{-1}=I\text{\hspace{1em}(4)}$$ $$R^{\prime }=R^{-1}\text{\hspace{1em}(5)}$$$$R_{right}=R_{left}^{\prime }\text{\hspace{1em}(6)}$$

由于旋转矩阵的逆矩阵与转置矩阵相同，在处理时一定要搞清楚是左乘矩阵，还是右乘矩阵。
本文算法操作的都为左乘旋转矩阵。

从四元数到左乘旋转矩阵

四元数$Q=\cos \frac{\theta }{2}+u^R\sin \frac{\theta }{2}$，$Q$包含了旋转的全部信息：$\theta$ 为转过的角度，$u^R$为旋转轴和旋转方向。
也看到两种四元数表达方式，旋转角度在前，和在后，本文的四元数旋转角度为第一项，也就是$q_0=\cos \frac{\theta }{2}$。
四元数$Q=[q_0,q_1,q_2,q_3]$。
左乘旋转矩阵为：
$$R_{left}=\left[\begin{array}{ccc}{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\end{array}\right]\text{\hspace{1em}(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(:,2)代$q_1$，q(:,3)代$q_2$，q(:,4)代$q_3$，对角元利用了四元数的性质$q_0^2+q_1^2+q_2^2+q_3^2=1$。

旋转矩阵转四元数

算法1

算法出自秦永元的惯性导航，由于其书中是右乘矩阵，在此改成左乘矩阵。
设旋转矩阵$R_{left}$为
$$\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]\text{\hspace{1em}(8)}$$
根据四元数与旋转矩阵对应关系，可列方程：
$$\{\begin{array}{c}{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}\end{array}\text{\hspace{1em}(9)}$$

从上述方程可解得
$$\{\begin{array}{c}\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{array}\text{\hspace{1em}(10)}$$
$$\{\begin{array}{c}4q_0{q}_1=T_{23}-T_{32}\\ 4q_0{q}_2=T_{31}-T_{13}\\ 4q_0{q}_3=T_{12}-T_{21}\end{array}\text{\hspace{1em}(11)}$$
确定$q_0$、$q_1$、$q_2$、$q_3$的符号，可按下式确定
$$\{\begin{array}{c}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{array}\text{\hspace{1em}(12)}$$
上式中，$sign(q_0)$的符号可以任选，在表示旋转上$Q$与$-Q$是等价的,$-Q$的旋转轴与$Q$完全相反，$-Q$在反向旋转轴上，旋转了$-q_0$,等价于正向旋转轴上旋转$q_0$。

算法2

论文New Method for Extracting the Quaternion from a Rotation Matrix的算法。
算法先把左乘旋转矩阵$R_{left}$转化成$K_3$矩阵，如下：
$$K_3=\frac{1}{3}\left[\begin{array}{cccc}{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{array}\right]\text{\hspace{1em}(13)}$$
$K_3$矩阵用四元数的方式表示为：
$$\frac{1}{3}\left[\begin{array}{cccc}4q_1^2-1& 4q_1{q}_2& 4q_1{q}_3& 4q_1{q}_0\\ 4q_1{q}_2& 4q_2^2-1& 4q_2{q}_3& 4q_2{q}_0\\ 4q_1{q}_3& 4q_2{q}_3& 4q_3^2-1& 4q_3{q}_0\\ 4q_1{q}_0& 4q_2{q}_0& 4q_3{q}_0& 4q_0^2-1\end{array}\right]\text{\hspace{1em}(14)}$$
矩阵可转化为
$$\frac{4}{3}\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_0\end{array}\right]\left[\begin{array}{cccc}{q}_1& q_2& q_3& q_0\end{array}\right]-\frac{1}{3}{I}_{4\ast 4}\text{\hspace{1em}(15)}$$
可以看出,$K_3$矩阵只有一个特征向量，$[q_1,q_2,q_3,q_0]$，其特征值为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
    v3=[nv1(2),nv1(3),nv1(1)];
    v4=cross(v1,v3);
    nv4=v4/norm(v4);
    q = [0, [nv4(1),nv4(2),nv4(3)]];
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}]$，如此类推，y轴[0,1,0]转到$[T_{21},T_{22},T_{23}]$,z轴[0,0,1]转到$[T_{31},T_{32},T_{33}]$。

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

3 四元数$q_1$作用下，旋转后的y、z轴向量记作$[{y_x}^{\prime },{y_y}^{\prime },{y_z}^{\prime }]$和$[{z_x}^{\prime },{z_y}^{\prime },{z_z}^{\prime }]$。$[{y_x}^{\prime },{y_y}^{\prime },{y_z}^{\prime }]$与$[T_{21},T_{22},T_{23}]$之间的角度，和$[{z_x}^{\prime },{z_y}^{\prime },{z_z}^{\prime }]$与$[T_{31},T_{32},T_{33}]$之间的角度相同。所以再加一个四元数$q_2$，使$[{y_x}^{\prime },{y_y}^{\prime },{y_z}^{\prime }]$转到$[T_{21},T_{22},T_{23}]$，就可以完成原坐标系到旋转后坐标系的转化。

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

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

---