chuyiwen/AutoControlSystem-G
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
chuyiwen
2025-07-29 13:16:30 +08:00
commit dca51db4eb
145 changed files with 721268 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

218
Trace/calibrate_test.py Normal file
View File

@ -0,0 +1,218 @@
import sys
import time
import numpy as np
import cv2
import glob
from math import *
import pandas as pd
from pathlib import Path
import openpyxl
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)
# 可视化棋盘格角点检测结果
img = cv2.drawChessboardCorners(img, (chess_board_x_num, chess_board_y_num), corners2, ret)
cv2.imshow('Corners', img)
cv2.waitKey(500) # 显示500ms
retval, rvec, tvec = cv2.solvePnP(objpoints[0], imgpoints[0], cam_mtx, cam_dist)
RT = np.column_stack(((cv2.Rodrigues(rvec))[0], tvec))
RT = np.row_stack((RT, np.array([0, 0, 0, 1])))
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 += "/image_*.png"
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)
print("chesstocam",RT)
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 = [5, 0, 0, 0, -12, 0] # 初始关节角度
fixed_joint_positions = [
[0.000,0.000,-0.005,0.001,-68.752,0.000],
[13.944,-2.732,-0.007,0.001,-71.402,0.000],
[7.256,-2.732,-0.011,0.000,-71.402,-13.003],
[1.061,3.196,-4.484,0.000,-71.402,-13.003],
[1.061,-7.045,-4.486,8.935,-60.210,-13.003],
[-12.811,-7.045,-4.488,15.131,-60.210,-5.156],
[-12.811,3.929,-12.911,15.131,-62.695,-5.156],
[14.269,3.929,-12.915,-7.996,-62.695,-5.156],
[14.269,3.929,-9.102,-9.642,-66.652,6.868],
[14.269,10.147,-13.732,-9.642,-66.652,6.868],
[4.356,10.147,-13.736,-2.658,-66.652,6.868],
[4.356,10.147,-13.736,-2.658,-66.652,21.705],
[4.356,10.147,-3.741,-2.658,-74.334,21.705]
]
# 保存 Excel 的路径和文件
excel_path = Path("calibration_images//robot_calibration_data.xlsx")
# 创建 DataFrame 来存储机械臂坐标
data_list = []
img_num = 0 # 图片编号
image_save_path = Path("calibration_images")
image_save_path.mkdir(exist_ok=True) # 创建目录保存图像
# 遍历固定点位，依次移动机械臂
for joint_angles in fixed_joint_positions:
# 移动机械臂到指定位置
robot.send_position_command(*joint_angles, MoveType.AXIS)
time.sleep(20)
# 获取图像并保存
_, 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()
position = [real_position.X, real_position.Y, real_position.Z,
real_position.U, real_position.V, real_position.W]
# 将当前机械臂的坐标和图片路径存储为元组
data_list.append([position, str(img_path)])
# 将数据写入 Excel
df = pd.DataFrame(data_list, columns=["Position", "Image Path"])
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 + '/机械臂末端位姿.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])
print("end_base",RT)
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")
np.set_printoptions(suppress=True)
# 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 = 11 # 标定板长度
chess_board_y_num = 8
chess_board_len = 30
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)