如何计算两个IMU传感器四元数之间的夹角？

要计算两个四元数对应的姿态之间的夹角，本质是求将一个姿态旋转到另一个姿态所需的旋转角度，可按以下步骤实现：

核心原理 #

两个单位四元数 q1（对应第一个IMU姿态）和 q2（对应第二个IMU姿态）之间的相对旋转四元数 q_rel，可通过 q1 的共轭与 q2 相乘得到（单位四元数的逆等于其共轭）。从相对四元数中提取旋转角度的公式为：
θ = 2 * arccos(|q_rel.w|)
其中 q_rel.w 是相对四元数的实部。

基于tf.transformations的实现代码 #

结合你已有的代码，直接添加以下步骤即可：

import numpy as np
import tf.transformations

# 你已有的四元数转换代码
quaternion_angle_1 = tf.transformations.quaternion_from_euler(roll_1, pitch_1, yaw_1)
quaternion_angle_2 = tf.transformations.quaternion_from_euler(roll_2, pitch_2, yaw_2)

# 1. 计算第一个四元数的共轭（单位四元数的逆等于共轭）
q1_conj = tf.transformations.quaternion_conjugate(quaternion_angle_1)

# 2. 计算相对旋转四元数：q_rel = q1⁻¹ * q2
q_rel = tf.transformations.quaternion_multiply(q1_conj, quaternion_angle_2)

# 3. 计算旋转角度（弧度制），用clip处理浮点精度问题
angle_rad = 2 * np.arccos(np.clip(q_rel[3], -1.0, 1.0))

# 可选：转换为角度制
angle_deg = np.degrees(angle_rad)

print(f"两个姿态的夹角（弧度）：{angle_rad}")
print(f"两个姿态的夹角（角度）：{angle_deg}")

注意事项 #

• 确保输入的四元数是单位四元数：quaternion_from_euler 函数默认输出单位四元数，无需额外归一化。
• np.clip 的作用：浮点运算误差可能导致 q_rel[3] 轻微超出 [-1, 1] 范围，用clip限制范围可避免arccos报错。

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