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

Posted 2023-04-14

【中文标题】四元数旋转与旋转矩阵略有不同

【英文标题】：quaternion rotation vs. rotation matrix have a slight difference

【发布时间】：2019-02-10 07:56:16

【问题描述】：

我有一个随时间变化的 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 变量的那一行。

【问题讨论】：

还要确保在乘法后标准化四元数，以避免尺度漂移。

【参考方案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)