3D视觉机械手眼标定仿真

对于3D相机引导机器人运动，通常包括Eye To Hand（相机固定安装）和Eye In Hand（相机装在robot上）。相机出厂完成内参标定（即相机能直接输出产品在相机坐标系下的xyz，单位mm，也包括姿态）。但要相机联合机器人运动，需要完成相机和机器人的手眼标定。

对于Eye In Hand，通常让机器人带着相机对标定板不同姿态进行拍摄，记录对应的tool0位姿和特征在相机坐标系下的位姿（由于标定棋盘格的每格间距已知，可以通过找到棋盘格上所有交叉点并计算各自关系后，找到棋盘格角点在相机坐标系下的xyz，单位mm）。

 

 

 

有了对应机器人不同位姿时的tool0位姿和特征在相机坐标系的位姿，就可以计算tool0到相机坐标系的位姿了（tool0到相机关系，标定板到机器人base的关系不变）。具体即求解AX=XB。相关资料可以网络搜索。

本仿真假设，已经可以获得特征在相机坐标系下的位姿，基于这些位姿，进行标定（即完成AX =XB的求解）。

相关Smart组件

PROC mainCalib()  VAR robtarget p:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];  Open "HOME:"\File:="cam1.txt",iodev1\write;  FOR i FROM 0 TO 13 DO      ! 机器人走p1000-p1013 共14个标定点  GetDataVal "p"+ValToStr(1000+i),p;  Movejp,v1000,fine,tool0\WObj:=wobj0;  waittime 0.5;  rCam2;  ENDFOR  close iodev1;  stop;ENDPROC

PROC rCam2()  VAR robtarget pCurr:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];  VAR pose pRob;  VAR string s;  waittime 0.5;  pulsedo do_CamCalib;  waittime 0.5;  camData:=[pCam,oCam];  write iodev1,pose2String(camData);  pCurr:=CRobT(\Tool:=tool0);  pRob:=[pCurr.trans,pCurr.rot];  write iodev1,s+pose2String(pRob);  ! 记录对应的相机坐标系下的特征位姿和机器人tool0位姿到txtENDPROC

FUNC string pose2String(pose p)  VAR num rx;  VAR num ry;  VAR num rz;  VAR string s;rx:=EulerZYX(\x,p.rot);  ry:=EulerZYX(\y,p.rot);  rz:=EulerZYX(\z,p.rot);s:=numtostr(p.trans.x,3)+","+numtostr(p.trans.y,3)+","+numtostr(p.trans.z,3)+","+numtostr(rx,3)+","+numtostr(ry,3)+","+numtostr(rz,3);RETURN s;ENDFUNC

使用记录的14组相机坐标系下的特征位姿和机器人tool0的位姿数据，使用opencv中的calibrateHandEye函数求解，求解方法选取CALIB_HAND_EYE_TSAI。

下图为计算结果，左侧为理论值，右侧为计算值。求解正确。

import cv2import numpy as npimport transforms3d

def pose_vectors_to_end2base_transforms(pose_vectors):  # 提取旋转矩阵和平移向量 R_end2bases = []t_end2bases = []

# 迭代遍历每个位姿的旋转矩阵和平移向量  for pose_vector in pose_vectors:  # 提取旋转矩阵和平移向量 R_end2base = euler_to_rotation_matrix(pose_vector[3], pose_vector[4], pose_vector[5]) t_end2base = pose_vector[:3]

# 提取旋转矩阵和平移向量 R_end2bases.append(R_end2base)t_end2bases.append(t_end2base)    return R_end2bases, t_end2bases

def euler_to_rotation_matrix(rx, ry, rz, unit='deg'):  # rx, ry, rz是欧拉角，单位是度  ''' 将欧拉角转换为旋转矩阵：R = Rz * Ry * Rx :param rx: x轴旋转角度 :param ry: y轴旋转角度 :param rz: z轴旋转角度 :param unit: 角度单位，'deg'表示角度，'rad'表示弧度 :return: 旋转矩阵 '''  if unit == 'deg':  # 把角度转换为弧度 rx = np.radians(rx) ry = np.radians(ry) rz = np.radians(rz)

