多坐标系转换和手眼标定问题

背景：UMI 项目引入 HTC Vive 模组进行定位，现在 tracker 输出的原始坐标的坐标系是以基站为原点，我想获得桌面坐标系下的位置坐标，但 tracker 相对末端原点实际是有个转移的，tracker 和 末端可以看成是属于同一个刚体，要想获取末端坐标系（末端坐标系实际是在随末端运动而变化的）到桌面坐标系的转移矩阵，需要标定和测量哪些数据，如何标定

---

个人理解

这是一个经典的多坐标系转换 + Hand-Eye 标定问题。

Base Station 坐标系 (固定)
    ↓ T_base_to_desktop (需标定)
Desktop 坐标系 (固定)
    ↓ T_desktop_to_gripper (目标：实时计算)
Gripper 坐标系 (运动)
    ↑ T_tracker_to_gripper (需标定，固定偏移)
Tracker 坐标系 (运动)
    ↑ T_base_to_tracker (OpenVR 实时输出)
Base Station 坐标系

显然存在等式：

T_base_gripper = T_base_tracker @ T_tracker_gripper = T_base_desktop @ T_desktop_gripper

我们要得到 desktop 到 gripper 的转移矩阵 T_desktop_gripper：

T_desktop_gripper = inv（T_base_desktop）@ T_base_tracker @ T_tracker_gripper

其中 T_base_tracker 可根据 tracker 输出的原始位姿数据获得，那便只需要标定的两个变换矩阵：T_base_desktop 和 T_tracker_gripper.

T_base_desktop 可在桌面标 3 个点，记录桌面坐标系下的 3 点坐标，代表桌面坐标系，并记录对应的基站坐标系下的坐标，可据此算出 T_base_desktop;

T_tracker_to_gripper 是固定矩阵，有几种方法可以测量：

• 方法 A：物理测量。需要知道 tracker 坐标系的定义，且测量容易不准
• 方法 B：已知点反推。将 Gripper 末端对准已知的桌面标定点（如 P1），反推偏移，精度高

---

标定算法详细流程

问题建模

已知量：

• 桌面坐标系定义：X=前，Y=左，Z=上（FLU 坐标系）
• 3 个标定点在桌面坐标系下的坐标：
  • P1 = (0, 0, 0) — 原点
  • P2 = (0, -0.2, 0) — Y 轴负方向（向右）20cm
  • P3 = (0.2, 0, 0) — X 轴正方向（向前）20cm
• TCP（Tool Center Point）在接触点上方的距离：contact_offset_z（如 0.12m）
• 标定时测量的 3 个 Tracker 位姿：T_base_to_tracker_i (i=1,2,3)

未知量：

• T_base_to_desktop：基站坐标系到桌面坐标系的变换（6 自由度）
• T_tracker_to_tcp：Tracker 坐标系到 TCP 的固定变换（6 自由度）

约束方程：

对于每个标定点 i，以下两条路径应到达同一位姿（位置 + 姿态）：

路径1（从桌面出发）：
# 构建 TCP 在桌面坐标系下的位姿
T_desktop_to_tcp_i = [
    [R_desktop_to_tcp,  desktop_point_i + [0, 0, contact_offset_z]],
    [0, 0, 0, 1]
]
# 变换到基站坐标系
T_base_to_tcp_expected = T_base_to_desktop @ T_desktop_to_tcp_i

路径2（从Tracker出发）：
# 直接通过 Tracker 位姿和固定偏移得到 TCP 位姿
T_base_to_tcp_measured = T_base_to_tracker_i @ T_tracker_to_tcp

因此有约束（完整的 4x4 矩阵相等）：

T_base_to_desktop @ T_desktop_to_tcp_i = T_base_to_tracker_i @ T_tracker_to_tcp

其中：

• T_desktop_to_tcp_i：TCP 在桌面坐标系下的位姿（位置=接触点 + 偏移，姿态=标定底座姿态）
• T_tracker_to_tcp：Tracker 到 TCP 的固定变换（位置 + 姿态，需要标定求解）

