四元数旋转与旋转矩阵有轻微差别

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)