从四元数求解横滚/俯仰/偏航角：六轴机械臂位姿适配技术问询

核心实现路径（旋转向量→旋转矩阵→四元数） #

你的思路完全可行，下面是具体落地的代码和逻辑：

1. 旋转向量转旋转矩阵 #

用罗德里格斯公式直接计算，适配小角度近似避免数值误差：

import numpy as np

def rvec_to_rotmat(rvec):
    theta = np.linalg.norm(rvec)
    # 角度极小时直接返回单位矩阵
    if theta < 1e-6:
        return np.eye(3)
    axis = rvec / theta
    cos_t = np.cos(theta)
    sin_t = np.sin(theta)
    # 罗德里格斯公式展开计算旋转矩阵
    rotmat = cos_t * np.eye(3) + (1 - cos_t) * np.outer(axis, axis) + sin_t * np.array([
        [0, -axis[2], axis[1]],
        [axis[2], 0, -axis[0]],
        [-axis[1], axis[0], 0]
    ])
    return rotmat

2. 旋转矩阵转四元数 #

分四种情况计算，避免数值不稳定，彻底规避万向锁：

def rotmat_to_quat(rotmat):
    trace = np.trace(rotmat)
    if trace > 0:
        s = np.sqrt(trace + 1.0) * 2
        qw = 0.25 * s
        qx = (rotmat[2,1] - rotmat[1,2]) / s
        qy = (rotmat[0,2] - rotmat[2,0]) / s
        qz = (rotmat[1,0] - rotmat[0,1]) / s
    elif rotmat[0,0] > rotmat[1,1] and rotmat[0,0] > rotmat[2,2]:
        s = np.sqrt(1.0 + rotmat[0,0] - rotmat[1,1] - rotmat[2,2]) * 2
        qw = (rotmat[2,1] - rotmat[1,2]) / s
        qx = 0.25 * s
        qy = (rotmat[0,1] + rotmat[1,0]) / s
        qz = (rotmat[0,2] + rotmat[2,0]) / s
    elif rotmat[1,1] > rotmat[2,2]:
        s = np.sqrt(1.0 + rotmat[1,1] - rotmat[0,0] - rotmat[2,2]) * 2
        qw = (rotmat[0,2] - rotmat[2,0]) / s
        qx = (rotmat[0,1] + rotmat[1,0]) / s
        qy = 0.25 * s
        qz = (rotmat[1,2] + rotmat[2,1]) / s
    else:
        s = np.sqrt(1.0 + rotmat[2,2] - rotmat[0,0] - rotmat[1,1]) * 2
        qw = (rotmat[1,0] - rotmat[0,1]) / s
        qx = (rotmat[0,2] + rotmat[2,0]) / s
        qy = (rotmat[1,2] + rotmat[2,1]) / s
        qz = 0.25 * s
    return np.array([qw, qx, qy, qz])

四元数直观认知技巧 #

• 把四元数和旋转向量直接绑定：四元数[qw, qx, qy, qz]本质是[cos(θ/2), sin(θ/2)*x, sin(θ/2)*y, sin(θ/2)*z]，其中θ是旋转向量的模长，x/y/z是单位化后的旋转轴，和你输入的旋转向量一一对应。
• 用pyquaternion做验证对比，快速建立匹配感：

from pyquaternion import Quaternion

# 手动计算结果
manual_quat = rotmat_to_quat(rvec_to_rotmat([np.pi/2, 0, 0]))
# 工具包计算结果
tool_quat = Quaternion(axis=[1,0,0], angle=np.pi/2)
# 浮点误差范围内验证一致性
print(np.allclose(manual_quat, [tool_quat.w, tool_quat.x, tool_quat.y, tool_quat.z]))

WCP定位适配注意点 #

• 明确旋转向量的基准坐标系：输入的旋转向量是从基坐标系到WCP的旋转，还是相对于WCP的姿态调整，必须和机械臂的坐标系定义对齐，避免姿态偏移。
• 机械臂逆解直接传四元数：多数工业机器人SDK支持直接传入四元数作为姿态输入，不需要再转回欧拉角，从根源上避免万向锁问题。

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