基于鱼眼相机的机械臂抓取流程

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))#平移向量

相似坐标转世界坐标参考https://www.yii666.com/blog/331694.html

之后可通过控制机械臂移动到目标位置