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

2022/8/4 1:23:11

本文主要是介绍下相机定位算法原理及实现,对大家解决编程问题具有一定的参考价值,需要的程序猿们随着小编来一起学习吧!

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

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

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机械手驱动
  • 光源及相机驱动
  • 一种面向接口接口、依赖注入的运控框架的总体介绍及分层实现


这篇关于下相机定位算法原理及实现的文章就介绍到这儿,希望我们推荐的文章对大家有所帮助,也希望大家多多支持为之网!


扫一扫关注最新编程教程