3. 手眼标定原理与数学模型

任务目标

本案例完成手眼标定的数学建模：推导眼在手上（eye-in-hand）与眼在手外（eye-to-hand）两种构型下，机械臂末端与相机之间位姿变换关系的统一表达式 AX = XB，并给出基于该模型的求解思路与最小验证方法。

坐标系定义与符号约定

符号	含义
`H_base→end`	机械臂基座到末端法兰的齐次变换矩阵（由机器人正运动学给出）
`H_cam→obj`	相机坐标系到标定板（物体）坐标系的变换（由相机外参标定给出）
`X`	待求的手眼变换矩阵（相机与末端之间的固定关系）

眼在手上（Eye-in-Hand）推导

相机固定在机械臂末端，标定板固定在世界中。对两个不同机械臂位姿 i 和 j，标定板在相机坐标系下的位姿满足闭环约束：

H<sub>base→end</sub><sup>i</sup> · X · H<sub>cam→obj</sub><sup>i</sup> = H<sub>base→end</sub><sup>j</sup> · X · H<sub>cam→obj</sub><sup>j</sup>

整理后得到：

(H<sub>base→end</sub><sup>j</sup>)<sup>−1</sup> · H<sub>base→end</sub><sup>i</sup> · X = X · H<sub>cam→obj</sub><sup>j</sup> · (H<sub>cam→obj</sub><sup>i</sup>)<sup>−1</sup>

令 A = (H_end^j→base) · H_{base→endⁱ}，B = H_cam→obj^j · H_obj→camⁱ，即得标准形式：

<strong>AX = XB</strong>

眼在手外（Eye-to-Hand）推导

相机固定在世界中，标定板固定在机械臂末端。同样取两个位姿 i、j，标定板在相机坐标系下的位姿满足：

H<sub>cam→obj</sub><sup>i</sup> = H<sub>cam→base</sub> · H<sub>base→end</sub><sup>i</sup> · X

其中 X 为标定板到末端的固定变换。两式相消后同样得到 AX = XB，其中：

A = H<sub>base→end</sub><sup>j</sup> · (H<sub>base→end</sub><sup>i</sup>)<sup>−1</sup>
B = (H<sub>cam→obj</sub><sup>j</sup>)<sup>−1</sup> · H<sub>cam→obj</sub><sup>i</sup>

AX=XB 的求解原理

将齐次矩阵分解为旋转 R 和平移 t，AX=XB 等价于：

R<sub>A</sub>·R<sub>X</sub> = R<sub>X</sub>·R<sub>B</sub>
R<sub>A</sub>·t<sub>X</sub> + t<sub>A</sub> = R<sub>X</sub>·t<sub>B</sub> + t<sub>X</sub>

旋转部分用轴角表示后，可通过线性最小二乘或四元数方法求解 R_X，再代入平移方程解出 t_X。实际工程中通常采集 15～20 组不同位姿数据，用 Tsai-Lenz 或 Daniilidis 等方法求解。

最小验证实现（Python + NumPy）

以下代码构造两组已知变换，验证 AX=XB 关系成立，并演示如何从两组数据中恢复 X（使用解析两步法）：

import numpy as np

def make_T(R, t):
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

def rot_z(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])

def rot_x(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])

# 构造真实手眼变换 X（待求解目标）
X_true = make_T(rot_x(0.3) @ rot_z(0.2), [0.1, -0.05, 0.15])

# 生成两组机械臂位姿（眼在手上示例）
T_bi = make_T(rot_z(0.5), [0.3, 0.1, 0.2])   # base→end i
T_bj = make_T(rot_z(0.8) @ rot_x(0.1), [0.4, -0.1, 0.3])  # base→end j

# 由闭环约束反推相机观测到的标定板位姿
T_ci = np.linalg.inv(T_bi) @ X_true   # 实际应为 X^-1 @ T_bi^-1，此处为演示简化
# 正确构造：T_bi @ X @ T_ci = T_bj @ X @ T_cj
# 令 T_ci = inv(X) @ inv(T_bi) @ T_bj @ X @ T_cj，取 T_cj = I 简化
T_cj = np.eye(4)
T_ci = np.linalg.inv(X_true) @ np.linalg.inv(T_bi) @ T_bj @ X_true @ T_cj