姿态约束：

通过使用标定底座，可以确保 Gripper 在 3 个标定位置保持姿态一致。因此除了位置约束，还应添加姿态约束：

# 对于每个标定点 i，完整的 4x4 变换矩阵应该匹配
# 左边：从桌面出发
T_desktop_to_tcp = [
    [R_desktop_to_tcp, [0, 0, contact_offset_z]],  # 姿态对齐，TCP在接触点正上方
    [0, 0, 0, 1]
]
T_base_to_tcp_expected = T_base_to_desktop @ T_desktop_to_tcp @ translate([desktop_point_i, 0])

# 右边：从 Tracker 出发
T_base_to_tcp_measured = T_base_to_tracker_i @ T_tracker_to_tcp

# 位置残差（3维）
residual_position_i = T_base_to_tcp_measured[:3, 3] - T_base_to_tcp_expected[:3, 3]

# 姿态残差（使用旋转矩阵的 Frobenius 范数，3x3=9个元素）
R_expected = T_base_to_tcp_expected[:3, :3]
R_measured = T_base_to_tcp_measured[:3, :3]
residual_rotation_i = (R_measured - R_expected).flatten()  # 9维向量

# 或使用旋转角度差异（更紧凑，1维）
R_diff = R_expected.T @ R_measured
residual_rotation_i = arccos(clip((trace(R_diff) - 1) / 2, -1, 1))

实际应用中，通过标定底座可以保证姿态一致性，因此同时使用位置和姿态约束，提高标定精度。

算法步骤

步骤 1：生成初始估计

使用 Tracker 位置作为粗略估计，为优化算法提供起点：

# 1.1 粗略估计桌面中心：Tracker 位置的平均值
tracker_positions = [T_base_to_tracker_i[:3, 3] for i in [1,2,3]]
approximate_desktop_center = mean(tracker_positions)
 
# 1.2 粗略估计桌面法向：Tracker Z轴的平均方向
z_axes = [T_base_to_tracker_i[:3, 2] for i in [1,2,3]]
approximate_desktop_z = normalize(mean(z_axes))
 
# 1.3 构建桌面 X 轴：第1、2个 Tracker 位置的连线（投影到垂直于Z的平面）
approximate_desktop_x = tracker_positions[1] - tracker_positions[0]
approximate_desktop_x = approximate_desktop_x - dot(approximate_desktop_x, approximate_desktop_z) * approximate_desktop_z
approximate_desktop_x = normalize(approximate_desktop_x)
 
# 1.4 构建桌面 Y 轴（右手系）
approximate_desktop_y = cross(approximate_desktop_z, approximate_desktop_x)
 
# 1.5 组装初始 T_base_to_desktop
initial_T_base_to_desktop = [
    [x_x, y_x, z_x, center_x],
    [x_y, y_y, z_y, center_y],
    [x_z, y_z, z_z, center_z],
    [0,   0,   0,   1       ]
]

步骤 2：非线性最小二乘优化

使用 scipy.optimize.minimize 同时优化 T_base_to_desktop 和 T_tracker_to_tcp：

参数化：

• 旋转矩阵用 Rodrigues 旋转向量表示（3 个参数）
• 平移向量直接表示（3 个参数）
• 总共 12 个优化变量：
  • rvec_desktop (3) + tvec_desktop (3) → T_base_to_desktop
  • rvec_tcp (3) + tvec_tcp (3) → T_tracker_to_tcp

目标函数（残差）：

对于每个标定点 i：

# 构建完整的 4x4 变换矩阵
 
# 左边：从桌面出发
# TCP 在桌面坐标系下的位姿（假设姿态对齐，Z轴垂直向上）
T_desktop_to_tcp = np.eye(4)
T_desktop_to_tcp[:3, :3] = np.eye(3)  # 姿态对齐（可以是任意一致的姿态）
T_desktop_to_tcp[:3, 3] = desktop_point_i + [0, 0, contact_offset_z]
 
