第八章視覺抓取 · 高斯機械臂開發文檔

## 功能本模塊實現相機參數標定、機械臂手眼標定、彩色方塊的視覺抓取功能 ## 節點 ### 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)。 ![avatar](https://box.kancloud.cn/e1b8da6d57e8bff1d602833f37b4736f_600x423.png) 公式為： ![avatar](https://box.kancloud.cn/29dc5978d3a26c0afee745e963d1165c_358x89.png) 簡寫為： ![avatar](https://box.kancloud.cn/364209fef1a2acfea122a41f6a61530e_132x19.png) 其中： - (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 相機安裝在機械臂末端，如下所示： ![avatar](https://box.kancloud.cn/b064780cb972719d6effbdcef98f0546_493x346.png) 上一節相機參數標定解決的是上圖中右側的問題，也就是目標到相機的變換。而機械臂手眼標定則解決的是左側的問題，也就是相機到機械臂末端的變換。 visp_hand2eye_calibration包中的visp_hand2eye_calibration_calibrator節點就提供了計算手眼標定矩陣的服務。服務名稱：[compute_effector_camera_quick](https://github.com/lagadic/vision_visp/blob/master/visp_hand2eye_calibration/srv/compute_effector_camera_quick.srv) - 客戶端： + visp_hand2eye_calibration/TransformArray camera_object + visp_hand2eye_calibration/TransformArray world_effector - 服務端： + geometry_msgs/Transform effector_camera 其中，客戶端發送兩部分內容： - 一組camera_object：即相機到目標的坐標變換（也就是相機參數標定時求得的相機外參） - 一組對應的world_effector：即拍攝圖片時機械臂末端的坐標（可通過機械臂狀態接口讀取）調用服務后，會返回手眼標定的結果，即相機到機械臂末端的坐標變換。手眼標定需要依賴相機參數標定時計算出的每個圖片對應的相機外參。所以，進行相機標定及手眼標定的總體步驟為： - 第一步：采集15-20個圖片，并記錄每個圖片采集時對應的機械臂末端姿態。 - 第二步：先利用采集的圖片進行相機參數標定，相機參數標定除了會計算出相機的內參矩陣及畸變系數外，還會計算出每個圖片對應的相機外參參數。 - 第三步：根據標定結果中的每個圖片對應的相機外參參數，結合每個圖片對應的機械臂末端姿態，再進行機械臂手眼標定。 ### 3.彩色方塊的視覺抓取相機參數和手眼參數標定完成后，就可以進行基于視覺的抓取了。其步驟為： - 首先將機械臂末端移動到拍照點（拍照高度需要已知，也就是物體到相機光心的距離），并保證相機垂直向下。 - 相機拍攝桌面上的目標，識別其中彩色方塊的位置。 - 根據相機內參及拍照高度，將彩色方塊中心點的像素坐標轉換為相機坐標系下的坐標。 - 根據手眼矩陣，將相機坐標系下的坐標轉換為機械臂坐標系下的坐標。 - 控制機械臂末端移動到機械臂坐標系下的坐標，抓取方塊。在`launch/vision_grasp.launch`文件中，配置了抓取時用到的參數。 - `<param name="cali_file" type="string" value="$(find gauss_vision)/data/cfg/"/>`。此參數指定了抓取所用到的標定參數文件所在目錄，包括相機內參和手眼矩陣。 - `<param name="object_hight" type="double" value="0.095"/>`。抓取對象的高度，調整此參數可控制抓取時機械臂末端距離桌面的高度。如果末端吸盤接觸不到方塊，則調小此參數。如果末端吸盤接觸方塊后下壓太多，則調大此參數。 - `<param name="camera_to_object_distance" type="double" value="0.20"/>`。相機光心到物體的距離,即Zc，用來計算物體從像素坐標系到相機坐標系的坐標變換。 - `<param name="handeye_param_x" type="double" value="0.054"/>`等。這一系列參數設置了手眼矩陣，包括位置(x,y,z)和角度(roll,pitch,yaw)。 - `<param name="robot_capture_image_pose_x" type="double" value="0.2"/>`等。這一系列參數設置了相機拍照點位姿。 - `<param name="robot_safe_pose_1_x" type="double" value="0.3"/>`等。這一系列參數設置了機械臂抓取所要經過的點。 - `<param name="robot_place_pose_x" type="double" value="0.17"/>`等。這一系列參數設置了機械臂抓取方塊后，最終放置點。 ## 啟動 Gauss機械臂啟動后，會自動啟動基礎控制模塊。如果是視覺版機械臂，則開機也會自動啟動視覺模塊。不同的啟動項通過配置`~/Projects/gauss_ws/src/gauss/gauss_bringup/config/rpi_ros_processes.yaml` 文件進行修改。開機時系統會自動執行`gauss-ros-start`指令，此指令會啟動上述文件中配置的所有模塊。 ### 對于非視覺版機械臂非是絕版機械臂只啟動基礎模塊，其配置文件內容為： `~/Projects/gauss_ws/src/gauss/gauss_bringup/config/rpi_ros_processes.yaml` ``` processes: - name: 'controllers' launch_on_startup: true delay_before_start: 0.0 cmd: 'roslaunch gauss_bringup controllers.launch' args: [] dependencies: [] - name: 'robot_commander' launch_on_startup: true delay_before_start: 4.0 # Additional delay for Moveit to load controllers cmd: 'roslaunch gauss_bringup robot_commander.launch' # cmd: 'roslaunch gauss_bringup robot_commander_pilz.launch' args: [] dependencies: - controllers - name: 'user_interface' launch_on_startup: true delay_before_start: 0.0 cmd: 'roslaunch gauss_bringup user_interface.launch' args: [] dependencies: - controllers - robot_commander ``` 此時，如果想啟動視覺模塊，需要在Gauss基礎功能啟動后，打開新的客戶端，執行以下指令啟動視覺模塊： ``` $ roslaunch gauss_vision gauss_vision.launch ``` ### 對于視覺版機械臂視覺版機械臂的 `~/Projects/gauss_ws/src/gauss/gauss_bringup/config/rpi_ros_processes.yaml` 文件最后加入了視覺模塊啟動配置。即： ``` - name: 'gauss_vision' launch_on_startup: true delay_before_start: 10.0 cmd: 'roslaunch gauss_vision gauss_vision.launch' args: [] dependencies: - controllers - robot_commander - user_interface ``` 加入此配置后，執行gauss-ros-start在啟動基礎模塊的基礎上，還會啟動 `roslaunch gauss_vision gauss_vision.launch`。（gauss_vision包位于`~/Projects/gauss_ws/src/`文件夾下。）加入視覺模塊的完整啟動文件內容如下所示： `~/Projects/gauss_ws/src/gauss/gauss_bringup/config/rpi_ros_processes.yaml` ``` processes: - name: 'controllers' launch_on_startup: true delay_before_start: 0.0 cmd: 'roslaunch gauss_bringup controllers.launch' args: [] dependencies: [] - name: 'robot_commander' launch_on_startup: true delay_before_start: 4.0 # Additional delay for Moveit to load controllers cmd: 'roslaunch gauss_bringup robot_commander.launch' # cmd: 'roslaunch gauss_bringup robot_commander_pilz.launch' args: [] dependencies: - controllers - name: 'user_interface' launch_on_startup: true delay_before_start: 0.0 cmd: 'roslaunch gauss_bringup user_interface.launch' args: [] dependencies: - controllers - robot_commander - name: 'gauss_vision' launch_on_startup: true delay_before_start: 10.0 cmd: 'roslaunch gauss_vision gauss_vision.launch' args: [] dependencies: - controllers - robot_commander - user_interface ``` ## 標定功能Service列表標定功能所提供的Service列表如下： #### (1) gauss/vision/calib/start_stop 功能：啟動/關閉標定模塊客戶端： * std_msgs/Bool cmd: True表示啟動, False表示關閉服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (2) gauss/vision/calib/get_camera_status 功能：獲取相機工作狀態客戶端： * 空服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (3) gauss/vision/calib/create_flow 功能：創建一個標定流程客戶端： * std_msgs/String flow_name * std_msgs/String remark 服務端： * std_msgs/Int8 flow_id:指定標定流程id * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (4) gauss/vision/calib/get_image_and_pose 功能：獲取當前圖像及末端位姿客戶端： * 空服務端： * v_c_image_flow bin_image: 圖像數據 * geometry_msgs/Transform pose:末端位姿 * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (5) gauss/vision/calib/save_image_and_pose 功能：檢驗并保存當前圖像客戶端： * std_msgs/UInt8 flow_id: 指定流程id 服務端： * v_c_image_flow bin_image: 檢驗后的圖像 * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (6) gauss/vision/calib/calc_calib_param 功能：計算相機標定與手眼標定參數客戶端： * std_msgs/UInt8 flow_id: 指定流程id 服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描信息 #### (7) gauss/vision/calib/get_calib_param 功能：獲取指定標定流程的標定參數客戶端： * std_msgs/UInt8 flow_id: 指定標定流程id 服務端： * geometry_msgs/Vector3 camera_param1 相機內參 * geometry_msgs/Vector3 camera_param2 相機內參 * geometry_msgs/Vector3 camera_param3 相機內參 * std_msgs/Float64 k1, k2, p1, p2, k3: 畸變系數 * geometry_msgs/Transform pose: 手眼變換矩陣 * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (8) gauss/vision/calib/get_flow_image_id_list 功能：獲取指定流程中存在的圖像id列表客戶端： * std_msgs/UInt8 flow_id: 指定標定流程id 服務端： * string[] flow_image_id_list: 該流程圖像id數組 * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (9) gauss/vision/calib/get_flow_image_and_pose 功能：獲取指定標定流程的某一圖像及末端位姿客戶端： * std_msgs/UInt8 flow_id: 指定標定流程id * std_msgs/UInt8 image_id: 指定圖像id 服務端： * v_c_image_flow bin_image: 圖像數據 * geometry_msgs/Transform pose: 機械臂在該圖像下的位姿 * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (10) gauss/vision/calib/get_local_flow_id_list 功能：獲取本地已存在的標定流程id列表客戶端： * 空服務端： * v_c_flow_info[] local_flow_id_array: 本地的標定流程信息數組 * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (11) gauss/vision/calib/set_calib_param 功能：配置指定流程的標定參數作為最終應用客戶端： * std_msgs/UInt8 flow_id: 指定流程id 服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (12) gauss/vision/calib/delete_flow_id 功能：刪除本地指定的標定流程客戶端： * std_msgs/UInt8 flow_id: 指定流程id(不能刪除默認的0號) 服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (13) gauss/vision/calib/delete_flow_image 功能：刪除本地指定的標定流程中的某圖像客戶端： * std_msgs/UInt8 flow_id: 指定流程id(不能刪除默認的0號) * std_msgs/UInt8 image_id: 指定圖像id 服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (14) gauss/vision/calib/get_current_bin_image 功能：獲取當前的二進制圖像客戶端： * 空服務端： * v_c_image_flow bin_image: 圖像數據 * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (15) gauss/vision/calib/get_flow_image_list 功能：獲取指定流程中存在的圖像客戶端： * std_msgs/UInt8 flow_id: 指定流程id 服務端： * v_c_image_info[] image_info_array: 圖像信息數組(string id; v_c_image_flow bin_image) * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (16) gauss/vision/calib/edit_flow_info 功能：編輯流程信息客戶端： * std_msgs/String flow_id: 指定流程id * std_msgs/String flow_name:流程名 * std_msgs/String remark:流程注釋服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (17) gauss/vision/calib/get_local_flow_info 功能：獲取本地指定流程的信息客戶端： * std_msgs/UInt8 flow_id: 指定流程id 服務端： * v_c_flow_info local_flow_info: 本地的標定流程信息 * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 ### 視覺抓取功能Service列表視覺抓取功能所提供的Service列表如下： #### (1) gauss/vision/grasp/init_task 功能：任務初始化，包括各驅動、算法模塊客戶端： * 空服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (2) gauss/vision/grasp/get_camera_status 功能：獲取相機工作狀態客戶端： * 空服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (3) gauss/vision/grasp/get_tool_status 功能：獲取末端工具狀態客戶端： * 空服務端： * std_msgs/Int8 tool_id: 工具id * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (4) gauss/vision/grasp/get_task_status 功能：獲取任務狀態客戶端： * 空服務端： * std_msgs/String task_status - 1.“StatusWaitConfig”:等待配置 - 2.“StatusWaitInit”:等待初始化 - 3.“StatusIdle”:空閑態 - 4.“StatusRunning”:運行態 - 5.“StatusSuspend“:暫停態 - 6.“StatusStop“:停止態 - 7.“StatusErr“:錯誤態 #### (5) gauss/vision/grasp/start_task（注意：此處為Action，并非Service）功能：開始執行任務客戶端： * gauss_vision/v_g_start_taskAction 服務端： * [實時反饋] + std_msgs/Int8 task_phase:任務執行階段，取值為： - 1“moving_to_detect_pos”:移動至拍照點 - 2“detecting_cube”:檢測色塊 - 3“moving_to_cube”:移動至色塊抓取位置 - 4“picking_cube”:拾取色塊 - 5“safe pose 1“:移動至安全位置1 - 6“safe pose 2“:移動至安全位置2 - 7“moving_to_container”:移動至放置區 - 8“placing_cube”:放置色塊 - 9“moving_to_origin”:移動至原點 * [最終執行結果] - std_msgs/Int8 ack_nak: 錯誤碼,0表示成功,非0為失敗 - std_msgs/String status_message: 失敗描述信息 #### (6) gauss/vision/grasp/pause_task 功能：暫停任務客戶端： * 空服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (7) gauss/vision/grasp/continue_task 功能：恢復任務客戶端： * 空服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (8) gauss/vision/grasp/stop_task 功能：停止任務并歸位移動至原點（如果已拾取色塊，需將色塊放置到圓盤后再歸位）客戶端： * 空服務端： * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息 #### (9) gauss/vision/grasp/get_cube_detection_result 功能：獲取色塊檢測結果客戶端： * 空服務端： * v_c_image_flow bin_image:色塊圖像 * std_msgs/Int8 num:檢測到的色塊個數 * geometry_msgs/Vector3 pose:當前色塊坐標 * std_msgs/Int8 ack_nak: 錯誤碼,0表示ok,非0為錯誤 * std_msgs/String status_message: 錯誤描述信息