移动相机定位(跟着机械手跑,不带旋转轴)
9点标定
HTuple Row = new HTuple();//图像row
HTuple Column = new HTuple();//图像column
HTuple X = new HTuple();//机械x
HTuple Y = new HTuple();//机械y
for (int i = 0; i < 9; i++)
{
Row[i] = Convert.ToDouble(dataGridView3.Rows[i].Cells[0].Value.ToString());
Column[i] = Convert.ToDouble(dataGridView3.Rows[i].Cells[1].Value.ToString());
X[i] = Convert.ToDouble(dataGridView3.Rows[8 - i].Cells[2].Value.ToString());
Y[i] = Convert.ToDouble(dataGridView3.Rows[8 - i].Cells[3].Value.ToString());
}
HOperatorSet.VectorToHomMat2d(Row, Column, Y, X, out homMat2d);
homMat2d: 9点标定矩阵计算位置
//获取图像中心对应的 X Y 机械坐标
HOperatorSet.AffineTransPoint2d(homMat2d, height / 2, width / 2, out Y2, out X2);
HTuple BX, BY;
//获取图像定位的 X Y
HOperatorSet.AffineTransPoint2d(homMat2d, row, column, out BY, out BX);
//定位位置(BX,BY) - 图像中心位置(X2,Y2) = 定位位置到图像中心位置的偏差(X1,Y1)
double X1 = BX.D - X2.D;
double Y1 = BY.D - Y2.D;
//当前机械位置(X3,Y3) + 到图像中心位置的偏差(X1,Y1)
double AX = X3 + X1;
double AY = Y3 + Y1;
(AX,YX):就是最后定位的位置(此位置是图像中心能对准实际定位位置,最后定位的位置)固定向上相机纠偏(从下往上拍,带旋转角度)
9点标定
HTuple rows = new HTuple();
HTuple columns = new HTuple();
HTuple robot_x = new HTuple();
HTuple robot_y = new HTuple();
for (int i = 0; i < dataGridView_Cam1.Rows.Count-1; i++)
{
rows [i] = Convert.ToDouble(dataGridView_Cam1.Rows[i].Cells[1].Value.ToString());
columns [i] = Convert.ToDouble(dataGridView_Cam1.Rows[i].Cells[2].Value.ToString());
robot_x [i] = Convert.ToDouble(dataGridView_Cam1.Rows[i].Cells[3].Value.ToString());
robot_y [i] = Convert.ToDouble(dataGridView_Cam1.Rows[i].Cells[4].Value.ToString());
}
HOperatorSet.VectorToHomMat2d(rows , columns , robot_y , robot_x , out homMat2d);
homMat2d:9点标定矩阵旋转标定
HTuple CenterX = new HTuple();
HTuple CenterY = new HTuple();
HTuple robot_xs= new HTuple();
HTuple robot_ys= new HTuple();
for (int i = 9; i < 22; i++)
{
HTuple Robot_Point1_X2, Robot_Point1_Y2;
//将图像旋转的像素坐标转换成机械坐标
HOperatorSet.AffineTransPoint2d(homMat2d,
double.Parse(dataGridView_Cam1.Rows[i].Cells[1].Value.ToString()),
double.Parse(dataGridView_Cam1.Rows[i].Cells[2].Value.ToString()),
out Robot_Point1_Y2, out Robot_Point1_X2);
robot_xs[i - 9] = Robot_Point1_X2.D;
robot_ys[i - 9] = Robot_Point1_Y2.D;
}
//根基全部变换后的机械坐标 计算旋转中心
Get_Cirrle_Center(robot_xs, robot_ys, out CenterX, out CenterY, halconWindow1);
(CenterX,CenterY):就是圆心(要保存)
//拟合圆心
public void Get_Cirrle_Center(HTuple hv_Rows, HTuple hv_Cols, out HTuple hv_Center_Row, out HTuple hv_Center_Column)
{
// Local iconic variables
HObject ho_Contour, ho_ContCircle, ho_ContCircle1;
HObject ho_Cross;
// Local control variables
HTuple hv_Radius = new HTuple(), hv_StartPhi = new HTuple();
HTuple hv_EndPhi = new HTuple(), hv_PointOrder = new HTuple();
HTuple hv_DistanceMin = new HTuple(), hv_DistanceMax = new HTuple();
// Initialize local and output iconic variables
HOperatorSet.GenEmptyObj(out ho_Contour);
HOperatorSet.GenEmptyObj(out ho_ContCircle);
HOperatorSet.GenEmptyObj(out ho_ContCircle1);
HOperatorSet.GenEmptyObj(out ho_Cross);
hv_Center_Row = new HTuple();
hv_Center_Column = new HTuple();
ho_Contour.Dispose();
HOperatorSet.GenContourPolygonXld(out ho_Contour, hv_Rows, hv_Cols);
//拟合一个圆XLD
hv_Center_Row.Dispose(); hv_Center_Column.Dispose(); hv_Radius.Dispose(); hv_StartPhi.Dispose(); hv_EndPhi.Dispose(); hv_PointOrder.Dispose();
HOperatorSet.FitCircleContourXld(ho_Contour, "geotukey", -1, 0, 0, 3, 2, out hv_Center_Row, out hv_Center_Column, out hv_Radius, out hv_StartPhi, out hv_EndPhi, out hv_PointOrder);
//创建圆弧xld
ho_ContCircle.Dispose();
HOperatorSet.GenCircleContourXld(out ho_ContCircle, hv_Center_Row, hv_Center_Column, hv_Radius, hv_StartPhi, hv_EndPhi, "negative", 1);
using (HDevDisposeHelper dh = new HDevDisposeHelper())
{
hv_DistanceMin.Dispose(); hv_DistanceMax.Dispose();
HOperatorSet.DistancePc(ho_ContCircle, hv_Rows.TupleSelect(2), hv_Cols.TupleSelect(2), out hv_DistanceMin, out hv_DistanceMax);
}
//判断第三点在不在拟合的弧线上面，这一步主要用点的顺序确定圆弧的其实位置
if ((int)(new HTuple(hv_DistanceMin.TupleGreater(1))) != 0)
{
ho_ContCircle.Dispose();
HOperatorSet.GenCircleContourXld(out ho_ContCircle, hv_Center_Row, hv_Center_Column, hv_Radius, hv_StartPhi, hv_EndPhi, "positive", 1);
}
//拟合一个整圆(其实就是弧度值0到2PI)
ho_ContCircle1.Dispose();
HOperatorSet.GenCircleContourXld(out ho_ContCircle1, hv_Center_Row, hv_Center_Column, hv_Radius, 0, 6.28318, "positive", 1);
using (HDevDisposeHelper dh = new HDevDisposeHelper())
{
ho_Cross.Dispose();
HOperatorSet.GenCrossContourXld(out ho_Cross, hv_Center_Row, hv_Center_Column, hv_Radius / 5, 0);
}
ho_Contour.Dispose();
ho_ContCircle.Dispose();
hv_Radius.Dispose();
hv_StartPhi.Dispose();
hv_EndPhi.Dispose();
hv_PointOrder.Dispose();
hv_DistanceMin.Dispose();
hv_DistanceMax.Dispose();
return;
}
基准设定
假设(row_Standar ,col_Standar ,angle_Standar )为产品特征点基准
HTuple row_Standar = new HTuple(0.0);
HTuple col_Standar = new HTuple(0.0);
//这是角度 不是弧度
HTuple angle_Standar = new HTuple(0.0);
//换算后的机械位置
HTuple Robot_x_Standar = new HTuple(0.0);
HTuple Robot_y_Standar = new HTuple(0.0);
HOperatorSet.AffineTransPoint2d(homMat2d, row_Standar , col_Standar , out Robot_y_Standar, out Robot_x_Standar);
//此时(Robot_x_Standar ,Robot_y_Standar ,angle_Standar )为产品基准位置角度计算偏移量
前面保存的产品基准位置角度(Robot_x_Standar ,Robot_y_Standar ,angle_Standar )
//产品定位的 row,column,angle
HTuple Center_Row = new HTuple(0.00);
HTuple Center_Column = new HTuple(0.00);
HTuple Angle = new HTuple(0.00);//此为角度 不是弧度
HTuple robot_x = new HTuple(0.0);
HTuple robot_y = new HTuple(0.0);
//将定位的(row,column) 转换成机械坐标(robot_x ,robot_y )
HOperatorSet.AffineTransPoint2d(homMat2d, Center_Row, Center_Column, out robot_y, out robot_x);
//极坐标转换
//要转换的角度(基准角度-定位角度)
double rotateR = angle_Standar - Angle.D;
//极坐标转换后的位置
double robot_Rotate_X = 0, robot_Rotate_Y = 0;
//将定位的位置(机械坐标)旋转中心旋转offsetU 度 得到旋转后的坐标(robot_Rotate_X,robot_Rotate_Y)
Work.RotateAngle(CenterX ,CenterY ,rotateR ,robot_x.D, robot_y.D, ref robot_Rotate_X, ref robot_Rotate_Y);
//计算补偿
// xy补偿 (基准位置(Robot_x_Standar ,Robot_y_Standar )-旋转后的位置(robot_Rotate_X,robot_Rotate_Y) = 补偿)
double offsetX = Robot_x_Standar - robot_Rotate_X;
double offsetY = Robot_y_Standar - robot_Rotate_Y;
double offsetr = Angle.D - angle_Standar ;
//最终位置
X = 贴料位置x + offsetX ;
Y = 贴料位置y + offsetY ;
R = 贴料位置r + offsetr ;
贴料位置(可以是死位置 也可以是定位的位置)
//极坐标转换
public static string RotateAngle(double XRotation, double YRotation, double ARotate, double XBefore, double YBefore, ref double XAfter, ref double YAfter)
{
try
{
double Rad = 0;
Rad = ARotate * Math.PI / 180;
XAfter = (XBefore - XRotation) * Math.Cos(Rad) - (YBefore - YRotation) * Math.Sin(Rad) + XRotation;
YAfter = (XBefore - XRotation) * Math.Sin(Rad) + (YBefore - YRotation) * Math.Cos(Rad) + YRotation;
return "OK";
}
catch (Exception ex)
{
return ex.Message;
}
}
固定上相机(往下拍 相机固定 带旋转)
9点标定+旋转标定(和固定下相机一样标定)
得到:homMat2d 9点标定矩阵 (CenterX,CenterY):圆心坐标(机械位置)(要保存)
设定基准点 (和固定下相机一样)
假设(row_Standar ,col_Standar ,angle_Standar )为产品特征点基准
HTuple row_Standar = new HTuple(0.0);
HTuple col_Standar = new HTuple(0.0);
//这是角度 不是弧度
HTuple angle_Standar = new HTuple(0.0);
//换算后的机械位置
HTuple Robot_x_Standar = new HTuple(0.0);
HTuple Robot_y_Standar = new HTuple(0.0);
HOperatorSet.AffineTransPoint2d(homMat2d, row_Standar , col_Standar , out Robot_y_Standar, out Robot_x_Standar);
//此时(Robot_x_Standar ,Robot_y_Standar ,angle_Standar )为产品基准位置角度计算取料位
前面保存的产品基准位置角度(Robot_x_Standar ,Robot_y_Standar ,angle_Standar )
//产品定位的 row,column,angle
HTuple Center_Row = new HTuple(0.00);
HTuple Center_Column = new HTuple(0.00);
HTuple Angle = new HTuple(0.00);//此为角度 不是弧度
//定位角度
double location_angle = hv_Angle.TupleDeg().D;
//定位XY (机械位置)
HTuple Robot_Point1_X2, Robot_Point1_Y2;
//将定位的像素换算成机械
HOperatorSet.AffineTransPoint2d(homMat2d, Row, Column, out Robot_Point1_Y2, out Robot_Point1_X2);
double location_x = Robot_Point1_Y2.D;
double location_y = Robot_Point1_X2.D;
//1 先算出模板的旋转角度 模板角度 - 定位角度 = 角度偏差
//2 在把模板特征点XY 绕 旋转中心 按照角度偏差旋转 得到新的 X Y
//3 再用定位的XY - 旋转后的XY 得到新的XY 偏差
//基准角度 - 定位角度 = 要旋转的角度
double offsetAngle = angle_Standar - location_angle;
double x = 0.0;
double y = 0.0;
//将模板基准点饶旋转中心旋转(极坐标转换)
//固定下相机是旋转定位位置 而这里是旋转设定的基准位置
RotateAngle(CenterX, CenterY, offsetAngle*-1,Robot_x_Standar ,Robot_y_Standar, ref x, ref y);
//当前定位(location_x ,location_y) - 旋转后的基准点
double offsetx = location_x - x;
double offsety = location_y - y;
double offsetu = offsetAngle;
//基准取料机械位置(Standard_X,Standard_Y ,Standard_R ) + 补偿 以实际为准
double X = Standard_X + offsetx;
double Y = Standard_Y + offsety;
double U = Standard_R + offsetu;
扫一扫
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
