一. 课题背景
机械臂引导:3D视觉引导机械臂方案被越来越多的行业认可,如:快递分拣供包、汽车配件上下料、仓储拆码垛、仓配货品拣选。通过使用3D视觉系统,可以实时获取目标物体的三维位置和姿态信息。这使得机械臂能够准确地定位和操作目标物体,实现高精度的任务执行。
二. 环境介绍
1、Halcon 21.05
2、OpenCV Sharp
3、Visual Studio 2019

演示视频如附件所示
三. 手眼标定
手眼标定是为了获得相机坐标系与机器人坐标系之间的转换关系,这里使用简单粗暴的光平面法获得相机坐标与机器人坐标系的转换关系;
首先准备一个长方形物体沿机械臂X轴和Y轴运动方向摆放(其中长方形的长边对应机械臂的X轴方向,短边对应机械臂的Y轴方向),使用相机获取点云如下图所示:

对点云进行滤波,去除无关背景提取出长方形物体点云,使用halon将其拟合为一个光平面如下图所示:

原始相机坐标系与长方形光面点云的位置关系如下所示:

计算光平面的三个主轴,得到一个以点的平均值为中心的姿势,对应于姿势的坐标系具有沿第一主轴的x轴、沿第二主轴的y轴和沿第三主轴的z轴。

使用该Pose对点云模型进行刚体变换,得到变换后的光平面点云ObjectModel3DStandard,计算光平面点云ObjectModel3DStandard的最小外接矩形,对最小外接矩形的姿态进行反转得到pose1 ,通过pose1对ObjectModel3DStandard进行刚体变换

变换后如下图所示:

