设计思路:

1.根据张正友标定法获取图片的角点(角点方向从左上到右下)

2.根据相机内部的Left_correct_2_RGB的旋转平移矩阵将角点移动到深度相机上

3.通过PnP求解获取标定板到深度相机的旋转平移矩阵

4.直接获取机器人的x,y,z

5.通过欧拉角获取机器人的旋转矩阵(xyz)

6.使用calibrateHandEye来求解基底到相机直接的旋转平移矩阵

代码如下(改动过很多次了)

import cv2
import numpy as np
import pandas as pd
from math import *

#########################配置数据##################################
# RGB相机内参
RGBK = [3510.563, 0, 1835.4205, 0, 3510.8389, 1548.5492, 0, 0, 1]
RGBK = np.array(RGBK, np.float32).reshape(3, 3)
# RGB相机畸变系数
RGBdistCoeffs = np.array([-0.08198119,0.12729025,0.00047783172,0.0001023462,0], dtype=np.float64)
# 左相机到RGB相机的旋转向量
R_Left_correct_2_RGB = [0.99999559, -0.0026270933, 0.00141842, 0.0026299292, 0.99999452, -0.0020251102, -0.0013965141,
                        0.002028788, 0.99999696]
R_Left_correct_2_RGB = np.array(R_Left_correct_2_RGB,np.float32).reshape(3,3)
# 左相机到RGB相机的平移向量
T_Left_correct_2_RGB = [-130.87146, -0.51509166, -2.737951]
# 棋盘格x,y,mm
chress_border = [5,6, 30]
imagePath = './Test11/'
RobotPathName = './RobotPoint11.xlsx'
criteria=(cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER,30,0.001)
####################################################################

###################左相机到RGB相机的转换###############################
def LeftToRGB(x_left,y_left,z_left):
    P_Left = np.array([x_left, y_left, z_left], dtype=np.float32)
    P_RGB = np.dot(R_Left_correct_2_RGB,P_Left)+T_Left_correct_2_RGB
    return P_RGB
####################################################################

###################提取BoardTRCamera##############################

'''
定义需要使用到一下数据 棋盘格到相机的旋转矩阵,棋盘格到相机的平移矩阵
'''
BoardTRCamera=np.eye(4)
BoardTCamera=[]
BoardRCamera=[]
LeftToRGBArray=[]
'''
显示棋盘格上标定的图像
'''
def DispalyImage(img,corners,ret,col,row):
    cv2.namedWindow('img',cv2.WINDOW_NORMAL)
    cv2.resizeWindow('img',600,700)
    cv2.drawChessboardCorners(img,(col,row),corners,ret)
    cv2.imshow('img',img)
    cv2.waitKey(0)
    cv2.destroyWindow('img')
    return
'''
获取棋盘格文件中的坐标数据
'''
def GetImageALLPoint(imagePath,distance,col,row):
    object_point = np.zeros((col*row, 3), np.float64)
    object_point[:, :2] = np.mgrid[0:col, 0:row].T.reshape(-1, 2)*distance
    for i in range(1, 32):
        if i < 10:
            strpath = imagePath + "image_000" + str(i) + ".bmp"
        else:
            strpath = imagePath + "image_00" + str(i) + ".bmp"
        if i == 12 or i == 14 or i==23 or i==16:
            continue
        img = cv2.imread(strpath)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        ret, corners = cv2.findChessboardCorners(gray, (col,row), None)
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        print(strpath)
        #DispalyImage(img, corners, ret,col,row)
        # 将Left中获取的corners中的坐标传递到RGB中
        if ret:
            retval,rvec,camtbroad =  cv2.solvePnP(object_point,corners2.reshape(30,2),RGBK,distCoeffs=RGBdistCoeffs)
            print(rvec)
            #将获取的旋转变量转换成3*3的矩阵
            #camrbroad, _ = cv2.Rodrigues(rvec)
            BoardTCamera.append(camtbroad)
            BoardRCamera.append(rvec)
        #转RGB
        # for i in range(0,col*row):
        #     RGBx,RGBy,RGBZ=LeftToRGB(corners2[i][0][0], corners2[i][0][1],0)
        #     LeftToRGBArray.append(RGBx)
        #     LeftToRGBArray.append(RGBy)
        # if ret:
        #     retval,rvec,camtbroad =  cv2.solvePnP(object_point,np.array(LeftToRGBArray).reshape(30,1,2),RGBK,distCoeffs=RGBdistCoeffs)
        #     LeftToRGBArray.clear()
        #     #将获取的旋转变量转换成3*3的矩阵
        #     camrbroad, _ = cv2.Rodrigues(rvec)
        #     BoardTCamera.append(camtbroad)
        #     BoardRCamera.append(rvec)
#########################################################################
###################提取BoardTRCamera##############################
GetImageALLPoint(imagePath,chress_border[2],chress_border[0],chress_border[1])
#########################################################################
######################获取机器人的坐标(x,y,z,ax,ay,az)#####################
#############旋转矩阵转换成欧拉角###################
def rotation_matrix_to_euler_angles(R):
    sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
    singular = sy < 1e-6

    if not singular:
        x = np.arctan2(R[2, 1], R[2, 2])
        y = np.arctan2(-R[2, 0], sy)
        z = np.arctan2(R[1, 0], R[0, 0])
    else:
        x = np.arctan2(-R[1, 2], R[1, 1])
        y = np.arctan2(-R[2, 0], sy)
        z = 0

    return z, y, x  # ZYX order: yaw, pitch, roll
