当前位置：首页 > news >正文

通过四元数求机器人本体坐标旋转量

news 2025/9/16 6:59:22

是的，通过两次姿态数据（以四元数表示）的差值，可以确定机器人在两个时刻之间的旋转角度变化。具体步骤如下：

获取四元数：假设两个时刻的四元数分别为 ( q_1 ) 和 ( q_2 )。
计算四元数的差值：
- 将四元数 ( q_1 ) 的逆（反转）表示为 ( q_1^{-1} )。
- 进行四元数乘法：( q_{\Delta} = q_2 \cdot q_1^{-1} )，得到差值四元数 ( q_{\Delta} )。
转换为旋转角度：
- 从差值四元数 ( q_{\Delta} ) 中提取旋转轴 ( \mathbf{v} ) 和旋转角度 ( \theta )。
- 四元数 ( q_{\Delta} = \left(\cos\left(\frac{\theta}{2}\right), \sin\left(\frac{\theta}{2}\right)\mathbf{v}\right) )。

对于具体的计算，你可以使用以下公式：

四元数的逆：( q_1^{-1} = (q_{w1}, -q_{x1}, -q_{y1}, -q_{z1}) )（假设 ( q_1 = (q_{w1}, q_{x1}, q_{y1}, q_{z1}) )）。
四元数乘法：( q_{\Delta} = q_2 \cdot q_1^{-1} )。

最后，从 ( q_{\Delta} ) 提取旋转轴和旋转角度。

import numpy as npdef quaternion_conjugate(q):q = np.array(q)return np.array([q[0], -q[1], -q[2], -q[3]])def quaternion_multiply(q1, q2):w1, x1, y1, z1 = q1w2, x2, y2, z2 = q2w = w1*w2 - x1*x2 - y1*y2 - z1*z2x = w1*x2 + x1*w2 + y1*z2 - z1*y2y = w1*y2 - x1*z2 + y1*w2 + z1*x2z = w1*z2 + x1*y2 - y1*x2 + z1*w2return np.array([w, x, y, z])def quaternion_to_axis_angle(q):if q[0] > 1:q = q / np.linalg.norm(q)  # normalize the quaternion if neededangle = 2 * np.arccos(q[0])s = np.sqrt(1 - q[0]**2)  # assuming q[0] is the scalar partif s < 0.001:x = q[1]y = q[2]z = q[3]else: x = q[1] / sy = q[2] / sz = q[3] / sreturn angle, np.array([x, y, z])# 示例四元数
q1 = [0.9659, 0, 0.2588, 0]  # 初始姿态
q2 = [1, 0, 0, 0]            # 最终姿态# 计算 q1 的逆
q1_inv = quaternion_conjugate(q1)# 计算差值四元数 q_Δ
q_delta = quaternion_multiply(q2, q1_inv)# 提取旋转轴和角度
angle, axis = quaternion_to_axis_angle(q_delta)print(f"旋转角度: {np.degrees(angle)} 度")
print(f"旋转轴: {axis}")