一. 课题背景
机械臂引导:3D视觉引导机械臂方案被越来越多的行业认可,如:快递分拣供包、汽车配件上下料、仓储拆码垛、仓配货品拣选。通过使用3D视觉系统,可以实时获取目标物体的三维位置和姿态信息。这使得机械臂能够准确地定位和操作目标物体,实现高精度的任务执行。
二. 环境介绍
使用halcon和PCL两种解决方案,实现色块分拣和水果分拣。
(一)积木分拣
1、pcl 1.11.1
2、OpenCV
3、Visual Studio 2019
4、VTK 8.2
5、QT 5.13.2
(二)水果分拣
1、Halcon 21.05
2、OpenCV Sharp
3、Visual Studio 2019
三. 手眼标定
由于之前的文章已经对手眼标定进行了介绍,所以这里不在重复介绍。
四. 色块分拣
(一)解析RGB图和点云数据,合成带RGB信息点云
g_pRgbData = (unsigned char*)malloc(stFrameData.stImageData[1].nDataLen);
memset(g_pRgbData, 0, stFrameData.stImageData[1].nDataLen);
ConvertRGB8Planner2BGR8Packed(stFrameData.stImageData[1].pData, stFrameData.stImageData[1].nWidth, stFrameData.stImageData[1].nHeight, g_pRgbData);
cv::Mat mCvmat = cv::Mat(stFrameData.stImageData[1].nHeight, stFrameData.stImageData[1].nWidth, CV_8UC3, g_pRgbData);
cv::Mat bgrImage;
cvtColor(mCvmat, bgrImage,cv::COLOR_RGB2BGR);
QImage Qmtemp = QImage((const unsigned char*)(bgrImage.data), bgrImage.cols, bgrImage.rows, bgrImage.step, QImage::Format_RGB888);
Qmtemp = Qmtemp.scaled(ui.label_2->gt;size(), Qt::IgnoreAspectRatio, Qt::SmoothTransformation);
ui.label_2->gt;setPixmap(QPixmap::fromImage(Qmtemp));
ui.label_2->gt;resize(Qmtemp.size());
ui.label_2->gt;show(); int nPointNum = imgCloud.nDataLen / (sizeof(float) * 3);
float* pSrcValue = (float*)imgCloud.pData;
uchar* rgbvalue = (uchar*)mCvmat.data;
PointCloud::Ptr cloud(new PointCloud);
for (int nPntIndex = 0; nPntIndex < nPointNum;++nPntIndex)
{
PointT p;
p.x = pSrcValue[nPntIndex*3+0];
p.y = pSrcValue[nPntIndex * 3 + 1];
p.z = pSrcValue[nPntIndex * 3 + 2];
p.r = rgbvalue[nPntIndex * 3 + 2];
p.g = rgbvalue[nPntIndex * 3 + 1];
p.b = rgbvalue[nPntIndex * 3 + 0];
cloud->points.push_back(p);
}
cloud->height = 1;
cloud->width = cloud->points.size();
cloudRGB = cloud;
viewer->removeAllPointClouds();
viewer->removeAllShapes();
pcl::visualization::PointCloudColorHandlerRGBField rgb(cloudRGB);
viewer->addPointCloud(cloudRGB, rgb, "point_cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "point_cloud");
ui.qvtkWidget->update();
VTK初始化
viewer.reset(new pcl::visualization::PCLVisualizer("viewer", false));
viewer->setBackgroundColor(0, 0.3, 0.4);
ui.qvtkWidget->SetRenderWindow(viewer->getRenderWindow());
viewer->setupInteractor(ui.qvtkWidget->GetInteractor(), ui.qvtkWidget->GetRenderWindow());
ui.qvtkWidget->update();
viewer1.reset(new pcl::visualization::PCLVisualizer("viewer1", false));
viewer1->setBackgroundColor(0, 0.3, 0.4);
ui.qvtkWidget_2->SetRenderWindow(viewer1->getRenderWindow());
viewer1->setupInteractor(ui.qvtkWidget_2->GetInteractor(), ui.qvtkWidget_2->GetRenderWindow());
ui.qvtkWidget_2->update();

(二)点云预处理
由于点云中存在着大量的无关背景和离群点,所以通过直通滤波与统计滤波对点云进行预处理
pcl::PassThrough<pcl::PointXYZRGB> pass;//设置滤波器对象
pass.setInputCloud(cloudRGB);
pass.setFilterFieldName("z");//设置滤波方向
pass.setFilterLimits(500, 750);
pass.setFilterLimitsNegative(false);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZRGB>());
pass.filter(*cloud_out);
pass.setInputCloud(cloud_out);
pass.setFilterFieldName("x");
pass.setFilterLimits(-330,70);
pass.filter(*cloud_out);
pass.setInputCloud(cloud_out);
pass.setFilterFieldName("y");
pass.setFilterLimits(-400, 100);
pass.filter(*cloud_out);
//统计滤波
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> outrem;//创建统计滤波对象
outrem.setInputCloud(cloud_out);
outrem.setMeanK(800);//附近邻近点数
outrem.setStddevMulThresh(1);//标准差系数
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out1(new pcl::PointCloud<pcl::PointXYZRGB>);
outrem.filter(*cloud_out1);
(三)色块分割
由于需要将不同颜色的积木分拣到不同的区域,所以这里采用设定颜色阈值的分割不同颜色积木的点云
pcl::PointCloud::Ptr redfilteredCloud(new pcl::PointCloud);
pcl::PointCloud::Ptr greenfilteredCloud(new pcl::PointCloud);
pcl::PointCloud::Ptr bluefilteredCloud(new pcl::PointCloud);
pcl::PointCloud::Ptr orginfilteredCloud(new pcl::PointCloud);
pcl::PointCloud::Ptr allpointcloud(new pcl::PointCloud);
std::vector::Ptr> cloudList;
Eigen::Vector4f centroid; // 质心
string color_flag;
pcl::PointXYZ center;
for (size_t i = 0; i < cloud_out1->points.size(); ++i) {
const pcl::PointXYZRGB& point = cloud_out1->points[i];
if (point.r >= 200 && point.r <= 255 && point.g >=60 && point.g <= 100 && point.b >= 10 && point.b <= 60)
{
redfilteredCloud->points.push_back(point);
allpointcloud->points.push_back(point);
}
}
for (size_t i = 0; i < cloud_out1->points.size(); ++i) {
const pcl::PointXYZRGB& point = cloud_out1->points[i];
if (point.r >= 140 && point.r <= 190 && point.g >= 210 && point.g <= 255 && point.b >= 80 && point.b <= 130) {
greenfilteredCloud->points.push_back(point);
allpointcloud->points.push_back(point);
}
}
for (size_t i = 0; i < cloud_out1->points.size(); ++i) {
const pcl::PointXYZRGB& point = cloud_out1->points[i];
if (point.r >= 80 && point.r <= 120 && point.g >= 180 && point.g <= 210 && point.b >= 230) {
bluefilteredCloud->points.push_back(point);
allpointcloud->points.push_back(point);
}
}
for (size_t i = 0; i < cloud_out1->points.size(); ++i) {
const pcl::PointXYZRGB& point = cloud_out1->points[i];
if (point.r >= 230 && point.g >= 160 && point.g <= 190 && point.b >= 60 && point.b <= 100) {
orginfilteredCloud->points.push_back(point);
allpointcloud->points.push_back(point);
}
}
cloudList.push_back(redfilteredCloud);
cloudList.push_back(greenfilteredCloud);
cloudList.push_back(bluefilteredCloud);
cloudList.push_back(orginfilteredCloud); allpointcloud->width = allpointcloud->points.size();
allpointcloud->height = 1;
redfilteredCloud->width = redfilteredCloud->points.size();
redfilteredCloud->height = 1;
greenfilteredCloud->width = greenfilteredCloud->points.size();
greenfilteredCloud->height = 1;
bluefilteredCloud->width = bluefilteredCloud->points.size();
bluefilteredCloud->height = 1;
orginfilteredCloud->width = orginfilteredCloud->points.size();
orginfilteredCloud->height = 1;
由于需要计算每个积木点云的质心坐标用于抓取,而通过颜色阈值分割的点云中可能存在多个相同颜色的色块,所以这里通过点的数量来判断每种颜色的点云中积木的数量是否大于一个,如果数量大于一个,通过聚类分割对该颜色的点云进行继续分割,分割出所有积木点云,并计算其质心坐标,通过vecflag来标记积木点云颜色种类;
for (size_t k = 0; k < cloudList.size(); ++k) {
if(900points.size()<=1400)
{
pcl::compute3DCentroid(*cloudList[k], centroid);
vecX.push_back(centroid(0));
vecY.push_back(centroid(1));
vecZ.push_back(centroid(2));
if (k == 0)
{
vecflag.push_back('R');
}
else if (k == 1)
{
vecflag.push_back('G');
}
else if (k == 2)
{
vecflag.push_back('B');
}
else if (k == 3)
{
vecflag.push_back('O');
}
}
else if(cloudList[k]->points.size()>1400)
{
pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
tree->setInputCloud(cloud_out);
std::vector::Ptr> result;
std::vector cluster_indices;
pcl::EuclideanClusterExtraction ec;
ec.setClusterTolerance(1.4);
ec.setMinClusterSize(650);
ec.setMaxClusterSize(2000);
ec.setSearchMethod(tree); //设置点云的搜索机制
ec.setInputCloud(cloud_out); //设置原始点云
ec.extract(cluster_indices); //从点云中提取聚类
std::vector::Ptr> Eucluextra; //用于储存欧式分割后的点云
for (std::vector::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
pcl::PointCloud::Ptr cloud_cluster1(new pcl::PointCloud);
for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
cloud_cluster1->points.push_back(cloud_out->points[*pit]);
cloud_cluster1->width = cloud_cluster1->points.size();
cloud_cluster1->height = 1;
cloud_cluster1->is_dense = true;
Eucluextra.push_back(cloud_cluster1);
}
for (int t = 0; t < Eucluextra.size(); t++)
{
pcl::compute3DCentroid(*Eucluextra[t], centroid);
vecX.push_back(centroid(0));
vecY.push_back(centroid(1));
vecZ.push_back(centroid(2));
}
if (k == 0)
{
vecflag.push_back('R');
}
else if (k == 1)
{
vecflag.push_back('G');
}
else if (k == 2)
{
vecflag.push_back('B');
}
else if (k == 3)
{
vecflag.push_back('O');
}
}
}
(四)坐标转换及可视化
Matrix_A = Eigen::Matrix4f::Identity();
Matrix_A(0, 0) = -0.995575;
Matrix_A(0, 1) = 0.0929176;
Matrix_A(0, 2) = -0.139971;
Matrix_A(0, 3) = -43.9651;
Matrix_A(1, 0) = -0.0931555;
Matrix_A(1, 1) = -0.995499;
Matrix_A(1, 2) = 0.0174332;
Matrix_A(1, 3) = 19.352;
Matrix_A(2, 0) = -0.0123142;
Matrix_A(2, 1) = 0.01866;
Matrix_A(2, 2) = 0.99975;
Matrix_A(2, 3) = 1031.71;
Matrix_A(3, 0) = 0;
Matrix_A(3, 1) = 0;
Matrix_A(3, 2) = 0;
Matrix_A(3, 3) = 1;
Matrix_B = Eigen::Matrix4f::Identity();
Matrix_B(0, 0) = -0.982777;
Matrix_B(0, 1) = -0.182271;
Matrix_B(0, 2) = -0.0304562;
Matrix_B(0, 3) = -70.6744;
Matrix_B(1, 0) = -0.181623;
Matrix_B(1, 1) = -0.983103;
Matrix_B(1, 2) = 0.0228512;
Matrix_B(1, 3) = -70.6666;
Matrix_B(2, 0) = -0.0341067;
Matrix_B(2, 1) = 0.0169261;
Matrix_B(2, 2) = -0.999275;
Matrix_B(2, 3) = 2061.22;
Matrix_B(3, 0) = 0;
Matrix_B(3, 1) = 0;
Matrix_B(3, 2) = 0;
Matrix_B(3, 3) = 1;
for (size_t i = 0; i < vecX.size(); ++i) {
char color =vecflag[i];
QString colorchinese;
if (color == 'R')
{
colorchinese = u8"红色";
}
else if (color == 'G')
{
colorchinese = u8"绿色";
}
else if (color == 'B')
{
colorchinese = u8"蓝色";
}
else if (color == 'O')
{
colorchinese = u8"橙色";
}
pcl::PointXYZ o;
o.x = vecX[i];
o.y = vecY[i];
o.z = vecZ[i];
viewer1->addSphere(o, 3, 255, 255, 101, std::to_string(i*3), 0);
Eigen::Vector4f eigen_vector(vecX[i], vecY[i], vecZ[i], 0);
Eigen::Vector4f resulat = Matrix_A* eigen_vector;
Eigen::Vector4f resultS = Matrix_B * resulat;
float jx = 744.50;
float jy = 304.77;
float jz = -38.90;
float cx = 105.976;
float cy = 6.337556;
float cz = 4.744299;
float px = jx - cx;
float py = jy - cy;
float pz = jz - cz;
float x = resultS[0] + px;
float y= resultS[1] + py;
float z = resultS[2] + pz;
QString canshu = QString(u8"颜色:""%1"" X:""%2"" Y:""%3"" Z:""%4\n").arg(colorchinese).arg(x).arg(y).arg(z);
string txt = "("+to_string(color)+to_string(x)+","+ to_string(y)+","+ to_string(z)+",179,0,0"+")"; Send(txt); ui.textBrowser->insertPlainText(canshu);
}
pcl::io::savePCDFile("filtered_point_cloud.pcd", *redfilteredCloud);
pcl::io::savePCDFile("greenfilteredCloud.pcd", *greenfilteredCloud);
pcl::io::savePCDFile("bluefilteredCloud.pcd", *bluefilteredCloud);
pcl::io::savePCDFile("orginfilteredCloud.pcd", *orginfilteredCloud);
viewer1->addPointCloud(allpointcloud, std::to_string(8));
viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, std::to_string(8));
ui.qvtkWidget_2->update();
点云处理窗口中的银白色球体为机械臂每次的抓取点,机器人通信的相关程序已经在之前的文章中进行了介绍,所以这里不再重复介绍。

