1? 問題概述

? ? ? ? 本例程演示了在Eye-in-Hand的情況下，當標定板與機器人基準位置保持不變應該如何進行手眼標定。機器人通過標定板來進行手眼標定。在這樣的情況下，手眼標定的目的是確定兩個未知量：

? ? ? ? 1）在標定板坐標系下機器人基座的位姿（CalObjInBasePose）

? ? ? ? 2）在機器人末端工具中心點坐標系下相機的位姿（ToolInCamPose）

? ? ? ? 從理論上來說，至少需要獲取在相機坐標系下的標定物的三個位姿。但實際上，卻至少需要使用十個位姿。相應的，在機器人基座坐標系下的機械手夾具的位姿（ToolInBasePose）對于不同的用于標定的圖像而言是變化的，原因是因為它是通過移動相機來描述機器人的位姿。標定板的位姿通過固定在機械手末端夾具上的相機拍攝相應的圖像來獲得。為了獲取更好的標定結果，調整相機與標定板的位置以使標定板在圖像中呈傾斜的情況是很重要的。相機標定完成后，估計出的轉換矩陣將被提取出來，并用于計算在相機坐標系下標定板的位姿。

2? 例程解讀

2.1? 初始化

? ? ? ? 在這一部分的工作中，需要完成對顯示參數的初始化（主要包括2D圖像的顯示以及3D模型的顯示），以及用于標定的模型的初始化（利用已獲得的相機內參與標定板參數，同時還可以指定進行標定的方法）

2.1.1? 生成圖像與數據目錄

? ? ? ? 針對輸入的圖像進行相應的操作，目的是為了服務2D圖像的顯示。

* Directories with calibration images and data files ImageNameStart := '3d_machine_vision/hand_eye/movingcam_calib3cm_' //輸入圖像的名稱 DataNameStart := 'hand_eye/movingcam_' //輸入數據的名稱 NumImages := 14 //輸入圖像的數量 read_image (Image, ImageNameStart + '00') //根據名稱讀取圖像 dev_close_window () //關閉圖像窗口 get_image_size (Image, Width, Height) //獲取圖像的大小，單位是像素 dev_open_window (0, 0, Width, Height, 'black', WindowHandle) //根據圖像的參數打開相應的窗口 dev_set_line_width (2) //設置圖像線寬 dev_set_draw ('margin') //設置圖像繪制的方式：邊界 dev_display (Image) //顯示圖像 set_display_font (WindowHandle, 14, 'mono', 'true', 'false') //設置顯示的一些參數（這是一個外部函數） ParamName := ['color_0','color_1','color_2','color_3','color_4','color_5','color_6','alpha_6']//創建一個字符串數組作為參數名稱 ParamValue := ['red','green','blue','red','green','blue','white',0.7] //為這些參數給定相應的值

2.1.2? 初始化三維坐標系

? ? ? ? 這部分主要是為了初始化相應的三維坐標系：一個是以機械手夾具中心點為原點的三維坐標系，另一個是機器人基座的三維坐標系。

* Labels for the visualized 3D object models. tuple_gen_const (7, '', Labels) //創建一個元數組用于保存Labels Labels[0] := 'Robot\'s Tool' //聲明相應的名稱 Labels[3] := 'Robot\'s Base' Instructions[0] := 'Rotate: Left button' //重新聲明Instruction Instructions[1] := 'Zoom: Shift + left button' Instructions[2] := 'Move: Ctrl + left button' * Set size for 3D visualization in [m] ArrowThickness := 0.005 //初始化箭頭線條 ArrowLength := 0.05 //初始化箭頭長度 gen_robot_tool_and_base_object_model_3d (ArrowThickness, ArrowLength, OM3DToolOrigin, OM3DBase) //創建機械手與機器人的三維模型

2.1.3? 初始化標定模型

? ? ? ? 這里做的工作主要是初始化一個用于標定的模型CalibDataID。需要通過標定文件，相機初始內參以及用于進行標定的方法進行初始化。