# TCP 在基站坐标系下的位姿（预期）
T_base_to_tcp_expected = T_base_to_desktop @ T_desktop_to_tcp
 
# 右边：从 Tracker 出发
# TCP 在基站坐标系下的位姿（测量）
T_base_to_tcp_measured = T_base_to_tracker_i @ T_tracker_to_tcp
 
# 位置残差（3维）
residual_position_i = T_base_to_tcp_measured[:3, 3] - T_base_to_tcp_expected[:3, 3]
 
# 姿态残差（使用旋转向量表示，3维）
R_expected = T_base_to_tcp_expected[:3, :3]
R_measured = T_base_to_tcp_measured[:3, :3]
R_diff = R_expected.T @ R_measured  # 相对旋转
rvec_diff = rotation_matrix_to_rodrigues(R_diff)  # 转为旋转向量
residual_rotation_i = rvec_diff  # 3维向量
 
# 组合残差（6维：3位置 + 3旋转）
residual_i = np.concatenate([residual_position_i, residual_rotation_i])

总残差向量：[residual_1, residual_2, residual_3]（18 维向量：每个点 6 维）

残差权重（可选）：

可以对位置和姿态残差设置不同权重：

# 位置权重：1.0（米）
# 姿态权重：0.1（弧度转换为等效长度，约 0.1m @ 1rad）
weight_position = 1.0
weight_rotation = 0.1
 
residual_weighted = np.concatenate([
    residual_position_i * weight_position,
    residual_rotation_i * weight_rotation
])

优化求解：

from scipy.optimize import minimize
 
result = minimize(
    fun=compute_residuals,
    x0=initial_params,  # 从步骤1的初始估计得到
    method='L-BFGS-B',  # 带边界的拟牛顿法
    options={'ftol': 1e-9, 'gtol': 1e-9}
)
 
# 解包优化结果
T_base_to_desktop_opt = params_to_matrix(result.x[:6])
T_tracker_to_tcp_opt = params_to_matrix(result.x[6:12])

步骤 3：验证和保存

计算标定误差：

position_errors = []
rotation_errors = []
 
for i in [1, 2, 3]:
    # 构建预期的 TCP 位姿
    T_desktop_to_tcp = np.eye(4)
    T_desktop_to_tcp[:3, 3] = desktop_points[i] + [0, 0, contact_offset_z]
    T_base_to_tcp_expected = T_base_to_desktop @ T_desktop_to_tcp
 
    # 测量的 TCP 位姿
    T_base_to_tcp_measured = T_base_to_tracker_i @ T_tracker_to_tcp
 
    # 位置误差
    position_error = norm(T_base_to_tcp_measured[:3, 3] - T_base_to_tcp_expected[:3, 3])
    position_errors.append(position_error)
 
    # 姿态误差（旋转角度）
    R_expected = T_base_to_tcp_expected[:3, :3]
    R_measured = T_base_to_tcp_measured[:3, :3]
    R_diff = R_expected.T @ R_measured
    rotation_angle = arccos(clip((trace(R_diff) - 1) / 2, -1, 1))
    rotation_errors.append(degrees(rotation_angle))
 
mean_position_error = mean(position_errors)
max_position_error = max(position_errors)
mean_rotation_error = mean(rotation_errors)
max_rotation_error = max(rotation_errors)
 
print(f"平均位置误差: {mean_position_error*1000:.2f} mm")
print(f"最大位置误差: {max_position_error*1000:.2f} mm")
print(f"平均姿态误差: {mean_rotation_error:.2f}°")
print(f"最大姿态误差: {max_rotation_error:.2f}°")

如果 mean_position_error < 1mm 且 mean_rotation_error < 0.5°，则标定成功，保存：

• T_base_to_desktop：用于后续将 Tracker 位姿转换到桌面坐标系
• T_tracker_to_tcp：Tracker 到 TCP 的固定偏移

实时坐标转换

标定完成后，实时运行时：

