一、标定目标
确定工具中心点（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]
这样通过采集点位建立线性方程组，通过解最小二乘方程组实现了机器人工具坐标标定的全过程。