📚 课程目录第4章手眼标定数学模型

4、手眼标定数学模型：坐标系变换链、AX=XB问题的推导、刚体变换与齐次矩阵

好，咱们进入正题。手眼标定，说白了就是搞清楚相机和机器人手臂之间到底是怎么个相对位置关系。你想想看，机器人要抓一个零件，眼睛（相机）看到了零件在哪儿，但手（机械臂）得知道往哪儿伸。这中间缺的就是一个坐标变换关系。

我个人习惯，先把数学底子打牢。不然你后面标定出来结果不对，都不知道是采集的问题还是算法的问题。这一章，咱们就把坐标系变换链、AX=XB这个核心方程，还有刚体变换和齐次矩阵这些概念，一次性讲透。

4.1 坐标系变换链：从像素到机械臂底座

在眼在手上（Eye-in-Hand）的配置里，相机是装在机械臂末端法兰盘上的。所以，一个完整的坐标变换链长这样：

像素坐标系 (Pixel)：图像上点的位置，单位是像素。
图像坐标系 (Image)：相机感光芯片上的物理位置，单位是毫米。
相机坐标系 (Camera)：以相机光心为原点，Z轴指向镜头前方。
标定板坐标系 (Calibration Object)：标定板上的世界坐标系，通常以某个角点为原点。
机械臂末端坐标系 (End-Effector/Tool)：法兰盘中心，机器人自己知道的位置。
机械臂基座坐标系 (Base/Robot)：机器人的参考系，所有运动都基于此。

我们最终要找到的，是 相机坐标系 到 机械臂末端坐标系 的变换关系。这个关系，就是传说中的 手眼矩阵，记作 CamInHand 或者 X。

核心变换链公式：

从标定板上的一个点 P，到机器人基座坐标系下的坐标，路径是这样的：

P_base = T_base_to_末端 * T_末端到相机 * P_camera

其中 T_末端到相机 就是我们要求的 X。

我在项目里见过不少新手，上来就对着公式硬套，结果坐标系搞反了。记住，变换矩阵的乘法顺序是从右往左读的，先乘最右边的，再往左乘。

4.2 刚体变换与齐次矩阵：数学工具

刚体变换，就是只包含旋转和平移，不包含缩放和剪切。在三维空间里，一个刚体变换可以用一个 4x4 的齐次矩阵来表示。为什么用4x4？因为3x3的旋转矩阵只能处理旋转，加上平移就得用齐次坐标，把平移塞进矩阵里。

一个标准的齐次变换矩阵长这样：

| R11  R12  R13  Tx |
| R21  R22  R23  Ty |
| R31  R32  R33  Tz |
| 0    0    0    1  |

左上角的 3x3 是旋转矩阵 R，右上角的 3x1 是平移向量 T。最后一行永远是 [0, 0, 0, 1]。

我的小技巧： 在HALCON里，你不需要手动拼这个矩阵。用 pose_to_hom_mat3d 这个算子，可以直接把一个位姿（Pose）转换成齐次矩阵。反过来，用 hom_mat3d_to_pose 也能把矩阵转回位姿。非常方便。

为什么要强调这个？因为后面求解 AX=XB，我们操作的就是这些 4x4 矩阵。

4.3 AX=XB 问题的推导：核心方程

好，重头戏来了。为什么手眼标定最后会归结到 AX=XB？

我们来看两次不同的拍照姿势。假设机械臂带着相机移动了两个位置：位置1和位置2。

在位置1，机械臂末端相对于基座的变换是 A1（机器人告诉我们的）。
在位置2，机械臂末端相对于基座的变换是 A2。
在位置1，相机看到的标定板相对于相机的变换是 B1（通过标定板图像算出来的）。
在位置2，相机看到的标定板相对于相机的变换是 B2。

我们要求的是相机相对于末端的变换 X。

现在，我们考虑标定板上的同一个物理点。这个点在机器人基座坐标系下的坐标是固定不变的。根据变换链，我们可以写出两个等式：

A1 * X * B1 = A2 * X * B2

这个等式成立，是因为等式两边都等于同一个点从标定板坐标系到机器人基座坐标系的变换。

然后，我们稍微整理一下。把 A2 的逆矩阵乘到左边，把 B1 的逆矩阵乘到右边：

(A2⁻¹ * A1) * X = X * (B2 * B1⁻¹)

你看，左边变成了一个已知量（两个机器人位姿之间的相对变换），右边也变成了一个已知量（两个相机位姿之间的相对变换）。我们令：

A = A2⁻¹ * A1
B = B2 * B1⁻¹

于是，方程就变成了：

A * X = X * B

这就是手眼标定的核心方程。A 是机器人末端的相对运动，B 是相机的相对运动，X 就是我们要求的未知的手眼矩阵。

我曾经踩过的坑： 注意 A 和 B 的定义顺序！不同资料里可能定义不一样。有的把 A 定义为 A1⁻¹ * A2，那 B 的定义也要相应调整。一定要搞清楚你用的算法库（比如HALCON的 hand_eye_calibration）里，A 和 B 到底是怎么定义的。否则求出来的 X 是反的。

4.4 求解 AX=XB：几种经典方法

方程有了，怎么解？AX=XB 是一个典型的矩阵方程，不能直接除。学术界提出了很多解法，我简单归纳一下：

方法类别	代表算法	核心思想	我的评价
两步法	Tsai-Lenz, Park-Martin	先解旋转部分，再解平移部分。把问题拆开。	经典，稳定。HALCON早期版本主要用这个。
单步法	Andreff, Horaud	旋转和平移同时求解，用非线性优化。	理论上更精确，但对噪声敏感。
李代数法	基于李群李代数	把旋转矩阵映射到李代数空间，变成线性问题。	数学优美，效率高。HALCON现在的算法底层用了这个。

在HALCON里，你不需要自己实现这些算法。直接用 hand_eye_calibration 算子就行。你只需要提供一组 A 和 B 的对应关系，算子内部会自动选择最优的求解策略。

关于数据量： 理论上，两组不同的位姿（两个方程）就能解出 X。但实际中，噪声很大。我建议至少采集 15-20 组不同姿态的数据。姿态差异要大，不要只在同一个平面内转，要有俯仰、摇摆的变化。这样方程才够“病态”，解出来的结果才稳定。

4.5 知识结构图：本章核心逻辑

下面这张图，把整个数学模型的脉络梳理了一遍。从物理世界到数学方程，再到求解方法，一目了然。