# 1. 从 OpenVR 获取实时 Tracker 位姿
T_base_to_tracker_rt = get_tracker_pose()
 
# 2. 计算 TCP 在桌面坐标系下的位姿
T_desktop_to_tcp = inv(T_base_to_desktop) @ T_base_to_tracker_rt @ T_tracker_to_tcp
 
# 3. 提取位置和旋转
position = T_desktop_to_tcp[:3, 3]  # (x, y, z) 在桌面坐标系下
rotation = T_desktop_to_tcp[:3, :3]  # 3x3 旋转矩阵

姿态角度的推算

姿态有多种表示方法，根据应用场景选择：

1. 旋转矩阵 → 欧拉角（ZYX 顺序）

import numpy as np
 
def rotation_matrix_to_euler_zyx(R):
    """
    旋转矩阵转欧拉角（ZYX内旋顺序，即 Roll-Pitch-Yaw）
 
    Args:
        R: 3x3 旋转矩阵
 
    Returns:
        (roll, pitch, yaw) 弧度，对应绕 X-Y-Z 轴的旋转
    """
    # 处理万向锁情况
    sy = np.sqrt(R[0, 0]**2 + R[1, 0]**2)
 
    singular = sy < 1e-6
 
    if not singular:
        roll = np.arctan2(R[2, 1], R[2, 2])      # 绕 X 轴
        pitch = np.arctan2(-R[2, 0], sy)         # 绕 Y 轴
        yaw = np.arctan2(R[1, 0], R[0, 0])       # 绕 Z 轴
    else:
        # 万向锁情况（pitch = ±90°）
        roll = np.arctan2(-R[1, 2], R[1, 1])
        pitch = np.arctan2(-R[2, 0], sy)
        yaw = 0
 
    return roll, pitch, yaw
 
# 使用示例
R = T_desktop_to_tcp[:3, :3]
roll, pitch, yaw = rotation_matrix_to_euler_zyx(R)
print(f"Roll:  {np.degrees(roll):.2f}°")   # 绕 X 轴（横滚）
print(f"Pitch: {np.degrees(pitch):.2f}°")  # 绕 Y 轴（俯仰）
print(f"Yaw:   {np.degrees(yaw):.2f}°")    # 绕 Z 轴（偏航）

2. 旋转矩阵 → 四元数

def rotation_matrix_to_quaternion(R):
    """
    旋转矩阵转四元数（w, x, y, z）
 
    Args:
        R: 3x3 旋转矩阵
 
    Returns:
        (w, x, y, z) 四元数，满足 w² + x² + y² + z² = 1
    """
    trace = np.trace(R)
 
    if trace > 0:
        s = 0.5 / np.sqrt(trace + 1.0)
        w = 0.25 / s
        x = (R[2, 1] - R[1, 2]) * s
        y = (R[0, 2] - R[2, 0]) * s
        z = (R[1, 0] - R[0, 1]) * s
    elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
        s = 2.0 * np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])
        w = (R[2, 1] - R[1, 2]) / s
        x = 0.25 * s
        y = (R[0, 1] + R[1, 0]) / s
        z = (R[0, 2] + R[2, 0]) / s
    elif R[1, 1] > R[2, 2]:
        s = 2.0 * np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])
        w = (R[0, 2] - R[2, 0]) / s
        x = (R[0, 1] + R[1, 0]) / s
        y = 0.25 * s
        z = (R[1, 2] + R[2, 1]) / s
    else:
        s = 2.0 * np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])
        w = (R[1, 0] - R[0, 1]) / s
        x = (R[0, 2] + R[2, 0]) / s
        y = (R[1, 2] + R[2, 1]) / s
        z = 0.25 * s
 
    return w, x, y, z
 
# 使用示例
R = T_desktop_to_tcp[:3, :3]
w, x, y, z = rotation_matrix_to_quaternion(R)
print(f"Quaternion: w={w:.4f}, x={x:.4f}, y={y:.4f}, z={z:.4f}")

3. 旋转矩阵 → 旋转向量（轴角表示）

