计算两个3D笛卡尔坐标系之间的转换四元数

Question

计算两个3D笛卡尔坐标系之间的转换四元数

transformationquaternionscartesian

7

我有两个已知单位向量的笛卡尔坐标系：

系统A(x_A,y_A,z_A)

和

系统B(x_B,y_B,z_B)

两个系统共享相同的原点(0,0,0)。我正在尝试计算四元数，以便可以在系统A中表示系统B中的向量。

我熟悉四元数的数学概念。我已经从这里实现了所需的数学：http://content.gpwiki.org/index.php/OpenGL%3aTutorials%3aUsing_Quaternions_to_represent_rotation 一种可能的解决方案是计算欧拉角并将其用于3个四元数。将它们相乘会得到一个最终的四元数，以便可以转换我的向量：

v(A) = q*v(B)*q_conj

但这将再次引入万向锁，这正是一开始不使用欧拉角的原因。

有什么想法如何解决这个问题吗？

- Mo3bius

6个回答

1

你使用的是什么编程语言？如果是C++，可以自由使用我的开源库：

http://sourceforge.net/p/transengine/code/HEAD/tree/transQuaternion/

简而言之，您需要将向量转换为四元数，进行计算，然后将四元数转换为变换矩阵。

以下是代码片段：

从向量获取四元数：

cQuat nTrans::quatFromVec( Vec vec ) {
    float angle = vec.v[3];
    float s_angle = sin( angle / 2);
    float c_angle = cos( angle / 2);
    return (cQuat( c_angle, vec.v[0]*s_angle, vec.v[1]*s_angle, 
                   vec.v[2]*s_angle )).normalized();
 }

并且对于从四元数转换为矩阵：

Matrix nTrans::matFromQuat( cQuat q ) {
    Matrix t;
    q = q.normalized();
    t.M[0][0] = ( 1 - (2*q.y*q.y + 2*q.z*q.z) );
    t.M[0][1] = ( 2*q.x*q.y + 2*q.w*q.z);         
    t.M[0][2] = ( 2*q.x*q.z - 2*q.w*q.y);   
    t.M[0][3] = 0;
    t.M[1][0] = ( 2*q.x*q.y - 2*q.w*q.z);        
    t.M[1][1] = ( 1 - (2*q.x*q.x + 2*q.z*q.z) ); 
    t.M[1][2] = ( 2*q.y*q.z + 2*q.w*q.x);         
    t.M[1][3] = 0;
    t.M[2][0] = ( 2*q.x*q.z + 2*q.w*q.y);       
    t.M[2][1] = ( 2*q.y*q.z - 2*q.w*q.x);        
    t.M[2][2] = ( 1 - (2*q.x*q.x + 2*q.y*q.y) );
    t.M[2][3] = 0;
    t.M[3][0] = 0;                  
    t.M[3][1] = 0;                  
    t.M[3][2] = 0;              
    t.M[3][3] = 1;
    return t;
 }

- Captain Skyhawk

尽管你的代码很有趣，但它并没有真正解决我的问题。最终我使用了方向余弦矩阵（DCM）来完成任务。如果有人能提供一种获取变换四元数的方法，我仍然很感兴趣，但我不确定是否可以直接获取此四元数而不使用欧拉或 DCM。 - Mo3bius

1

将矩阵A和B定义为3x3矩阵，使得A的列是x_A、x_B和x_C，B的列也同样定义。然后，将坐标系A转换为B的变换T是解TA = B，因此T = BA^{-1}。从变换的旋转矩阵T可以使用标准方法计算四元数。

- Carl

1

我刚遇到了同样的问题。我已经在解决方案的轨道上，但是我卡住了。

因此，您需要两个向量，在两个坐标系统中都已知。在我的情况下，我有两个规范正交向量，位于设备坐标系中，并且我想找到四元数，将设备坐标旋转到全局方向（其中北面为正Y，“上”为正Z）。因此，在我的情况下，我已经测量了设备坐标空间中的向量，并且我正在“定义”向量本身以形成全局系统的规范正交基。

话虽如此，请考虑四元数的轴角解释，存在某个向量V，可以将设备坐标绕某个角度旋转，以匹配全局坐标。我将我的（负）重力向量G和磁场M称为，并且对它们进行了归一化。

V、G和M都描述了单位球上的点。 Z_dev和Y_dev也是如此（我的设备坐标系的Z和Y基）。目标是找到一个旋转，将G映射到Z_dev，将M映射到Y_dev。为了使V将G旋转到Z_dev，由G和V定义的点之间的距离必须与由V和Z_dev定义的点之间的距离相同。在方程中：

|V - G| = |V - Z_dev|

这个方程的解形成一个平面（所有点到G和Z_dev的距离相等）。但是，V被限制为单位长度，这意味着解是以原点为中心的环形--仍然有无限多个点。

但是，对于Y_dev，M和V也是同样的情况：

|V - M| = |V - Y_dev|

这个方程的解也是以原点为中心的环形。这些环有两个交点，其中一个是另一个的负数。任何一个都是有效的旋转轴（在一种情况下旋转角度只是负数）。

使用上述两个方程，以及这些向量都是单位长度的事实，您应该能够解出V。

然后，您只需要找到要旋转的角度，这可以使用从V到相应基础（对我来说是G和Z_dev）的向量来完成。