* Load the calibration plate description file. * Make sure that the file is in the current directory or * in HALCONROOT/calib, or use an absolute path. CalTabFile := 'caltab_30mm.descr' //加載標定文件 * Read the initial values for the internal camera parameters read_cam_par (DataNameStart + 'start_campar.dat', StartCamParam) //獲取相機初始內參 * Create the calibration model for the hand eye calibration * where the calibration object is observed with a camera create_calib_data ('hand_eye_moving_cam', 1, 1, CalibDataID) //創建一個標定數據對象CalibDataID * Set the camera type used set_calib_data_cam_param (CalibDataID, 0, [], StartCamParam) //設置用于標定的相機參數 * Set the calibration object set_calib_data_calib_object (CalibDataID, 0, CalTabFile) //設置用于標定的標定板參數 * Start the loop over the calibration images * Set the optimization method to be used set_calib_data (CalibDataID, 'model', 'general', 'optimization_method', 'nonlinear')//設置標定所使用的方法 disp_message (WindowHandle, 'The calibration data model was created', 'window', 12, 12, 'black', 'true') disp_continue_message (WindowHandle, 'black', 'true')

2.2? 構建坐標系

2.2.1? 構建標定板坐標系

? ? ? ? 根據之前創建的標定對象CalibDataID在圖像中尋找相應的標定板圖像，獲取標定板的輪廓與其中的角點，并在其中心根據相機內參以及標定板位姿初始化一個標定板坐標系。

dev_open_window (0, Width + 10, Width, Height, 'black', WindowHandleR) *利用窗口句柄新建一個新窗口 set_display_font (WindowHandleR, 14, 'mono', 'true', 'false') *初始化這個新窗口 for I := 0 to NumImages - 1 by 1 *遍歷輸入的圖像dev_set_window (WindowHandle) *設置當前的窗口dev_clear_window () *清空當前窗口內容read_image (Image, ImageNameStart + I$'02d')dev_display (Image) *讀取并顯示相應的圖片* Search for the calibration plate, extract the marks and the* pose of it, and store the results in the calibration data* The poses are stored in the calibration data model for use by* the hand eye calibration and do not have to be set explicitlyfind_calib_object (Image, CalibDataID, 0, 0, I, [], []) *在輸入的圖像中根據之前初始化的標定模型，尋找標定板get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, I) *提取標定板的輪廓get_calib_data_observ_points (CalibDataID, 0, 0, I, RCoord, CCoord, Index, PoseForCalibrationPlate) *通過提取標定板中的點，來獲取標定板的位姿* Visualize the extracted calibration marks and the estimated pose (coordinate system)dev_set_color ('green') *用綠色顯示標定板輪廓dev_display (Image)dev_display (Caltab)dev_set_color ('yellow') *用黃色顯示角點顏色disp_cross (WindowHandle, RCoord, CCoord, 6, 0) *顯示角點（通過計算行列交點）dev_set_colored (3)disp_3d_coord_system (WindowHandle, StartCamParam, PoseForCalibrationPlate, 0.01)*根據相機的內參和標定板的位姿顯示三維坐標系disp_message (WindowHandle, 'Extracting data from calibration image ' + (I + 1) + ' of ' + NumImages, 'window', 12, 12, 'black', 'true')

2.2.2? 構建機器人基座坐標系與夾具坐標系

? ? ? ? 根據保存的機器人工具坐標系到機器人基座坐標系下的轉換矩陣，將原有的機器人夾具坐標系轉換至基座坐標系下，顯示這兩個坐標系并將轉換關系保存在CalibDataID所指向的模型對象中。

* Read pose of tool in robot base coordinates (ToolInBasePose) *讀取機械手在機器人基座坐標系下的位姿read_pose (DataNameStart + 'robot_pose_' + I$'02d' + '.dat', ToolInBasePose)if (I == 0)PoseIn := [-0.006,-0.296,12,178,2,270,0]elsePoseIn := PoseOutendifrigid_trans_object_model_3d (OM3DToolOrigin, ToolInBasePose, OM3DTool) *根據機械手在機器人基座坐標系下的轉換矩陣，將機械手坐標系轉換至基座坐標系下visualize_object_model_3d (WindowHandleR, [OM3DTool,OM3DBase], [], PoseIn, ParamName, ParamValue, 'Position of robot tool coordinate system in robot base coordinate system', Labels, Instructions, PoseOut) *模型可視化clear_object_model_3d (OM3DTool) *清除機械手坐標系* Set the pose tool in robot base coordinates in the calibration data modelset_calib_data (CalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose) *保存機械手坐標系到基座坐標系的轉換關系至標定模型CalibDataID中 endfor dev_set_window (WindowHandleR) *關閉三維視窗 dev_close_window () disp_message (WindowHandle, 'All relevant data has been set in the calibration data model', 'window', 12, 12, 'black', 'true') disp_continue_message (WindowHandle, 'black', 'true') stop ()

