魚眼カメラに基づいたロボットアームの掴みプロセス

1. カメラのキャリブレーション

カメラのキャリブレーションは、ROS の Camera_calibration ツールを使用して実行されます。このツールは魚眼カメラもキャリブレーションできます。キャリブレーションプレートの白黒グリッドサイズは12x8、1つの正方形のサイズは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

その後、ロボットアームが目標位置に移動するように制御できます。