2D视觉定位引导系列 8 先拍后放原理及动画模拟仿真

往期回顾：

第6篇讲的先拍后抓，先拍照，引导机械手调整姿态去抓取物料

第7篇讲先抓后拍，物料抓到已经机械手上了，相机(多数是下相机)拍照，引导机械手去放料。

那么本篇我们主题是先拍(拍物料放置区域，如手机屏外框)，引导机械手 去放料。整个计算逻辑和先拍后抓一样。理解先拍后抓后

理解本篇比较容易。这种模式要求物料来料位置和角度误差较小，只拍物料放置框就可保证能准确放置进去。如果来料的误差也很大，那就是我们

下篇文章要涉及内容，对位贴合，暂且不表。

仿真部件

和上篇一样，先来认识下各个部件(和上篇文章基本一致)

1.蓝色矩形框表示相机

2.绿色矩形表示示教放料框模板，例如 IC 测试治具。

3.红色矩形表示实际物料放置框来料的情况，相对与模板已经发生了角度和位置偏移。

4.粉红色的表示机械手 带一个 矩形吸盘，其中圆形表示法兰盘。这个可以看看以前几期文章

                                                             

应用场景

1.6轴 机械手，4轴scara机器人，龙门机械手。

 相机放置物料放置框上方，物料放置框随流水线来，每次有位置和角度误差。拍照后，计算出物料放到框里的坐标和角度。

这种使用场景的定位引导分三步走

1.示教 做 基准

2.实际来料 获取特征坐标和角度

3.偏差计算

和先拍再抓基本一致，连计算偏移量的方法都如出一辙。

1.示教

记住基准,以便后面来料比较计算

示教主要确定来料的基准位置，以后每次实际来料和基准比较，计算出偏差，反向补偿到放片位置上，确保能准确放置。

示教要记住几组关键数据。

第一组：模板上特征点(可能是十字Mark等特征Mark图案，也可能是线的交点)的坐标和角度，本例取蓝色矩形A点作为特征点PA 记为(Mode_X,Mode_Y)，取AB线段连接的角度为特征角度记为 Mode_Phi，如下图

第二组：示教的机械坐标，放片位置的基准机械位置，即为(TeachPos_X,TeachPos_Y)

示教

*第1步：示教

*这里演示做模板的过程，正常的模板图片有个模板点，一般是十字Mark 或者 边线交点。还有模板角度，取一个边能反映模板的角度变化角度就行。

*这里步不去求边线交点了，直接拿上面的(mode_row[0],mode_col[0])作为模板点。第一点和第二点连线角度作为模板角度，计算如下

*模拟取模板点的像素坐标

Mode_X:=mode_row[0]-cam_origin_row

Mode_Y:=mode_col[0]-cam_origin_col

*模拟取模板角度

angle_lx(mode_row[0],mode_col[0],mode_row[1],mode_col[1],Mode_Phi)

*模拟取示教物理坐标

TeachPos_X:=3000

TeachPos_Y:=5000

一般示教流程；

现场的示教流程一般是丛后往前示教。物料先放到放料位置放好，机械手过去将料吸起来，记住此时的示教位置为(TeachPos_X,TeachPos_Y)。机械手吸着物料移移开。相机拍照，拍照后，计算模板的特征点和角度，记下来(Mode_X,Mode_Y) 角度Mode_Phi。

实际实用中经常会出现相机是安装在机械手上的情况，这种情况与上面的其实没有大的区别，多了一个拍照位置而已。坐标的计算稍微复杂一点。

2.来料拍照

这里上的料是 物料放置框，如手机外壳，IC 测试座等。

计算来料的特征点和角度 如上图红框的C点位来料特征点(Current_X,Current_Y),直线CD的角度 记为Current_Phi。如上图

特征点的选取一般选2条边的交点。

*假设来料绕左上顶点旋转5°，平移(40,50)

*定义实际来料，区域，通过上面坐标平移旋转得到，分2步，第一步饶第一点(mode_row[0], mode_col[0])旋转5°，第二步，平移(40,50)

hom_mat2d_identity(HomMat2DIdentity)