PS:由于实验器材有限,缺少气泵和吸盘,而我们使用的柔性夹爪又比较大,容易碰到相邻的物料所以积木摆放的比较远。
演示视频请在附件中下载
五. 水果分拣
(一)点云预处理
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)
visualize_object_model_3d (WindowHandle,cloud_a, [], [], ['red_channel_attrib','green_channel_attrib','blue_channel_attrib'], ['red','green','blue'], [], [], [], PoseOut)
select_points_object_model_3d (SampledObjectModel3D, 'point_coord_z',900, 1030, ObjectModel3D)
select_points_object_model_3d (ObjectModel3D, 'point_coord_x', -260, 240, ObjectModel3D1)
select_points_object_model_3d (ObjectModel3D1, 'point_coord_y', -160, 180, ObjectModel3D2)
connection_object_model_3d (ObjectModel3D2, 'distance_3d',2.3, ObjectModel3DConnected)//聚类分割
get_object_model_3d_params (ObjectModel3DConnected, 'num_points', GenParamValue1)
select_object_model_3d (ObjectModel3DConnected, 'num_points', 'and',1000,5000, ObjectModel3DSelected)
union_object_model_3d (ObjectModel3DSelected, 'points_surface', UnionObjectModel3D1)
(二)将分割后的点云转换到机械臂坐标系下
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)
rigid_trans_object_model_3d (UnionObjectModel3D1, PoseA, A)
rigid_trans_object_model_3d (A, PoseB, UnionObjectModel3D1result)
(三)抓取坐标计算
通过计算点云的最小外接矩形长和宽大小来分辨橘子和苹果,通过grap_flag来记录水果的种类
grap_flag:=[]
x:=[]
y:=[]
z:=[]
pp:=[]
for Index := 0 to |ObjectModel3DSelected|-1 by 1
smallest_bounding_box_object_model_3d (ObjectModel3DSelected[Index], 'oriented', PoseBox,Length1, Length2, Length3)
rigid_trans_object_model_3d (ObjectModel3DSelected[Index], PoseA, A)
rigid_trans_object_model_3d (A, PoseB, result)
get_object_models_center (result, Center)
point:=[Center[0],Center[1],Center[2], 0,0,0,0]
gen_sphere_object_model_3d (point, 4, pPoint)
pp:=[pp,pPoint]
x:=[x,Center[0]]
y:=[y,Center[1]]
z:=[z,Center[2]]
if(Length1>=70 and Length2>=70)
grap_flag:=[grap_flag,0]
else
grap_flag:=[grap_flag,1]
endif
endfor
将上述的halcon代码封装为外部函数,在winform中通过halcon引擎对其进行调用,上位机和机器人通讯程序已经在之前的文章中进行了介绍,所以这里也不在重复介绍,同样由于实验器材有限,缺少气泵和吸盘,而我们使用的柔性夹爪又比较大,在抓取的时候容易碰到旁边的水果,所以摆放的比较远。
水果分拣演示视频请在附件中下载