PAYATHON 2026

6DOF 机械臂上 ArUco 标记的 roll/pitch/yaw 计算为何异常？

排障赵师傅

手动公式的适用前提

在右手坐标系下使用列向量，且相对旋转满足

[ R=R_Y(\alpha)R_Z(\beta) ]

时，以下两条公式成立：

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]))

这个理想模型中，改变 Y 轴角度不会影响提取出的 Z 轴角度。如果出现约 5～10 度的联动变化，应检查实际关节轴与标记坐标轴是否对齐、标记是否存在安装偏置，以及视觉位姿估计是否有误差。

现有代码中可以确认两个问题：

  • SciPy 用小写序列表示外禀旋转，大写序列表示内禀旋转。'yzx' 是外禀旋转，原注释有误。
  • as_euler() 按序列顺序返回角度。'xzy' 返回 X、Z、Y，原来的显示代码却将其标成了 X、Y、Z。

题目没有提供可访问的测试图片地址或原始位姿数据，仅凭代码还无法确定这 5～10 度偏差的具体来源。

相对旋转 home_R.T @ global_R 何时正确？

设：

  • (R_{C\leftarrow M1})：将 M1 坐标系中的向量转换到相机坐标系；
  • (R_{C\leftarrow M2})：将 M2 坐标系中的向量转换到相机坐标系。

按 OpenCV 常用的物体到相机位姿约定：

[ p_C=R_{C\leftarrow M}p_M+t_{C\leftarrow M} ]

M2 相对于 M1 的旋转为：

[ R_{M1\leftarrow M2}

R_{C\leftarrow M1}^{T}R_{C\leftarrow M2} ]

因此，下面的计算：

R = home_R.T @ global_R

要求 home_R 表示 M1 到相机的旋转，global_R 表示 M2 到同一相机坐标系的旋转。

如果相机会移动，两者应取自同一时刻的观测。若 home_R 是相机在旧位置时保存的结果，后续相机运动也会计入相对旋转。

基座上的标记坐标系与机械臂基座坐标系也需要区分。M1 固定在基座上，说明两者之间的变换恒定，但两组坐标轴未必重合。

为什么理想模型不会发生角度串扰？

令 (c_\alpha=\cos\alpha)、(s_\alpha=\sin\alpha)，对 (\beta) 同理，则：

[ R_Y(\alpha)R_Z(\beta)= \begin{bmatrix} c_\alpha c_\beta & -c_\alpha s_\beta & s_\alpha\ s_\beta & c_\beta & 0\ -s_\alpha c_\beta & s_\alpha s_\beta & c_\alpha \end{bmatrix} ]

由矩阵元素可得：

[ \alpha=\operatorname{atan2}(R_{02},R_{22}) ]

[ \beta=\operatorname{atan2}(R_{10},R_{11}) ]

其中 (R_{10}) 和 (R_{11}) 与 (\alpha) 无关。如果固定第二个关节、只转动第一个关节时，提取出的 (\beta) 仍发生变化，就说明观测矩阵偏离了这个模型。

还需要明确矩阵乘法对应的旋转顺序。对于列向量：

[ p’=R_Y(\alpha)R_Z(\beta)p ]

右侧的 (R_Z) 先作用。若按运动坐标轴描述姿态构造，同一个乘积也可以表示先绕 Y 轴转动，再绕随第一次旋转而移动的 Z 轴转动。如果第二次旋转绕的是固定坐标系的 Z 轴，就需要重新判断乘积顺序。

因此，描述“先绕 Y，再绕 Z”时，要说明第二根轴是固定轴，还是随机构运动的轴。

优先检查标记轴与关节轴是否一致

ArUco 标记的坐标轴由位姿估计所用的三维角点定义决定。常见定义将角点放在 (Z=0) 平面上，此时 X、Y 位于标记平面内，Z 是平面法向。

题目将“标记平面内的一条轴”称为 Z 轴，需要核实这套命名与位姿估计坐标系的关系：

  • 只是给实际机械轴重新命名，OpenCV 返回的矩阵不会自动采用这套命名。
  • 已经做过坐标变换的，需要确认变换方向，以及变换矩阵应左乘还是右乘。
  • M2 与末端机构之间若有固定安装夹角，也应将其纳入模型。

用 (B) 表示机械臂基座坐标系，(T) 表示所选末端机械坐标系。假设实际运动模型为：

[ R_{B\leftarrow T}=R_Y(\alpha)R_Z(\beta) ]

则观测到的标记相对旋转为：

[ R_{M1\leftarrow M2}

R_{M1\leftarrow B} R_Y(\alpha)R_Z(\beta) R_{T\leftarrow M2} ]

提取角度前，应先消除两侧的固定变换：

[ R_{B\leftarrow T}

R_{M1\leftarrow B}^{T} R_{M1\leftarrow M2} R_{T\leftarrow M2}^{T} ]

这些固定变换需要根据安装几何或通过标定获得。旋转通常不可交换，因此直接减去零位时的 Euler 角，一般无法完成同样的修正。

SciPy 应怎样提取和标注角度？

坐标轴已经对齐、且模型满足 (R=R_Y(\alpha)R_Z(\beta)) 时，可以使用：

