6DOF机械臂ArUco标记Roll/Pitch/Yaw角度计算问题求助

问题概述 #

需要计算6DOF机械臂上ArUco标记（M2）相对于基座标记（M1）的旋转角度，机械结构约束M2仅能先绕垂直轴（Y轴）旋转，再绕标记平面轴（Z轴）旋转。当前通过home_R.T @ R计算相对旋转矩阵，手动推导的角度提取公式结果接近预期，但存在Y轴旋转影响Z轴角度（偏差5-10度）的问题；尝试SciPy欧拉角提取时结果偏差大，且返回三轴旋转（不符合机械约束）。

当前实现与问题 #

手动角度提取代码 #

def rotate_to_base(home_R, R):
    return home_R.T @ R

# 计算相对旋转矩阵
global_R, _ = cv2.Rodrigues(global_rvec)
R = rotate_to_base(home_R, global_R)

# 手动提取角度（Y*Z顺序）
theta_y_deg = np.degrees(np.arctan2(R[0,2], R[2,2]))
theta_z_deg = np.degrees(np.arctan2(R[1,0], R[1,1]))

核心问题分析 #

1. 矩阵元素映射错误导致角度耦合
   假设旋转顺序为先绕自身Y轴（内旋），再绕自身Z轴（内旋），总旋转矩阵应为R = R_z(theta_z) @ R_y(theta_y)。展开后矩阵元素如下：

   R = [
       [cosθz cosθy, -sinθz, cosθz sinθy],
       [sinθz cosθy, cosθz, sinθz sinθy],
       [-sinθy,      0,      cosθy]
   ]
   

   当前提取公式存在两个关键错误：

   • 提取theta_y时使用了R[0,2]（含cosθz项），导致Z轴旋转会干扰Y角度计算；正确的Y角度应从R[2,0]和R[2,2]提取，这两个元素仅与theta_y相关。
   • 提取theta_z时使用了R[1,0]（含cosθy项），导致Y轴旋转会干扰Z角度计算；正确的Z角度应从R[0,1]和R[1,1]提取，这两个元素仅与theta_z相关。

2. SciPy欧拉角顺序不匹配
   使用的xzy/yzx顺序与机械约束的Y-Z旋转顺序不匹配，导致提取出冗余的X轴角度，且结果偏差。需要指定与机械约束一致的旋转顺序。

修正方案 #

1. 修正手动角度提取公式 #

使用仅与目标角度相关的矩阵元素，消除耦合：

# 提取Y轴旋转角（不受Z旋转影响）
theta_y_deg = np.degrees(np.arctan2(-R[2, 0], R[2, 2]))
# 提取Z轴旋转角（不受Y旋转影响）
theta_z_deg = np.degrees(np.arctan2(-R[0, 1], R[1, 1]))

2. 正确使用SciPy提取欧拉角 #

指定与机械约束匹配的内旋Y→Z顺序，SciPy会自动约束X轴角度为0（符合机械结构限制）：

from scipy.spatial.transform import Rotation as Rot

r = Rot.from_matrix(R)
# 内旋顺序：先Y轴，后Z轴，仅返回Y、Z角度（X轴理论为0）
angles_yz = r.as_euler('yz', degrees=True)
theta_y_scipy = angles_yz[0]
theta_z_scipy = angles_yz[1]
theta_x_scipy = angles_yz[2]  # 因测量/标定误差可能存在微小值

3. 额外验证步骤 #

• 用已知姿态的标定板测试：固定Y轴角度，调整Z轴角度，验证Z角度提取是否稳定；反之亦然。
• 确认home_R的正确性：确保基座标记M1的旋转矩阵是相对于世界坐标系的准确姿态，否则相对旋转矩阵计算会出错。

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