四元数旋转与旋转矩阵略有不同

Question

我有一个随时间变化的 3D 旋转，表示为围绕每个轴（滚动、俯仰、偏航）的瞬时旋转。

我正在尝试随着时间的推移累积这种旋转（总共大约 50k 次测量）。我试过用两种不同的方式来做。使用旋转矩阵，并使用四元数计算。旋转矩阵的实现似乎给出了正确的结果，但我知道它不太推荐用于累积许多旋转。

这 2 个结果看起来非常相似，但随着时间的推移，这 2 个结果之间会累积轻微的差异（每 250 次测量大约 1 度）。我不确定这种差异从何而来。是不是矩阵乘法运算的浮点精度比较高，还是四元数初始化参数不对造成的

这是我使用的代码：

# Last known rotation. As quaternion and as rotation matrix
last_rotation_matrix = ....
last_quaternion_rotation = .....

# time_diff_seconds is approximately 4/1000 
# the momentary rotation speed is around 0-1 radian per second. so [roll,pitch,yaw] are around 0-0.004)
roll = rotation_x_speed * time_diff_seconds
pitch = rotation_y_speed * time_diff_seconds
yaw = rotation_z_speed * time_diff_seconds
total_rotation = np.sqrt(roll**2 + pitch**2 + yaw**2)

# This function creates a rotation matrix based on the given parameters
diff_rotation_matrix = rotation_matrix(roll, pitch, yaw)
# THIS IS THE LINE THAT I SUSPECT:
diff_quaternion_rotation = Quaternion(axis=[rotation_x_speed, rotation_y_speed, rotation_z_speed], radians=total_rotation)

new_rotation_matrix = diff_quaternion_rotation.dot(last_rotation_matrix)
new_quaternion_rotation = diff_quaternion_rotation * last_rotation_matrix

我怀疑是初始化 diff_quaternion_rotation 变量的行。

Answer 1

total_rotation = np.sqrt(roll**2 + pitch**2 + yaw**2)

这是错误的 - 不能以这种方式添加欧拉角。你的轴计算也不正确。

而是有一个 explicit algorithm 用于将欧拉角转换为四元数：

（如果您的自定义库没有此功能）：

cy, sy = cos(yaw * 0.5), sin(yaw * 0.5)
cr, sr = cos(roll * 0.5), sin(roll * 0.5)
cp, sp = cos(pitch * 0.5), sin(pitch * 0.5)

diff_quaternion_rotation = Quaternion(w = cy * cr * cp + sy * sr * sp,
                                      x = cy * sr * cp - sy * cr * sp,
                                      y = cy * cr * sp + sy * sr * cp,
                                      z = sy * cr * cp - cy * sr * sp)

四元数旋转与旋转矩阵略有不同

quaternion rotation vs. rotation matrix have a slight difference

python

3d

quaternions

gyroscope

rotational-matrices