最终，我在解决V时陷入了困境...但无论如何，我认为您需要的一切都在这里--也许您比我运气更好。

- Aaron

0

你需要用一个四元数Q来表达B相对于A的方向。然后通过使用由Q导出的旋转矩阵R，可以将B中的任意向量转换为A中的向量，例如 vectorInA = R*vectorInB。

这个网站上有一个演示脚本（包括一个漂亮的可视化效果），可以实现这一点，在Matlab/Octave库中提供：http://simonbox.info/index.php/blog/86-rocket-news/92-quaternions-to-model-rotations

- Simon Box

0

使用四元数代数即可计算所需结果。

给定两个单位向量v1和v2，您可以直接嵌入到四元数代数并获得相应的纯四元数q1和q2。旋转四元数Q将这两个向量对齐，使得：

Q q1 Q* = q2

给出：

Q = q1(q1 + q2)/(||q1 + q2||)

上述乘积是四元数乘积。

- Mauricio Cele Lopez Belon

网页内容由stack overflow 提供, 点击上面的

可以查看英文原文，
原文链接

- Arrahed · Accepted Answer

您可以通过本文描述的方法计算出代表从一个坐标系到另一个坐标系的最佳转换的四元数：

Paul J. Besl和Neil D. McKay "Method for registration of 3-D shapes", Sensor Fusion IV: Control Paradigms and Data Structures, 586 (April 30, 1992); http://dx.doi.org/10.1117/12.57955 该论文不是开放获取的，但我可以为您展示Python实现：

def get_quaternion(lst1,lst2,matchlist=None):
    if not matchlist:
        matchlist=range(len(lst1))
    M=np.matrix([[0,0,0],[0,0,0],[0,0,0]])

    for i,coord1 in enumerate(lst1):
        x=np.matrix(np.outer(coord1,lst2[matchlist[i]]))
        M=M+x

    N11=float(M[0][:,0]+M[1][:,1]+M[2][:,2])
    N22=float(M[0][:,0]-M[1][:,1]-M[2][:,2])
    N33=float(-M[0][:,0]+M[1][:,1]-M[2][:,2])
    N44=float(-M[0][:,0]-M[1][:,1]+M[2][:,2])
    N12=float(M[1][:,2]-M[2][:,1])
    N13=float(M[2][:,0]-M[0][:,2])
    N14=float(M[0][:,1]-M[1][:,0])
    N21=float(N12)
    N23=float(M[0][:,1]+M[1][:,0])
    N24=float(M[2][:,0]+M[0][:,2])
    N31=float(N13)
    N32=float(N23)
    N34=float(M[1][:,2]+M[2][:,1])
    N41=float(N14)
    N42=float(N24)
    N43=float(N34)

    N=np.matrix([[N11,N12,N13,N14],\
              [N21,N22,N23,N24],\
              [N31,N32,N33,N34],\
              [N41,N42,N43,N44]])


    values,vectors=np.linalg.eig(N)
    w=list(values)
    mw=max(w)
    quat= vectors[:,w.index(mw)]
    quat=np.array(quat).reshape(-1,).tolist()
    return quat

此函数返回所需的四元数。参数lst1和lst2是numpy数组列表，每个数组都代表一个三维向量。如果两个列表都是长度为3（且包含正交单位向量），则四元数应该是精确的变换。如果您提供了更长的列表，则会得到最小化两点集之间差异的四元数。可选的matchlist参数用于告诉函数要将lst2中哪个点转换为lst1中的哪个点。如果未提供matchlist，则函数假定lst1中的第一个点应与lst2中的第一个点匹配，依此类推...

C ++中一种类似于3点集的函数如下：

#include <Eigen/Dense>
#include <Eigen/Geometry>

using namespace Eigen;

/// Determine rotation quaternion from coordinate system 1 (vectors
/// x1, y1, z1) to coordinate system 2 (vectors x2, y2, z2)
Quaterniond QuaternionRot(Vector3d x1, Vector3d y1, Vector3d z1,
                          Vector3d x2, Vector3d y2, Vector3d z2) {

    Matrix3d M = x1*x2.transpose() + y1*y2.transpose() + z1*z2.transpose();

    Matrix4d N;
    N << M(0,0)+M(1,1)+M(2,2)   ,M(1,2)-M(2,1)          , M(2,0)-M(0,2)         , M(0,1)-M(1,0),
         M(1,2)-M(2,1)          ,M(0,0)-M(1,1)-M(2,2)   , M(0,1)+M(1,0)         , M(2,0)+M(0,2),
         M(2,0)-M(0,2)          ,M(0,1)+M(1,0)          ,-M(0,0)+M(1,1)-M(2,2)  , M(1,2)+M(2,1),
         M(0,1)-M(1,0)          ,M(2,0)+M(0,2)          , M(1,2)+M(2,1)         ,-M(0,0)-M(1,1)+M(2,2);

    EigenSolver<Matrix4d> N_es(N);
    Vector4d::Index maxIndex;
    N_es.eigenvalues().real().maxCoeff(&maxIndex);

    Vector4d ev_max = N_es.eigenvectors().col(maxIndex).real();

    Quaterniond quat(ev_max(0), ev_max(1), ev_max(2), ev_max(3));
    quat.normalize();

    return quat;
}