下相机定位算法原理及实现

1.什么是下相机定位算法？

• 下相机定位算法：也叫底部相机，相机固定在机台上，机械手/龙门架吸取产品后，移动的底部相机上方，通过拍照及算法纠偏来修正产品放置位置
• 

2、定位算法的原理和难点在哪？

• 原理： 通过下相机拍产品的两个mark点（也有拍一个的，后面有机会再讲），来计算当前产品与模版产品的偏移角度及X,Y方向误差
• 难点1：下相机拍2个Mark点一般是通过两次拍照得出（产品较小时相机视野大时可以一次拍2个mark点），怎么把2个mark点统一到一个坐标系下
• 难点2：一般下相机拍照点与放置点还有一定的角度；所有会要二次旋转计算
• 难点3：下相机拍照点不是最终产品放置点，怎么通过拍照点的差异来推导出最终放置点差异

3、解决方案

• 前提：已经通过下相机9点标定，可以通过计算得到2个mark点对应的龙门架/机械手坐标
• 难点1：假如拍mark1时，龙门架的点位是P1，mark1对应机械手点位是P1-1, 拍mark2时龙门架点位是P2,mark2对应机械手点位是P2-1
  P2和P1的坐标U轴差为U1，P2-1绕P2旋转U1度，得到位置P2-1-1。然后P2-1-1再平移（P2-P1）得到点P2-1-2（这样相当于把相机视野放大了）。
  这样P1-1和点P2-1-2就在一个坐标系了
• 难点2：为了避免产生二次旋转误差，让拍照模版点旋转到与放置位的U轴一样。再进行X,Y计算即可。
• 难点3：经过第二步，运行时拍照点与模版拍照点的X,Y误差就等于模版放置位与运行放置位的X,Y误差了

4、部分核心源码

  public Position GetCorrectionPoi(Position firstMarkPoi, Position firstTackPhotoPoi, Position currentTackPhotoPoi, Position dstPoi, out Position offset)
        {
            //拍照点相对于相机在移动，所以第一次的拍照点与第二次的拍照点得到的mark点坐标不在同一坐标系下
            //1、故需要把第二个点先根据当前拍照点旋转，然后根据firstTackPhotoPoi，currentTackPhotoPoi平移得到与第一个点同一坐标系点位
            //2、然后把第一个点旋转到与放置位平行
            Position dstPoiCopy = dstPoi.Copy();
            //获取当前的拍照的mark点对应的机械手坐标
            Position currentIRobotPoi = this.getRunCameraPoi(1);
            //旋转
            double angle1 = firstTackPhotoPoi.U - currentTackPhotoPoi.U;
            Position currentIRobotPoi1 = this.getRotatePoi(currentIRobotPoi, angle1, currentTackPhotoPoi);
            //平移
            Position currentIRobotPoi2 = this.getTransPoi(currentIRobotPoi1, currentTackPhotoPoi, firstTackPhotoPoi);
            //同理模版中的第二个点也需要做同样动作
            //获取模板的两个点位
            List<Position> templateRobotPoiList1 = this.getTemplateRobotPoiList();
            Position tempMark1Poi = templateRobotPoiList1[0];
            Position tempMark2Poi = templateRobotPoiList1[1];
            Position tempMark2Poi1 = this.getRotatePoi(tempMark2Poi, angle1, currentTackPhotoPoi);
            Position tempMark2Poi2 = this.getTransPoi(tempMark2Poi1, currentTackPhotoPoi, firstTackPhotoPoi);
            //创建新的点组合
            List<Position> currentIRobotPoiList = new List<Position>() { firstMarkPoi, currentIRobotPoi2 };
            List<Position> templateRobotPoiList = new List<Position>() { tempMark1Poi, tempMark2Poi2 };
            //获取旋转角度 
            double offsetAngle = this.getOffsetAngle(currentIRobotPoiList, templateRobotPoiList);
            //全部使用第一个点做平移计算
            //模板点旋转到与放置为平行
            double tempOffsetAngle = dstPoi.U - firstTackPhotoPoi.U;
            Position rotatedTempPoi = this.getRotatePoi(tempMark1Poi, tempOffsetAngle, firstTackPhotoPoi);
            //第一次拍照点旋转到与放置为平行
            Position rotatedCurrentPoi = this.getRotatePoi(firstMarkPoi, tempOffsetAngle - offsetAngle, firstTackPhotoPoi);
            //第一次拍照点与模板的平移
            Position tmpDstPoi = this.getTransPoi(firstTackPhotoPoi, rotatedCurrentPoi, rotatedTempPoi);
            //模板与放置位的平移 
            Position newDstPoi = this.getTransPoi(tmpDstPoi, firstTackPhotoPoi, dstPoi);
            //角度补偿
            newDstPoi.Z = dstPoi.Z;
            //防止旋转过大
            newDstPoi.U = (dstPoi.U - offsetAngle) % 360;
            //********************偏移计算*********************
            offset = this.getTransOffset(rotatedCurrentPoi, firstMarkPoi);
            offset.U = tempOffsetAngle;
            //模板2个Mark点与运行2个Mark点的距离偏差
            double templateDistance = HsNcCommon.VisionHelper.distancePP(templateRobotPoiList[0].X, templateRobotPoiList[0].Y, templateRobotPoiList[1].X, templateRobotPoiList[1].Y);
            double runDistance = HsNcCommon.VisionHelper.distancePP(currentIRobotPoiList[0].X, currentIRobotPoiList[0].Y, currentIRobotPoiList[1].X, currentIRobotPoiList[1].Y);
            offset.Z = runDistance - templateDistance;
            return newDstPoi;
        }

5. 后续计划[敬请期待]，如需完整代码请微信联系

• 基于头部相机的载具定位算法
• 基于顶部相机的塑盘取料算法
• 基于头部相机的检测算法实现
• EPson机械手驱动
• 光源及相机驱动
• 一种面向接口接口、依赖注入的运控框架的总体介绍及分层实现