Halcon执行手眼标定， 发那科机器人三点法标定

固定相机

原理: ObjInCam = BaseInCam × ToolInBase × ObjInTool

其中 对标定板的多次拍照可以确定CalObjInCam
机器人内部参数可以读取到ToolInBase

然后生成手眼标定参数集合 CalibDataID , CalibDataID中存放了相机内部参数, 标定板描述文件等, 通过手眼标定确定等号右边第一和第三个参数 BaseInCam , ObjInTool

最终要求得ObjInBase
ObjInBase = CamInBase × ObjInCam

手眼标定确定了 BaseInCam, 求出逆矩阵即可
ObjInCam 为照相机拍标定板确定
二者相乘得到ObjInBase

注意:
如果没有夹具, 则直接
ToolInBase = ToolInBase
如果有夹具, 则ToolInBase × GripperInTool = GripperInBase

则ToolInBase = GripperInBase×Inv(GripperInTool)

固定相机:


*关闭程序计数器，变量更新，图像更新窗口
dev_update_off ()
* 校正图像路径
*ImageNameStart := '3d_machine_vision/handeye/stationarycam_calib3cm_'
ImageNameStart:= '3d_machine_vision/calib/' 
ImgPath := '3d_machine_vision/calib/'
*read_image (Image, ImgPath + 'calib_')
*机器人工具坐标系的位姿
*stationarycam_start_campar.dat
DataNameStart := 'handeye/stationarycam_'

*校正图像的数目
NumImages := 17

*读取一张图像
read_image (Image, ImageNameStart+'calib_'+'02')

*获取图像的大小
get_image_size (Image, Width, Height)
* 关闭已经打开的窗口
dev_close_window ()

*打开新窗口
dev_open_window (0, 0, Width, Height, 'black', WindowHandle)

*设置线宽
dev_set_line_width (2)

*设置区域填充方式为margin
dev_set_draw ('margin')

*显示图像
dev_display (Image)

* 设置字体
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')

**************************************************************准备手眼标定的数据**************************************************************************************


* 标定板描述文件
CalTabFile := 'caltab_30mm.descr'

* 读取摄像机内部参数
read_cam_par (DataNameStart + 'start_campar.dat', StartCamParam)

* 创建手眼标定模型

create_calib_data ('hand_eye_stationary_cam', 1, 1, CalibDataID)

* 重要, 将 摄像机内部参数  StartCamParam 写入手眼标定模型CalibDataID中
* 对手眼标定模型设置摄像机内部参数
set_calib_data_cam_param (CalibDataID, 0, 'area_scan_division', StartCamParam)


* 对手眼标定模型设置标定板描述文件
set_calib_data_calib_object (CalibDataID, 0, CalTabFile)

*采用非线性算法获取精确校准姿态 再次校准 CalibDataID
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')
stop ()




**************************************************************获取标定板MARK点坐标和标定板相对相机的位姿获取ObjInCam**************************************************************************************

for I := 1 to NumImages  by 1
    *读取含标定板的图像
    *read_image (Image, ImageNameStart + I$'02d')
     read_image (Image, ImgPath + 'calib_' + I$'02d')
     dev_display (Image)
    *寻找标定板对象
    find_calib_object (Image, CalibDataID, 0, 0, I, [], [])
    
    *获取标定板轮廓
    get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, I)
    
    *********非常重要 得到第一个已知参数
    *获取标定板MARK点坐标和标定板相对相机的位姿 Mark点: 标定板上的圆
    get_calib_data_observ_points (CalibDataID, 0, 0, I, RCoord, CCoord, Index, CalObjInCamPose)

    
    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, CalObjInCamPose, 0.01)

    
    * 读取机器人基座坐标系下机器人工具位姿
    read_pose (DataNameStart + 'robot_pose_' + I$'02d' + '.dat', ToolInBasePose)

    
    * 设置机器人基座坐标系下机器人工具位姿设置到手眼标定模型里 CalibDataID
    set_calib_data (CalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)
    * Uncomment to inspect visualization
    disp_message (WindowHandle, 'Extracting data from calibration image ' + (I + 1) + ' of ' + NumImages, 'window', -1, -1, 'black', 'true')
    disp_continue_message (WindowHandle, 'black', 'true')
    wait_seconds (1)
