2D机械手手眼标定（一）

2D指的是机械手只在一个平面上运动，其姿势可以用2维坐标加上一个旋转角度表示。这种情况下机械手手眼标定就是计算机械手坐标和图像坐标之间的仿射变换。如果机械手坐标平面和图像坐标平面不平行的话还会有透视变换，也叫单应性变换，但这里不考虑透视的情况。在标定的时候机械手坐标是机械手系统返回的坐标；而图像坐标一般是基于模板的定位算法返回的坐标。二者的坐标是确定的，但是它们的旋转中心（机械手旋转轴和模板定位算法的中心）却不相同。因此需要把它们的旋转中心转换到同一个中心上才能做仿射变换。本文下方的公式是把机械手旋转轴转换到模板定位算法的中心再做仿射变换的。下面是一个示意图。图中灰色图形代表物体；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)式求解的流程如下：

已知条件	${ \left(R_{x,i},R_{y,i},R_{\theta,i} | I_{x,i},I_{y,i},I_{\theta,i} \right),i=1,2,...,N }$
流程	确定常数${ C }$的值。随机选择一个${ h_{\theta} \in [0,2\pi) }$； 判断是否满足结束条件。满足则跳至第5步，否则执行下一步。结束条件可以是达到指定循环次数，或优化结果连续数次变化量小于指定的误差等； 将${ h_{\theta} }$视为已知量，使用(3)式求解得到${ \mathbf{K} }$。保存结果${ \left( {\mathbf{K},h_{\theta}} \right) }$； 使用公式${ h_{\theta}=h_{\theta} - \left. \frac{\partial e}{\partial h_{\theta}} \right|_{\mathbf{K},h_{\theta}} \cdot C }$更新${ h_{\theta} }$，偏导数公式见(2)式。然后返回第2步； 输出10个待求未知数最优值：${ \left( {\mathbf{K},h_{\theta}} \right) }$。
说明	第1步中随机选择${ h_{\theta} }$经过测试在某些情况下难以收敛到最优解。鉴于${ h_{\theta} }$取值范围有限是${ [0,2\pi) }$，所以可以直接遍历区间${ [0,2\pi) }$，比如每隔1°计算一次拟合误差，取拟合误差最小的${ h_{\theta} }$作为迭代起始值。

标定完成后，用图像坐标反解机械手坐标一般忽略变换矩阵方程中有${ h_{l} }$的那一项。因为只要计算出机械手坐标就能抓到物体（物体在机械手正下方）不需要考虑旋转臂，而且如果物体不一样旋转臂长也会变。声明：多年以前我曾看过一篇康耐视的专利描述了2D手眼标定的方法，具体内容我记不清了。大概跟本文方法相似，所以本文内容仅供研究学习只用，勿用于商业。