VM实现定位引导方法

VM实现定位引导项目的方法

一、 手眼标定抓取原理

1. 标定的本质
标定的目的是统一坐标系，将图像坐标系中的点转换到物理坐标系中，然后在物理坐标系中进行数值处理。图1为从数学角度来理解标定原理。

标定：已知像素坐标系A有一系列点Px1，Py1；物理坐标系B有一系列点Wx1，Wy1；通过标定运算获得坐标系A到坐标系B的转换关系Matrix，即生成的标定文件。
生产：已知坐标系A的一个点currentPtA；求解该点在坐标系B中的对应点currentPtAMapB。
求解公式：currentPtAMapB = currentPtAMatrix
2. 标定分类
标定的目的是统一坐标系，但是标定的过程不一样，标定的流程也会有所不同。如图2所示是外参标定的常用方法，本文以手眼标定中十二点标定为例讲解。
3.十二点标定
十二点标定是九点标定和旋转标定的总和。主要用于处理不共轴的问题。不共轴：即运行点和基准点不重合，当基准点发生旋转时，运行点会发生变化的现象。，图3为运行点与基准点不共轴示意图。

九标定的结果是个3×3的矩阵Matrix，旋转标定的结果是点Center，而十二点标定则是将前者统一起来，做了原点归一化，将旋转中心当做原点（零点）。将旋转中心（CenterX，CenterY）与12点标定矩阵相乘，即做标定转换，可发现输出结果为（0,0），由此可见十二点标定是将原点定在了旋转中心上，旋转中心即为零点，图4为标定矩阵统一过程。