2.3? 執行手眼標定

2.3.1? 檢查用于手眼標定的位姿是否一致

* Check the input poses for consistency check_hand_eye_calibration_input_poses (CalibDataID, 0.05, 0.005, Warnings) *檢查用于進行標定的位姿矩陣是否具有一致性 if (|Warnings| != 0)* There were problem detected in the input poses. Inspect Warnings and* remove erroneous poses with remove_calib_data and remove_calib_data_observ.dev_inspect_ctrl (Warnings)stop () endif *如果檢查到位姿矩陣不具備一致性則清空相應位姿

2.3.2? 進行手眼標定并保存相應參數

? ? ? ? 下面開始進行手眼標定，相機的標定已經在手眼標定之前完成。

dev_display (Image) disp_message (WindowHandle, 'Performing the hand-eye calibration', 'window', 12, 12, 'black', 'true') calibrate_hand_eye (CalibDataID, Errors) *進行手眼標定 * Query the error of the camera calibration *獲取相機標定的誤差 get_calib_data (CalibDataID, 'model', 'general', 'camera_calib_error', CamCalibError) * Query the camera parameters and the poses *獲取相機的參數 get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam) * Get poses computed by the hand eye calibration *獲取機械手末端夾具在相機坐標系下的坐標 get_calib_data (CalibDataID, 'camera', 0, 'tool_in_cam_pose', ToolInCamPose) get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_base_pose', CalObjInBasePose)*計算標定板在基座坐標系下的坐標 * Get the plane in base coordinate system pose by translating the * CalObjInBasePose by the calibration object's thickness in the * z-direction. set_origin_pose (CalObjInBasePose, 0, 0, 0.005, PlaneInBasePose) *將標定板根據其厚度矯正至與base同平面 dev_get_preferences ('suppress_handled_exceptions_dlg', PreferenceValue) dev_set_preferences ('suppress_handled_exceptions_dlg', 'true') *獲取并保存相關參數 try* Handle situation where user does not have the permission* to write in the current directory.* * Store the camera parameters to file *保存相關參數write_cam_par (CamParam, DataNameStart + 'final_campar.dat')* Save the hand eye calibration results to filewrite_pose (ToolInCamPose, DataNameStart + 'final_pose_cam_tool.dat')write_pose (CalObjInBasePose, DataNameStart + 'final_pose_base_calplate.dat')write_pose (PlaneInBasePose, DataNameStart + 'final_pose_base_plane.dat') catch (Exception)* do nothing endtry dev_set_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)

2.3.3? 顯示標定誤差

dev_display (Image) * Display calibration errors disp_results (WindowHandle, CamCalibError, Errors) *顯示手眼標定后的誤差與相機誤差 disp_continue_message (WindowHandle, 'black', 'true') stop ()

2.4? 顯示標定后的三維模型

2.4.1? 計算姿態指標和標定對象指標

* For the given camera, get the corresponding pose indices and calibration object indices query_calib_data_observ_indices (CalibDataID, 'camera', 0, CalibObjIdx, PoseIds)

2.4.2? 可視化

