scare机器人如何手眼标定_SCARA机器人手眼标定之目标抓取
SCARA机器人手眼标定之目标抓取的例子：
显示如何基于由SCARA手眼校准确定的校准信息，使用SCARA机器人执行拾取和放置应用程序。 在第一步骤中，根据模型图像定义形状模型。 然后，基于该形状模型，在每个图像中搜索对象。 对于一个选定的对象，计算可用于掌握该对象的机器人坐标。 为了使该示例适用于实际应用，必须从相机获取图像(而不是从文件读取)，并且必须实施机器人的控制(而不是在本示例中被注释掉的相应行)。 通常，图像必须在匹配之前校正。 如果摄像机完全正交于测量平面，则此步骤可以仅被省略。 要使用提供的示例图像运行示例程序，RectifyImages必须设置为true。
RectifyImages := true
*使用实例图像
RectifyImages := true
***********读取手眼标定结果***********************************
try
* 读取机器人坐标系下摄像机位姿
read_pose ('cam_in_base_pose.dat', CamInBasePose)
*读取摄像机内参
read_cam_par ('camera_parameters.dat', CameraParam)
*读取摄像机坐标系下测量平面位姿
read_pose ('measurement_plane_in_cam_pose.dat', MPInCamPose)
catch (Exception)
*当没有找到时，可以通过SCARA机械手有眼标定来获取位姿和摄像机内参
CamInBasePose := [0.05592166548,0.19497621789,0.48025117245,180.09816119,29.85593363,179.94389014,0]
CameraParam := [0.0165251,-642.277,4.65521e-006,4.65e-006,595.817,521.75,1280,1024]
MPInCamPose := [0.0045679683065,-0.0028695297318,0.4088853425,359.78658429,29.732027579,0.22946472765,0]
endtry
if (RectifyImages)
*获取摄像机坐标系下测量平面位姿校正后的映射关系
prepare_rectification_map (Map, CameraParam, MPInCamPose, MappingScale, MPInCamPoseMapping)
*将图像坐标系下圆点[0,0]坐标转换为世界坐标系下MapUpperLeftX, MapUpperLeftY坐标
image_points_to_world_plane (CameraParam, MPInCamPoseMapping, \
0, 0, 'm', MapUpperLeftX, MapUpperLeftY)
endif
*
dev_update_off ()
*该参数确定使用find_shape_model查找的形状模型可以部分在图像外面,允许
set_system ('border_shape_models', 'true')
***************对含标定板图像进行校正********************
//这里，应该建立与机器人的连接，并且机器人应该移动到一个定义的待机姿态，允许获取测量平面的未被遮挡的图像
//定义要抓取的对象形状模型
//获取模型的图像
* 读取图像模板
read_image (Image, '3d_machine_vision/handeye/scara_stationary_cam_setup_01_metal_parts_01')
if (RectifyImages)
*通过映射投影图对图像进行校正
map_image (Image, Map, ModelImage)
else
copy_image (Image, ModelImage)
endif
*关闭打开的窗口
dev_close_window ()
*打开一个新窗口
dev_open_window_fit_image (ModelImage, 0, 0, 600, 600, WindowHandle)
*设置字体信息:字体大小14，字体类型:mono,粗体
set_display_font (WindowHandle, 16, 'mono', 'true', 'false')
*清除窗口内容
dev_clear_window ()
*显示变换后的图像
dev_display (ModelImage)
*设置字宽度
dev_set_line_width (2)
********创建形状模板*********************************
*产生一个标准矩形
gen_rectangle1 (ModelROI, 400, 500, 1100, 1300)
*对图像进行高斯滤波
gauss_filter (ModelImage, ImageGauss, 5)
*剪切矩形里图像
reduce_domain (ImageGauss, ModelROI, ImageReduced)
*用剪切图像创建形状模板
create_shape_model (ImageReduced, 'auto', rad(0), rad(360), 'auto',\
'auto', 'use_polarity', [10,50], 'auto', ModelID)
*获取模板矩形区域的面积，中心坐标；
area_center (ModelROI, ModelROIArea, ModelROIRow, ModelROIColumn)
*显示模板轮廓
dev_display_shape_matching_results (ModelID, 'green', ModelROIRow, ModelROIColumn, 0, 1, 1, 0)
*通过记录相应的机器人姿势来定义物体上的抓取点
DefineGraspingPointByRobot := true
//指定对象的抓取点
//它要在图像中(只有当对象可以由工具以任何方向拾取时)
//或者通过用机器人抓取它并记录相应的机器人姿态
if (DefineGraspingPointByRobot)
dev_set_colored (12)
*机器人坐标系下模板抓取点位姿
GraspingPointModelInBasePose := [0.2592,0.1997,0.1224,0,0,1.2572,0]
*位姿反转，获取摄像机坐标系下机器人位姿
pose_invert (CamInBasePose, BaseInCamPose)
*将摄像机坐标系下机器人位姿转换为其次变换矩阵
pose_to_hom_mat3d (BaseInCamPose, BaseInCamHomMat3D)
*将仿射三维变换应用于点【GraspingPointModelInBasePose[0], \
*GraspingPointModelInBasePose[1], GraspingPointModelInBasePose[2]】
affine_trans_point_3d (BaseInCamHomMat3D, GraspingPointModelInBasePose[0], \
GraspingPointModelInBasePose[1], GraspingPointModelInBasePose[2], Qx, Qy, Qz)
*把点【Qx, Qy, Qz】投影到图像坐标系
project_3d_point (Qx, Qy, Qz, CameraParam, GraspingPointModelRow, GraspingPointModelColumn)
*抓取模板角度,位姿格式:[TransX; TransY ; TransZ; RotX; RotY; RotZ]
GraspingPointModelAngle := GraspingPointModelInBasePose[5]
*将图像坐标系下抓取位置点转换为世界坐标下,参数Scale可以缩放得到的三维坐标
image_points_to_world_plane (CameraParam, MPInCamPoseMapping, GraspingPointModelRow,\
GraspingPointModelColumn, MappingScale, GraspingPointModelXMP, GraspingPointModelYMP)
*当前抓取坐标减去原点坐标
GraspingPointModelRow := GraspingPointModelYMP - MapUpperLeftY / MappingScale
GraspingPointModelColumn := GraspingPointModelXMP - MapUpperLeftX / MappingScale
endif
*显示模板矩形的面积和中心坐标
area_center (ModelROI, ModelROIArea, ModelROIRow, ModelROIColumn)
*设置模板原点坐标，该原点相对于形状模板重心的位置[GraspingPointModelRow - ModelROIRow,
* GraspingPointModelColumn - ModelROIColumn]
*设置手眼抓取零件位置与零件重心偏移量
set_shape_model_origin (ModelID, GraspingPointModelRow - ModelROIRow, \
GraspingPointModelColumn - ModelROIColumn)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
*******循环读取机器人要抓取的物体的图像*********************
*将摄像机坐标系下测量平面位姿转换为其次变换矩阵
pose_to_hom_mat3d (MPInCamPoseMapping, MPInCamHomMat3DMapping)
//在下列图像中找被抓取的物体
for ImageIdx := 2 to 6 by 1
* 获取图像
read_image (Image,\
'3d_machine_vision/handeye/scara_stationary_cam_setup_01_metal_parts_'+ ImageIdx$'02d')
*校正图像
if (RectifyImages)
map_image (Image, Map, SearchImage)
else
copy_image (Image, SearchImage)
endif
dev_clear_window ()
dev_display (SearchImage)
*在校正图像中寻找工件模板
find_shape_model (SearchImage, ModelID, rad(0), rad(360), 0.5, 0, 0.5,\
'least_squares', [0,3], 0.9, Row, Column, Angle, Score)
if (|Row|
disp_message (WindowHandle,'No objects found', 'window', 12, 12, 'black', 'true')
continue
endif
*
* 获取最左边工件的抓取坐标和角度
LeftmostIdx := sort_index(Column)[0]
GraspingPointRow := Row[LeftmostIdx]
GraspingPointColumn := Column[LeftmostIdx]
GraspingPointAngle := Angle[LeftmostIdx]
*
* 显示匹配结果和待抓取的零件
dev_display_shape_matching_results (ModelID, 'blue', Row, Column, Angle, 1, 1, 0)
dev_display_shape_matching_results (ModelID, 'green', GraspingPointRow,\
GraspingPointColumn, GraspingPointAngle, 1, 1, 0)
disp_message (WindowHandle, |Row| + ' objects found (Green: Object to be grasped)',\
'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
*
* 计算出机器人坐标系下校正对象的位姿
calculate_point_to_approach_scara_stationary (GraspingPointRow, GraspingPointColumn, \
GraspingPointAngle + rad(GraspingPointModelAngle), RectifyImages, \
MapUpperLeftX, MapUpperLeftY,\
MappingScale, MPInCamHomMat3DMapping, CameraParam, MPInCamPose, \
CamInBasePose, ObjInBasePose)
*
*将摄像机坐标系下校正对象位姿中X,Y,Z偏移量转换为mm
ToolInBasePoseMM := [ObjInBasePose[0:2] * 1000,ObjInBasePose[3:6]]
* Pick and place the object
*在这里，机器人应该移动到上面确定的对象的位置(ToolInBasePoseMM)，在那里对象应该被拾取，
*然后放置在某个预定位置(像PlacePositionInBasePoseMM)。
*最后，机器人应该再次移动到备用姿势，以便拍摄测量平面的未被遮挡的图像。
stop ()
endfor
//这儿机器人连接被关闭
*清除形状模板
clear_shape_model (ModelID)
**该参数确定使用find_shape_model查找的形状模型可以部分在图像外面,禁止
set_system ('border_shape_models', 'false')
dev_clear_window ()
#转载请注明出处 www.skcircle.com 《少有人走的路》勇哥的工业自动化技术网站。如果需要本贴图片源码等资源，请向勇哥索取。