基于鱼眼相机的机械臂抓取流程-Toy模板网

这篇具有很好参考价值的文章主要介绍了基于鱼眼相机的机械臂抓取流程。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

1. 相机标定

相机标定使用ROS中camera_calibration工具进行标定，该工具也可以标定鱼眼相机。标定板黑白格大小为12x8，单个方块大小20mm
标定后即可得到相机内参。后续使用时需要通过参数矫正鱼眼相机的图片

rosrun camera_calibration cameracalibrator.py --size 11x8 --square 0.020 image:=/usb_cam/image_raw camera:=/usb_cam --fisheye-fix-skew --fisheye-recompute-extrinsicsts --fisheye-check-conditions

2. 工具标定

工具标定根据机械臂厂商提供的工具进行

3. 手眼标定

参考手眼标定。
注意事项：image_callback函数中获取的是未矫正的图像，应用时需要对图像进行矫正。
鱼眼相机矫正方法：
ros标定后，在.ros下camera_info下有head_camera.yaml参数文件

with open(camera_info_file) as f:
	camera_params = yaml.load(f.read(),Loader=yaml.FullLoader)
	camera_width = camera_params['image_width']
	camera_height = camera_params['image_height']
	self.image_size = (camera_width,camera_height)
	# self.K_ = np.asarray([camera_params['camera_matrix']['data']]).reshape((3, 3))
	self.D = np.asarray(camera_params['distortion_coefficients']['data']).reshape((1, 4))
	self.K=np.asarray([camera_params['camera_matrix']['data']]).reshape((3, 3))
	self.Re =np.asarray([camera_params['rectification_matrix']['data']],dtype=np.float64).reshape((3, 3)) 
	self.P =np.asarray([camera_params['projection_matrix']['data']]).reshape((3, 4))
	# 根据alpha比例因子计算新的相机内参，1：所有像素保留，但存在黑色边框。0：损失最多的像素，没有黑色边框.返回矩阵和ROI
	# self.K,_ = cv2.getOptimalNewCameraMatrix(self.K, self.D, self.image_size, 0, self.image_size)
	if camera_type==0:#鱼眼相机
	    self.P=cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(self.K,self.D,self.image_size,None)
	    self.map1,self.map2 = cv2.fisheye.initUndistortRectifyMap(self.K, self.D, None, self.P, self.image_size, cv2.CV_32FC1)

在image_callback中对原始图像进行矫正：

frame_result =cv2.remap(image, fisheye.map1, fisheye.map2, interpolation=cv2.INTER_LINEAR)

4. 像素坐标转世界坐标

像素坐标转世界坐标，需要知道相机内参和外参

#获取工具中心点的位置和姿态
 RT_end_to_base = np.column_stack((rvecs_robot[i],tvecs_robot[i]))
 RT_end_to_base=np.row_stack((RT_end_to_base,np.array([0,0,0,1])))

  RT_cam_to_end=np.column_stack((R,T))#手眼标定结果
  RT_cam_to_end=np.row_stack((RT_cam_to_end,np.array([0,0,0,1])))
  

  Rmat=Rotation.from_matrix(R)
  rx,ry,rz=Rmat.as_euler('xyz',degrees=True)
  # print("rotation(x,y,z)",rx,ry,rz)
  print("相机到机械臂末端的矩阵",T,rx,ry,rz)
  
  #获取相机在机械臂坐标系下的位置和姿态
  Matrix_camera2Base = np.dot(RT_end_to_base,RT_cam_to_end)
  Matrix_base2Cam=np.linalg.inv(Matrix_camera2Base)
  rvec_cam=cv2.Rodrigues(Matrix_base2Cam[:3,:3])[0]#旋转向量
  tvec_cam = Matrix_base2Cam[:3, 3].reshape((3, 1))#平移向量