267 lines
10 KiB
Python
267 lines
10 KiB
Python
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) |