少有人走的路

*该例程说明了怎样给SCARA机器人做手眼标定。这种情况下，相机相对与机械臂而言是固定安装的，所以能探测到机械臂的整个工作控件范围。

用于标定的标定板是固定安装在机械臂的末端工具上。最后，标定结果和用于确定要被抓取的物体的位置的数据都保存到了本地文件中。在例程pick_and_place_scara_stationary_cam.hdev程序中有被使用到。
*提供校准板的描述文件和先前校准过的摄像机的摄像机参数

*首先是要提供标定板的描述文件和相机的内参，这两个步骤需要事先进行操作完成

CalibObjDescr := 'calibrate_hand_eye_scara_setup_01_calplate.cpd'
CalibrationPlateThickness := 0.003
gen_cam_par_area_scan_division (0.0165251, -642.277, 4.65521e-006, 4.65e-006, 
595.817, 521.75, 1280, 1024, CameraParam)

*相关的设置及窗口初始化 
*设置要抓取的物体的厚度，以便能够
*正确定义测量平面的姿势

ObjectThickness := 0.001

dev_close_window ()
dev_open_window_fit_size (0, 0, 1280, 1024, 640, -1, WindowHandle)
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
dev_update_off ()

*获取标准图像和对应的机器人姿态，这个姿态是指机械臂末端的姿态，从机器人的示教器或者从控制器读取出来
* 1.获取校准图像和相应的机器人姿势
* 
*创建新的校准模型

create_calib_data ('hand_eye_scara_stationary_cam', 1, 1, CalibDataID)

*在校准模型中设置校准板

set_calib_data_calib_object (CalibDataID, 0, CalibObjDescr)
for Index := 1 to 10 by 1
    * Read the calibration image
    read_image (CalibImage, 
    '3d_machine_vision/hand_eye/scara_stationary_cam_setup_01_calib_' + Index$'02')
    * Read the corresponding robot pose (pose of the tool in the
    * robot base coordinate system)
    read_pose ('scara_stationary_cam_setup_01_tool_in_base_pose_' + 
    Index$'02' + '.dat', ToolInBasePose)
    * Set the robot pose in the calibration model
    set_calib_data (CalibDataID, 'tool', Index, 'tool_in_base_pose', ToolInBasePose)
    * Determine the pose of the calibration plate in the camera
    * coordinate system and set the pose in the calibration model
    find_calib_object (CalibImage, CalibDataID, 0, 0, Index, [], [])
    * Visualize
    dev_display (CalibImage)
    get_calib_data_observ_pose (CalibDataID, 0, 0, Index, ObjInCameraPose)
    disp_caltab (WindowHandle, CalibObjDescr, CameraParam, ObjInCameraPose, 1)
    disp_message (WindowHandle, 'Calibration image ' + Index + 
    ' of 10', 'window', 12, 12, 'black', 'true')
    wait_seconds (0.2)
endfor
disp_continue_message (WindowHandle, 'black', 'true')
stop ()

* 
* 2.检查输入姿势是否一致
* 

check_hand_eye_calibration_input_poses (CalibDataID, 0.05, 0.005, Warnings)
if (|Warnings| != 0)
    * There were problem detected in the input poses. Inspect Warnings and
    * remove erroneous poses with remove_calib_data and remove_calib_data_observ.
    dev_inspect_ctrl (Warnings)
    stop ()
endif

* 3.执行手眼校准
*实施手眼标定

calibrate_hand_eye (CalibDataID, Errors)
* Get the result of the calibration, i.e., the pose of
* the robot base in the camera coordinate system
get_calib_data (CalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInCamPosePre)
* Free the calibration model
clear_calib_data (CalibDataID)
* Visualize
disp_preliminary_result (WindowHandle, BaseInCamPosePre, Errors)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()

* 3.修复姿势的不确定
* 
*校准SCARA机器人时， 无法明确确定所有姿势参数。在摄像机静止的情况下，无法确定ObjInToolPose的Z方向平移。就是说，*位姿矩阵中的第三行第四列的这个元素在这之前的标定过程中是不能够明确的，需要其他辅助手段进行校正。
*因此，有必要通过在机器人坐标系中将机器人移动到已知高度的姿势来将未知平移固定在* Z. 为此，校准板（从机器人上拆下）*放置在任意位置，以便可以由相机观察到。然后必须在摄像机坐标系（ - >  ObjInCamPoseRef）  中确定校准板的姿势。之后，*机器人的工具是被手动地移动到校准板的原点（ - > ToolInBasePoseRef）。这两个姿势可用于通过使用算子procedure fix_scara_ambiguity_stationary_cam 来修复矩阵中z方向平移量的偏差。下面是具体操作用到的算子。

*首先将标定板取下来，放在相机能排到的地方，当然，也必须在机器的工作空间范围内，然后让相机拍照，最后
*读取图片
read_image (ImageRef, '3d_machine_vision/hand_eye/scara_stationary_cam_setup_01_calib_ref')
*该算子将获取的标定板图像和标定板描述文件作为输入，输出标定板在相机坐标系下的参考坐标系
get_calib_plate_pose (ImageRef, CameraParam, CalibObjDescr, ObjInCamPoseRef)
*获得机械臂末端在基坐标系下的描述，作为参数输入
read_pose ('scara_stationary_cam_setup_01_tool_in_base_pose_ref.dat', ToolInBasePoseRef)
*修复矩阵中的z方向偏移量，该算子前三个参数为输入参数，最后一个为输出参数
fix_scara_ambiguity_stationary_cam (BaseInCamPosePre, ToolInBasePoseRef, ObjInCamPoseRef, ZCorrection)
set_origin_pose (BaseInCamPosePre, 0, 0, ZCorrection, BaseInCamPose)

*可视化

disp_final_results（WindowHandle，BaseInCamPosePre，BaseInCamPose）
disp_end_of_program_message（WindowHandle，'black'，'true'）

*执行手眼校准后，生成的姿势
* BaseInCamPose可用于机器人抓取应用程序：
*让我们假设相机获取了
应该抓住的物体的图像。从图像中，
必须通过
*使用图像处理确定*摄像机坐标系中对象的姿势（ - > ObjInCamPose）。
*基于此姿势和校准结果
*（BaseInCamPose），可以计算机器人姿势，这是
抓取对象所必需的*（ - > ObjInBasePose）：

pose_invert (BaseInCamPose, CamInBasePose)
create_pose (0.0035, -0.0128, 0.7981, 1.345, 356.158, 180.194, 'Rp+T', 'gba', 'point', ObjInCamPose)
pose_compose (CamInBasePose, ObjInCamPose, ObjInBasePose)

* 4.准备
要抓取的*物体的姿态估计所需的附加数据
* 
*确定测量平面的姿态

set_origin_pose (ObjInCamPoseRef, 0, 0, CalibrationPlateThickness - ObjectThickness,
 MPInCamPose)

* 5.在拾放应用中写下使用它们的结果
* - 手眼校准的结果

write_pose (CamInBasePose, 'cam_in_base_pose.dat')

从原始图像中估计要抓取的物体的姿势估计所需的数据

write_cam_par (CameraParam, 'camera_parameters.dat')
write_pose (MPInCamPose, 'measurement_plane_in_cam_pose.dat')

本文出自勇哥的网站《少有人走的路》wwww.skcircle.com，转载请注明出处！讨论可扫码加群：