* Compute the pose of the calibration object in the camera coordinate * system via calibrated poses and the ToolInBasePose and visualize it. * Set sizes for 3D visualization in [m] CameraSize := 0.05 *這兩個參數是用來初始化相機的 CameraConeLength := 0.3 get_calib_data (CalibDataID, 'calib_obj', 0, 'x', PX) *獲取標定塊的坐標數據 get_calib_data (CalibDataID, 'calib_obj', 0, 'y', PY) get_calib_data (CalibDataID, 'calib_obj', 0, 'z', PZ) gen_object_model_3d_from_points (PX, PY, PZ, OM3DObjectOrig) *根據坐標數據初始化標定塊 rigid_trans_object_model_3d (OM3DObjectOrig, CalObjInBasePose, OM3DObject) *根據標定塊在基座坐標系下的轉換矩陣對標定塊進行轉換 clear_object_model_3d (OM3DObjectOrig) dev_open_window (0, Width + 10, Width, Height, 'black', WindowHandleR) *新建一個窗口用于進行三維顯示 set_display_font (WindowHandleR, 14, 'mono', 'true', 'false') ParamName := ['color_0','color_1','color_2','color_3','color_4','color_5','color_6','color_7','alpha_7','color_8','color_9','color_10','alpha_8','alpha_9','alpha_10','point_size'] ParamValue := ['red','red','green','blue','red','green','blue','white',0.7,'magenta','yellow','white',0.5,0.5,0.5,5] *初始化參數名稱與數值 * Labels for the visualized 3D object models. tuple_gen_const (11, '', Labels) *初始化三維坐標系的坐標軸 Labels[0] := 'Calibration Object' Labels[1] := 'Robot\'s Tool' Labels[4] := 'Robot\'s Base' Labels[8] := 'Camera' for I := 0 to NumImages - 1 by 1dev_set_window (WindowHandle)dev_clear_window ()read_image (Image, ImageNameStart + I$'02d')dev_display (Image) *讀取相應的圖片* Obtain the pose of the tool in robot base coordinates used in the calibration.* The index corresponds to the index of the pose of the observation object.get_calib_data (CalibDataID, 'tool', PoseIds[I], 'tool_in_base_pose', ToolInBasePose)* Compute the pose of the calibration object relative to the cameracalc_calplate_pose_movingcam (CalObjInBasePose, ToolInCamPose, ToolInBasePose, CalObjInCamPose)*通過相機參數計算標定塊的位姿* Display the coordinate systemdev_set_colored (3)disp_3d_coord_system (WindowHandle, CamParam, CalObjInCamPose, 0.01)Message := 'Using the calibration results to display 'Message[1] := 'the coordinate system in image ' + (I + 1) + ' of ' + NumImagesdisp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')gen_camera_and_tool_moving_cam_object_model_3d (ToolInCamPose, ToolInBasePose, CameraSize, CameraConeLength, OM3DToolOrigin, CamParam, OM3DCamera, OM3DTool)if (I == 0)PoseIn := [-0.006,-0.296,12,178,2,270,0]elsePoseIn := PoseOutendifvisualize_object_model_3d (WindowHandleR, [OM3DObject,OM3DTool,OM3DBase,OM3DCamera], [], PoseIn, ParamName, ParamValue, [], Labels, Instructions, PoseOut)clear_object_model_3d (OM3DTool)clear_object_model_3d (OM3DCamera) endfor

2.5? 清除模型并計算相應矩陣

clear_calib_data (CalibDataID) clear_object_model_3d (OM3DObject) clear_object_model_3d (OM3DToolOrigin) clear_object_model_3d (OM3DBase) dev_set_window (WindowHandleR) dev_close_window () * * After the hand-eye calibration the computed pose * ToolInCamPose can be used in robotic grasping applications. * To grasp an object with the robot, typically, its pose * with respect to the camera is determined (which * is simulated here by setting the object's pose to the * pose of the calibration object) ObjInCamPose := CalObjInCamPose * If the tool coordinate system is placed at the gripper * and a detected object ObjInCamPose shall be grasped * (here the calibration object), * the pose of the detected object relative * to the robot base coordinate system has to be computed. pose_invert (ToolInCamPose, CamInToolPose) pose_compose (ToolInBasePose, CamInToolPose, CamInBasePose) pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)

3? 思路整理

? ? ? ? 本Halcon例程的目的是完成 Eye-in-Hand 條件下的手眼標定。以解決??問題來完成手眼標定的角度來看，我們只需要獲得機械手夾具到機器人基座坐標系的轉換關系 ?以及相機標定的參數??即可解決這一手眼標定問題。

? ? ? ? 在本例程中，已經獲得了tool到base的轉換關系以及cam的參數。因此，還需要進行的工作就是用??完成攝像機標定，構建相應的三維坐標系并評估評定的結果。

? ? ? ? 例程的整體流程梳理如下：

? ? ? ? 1）初始化相應的參數：二維圖像輸出窗口，三維坐標系的坐標軸與窗口，用于進行手眼標定的對象CalibDataID；

? ? ? ? 2）根據手眼標定的對象，在輸入的圖像中尋找相應的標定板，并建立坐標系；

? ? ? ? 3）創建base與tool的三維坐標系，并根據輸入的轉換關系信息進行顯示；

? ? ? ? 4）進行手眼標定，輸出并保存相關參數；

? ? ? ? 5）根據標定后的轉換關系，創建base、tool、cam與obj的三維坐標系，并進行顯示；

? ? ? ? 6）輸出相應的轉換關系。

總結

以上是生活随笔為你收集整理的标定学习笔记（五）-- Halcon手眼标定例程：Hand-eye-Calibration with a moving cam的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。