endfor
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 ()


**************************************************************执行手眼标定**************************************************************************************
dev_display (Image)
disp_message (WindowHandle, 'Performing the hand-eye calibration', 'window', 12, 12, 'black', 'true')

*执行手眼标定
calibrate_hand_eye (CalibDataID, Errors)

* 查询手眼标定的错误情况
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)

* 获取摄像机坐标系下机器人基座的位姿 : BaseInCamPose 未知 需要求出的 等号右边第一个参数
get_calib_data (CalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInCamPose)

* 获取机器人工具坐标系下校正对象的位姿 : ObjInToolPose 未知 需要求出的 等号右边第三个参数
get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_tool_pose', ObjInToolPose)

* 错误对话框是否被抑制
dev_get_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)

dev_set_preferences ('suppress_handled_exceptions_dlg', 'true')

**************************************************************保存手眼标定的结果到本地硬盘**************************************************************************************
****************************
***********************
*********
***CalObjInCamPose = BaseInCamPose * ToolInBasePose * ObjInToolPose
***已知条件:CalObjInCamPose 通过标定板图像(校正对象), ToolInBasePose(系统)
***在有夹具下: ToolInBasePose(系统) = ObjInbase*Inv(GripperInTool)
***要求等号右边的1和3

try

    * 保存相机内部参数参数到本地文件 : CamParam
    write_cam_par (CamParam, DataNameStart + 'final_campar.dat')
    
    * 将摄像机坐标系下的机器人底座位姿保存到本地   获得了未知参数 等号右边 参数1
    write_pose (BaseInCamPose, DataNameStart + 'final_pose_cam_base.dat')
   
    
    * 将机器人工具坐标系下的校正对象的位姿保存到本地   获得了未知参数 等号右边 参数3
    write_pose (ObjInToolPose, DataNameStart + 'final_pose_tool_calplate.dat')
    
catch (Exception)
    * Do nothing
endtry
dev_set_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
* Display calibration errors of the hand-eye calibration
disp_results (WindowHandle, CamCalibError, Errors)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()


**************************************************************计算摄像机坐标系下校正对象的位姿**************************************************************************************
* 查询摄像机，校正对象，校正对象位姿的相对关系
query_calib_data_observ_indices (CalibDataID, 'camera', 0, CalibObjIdx, PoseIds)

for I := 0 to NumImages - 1 by 1
    
    read_image (Image, ImageNameStart + I$'02d')

    *获取机器人基准坐标中机器人工具的姿态 : 系统获得ToolInBasePose
    get_calib_data (CalibDataID, 'tool', PoseIds[I], 'tool_in_base_pose', ToolInBasePose)
    
    dev_display (Image)

    *计算摄像机坐标系下校正对象的位姿
    calc_calplate_pose_stationarycam (ObjInToolPose, BaseInCamPose, ToolInBasePose, CalObjInCamPose)
    dev_set_colored (3)
    disp_3d_coord_system (WindowHandle, CamParam, CalObjInCamPose, 0.01)
    Message := 'Using the calibration results to display the'
    Message[1] := 'coordinate system in image ' + (I + 1) + ' of ' + NumImages
    disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
    if (I < NumImages - 1)
        disp_continue_message (WindowHandle, 'black', 'true')
        stop ()
    endif
endfor
* 释放手眼标定模板内存
clear_calib_data (CalibDataID)
* 
**************************************************************获取机器人基座坐标系中校正对象的位姿**************************************************************************************

ObjInCamPose := CalObjInCamPose

***CalObjInCamPose = BaseInCamPose * ToolInBasePose * ObjInToolPose


