前言
标定话题的探讨，暂时告一段落，现在开始具体应用了
我们一如既往像以前一样，边讲解原理，并用halcon 代码仿真整个过程。让大家即使在没有相机和机械手的情况下依然能学习和了解这个纠偏过程。中间有问题的，欢迎及时提问。
先看下仿真视频 ，先标定，后做引导仿真
仿真部件
1.蓝色矩形框表示相机
2.绿色矩形表示示教的标准模板物料，例如 手机屏幕
3.红色矩形表示实际来料的情况，相对与模板已经发生了角度和位置偏移。
4.粉红色的表示机械手 带一个 矩形吸盘，其中圆形表示法兰盘。这个可以看看以前几期文章
应用场景
先怕再抓 实际主要有以下2中应用场景
1.6轴 机械手，4轴scara机器人，龙门机械手。
来料先放到一个固定不动的载台上。上相机拍照，机械手调整取料姿态去取料。取完料 到放片位置放片。
2.XYR载台
来料先放到XYR载台上，上相机拍照，XYR调整物料位置。
本实验仿真第一种场景，其他情况 有空再模拟
引导定位流程
这种使用场景的定位引导分三步走
1.示教 记住基准，已变后面来料比较计算
示教主要决定机械手取物料的相对姿态，就是吸盘吸到物料时，吸盘和物料的相对位置。这样以后每次通过视觉纠正，每次机械手取物料的相对姿态都是和示教时候一致
示教要记住几组关键数据。
第一组：模板上特征点(可能是十字Mark等特征Mark图案，也可能是线的交点)的坐标和角度，本例取蓝色矩形A点作为特征点PA 记为(Mode_X,Mode_Y)，取AB线段连接的角度为特征角度记为 Mode_Phi，如下图
第二组：示教的机械坐标，就是取标准物料的机械坐标即为(TeachPos_X,TeachPos_Y)
第三组：本例不用，如果是相对标定还得记下拍照位置坐标。
示教
相关代码：
*这里演示做模板的过程，正常的模板图片有个模板点，一般是十字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.来料拍照，计算来料的特征点和角度 如上图红框的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.计算偏移量
偏移量计算的方法比较多，但是最终结果都都基本一致。
对于这个案例，我们可以先这样假想一下，机械手先带着示教的标准物料绕示教的机械位置(TeachPos_X,TeachPos_Y)旋转(Current_Phi-Mode_Phi)角度，
当前来料(红色矩形)和旋转后的标准物料(绿色矩形)角度姿态算是调整一致了，看下图，然后在适当调整X,Y方向位置使其和来料重合。如下图，此时模板上的特征点A已经移动到特征点F了。
此时的C点物理坐标减去F点物理坐标即可到X，Y方向的机械偏移。上面这个假想意思是，我们把示教模板移动到实际的来料位置，机器人需要怎么运动。
上面有个非常关键的点，就是物料的旋转是绕着法兰盘中心旋转，计算也是绕着法兰盘中心坐标旋转计算。我们应该明确一个点，就是所有的旋转必须得绕这个法兰盘来进行。
(我曾遇到过一个问题，6周机器人，末端法兰盘 和 基座上那个轴都可以旋转，结果在标定时候，走旋转标定的时候，机器人走的基座那个旋转轴，没有走末端法兰盘那个轴，导致标定数据异常。
也出现过给纠偏数据，本来是末端法兰盘旋转，结果基座旋转导致位置异常的也有）
说到旋转不得不提下面这个公式
*著名的绕点旋转公式
*在平面坐标上，任意点P(x1,y1)，绕一个坐标点Q(x2,y2)旋转θ角度后,新的坐标设为(x, y)的计算公式：
*x= (x1 – x2)*cos(θ) – (y1 – y2)*sin(θ) + x2 ;
*y= (x1 – x2)*sin(θ) + (y1 – y2)*cos(θ) + y2 ;
旋转计算非常重要的一点是绕哪个点旋转，如果旋转的旋转点不正确，那么计算结果会差很多。对我引动定位可以认为旋转轴的中心坐标就是旋转参考点
对于此例，就是法兰盘中心坐标。
相关代码：
* 将相关特征点转化为物理坐标系
*模板特征点
affine_trans_point_2d(HomMat2D1,Mode_X,Mode_Y,Wmodex,Wmodey)
*来料实际特征点
affine_trans_point_2d(HomMat2D1,Current_X,Current_Y,Wcurx,Wcury)
*想象机械手带着模板先旋转一定角度(Current_Phi-Mode_Phi),再移动(MoveX,MoveY) 后绿色框框和红色框重合
hom_mat2d_identity(HomMat2DIdentity)
hom_mat2d_rotate(HomMat2DIdentity, Current_Phi-Mode_Phi, arm_center_row, arm_center_col, HomMat2DRotate3)
affine_trans_pixel(HomMat2DRotate3,mode_row,mode_col,RowTrans1, ColTrans1)
gen_region_polygon(ModeRotate, RowTrans1, ColTrans1)
dev_set_color ('dark green')
dev_display(ModeRotate)
*计算模板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
最终偏移数据XY(DX,DY,) 角度dphi。在角度数据发送给机械手之前可能要做些处理，有些机械手不能接受角度数据，只能接受位置数据(如脉冲数)。dphi需要乘以系数，这个系数需要标定或者设备提供
实际使用中可能出现最终发送的角度要取反的问题，需要现场灵活应对。
4.验证
下图的黄色部分相对红色的位置与和粉色相对绿色部分基本一致，说明引导的位置没问题。
总结：
1.先做好相机标定，相机的标定分为相对标定和绝对标定，本篇位置使用的是绝对标定数据，相对标定需要修改下纠偏计算部分过程，大家可自行思考，后面我也会继续讲
2.示教记录好三组关键数据，且要明白记录着三组数据的意义，他们的准确性绝对后面计算是否正确
3.纠偏计算需要点空间想象力，纠正的是机器人的抓起姿态
往期文章
完整仿真代码，请加群索取
联系我：加群(277957302)交流
暂无评论