时间戳

首页 > 解决方案 > 如何将旋转矩阵转换为四元数

rotation - 如何将旋转矩阵转换为四元数

问题描述

我可以将旋转矩阵转换为四元数吗？我知道如何将四元数转换为旋转矩阵，但我找不到与之相反的方法。我可以向您展示如何将四元数转换为旋转矩阵的代码，如下所示。

示例（C++）：Quaterniond quat; MatrixXd t; t = quat.matrix(); 我想知道像这样将旋转矩阵转换为四元数的方法。

标签： rotationeigenquaternions

解决方案

将方向余弦矩阵 D 转换为四元数 q 的数值稳定算法如下：

T = D(1,1) + D(2,2) + D(3,3)
M = max( D(1,1), D(2,2), D(3,3), T )
qmax = (1/2) * sqrt( 1 – T + 2*M )
if( M == D(1,1) )
        qx = qmax
        qy =  ( D(1,2) + D(2,1) ) / ( 4*qmax )
        qz =  ( D(1,3) + D(3,1) ) / ( 4*qmax )
        qw = ±( D(3,2) - D(2,3) ) / ( 4*qmax )
elseif( M == D(2,2) )
        qx =  ( D(1,2) + D(2,1) ) / ( 4*qmax )
        qy = qmax
        qz =  ( D(2,3) + D(3,2) ) / ( 4*qmax )
        qw = ±( D(1,3) - D(3,1) ) / ( 4*qmax )
elseif( M == D(3,3) )
        qx =  ( D(1,3) + D(3,1) ) / ( 4*qmax )
        qy =  ( D(2,3) + D(3,2) ) / ( 4*qmax )
        qz = qmax
        qw = ±( D(1,3) - D(3,1) ) / ( 4*qmax )
else
        qx = ±( D(3,2) - D(2,3) ) / ( 4*qmax )
        qy = ±( D(1,3) - D(3,1) ) / ( 4*qmax )
        qz = ±( D(2,1) - D(1,2) ) / ( 4*qmax )
        qw = qmax
endif

请注意，四元数中存在符号歧义。上面的算法任意选择最大元素 qmax 的符号为正，但选择该符号为负同样有效（即，本质上翻转了结果的所有符号）。由用户根据应用确定哪个是更合适的选择。

± 选择是根据您使用的四元数约定进行的：

为 Hamilton Left Chain Convention 或 JPL Right Chain Convention 选择 +

选择 - 汉密尔顿右链公约或 JPL 左链公约

汉密尔顿约定意味着四元数元素 i、j、k 以右手方式进行乘法（如叉积）：

 i * j = k , j * k = i , k * i = j

JPL 约定意味着四元数元素 i、j、k 以左手方式进行乘法（叉积的负数）：

 i * j = -k , j * k = -i , k * i = -j

Right Chain 表示向量上的四元数旋转操作在右侧具有未修改的四元数：

 D * v1 = v2 = q^-1 * v1 * q

Left Chain 表示向量上的四元数旋转操作在左侧具有未修改的四元数：

 D * v1 = v2 = q * v1 * q^-1

为了完整起见，这里是另一个方向的算法，将四元数转换为方向余弦矩阵：

 D = (qw^2 - dot(qv,qv))*I3 + 2*qv*qv^T ± 2*qw*Skew(qv)

其中 ^T 表示转置（用于该术语中的外部产品）和

qv = [qx]
     [qy]
     [qz]

I3 = [1 0 0]
     [0 1 0]
     [0 0 1]

Skew(qv) = [  0 -qz  qy]
           [ qz   0 -qx]
           [-qy  qx   0]

推荐阅读