hom_mat2d_rotate(HomMat2DIdentity, rad(5), mode_row[0], mode_col[0], HomMat2DRotate)

hom_mat2d_translate(HomMat2DRotate, 40, 50, HomMat2DTranslate1)

*模拟来料旋转和平移

affine_trans_pixel(HomMat2DTranslate1,mode_row,mode_col,RowTrans, ColTrans)

gen_region_polygon(ProductRegion, RowTrans, ColTrans)

dev_set_color ('red')

dev_display(ProductRegion)

*模拟取来料的特征点坐标，这里为了简单，直接获取了，实际工作中要通过模板匹配或者模板匹配加边线交点来获取坐标

Current_X:=RowTrans[0]-cam_origin_row

Current_Y:=ColTrans[0]-cam_origin_col

angle_lx(RowTrans[0],ColTrans[0],RowTrans[1],ColTrans[1],Current_Phi)

3.计算偏移量

这种模式换个角度思考，其实和先拍后抓是一个原理，都是要保证机械手相对物料姿态一致，不同的是前者是取料，相对的是待取物料，后者用来放料，相对的是放料框。

此部分不再赘述，请参照第6篇文章先拍后取。

这里直接上关键代码了

计算代码

*第3步 纠偏计算

* 将相关特征点转化为物理坐标系

*模板特征点

affine_trans_point_2d(HomMat2D1,Mode_X,Mode_Y,Wmodex,Wmodey)

*来料实际特征点

affine_trans_point_2d(HomMat2D1,Current_X,Current_Y,Wcurx,Wcury)

*计算模板Mark 绕旋转中心旋转后的坐标

*著名的绕点旋转公式

*在平面坐标上，任意点P(x1,y1)，绕一个坐标点Q(x2,y2)旋转θ角度后,新的坐标设为(x, y)的计算公式：

*x= (x1 - x2)*cos(θ) - (y1 - y2)*sin(θ) + x2 ;

*y= (x1 - x2)*sin(θ) + (y1 - y2)*cos(θ) + y2 ;

dphi:=Current_Phi-Mode_Phi

newModeX:=(Wmodex-TeachPos_X)*cos(dphi)-(Wmodey-TeachPos_Y)*sin(dphi)+TeachPos_X

newModeY:=(Wmodex-TeachPos_X)*sin(dphi)+(Wmodey-TeachPos_Y)*cos(dphi)+TeachPos_Y

*计算最终位置偏移

Dx:=Wcurx-newModeX

Dy:=Wcury-newModeY

角度DeltaPhi。在角度数据发送给机械手之前可能要做些处理，有些机械手不能接受角度数据，只能接受位置数据(如脉冲数)。dphi需要乘以系数，这个系数需要标定或    者设备提供

实际使用中可能出现最终发送的角度要取反的问题，需要现场灵活应对。

4.验证

如下图，红色是来物料放置框 实际情况。经过第三步计算的补偿数据，运行如下。物料刚好可以放到物料框中

总结：

1.先拍后放与先拍后抓的的示教流程和计算逻辑基本一致，正常示教逻辑都一致。

2.示教机械点应该记哪个位置，可以简单理解为在哪个位置做旋转纠偏，就把这个位置设定为.(TeachPos_X,TeachPos_Y)

总结：

1.先拍后取，和先取后拍的处理方式差别不是很大。区别主要是再纠偏计算。先拍后取 旋转的是基准模板(这个是想象的旋转，实际不旋转)

先取后拍 旋转的是实际来料，这个容易理解。

2.旋转的时候都是绕着固定机械点旋转的。可以认为绕那个点旋转就设定那个点为(TeachPos_X,TeachPos_Y)。务必理解这个旋转的点。

完整代码​​​​​​​

2D视觉定位引导系列 1 图解9点标定及仿真

2D视觉定位引导系列 2 图解12点标定及仿真

2D视觉定位引导系列 3 探讨海康 VisionMaster N点标定的内部原理

2D视觉定位引导系列 4 相机运动模式下的标定和仿真

2D视觉定位引导系列 6 先拍后取原理及模拟仿真

2D视觉定位引导系列 7 先取后拍原理及动画模拟仿真 – 麦田 (sy9527.com)