在机械臂与摄像头进行协同工作时，为了使得相机（亦即机器人的眼）与机器人（亦即机器人的手）坐标系之间建立关系就必须要对机器人与相机坐标系进行标定，该标定过程也就叫做手眼标定。
由于机器人是运动的，因此标定的主要是相对不变的量，大概分为以下两种：
- 对于eye-to-hand情况，机器人手眼标定即标定得到机器人基座与相机之间的坐标变换关系。对于eye-in-hand情况，机器人手眼标定即标定得到机器人末端与相机之间的坐标变换关系；
如图，空间中有一个点P，以及三个坐标系A,B,C，P在A,B,C坐标系下点对应的坐标分别是a,b,c。那么有如下运算关系：
- ,M是变换矩阵，R是3x3旋转矩阵，T是3x1平移矩阵，。因此有。通过这个公式可以看出，要反过来的话，只需要求一个逆即可
对机械臂来说一共有四个坐标系。基底（base）、末端（end）、相机（camera）、标定板（board） 眼在手内
眼在手外 总结无论是眼在手上还是眼在手外，最终都可以化解成 AX=XB,的形式。拍摄N张图片则可以得到N-1个方程。（一般要n>10效果好点）问题变转化成先去求N-1个A,B，再通过A，B联立解得X两种的方程区别不大，A都是类似的，不过要主要的是B。一个是end to base，一个是base to end。不过问题也不大，参考前面的推导，其实也就是求一个逆而已同理，求得的X进行求逆，就可以得到坐标系变换的逆变换
3.X求解cv.calibrateHandEye()是核心的函数，该函数很适合眼在手上的标定，之后的项目都是眼在手上的标定，因此以下都是眼在手上的说明：
输入参数一：R_gripper2base: 输入参数二： R_target2cam：
输出：R_cam2gripper：
因此，只需要把N对参数作为输入，则可以得到标定
求解参数一、可以由机械臂内部直接得到。（位姿一般为6个参数，。通过这六个参数便能得到）
求解参数二objectpoints:以棋盘格左上角为原点建立，由于棋盘格大小已知，因此可以建立
imagepoints:用cv2.findchessboardCorner函数找到
（真实点与图像点是一一对应的关系）
相机内参、相机畸变：这个由说明手册提供或者自己进行相机标定
代码实现import cv2
import numpy as np
import glob
from math import *
import pandas as pd
import os
import json
K = np.array([[1373.5709486833512, 0.0, 976.1956272790611],
[0.0, 1384.2380687124548, 670.6014863714781],
[0, 0, 1]], dtype=np.float64) # 内参
dist = np.array(
[0.00968424149204635, 0.025415217557397604, 0.0009639920437816544, -0.00046553191699270647, -0.01217641000574228])
chess_board_x_num = 11 # 棋盘格x方向格子数
chess_board_y_num = 8 # 棋盘格y方向格子数
chess_board_len = 30 # 单位棋盘格长度,mm
global retval
# 用于根据欧拉角计算旋转矩阵
def myRPY2R_robot(x, y, z):
Rx = np.array([[1, 0, 0], [0, cos(x), -sin(x)], [0, sin(x), cos(x)]])
Ry = np.array([[cos(y), 0, sin(y)], [0, 1, 0], [-sin(y), 0, cos(y)]])
Rz = np.array([[cos(z), -sin(z), 0], [sin(z), cos(z), 0], [0, 0, 1]])
R = Rz @ Ry @ Rx
return R
# 用于根据位姿计算变换矩阵(end to base)
def pose_robot(x, y, z, Tx, Ty, Tz):
thetaX = x / 180 * pi
thetaY = y / 180 * pi
thetaZ = z / 180 * pi
R = myRPY2R_robot(thetaX, thetaY, thetaZ)
t = np.array([[Tx], [Ty], [Tz]])
RT1 = np.column_stack([R, t]) # 列合并
RT1 = np.row_stack((RT1, np.array([0, 0, 0, 1])))
# RT1=np.linalg.inv(RT1)
return RT1
# 用来从棋盘格图片得到相机外参(board to camera)
def get_RT_from_chessboard(img_path, chess_board_x_num, chess_board_y_num, K, chess_board_len,dist):
global tvec
'''
:param img_path: 读取图片路径
:param chess_board_x_num: 棋盘格x方向格子数
:param chess_board_y_num: 棋盘格y方向格子数
:param K: 相机内参
:param chess_board_len: 单位棋盘格长度,mm
:return: 相机外参
'''
img = cv2.imread(img_path)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = gray.shape[::-1]
ret, corners = cv2.findChessboardCorners(gray, (chess_board_x_num, chess_board_y_num), None)
corner_points = np.zeros((2, corners.shape[0]), dtype=np.float64)
for i in range(corners.shape[0]):
corner_points[:, i] = corners[i, 0, :]
object_points = np.zeros((3, chess_board_x_num * chess_board_y_num), dtype=np.float64)
flag = 0
for i in range(chess_board_y_num):
for j in range(chess_board_x_num):
object_points[:2, flag] = np.array([(11 - j - 1) * chess_board_len, (8 - i - 1) * chess_board_len])
flag += 1
retval, rvec, tvec = cv2.solvePnP(object_points.T, corner_points.T, K, distCoeffs=dist)
RT = np.column_stack(((cv2.Rodrigues(rvec))[0], tvec))
RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
return RT
def set_setting_file(path):
try:
with open(path) as json_file:
setting_dict_string = json_file.readline()[:-2]
return json.loads(setting_dict_string)
except:
return {}
def read_xyz_Rxyz(good_picture, folder, ele_data_dict={}):
"""用于读取每个图片对应的json文件中，对应的"""
for i in good_picture:
ele_data_dict[i] = set_setting_file(folder + '/' + str(i) + '.json')
return ele_data_dict
folder = r"save_cail_data" # 棋盘格图片存放文件夹
'''
这个地方很奇怪的特点，有些棋盘格点检测得出来，有些检测不了，可以通过函数get_RT_from_chessboard的运行时间来判断
'''
good_picture = [3, 4, 5, 7, 8, 9, 10, 11, 12, 13, 14] # 存放可以检测出棋盘格角点的图片
ele_data_dict = read_xyz_Rxyz(good_picture, folder)
file_num = len(good_picture)
# 计算board to cam 变换矩阵
R_all_chess_to_cam_1 = []
T_all_chess_to_cam_1 = []
for i in good_picture:
image_path = folder + '/' + str(i) + '.bmp'
RT = get_RT_from_chessboard(image_path, chess_board_x_num, chess_board_y_num, K, chess_board_len, dist)
# RT=np.linalg.inv(RT)
R_all_chess_to_cam_1.append(RT[:3, :3])
T_all_chess_to_cam_1.append(RT[:3, 3].reshape((3, 1)))
# 计算end to base变换矩阵
R_all_end_to_base_1 = []
T_all_end_to_base_1 = []
for i in good_picture:
RT = pose_robot(ele_data_dict[i]["x"], ele_data_dict[i]["y"], ele_data_dict[i]["z"],
ele_data_dict[i]["Rx"], ele_data_dict[i]["Ry"], ele_data_dict[i]["Rz"])
R_all_end_to_base_1.append(RT[:3, :3])
T_all_end_to_base_1.append(RT[:3, 3].reshape((3, 1)))
print("n")
R, T = cv2.calibrateHandEye(R_all_end_to_base_1, T_all_end_to_base_1, R_all_chess_to_cam_1,
T_all_chess_to_cam_1) # 手眼标定
print("手眼矩阵分解得到的旋转矩阵")
print(R)
print("n")
print("手眼矩阵分解得到的平移矩阵")
print(T)
RT = np.column_stack((R, T))
RT = np.row_stack((RT, np.array([0, 0, 0, 1]))) # 即为cam to end变换矩阵
print("n")
print('相机相对于末端的变换矩阵为(cam to end)：')
print(RT)
# # 结果验证，原则上来说，每次结果相差较小
# for i in range(len(good_picture)):
# # 得到机械手末端到基座的变换矩阵，通过机械手末端到基座的旋转矩阵与平移向量先按列合并，然后按行合并形成变换矩阵格式
# RT_end_to_base = np.column_stack((R_all_end_to_base_1[i], T_all_end_to_base_1[i]))
# RT_end_to_base = np.row_stack((RT_end_to_base, np.array([0, 0, 0, 1])))
# # print(RT_end_to_base)
#
# # 标定版相对于相机的齐次矩阵
# RT_chess_to_cam = np.column_stack((R_all_chess_to_cam_1[i], T_all_chess_to_cam_1[i]))
# RT_chess_to_cam = np.row_stack((RT_chess_to_cam, np.array([0, 0, 0, 1])))
# # print(RT_chess_to_cam)
#
# # 手眼标定变换矩阵
# RT_cam_to_end = np.column_stack((R, T))
# RT_cam_to_end = np.row_stack((RT_cam_to_end, np.array([0, 0, 0, 1])))
# # print(RT_cam_to_end)
#
# # 即为固定的棋盘格相对于机器人基坐标系位姿
#
# RT_chess_to_base = RT_end_to_base @ RT_cam_to_end @ RT_chess_to_cam
# print('第', i, '次')
# print(RT_chess_to_base)
