7.2 halcon手眼标定实现与应用

在halcon手眼 标定中，主要分为相机固定和相机随机械手移动两种标定方式。
在固定 相机时，标定 板固定 在夹具上，机械手带着标定板进行一系列的移动，
在每个位置让相机对标定板拍照，同时记录下机械手的位置信息(信息由机械手系统提供)。
相机运动时，是标定板固定不动，机械手带着相机在不同位置拍照，同时记录拍照时机械手对应的位姿，
最后求得委屈什么坐标系与机械手基础坐标系关系。

当前帖子演示如何进行SCARA机器人的手眼标定。
相机相对机器人固定。
标定板固定于机器手工具末端上。
在代码最后，标定结果及数据会被保存到磁盘文件。
保存的数据可以在后续用于计算要抓取物体的位置。
保存的结果可以在halcon例程pick_and_place_scara_stationary_cam.hdev中使用。
文件位于C:/Users/Public/Documents/MVTec/HALCON-17.12-Progress/examples/hdevelop/Calibration/Hand-Eye
下图为标定结果

halcon手眼标定实现与应用

下面为具体代码与详细注释

1. 
   
2. CalibObjDescr := 'calibrate_hand_eye_scara_setup_01_calplate.cpd'
   
3. CalibrationPlateThickness := 0.003
   
4. CameraParam := ['area_scan_division',0.0165251, -642.277, 4.65521e-006, 4.65e-006, 595.817, 521.75, 1280, 1024]
   
5. *设置要抓取物体厚度，以实现确定抓取平面的位姿
   
6. ObjectThickness := 0.001
   
7. 
   
8. dev_close_window ()
   
9. dev_open_window_fit_size (0, 0, 1280, 1024, 640, -1, WindowHandle)
   
10. set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
    
11. dev_update_off ()
    
12. *
    
13. * 步骤1 读取标定板图像及机械手位姿。
    
14. *创建标定模型。模型类型为手眼相对固定形式。
    
15. *'hand_eye_scara_stationary_cam'为标定类型, 1, 1为标定时相机及标定板个数, CalibDataID为输出句柄。
    
16. create_calib_data ('hand_eye_scara_stationary_cam', 1, 1, CalibDataID)
    
17. * Set the camera parameters in the calibration model
    
18. *设置标定用相机的参数与类型。CalibDataID为模型句柄, 0为相机索引, []为相机类型，表示面扫,
    
19. *CameraParam为相机内参。
    
20. set_calib_data_cam_param (CalibDataID, 0, [], CameraParam)
    
21. *设置标定板的描述文件
    
22. set_calib_data_calib_object (CalibDataID, 0, CalibObjDescr)
    
23. for Index := 1 to 10 by 1
    
24.     * 读取标定板图像
    
