从四元数求相对角速度

Question

我想通过提供的矢量模拟陀螺仪读数（局部帧中的角速度）。 我有采样为1s的向量（x，y，z）：

[0, 0, 1000]
[469.47156279, 0.0, 882.94759286]
[981.62718345, 0.0, 190.80899538]

将Z沿Y轴旋转28度，然后再绕Y轴旋转51度。 向量如下所示：

我用四元数来描述相对旋转

scipy.spatial.transform.Rotation.align_vectors

以一种

1. vector_orientation = quaternion_1
2. vector_orientation = quaternion_2 * quaternion_1
3. vector_orientation = quaternion_3 * quaternion_2 * quaternion_1

我发现了这个How to compute angular velocity using numpy-quaternion问题，所以我尝试实现建议的代码来计算方向2和方向3之间的角速度：

# quaternion_3 is relative to quaternion_2, so I guess I don't need quaternion difference
delta_q = quaternion_3
delta_q_len = np.linalg.norm(delta_q[1:])
delta_q_angle = 2*np.arctan2(delta_q_len, delta_q[0])
w = delta_q[1:] * delta_q_angle * 1

我得到了一些速度-[0.0, 0.38320564, 0.0]

但是，如果我想通过使用以下等式获得导数，将其应用于四元数_2来检查结果：

因此，在Python中：

omega = np.array([[0, -w[0], -w[1], -w[2]], 
                  [w[0], 0, w[2], -w[1]], 
                  [w[1], -w[2], 0, w[0]], 
                  [w[2], w[1], -w[0], 0]])
quaternion_derivative = 0.5 * omega @ quaternion_2

并将其与

quaternion_result = (2 * quaternion_derivative / quaternion_2).normalized()

问题是quaternion_result != quaternion_3。有人知道我做错了什么吗

编辑： 添加（非）工作示例。为了更好地演示，还改变了角度。我使用https://github.com/moble/quaternion处理四元数：

import numpy as np
from scipy.spatial.transform import Rotation as R
import quaternion as qt

def get_quaternion_rotation(v1, v2):
    rotation,_ = R.align_vectors([v1], [v2])
    qt = rotation.as_quat()
    return np.array([qt[3], qt[0], qt[1], qt[2]])

def rotate_vector(vector, axis, angle):
    # normalize
    unit_axis = axis/np.linalg.norm(axis)
    angle_radians = np.radians(angle)
    rotation_vector = angle_radians * unit_axis
    rotation = R.from_rotvec(rotation_vector)
    return rotation.apply(vector)

# 1. vector is just 1000 in Z
vector_1 = np.array([0, 0, 1000])
# 2. vector rotates vector_1 28 degrees by Y
vector_2 = rotate_vector(vector_1, [0,1,0], 28)
# 3. vector rotates vector_2 51 degrees by Y
vector_3 = rotate_vector(vector_2, [0,1,0], 51)

# rotations between vectors
quaternion_1 = get_quaternion_rotation([0,0,0], vector_1)
quaternion_2 = get_quaternion_rotation(vector_2, vector_1)
quaternion_3 = get_quaternion_rotation(vector_3, vector_2)

delta_q = quaternion_3
delta_q_len = np.linalg.norm(delta_q[1:])
delta_q_angle = 2 * np.arctan2(delta_q_len, delta_q[0])
w = delta_q[1:] * delta_q_angle * 1

omega = np.array([[0, -w[0], -w[1], -w[2]], 
                  [w[0], 0, w[2], -w[1]], 
                  [w[1], -w[2], 0, w[0]], 
                  [w[2], w[1], -w[0], 0]])


qt_derivative = 0.5 * omega @ quaternion_2
# converting to quaternion type for normalizing
qt_derivative = qt.quaternion(qt_derivative[0], qt_derivative[1], qt_derivative[2], qt_derivative[3]).normalized()
# apply derivative to quaternion_2
qt_result = (2 * qt_derivative / qt.quaternion(quaternion_2[0], quaternion_2[1], quaternion_2[2], quaternion_2[3])).normalized()

print(f"Quaternion 3 = {quaternion_3}")
print(f"Quaternion result = {qt.as_float_array(qt_result)}")


rotation_quaternion_3 = R.from_quat([quaternion_3[1], quaternion_3[2], quaternion_3[3], quaternion_3[0]])
print(f"Applied rotation from quaternion_3 to vector_2: {rotation_quaternion_3.apply(vector_2)}")
rotation_result = R.from_quat([qt_result.x, qt_result.y, qt_result.z, qt_result.w])
print(f"Applied rotation from quaternion result to vector_2: {rotation_result.apply(vector_2)}")

结果是：

Quaternion 3 = [0.90258528 0.         0.4305111  0.        ]
Quaternion result = [-2.77555756e-17 -2.76809689e-17 -1.00000000e+00 -1.11022302e-16]
Applied rotation from quaternion_3 to vector_2: [981.62718345   0.         190.80899538]
Applied rotation from quaternion result to vector_2: [-4.69471563e+02  2.22044605e-13 -8.82947593e+02]