1. 背景
在机器人视觉应用中，经常需要根据图像中的目标点位置，计算该点在机器人坐标系下的三维坐标。例如焊缝定位、孔位识别、工件抓取、视觉引导加工等场景，都要求系统能够从相机图像中的二维像素点 (u, v) 推算出机器人 base 坐标系下的空间点 (X, Y, Z)。
需要强调的是，单目 2D 相机本身无法仅凭一张图像直接恢复目标点的真实深度。一个像素点对应的是从相机光心出发的一条空间射线，而不是唯一的三维点。要从这条射线上确定唯一的 3D 坐标，必须额外引入约束，例如深度图、双目视差、结构光测距，或者本文所讨论的已知工作平面约束。
本文介绍一种典型工业视觉定位方法：
图像像素点 → 相机内参反投影为相机坐标系射线 → 通过手眼标定和机器人当前位姿将射线变换到机器人 base 坐标系 → 与已知工作平面求交 → 得到 3D 点。
该方法适用于目标点位于已知平面上的场景，例如工件表面、焊接平面、装配基准面、台面或孔所在平面。
2. 核心思想
整个算法的核心结论可以概括为：
单目相机提供的是方向，机器人位姿和手眼标定提供相机在机器人坐标系下的位置与姿态，已知平面提供深度约束。三者结合后，可以将二维图像点恢复为该平面上的三维空间点。
也就是说，算法并不是从单张图像中“直接测深度”，而是利用几何约束把不确定的射线裁剪成一个确定的空间交点。
流程如下：
像素点 (u, v)
↓
相机内参 K 与畸变参数 dist
↓
去畸变归一化相机坐标 (xn, yn)
↓
相机坐标系下的射线 ray_cam = [xn, yn, 1]（主要表征tan角度）
↓
手眼标定 + 当前机器人 TCP 位姿
↓
base 坐标系下的射线 origin_base + λ · dir_base
↓
与工作平面 Z = z0 求交
↓
base 坐标系下的 3D 点 P_base = [X, Y, Z]
3. 单目成像模型：像素点只能确定一条射线
在理想针孔相机模型中，空间点在相机坐标系下表示为：
Pc = [Xc, Yc, Zc]
其投影到图像平面上的像素坐标为：
u = fx · Xc / Zc + cx
v = fy · Yc / Zc + cy
其中：
fx, fy：焦距，单位为像素
cx, cy：主点坐标
K：相机内参矩阵
相机内参矩阵为：
K = [ fx 0 cx
0 fy cy
0 0 1 ]
将投影公式反过来，可以得到：
Xc / Zc = (u – cx) / fx
Yc / Zc = (v – cy) / fy
定义：
xn = Xc / Zc
yn = Yc / Zc
则有：
Pc = Zc · [xn, yn, 1]
这说明一个像素点 (u, v) 并不能确定唯一的三维点，因为 Zc 仍然未知。
对于任意不同的深度：
Zc = 100 mm
Zc = 500 mm
Zc = 1000 mm
都可以得到不同的三维点，但这些点都会投影到同一个像素位置上。因此：
单个 2D 像素点 + 相机内参 = 一条 3D 射线
而不是一个唯一的 3D 点。
4. 像素反投影：由像素坐标得到方向角的 tan 值
工程实现中，通常不会直接手写：
xn = (u – cx) / fx
yn = (v – cy) / fy
因为真实镜头存在畸变，尤其是径向畸变和切向畸变。因此代码一般使用 OpenCV 提供的反投影接口：
xn, yn = cv2.undistortPoints(points, K, dist)[0, 0]
这一步同时完成三件事：
1. 减去主点 cx, cy
2. 除以焦距 fx, fy
3. 根据畸变参数 dist 做去畸变修正
返回的 xn, yn 是去畸变后的归一化相机坐标，不是像素坐标，也不是物理距离坐标。
从几何上看：
xn = Xc / Zc = tan(theta_x)
yn = Yc / Zc = tan(theta_y)
其中：
theta_x：射线在 X-Z 平面内相对光轴的水平夹角
theta_y：射线在 Y-Z 平面内相对光轴的垂直夹角
因此可以把 xn, yn 理解成相机视线方向角的正切值。
得到归一化坐标后，相机坐标系下的射线可以表示为：
ray_cam = [xn, yn, 1]
如果需要单位方向向量，则进行归一化：
ray_cam = normalize([xn, yn, 1])
需要注意：
xn, yn 是无量纲比例值
不是像素距离
不是毫米
不是米
本质上表示方向
5. 已知深度时的三维恢复
如果已知目标点在相机坐标系下的深度 Zc，那么三维点可以直接计算：
Xc = xn · Zc
Yc = yn · Zc
Zc = Zc
即：
Pc = [xn · Zc, yn · Zc, Zc]
例如：
xn = 0.1
yn = -0.05
Zc = 1000 mm
则：
Xc = 0.1 × 1000 = 100 mm
Yc = -0.05 × 1000 = -50 mm
Zc = 1000 mm
得到：
Pc = [100, -50, 1000] mm
这就是“根据内参求 tan，然后补充 Z”的最直接形式。
但是在实际机器人项目中，往往并不知道目标点在相机坐标系下的 Zc。更常见的情况是：已知目标点位于机器人 base 坐标系下的某个工作平面上，例如：
Z_base = z0
此时不能直接在相机坐标系中补 Zc，而需要把射线变换到机器人 base 坐标系，再与该平面求交。
6. 手眼标定：将相机射线变换到机器人 base 坐标系
相机反投影得到的是相机坐标系下的射线：
origin_cam = [0, 0, 0]
dir_cam = ray_cam
但机器人运动控制需要的是机器人 base 坐标系下的三维点。因此必须知道相机坐标系与机器人末端之间的外参关系，这就是手眼标定的作用。
典型坐标变换链路为：
T_base_cam = T_base_flange · T_flange_cam
其中：
T_base_flange：机器人当前法兰在 base 坐标系下的位姿
T_flange_cam：相机相对于法兰的手眼外参
T_base_cam：相机在 base 坐标系下的位姿
如果系统中使用的是 TCP 位姿，还需要根据工具偏置进行转换：
T_base_flange = T_base_tcp · inv(T_flange_tcp)
得到 T_base_cam 后，可以提取：
R_base_cam：相机到 base 的旋转矩阵
t_base_cam：相机光心在 base 坐标系下的位置
于是，相机光心在 base 坐标系下为：
origin_base = t_base_cam
相机射线方向变换到 base 坐标系下为：
dir_base = R_base_cam · dir_cam
最终，像素点对应的空间射线在机器人 base 坐标系下可以写成：
P_base(λ) = origin_base + λ · dir_base
其中 λ 是射线参数，表示沿射线方向前进的尺度。
7. 平面约束：用工作平面确定唯一三维点
已知目标点位于机器人 base 坐标系下的工作平面：
Z_base = z0
射线方程为：
P_base(λ) = origin_base + λ · dir_base
展开 Z 分量：
Pz = origin_z + λ · dir_z
由于目标点在平面上，所以：
origin_z + λ · dir_z = z0
解得：
λ = (z0 – origin_z) / dir_z
再代回射线方程：
P_base = origin_base + λ · dir_base
即可得到最终三维点：
P_base = [X_base, Y_base, Z_base]
这个点位于机器人 base 坐标系下，单位取决于系统中外参与机器人位姿的单位。如果机器人位姿和手眼平移统一使用 mm，则输出点单位也是 mm。
8. 算法伪代码
完整流程可以抽象为如下伪代码：
# 1. 输入像素点
points = np.array([[[u, v]]], dtype=np.float32)
# 2. 像素点 -> 去畸变归一化相机坐标
xn, yn = cv2.undistortPoints(points, K, dist)[0, 0]
# 3. 构造相机坐标系下的射线
ray_cam = np.array([xn, yn, 1.0])
ray_cam = ray_cam / np.linalg.norm(ray_cam)
# 4. 根据机器人当前位姿和手眼标定计算相机在 base 下的位姿
T_base_flange = T_base_tcp @ np.linalg.inv(T_flange_tcp)
T_base_cam = T_base_flange @ T_flange_cam
R_base_cam = T_base_cam[:3, :3]
origin_base = T_base_cam[:3, 3]
# 5. 将射线方向从相机坐标系变换到 base 坐标系
dir_base = R_base_cam @ ray_cam
# 6. 与 base 坐标系下的工作平面 Z = z0 求交
lam = (z0 – origin_base[2]) / dir_base[2]
P_base = origin_base + lam * dir_base
最终：
X, Y, Z = P_base
就是机器人 base 坐标系下的三维点。
9. 方法适用场景
该方法尤其适合目标点位于稳定、已知平面的工业视觉任务，例如：
焊接工件表面定位
孔位中心定位
板材边缘或角点定位
平面工装上的目标抓取
激光打标位置计算
机器人视觉引导加工
这些场景有一个共同特点：目标点并不是任意悬浮在三维空间中，而是落在一个可建模的平面上。
因此，虽然系统使用的是单目相机，但借助已知平面约束，仍然可以恢复出平面上的三维坐标。
10. 方法限制
该方法依赖一个非常关键的假设：
目标点必须位于已知工作平面上
如果目标点不在该平面上，计算结果就会产生误差。
例如：
目标悬空
工件高度变化
表面不是水平面
目标位于倾斜面
工件存在变形
平面高度 z0 设置错误
这些情况都会导致射线与错误平面求交，从而得到错误的三维坐标。
尤其需要注意的是，平面高度错误不仅会导致 Z 错误，也会导致 X 和 Y 错误。因为射线通常是倾斜的，当平面高度变化时，交点会沿着射线移动，横向位置也会随之改变。
11. 工程中的注意事项
11.1 相机内参精度
相机内参包括：
fx, fy, cx, cy
dist
其中 dist 是镜头畸变参数。内参误差会直接影响像素点反投影出的射线方向。
在图像边缘区域，畸变影响通常更明显，因此去畸变参数的准确性非常关键。
11.2 手眼标定精度
手眼外参描述的是相机相对于机器人末端的刚体变换关系。如果手眼标定不准确，射线从相机坐标系变换到 base 坐标系时就会发生方向或位置偏差。
其中姿态误差尤其敏感。例如相机姿态误差只有零点几度，在较长工作距离下也可能引入明显的毫米级甚至厘米级定位误差。
11.3 图像与机器人位姿同步
在机器人运动过程中，图像采集时刻与 TCP 位姿时刻必须尽可能同步。
如果使用的 TCP 位姿不是图像曝光瞬间的真实位姿，则计算出的 T_base_cam 会偏离实际相机位姿，最终导致 3D 结果错误。
因此，工程上应重点关注：
图像时间戳
机器人位姿时间戳
消息同步策略
运动过程中的采样延迟
11.4 平面模型是否足够准确
如果工件表面是水平平面，可以使用：
Z = z0
如果工件表面是倾斜平面，则应使用一般平面方程：
n · P + d = 0
此时射线与平面的交点公式应改为：
λ = -(n · origin + d) / (n · dir)
再计算：
P = origin + λ · dir
这比简单的 Z = z0 更通用，也更适合倾斜工件或复杂夹具场景。
11.5 像素检测稳定性
上游视觉算法通常会从检测框、分割 mask、边缘、角点或特征区域中提取目标像素点。如果像素点存在抖动，最终三维点也会抖动。
在理想情况下，单个像素对应的物理尺度可以粗略估计为：
1 pixel ≈ Z / fx
例如：
Z = 1000 mm
fx = 5700
则：
1 pixel ≈ 1000 / 5700 ≈ 0.175 mm
这只是理想相机模型下的估计。实际系统误差还会叠加手眼标定误差、机器人重复定位误差、平面高度误差、时间同步误差和视觉检测误差。
总结
基于单目相机、手眼标定和已知平面的 2D 到 3D 计算，本质上是一个几何反投影问题。
其核心链路为：
1. 从图像中检测目标像素点 (u, v)
2. 使用相机内参和畸变参数将像素点反投影为归一化相机坐标 (xn, yn)
3. 将 (xn, yn) 理解为方向角的 tan 值，构造相机坐标系下的空间射线
4. 通过机器人当前位姿和手眼标定，将射线变换到机器人 base 坐标系
5. 使用已知工作平面 Z = z0 作为深度约束
6. 计算射线与平面的交点，得到机器人 base 坐标系下的三维点
用一句话总结：
单目相机无法直接从一个像素恢复深度，但可以恢复方向；当目标点位于已知平面时，平面约束可以提供缺失的深度信息，从而将二维像素点唯一映射为机器人坐标系下的三维点。
因此，这种方法的关键不在于“单目相机直接测出了 3D”，而在于：
相机内参提供方向
手眼标定提供坐标变换
机器人位姿提供相机在 base 下的位置
工作平面提供深度约束
射线与平面求交得到最终 3D 点
只要这些条件成立，并且相机标定、手眼标定、位姿同步和工作平面参数足够准确，该方法就可以在工业机器人视觉定位场景中实现稳定可靠的二维到三维坐标转换。