注意：由上面的关系可知，当基准拍照位置变化后，需要重新做12点标定，因为旋转中心发生了移动。所以12点标定的拍照点必须与生产时的拍照点完全一致。
4. 十二点抓取定位算法原理
1）将图像转物理坐标：即将相机拍照经过算法提取得到的图像点坐标通过标定矩阵转换得到图像点对应的物理坐标点。
2）计算抓取偏差量：由于不共轴，若只是将运行图像转物理坐标-基准图像转物理坐标=机械手抓取偏差量，虽然吸嘴中心和物料特征对上，但一旦物料角度有变化，那么机械手旋转该角度后，吸嘴中心就不会再对上物料特征，导致定位失败。所以需要将机械手的抓取定位偏差量分解为平移偏差量和旋转偏差量两部分。
平移偏差量MoveOffset：运行图像转物理坐标-基准图像转物理坐标。
MoveOffsetX = runWorldX – markWorldX;
MoveOffsetX = runWorldY– markWorldY;
旋转偏差量RotateOffset：机械手旋转偏差角度DR后带来的坐标偏差量。机械手旋转量是示教位绕着基准位旋转的坐标变化量，其中示教位为TeachPos，基准位为BasicWorld（基准图像转物理坐标+拍照基准位（标定第五点拍照位））。
TR = DR / 180 * Math.PI;（根据机械手坐标系与图像坐标系手性相同或相反，可能需要乘以-1）
RotateX = （TeachPosX – BasicWorldX） Cos(TR) – (TeachPosY – BasicWorldY）* Sin(TR);
RotateY = （TeachPosX – BasicWorldX）* Sin(TR) + (TeachPosY – BasicWorldY）* Cos(TR);
RotateOffsetX = RotateX – TeachPosX;
RotateOffsetY= RotateY – TeachPosY;
最终，机械手的抓取定位偏差量为：
OffsetX = MoveOffsetX + RotateOffsetX;
OffsetY = MoveOffsetY + RotateOffsetY;
OffsetR = DR;（根据机械手与图像旋转一致性相同或相反，可能需要乘以-1）
二、 基于VM搭建手眼标定与生产方案。
这里在VisionMaster例程中的N点标定方案的基础上进行修改，方案默认路径为C:\Program Files\VisionMaster3.4.0\Applications\Samples\CH\软件功能展示\标定\N点标定。
(一) 标定方案：
1.标定矩阵求解过程主要集成在N点标定模块中，输入12组对应的图像点与物理坐标点就可以自动求解标定矩阵。标定方案示例如图5所示。

图5 标定流程
2.方案模块讲解
快速匹配模块：用于提取图像特征点。
N点标定模块:订阅图像坐标和物理坐标(可以使用脚本模块或在N点标定模块中设置具体设置方式如图7所示，十二点标定基准点设置为第五点坐标)以及相关参数设置，达到输入次数(十二点标定为十二组对应的坐标值)即可完成标定计算。参数设置如图8所示。根据实际情况选取相机模式和自由度。

图7 物理坐标基准设置

图8 N点标定模块参数设置
3. 标定结果及标定矩阵保存
输入对应的标定参数后即完成标定过程，标定完成状态如图9所示。

图9 标定完成状态图
标定完成后在N点标定模块中点击生成标定文件，然后选择保存路径即可将标定结果保存。如图10所示为保存过程及结果。

图10 标定结果保存

4. 示教
   示教过程：人工引导机械手到物料位置，使得机械手刚好抓取到物料，记下此时的机械手物理坐标和相机在基准位拍照得到的图像基准坐标。
   (二) 生产方案：
   1.实际生产过程只需计算出机械手到待抓取点的偏差量，然后控制机械手进行移动就可以实现抓取功能。这里的偏差计算可以根据上文提到的十二点抓取算法原理编写脚本实现，参考代码见附录。

2.方案模块讲解
标定转换模块：输入参数为图像坐标和标定文件，输出参数为经过标定转换后的物理坐标。具体配置如图13所示。

图13 标定转换模块参数配置
变量计算模块：根据机械手坐标系与图像坐标系手性相同或相反来判断角度值是否需要乘以负一。

3. 偏差计算结果
   偏差计算结果如图15所示，偏差结果包含x轴偏移量、y轴偏移量和旋转角度偏移量。这里的偏移量是指运行图像到示教位的物理偏差，将偏差量加上示教位坐标就可以转换成机械手抓取移动的绝对坐标，然后通过通讯发送给机械手进行机械臂移动操作，就可以实现抓取任务。
   

图15 偏差计算结果

附件：利用脚本实现偏差计算代码(GetFloatValue为需要配置的输入参数，均为物理坐标，SetFloatValue为需要配置的输出参数)

C#
using System;
using System.Text;
using System.Windows.Forms;
using Script.Methods;
class UserScript:ScriptMethods,IProcessMethods
{
   //the count of process
    //执行次数计数
    int processCount ;
    float TeachPosX,TeachPosY,markWorldX,markWorldY,markWorldR,runWorldX,runWorldY,runWorldR;
    float calibSnapX,calibSnapY,snapX,snapY;  
    
    /// 

    /// Initialize the field's value when compiling
    /// 预编译时变量初始化
    /// 
    public void Init()
    {
        //You can add other global fields here
        //变量初始化，其余变量可在该函数中添加
        processCount = 0;     
    }
    
    ///旋转公式
    public void rotateMethod(float dr, float runPointx, float runPointy, float centerPointx, float centerPointy, out float rotatex, out float rotatey)
    {
        rotatex = 0f;
        rotatey = 0f;
        float drRad = (float)(dr / 180.0 * Math.PI);
        rotatex = (float)((runPointx - centerPointx) * Math.Cos(drRad) - (runPointy - centerPointy) * Math.Sin(drRad) + centerPointx);
        rotatey = (float)((runPointx - centerPointx) * Math.Sin(drRad) + (runPointy - centerPointy) * Math.Cos(drRad) + centerPointy);
    }

    /// 

    /// Enter the process function when running code once
    /// 流程执行一次进入Process函数
    /// 
    /// 
    public bool Process()
    {
        //You can add your codes here, for realizing your desired function
        //每次执行将进入该函数，此处添加所需的逻辑流程处理
        //MessageBox.Show("Process Success");
        GetFloatValue ("TeachPosX",ref TeachPosX);    //机械手示教抓取位X
        GetFloatValue ("TeachPosY",ref TeachPosY);    //机械手示教抓取位Y
        GetFloatValue ("markWorldX",ref markWorldX);  //基准图像点标定转换物理坐标X
        GetFloatValue ("markWorldY",ref markWorldY);  //基准图像点标定转换物理坐标Y
        GetFloatValue ("markWorldR",ref markWorldR);  //基准图像点角度R
        GetFloatValue ("runWorldX",ref runWorldX);    //运行图像点标定转换物理坐标X
        GetFloatValue ("runWorldY",ref runWorldY);    //运行图像点标定转换物理坐标Y
        GetFloatValue ("runWorldR",ref runWorldR);    //运行图像点角度R       
        GetFloatValue ("calibSnapX",ref calibSnapX);  //标定第五点拍照位X
        GetFloatValue ("calibSnapY",ref calibSnapY);  //标定第五点拍照位Y
        GetFloatValue ("snapX",ref snapX);            //生产实际拍照位X
        GetFloatValue ("snapY",ref snapY);            //生产实际拍照位Y     
        
        //角度偏差
        float dr = runWorldR - markWorldR;
        
        //1.计算平移偏差
        float moveoffsetX = runWorldX - markWorldX;
        float moveoffsetY = runWorldY - markWorldY;
 
        //2.计算旋转偏差
        float basicWorldX = markWorldX + calibSnapX;
        float basicWorldY = markWorldY + calibSnapY;
        
        float rotatedX,rotatedY;
         rotateMethod(-1*dr,TeachPosX,TeachPosY,basicWorldX,basicWorldY,out rotatedX,out rotatedY);
        float rotateoffsetX = rotatedX - TeachPosX;
        float rotateoffsetY = rotatedY - TeachPosY;
        
        //3.计算拍照位偏差
        float snapoffsetX = snapX - calibSnapX;
        float snapoffsetY = snapY - calibSnapY;
        
        //4.计算机械手偏移量
        float offsetX = moveoffsetX + rotateoffsetX + snapoffsetX;
        float offsetY = moveoffsetY + rotateoffsetY + snapoffsetY;
        SetFloatValue("offsetX", offsetX);
        SetFloatValue("offsetY", offsetY);
        SetFloatValue("offsetR", dr);
      
        return true;
    }
}