7

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

系统 A(x_A,y_A,z_A)

和

系统 B(x_B,y_B,z_B)

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

我熟悉四元数的数学概念。我已经从这里实现了所需的数学：http ://content.gpwiki.org/index.php/OpenGL%3aTutorials%3aUsing_Quaternions_to_represent_rotation

一种可能的解决方案是计算欧拉角并将它们用于 3 个四元数。将它们相乘会得到最后一个，这样我就可以转换我的向量：

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

但这会再次包含 Gimbal Lock，这就是一开始不使用欧拉角的原因。

任何想法如何解决这个问题？

4

6 回答 6

7

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

Paul J. Besl 和 Neil D. McKay “注册 3-D 形状的方法”，传感器融合 IV：控制范式和数据结构，586（1992 年 4 月 30 日）；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.arrays 列表，其中每个数组代表一个 3D 向量。如果两个列表的长度都为 3（并且包含正交单位向量），则四元数应该是精确的变换。如果您提供更长的列表，您将获得最小化两个点集之间差异的四元数。可选的 matchlist 参数用于告诉函数 lst2 的哪个点应该转换为 lst1 的哪个点。如果未提供匹配列表，则该函数假定 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;
}
于 2014-05-20T13:01:14.940 回答
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;
 }
于 2013-05-30T17:14:18.747 回答
1

我刚刚遇到了同样的问题。我正在寻找解决方案，但我陷入了困境。

因此，您需要两个在两个坐标系中都已知的向量。就我而言，我在设备的坐标系（重力和磁场）中有 2 个正交向量，我想找到从设备坐标旋转到全局方向的四元数（其中北为正 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. 的代数接近尾声时陷入了困境。但无论哪种方式，我认为你需要的一切都在这里——也许你会比我有更好的运气。

于 2014-06-06T14:49:55.180 回答
1

按照您给它们定义 3x3 矩阵 A 和 B，因此 A 的列是 x_A、x_B 和 x_C，并且 B 的列的定义类似。那么坐标系A到B的变换T就是解TA=B，所以T=BA^{-1}。从变换的旋转矩阵 T 中，您可以使用标准方法计算四元数。

于 2019-08-08T05:47:44.020 回答
0

您需要将 B 相对于 A 的方向表示为四元数 Q。然后 B 中的任何向量都可以转换为 A 中的向量，例如，通过使用从 Q 派生的旋转矩阵 R。vectorInA = R*vectorInB。

在此站点上的 Matlab/Octave 库中有一个演示脚本（包括一个很好的可视化）：http: //simonbox.info/index.php/blog/86-rocket-news/92-quaternions-to -模型旋转

于 2013-05-30T17:07:04.403 回答
0

您可以仅使用四元数代数来计算您想要的。

给定两个单位向量 v 1和 v 2，您可以将它们直接嵌入到四元数代数中并得到相应的纯四元数 q 1和 q 2。对齐两个向量的旋转四元数 Q 使得：

Q q 1 Q * = q 2

是（谁）给的：

Q = q 1 (q 1 + q 2 )/(||q 1 + q 2 ||)

上述产品是四元数产品。

于 2019-01-30T18:39:58.150 回答