# 计算 A 和 B
A = np.linalg.inv(T_bj) @ T_bi
B = T_cj @ np.linalg.inv(T_ci)

# 验证 AX == XB
AX = A @ X_true
XB = X_true @ B
print("AX == XB 误差（应接近0）:", np.max(np.abs(AX - XB)))

# ---- 从 A、B 恢复 X（旋转部分：轴角线性法）----
def rot_to_axisangle(R):
    angle = np.arccos(np.clip((np.trace(R) - 1) / 2, -1, 1))
    if angle < 1e-8:
        return np.zeros(3), 0.0
    r = np.array([R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]])
    return r / (2*np.sin(angle)), angle

def axisangle_to_rot(axis, angle):
    K = np.array([[0, -axis[2], axis[1]],
                  [axis[2], 0, -axis[0]],
                  [-axis[1], axis[0], 0]])
    return np.eye(3) + np.sin(angle)*K + (1-np.cos(angle))*(K@K)

# 取两组数据（实际需多组最小二乘，此处演示两组求解）
A1, B1 = A, B
# 构造第二组数据
T_bi2 = make_T(rot_z(1.0) @ rot_x(0.2), [0.2, 0.3, -0.1])
T_bj2 = make_T(rot_z(1.3) @ rot_x(0.4), [0.5, 0.2, 0.1])
T_cj2 = np.eye(4)
T_ci2 = np.linalg.inv(X_true) @ np.linalg.inv(T_bi2) @ T_bj2 @ X_true @ T_cj2
A2 = np.linalg.inv(T_bj2) @ T_bi2
B2 = T_cj2 @ np.linalg.inv(T_ci2)

# 旋转部分：P·r_x = Q，其中 P、Q 由轴角叉乘构造
def skew(v):
    return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])

P = np.zeros((6, 3))
Q = np.zeros(6)
for idx, (Ai, Bi) in enumerate([(A1, B1), (A2, B2)]):
    rA, angA = rot_to_axisangle(Ai[:3,:3])
    rB, angB = rot_to_axisangle(Bi[:3,:3])
    P[3*idx:3*idx+3] = skew(rA + rB)
    Q[3*idx:3*idx+3] = rA - rB

r_x = np.linalg.lstsq(P, Q, rcond=None)[0]
angle_x = 2 * np.arctan2(np.linalg.norm(r_x), 2)  # 近似，实际需归一化
axis_x = r_x / (np.linalg.norm(r_x) + 1e-10)
R_x = axisangle_to_rot(axis_x, np.linalg.norm(r_x))

# 平移部分：由两组方程联立
C = np.zeros((6, 3))
d = np.zeros(6)
for idx, (Ai, Bi) in enumerate([(A1, B1), (A2, B2)]):
    RA, tA = Ai[:3,:3], Ai[:3,3]
    RB, tB = Bi[:3,:3], Bi[:3,3]
    C[3*idx:3*idx+3] = RA - np.eye(3)
    d[3*idx:3*idx+3] = R_x @ tB - tA

t_x = np.linalg.lstsq(C, d, rcond=None)[0]
X_est = make_T(R_x, t_x)

print("估计 X 与真实 X 的误差（应接近0）:", np.max(np.abs(X_est - X_true)))

运行与验证

依赖：Python 3.8+，NumPy（pip install numpy）
运行：将上述代码保存为 hand_eye_check.py，执行 python hand_eye_check.py
通过条件：两行输出均接近 0（数值误差 < 1e-6），说明：
(a) 构造的 A、B 确实满足 AX=XB；
(b) 两步法能从两组位姿中正确恢复手眼矩阵 X。

关键说明

实际标定中需采集 15 组以上位姿，并用最小二乘或非线性优化求解，本案例仅用两组数据验证数学原理。
眼在手上与眼在手外的 A、B 构造方式不同，但最终都归结为同一 AX=XB 问题。
代码中 T_ci 的构造方式模拟了“由真实 X 反推相机观测”，用于验证推导正确性；实际标定时 T_ci 由相机标定直接测得。

📚 课程目录