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

Question

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

系统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

但是,这将再次纳入万向锁,这是不使用欧拉在开始角度的原因.

任何idead如何解决这个问题？

Answer 1

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

Paul J. Besl和Neil D. McKay"注册三维形状的方法",传感器融合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;
}

Answer 2

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

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