3D机械手手眼标定（记录）

经过多次在百度上的搜索，目前理清了3D机械手手眼标定的流程。由于没有实际的机械手做实验，所以本文内容没有得到实际的验证。以下内容是基于眼在手上为基础叙述的。眼在手外目前没有研究。首先我们要知道眼在手上的标定目标是计算相机到机械手的变换矩阵。在进行手眼标定之前需要获取相机内参。如果没有可以采用张正友标定法计算相机内参。下图是眼在手上标定示意图，图片来自于网络。

标定过程中，机械手的姿势可以用位置${ \left( x,y,z \right) }$加四元数${ \left( q_{w},q_{x},q_{y},q_{z} \right) }$表示。四元数转旋转矩阵公式如下：

$${ ^{b}R_{g}=\begin{pmatrix} 2\left(q_{w}^{2}+q_{x}^{2}\right)-1 & 2\left(q_{x}q_{y}-q_{w}q_{z}\right) & 2\left(q_{x}q_{z}+q_{w}q_{x}\right) \\ 2\left(q_{x}q_{y}+q_{w}q_{z}\right) & 2\left(q_{w}^{2}+q_{y}^{2}\right)-1 & 2\left(q_{y}q_{z}-q_{w}q_{x}\right) \\ 2\left(q_{x}q_{z}+q_{w}q_{x}\right) & 2\left(q_{y}q_{z}+q_{w}q_{x}\right) & 2\left(q_{w}^{2}+q_{z}^{2}\right)-1 \end{pmatrix} }$$

对应的平移矩阵是：

$${ ^{b}T_{g}=\begin{pmatrix} x \\ y \\ z \end{pmatrix} }$$

相机相对于标定板的姿势可以通过OpenCV的cv::solvePnP(...)计算，即可得到${ ^{c}R_{t},^{c}T_{t} }$。

在我们得到N个机械手姿势和对应的相机姿势之后，可以任取机械手姿势中的两个姿势计算两个姿势之间的变换${ ^{b}R_{g\left(i,j\right) } }$和${ ^{b}T_{g\left(i,j\right) } }$，其中i，j表示序号，对应的相机姿势也可以任取两个计算${ ^{c}R_{t\left(i,j\right) } }$和${ ^{c}T_{t\left(i,j\right) } }$。一共有${ \mathrm{C}_{N}^{2} }$种组合。第i，j两个姿势之间的变换公式如下：

$${ \begin{pmatrix} ^{b}R_{g\left( i,j \right)} & ^{b}T_{g\left( i,j \right)} \\ 0 & 1 \end{pmatrix}=\begin{pmatrix} ^{b}R_{g\left( j \right)} & ^{b}T_{g\left( j \right)} \\ 0 & 1 \end{pmatrix} \begin{pmatrix} ^{b}R_{g\left( i \right)} & ^{b}T_{g\left( i \right)} \\ 0 & 1 \end{pmatrix}^{-1} }$$

$${ \begin{pmatrix} ^{c}R_{t\left( i,j \right)} & ^{c}T_{t\left( i,j \right)} \\ 0 & 1 \end{pmatrix}=\begin{pmatrix} ^{c}R_{t\left( j \right)} & ^{c}T_{t\left( j \right)} \\ 0 & 1 \end{pmatrix} \begin{pmatrix} ^{c}R_{t\left( i \right)} & ^{c}T_{t\left( i \right)} \\ 0 & 1 \end{pmatrix}^{-1} }$$

得到上述的数据后就可以带入OpenCV的cv::calibrateHandEye(...)函数进行标定了。该函数参数如下：

$${ calibrateHandEye(^{b}R_{g\left( i,j \right)},^{b}R_{g\left( i,j \right)},^{c}R_{t\left( i,j \right)},^{c}R_{t\left( i,j \right)},R_{output},T_{output}\left[,Method \right]); }$$

标定结果就是相机到机械手的变换矩阵。然后就可以利用${ R_{output} }$和${ T_{output} }$进行坐标变换。