坐标系与变换：相机坐标系、图像坐标系、世界坐标系、刚体变换与齐次坐标

做视觉伺服控制，说白了就是让机器人「看见」并「动起来」。但这里有个核心问题——机器人看到的和它实际运动的，是两个完全不同的世界。我刚开始接触这个领域时，就被坐标系绕得晕头转向。后来踩了不少坑才明白：搞不懂坐标系变换，视觉伺服就是空中楼阁。

今天咱们就把这几个坐标系和变换彻底捋清楚。你想想看，一个像素点怎么变成机器人能理解的3D位置？答案就在下面。

1. 四个坐标系，一个都不能少

视觉伺服系统里，我们通常要打交道的是四个坐标系。我习惯把它们比作「四层翻译」：

• 世界坐标系：全局参考系，机器人工作的「大地图」
• 相机坐标系：以相机光心为原点，Z轴指向镜头前方
• 图像坐标系：在成像平面上，单位是毫米或米
• 像素坐标系：最终我们看到的图像，单位是像素

嗯，这里要注意：像素坐标系和图像坐标系经常被混淆。我在项目中就见过有人直接把像素坐标当物理坐标用，结果定位偏差大得离谱。

2. 从世界到相机：刚体变换

世界坐标系和相机坐标系之间是什么关系？说白了就是旋转加平移。一个刚体在三维空间里运动，只有6个自由度——3个旋转，3个平移。

数学上，我们用旋转矩阵 R 和平移向量 t 来描述：

P_c = R * P_w + t

其中 P_w 是世界坐标，P_c 是相机坐标。R 是3x3的正交矩阵，t 是3x1的向量。

我做过一个项目，需要把机械臂末端抓取的工件坐标映射到相机视野里。当时R矩阵算错了一个符号，结果机器人直接往反方向抓取——嗯，那次差点把夹具撞坏。从那以后，我每次做变换都会先画个草图验证方向。

3. 从相机到图像：透视投影

相机坐标系到图像坐标系，这一步是「从3D到2D」的关键。用的是小孔成像模型：

x = f * X_c / Z_c
y = f * Y_c / Z_c

这里 (X_c, Y_c, Z_c) 是相机坐标，(x, y) 是图像坐标，f 是焦距。

你会发现，Z_c 出现在分母上。这意味着什么？物体离相机越远，它在图像上就越小。这个道理很简单，但实际调试时经常被忽略。我记得有一次，视觉伺服系统在近距离跟踪时表现完美，一拉远距离就开始震荡。查了半天，发现是深度信息 Z_c 的噪声被放大了。

4. 从图像到像素：离散化

图像坐标系是连续的物理坐标，但计算机只能处理离散的像素。这一步就是简单的缩放加平移：

u = x / dx + u0
v = y / dy + v0

其中 dx, dy 是每个像素的物理尺寸，u0, v0 是图像中心（主点）的像素坐标。

这些参数从哪里来？相机标定。我建议你每次换镜头或者调整焦距后，都重新标定一次。别偷懒，我曾经因为用了旧的标定参数，导致整个视觉伺服系统偏差了3个像素——在精密装配任务里，这足以让零件报废。

5. 齐次坐标：让变换变得优雅

你有没有觉得上面的变换写起来很麻烦？旋转平移一个公式，投影又一个公式，还得考虑非线性。这时候齐次坐标就派上用场了。

说白了，齐次坐标就是在三维坐标后面加一个1，变成四维：

P_h = [X, Y, Z, 1]^T

这样一来，所有的变换都可以统一成矩阵乘法：

P_c_h = [R | t] * P_w_h

透视投影也能写成线性形式：

p_h = K * [I | 0] * P_c_h

其中 K 是相机内参矩阵，包含了焦距和主点信息。

我个人习惯把所有变换串联成一个4x4的矩阵，这样从世界坐标到像素坐标，一步到位：

p_h = K * [R | t] * P_w_h

你看，多简洁。而且齐次坐标还有一个好处——它可以表示无穷远点。当 Z_c = 0 时，对应的就是方向向量，这在视觉伺服中经常用到。

6. 知识体系总览

下面这张图把整个坐标系变换的流程串起来了。我建议你把它保存下来，调试时对照着看：

7. 实战中的坐标系管理

说了这么多理论，咱们聊聊实际怎么用。我总结了几条经验：

1. 统一坐标系约定：团队里必须统一坐标系的手性（左手系还是右手系）和轴向定义。我见过两个模块用不同手性，结果变换链全乱套。
2. 可视化验证：每次做完变换，把坐标点画出来看看。用OpenCV或者Matplotlib都行，肉眼确认比算一百遍公式都管用。
3. 单元测试变换函数：写一个简单的测试——把一个已知点从世界坐标变换到像素坐标，再反变换回来，看误差有多大。
4. 注意浮点精度：齐次坐标变换涉及大量矩阵乘法，浮点误差会累积。我一般用double类型，float在长距离变换时精度不够。

好了，坐标系与变换这部分就聊到这儿。你把这些概念吃透了，后面学视觉伺服控制算法就会轻松很多。记住一句话：坐标系变换是视觉伺服的「语法」，语法不对，句子再漂亮也是错的。

---