如何计算两个旋转四元数的夹角及判断旋转方向？

问题描述 #

我有一组四元数数据集，想了解每个四元数对应的旋转角度大小，以及这些旋转是否沿同一方向（顺时针/逆时针）。目前我采用以下代码计算旋转角度，但不确定是否正确：

def quaternion_angle(q1, q2):
    dot_product = np.dot(q1, q2)
    # Ensure dot product is within -1 and 1 for arccos
    dot_product = np.clip(dot_product, -1.0, 1.0)
    return np.arccos(dot_product) * 2 * 180/np.pi  # convert to degrees 

代码正确性分析 #

你的代码用来计算两个四元数之间的相对旋转角度是正确的，但有两个前提：

1. 输入的q1和q2必须是单位四元数（如果不是，需要先做归一化处理）
2. 公式2 * arccos(dot(q1, q2))的几何意义是从q1旋转到q2所需的角度，转换为角度的计算也没问题

如果要计算单个四元数自身的旋转角度（即它对应的轴角表示中的角度），逻辑是：对于单位四元数q = [w, x, y, z]，旋转角θ满足w = cos(θ/2)，因此θ的计算公式为：

def single_quaternion_angle(q):
    # 确保是单位四元数
    q = q / np.linalg.norm(q)
    w = np.clip(q[0], -1.0, 1.0)
    return 2 * np.arccos(w) * 180/np.pi

旋转方向判断 #

四元数的旋转方向由其虚部（x,y,z）定义的旋转轴和角度符号决定，判断多个旋转是否同方向可以按以下方式：

1. 对于两个四元数q_a和q_b，若它们的点积为负，说明q_b和-q_b等价（-q与q表示完全相同的旋转），但如果是判断相对旋转方向，需要计算相对旋转四元数
2. 计算相对旋转四元数：q_rel = q_target * q_source.inverse()（四元数乘法顺序注意，这里是从q_source旋转到q_target的相对四元数）
3. 比较两个相对旋转四元数的虚部点积：若点积为正，说明旋转轴方向一致，旋转方向相同；反之则相反

以下是判断旋转方向是否一致的代码示例：

import numpy as np

def quaternion_inverse(q):
    # 单位四元数的逆等于其共轭
    return np.array([q[0], -q[1], -q[2], -q[3]])

def quaternion_multiply(q1, q2):
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    w = w1*w2 - x1*x2 - y1*y2 - z1*z2
    x = w1*x2 + x1*w2 + y1*z2 - z1*y2
    y = w1*y2 - x1*z2 + y1*w2 + z1*x2
    z = w1*z2 + x1*y2 - y1*x2 + z1*w2
    return np.array([w, x, y, z])

def is_same_rotation_dir(q_source1, q_target1, q_source2, q_target2):
    # 计算两组相对旋转四元数
    q_rel1 = quaternion_multiply(q_target1, quaternion_inverse(q_source1))
    q_rel2 = quaternion_multiply(q_target2, quaternion_inverse(q_source2))
    # 归一化虚部后看点积
    v1 = q_rel1[1:] / np.linalg.norm(q_rel1[1:])
    v2 = q_rel2[1:] / np.linalg.norm(q_rel2[1:])
    return np.dot(v1, v2) > 0

关键注意事项 #

• 所有计算必须基于单位四元数，否则点积和逆的计算都会出错
• 四元数的乘法顺序很重要，相对旋转的计算顺序不能搞反

内容的提问来源于stack exchange，提问作者helixjolt