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

本文为博主“声时刻”原创文章,未经博主允许不得转载。
联系方式:[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} aRleft=b(1)

  • 右乘矩阵:
    R r i g h t ∗ a ′ = b ′ (2) R_{right}\ast {a}'={b}'\tag{2} Rrighta=b(2)

  • 旋转矩阵的性质:

R ∗ R ′ = I (3) R\ast {R}'=I \tag{3} RR=I(3) R ∗ R − 1 = I (4) R\ast R^{-1}=I \tag{4} RR1=I(4) R ′ = R − 1 (5) {R}'= R^{-1} \tag{5} R=R1(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+q12q22q322(q1q2q0q3)2(q0q2+q1q3)2(q1q2+q0q3)q02+q22q12q322(q2q3q0q1)2(q1q3q0q2)2(q2q3+q0q1)q02+q32q12q22(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+q12q22q32=T11q02q12+q22q32=T22q02q12q22+q32=T33q02+q12+q22+q32=12(q1q2+q0q3)=T122(q1q3q0q2)=T132(q1q2q0q3)=T212(q2q3+q0q1)=T232(q0q2+q1q3)=T312(q2q3q0q1)=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+T11T22T33 q2=211T11+T22T33 q3=211T11T22+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=T23T324q0q2=T31T134q0q3=T12T21(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(T23T32)sign(q2)=sign(q0)sign(T31T13)sign(q3)=sign(q0)sign(T12T21)(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=31T11T22T33T21+T12T31+T13T23T32T21+T12T22T11T33T32+T23T31T13T31+T13T32+T23T33T11T22T12T21T23T32T31T13T12T21T11+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} 314q1214q1q24q1q34q1q04q1q24q2214q2q34q2q04q1q34q2q34q3214q3q04q1q04q2q04q3q04q021(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} 34q1q2q3q0[q1q2q3q0]31I44(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的旋转。


你可能感兴趣的:(imu)