机械臂与相机的坐标变换
本文记录机械臂与相机配合时涉及的坐标转换、手眼标定，以及根据目标位置计算相机和机械臂末端姿态的方法。
1. 坐标系与符号约定
定义三个坐标系：
- \(\{B\}\)：机器人基座坐标系（Base）
- \(\{E\}\)：机械臂末端或法兰坐标系（End/Flange）
- \(\{C\}\)：相机坐标系（Camera）
使用 \({}^A T_B\) 表示“从坐标系 \(\{B\}\) 到坐标系 \(\{A\}\) 的齐次变换”。若点 \(p\) 在 \(\{B\}\) 中的齐次坐标为 \({}^B p\)，则：
\[ {}^A p={}^A T_B\,{}^B p \]
齐次变换矩阵可写为：
\[ {}^A T_B= \begin{bmatrix} {}^A R_B & {}^A t_B\\ 0 & 1 \end{bmatrix} \]
其中 \({}^A R_B\) 表示旋转，\({}^A t_B\) 表示坐标系 \(\{B\}\) 原点在 \(\{A\}\) 中的位置。
不同机器人 SDK 和标定库可能使用不同的矩阵命名方式。实际计算时，重点是明确矩阵把坐标从哪个坐标系转换到哪个坐标系。
2. 相机、末端与基座之间的关系
对于眼在手上（Eye-in-Hand）的系统，相机固定安装在机械臂末端。手眼矩阵记为：
\[ {}^E T_C \]
它表示从相机坐标系 \(\{C\}\) 到末端坐标系 \(\{E\}\) 的变换：
\[ {}^E T_C= \begin{bmatrix} {}^E R_C & {}^E t_C\\ 0 & 1 \end{bmatrix} \]
其中：
\[ {}^E t_C= \begin{bmatrix} t_x\\t_y\\t_z \end{bmatrix} \]
表示相机原点在末端坐标系中的位置。
机器人控制器给出的末端当前位姿记为 \({}^B T_E\)。相机在基座坐标系中的位姿为：
\[ \boxed{{}^B T_C={}^B T_E\,{}^E T_C} \]
这条关系是后续目标坐标转换和相机姿态控制的基础。
3. 从图像坐标得到基座坐标
假设目标在图像中的像素坐标为 \((u,v)\)，对应深度为 \(Z\)，相机内参为 \((f_x,f_y,c_x,c_y)\)。在图像已经去畸变的情况下，目标在相机坐标系中的坐标为：
\[ X=\frac{(u-c_x)Z}{f_x} \]
\[ Y=\frac{(v-c_y)Z}{f_y} \]
\[ {}^C p_T= \begin{bmatrix} X\\Y\\Z\\1 \end{bmatrix} \]
再依次经过手眼矩阵和机械臂末端位姿，得到目标在基座坐标系中的位置：
\[ \boxed{{}^B p_T={}^B T_E\,{}^E T_C\,{}^C p_T} \]
对应的坐标链路为：
像素坐标系 → 相机坐标系 → 末端坐标系 → 基座坐标系4. 手眼标定
手眼标定的目的是求出相机与机械臂末端之间固定的位姿关系：
\[ X={}^E T_C \]
在机械臂运动过程中，记录多组机械臂末端位姿，同时由相机观测标定板的位姿。相邻两次采样之间的运动满足：
\[ \boxed{AX=XB} \]
其中：
- \(A\)：机械臂末端在两次采样之间的相对运动；
- \(B\)：相机在两次采样之间的相对运动；
- \(X\)：相机坐标系与末端坐标系之间的固定变换。
4.1 机械臂相对运动 \(A\)
设机器人控制器记录的两次末端位姿为 \({}^B T_{E_1}\) 和 \({}^B T_{E_2}\)，则相对运动为：
\[ A=({}^B T_{E_1})^{-1}{}^B T_{E_2} \]
用二维齐次矩阵作简化示例。第一次末端在基座坐标系中向右平移 \(500\,\mathrm{mm}\)：
\[ {}^B T_{E_1}= \begin{bmatrix} 1&0&500\\ 0&1&0\\ 0&0&1 \end{bmatrix} \]
第二次末端位于 \(x=600\,\mathrm{mm}\)，并旋转 \(90^\circ\)：
\[ {}^B T_{E_2}= \begin{bmatrix} 0&-1&600\\ 1&0&0\\ 0&0&1 \end{bmatrix} \]
因此：
\[ A=({}^B T_{E_1})^{-1}{}^B T_{E_2} = \begin{bmatrix} 0&-1&100\\ 1&0&0\\ 0&0&1 \end{bmatrix} \]
实际三维场景使用 \(4\times4\) 齐次矩阵，计算方式相同。
4.2 相机相对运动 \(B\)
\(B\) 表示相机在两次采样之间的相对运动，而不是某一次拍摄得到的相机外参。
每次采样时，可利用棋盘格等标定板的已知三维点和图像中的二维角点，通过 PnP 求出标定板相对于相机的位姿。根据两次观测位姿，可以进一步计算相机的相对运动 \(B\)。
在代入 \(AX=XB\) 前，需要确保 \(A\) 和 \(B\) 的变换方向与所使用的手眼标定算法一致。
5. 根据目标位置构造相机姿态
已知目标在基座坐标系中的位置 \({}^B p_T\) 后，可以让相机到达位置 \({}^B p_C\)，并使光轴朝向目标。
假设相机光轴是相机坐标系的正 \(Z\) 轴，则期望的 \(Z\) 轴方向为：
\[ z_{new}= \frac{{}^B p_T-{}^B p_C} {\|{}^B p_T-{}^B p_C\|} \]
只有 \(z_{new}\) 还不能唯一确定相机姿态，因为相机仍然可以绕光轴旋转。还需要确定 \(x_{new}\) 和 \(y_{new}\)，并组成旋转矩阵：
\[ {}^B R_C= \begin{bmatrix} x_{new}&y_{new}&z_{new} \end{bmatrix} \]
相机的目标位姿为：
\[ {}^B T_C= \begin{bmatrix} {}^B R_C&{}^B p_C\\ 0&1 \end{bmatrix} \]
由
\[ {}^B T_C={}^B T_E\,{}^E T_C \]
可以反推出机械臂末端的目标位姿：
\[ \boxed{{}^B T_E={}^B T_C({}^E T_C)^{-1}} \]
6. 确定相机的 X/Y 轴
6.1 使用固定参考方向
如果希望相机画面的“上方”尽量朝向基座坐标系的 \(Z\) 方向，可以取参考向量：
\[ u= \begin{bmatrix} 0\\0\\1 \end{bmatrix} \]
然后计算：
\[ x_{new}=\frac{u\times z_{new}}{\|u\times z_{new}\|} \]
\[ y_{new}=z_{new}\times x_{new} \]
当 \(u\) 与 \(z_{new}\) 接近平行时，叉积接近零，需要换用另一个参考方向。
6.2 尽量保留原来的朝向
如果希望相机转向时尽量延续原来的姿态，可以保留原相机 \(X\) 轴的朝向趋势。设原来的 \(X\) 轴为 \(x_{old}\)，先将它投影到垂直于新 \(Z\) 轴的平面：
\[ \tilde{x}=x_{old}-(x_{old}^{T}z_{new})z_{new} \]
再进行归一化：
\[ \boxed{x_{new}=\frac{\tilde{x}}{\|\tilde{x}\|}} \]
最后计算：
\[ \boxed{y_{new}=z_{new}\times x_{new}} \]
这种方法是在满足新光轴朝向的前提下，使新的 \(X\) 轴尽可能接近原来的 \(X\) 轴。若 \(x_{old}\) 与 \(z_{new}\) 接近平行，可以改用原来的 \(Y\) 轴作为参考。
7. 相机位置不变，只改变朝向
如果相机只需要原地转向，则保持当前 \({}^B p_C\) 不变，根据目标位置重新计算 \(z_{new}\)、\(x_{new}\) 和 \(y_{new}\)，构造新的 \({}^B T_C\)。
之后仍通过下式计算对应的末端目标位姿：
\[ {}^B T_E={}^B T_C({}^E T_C)^{-1} \]