根据手眼标定，进行产品定位（眼在手上）

原创已于 2025-06-09 15:34:16 修改 · 154 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#halcon

于 2025-06-09 14:37:01 首次发布

根据手眼标定，进行产品定位

要算出检测对象在机械手基坐标系下的位置
=
机械手基坐标系下的工具坐标系
x
工具坐标系下相机的位置
x
相机坐标系下的检测对象坐标

1.相机得到目标位置
2.转换成工具坐标系下位置
3.工具坐标系转基坐标系

一读取手眼标定结果

1.读取手眼标定后的相机内参
2.读取相机坐标系下的机械手坐标系的位姿
3.转为齐次变换矩阵
4.读取机械手工具坐标系下的标定板对象的位姿
5.转为齐次变换矩阵
6.读取机械手工具坐标系下的夹具的位姿
7.转为齐次变换矩阵

二设置参考坐标系

1.确定描述文件路径
2.定义参考坐标系
3.将参考坐标系位姿转换为其次变换矩阵

三获取目标位置

四获取抓取位姿

五转换抓取位姿

六确定位姿


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

*关闭打开的窗口
dev_close_window ()

*校正图像和数据路径
ImageNameStart := '3d_machine_vision/hand_eye/stationarycam_'
DataNameStart := 'handeye/stationarycam_'
*读取图像
read_image (Image, ImageNameStart + 'nut12_square')

*获取图像的大小
get_image_size (Image, Width, Height)

*打开新窗口
dev_open_window_fit_image (Image, 0, 0, Width, Height, WindowHandle)

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

*设置线宽度
dev_set_line_width (2)

*设置字体信息:字体大小:14;字体类型:mono;粗体;
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')

*显示图像
dev_display (Image)
disp_message (WindowHandle, 'Object to grasp', 'window', 12, 12, 'black', 'true')


*****************************************************************************第一.读取手眼标定结果****************************************************************************************
* 读取手眼标定后的摄像机内部参数
read_cam_par (DataNameStart + 'final_campar.dat', CamParam)

*读取摄像机坐标系下的机器人坐标系的位姿
read_pose (DataNameStart + 'final_pose_cam_base.dat', BaseInCamPose)
*将位姿转换为齐次变换矩阵
pose_to_hom_mat3d (BaseInCamPose, cam_H_base)

*读取机器人工具坐标系下的标定板对象的位姿
read_pose (DataNameStart + 'final_pose_tool_calplate.dat', CalplateInToolPose)
*将位姿转换为齐次变换矩阵
pose_to_hom_mat3d (CalplateInToolPose, tool_H_calplate)

* 读取机器人工具坐标系下的夹具的位姿
read_pose (DataNameStart + 'pose_tool_gripper.dat', GripperInToolPose)
*将位姿转换为齐次变换矩阵
pose_to_hom_mat3d (GripperInToolPose, tool_H_gripper)
stop ()


*****************************************************************************第二.设置参考坐标系,并获取抓取位置********************************************************************************
*校正板描述文件
CalplateFile := 'caltab_30mm.descr'

*定义参考坐标系
define_reference_coord_system (ImageNameStart + 'calib3cm_00', CamParam, CalplateFile, WindowHandle, PoseRef)

*将参考坐标系位姿转换为其次变换矩阵
pose_to_hom_mat3d (PoseRef, cam_H_ref)
Message := 'Defining a reference coordinate system'
Message[1] := 'based on a calibration image'
disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
*显示图像
dev_display (Image)

*显示参考坐标系
disp_3d_coord_system (WindowHandle, CamParam, PoseRef, 0.01)

*设置输出对象的颜色
dev_set_color ('yellow')

*对图像进行阈值，分割出零件区域
threshold (Image, BrightRegion, 60, 255)

*对分割出的区域进行连通性处理
connection (BrightRegion, BrightRegions)

*过滤出最大的区域
select_shape (BrightRegions, Nut, 'area', 'and', 500, 99999)

*对过滤出的区域进行填充
fill_up (Nut, NutFilled)

*将区域转换为轮廓
gen_contour_region_xld (NutFilled, NutContours, 'border')

*将轮廓分割为线轮廓
segment_contours_xld (NutContours, LineSegments, 'lines', 5, 4, 2)
fit_line_contour_xld (LineSegments, 'tukey', -1, 0, 5, 2, RowBegin, ColBegin, RowEnd, ColEnd, Nr, Nc, Dist)
gen_empty_obj (Lines)
for I := 0 to |RowBegin| - 1 by 1
    gen_contour_polygon_xld (Contour, [RowBegin[I],RowEnd[I]], [ColBegin[I],ColEnd[I]])
    concat_obj (Lines, Contour, Lines)
endfor

*根据线集合生成轮廓
gen_polygons_xld (Lines, Polygon, 'ramer', 2)