########################################################3
def rotation_vector_to_rotation_matrix(rotation_vector):
    # 提取旋转角度（弧度）和旋转轴
    theta = np.linalg.norm(rotation_vector)
    axis = rotation_vector / (theta + 1e-7)  # 避免除以零，加入小常数

    # 构建反对称矩阵（叉积矩阵）
    axis_x, axis_y, axis_z = axis
    N = np.array([
        [0, -axis_z, axis_y],
        [axis_z, 0, -axis_x],
        [-axis_y, axis_x, 0]
    ])

    # 应用罗德里格斯旋转公式
    I = np.eye(3)  # 3x3单位矩阵
    R = np.cos(theta) * I + (1 - np.cos(theta)) * np.dot(axis, axis.T) + np.sin(theta) * N

    # 或者使用更简洁的公式（基于旋转向量的直接构造）
    # 但注意上面的公式在数学上是等价的，只是实现方式不同
    # R = np.identity(3) + np.sin(theta) * N + (1 - np.cos(theta)) * np.outer(axis, axis)

    return R
'''
定义基础变量
'''
BaseTEnd=[]
BaseREnd=[]
'''
从文件中读取数据
'''
def LoadExcel(path,i):
    Context = pd.read_excel(path)
    return Context.iloc[i-1]['x'], Context.iloc[i-1]['y'], Context.iloc[i-1]['z'], Context.iloc[i-1]['ax'],Context.iloc[i-1]['ay'], Context.iloc[i-1]['az']
'''
将欧拉角转换成四元矩阵
'''
def Eular_Angle_to_Quaternion(x, y, z):
    # 将欧拉角转换为四元数
    cy = cos(z * 0.5)
    sy = sin(z * 0.5)
    cp = cos(y * 0.5)
    sp = sin(y * 0.5)
    cr = cos(x * 0.5)
    sr = sin(x * 0.5)

    w = cy * cp * cr + sy * sp * sr
    x = cy * cp * sr - sy * sp * cr
    y = sy * cp * sr + cy * sp * cr
    z = sy * cp * cr - cy * sp * sr

    return np.array([w, x, y, z])
def Quaternion_to_RotationMatrix(q):
    # 将四元数转换为旋转矩阵
    w, x, y, z = q
    return np.array([
        [1 - 2 * y * y - 2 * z * z, 2 * x * y - 2 * z * w, 2 * x * z + 2 * y * w],
        [2 * x * y + 2 * z * w, 1 - 2 * x * x - 2 * z * z, 2 * y * z - 2 * x * w],
        [2 * x * z - 2 * y * w, 2 * y * z + 2 * x * w, 1 - 2 * x * x - 2 * y * y]
    ])
'''
通过欧拉角的转换关系来获取旋转矩阵
'''
def Eular_Angle(x,y,z,seq):
    rotx = np.array([[1,0,0],[0,cos(x),-sin(x)],[0,sin(x),cos(x)]])
    roty = np.array([[cos(y),0,sin(y)],[0,1,0],[-sin(y),0,cos(y)]])
    rotz = np.array([[cos(z),-sin(z),0],[sin(z),cos(z),0],[0,0,1]])
    if seq == 'zyx':
        R = rotx @ roty @ rotz
    if seq == 'yzx':
        R = rotx @ rotz @ roty
    if seq == 'zxy':
        R = roty @ rotx @ rotz
    if seq == 'yxz':
        R = rotz @ rotx @ roty
    if seq == 'xyz':
        R = rotz @ roty @ rotx
    if seq == 'xzy':
        R = roty @ rotz @ rotx
    return R
'''
获取平移矩阵和旋转矩阵
'''
def GetAllRobotPoint(path):
    for i in range(1,32):
        if i == 12 or i == 14  or i==23 or i==16:
            continue
        x, y, z, ax, ay, az = LoadExcel(path, i)
        print(str(x)+","+str(y)+","+str(z)+","+str(ax)+","+str(ay)+","+str(az))
        thetax = ax / 180 * pi
        thetay = ay / 180 * pi
        thetaz = az / 180 * pi
        #R = Eular_Angle(thetax, thetay, thetaz, 'xyz')
        R = Quaternion_to_RotationMatrix(Eular_Angle_to_Quaternion(thetax, thetay, thetaz))
        T = np.array([[x],[y],[z]])
        t_base2end = T.T
        R_base2end = R.T
        #################################基于四元齐次矩阵的逆#########################
        #T = np.array([x, y, z]).T
        #四元齐次转换
        # pose_matrix = np.eye(4)
        # pose_matrix[:3, :3] = R
        # pose_matrix[:3, 3] = T
        # 完成BaseRTEnd到EndRTBase转换方式1
        # pose_matrix = np.eye(4)
        # pose_matrix[:3, :3] = -R.T
        # pose_matrix[:3, 3] = T
        #完成BaseEnd到EndRTBase转换方式2
        # pose_matrix_inv = np.linalg.inv(pose_matrix)
        # R_base2end = pose_matrix_inv[:3, :3]
        # t_base2end = pose_matrix_inv[:3, 3]
        ########################################################################
        print(t_base2end)
        BaseTEnd.append(t_base2end)
        BaseREnd.append(R_base2end)
#########################################################################
GetAllRobotPoint(RobotPathName)
BaseRCamera,BaseTCamera = cv2.calibrateHandEye(BaseREnd,BaseTEnd,BoardRCamera,BoardTCamera,cv2.CALIB_HAND_EYE_TSAI)
print('测试')
print(BaseTCamera)
print(BaseRCamera)
yaw_deg = np.degrees(rotation_matrix_to_euler_angles(BaseRCamera)[0])
pitch_deg = np.degrees(rotation_matrix_to_euler_angles(BaseRCamera)[1])
roll_deg = np.degrees(rotation_matrix_to_euler_angles(BaseRCamera)[2])
print(yaw_deg)
print(pitch_deg)
print(roll_deg)