4. 四元数表示姿态

任务目标

在Stewart平台运动学中，姿态描述通常采用旋转矩阵，但四元数具有无奇异性、插值方便等优势。本案例实现四元数与旋转矩阵的双向转换，并通过数值验证确保转换闭环正确。

核心实现

import numpy as np

def quat_to_rot(q):
    """四元数(q0,q1,q2,q3)转旋转矩阵, q0为实部"""
    q0, q1, q2, q3 = q
    return np.array([
        [1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3),   2*(q1*q3+q0*q2)],
        [2*(q1*q2+q0*q3),   1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)],
        [2*(q1*q3-q0*q2),   2*(q2*q3+q0*q1),   1-2*(q1**2+q2**2)]
    ])

def rot_to_quat(R):
    """旋转矩阵转四元数(Shepperd方法, 数值稳定)"""
    tr = np.trace(R)
    if tr > 0:
        S = np.sqrt(tr + 1.0) * 2
        q0 = 0.25 * S
        q1 = (R[2,1] - R[1,2]) / S
        q2 = (R[0,2] - R[2,0]) / S
        q3 = (R[1,0] - R[0,1]) / S
    elif R[0,0] > R[1,1] and R[0,0] > R[2,2]:
        S = np.sqrt(1.0 + R[0,0] - R[1,1] - R[2,2]) * 2
        q0 = (R[2,1] - R[1,2]) / S
        q1 = 0.25 * S
        q2 = (R[0,1] + R[1,0]) / S
        q3 = (R[0,2] + R[2,0]) / S
    elif R[1,1] > R[2,2]:
        S = np.sqrt(1.0 + R[1,1] - R[0,0] - R[2,2]) * 2
        q0 = (R[0,2] - R[2,0]) / S
        q1 = (R[0,1] + R[1,0]) / S
        q2 = 0.25 * S
        q3 = (R[1,2] + R[2,1]) / S
    else:
        S = np.sqrt(1.0 + R[2,2] - R[0,0] - R[1,1]) * 2
        q0 = (R[1,0] - R[0,1]) / S
        q1 = (R[0,2] + R[2,0]) / S
        q2 = (R[1,2] + R[2,1]) / S
        q3 = 0.25 * S
    return np.array([q0, q1, q2, q3])

def normalize_quat(q):
    """四元数归一化"""
    return q / np.linalg.norm(q)

# 验证: 随机生成单位四元数, 检查双向转换闭环
np.random.seed(42)
test_cases = []
for _ in range(1000):
    raw = np.random.randn(4)
    q_in = normalize_quat(raw)
    R = quat_to_rot(q_in)
    q_out = rot_to_quat(R)
    q_out = normalize_quat(q_out)
    # 处理q与-q等价性
    if np.dot(q_in, q_out) < 0:
        q_out = -q_out
    test_cases.append((q_in, q_out, R))

# 判定: 四元数误差 < 1e-10, 旋转矩阵正交性误差 < 1e-10
max_q_err = max(np.max(np.abs(q_in - q_out)) for q_in, q_out, _ in test_cases)
max_R_err = max(np.max(np.abs(R.T @ R - np.eye(3))) for _, _, R in test_cases)
print(f"最大四元数转换误差: {max_q_err:.2e}")
print(f"最大旋转矩阵正交性误差: {max_R_err:.2e}")
print("验证通过" if max_q_err < 1e-10 and max_R_err < 1e-10 else "验证失败")

运行与验证

依赖：仅需NumPy（pip install numpy）。

运行方式：将代码保存为 quat_rotation.py，执行 python quat_rotation.py。

验证逻辑：程序生成1000个随机单位四元数，依次执行四元数→旋转矩阵→四元数的往返转换。通过条件：

• 转换前后四元数最大分量误差 < 1e-10（考虑q与-q等价性）；
• 生成的旋转矩阵满足正交性（R^TR ≈ I），误差 < 1e-10。

预期输出：两行误差数值均小于1e-10，最后打印“验证通过”。

关键说明

• 旋转矩阵转四元数采用Shepperd方法，避免传统算法在特定角度（如180°）附近出现的数值不稳定问题；
• 四元数与其负值表示同一旋转，验证时需处理该等价性；
• 本实现可直接嵌入Stewart平台正逆运动学解算中，用于姿态插值或奇异规避。