def rotation_matrix_to_axis_angle(R):
    """
    旋转矩阵转轴角表示（Rodrigues 旋转向量）
 
    Args:
        R: 3x3 旋转矩阵
 
    Returns:
        rvec: 3维旋转向量，方向为旋转轴，模长为旋转角度（弧度）
    """
    # 使用 OpenCV 的实现（如果可用）
    try:
        import cv2
        rvec, _ = cv2.Rodrigues(R)
        return rvec.flatten()
    except ImportError:
        pass
 
    # 手动实现
    angle = np.arccos((np.trace(R) - 1) / 2)
 
    if angle < 1e-10:
        # 接近单位矩阵，无旋转
        return np.zeros(3)
 
    if angle < np.pi - 1e-6:
        # 一般情况
        axis = np.array([
            R[2, 1] - R[1, 2],
            R[0, 2] - R[2, 0],
            R[1, 0] - R[0, 1]
        ]) / (2 * np.sin(angle))
    else:
        # 旋转角接近 180°，需要特殊处理
        # 找到最大对角元素
        i = np.argmax(np.diag(R))
        axis = np.zeros(3)
        axis[i] = np.sqrt((R[i, i] + 1) / 2)
        j = (i + 1) % 3
        k = (i + 2) % 3
        axis[j] = R[i, j] / (2 * axis[i])
        axis[k] = R[i, k] / (2 * axis[i])
 
    return axis * angle
 
# 使用示例
R = T_desktop_to_tcp[:3, :3]
rvec = rotation_matrix_to_axis_angle(R)
angle = np.linalg.norm(rvec)
axis = rvec / angle if angle > 1e-10 else np.array([0, 0, 1])
print(f"旋转轴: {axis}")
print(f"旋转角: {np.degrees(angle):.2f}°")

4. 姿态差异的度量

比较两个姿态的差异：

def rotation_difference_angle(R1, R2):
    """
    计算两个旋转矩阵之间的角度差异
 
    Args:
        R1, R2: 3x3 旋转矩阵
 
    Returns:
        angle: 角度差异（弧度）
    """
    # 相对旋转矩阵
    R_diff = R1.T @ R2
 
    # 提取旋转角度
    trace = np.trace(R_diff)
    angle = np.arccos(np.clip((trace - 1) / 2, -1.0, 1.0))
 
    return angle
 
# 使用示例：比较标定时的姿态一致性
R1 = T_base_to_tracker_1[:3, :3]
R2 = T_base_to_tracker_2[:3, :3]
angle_diff = rotation_difference_angle(R1, R2)
print(f"姿态差异: {np.degrees(angle_diff):.2f}°")

5. 实际应用建议

标定时的姿态检查：

# 在标定对话框中显示姿态信息
def check_calibration_pose_consistency(t_base_to_trackers):
    """
    检查3次标定测量的姿态一致性
 
    Returns:
        max_angle_diff: 最大姿态差异（度）
    """
    rotations = [T[:3, :3] for T in t_base_to_trackers]
 
    max_diff = 0
    for i in range(len(rotations)):
        for j in range(i + 1, len(rotations)):
            diff = rotation_difference_angle(rotations[i], rotations[j])
            max_diff = max(max_diff, diff)
 
    return np.degrees(max_diff)
 
# 使用
max_angle_diff = check_calibration_pose_consistency([T1, T2, T3])
if max_angle_diff > 5.0:
    print(f"⚠️ 警告：标定时姿态差异过大 ({max_angle_diff:.2f}°)")
    print("   建议重新标定，保持姿态一致")

实时运行时的姿态输出：

# 获取 Gripper 在桌面坐标系下的姿态
T_desktop_to_tcp = inv(T_base_to_desktop) @ T_base_to_tracker_rt @ T_tracker_to_tcp
 
# 提取位置
position = T_desktop_to_tcp[:3, 3]
 
# 提取姿态（根据需要选择表示方式）
rotation_matrix = T_desktop_to_tcp[:3, :3]
 
