From 563ad9bb672de2cad6254a143629aef573955f2c Mon Sep 17 00:00:00 2001 From: Dream Date: Tue, 24 Sep 2024 11:12:02 +0000 Subject: [PATCH] =?UTF-8?q?update=20=E6=B7=BB=E5=8A=A0=E8=87=AA=E5=8A=A8?= =?UTF-8?q?=E6=A0=87=E5=AE=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Trace/calibrate.py | 267 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 267 insertions(+) create mode 100644 Trace/calibrate.py diff --git a/Trace/calibrate.py b/Trace/calibrate.py new file mode 100644 index 0000000..7e3a39b --- /dev/null +++ b/Trace/calibrate.py @@ -0,0 +1,267 @@ +import sys +import time + +import numpy as np +import cv2 +import glob +from math import * +import pandas as pd +from pathlib import Path +from COM.COM_Robot import RobotClient +from app import MainWindow +from Vision.tool.CameraRVC import camera +from Model.RobotModel import DataAddress,MoveType +from PySide6.QtWidgets import QMainWindow, QApplication, QLineEdit + + +# 用于根据欧拉角计算旋转矩阵 +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 = Rx@Ry@Rz + R = Rz @ Ry @ Rx + return R + + +# 用于根据位姿计算变换矩阵 +def pose_robot(x, y, z, Tx, Ty, Tz): + thetaX = Tx / 180 * pi + thetaY = Ty / 180 * pi + thetaZ = Tz / 180 * pi + R = myRPY2R_robot(thetaX, thetaY, thetaZ) + t = np.array([[x], [y], [z]]) + RT1 = np.column_stack([R, t]) # 列合并 + RT1 = np.row_stack((RT1, np.array([0, 0, 0, 1]))) + return RT1 + + +''' +#用来从棋盘格图片得到相机外参 +:param img_path: 读取图片路径 +:param chess_board_x_num: 棋盘格x方向格子数 +:param chess_board_y_num: 棋盘格y方向格子数 +:param chess_board_len: 单位棋盘格长度,mm +:param cam_mtx: 相机内参 +:param cam_dist: 相机畸变参数 +:return: 相机外参 +''' + + +def get_RT_from_chessboard(img_path, chess_board_x_num, chess_board_y_num, chess_board_len, cam_mtx, cam_dist): + # termination criteria + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) + # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) + # 标定板世界坐标 + objp = np.zeros((chess_board_y_num * chess_board_x_num, 3), np.float32) + for m in range(chess_board_y_num): + for n in range(chess_board_x_num): + objp[m * chess_board_x_num + n] = [n * chess_board_len, m * chess_board_len, 0] + + # Arrays to store object points and image points from all the images. + objpoints = [] # 3d point in real world space + imgpoints = [] # 2d points in image plane. + + img = cv2.imread(img_path) + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + # Find the chess board corners + ret, corners = cv2.findChessboardCorners(gray, (chess_board_x_num, chess_board_y_num), None) + # If found, add object points, image points (after refining them) + if ret == True: + objpoints.append(objp) + corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) + imgpoints.append(corners2) + + retval, rvec, tvec = cv2.solvePnP(objpoints[0], imgpoints[0], cam_mtx, cam_dist) + # print(rvec.reshape((1,3))) + + RT = np.column_stack(((cv2.Rodrigues(rvec))[0], tvec)) + RT = np.row_stack((RT, np.array([0, 0, 0, 1]))) + # print(RT) + return RT + return 0 + + +# 计算board to cam 变换矩阵 +# img_path = "./hand_eye" 棋盘格存放文件夹 +def get_chess_to_cam_mtx(img_path, chess_board_x_num, chess_board_y_num, chess_board_len, cam_mtx, cam_dist): + img_path += "/*.jpg" + R_all_chess_to_cam_1 = [] + T_all_chess_to_cam_1 = [] + images = glob.glob(img_path) + for fname in images: + RT = get_RT_from_chessboard(fname, chess_board_x_num, chess_board_y_num, chess_board_len, cam_mtx, cam_dist) + R_all_chess_to_cam_1.append(RT[:3, :3]) + T_all_chess_to_cam_1.append(RT[:3, 3].reshape((3, 1))) + return R_all_chess_to_cam_1, T_all_chess_to_cam_1 + +def save_images(image, path, img_num): + """保存拍摄的图片""" + img_path = path / f'image_{img_num}.png' + cv2.imwrite(str(img_path), image) + +def save_pic_and_excel(): # 将机械臂的末端位姿保存在excel文件里,以及拍摄的图片保存在pic里# 动，拍照，保存 + robot.send_switch_tool_command() + # 定义初始关节位置 + initial_joint_angles = [0, 0, 0, 0, 5, 0] # 初始关节角度 + move_step = 5 # 每次移动5度 + move_range = [-15, 15] # 关节移动的阈值范围 + + # 保存 Excel 的路径和文件 + excel_path = Path("robot_calibration_data.xlsx") + + # 创建 DataFrame 来存储机械臂坐标 + data = { + "x1": [], "x2": [], "x3": [],"x4": [],"x5": [],"x6": [],"image_path": [] + } + + img_num = 0 # 图片编号 + image_save_path = Path("calibration_images") + image_save_path.mkdir(exist_ok=True) # 创建目录保存图像 + + # 遍历各轴，依次移动 + for axis in range(6): + if axis==4: + continue + for move in range(move_range[0], move_range[1] + 1, move_step): + # 计算目标关节位置 + joint_angles = initial_joint_angles.copy() + joint_angles[axis] = move + + # 移动机械臂到指定位置 + robot.send_position_command(*joint_angles,MoveType.AXIS) + # 拍照 + time.sleep(10) + _,rgb_image = ca.get_img() + + # 保存图片 + save_images(rgb_image, image_save_path, img_num) + img_path = image_save_path / f'image_{img_num}.png' + img_num += 1 + + # 获取当前机械臂的世界坐标 + real_position = robot.robotClient.status_model.getRealPosition() + x1 = real_position.X + x2 = real_position.Y + x3 = real_position.Z + x4 = real_position.U + x5 = real_position.V + x6 = real_position.W + + + # 保存数据到DataFrame + + data["x1"].append(x1) + data["x2"].append(x2) + data["x3"].append(x3) + data["x4"].append(x4) + data["x5"].append(x5) + data["x6"].append(x6) + data["image_path"].append(str(img_path)) + + # 将数据写入Excel + df = pd.DataFrame(data) + df.to_excel(excel_path, index=False) + print(f"数据已保存至 {excel_path}") + + +# 计算end to base变换矩阵 +# path = "./hand_eye" 棋盘格和坐标文件存放文件夹 +def get_end_to_base_mtx(path): + img_path = path + "image_/*.png" + coord_file_path = path + '/robot_calibration_data.xlsx' # 从记录文件读取机器人六个位姿 + sheet_1 = pd.read_excel(coord_file_path) + R_all_end_to_base_1 = [] + T_all_end_to_base_1 = [] + # print(sheet_1.iloc[0]['ax']) + images = glob.glob(img_path) + # print(len(images)) + for i in range(len(images)): + pose_str = sheet_1.values[i][0] + pose_array = [float(num) for num in pose_str[1:-1].split(',')] + # print(pose_array) + RT = pose_robot(pose_array[0], pose_array[1], pose_array[2], pose_array[3], pose_array[4], pose_array[5]) + R_all_end_to_base_1.append(RT[:3, :3]) + T_all_end_to_base_1.append(RT[:3, 3].reshape((3, 1))) + return R_all_end_to_base_1, T_all_end_to_base_1 + + +if __name__ == "__main__": + print("手眼标定测试\n") + app = QApplication(sys.argv) + robot = MainWindow() + ca = camera() + dataaddress = DataAddress() + + + # 相机内参 + cam_mtx = np.array([[2402.1018066406, 0.0000000000, 739.7069091797], + [0.0000000000, 2401.7873535156, 584.7304687500], + [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]], dtype=np.float64) + + # 畸变系数 + cam_dist = np.array([-0.0424814112, 0.2438604534, -0.3833343089, -0.3833343089, 0.0007602088], + dtype=np.float64) + save_pic_and_excel() + chess_board_x_num = 12 - 1 # 标定板长度 + chess_board_y_num = 9 - 1 + chess_board_len = 30 / 1000 + path = "./calibration_images" + + R_all_chess_to_cam_1, T_all_chess_to_cam_1 = get_chess_to_cam_mtx(path, chess_board_x_num, chess_board_y_num, + chess_board_len, cam_mtx, cam_dist) + R_all_end_to_base_1, T_all_end_to_base_1 = get_end_to_base_mtx(path) + 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, + cv2.CALIB_HAND_EYE_TSAI) # 手眼标定 + RT = np.column_stack((R, T)) + RT = np.row_stack((RT, np.array([0, 0, 0, 1]))) # 即为cam to end变换矩阵00 + + print('相机相对于末端的变换矩阵为：') + print(RT) + + chess_base_mtx = np.zeros((4, 4), np.float32) + + # 结果验证,原则上来说,每次结果相差较小 + for i in range(20): + 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]))) + + 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]))) + + + 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]))) + R_cam_to_end = RT_cam_to_end[:3, :3] + T_cam_to_end = RT_cam_to_end[:3, 3] + rotation_vector, _ = cv2.Rodrigues(R_cam_to_end) + print("11111111", rotation_vector) + print("222222222", R_cam_to_end) + thetaX = rotation_vector[0][0] * 180 / pi + thetaY = rotation_vector[1][0] * 180 / pi + thetaZ = rotation_vector[2][0] * 180 / pi + # print(RT_cam_to_end) + + RT_chess_to_base = RT_end_to_base @ RT_cam_to_end @ RT_chess_to_cam # 即为固定的棋盘格相对于机器人基坐标系位姿 + + print('') + print(RT_chess_to_base) + chess_base_mtx += RT_chess_to_base + + chess_pose = np.array([0, 0, 0, 1]) + # 转换棋盘坐标到像素坐标 + world_pose = RT_chess_to_base @ chess_pose.T + cam_pose = np.linalg.inv(RT_end_to_base @ RT_cam_to_end) @ world_pose + print(cam_pose) + # cam_pose = RT_chess_to_cam@chess_pose.T + zero3 = np.zeros((3, 1)) + imgTocam_matrix = np.hstack((cam_mtx, zero3)) # 3*4 矩阵 + imgTocam_matrix = np.row_stack((imgTocam_matrix, np.array([0, 0, 0, 1]))) + img_pose = imgTocam_matrix @ (cam_pose / cam_pose[2]) + + # 像素坐标转换为世界坐标（机械臂基坐标） + cam_pose = np.linalg.inv(imgTocam_matrix) @ img_pose + cam_pose = cam_pose * (1 / cam_pose[3]) + world_pose = RT_end_to_base @ RT_cam_to_end @ cam_pose + + print(chess_base_mtx / 20) \ No newline at end of file