from scipy.spatial.transform import Rotation as Rot

r = Rot.from_matrix(R)

# 内禀 Y、Z、X；对应 R = Ry @ Rz @ Rx
angle_y, angle_z, angle_x = r.as_euler("YZX", degrees=True)

# 等价的反序外禀表示；返回顺序为 X、Z、Y
angle_x2, angle_z2, angle_y2 = r.as_euler("xzy", degrees=True)

原代码中的：

angles2 = r.as_euler("yzx", degrees=True)

表示外禀 Y、Z、X，对应的矩阵组合是：

[ R=R_X(\gamma)R_Z(\beta)R_Y(\alpha) ]

它与假设的 (R_YR_Z) 模型不同，提取结果不能直接比较。

机构只有两个转动自由度，也不代表任意 Euler 序列中都有一个角恒为零。只有坐标系和分解顺序与机构匹配时，才有这种预期；测量误差仍可能使第三个角偏离零。

Euler 角还有分支和万向节锁的问题。YZX 的中间角接近 ±90° 时，三个 Euler 角的分解会出现奇异性；超出其标准返回区间后，同一姿态也可能表示为另一组角度。对于这里明确的两参数模型，手动公式更便于按机械含义解释。

用重建误差判断两轴模型是否适用

先提取两个角，再用它们重建旋转矩阵，计算重建结果与观测结果之间的旋转角差。

用于提取角度的 R 应已消除固定坐标变换，表示机械坐标系之间的旋转：

import cv2
import numpy as np
from scipy.spatial.transform import Rotation as Rot


def rotation_yz(alpha, beta):
    """弧度输入，返回 Ry(alpha) @ Rz(beta)。"""
    ca, sa = np.cos(alpha), np.sin(alpha)
    cb, sb = np.cos(beta), np.sin(beta)

    return np.array([
        [ca * cb, -ca * sb, sa],
        [sb,       cb,      0.0],
        [-sa * cb, sa * sb, ca],
    ])


R_C_M2, _ = cv2.Rodrigues(global_rvec)
R_M1_M2 = home_R.T @ R_C_M2

# R_M1_B：机械臂基座坐标系到 M1 坐标系的固定旋转
# R_T_M2：M2 坐标系到末端机械坐标系的固定旋转
# 仅在对应坐标轴确实重合时，才可将它们设为单位矩阵。
R = R_M1_B.T @ R_M1_M2 @ R_T_M2.T

alpha = np.arctan2(R[0, 2], R[2, 2])
beta = np.arctan2(R[1, 0], R[1, 1])

R_model = rotation_yz(alpha, beta)
R_error = R_model.T @ R

model_error_deg = np.degrees(
    Rot.from_matrix(R_error).magnitude()
)

print(f"Y angle: {np.degrees(alpha):.2f} deg")
print(f"Z angle: {np.degrees(beta):.2f} deg")
print(f"Model residual: {model_error_deg:.2f} deg")

残差衡量的是观测姿态与重建姿态之间的差异。这里的 alpha、beta 直接取自矩阵元素，因此得到的两轴拟合结果在带噪数据下不一定最优。

如果需要严格施加机械约束，可以进一步求解：

[ \min_{\alpha,\beta} \left| \operatorname{Log} \left[ \left(R_Y(\alpha)R_Z(\beta)\right)^T R_{\mathrm{measured}} \right] \right|^2 ]

其中 (\operatorname{Log}) 表示旋转的旋转向量形式。拟合前仍须校准坐标轴，否则优化会把坐标偏差吸收到角度中。

排查顺序与注意事项

  1. 用已知角度构造 rotation_yz(alpha, beta)，确认提取公式与显示标签一致。
  2. 校准 M1 与基座、M2 与末端机械坐标系之间的固定旋转。
  3. 固定第二个关节，扫描第一个关节，再反向测试，同时记录提取角度和模型残差。
  4. 在机械臂完全静止时采集多帧，观察 M1、M2 各自的位姿波动，并检查相机标定、角点检测和重投影误差。

平面标记的姿态估计可能出现歧义或解跳变，两块标记的估计误差也都会计入相对旋转。视觉误差能否解释这 5～10 度偏差，需要结合标记在图像中的尺寸、视角和标定质量判断，不能仅凭偏差数值排除。

原代码中还有两处容易混淆。首先是：

rvec, _ = cv2.Rodrigues(global_R)

这里得到的仍是 M2 相对于相机的旋转向量。要得到相对旋转向量，应传入相应的相对旋转矩阵。旋转向量的三个分量也不能直接当作 roll、pitch、yaw。

其次，显示位置时使用的 tvec 不一定是 M2 相对于 M1 的平移。相对平移应按下式计算：

[ t_{M1\leftarrow M2}

R_{C\leftarrow M1}^{T} \left(t_{C\leftarrow M2}-t_{C\leftarrow M1}\right) ]

代码中建议保留 angle_y、angle_z 这类轴名。roll、pitch、yaw 的命名依赖约定，需要先明确轴、参考坐标系和旋转顺序，才能判断数值是否符合机构运动。

备注：内容仅供参考。