*提取并行的XLD
gen_parallels_xld (Polygon, ParallelLines, 50, 100, rad(10), 'true')

*显示XLD
dev_display (ParallelLines)

*返回一个XLD并行的数据
get_parallels_xld (ParallelLines, Row1, Col1, Length1, Phi1, Row2, Col2, Length2, Phi2)

*获取零件4个交点的位置坐标
CornersRow := [Row1[0],Row1[1],Row2[0],Row2[1]]
CornersCol := [Col1[0],Col1[1],Col2[0],Col2[1]]


*****************************************************************************第三.在摄像机坐标系下,获取抓取位姿cam_H_grasp********************************************************************************
*方法一: 把角点转换到参考坐标系并获取抓取位置
*PoseRef为零件坐标系位姿，将零件坐标系位姿从图像坐标系转换为世界坐标系下。
image_points_to_world_plane (CamParam, PoseRef, CornersRow, CornersCol, 'm', CornersX_ref, CornersY_ref)

* 零件的中心坐标
CenterPointX_ref := sum(CornersX_ref) * 0.25
CenterPointY_ref := sum(CornersY_ref) * 0.25
* 零件抓取位置的坐标和角度
GraspPointsX_ref := [(CornersX_ref[0] + CornersX_ref[1]) * 0.5,(CornersX_ref[2] + CornersX_ref[3]) * 0.5]
GraspPointsY_ref := [(CornersY_ref[0] + CornersY_ref[1]) * 0.5,(CornersY_ref[2] + CornersY_ref[3]) * 0.5]
GraspPhiZ_ref := atan((GraspPointsY_ref[1] - GraspPointsY_ref[0]) / (GraspPointsX_ref[1] - GraspPointsX_ref[0]))
* 对参考坐标系进行平移和旋转变换
affine_trans_point_3d (cam_H_ref, GraspPointsX_ref, GraspPointsY_ref, [0,0], GraspPointsX_cam, GraspPointsY_cam, GraspPointsZ_cam)

*将抓取点位置投影到图像坐标系下
project_3d_point (GraspPointsX_cam, GraspPointsY_cam, GraspPointsZ_cam, CamParam, GraspPointsRow, GraspPointsCol)

*显示抓取位置G1，G2
display_grasping_points (GraspPointsRow, GraspPointsCol, WindowHandle)
disp_message (WindowHandle, 'Finding grasping points', 'window', -1, -1, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
* 构建参考坐标系到零件坐标系的齐次变换矩阵
hom_mat3d_identity (HomMat3DIdentity)
hom_mat3d_rotate (HomMat3DIdentity, GraspPhiZ_ref, 'z', 0, 0, 0, HomMat3D_RZ_Phi)
hom_mat3d_translate (HomMat3D_RZ_Phi, CenterPointX_ref, CenterPointY_ref, 0, ref_H_grasp)

*摄像机坐标系下参考坐标系位姿和参考坐标系下零件抓取坐标位姿相乘，从而获取摄像机坐标系下抓取位置位姿
hom_mat3d_compose (cam_H_ref, ref_H_grasp, cam_H_grasp)
hom_mat3d_to_pose (cam_H_grasp, GripperInCamPose)

*设置输出对象的颜色数
dev_set_colored (3)

*显示零件坐标系
disp_3d_coord_system (WindowHandle, CamParam, GripperInCamPose, 0.01)
Message := 'Determining the gripper pose'
Message[1] := 'via the reference coordinate system'
disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()



******************************************************************************第四.在机器人坐标系下转换抓取位姿********************************************************************************
* base_H_tool = base_H_cam * cam_H_ref * ref_H_grasp * gripper_H_tool

*反转摄像机坐标下机器人底座的位姿，从而获取机器人坐标系下摄像机的位姿
hom_mat3d_invert (cam_H_base, base_H_cam)

*将机器人坐标系下摄像机位姿和摄像机坐标系下夹具位姿相乘，从而获取机器人坐标系下夹具位姿。
hom_mat3d_compose (base_H_cam, cam_H_grasp, base_H_grasp)

*反转机器人工具坐标系下夹具位姿，从而获取夹具坐标系下机器人工具位姿
hom_mat3d_invert (tool_H_gripper, gripper_H_tool)

*机器人坐标系下夹具位姿和夹具坐标系下机器人工具位姿相乘，从而获取机器人坐标下机器人工具的位姿
hom_mat3d_compose (base_H_grasp, gripper_H_tool, base_H_tool)

*将机器人坐标下机器人工具的其次变换矩阵转换位位姿
hom_mat3d_to_pose (base_H_tool, PoseRobotGrasp)


******************************************************************************第五.转换位姿类型********************************************************************************
* 转换机器人的位姿类型
convert_pose_type (PoseRobotGrasp, 'Rp+T', 'abg', 'point', PoseRobotGrasp_ZYX)
dev_inspect_ctrl (PoseRobotGrasp_ZYX)