定位教程7---旋转中心标定之方法二

定位教程7—旋转中心标定之方法二

在许多实际项目中，抓取中心不是机器人旋转中心，遇到过这类项目的都比较清楚，如果不进行标定旋转中心话，误差会很大。
先上张大图：这是之前给客户做的ABB机器人介绍双相机定位，要求精度倒不是特别高（毕竟机器人也有一定的精度损失），看到照片，可能很多人会说，那简单啊，不就一个相机加一个九点标定的事儿吗？？？
对的，没错，如果条件允许的话，确实是可以在机器人手上安装一个视野范围较大的相机，然后进行九点标定即可，那么为什么我们这项目用双相机标定呢？？？主要是机构受限，迫于无奈，所以选择这种方式。当然了，我前面文章提到过的，定位项目，方法很多，没有唯一。

简化图：

我们要解决这类项目，首先需要建立吸盘中心与机器人旋转中心的关系，因为如果这两关系没建立，机器人旋转的角度PHI和平移的位置X,Y，会与实际需要补充的位置相差很多。

剖析标定过程：

1.标定机器人工具坐标

每个机器人在使用之前都现需要做工具标定，此类标定一般机器人都会提供教程，直接在示教器标定即可。

2.相机中心对准Mark点

求出两个相机的像素当量，通过比列系数把像素坐标转换为世界坐标，使用视觉软件使图像中心对准识别特征点MARK，尽量让MARK点坐标值为（0，0）。

3记录机器人当前坐标

在示教器上记录机器人当前机械坐标（X,Y），

求相机相对机器人关系

使用标尺测量机器人旋转中心到两个相机的相对位置分别为（C1_X,C1-Y），（C2_X,C2-Y）,那么相机1预估相对机器人旋转中心的坐标为[（X+C1_X),(Y+C1_Y)],相机2预估相对机器人旋转中心的坐标为[（X+C2_X),(Y+C2_Y)],里面运算统一写为加号，加或者减号主要看相对方向，如果相对机器人为正运动方向则为加，相对机器人为负方向则为减号。

使用机器人旋转中心，旋转一定角度，角度尽可能大（旋转角度太小，拟合圆会很差，求的圆心误差很大），旋转后平移机器人回到图像中心对准Mark特征点位置（即识别Mark点坐标为（0，0）），记录两个相机平移到Mark点中心分别平移了(R1_X,R1_Y)和（R2_X,R2_Y)。记录两相机相对机器人旋转中心坐标分别为[（X+C1_X+R1_X),Y+C1_Y+R1_Y)],[（X+C2_X+R2_X),Y+C2_Y+R2_Y)]，反复此操作，记录三个点（大于或者大于三个点）坐标，利用三点求圆心坐标公式：

        public static PointF FitCenter(List<PointF> pts, double epsilon = 0.1)
        {
            try
            {
                double totalX = 0, totalY = 0;
                int setCount = 0;

                for (int i = 0; i < pts.Count; i++)
                {
                    for (int j = 1; j < pts.Count; j++)
                    {
                        for (int k = 2; k < pts.Count; k++)
                        {
                            double delta = (pts[k].X - pts[j].X) * (pts[j].Y - pts[i].Y) - (pts[j].X - pts[i].X) * (pts[k].Y - pts[j].Y);

                            if (Math.Abs(delta) > epsilon)
                            {
                                double ii = Math.Pow(pts[i].X, 2) + Math.Pow(pts[i].Y, 2);
                                double jj = Math.Pow(pts[j].X, 2) + Math.Pow(pts[j].Y, 2);
                                double kk = Math.Pow(pts[k].X, 2) + Math.Pow(pts[k].Y, 2);

                                double cx = ((pts[k].Y - pts[j].Y) * ii + (pts[i].Y - pts[k].Y) * jj + (pts[j].Y - pts[i].Y) * kk) / (2 * delta);
                                double cy = -((pts[k].X - pts[j].X) * ii + (pts[i].X - pts[k].X) * jj + (pts[j].X - pts[i].X) * kk) / (2 * delta);

                                totalX += cx;
                                totalY += cy;

                                setCount++;
                            }
                        }
                    }
                }

                if (setCount == 0)
                {
                    return PointF.Empty;
                }

                return new PointF((float)totalX / setCount, (float)totalY / setCount);
            }
            catch (Exception ex)
            {
                Log.SaveError(ex);
                return PointF.Empty;
            }
        }

即可求出两个相机相对机器人旋转中心求出的真实旋转中心坐标分别为（R1_X,R1_Y）和（R2_X,R2_Y)。求（R1_X,R1_Y）与（C1_X,C1-Y）和（R2_X,R2_Y)与（C2_X,C2-Y）的偏差分别为（De1_X,De1_Y)和（De2_X,De2_Y)，那么两个相机真实相对机器人旋转中心的坐标分别为[（X+C1_X+De1_X),(Y+C1_Y+De1_Y)]，[（X+C2_X+De2_X),(Y+C2_Y+De2_Y)]。

得到两个相机相对机器人旋转中心真实坐标后，相当于建立了相机与机器人的关系，后期识别就直接使用这关系来处理就可以了。