3D机械手手眼标定
这里的3D指的是机械手可以在3维空间中任意活动，其姿势可以用3维的位置加上3个角度表示。经过多次在百度上的搜索，目前理清了3D机械手手眼标定的流程。由于没有实际的机械手做实验，所以本文内容没有得到实际的验证。以下将分别叙述眼在手上和眼在手外两种标定方法的流程。手眼标定的数学原理可以参考以下文章：
一、眼在手上的标定
首先我们要知道眼在手上的标定目标是计算相机到机械手末端的变换矩阵${ ^{g}R_{c},^{g}T_{c} }$，其中R表示3x3旋转矩阵，T表示3x1平移矩阵，下标c和上标g表示的是c到g的变换。本文的上下标中的c表示相机，g表示机械手末端，b表示机械手基座，t表示标定板。标定方法是把标定板放在工作台上不动，机械手带着相机不断变换几组不同的姿势。在进行手眼标定之前需要获取相机内参。如果没有可以采用张正友标定法计算相机内参${ K }$。相机内参表示的是相机坐标到像素坐标的变换矩阵。下图是眼在手上标定示意图，图片来自于网络。
标定过程中，机械手末端的姿势可以用位置${ \left( x,y,z \right) }$加四元数${ \left( q_{w},q_{x},q_{y},q_{z} \right) }$表示。此数据由机械手控制系统提供，注意鉴别一下它是g到b的变换还是b到g的变换，如果是反的用下述公式后还得再求个逆。本人没用过这种机械手也不知道详细情况。四元数转旋转矩阵公式如下：
$${ ^{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}^{-1} \begin{pmatrix} ^{b}R_{g\left( i \right)} & ^{b}T_{g\left( i \right)} \\ 0 & 1 \end{pmatrix} }$$
$${ \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} }$$
上述公式来源如下，下式中M表示由R和T组成的4×4的变换矩阵：
$${ \underbrace{^{b}M^{-1}_{g(j)}\left. ^{b}M_{g(i)} \right.}_{A} \underbrace{^{g}M_{c}}_{X}=\underbrace{^{g}M_{c}}_{X} \underbrace{^{c}M_{t(j)}\left. ^{c}M^{-1}_{t(i)} \right.}_{B} }$$
得到上述的数据后就可以带入OpenCV的cv::calibrateHandEye(...)函数进行标定了。该函数参数如下：
$${ calibrateHandEye(^{b}R_{g\left( i,j \right)},^{b}T_{g\left( i,j \right)},^{c}R_{t\left( i,j \right)},^{c}T_{t\left( i,j \right)},R_{output},T_{output}\left[,Method \right]); }$$
标定结果就是相机到机械手末端的变换矩阵。然后就可以利用${ R_{output} }$和${ T_{output} }$进行坐标变换。像素坐标${ \begin{pmatrix} u \\ v \\ 1 \end{pmatrix} }$到世界坐标${ \begin{pmatrix} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{pmatrix} }$的转换如下式：
$${ P_{c}=K^{-1}\begin{pmatrix} u \\ v \\ 1 \end{pmatrix} }$$
$${ \begin{pmatrix} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{pmatrix}=\begin{pmatrix} ^{b}R_{g} & ^{b}T_{g} \\ 0 & 1 \end{pmatrix}\begin{pmatrix} R_{output} & T_{output} \\ 0 & 1 \end{pmatrix}\begin{pmatrix} P_{c} \\ 1 \end{pmatrix} }$$
注意由于像素坐标到世界坐标转换增加了z轴的数据，所以上式中${ P_{c} }$可能需要缩放以适应世界坐标中${ z_{w} }$的值。这里的世界坐标就是机械手基座坐标，手眼标定只能做到这一步。
二、眼在手外的标定
这种标定的目标是计算相机到机械手基座的变换矩阵${ ^{b}R_{c},^{b}T_{c} }$。标定方法是相机固定不动，机械手夹着标定板不断变换几组不同的姿势。其标定示意图如下，图片来自于网络。
与眼在手上标定相似，标定过程中都获取了机械手末端姿势${ ^{b}R_{g},^{b}T_{g} }$和相机相对于标定板的姿势${ ^{c}R_{t},^{c}T_{t} }$。不同的是这次需要计算的两个机械手末端姿势之间的变换是${ ^{g}R_{b\left(i,j\right) } }$和${ ^{g}T_{b\left(i,j\right) } }$，注意矩阵的上下标。而两个相机姿势之间的变换与眼在手上相同仍是${ ^{c}R_{t\left(i,j\right) } }$和${ ^{c}T_{t\left(i,j\right) } }$。
$${ \begin{align*} \begin{pmatrix} ^{g}R_{b\left( i,j \right)} & ^{g}T_{b\left( i,j \right)} \\ 0 & 1 \end{pmatrix}=& \begin{pmatrix} ^{g}R_{b\left( j \right)} & ^{g}T_{b\left( j \right)} \\ 0 & 1 \end{pmatrix}^{-1} \begin{pmatrix} ^{g}R_{b\left( i \right)} & ^{g}T_{b\left( i \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} \end{align*} }$$
$${ \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} }$$
上述公式来源如下，下式中M表示由R和T组成的4×4的变换矩阵：
$${ \underbrace{^{g}M^{-1}_{b(j)}\left. ^{g}M_{b(i)} \right.}_{A} \underbrace{^{b}M_{c}}_{X}=\underbrace{^{b}M_{c}}_{X} \underbrace{^{c}M_{t(j)}\left. ^{c}M^{-1}_{t(i)} \right.}_{B} }$$
数据计算完成后就可以标定了。这种情况下的像素坐标${ \begin{pmatrix} u \\ v \\ 1 \end{pmatrix} }$到世界坐标${ \begin{pmatrix} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{pmatrix} }$的转换如下式：
$${ P_{c}=K^{-1}\begin{pmatrix} u \\ v \\ 1 \end{pmatrix} }$$
$${ \begin{pmatrix} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{pmatrix}=\begin{pmatrix} R_{output} & T_{output} \\ 0 & 1 \end{pmatrix}\begin{pmatrix} P_{c} \\ 1 \end{pmatrix} }$$