由于长方形的长边对应机器人的X轴,短边对应机器人的Y轴,此时相机坐标系的X轴平行于长方形物体的长边,长方形短边平行于相机坐标系Y轴,相机坐标系与机器人坐标系坐标轴的方向已经一致, pose0和pose1为相机坐标系与机器人坐标系的变换关系,接下来自定义两个姿态将Pose0和pose1保存下来
create_pose (-43.9651,19.352,1031.71,359.001,359.198,185.332,'Rp+T', 'gba', 'point', PoseA)
create_pose (-70.6744,-70.6666,2061.22,181.31,1.74528,169.493,'Rp+T', 'gba', 'point', PoseB)
利用PoseA和PoseB对原始长方形点云进行刚体变换,将其变换到机械臂坐标系下;
rigid_trans_object_model_3d (ObjectModel3D, PoseA, A)rigid_trans_object_model_3d (A, PoseB,ObjectModel3DResult)write_object_model_3d (ObjectModel3DResult,'ply', 'D:/项目程序/项目程序/海康3D/jingsai/BasicDemo_Callback/bin/win64/debug/point/unionpoint.ply',[],'false')
当前相机坐标系与机械臂坐标系的坐标轴虽然方向一致,但是在空间中还存在一定的偏移,需要计算偏移量对其进行补偿;
获取标记点的坐标(X,Y,Z),使用示教器将机器人移动到该点获得该点在机器人坐标系下的真实坐标(jx,jy,jz)
jx:=744.50
jy:=304.77
jz:=-38.90cx:=105.976cy:=6.337556cz:=4.744299
px:=jx-cx
py:=jy-cy
pz:=jz-cz
其中(px,py,pz)为偏移,此时我们就已经得到相机坐标系与机器人坐标系完整的转换关系。
四. 鞋底分拣
(一)解析图像与点云数据
由于SDK提供的C#接口不支持同时显示深度图与rgb图,所以对RGB图与深度进行解析,通过winform的picbox控件进行显示
解析RGB图并显示
Marshal.Copy(stFrameData.stImageData[1].pData, m_pcDataBuf,0,(int)stFrameData.stImageData[1].nDataLen);
int m = (int)m_stImageInfo.nHeight;
int n = (int)m_stImageInfo.nWidth;
Mat image = new Mat(new OpenCvSharp.Size(n, m), MatType.CV_8UC3);
Mat result = image.CvtColor(ColorConversionCodes.RGB2BGR);
for (int y = 0; y < m; y++)
{
for (int x = 0; x < n; x++)
{
image.Set(y, x, new Vec3b(m_pcDataBuf[x + y * n + 2 * m * n], m_pcDataBuf[x + y * n + m * n], m_pcDataBuf[x + y * n]));
}
}
System.Drawing.Bitmap bitmap = OpenCvSharp.Extensions.BitmapConverter.ToBitmap(image);
System.Drawing.Bitmap bitmaps = OpenCvSharp.Extensions.BitmapConverter.ToBitmap(image);
pictureBox2.Image = bitmap;
解析深度图,并显示
Mat depthData = new Mat((int)stFrameData.stImageData[0].nHeight, (int)stFrameData.stImageData[0].nWidth, MatType.CV_16UC1, stFrameData.stImageData[0].pData);
double scaleFactor = 255.0 / 1000.0;
Mat normalizedDepth = new Mat();
Cv2.ConvertScaleAbs(depthData, normalizedDepth, scaleFactor);
Mat depthImage8Bit = new Mat();
normalizedDepth.ConvertTo(depthImage8Bit, MatType.CV_8U);
double thresholdValue = 128;
double maxValue = 255;
Cv2.Threshold(depthImage8Bit, depthImage8Bit, thresholdValue, maxValue, ThresholdTypes.Binary);
Bitmap bmp1 = new Bitmap(depthImage8Bit.Cols, depthImage8Bit.Rows, (int)depthImage8Bit.Step(), System.Drawing.Imaging.PixelFormat.Format8bppIndexed, depthImage8Bit.Data);
pictureBox3.Image = bmp1;
pictureBox3.SizeMode = PictureBoxSizeMode.StretchImage;
解析点云数据,并将其转换为halcon的HTuple数据类型,使用hWindowControl控件对点云进行显示
Mv3dRgbdSDK.MV3D_RGBD_MapDepthToPointCloud(m_handle, stFrameData.stImageData[0], pointcloud);
Marshal.Copy(pointcloud.pData, pointData, 0, (int)pointcloud.nDataLen);
float[] floatArray = new float[pointData.Length / sizeof(float)];
for (int g = 0; g < floatArray.Length; g++)
{
floatArray[g] = BitConverter.ToSingle(pointData, g * sizeof(float));
}
int nPointNum = (int)pointcloud.nDataLen / (sizeof(float) * 3);
List<float> vecX = new List<float>(nPointNum);
List<float> vecY = new List<float>(nPointNum);
List<float> vecZ = new List<float>(nPointNum);
for (int nPntIndex = 0; nPntIndex < nPointNum; ++nPntIndex)
{
vecX.Add(floatArray[nPntIndex * 3 + 0]);
vecY.Add(floatArray[nPntIndex * 3 + 1]);
vecZ.Add(floatArray[nPntIndex * 3 + 2]);
}
HTuple a = new HTuple();
HTuple b = new HTuple();
HTuple c = new HTuple();
for (int r = 0; r < nPointNum; r++)
{
a[r] = vecX[r];
b[r] = vecY[r];
c[r] = vecZ[r];
}
HOperatorSet.GenObjectModel3dFromPoints(a, b, c, out hv_selectObjectModel3D);
HOperatorSet.WriteObjectModel3d(hv_selectObjectModel3D, "ply", "oud_zzss.ply", new HTuple(), "false");

