2D机械手手眼标定

2D情况下机械手手眼标定就是计算机械手坐标和图像坐标之间的仿射变换。如果机械手坐标平面和图像坐标平面不平行的话还会有透视变换，也叫单应性变换，但这里不考虑透视的情况。在标定的时候机械手坐标是机械手系统返回的坐标；而图像坐标一般是基于模板的定位算法返回的坐标。二者的坐标是确定的，但是它们的旋转中心（机械手旋转轴和模板定位算法的中心）却不相同。因此需要把它们的旋转中心转换到同一个中心上才能做仿射变换。本文下方的公式是把机械手旋转轴转换到模板定位算法的中心再做仿射变换的。下面是一个示意图。图中灰色图形代表物体；S是机械手旋转轴；T是模板定位算法的中心；${ R_{\theta} }$是机械手的方向，角度逆时针旋转为正；${ h_{l} }$是S、T之间的距离；${ h_{\theta} }$是一个角度补偿量：

从上图可得出机械手坐标${ \left( R_{x},R_{y},R_{\theta} \right) }$和图像坐标${ \left( I_{x},I_{y},I_{\theta} \right) }$之间的变换关系如下：

$${ \begin{equation} \begin{pmatrix} R_{x}+h_{l}cos \left( R_{\theta} + h_{\theta} \right) \\ R_{y}+h_{l} sin \left( R_{\theta} + h_{\theta} \right) \\ R_{\theta} \\ 1 \end{pmatrix} = \begin{pmatrix} a & b & 0 & d_{x} \\ c & d & 0 & d_{y} \\ 0 & 0 & k & d_{\theta} \\ 0 & 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} I_{x} \\ I_{y} \\ I_{\theta} \\ 1 \end{pmatrix} \end{equation} }$$

此方程共有10个未知数，每组坐标产生3个方程，最少需要4组坐标方程可解。它支持坐标的平移、旋转、缩放、切变和镜像。亦可以明显地看出来它不是线性方程组，不能用最小二乘法求解。但是它的非线性参数只有${ h_{\theta} }$一个，所以可以对${ h_{\theta} }$应用梯度下降法，而其它参数使用最小二乘法。现在我们对收集到的N组坐标构造如下误差函数：

$${ \begin{align*} e=&\sum_{i=1}^{N}\left[ R_{x,i}+h_{l}cos \left( R_{\theta,i} + h_{\theta} \right)-\left( aI_{x,i}+bI_{y,i}+d_{x} \right) \right]^{2}+ \\ &\sum_{i=1}^{N}\left[ R_{y,i}+h_{l}sin \left( R_{\theta,i} + h_{\theta} \right)-\left( cI_{x,i}+dI_{y,i}+d_{y} \right) \right]^{2}+ \\ &\sum_{i=1}^{N}\left[ R_{\theta,i}-\left( kI_{\theta,i}+d_{\theta} \right) \right]^{2} \end{align*} }$$

对${ h_{\theta} }$求偏导数：

$${ \begin{equation} \begin{split} \frac{\partial e}{\partial h_{\theta}}=-2&\sum_{i=1}^{N}h_{l}sin \left( R_{\theta,i} + h_{\theta} \right) \left[ R_{x,i}+h_{l}cos \left( R_{\theta,i} + h_{\theta} \right)-\left( aI_{x,i}+bI_{y,i}+d_{x} \right) \right]+ \\ 2&\sum_{i=1}^{N}h_{l}cos \left( R_{\theta,i} + h_{\theta} \right)\left[ R_{y,i}+h_{l}sin \left( R_{\theta,i} + h_{\theta} \right)-\left( cI_{x,i}+dI_{y,i}+d_{y} \right) \right] \\ =-2&\sum_{i=1}^{N}h_{l}sin \left( R_{\theta,i} + h_{\theta} \right) \left[ R_{x,i}-\left( aI_{x,i}+bI_{y,i}+d_{x} \right) \right]+ \\ 2&\sum_{i=1}^{N}h_{l}cos \left( R_{\theta,i} + h_{\theta} \right)\left[ R_{y,i}-\left( cI_{x,i}+dI_{y,i}+d_{y} \right) \right] \end{split} \end{equation} }$$

对另外9个未知数使用最小二乘法求解。我们把(1)式整理成${ \mathbf{X}\mathbf{K}=\mathbf{Y} }$的形式。矩阵${ \mathbf{K} }$代表待求的9个未知数，后续都用它表示：

$${ \begin{equation} \begin{pmatrix} I_{x} & I_{y} & 1 & 0 & 0 & 0 & 0 & 0 & -cos \left( R_{\theta} + h_{\theta} \right) \\ 0 & 0 & 0 & I_{x} & I_{y} & 1 & 0 & 0 & -sin \left( R_{\theta} + h_{\theta} \right) \\ 0 & 0 & 0 & 0 & 0 & 0 & I_{\theta} & 1 & 0 \end{pmatrix} \begin{pmatrix} a \\ b \\ d_{x} \\ c \\ d \\ d_{y} \\ k \\ d_{\theta} \\ h_{l} \end{pmatrix}=\begin{pmatrix} R_{x} \\ R_{y} \\ R_{\theta} \end{pmatrix} \end{equation} }$$

这里不再赘述最小二乘法的推导过程。可以用OpenCV中的cv::solve(...)函数求解。那么对(1)式求解的流程如下：

声明：多年以前我曾看过一篇康耐视的专利描述了2D手眼标定的方法，具体内容我记不清了。大概跟本文方法相似，所以本文内容仅供研究学习只用，勿用于商业。