pose_invert (BaseInCamPose, CamInBasePose)

*CalibDataID 有相机的内部参数, 标定板描述文件 标定板上Mark点坐标等
*最终结果之一  由 get_calib_data (CalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInCamPose) 得到等号右边第一个未知参数 BaseInCamPose
*由 get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_tool_pose', ObjInToolPose) 得到等号右边第三个未知参数 ObjInToolPose

*最终结果之二,等号左边 需要看有没有Cal 由 get_calib_data_observ_points (CalibDataID, 0, 0, I, RCoord, CCoord, Index, CalObjInCamPose) 得到 等号左边已知参数 CalObjInCamPose
*由 set_calib_data (CalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose) 得到等号右边第二个已知参数 ToolInBasePose

*获取机器人基座坐标系中校正对象的位姿ObjInBasePose 
pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)

移动相机

*关闭程序计数器，变量更新，图像窗口更新
dev_update_off ()

* 校正图像路径
ImageNameStart := '3d_machine_vision/handeye/movingcam_calib3cm_'

*机器人工具坐标系的位姿
DataNameStart := 'handeye/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)

*设置区域填充方式为margin
dev_set_draw ('margin')

*显示图像
dev_display (Image)

*设置字体信息，字体大小为14，字体为mono，粗体
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')

* 标定板描述文件
CalTabFile := 'caltab_30mm.descr'

* 读取摄像机内部参数
read_cam_par (DataNameStart + 'start_campar.dat', StartCamParam)

* 创建手眼标定模型
create_calib_data ('hand_eye_moving_cam', 1, 1, CalibDataID)

* 对手眼标定模型设置摄像机内部参数
set_calib_data_cam_param (CalibDataID, 0, 'area_scan_division', StartCamParam)

* 对手眼标定模型设置标定板描述文件
set_calib_data_calib_object (CalibDataID, 0, CalTabFile)

*采用非线性算法获取精确校准姿态
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')
stop ()
**************************************************************获取标定板MARK点坐标和标定板相对相机的位姿**************************************************************************************
for I := 0 to NumImages - 1 by 1
    *读取含标定板的图像
    read_image (Image, ImageNameStart + I$'02d')

    *寻找标定板对象
    find_calib_object (Image, CalibDataID, 0, 0, I, [], [])
    
     *获取标定板轮廓
    get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, I)
    
    *获取标定板MARK点坐标和标定板相对相机的位姿
    get_calib_data_observ_points (CalibDataID, 0, 0, I, RCoord, CCoord, Index, PoseForCalibrationPlate)

    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)
    
    * 读取机器人基座坐标系下机器人工具位姿
    read_pose (DataNameStart + 'robot_pose_' + I$'02d' + '.dat', ToolInBasePose)
    
    * 设置机器人基座坐标系下机器人工具位姿设置到手眼标定模型里
    set_calib_data (CalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)
    * Uncomment for inspection of visualization
*     disp_message (WindowHandle, 'Extracting data from calibration image ' + (I + 1) + ' of ' + NumImages, 'window', 12, 12, 'black', 'true')
*     disp_continue_message (WindowHandle, 'black', 'true')
*     stop ()
endfor
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 ()

**************************************************************执行手眼标定**************************************************************************************

*执行手眼标定
calibrate_hand_eye (CalibDataID, Errors)

* 查询手眼标定的错误情况
get_calib_data (CalibDataID, 'model', 'general', 'camera_calib_error', CamCalibError)

* 获取摄像机内部参数
get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam)

* 获取摄像机坐标系下机器人工具的位姿
get_calib_data (CalibDataID, 'camera', 0, 'tool_in_cam_pose', ToolInCamPose)

* 获取机器人基础坐标系下校正对象的位姿
get_calib_data (CalibDataID, 'calib_obj', 0, 'obj_in_base_pose', CalObjInBasePose)