# 方式1：欧拉角（适合人类理解）
roll, pitch, yaw = rotation_matrix_to_euler_zyx(rotation_matrix)
print(f"位置: ({position[0]:.3f}, {position[1]:.3f}, {position[2]:.3f})")
print(f"姿态: Roll={np.degrees(roll):.1f}° Pitch={np.degrees(pitch):.1f}° Yaw={np.degrees(yaw):.1f}°")
 
# 方式2：四元数（适合插值和避免万向锁）
w, x, y, z = rotation_matrix_to_quaternion(rotation_matrix)
pose_data = {
    "position": position.tolist(),
    "orientation": {"w": w, "x": x, "y": y, "z": z}
}

数学原理

这是一个非线性最小二乘问题，类似于机器人标定中的 Hand-Eye 标定：

• 冗余约束：3 个标定点提供 18 个方程（每点 6 维：3 位置 +3 姿态），但未知量只有 12 个（T_base_to_desktop 6 自由度 + T_tracker_to_tcp 6 自由度），系统超定
• 鲁棒性：利用所有测量点同时优化，误差相互抵消，比逐点迭代更稳定
• 收敛性：L-BFGS-B 算法对于这类问题收敛速度快，通常 10-20 次迭代即可收敛

标定点数量的影响

理论上：

• 最少点数：3 个点（提供 18 个方程，求解 12 个未知数）—— 系统超定，有唯一解
• 更多点数：4 个、5 个或更多点 —— 增加冗余约束，理论上会提高精度

实际效果：

标定点数    方程数    超定程度    预期效果
3个点      18个     1.5倍       基准精度
4个点      24个     2.0倍       误差降低 ~15-20%
5个点      30个     2.5倍       误差降低 ~20-30%
6+个点     36+个    3.0+倍      边际收益递减

为什么更多点不是总是更好：

1. 测量误差累积：每个点都有测量误差，点数过多可能引入更多噪声
2. 姿态一致性难度：标定点越多，保持所有点姿态完全一致的难度指数增加
3. 计算成本：优化变量固定（12 个），但残差维度线性增长（6N 维），计算时间增加
4. 过拟合风险：如果标定点分布不均匀（例如都在桌面一角），会降低泛化能力

最佳实践：

# 推荐配置：3-4个点，呈三角形或正方形分布
标定点布局示例（俯视图）：
 
方案A（3点 - 三角形）：     方案B（4点 - 正方形）：
    P3 (0.2, 0, 0)              P3 (0.2, 0.2, 0)  P4 (0.2, -0.2, 0)
     ▲                           ▲                  ▲
    /│\                         /│\                /│\
   / │ \                       / │ \              / │ \
  /  │  \                     /  │  \            /  │  \
 /   │   \                   /   │   \          /   │   \
P1 ──┼──→ P2               P1 ───┼───→ P2      /    │    \
(0,0,0) (0,-0.2,0)        (0,0,0) (0,-0.2,0)  ← ────┼──── →
 
优点：最少点数，快速      优点：对称分布，鲁棒     需要：更大标定空间

增加标定点的建议：

• ✅ 应该增加：如果标定误差 > 2mm，或桌面较大（>0.5m²）
• ✅ 应该增加：如果工作空间边缘精度要求高（在边缘增加标定点）
• ❌ 不建议增加：如果标定底座无法保证姿态一致性
• ❌ 不建议增加：如果标定点过于密集（<10cm 间距），测量误差占比大

姿态差异对标定结果的影响

影响机制：

姿态约束方程：

R_base_to_tcp_measured = T_base_to_tracker @ T_tracker_to_tcp[:3,:3]
R_base_to_tcp_expected = T_base_to_desktop @ R_desktop_to_tcp

如果 3 个标定点的姿态不一致（例如 P1 竖直，P2 倾斜 5°，P3 倾斜 -3°），会导致：