(二)点云预处理
通过点云滤波、降采样以及聚类分割对原始点云进行预处理去除无关点云,得到鞋底点云
sample_object_model_3d (cloud_a, 'fast', 1.5, [], [], SampledObjectModel3D)
get_object_model_3d_params (cloud_a, 'num_points', GenParamValuess)
get_object_model_3d_params (SampledObjectModel3D, 'num_points', GenParamValue1aa)
select_points_object_model_3d (SampledObjectModel3D, 'point_coord_z',500, 760, ObjectModel3D)
select_points_object_model_3d (ObjectModel3D, 'point_coord_x', -500, 325, ObjectModel3D1)
select_points_object_model_3d (ObjectModel3D1, 'point_coord_y', -350, 200, ObjectModel3D2)
connection_object_model_3d (ObjectModel3D2, 'distance_3d',2.2, ObjectModel3DConnected)
get_object_model_3d_params (ObjectModel3DConnected, 'num_points', GenParamValue1)
select_object_model_3d (ObjectModel3DConnected, 'num_points', 'and',2000,15000, ObjectModel3DSelected)
write_object_model_3d (ObjectModel3D2, 'ply', 'D:/项目程序/项目程序/海康3D/jingsai/BasicDemo_Callback/bin/win64/debug/point/g3.ply', [], 'false')
(三)读取模板的点云模型
由于鞋底分为左右脚和正反面,所以建立4个模板,分别为左脚正面、左脚反面、右脚正面、右脚反面
read_object_model_3d ('C:/Users/93733/Desktop/分拣鞋/新建文件夹/hkmodel/zuozheng.ply', 'm', [], [], zuo_zheng, Status)
read_object_model_3d ('C:/Users/93733/Desktop/分拣鞋/新建文件夹/hkmodel/zuo_fan.ply', 'm', [], [], zuo_fan, Status)
read_object_model_3d ('C:/Users/93733/Desktop/分拣鞋/新建文件夹/hkmodel/youzheng.ply', 'm', [], [], you_zheng, Status)
read_object_model_3d ('C:/Users/93733/Desktop/分拣鞋/新建文件夹/hkmodel/you_fan.ply', 'm', [], [], you_fan, Status)
rigid_trans_object_model_3d (zuo_zheng, PoseA, ObjectModel3DStandard2)
rigid_trans_object_model_3d (ObjectModel3DStandard2, PoseB, ObjectModel3D_zuozheng)
rigid_trans_object_model_3d (zuo_fan, PoseA, ObjectModel3DStandard2)
rigid_trans_object_model_3d (ObjectModel3DStandard2, PoseB, ObjectModel3D_zuo_fan)
rigid_trans_object_model_3d (you_zheng, PoseA, ObjectModel3DStandard2)
rigid_trans_object_model_3d (ObjectModel3DStandard2, PoseB, ObjectModel3D_you_zheng)
rigid_trans_object_model_3d (you_fan, PoseA, ObjectModel3DStandard2)
rigid_trans_object_model_3d (ObjectModel3DStandard2, PoseB, ObjectModel3D_you_fan)
create_pose (743, 158.23,-131.77, 179, 0, 0, 'Rp+T', 'gba', 'point', Pose_youzheng)
create_pose (751, 218.35,-131.66, 179, 0, 0, 'Rp+T', 'gba', 'point', Pose_youfan)
create_pose (726.22, 239.44,-131.15, 179, 0, 0, 'Rp+T', 'gba', 'point', Pose_zuozheng)
create_pose (689.30,216.93,-138.92, 179, 0, 0, 'Rp+T', 'gba', 'point', Pose_zuofan)
(四)进行3D表面匹配
首先通过刚体变换将目标点云变换到机器坐标系下,将四个模板点云分别与目标点云进行3D表面匹配,提取Scores最大的目标作为抓取目标,得到姿态Pose,利用Pose将模板变换到目标点云上,计算质心以及抓取姿态;
该部分只截取了部分代码,完整代码将在比赛最后提交的研究报告中开源

将上述halcon代码封装为外部函数,通过halcon引擎对其进行调用;
(五)上位机程序
机械臂作为客户端,上位机作为服务器,当机械臂离开规定区域时,客服端会发送拍照信号,在上位机开启一个线程来监听客户端是否发送拍照信号,当监听到客户端发送的拍照信号时,相机进行拍照,并将点云数据传送到halcon引擎中,进行点云处理以及表面匹配获取抓取坐标和姿态,当机械臂复位时,发送下一次的抓取坐标和姿态。
示教器端程序如图所示:

