如何将像素坐标转化为机械臂基座坐标_高斯机械臂视觉抓取说明

功能

本模块实现相机参数标定、机械臂手眼标定、彩色方块的视觉抓取功能

节点

1. vision_calibration

功能：实现相机内参标定，以及相机与机械臂的手眼变换矩阵标定

2. color_detect_location

功能：检测图像中的色块

3. vision_grasp

功能：实现视觉抓取

原理

1.相机参数标定

相机参数标定的目的是确定相机的内参和畸变参数。用的是张正友棋盘格标定法，参考方法为以下论文：

• Z. Zhang. A Flexible New Technique for Camera Calibration. IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000.
• J.Y.Bouguet. MATLAB calibration tool. http://www.vision.caltech.edu/bouguetj/calib_doc/

实现参考：https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html

相机成像原理如下图所示，展示了世界坐标系中的点P=(X,Y,Z)如何通过相机坐标系Fc映射到像素坐标系(u,v)。

公式为：

简写为：

其中：

• (X,Y,Z)为点在世界坐标系中的三维坐标。
• (u,v)为像素坐标系中的坐标。
• A为相机的内参矩阵，包含了焦距和光心坐标。
• (cx,cy)为光心坐标，一般位于图片中心点。
• fx,fy为焦距，以像素为单位
• R和t分别为世界坐标系到相机坐标系的旋转和平移

具体实现采用OpenCV的相机标定模块，主要用了其中的cv::calibrateCamera函数。
输入为N组不同角度拍摄到的棋盘角点坐标、每组对应的实际角点坐标，以及图像尺寸。
输出则为求得的相机内参矩阵、畸变系数、外参矩阵。

2.机械臂手眼标定

机械臂手眼标定用来计算机械臂末端到相机的坐标变换，此处采用的的是ROS中的visp_hand2eye_calibration包，地址为：http://wiki.ros.org/visp_hand2eye_calibration

相机安装在机械臂末端，如下所示：

上一节相机参数标定解决的是上图中右侧的问题，也就是目标到相机的变换。
而机械臂手眼标定则解决的是左侧的问题，也就是相机到机械臂末端的变换。

visp_hand2eye_calibration包中的visp_hand2eye_calibration_calibrator节点就提供了计算手眼标定矩阵的服务。

服务名称：compute_effector_camera_quick

• 客户端：
  • visp_hand2eye_calibration/TransformArray camera_object
  • visp_hand2eye_calibration/TransformArray world_effector

• 服务端：
  • geometry_msgs/Transform effector_camera

其中，客户端发送两部分内容：

• 一组camera_object：即相机到目标的坐标变换（也就是相机参数标定时求得的相机外参）
• 一组对应的world_effector：即拍摄图片时机械臂末端的坐标（可通过机械臂状态接口读取）

调用服务后，会返回手眼标定的结果，即相机到机械臂末端的坐标变换。

手眼标定需要依赖相机参数标定时计算出的每个图片对应的相机外参。所以，进行相机标定及手眼标定的总体步骤为：

• 第一步：采集15-20个图片，并记录每个图片采集时对应的机械臂末端姿态。
• 第二步：先利用采集的图片进行相机参数标定，相机参数标定除了会计算出相机的内参矩阵及畸变系数外，还会计算出每个图片对应的相机外参参数。
• 第三步：根据标定结果中的每个图片对应的相机外参参数，结合每个图片对应的机械臂末端姿态，再进行机械臂手眼标定。

3.彩色方块的视觉抓取

相机参数和手眼参数标定完成后，就可以进行基于视觉的抓取了。其步骤为：

• 首先将机械臂末端移动到拍照点（拍照高度需要已知，也就是物体到相机光心的距离），并保证相机垂直向下。
• 相机拍摄桌面上的目标，识别其中彩色方块的位置。
• 根