25.     read_image (CalibImage, '3d_machine_vision/hand_eye/scara_stationary_cam_setup_01_calib_' + Index
    
26. 当前这个实现就是将相机固定在一个地方，机械手带动标定 板在不同的位置进行拍照，
    
27. 再进行标定求出摄像机坐标系与机械手基础坐标系转换关系，其实就是平移矢量和放置矩阵。
    
28. 核心就是要确定 摄像机和机械手基础 坐标系间的关系 ，机械手基础坐标系就是以机械手底座
    
29. 为原点建立的坐标系，摄像 机坐标系是以摄像 机中心为原点 建立 的坐标系，知道 它们间的关系
    
30. 后就可以计算出要抓取物体可基础 坐标系下的坐标，从而 可以进行准确抓取。
    
31. 
    
32. 
    
33. 02')
    
34.     *读取图片对应的机械手位姿（机械手基础坐标系下夹具的位姿）
    
35.     *这里的对应意思是，标定板固定于机械手夹具，相机对标定板拍照，此时机械手夹具位姿。
    
36.     *文件在位于C:/Users/Public/Documents/MVTec/HALCON-17.12-Progress/examples/hdevelop/Calibration/Hand-Eye
    
37.     read_pose ('scara_stationary_cam_setup_01_tool_in_base_pose_' + Index
    
38. 当前这个实现就是将相机固定在一个地方，机械手带动标定 板在不同的位置进行拍照，
    
39. 再进行标定求出摄像机坐标系与机械手基础坐标系转换关系，其实就是平移矢量和放置矩阵。
    
40. 核心就是要确定 摄像机和机械手基础 坐标系间的关系 ，机械手基础坐标系就是以机械手底座
    
41. 为原点建立的坐标系，摄像 机坐标系是以摄像 机中心为原点 建立 的坐标系，知道 它们间的关系
    
42. 后就可以计算出要抓取物体可基础 坐标系下的坐标，从而 可以进行准确抓取。
    
43. 
    
44. 
    
45. 02' + '.dat', ToolInBasePose)
    
46.     *设置标定模型中机械手位姿，ToolInBasePose为上面从文件读取的数值。
    
47.     set_calib_data (CalibDataID, 'tool', Index, 'tool_in_base_pose', ToolInBasePose)
    
48.     *确定在相机坐标系下标定板的位姿，并设置到标定模型中。
    
49.     find_calib_object (CalibImage, CalibDataID, 0, 0, Index, [], [])
    
50.     * 显示结果
    
51.     dev_display (CalibImage)
    
52.     *从标定模型中获取标定板位姿信息。
    
53.     get_calib_data_observ_pose (CalibDataID, 0, 0, Index, ObjInCameraPose)
    
54.     *显示标定图像
    
55.     disp_caltab (WindowHandle, CalibObjDescr, CameraParam, ObjInCameraPose, 1)
    
56.     disp_message (WindowHandle, 'Calibration image ' + Index + ' of 10', 'window', 12, 12, 'black', 'true')
    
57.     wait_seconds (0.2)
    
58. endfor
    
59. disp_continue_message (WindowHandle, 'black', 'true')
    
60. stop ()
    
61. * 步骤2 执行手眼标定
    
62. calibrate_hand_eye (CalibDataID, Errors)
    
63. * Get the result of the calibration, i.e., the pose of
    
64. * the robot base in the camera coordinate system
    
65. *获取标定结果，当前获取在相机坐标系与机械手基础坐标系关系，也就是旋转矩阵和平移矢量。
    
66. *这也是相机固定类型的手眼标定目的：求得这种关系。有了各个坐标系间的转换关系后
    
67. *就可以换算出物体在机械手基础坐标系下的坐标，从而让机构手抓取物体。
    
68. get_calib_data (CalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInCamPosePre)
    
69. * 释放模型
    
70. clear_calib_data (CalibDataID)
    
71. * 可视化显示
    
72. disp_preliminary_result (WindowHandle, BaseInCamPosePre, Errors)
    
73. disp_continue_message (WindowHandle, 'black', 'true')
    
74. stop ()
    
75. *步骤3 确定详细位姿。
    
76. *在进行SCARA机械手标定时，无法确定全部位姿参数。在相机固定标定情况下，
    
77. *ObjInToolPose的Z轴转换不能确定，所以必须移动机械手到已知高度，确定Z轴的转换。
    
78. *首先解下标定板放在相机能拍全的任意位置，计算相机坐标系下标定板位姿(保存到ObjInCamPoseRef)
    
79. *然后人为移动机械手夹具到标定板中心（数据保存到ToolInBasePoseRef）
    
80. *前面两个变量传入函数fix_scara_ambiguity_stationary_cam就可确定Z轴的转换
    
81. *得到ObjInCamPoseRef
    
82. read_image (ImageRef, '3d_machine_vision/hand_eye/scara_stationary_cam_setup_01_calib_ref')
    
83. *get_calib_plate_pose (ImageRef, CameraParam, CalibObjDescr, ObjInCamPoseRef)
    
84. create_calib_data ('calibration_object', 1, 1, CalibDataID)
    
85. set_calib_data_cam_param (CalibDataID, 0, [], CameraParam)
    
86. set_calib_data_calib_object (CalibDataID, 0, CalibObjDescr)
    
87. find_calib_object (ImageRef, CalibDataID, 0, 0, 0, [], [])
    
88. get_calib_data_observ_pose (CalibDataID, 0, 0, 0, ObjInCamPoseRef)
    
89. clear_calib_data (CalibDataID)
    
90. *得到ToolInBasePoseRef
    
91. read_pose ('scara_stationary_cam_setup_01_tool_in_base_pose_ref.dat', ToolInBasePoseRef)
    
92. *fix_scara_ambiguity_stationary_cam (BaseInCamPosePre, ToolInBasePoseRef, ObjInCamPoseRef, ZCorrection)
    
93. pose_invert (BaseInCamPosePre, CamInBasePose)
    
94. pose_compose (CamInBasePose, ObjInCamPoseRef, ObjInBasePose)
    
95. ZCorrection := ObjInBasePose[2] - ToolInBasePoseRef[2]
    
96. *转换三维位姿的原点。
    
97. set_origin_pose (BaseInCamPosePre, 0, 0, ZCorrection, BaseInCamPose)
    
98. *
    
99. *可视化显示。
    
100. disp_final_results (WindowHandle, BaseInCamPosePre, BaseInCamPose)
     
101. disp_end_of_program_message (WindowHandle, 'black', 'true')
     
102. *
     
103. *手眼标定后，计算所得的BaseInCamPose可用于机械手抓取工作：
     
104. *我们假设相机对要抓取的物体拍摄一图像，计算得到其位姿保存到变量ObjInCamPose，
     
105. *通过此变量ObjInCamPose及前面标定的结果BaseInCamPose，
     
106. *机械手抓取物体的位姿ObjInBasePose就可以计算出来。
     
107. pose_invert (BaseInCamPose, CamInBasePose)
     
108. create_pose (0.0035, -0.0128, 0.7981, 1.345, 356.158, 180.194, 'Rp+T', 'gba', 'point', ObjInCamPose)
     
109. pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)
     
110. *
     
111. * 准备额外的数据来计算被抓取 物体位姿。
     
112. *计算测量平面位姿
     
113. set_origin_pose (ObjInCamPoseRef, 0, 0, CalibrationPlateThickness - ObjectThickness, MPInCamPose)
     
114. * 保存数据用于后续机械手抓取项目
     
115. * 保存标定结果
     
116. write_pose (CamInBasePose, 'cam_in_base_pose.dat')
     
117. *相机内参
     
118. write_cam_par (CameraParam, 'camera_parameters.dat')
     
119. *测量板位姿
     
120. write_pose (MPInCamPose, 'measurement_plane_in_cam_pose.dat')

复制代码

当前这个实现就是将相机固定在一个地方，机械手带动标定 板在不同的位置进行拍照，
再进行标定求出摄像机坐标系与机械手基础坐标系转换关系，其实就是平移矢量和放置矩阵。
核心就是要确定 摄像机和机械手基础 坐标系间的关系 ，机械手基础坐标系就是以机械手底座
为原点建立的坐标系，摄像 机坐标系是以摄像 机中心为原点 建立 的坐标系，知道 它们间的关系
后就可以计算出要抓取物体可基础 坐标系下的坐标，从而 可以进行准确抓取。