SCARA 机器人工具标定方法

一、标定目标

确定工具中心点（TCP）相对于末端法兰的偏移向量：

d = {x_t, y_t, z_t}

二、四点法（4-Point Method）标定 TCP 位置

让工具尖端接触一个固定空间点 C，从四个不同姿态接近，记录每次末端法兰在基坐标系中的位置和姿态角。

设第 i 次测量时：

• 法兰中心位置：P_i = {x_i, y_i, z_i}
• 工具绕 Z 轴旋转角：θ_i
• 工具局部 TCP 偏移：d = {x_t, y_t, z_t}

则 TCP 在基坐标系中的位置为：

P_TCP: i = P_i + R_z(θ_i)·d

其中 R_z(θ_i) 是绕 Z 轴的旋转矩阵：

R_z(θ_i) = [cos θ_i -sin θ_i 0;sin θ_i cos θ_i 0; 0 0 1]

展开后：

x_TCP,i = x_i + x_t cos θ_i - y_t sin θ_i

y_TCP,i = y_i + x_t sin θ_i + y_t cos θ_i

z_TCP,i = z_i + z_t

由于所有 P_TCP,i 都应等于同一个固定点 C = {x_c, y_c, z_c}，因此有：

x_i + x_t cos θ_i - y_t sin θ_i = x_c

y_i + x_t sin θ_i + y_t cos θ_i = y_c

z_i + z_t = z_c

移项得误差方程（用于最小二乘求解）：

x_t cos θ_i - y_t sin θ_i - x_c = -x_i

x_t sin θ_i + y_t cos θ_i - y_c = -y_i

z_t - z_c = -z_i

三、最小二乘法求解（推荐 ≥4 组数据）

将 n 组测量数据代入，构建线性方程组：

A x = b

其中未知数向量 x 为：

x = {x_t, y_t, z_t, x_c, y_c, z_c}

矩阵 A 的每一行（对应第 i 次测量）为：

A_i = [cos θ_i, -sin θ_i, 0, -1, 0, 0] → 对应 x 方程

[sin θ_i, cos θ_i, 0, 0, -1, 0] → 对应 y 方程

[ 0 , 0 , 1, 0, 0, -1] → 对应 z 方程

向量 b 的每一行为：

b_i = [-x_i, -y_i, -z_i]

求解最小二乘解：

x = (A^T A)^(-1) A^T b

最终，工具偏移量为：

x_t = x[1]

y_t = x[2]

z_t = x[3]

这样通过采集点位建立线性方程组，通过解最小二乘方程组实现了机器人工具坐标标定的全过程。