关于双机械手的视觉定位

这几个星期在做一个视觉定位相机。

项目大概是这样的。两个机械手，其中一个连着相机，另一个机械手连着夹爪。相机拍照，另一个机械手去拿物品，物品有一个缺口，旋转一定角度，放到一定位置。

这个项目中，图像坐标需要进行3次转换。

第一次转换是，相机坐标跟与之相连的机械手之间的转换。具体是获得图像中的坐标与缺口角度。将这个坐标转换为，当坐标在图像中心时，相机机械手的坐标。
这里需要进行一次标定。具体是获得一定数量的坐标点（到少4个，弄够10个就是9点标定法了）。我弄了5个点，产品在图像中四角与中心时的坐标，还有对应的机械手的坐标位置。四角坐标减去中心坐标，得出差值（相机手同样）。然后使用vector_to_hom_mat2d，这个算子来计算变换矩阵。这样，在获得相机机械手与图像中物品的坐标后，可以求得，物品在图像中心时，机械手的位置。具体就是先用物品的坐标与图像中心的差值，计算出机械手所移动的偏差，再加上现在机械手的位置。

然后，需要进行第二次转换。就是两机械手坐标转换。就是上一次计算出的，图像在中心时，相机机械手的位置，与抓取机械手之间的转换。
这个标定和普通的9点标定法没多大区别。我自己也就用了边边四角的点作标定点。
其中，在做标定时，一定要注意，抓取机械手的姿态必需一样的。同样的角度，同样的垂直度。我这边是用六轴机械手的，具体来说，就是在世界坐标体系下，uvw三个参数保持不变的情况下做标定。

最后，还需要做一次角度的转换。机械手抓取物品，旋转一定角度，放到一定位置，有两个方法。
一个是固定的姿态下去抓取物品，然后旋转，再把物品放到一定位置。
另一个方法是，先按照一定角度先旋转机械手，下去抓取物品，然后旋转回固定角度，把物品放好。
两个方法的区别就是，旋转瓶子在抓取前还是在抓取后。
而我这次的项目。因为夹爪是左右两边，和放置的地方有些限位。所以只能用第二种方法。

而第二种方法，需要旋转。就需要进行一定的变换。理想的状态下，夹爪的中心就是机械手旋转的中心。但这个理想状态下，特别是，如果夹爪比较长的时候，夹爪的中心随着夹爪旋转而偏转，产品“偏心”的问题。
偏心这个问题怎么解决呢。首先，让夹爪夹住一支笔，画下一点。记下位置与角度（记为位置1）。然后机械手旋转180度，让笔尖，回到刚刚画下的那一点那里，记下位置2。然后两位置相减，得出dx与dy。勾股定理（d = 开方根（dxdx+ dydy)）求得偏心圆的直径，而偏心的角度，只需要使用atan2（C#中有这个函数，其他的语言可能有差别。使用atan再变换）就可以求得偏心的角度。具体如下：

private void button_calibration3_calculate_Click(object sender, EventArgs e)
        {
   
            //计算偏心半径，与偏心角度
            PointD pos1 = new PointD(textBox_calibration3_fetch_hand_pos1_x.Text, textBox_calibration3_fetch_hand_pos1_y.Text);