* 错误对话框是否被抑制
dev_get_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_set_preferences ('suppress_handled_exceptions_dlg', 'true')
try

    * 保存相机内部参数参数到本地文件
    write_cam_par (CamParam, DataNameStart + 'final_campar.dat')
    
    * 将摄像机坐标系下的机器人工具位姿保存到本地
    write_pose (ToolInCamPose, DataNameStart + 'final_pose_cam_tool.dat')
    
    * 将机器人基础坐标系下的校正对象的位姿保存到本地
    write_pose (CalObjInBasePose, DataNameStart + 'final_pose_base_calplate.dat')
catch (Exception)
    * do nothing
endtry
dev_set_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_display (Image)
* Display calibration errors
disp_results (WindowHandle, CamCalibError, Errors)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()

**************************************************************计算摄像机坐标系下校正对象的位姿**************************************************************************************
* 查询摄像机，校正对象，校正对象位姿的相对关系
query_calib_data_observ_indices (CalibDataID, 'camera', 0, CalibObjIdx, PoseIds)

for I := 0 to NumImages - 1 by 1
    read_image (Image, ImageNameStart + I$'02d')
    dev_display (Image)
    
    *获取机器人基准坐标中机器人工具的姿态
    get_calib_data (CalibDataID, 'tool', PoseIds[I], 'tool_in_base_pose', ToolInBasePose)
    
    *计算摄像机坐标系下校正对象的位姿
    calc_calplate_pose_movingcam (CalObjInBasePose, ToolInCamPose, ToolInBasePose, CalObjInCamPose)
    *显示坐标系统
    dev_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 ' + NumImages
    disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
    if (I < NumImages - 1)
        disp_continue_message (WindowHandle, 'black', 'true')
        stop ()
    endif
endfor
* 释放手眼标定模板内存
clear_calib_data (CalibDataID)


**************************************************************获取机器人基座坐标系中校正对象的位姿**************************************************************************************
ObjInCamPose := CalObjInCamPose

*位姿反向，获取机器人工具坐标系下摄像机坐标系的位姿
pose_invert (ToolInCamPose, CamInToolPose)

*机器人基础坐标系下机器人工具坐标系的位姿和机器人工具坐标系下摄像机坐标系的位姿进行乘积，从而获取机器人基础坐标系下摄像机坐标系的位姿。
pose_compose (ToolInBasePose, CamInToolPose, CamInBasePose)

*机器人基础坐标系下摄像机坐标系的位姿和摄像机坐标系下校正对象坐标系的位姿进行乘积，从而获取机器人基础坐标系下校正对象坐标系的位姿。
pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)

固定相机:
在这里插入图片描述

移动相机:

在这里插入图片描述

三点法操作步骤：
记录接近点1

a.移动光标到接近点1（Approachpoint1）；

b.把示教坐标切换成全局坐标（WORLD）后移动机器人，使工具尖端接触到基准点；

c.按【SHIFT】+F5【RECORD】（位置记录）记录

在这里插入图片描述

记录接近点2

a.移动光标到接近点2（Approachpoint2）；

b.把示教坐标切换成关节坐标（JOINT），旋转J6轴（法兰面）至少90度，不要超过360度

c.把示教坐标切换成全局坐标（WORLD）后移动机器人，使工具尖端接触到基准点；

d.按【SHIFT】+F5【RECORD】（位置记录）记录；

在这里插入图片描述

记录接近点3

a.移动光标到接近点3（Approachpoint3）；

b.把示教坐标切换成关节坐标（JOINT），旋转J4轴和J5轴，不要超过90度；

c.把示教坐标切换成全局坐标（WORLD）后移动机器人，工具尖端接触到基准点；

d.按【SHIFT】+F5【RECORD】（位置记录）记录；

在这里插入图片描述

当三个点记录完成，新的工具坐标系被自动计算生成.Point1-3显示成USED，如下图所示：

在这里插入图片描述

版权声明：本文为helldoger原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接和本声明。