手眼标定中摄像头不动机械臂动的算法代码

手眼标定（Hand-eye calibration）是机器人视觉中的一个重要问题，用于确定机器人末端执行器（如机械臂末端）和相机之间的变换关系，从而实现机器人与环境中物体的精确定位与操作。在手眼标定中，需要对相机和机械臂进行标定，其中相机标定主要是确定相机内参和外参，而机械臂标定则是确定机械臂的关节参数和DH参数等。

在手眼标定中，一般采用相机和机械臂的运动来获取一组数据，然后通过计算来确定相机和机械臂之间的变换关系。其中，在相机和机械臂运动的过程中，相机可以不动，而机械臂动，也可以相机动，而机械臂不动，或者两者同时运动。不同的情况需要采用不同的算法来计算手眼标定。

下面是机械臂不动，相机运动的手眼标定算法代码示例：

import numpy as np
from scipy.spatial.transform import Rotation as R

# 机械臂末端位姿列表
robot_poses = [...]

# 相机位姿列表
cam_poses = [...]

# 机械臂末端到机械臂坐标系的变换矩阵
robot_to_base = [...]

# 相机到标定板的变换矩阵
cam_to_board = [...]

# 机械臂末端到相机坐标系的变换矩阵
robot_to_cam = []

# 遍历每一组机械臂位姿和相机位姿，计算机械臂末端到相机坐标系的变换矩阵
for robot_pose, cam_pose in zip(robot_poses, cam_poses):
    # 机械臂末端到机械臂坐标系的变换矩阵
    base_to_robot = np.linalg.inv(robot_to_base)
    # 相机到标定板的变换矩阵
    board_to_cam = np.linalg.inv(cam_to_board)
    # 机械臂末端到标定板的变换矩阵
    base_to_board = np.dot(robot_pose, base_to_robot)
    # 相机到相机坐标系的变换矩阵
    cam_to_cam_frame = np.dot(cam_pose, board_to_cam)
    # 机械臂末端到相机坐标系的变换矩阵
    robot_to_cam_frame = np.dot(cam_to_cam_frame, np.linalg.inv(base_to_board))
    robot_to_cam.append(robot_to_cam_frame)

# 使用SVD分解计算相机和机械臂之间的变换关系
A = np.zeros((3*len(robot_to_cam), 4))
B = np.zeros((3*len(robot_to_cam), 1))
for i in range(len(robot_to_cam)):
    R = robot_to_cam[i][:3, :3]
    T = robot_to_cam[i][:3, 3]
    A[3*i:3*i+3, :] = np.hstack((R - np.eye(3), np.reshape(T, (3, 1))))
    B[3*i:3*i+3, :] = -np.dot(R, np.reshape(T, (3, 1)))
X = np.linalg.lstsq(A, B, rcond=None)[0]
T = X[:3, :]
R = R.from_rotvec(X[3:, :]).as_matrix()

# 打印计算结果
print("Translation vector T:\n", T)
print("Rotation matrix R:\n", R)

在上面的代码中，`robot_poses`和`cam_poses`分别是机械臂和相机运动时记录的位姿序列，`robot_to_base`和`cam_to_board`分别是机械臂末端到机械臂坐标系和相机到标定板的变换矩阵，`robot_to_cam`是计算得到的机械臂末端到相机坐标系的变换矩阵序列。最后使用SVD分解得到相机和机械臂之间的变换关系，分别得到平移向量T和旋转矩阵R。