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 《少有人走的路》勇哥的工业自动化技术网站。如果需要本贴图片源码等资源，请向勇哥索取。