# 计算旋转矩阵Rz 、 Ry 、 Rx Rx = transforms3d.axangles.axangle2mat([1, 0, 0], rx) Ry = transforms3d.axangles.axangle2mat([0, 1, 0], ry) Rz = transforms3d.axangles.axangle2mat([0, 0, 1], rz)

# 计算旋转矩阵R = Rz * Ry * Rx    rotation_matrix = np.dot(Rz, np.dot(Ry, Rx))    return rotation_matrix

# 输入位姿数据，单位mm和deg，rx,ry,rzcam_vectors = np.array([[81.577, -150.0, 457.1, 180.0, 0.0, 180.0],[81.577, -121.352, 508.979, 156.943, -0.0, -180.0], [241.708, -36.401, 539.286, 149.556, -13.172, -174.283], [290.517, -64.324, 508.865, 162.734, -28.472, 150.013], [69.469, -339.806, 476.097, -140.116, -26.109, 76.111], [190.451, -362.69, 365.032, -165.512, -44.629, 118.144], [33.723, -136.956, 462.007, -169.627, -8.555, 126.873], [-29.059, -178.962, 461.6, -175.479, -12.176, 62.066], [273.806, 49.065, 316.482, -134.726, -38.125, 156.817], [251.463, 35.649, 519.906, 159.112, -39.7, 179.857], [487.189, 1.759, 298.214, -164.971, -34.483, 127.282], [183.242, -129.315, 614.898, 142.775, -39.7, 179.857], [87.772, -91.29, 581.099, 150.603, -1.757, 140.143], [-3.673, -373.158, 422.714, 175.192, -29.084, 63.077]])

pose_vectors = np.array([[451.0, 0.0, 807.1, 180.0, -0.0, -180.0], [451.0, -225.699, 807.1, 156.943, 0.0, 180.0], [491.727, -371.831, 807.1, 150.003, 14.239, 178.285], [310.387, -371.831, 807.1, 150.003, 14.239, 141.768], [310.387, 234.722, 807.1, 145.041, 32.781, 75.145], [310.387, 48.287, 807.1, 145.041, 32.781, 113.533], [310.387, 48.287, 807.1, 179.469, 13.406, 127.589], [310.387, 48.287, 807.1, 167.128, -1.653, 62.735], [310.387, 48.287, 807.1, -138.95, 42.771, -170.102],[230.439, -373.824, 807.1, 153.523, 36.577, 163.351], [344.305, -373.824, 807.1, 160.75, 32.516, 126.308], [117.59, -373.824, 807.1, 135.302, 30.472, 153.214], [381.339, -373.824, 807.1, 155.66, -17.102, 143.396], [339.278, -136.568, 807.1, 155.66, -17.102, 65.543]])# 求解手眼标定# R_end2bases：机械臂末端相对于机械臂基座的旋转矩阵# t_end2bases：机械臂末端相对于机械臂基座的平移向量R_end2bases, t_end2bases = pose_vectors_to_end2base_transforms(pose_vectors)

# R_board2cameras： 标定板在相机下的旋转矩阵# t_board2cameras ：标定板在相机下的平移向量R_board2cameras, t_board2cameras = pose_vectors_to_end2base_transforms(cam_vectors)# R_camera2end：相机相对于机械臂末端的旋转矩阵# t_camera2end：相机相对于机械臂末端的平移向量R_camera2end, t_camera2end = cv2.calibrateHandEye(R_end2bases, t_end2bases, R_board2cameras, t_board2cameras,method=cv2.CALIB_HAND_EYE_TSAI)

# 将旋转矩阵和平移向量组合成齐次位姿矩阵T_camera2end = np.eye(4)T_camera2end[:3, :3] = R_camera2endT_camera2end[:3, 3] = t_camera2end.reshape(3)

# 输出相机相对于机械臂末端的旋转矩阵和平移向量print("Camera to end rotation matrix:")print(R_camera2end)print("Camera to end translation vector:")print(t_camera2end)

# 输出相机相对于机械臂末端的位姿矩阵print("Camera to end pose matrix:")np.set_printoptions(suppress=True) # suppress参数用于禁用科学计数法print(T_camera2end)