六自由度JAKA机器人基于视觉的多颜色方块抓取（色块颜色识别、坐标提取、旋转角度提取）

青取之于蓝但没他蓝

已于 2024-01-03 19:15:18 修改

阅读量2.8k

点赞数 23

文章标签：机器人 opencv c++ 计算机视觉

于 2024-01-03 19:14:28 首次发布

本文链接：https://blog.csdn.net/indigo_xx/article/details/135335377

版权

一、任务描述

基于JAKA ZU3机器人平台与固定位置的彩色相机（非垂直），完成不同颜色木块的识别与抓取。编写图像处理算法获取图像中方块的位姿和颜色，并用机器人抓取。

图1：实验主要对象

二、视觉处理方案（重点）

本次实验中，视觉处理主要分为两部分，即图像坐标系同机器人 XY 平面坐标系之间的单应矩阵计算及坐标转换；方块图像采集、处理，最终提取图像中所有方块的属性，包括位置、角度和颜色。

2.1 图像标定与单应性矩阵计算

单应性矩阵的计算原理同“文档矫正”类似，但本实验中要注意三维平面的映射关系。如下图2，定义世界坐标系的原点在Jaka机器人底座，经验证，Jaka坐标系与世界坐标系之间无旋转映射，仅有Z坐标中的差距，两坐标系之间的刚体变换关系如下：

图2：平面机器人视觉系统坐标关系

因此，仅需要求出图像像素坐标系与世界坐标系之间的单应性矩阵，就能得出识别到的物块点在Jaka坐标系下的值。如下图2所示，使用目标相机得到照片，分别取标定点1-4的坐标值作为图像中已知点，而后使用平板采集四个点对应的孔的X坐标与Y坐标，作为世界坐标系已知点的X坐标与Y坐标。注意，本篇中容易忽视高度带来的单应性矩阵误差，误差造成原因如下图3所示，解决方法为采集图像点与标定时均选择物块上表面的点。

图3：高度带来的单应性矩阵误差图

将两个点组对应的代入进去，利用OpenCV库中findHomography()函数即可求得单应性矩阵，代码如下图4所示。

图4：标定点图

求解单应性矩阵代码：

/*
*函数名称：calculate_Homo
*功能：计算单应性矩阵
*输入参数：要得到的单应性矩阵
*/
void calculate_Homo(Mat* Homo) {
	vector<Point2f> ori;//三个目标点世界坐标,z:100,单位mm，rx=180,ry=0,rz=0
	ori.push_back(Point(294.923, 48.434));
	ori.push_back(Point(418.584, 32.481));
	ori.push_back(Point(435.834, 155.045));
	ori.push_back(Point(310.961, 172.286));

	vector<Point2f> dst;//三个目标点图像坐标
	dst.push_back(Point(708, 22));
	dst.push_back(Point(446, 104));
	dst.push_back(Point(595, 377));
	dst.push_back(Point(908, 246));
	*Homo = findHomography(dst, ori);//求得单应性矩阵
}

单应性矩阵求得后，尝试给定一点进行验证。已知靶标图像中所有特征点的坐标 $P_{M}$ ；相应的特征点在机器人坐标系中的点 $P_{B}$ ，上述算法得到的单应性矩阵H，其三者满足 $P_{B}=H*P_{M}$ 。因此，撰写代码对下面要求得的坐标进行求解，代码如下所示。

坐标变换函数

/*
*函数名称：turn_coordinate
*功能：将识别得到的图像坐标转换为机械臂的x，y，z世界坐标
*输入参数1：识别到的点在图像坐标系下的坐标点集（vector）<Point2f>*
*输入参数2：识别到的点在世界坐标系下的坐标点集（vector）<Point2f>*
*输入参数3：单应性矩阵（Mat）
*/
void turn_coordinate(vector<Point2f>* obj_image, Mat* Homo, vector<Point2f> &world_image) {
	short number = 5;
	Mat temp;
	for (int i = 0; i < number; i++) {
		temp = (Mat_<double>(3, 1) << (*obj_image)[i].x, (*obj_image)[i].y, 1);//图像坐标点坐标强行转换成1*3
		temp = *Homo * temp;
		temp = temp / temp.at<double>(2, 0);//得到世界坐标系值
		cout << "obj_world[" << i  << "]=" << temp<< endl;//输出监控
		world_image.push_back(Point(temp.at<double>(0, 0), temp.at<double>(1, 0)));//推入world坐标系，供后续使用
	}
	cout << "*********************************************" << endl;
}