1. 姿态约束冲突：优化器尝试找到一个 T_tracker_to_tcp 同时满足 3 个不同的旋转关系 → 无精确解
2. 优化器妥协：最小二乘会找到一个 ” 折中 ” 的 T_tracker_to_tcp，使总残差最小 → 系统性偏差
3. 位置精度下降：姿态误差通过杠杆效应放大位置误差（Δp = L × Δθ，L 为臂长）

定量分析：

假设标定底座姿态差异为 Δθ，TCP 到 Tracker 的距离为 L = 0.15m：

姿态差异 Δθ    位置误差贡献 (L×Δθ)    对标定精度的影响
0.5°           0.15m × 0.009 ≈ 1.3mm   可接受（< 2mm）
1.0°           0.15m × 0.017 ≈ 2.6mm   临界（≈ 2mm 目标）
2.0°           0.15m × 0.035 ≈ 5.2mm   不可接受（>> 2mm）
5.0°           0.15m × 0.087 ≈ 13mm    严重偏差

实际建议：

# 标定时检查姿态一致性
max_angle_diff = check_calibration_pose_consistency([T1, T2, T3])
 
if max_angle_diff < 0.5:
    print("✅ 姿态一致性极佳，预期精度 < 1mm")
elif max_angle_diff < 1.0:
    print("✅ 姿态一致性良好，预期精度 1-2mm")
elif max_angle_diff < 2.0:
    print("⚠️ 姿态差异较大，预期精度 2-5mm，建议重新标定")
else:
    print("❌ 姿态差异过大，标定结果不可靠，必须使用标定底座！")
    raise ValueError("姿态不一致，无法完成高精度标定")

为什么标定底座很重要：

无标定底座（手持标定）：         有标定底座：
- 姿态差异 3-10°                 - 姿态差异 < 0.5°
- 位置误差 5-20mm                - 位置误差 < 1mm
- 重复性差                        - 重复性好
- 不适合精密操作                  - 适合精密操作

应对姿态差异的策略：

1. 使用标定底座（推荐）：机械约束保证姿态一致性

2. 降低姿态权重（妥协方案）：

   weight_position = 1.0
   weight_rotation = 0.01  # 大幅降低姿态权重，只约束位置

   缺点：放弃姿态约束，T_tracker_to_tcp 的旋转部分可能不准确

3. 多次测量平均：每个标定点测量 5-10 次，使用平均姿态

标定精度影响因素（按重要性排序）

因素	影响程度	说明	改进方法
1. 姿态一致性	⭐⭐⭐⭐⭐	姿态差异 1° → 位置误差 ~2.6mm	使用标定底座（必须！）
2. 标定点位置精度	⭐⭐⭐⭐	标定点偏差 1mm → 系统偏差 1mm	使用精密标定板、激光定位
3. Tracker 测量噪声	⭐⭐⭐	测量噪声 0.5mm → 标定误差 ~0.3mm	多次采样平均（20 次）
4. contact_offset_z 精度	⭐⭐⭐	偏差 1mm → Z 方向系统误差 1mm	精确测量工具（游标卡尺）
5. 标定点数量	⭐⭐	3 点 vs 4 点 → 误差降低 ~15%	在精度要求高时增加到 4 点
6. 标定点分布	⭐⭐	聚集 vs 分散 → 边缘精度差异	覆盖整个工作空间

典型标定精度（理想条件）：

配置                              位置误差        旋转误差
标定底座 + 3点 + 精密标定板        < 1mm          < 0.5°
标定底座 + 4点 + 精密标定板        < 0.7mm        < 0.3°
手持标定（无底座）                 5-20mm         2-10°  ❌ 不推荐

标定质量检查清单：

# 标定完成后的验证步骤
1. 检查姿态一致性：max_angle_diff < 1.0° ✅
2. 检查位置残差：mean_position_error < 1mm ✅
3. 检查姿态残差：mean_rotation_error < 0.5° ✅
4. 检查重复性：重新标定 3 次，结果差异 < 0.5mm ✅
5. 检查边缘精度：在工作空间边缘测试，误差 < 2mm ✅