4. 手眼标定数学推导：从AX=XB到SVD求解
说实话，很多工程师一听到“数学推导”四个字就头大。我当年刚接触手眼标定时也一样，觉得这东西太抽象了。但后来我发现，只要把核心逻辑理清楚，数学其实没那么可怕。
这一章，我们就来啃下这块硬骨头。我会尽量用大白话讲，但该有的公式和推导，一个都不会少。
4.1 核心问题：AX=XB
先问一个问题：手眼标定到底在算什么？
简单说，就是求两个坐标系之间的变换关系。一个是相机坐标系，一个是机器人末端（法兰盘）坐标系。我们想知道相机相对于机器人末端到底在哪儿、朝哪个方向。
这个关系，用数学表达就是 AX = XB。
- A：机器人末端从位置1移动到位置2的变换矩阵（已知）
- B：相机从位置1移动到位置2的变换矩阵（已知）
- X：相机相对于机器人末端的变换矩阵（未知，我们要解的）
你想想看，机器人动一下，末端位姿变了，相机看到的标定板位姿也变了。这两个变化之间，就藏着我们要找的X。
关键理解：AX=XB的本质是“闭环约束”。机器人末端的变化A，经过X变换到相机坐标系，应该等于相机自身的变化B。这个等式成立，X就是对的。
4.2 两种主流解法：Tsai法与Park法
解AX=XB的方法很多，但工业界用得最多的就两种：Tsai法和Park法。我两种都用过，各有优劣。
| 方法 | 核心思路 | 优点 | 缺点 |
|---|---|---|---|
| Tsai法 | 先解旋转，再解平移 | 计算简单，速度快 | 对噪声敏感，旋转误差会传递到平移 |
| Park法 | 旋转和平移同时解 | 精度更高，鲁棒性好 | 计算稍复杂，需要李群李代数基础 |
Tsai法：我记得第一次用Tsai法时，觉得它特别直观。先把旋转矩阵R解出来，再代入求平移向量t。但后来发现，如果旋转估计有偏差，平移就跟着跑偏了。
Park法：这个方法用到了李群李代数，把旋转矩阵映射到李代数空间，用SVD求解。我个人习惯用Park法，尤其是在精度要求高的场景下。
我的建议：如果标定板摆放位置好、数据噪声小，Tsai法完全够用。如果现场振动大、数据有波动，优先选Park法。
4.3 李群李代数基础
说到Park法，就绕不开李群和李代数。别被名字吓到，其实没那么玄乎。
- 李群 SO(3)：三维旋转矩阵的集合。旋转矩阵有9个元素，但自由度只有3。说白了，就是“冗余表达”。
- 李代数 so(3)：旋转矩阵的“切线空间”。用3个参数就能表示旋转，就是旋转向量。
为什么要用李代数？因为旋转矩阵有约束（正交且行列式为1），直接优化很麻烦。映射到李代数后，就变成了无约束优化问题，可以用SVD等工具直接求解。
我曾经在一个项目中，直接用旋转矩阵做优化，结果迭代半天不收敛。换成李代数表示后，一次SVD就搞定了。
注意：李群到李代数的映射是指数映射（exp）和对数映射（log）。代码实现时，一定要用Rodrigues公式，别自己手写泰勒展开，容易出错。
4.4 SVD求解：核心步骤
SVD（奇异值分解）是解AX=XB的利器。以Park法为例，核心步骤是这样的：
- 构造数据矩阵：把多组A和B的旋转部分提取出来，构造一个3x3的矩阵M。
- SVD分解：对M做奇异值分解，得到M = UΣVT。
- 求旋转矩阵：RX = VUT。注意检查行列式，确保是旋转矩阵（行列式为+1）。
- 求平移向量：代入RX，用最小二乘法解线性方程组，得到tX。
代码实现大概长这样：
import numpy as np
def solve_ax_xb_svd(A_list, B_list):
# A_list, B_list: 列表，每个元素是4x4变换矩阵
M = np.zeros((3, 3))
for A, B in zip(A_list, B_list):
R_A = A[:3, :3]
R_B = B[:3, :3]
M += np.log(R_A) @ np.log(R_B).T # 李代数映射
U, S, Vt = np.linalg.svd(M)
R_X = Vt.T @ U.T
# 确保行列式为+1
if np.linalg.det(R_X) < 0:
Vt[-1, :] *= -1
R_X = Vt.T @ U.T
# 解平移
# ... (省略具体实现)
return R_X, t_X
避坑指南：SVD解出来的R_X可能行列式为-1（反射矩阵）。这时候要把V的最后一列取反，再乘回去。我曾经忘了这一步，结果标定出来的位姿全是镜像的，找了半天bug。
4.5 知识结构总览
下面这张图，把这一章的核心逻辑串起来了。你可以把它当作思维导图来用。
4.6 实战中的几点提醒
数学推导讲完了，最后说几个我在现场踩过的坑：
- 数据量要够：至少10组以上位姿数据。太少的话，SVD解出来的结果不稳定。
- 位姿要多样：别只在一个小范围内移动。旋转角度要大，平移距离要长，否则约束不够。
- 注意单位：平移向量单位要统一。我见过有人用毫米，有人用米，结果标定出来差1000倍。
- 验证结果：标定完一定要做验证实验。随便选一个位姿，把计算出的X代回去，看残差有多大。
一个小技巧：标定完成后，可以用机器人带着相机走一个矩形路径，看相机观测到的标定板位置是否平滑变化。如果出现跳变，说明标定有问题。
嗯，这一章的内容就到这儿。数学推导确实需要花点时间消化，但一旦理解了AX=XB的本质，后面的实操就会顺畅很多。