03-像素坐标转机械臂世界坐标原理详解
像素坐标转机械臂世界坐标原理详解
作者：黒漂技术佬
YOLO 说目标在 (320, 240) 像素位置，深度相机说距离 0.5 米。但机械臂只认自己坐标系下的毫米数。这中间怎么转？这篇文章把数学原理掰开揉碎讲清楚。
一、四个坐标系：先搞清楚谁是谁
在视觉抓取中，一个点的坐标会在四个坐标系之间辗转。新手最容易懵的就是"这个坐标是哪个坐标系下的"。先把四个坐标系搞清楚，后面的转换就是套公式。
1. 像素坐标系（Pixel Coordinate System）
这是图像的最底层坐标系。原点在图像左上角，u 轴向右，v 轴向下，单位是像素。YOLO 检测出的边界框坐标 (x1, y1, x2, y2) 就是像素坐标。比如一张 640×480 的图像，中心点像素坐标是 (320, 240)。
2. 图像坐标系（Image Coordinate System）
这个坐标系原点在图像中心（光轴与图像平面的交点），x 轴向右，y 轴向上，单位是物理长度（毫米）。它是像素坐标系和相机坐标系之间的桥梁。实际工程中这个坐标系经常被跳过，直接从像素坐标跳到相机坐标，但它有助于理解内参的物理意义。
3. 相机坐标系（Camera Coordinate System）
原点在相机光心（镜头中心），Z 轴沿光轴指向前方（物体方向），X 轴向右，Y 轴向下（注意：OpenCV 惯例是 Y 轴向下，和图像坐标系一致）。单位是毫米。这个坐标系下，一个点的坐标 (Xc, Yc, Zc) 的物理含义是：物体在相机前方 Zc 毫米处，水平偏移 Xc 毫米，垂直偏移 Yc 毫米。
4. 机械臂坐标系（Robot Coordinate System）/ 世界坐标系
原点在机械臂基座底部中心，X 轴正方向通常指机械臂正前方，Y 轴向左，Z 轴向上。单位是毫米。机械臂控制器接收的目标位置就是这个世界坐标系下的坐标。比如给机械臂发指令让它去 (350, 0, 200)，意思是末端移动到正前方 350mm、正中、高度 200mm 处。
坐标系关系图
像素坐标系 (u, v) ──[内参]──> 相机坐标系 (Xc, Yc, Zc) 单位: 像素 单位: 毫米 │ [外参] │ ▼ 机械臂坐标系 (Xw, Yw, Zw) 单位: 毫米二、坐标转换链路：像素→相机→机械臂
第一环：像素坐标 → 相机坐标
这一步用相机内参完成。先解释内参的四个关键参数。
相机内参详解
相机内参描述的是相机本身的光学特性，是相机出厂后固有的参数（严格来说需要标定校准）。内参矩阵通常写成：
| fx 0 cx | K = | 0 fy cy | | 0 0 1 |fx, fy（归一化焦距）：焦距 f 以像素为单位表示。物理焦距 F（毫米）除以单个像素的物理尺寸（毫米/像素）得到 fx = F / dx。fx 的物理含义：物体在相机前方 1 米处，每偏移 1 毫米，在图像上移动 fx 个像素。
cx, cy（主点坐标）：光轴与图像平面交点的像素坐标，理论上是图像中心 (width/2, height/2)，但实际有偏差，需要标定。
转换公式推导
已知像素坐标 (u, v) 和深度 Zc，反推相机坐标 (Xc, Yc, Zc)：
根据针孔相机模型的正向投影公式（相机坐标→像素坐标）：
u = fx × (Xc / Zc) + cx v = fy × (Yc / Zc) + cy反过来求解（像素坐标→相机坐标），已知深度 Zc：
Xc = (u - cx) × Zc / fx Yc = (v - cy) × Zc / fy Zc = depth （深度值，来自RGB-D相机或测距）这个公式就是上一篇代码里x = (cx - self.cx) * z / self.fx的来历。
第二环：相机坐标 → 机械臂坐标
这一步用相机外参完成。外参描述的是相机坐标系和机械臂坐标系之间的刚体变换关系。
相机外参详解
外参由两部分组成：
旋转矩阵 R（3×3）：描述相机坐标系相对于机械臂坐标系的旋转。三个角度（绕X、Y、Z轴的旋转，通常用欧拉角或旋转向量表示）决定相机朝向。
平移向量 T（3×1）：描述相机光心在机械臂坐标系下的位置。比如相机固定在机械臂正前方 500mm 处，那 T 大概是 [500, 0, 200]。
外参组合成 4×4 齐次变换矩阵：
| R11 R12 R13 Tx | | R21 R22 R23 Ty | H = | R31 R32 R33 Tz | | 0 0 0 1 |转换公式
相机坐标 (Xc, Yc, Zc) 转机械臂坐标 (Xw, Yw, Zw)：
| Xw | | R11 R12 R13 | | Xc | | Tx | | Yw | = | R21 R22 R23 | × | Yc | + | Ty | | Zw | | R31 R32 R33 | | Zc | | Tz |用齐次坐标写成矩阵乘法：
| Xw | | R11 R12 R13 Tx | | Xc | | Yw | | R21 R22 R23 Ty | | Yc | | Zw | = | R31 R32 R33 Tz | × | Zc | | 1 | | 0 0 0 1 | | 1 |外参矩阵 H 的获取就是手眼标定要做的事，这个在第四、五篇详细展开。
三、用实际数值演示完整转换过程
光看公式容易犯困，来个实际数值算一遍就清楚了。
场景设定
无人售货柜补货场景：
- 相机：Intel RealSense D435，分辨率 640×480
- 相机固定在机械臂外侧（眼在手外），俯视货架
- 目标：一瓶饮料，YOLO 检测到边界框中心在像素 (320, 280)
- 深度相机测得该点距离 450mm（0.45m）
第一步：准备内参
通过相机标定（张正友标定法）得到内参：
fx = 615.3 像素 fy = 615.1 像素 cx = 318.7 像素 cy = 241.2 像素注意 cx, cy 不正好是 (320, 240)，因为镜头安装有偏差，这就是为什么要标定。