上位机与机器人的通信程序如下所示:
void Robot_Init()
{
robot_socket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
try
{
IPAddress ip = IPAddress.Parse("192.168.1.200");
IPEndPoint point = new IPEndPoint(ip, 6000);
robot_socket.Bind(point);
robot_socket.Listen(1);
listBox1.Items.Add("服务器创建成功");
}
catch
{
listBox1.Items.Add("请检查robot的IP和端口");
}
Thread listen_th = new Thread(listenA);
listen_th.IsBackground = true;
listen_th.Start(robot_socket);
}
private void listenA(object o)
{
// listBox1.Items.Clear();
Socket socketwatch = o as Socket;
try
{
while (true)
{
mysocket = socketwatch.Accept();
// MessageBox.Show("有客户端接入");
listBox1.Items.Add("有客户端接入");
Thread receiveer = new Thread(receive);
receiveer.IsBackground = true;
receiveer.Start(mysocket);
}
}
catch
{
listBox1.Items.Add("错误");
}
}
private void receive(object o)
{
string str = "";
byte[] buffer = new byte[1024];
int len = 0;
int time_m = 0;
Socket mysocket = o as Socket;
bool fg = false;
while (true)
{
len = mysocket.Receive(buffer, 0, buffer.Length, SocketFlags.None);
str = Encoding.UTF8.GetString(buffer, 0, len);
if (str == "6")
{
robot_flag = true;
}
if (str == "success")
{
label3.Text = "success";
}
}
}
由于在halcon中得到的姿态表示方式为欧拉角,而我们使用的协作机械臂姿态表达方式为轴角,所以需要将欧拉角转换为轴角,转换过程如下所示:
public static double[,] matrix_multiply(double[,] a, double[,] b)
{
int n = a.GetLength(0);
int m = b.GetLength(1);
int q = a.GetLength(1);
double[,] result = new double[n, m];
if (a.GetLength(1) != b.GetLength(0))
{
throw new Exception("matrix1 的列数与 matrix2 的行数不相等");
}
for (int i = 0; i < n; i++)
{
for (int j = 0; j < m; j++)
{
for (int k = 0; k < q; k++)
{
result[i, j] += a[i, k] * b[k, j];
}
}
}
return result;
}
public static double[,] Rx(double r)
{
double r1 = r / 180 * Math.PI;
double[] x1 = { 1, 0, 0 };
double[] x2 = { 0, Math.Cos(r1), -Math.Sin(r1) };
double[] x3 = { 0, Math.Sin(r1), Math.Cos(r1) };
double[,] data = new double[3, 3];
for (int i = 0; i < 3; i++)
{
data[0, i] = x1[i];
data[1, i] = x2[i];
data[2, i] = x3[i];
}
return data;
}
public static double[,] Ry(double r)
{
double r1 = r / 180 * Math.PI;
double[] x1 = { Math.Cos(r1), 0, Math.Sin(r1) };
double[] x2 = { 0, 1, 0 };
double[] x3 = { -Math.Sin(r1), 0, Math.Cos(r1) };
double[,] data = new double[3, 3];
for (int i = 0; i < 3; i++)
{
data[0, i] = x1[i];
data[1, i] = x2[i];
data[2, i] = x3[i];
}
return data;
}
public static double[,] Rz(double r)
{
double r1 = r / 180 * Math.PI;
double[] x1 = { Math.Cos(r1), -Math.Sin(r1), 0 };
double[] x2 = { Math.Sin(r1), Math.Cos(r1), 0 };
double[] x3 = { 0, 0, 1 };
double[,] data = new double[3, 3];
for (int i = 0; i < 3; i++)
{
data[0, i] = x1[i];
data[1, i] = x2[i];
data[2, i] = x3[i];
}
return data;
}
public static double[] RotationMatrixTo_Vec(double[,] R)
{
Double[] vec = new Double[3];
double x, y, z, theta; // x,y,z 表示轴向量,theta表示角度
double epsilon = 1E-12;
double v = (R[0, 0] + R[1, 1] + R[2, 2] - 1.0f) / 2.0f; //获得角度
if (Math.Abs(v) < 1 - epsilon)
{
theta = Math.Acos(v);
x = 1 / (2 * Math.Sin(theta)) * (R[2, 1] - R[1, 2]);
y = 1 / (2 * Math.Sin(theta)) * (R[0, 2] - R[2, 0]);
z = 1 / (2 * Math.Sin(theta)) * (R[1, 0] - R[0, 1]);
}
else
{
if (v > 0.0f)
{
// \theta = 0, diagonal elements approaching 1
theta = 0;
x = 0;
y = 0;
z = 1;
}
else
{
theta = Math.PI;
if (R[0, 0] >= R[1, 1] && R[0, 0] >= R[2, 2])
{
x = Math.Sqrt((R[0, 0] + 1) / 2);
y = R[0, 1] / (2 * x);
z = R[0, 2] / (2 * x);
}
else if (R[1, 1] >= R[0, 0] && R[1, 1] >= R[2, 2])
{
y = Math.Sqrt((R[1, 1] + 1) / 2);
x = R[1, 0] / (2 * y);
z = R[1, 2] / (2 * y);
}
else
{
z = Math.Sqrt((R[2, 2] + 1) / 2);
x = R[2, 0] / (2 * z);
y = R[2, 1] / (2 * z);
}
}
}
vec[0] = x * theta;
vec[1] = y * theta;
vec[2] = z * theta;
return vec;
}
点击按键进行自动抓取
private void button5_Click(object sender, EventArgs e)
{
_zidong_img = new Thread(z_d_camera);
_zidong_img.Start();
} void z_d_camera()
{
while (cam_flag == true)
{
if ((firstcamera == false))
{
robot_flag = false;
firstcamera = true;
_zidong_img = new Thread(ReceiveThreadProcess);
_zidong_img.Start();
}
else if ((firstcamera == true && robot_flag == true) || ca_fl == true)
{
ca_fl = false;
robot_flag = false;
_zidong_img = new Thread(ReceiveThreadProcess);
_zidong_img.Start();
}
}
}
{
HEngine.SetProcedurePath(System.AppDomain.CurrentDomain.BaseDirectory + @"halcon");
Window1 = hWindowControl2.HalconWindow;
setwindow(Window1);
//通过halcon引擎调用外部函数
var sm1 = new HDevProcedureCall(new HDevProcedure("hk_math"));
sm1.SetInputCtrlParamTuple("cloud_a", hv_selectObjectModel3D);
sm1.SetInputCtrlParamTuple("WindowHandle", Window1);
sm1.Execute();
data_x = sm1.GetOutputCtrlParamTuple("x");
data_y = sm1.GetOutputCtrlParamTuple("y");
data_z = sm1.GetOutputCtrlParamTuple("z");
x = data_x / 1000;
y = data_y / 1000;
z = data_z / 1000;
data_rx = sm1.GetOutputCtrlParamTuple("rx");
data_ry = sm1.GetOutputCtrlParamTuple("ry");
data_rz = sm1.GetOutputCtrlParamTuple("rz");
try
{
double[] angle = Rzyx_to_vec(data_rx, data_ry, data_rz);
string txt = "(" + x.ToString() + "," + y.ToString() + "," + z.ToString() + "," + angle[0].ToString() + "," + angle[1].ToString() + "," + angle[2].ToString() + ")";
showTxt.Text = "x: " + x.ToString() + "\r\n" + "y: " + y.ToString() + "\r\n" + "z:" + z.ToString() + "\r\n" +"rx:" + angle[0].ToString() + "\r\n" + "ry:" + angle[1].ToString() + "\r\n" + "rz:" + angle[2].ToString() + "\r\n";
hv_selectObjectModel3D.Dispose();
mysocket.Send(Encoding.UTF8.GetBytes(txt));
listBox1.Items.Add("通讯成功");
}
catch
{
ca_fl = true;
}
}
窗口4绿色区域为机械臂每次抓取的鞋底,上位机运行时的界面如下所示:

演示视频如附件所示