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

版权声明：本文为博主原创文章，未经博主允许不得转载。

博主：shenshikexmu

联系方式：[e-mail]

旋转矩阵的两种表示

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

• 左乘矩阵：
  

  a∗Rleft=b

• 右乘矩阵：
  

  Rright∗a′=b′

• 旋转矩阵的性质：

R∗R′=I

R∗R−1=I

R′=R−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

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

旋转矩阵转四元数

算法1

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

⎡⎣⎢T11T21T31T12T22T32T13T23T33⎤⎦⎥

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

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪q20+q21−q22−q23=T11q20−q21+q22−q23=T22q20−q21−q22+q23=T33q20+q21+q22+q23=12(q1q2+q0q3)=T122(q1q3−q0q2)=T132(q1q2−q0q3)=T212(q2q3+q0q1)=T232(q0q2+q1q3)=T312(q2q3−q0q1)=T32

从上述方程可解得

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪|q0|=121+T11+T22+T33−−−−−−−−−−−−−−−√|q1|=121+T11−T22−T33−−−−−−−−−−−−−−−√|q2|=121−T11+T22−T33−−−−−−−−−−−−−−−√|q3|=121−T11−T22+T33−−−−−−−−−−−−−−−√

⎧⎩⎨⎪⎪4q0q1=T23−T324q0q2=T31−T134q0q3=T12−T21

确定q0、q1、q2、q3的符号，可按下式确定

⎧⎩⎨⎪⎪sign(q1)=sign(q0)sign(T23−T32)sign(q2)=sign(q0)sign(T31−T13)sign(q3)=sign(q0)sign(T12−T21)

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

算法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)];    endend

算法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*qqq=[...        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 quaternionsnv1 = 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)];endend

function [vector]=qMultiVec(vec,q)  %sensor frame to world framex = 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轴 向量[T11,T12,T13]，如此类推，y轴[0,1,0]转到[T21,T22,T23],z轴[0,0,1]转到[T31,T32,T33]。

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

3 四元数q1作用下，旋转后的y、z轴向量记作[yx′,yy′,yz′]和[zx′,zy′,zz′]。[yx′,yy′,yz′]与[T21,T22,T23]之间的角度，和[zx′,zy′,zz′]与[T31,T32,T33]之间的角度相同。所以再加一个四元数q2，使[yx′,yy′,yz′]转到[T21,T22,T23]，就可以完成原坐标系到旋转后坐标系的转化。

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

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

---