机械臂进行手眼标定（眼在手上）python代码

执行手眼标定（eye in hand）步骤：

收集数据：使用相机拍摄多张不同角度的标定板图像，并记录相机和机械臂的位姿数据。
提取标定板角点：使用OpenCV库中的函数cv2.findChessboardCorners()提取标定板图像中的角点坐标。
计算相机内参矩阵：使用OpenCV库中的函数cv2.calibrateCamera()计算相机的内参矩阵，包括焦距、主点和畸变系数等。
计算相机外参矩阵：对于每张标定板图像，使用OpenCV库中的函数cv2.solvePnP()计算相机的外参矩阵，即相机在世界坐标系中的位姿。
计算机械臂末端在世界坐标系中的位姿：对于每个机械臂的位姿数据，使用机械臂的运动学模型计算机械臂末端在世界坐标系中的位姿。
计算相机和机械臂之间的变换矩阵：将相机在世界坐标系中的位姿和机械臂末端在世界坐标系中的位姿作为输入，使用OpenCV库中的函数cv2.solvePnP()计算相机和机械臂之间的变换矩阵。
应用手眼标定结果计算目标世界坐标：对于每个目标点的像素坐标，使用相机的内参矩阵将其转换为相机坐标系下的坐标，然后使用相机和机械臂之间的变换矩阵将其转换为机械臂末端坐标系下的坐标，最后使用机械臂的逆运动学模型计算机械臂需要运动到的关节角度，从而实现目标点的定位。

import cv2
import numpy as np
import rospy
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge

# 订阅相机图像和相机信息
def image_callback(image_msg):
    global cv_image
    bridge = CvBridge()
    cv_image = bridge.imgmsg_to_cv2(image_msg, desired_encoding='bgr8')

def camera_info_callback(info_msg):
    global camera_info
    camera_info = info_msg

rospy.init_node('handeye_calibration')

# 订阅相机图像和相机信息
rospy.Subscriber('/camera/image_raw', Image, image_callback)
rospy.Subscriber('/camera/camera_info', CameraInfo, camera_info_callback)

# 获取标定板参数
pattern_size = (9, 6) # 内部角点数量
square_size = 0.0254 # 标定板方块大小
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# 定义物体点和图像点数组
object_points = []
image_points = []

# 检测标定板并提取角点
while len(image_points) < 20:
    if cv_image is not None and camera_info is not None:
        gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
        if ret:
            object_point = np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32)
            object_point[:, :2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2)
            object_point *= square_size

            corners_refined = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

            object_points.append(object_point)
            image_points.append(corners_refined)

        cv2.drawChessboardCorners(cv_image, pattern_size, corners_refined, ret)
        cv2.imshow('image', cv_image)
        cv2.waitKey(100)

# 计算相机和机械臂姿态之间的变换
camera_matrix = np.array(camera_info.K).reshape((3, 3))
dist_coeffs = np.array(camera_info.D)[:4]
object_points_array = np.array(object_points)

rvecs_camera = []
tvecs_camera = []
rvecs_robot = []
tvecs_robot = []
for i in range(len(image_points)):
    # 计算相机姿态
    success, rvec, tvec = cv2.solvePnP(
        object_points_array[i],
        image_points[i],
        camera_matrix,
        dist_coeffs)
    rvecs_camera.append(rvec)
    tvecs_camera.append(tvec)

    # 获取机械臂姿态，这里假设机械臂已经正确标定，因此可以直接使用机械臂末端执行器的位姿作为机械臂姿态
    robot_pos = get_robot_pos()
    rvecs_robot.append(robot_pos[:3])
    tvecs_robot.append(robot_pos[3:])

# 执行手眼标定
ret, H = cv2.calibrateHandEye(
    np.array(rvecs_robot),
    np.array(tvecs_robot),
    np.array(rvecs_camera),
    np.array(tvecs_camera),
    method=cv2.CALIB_HAND_EYE_TSAI)

print(f'Hand-eye transformation:\n{
      
      H}')

cv2.destroyAllWindows()

机械臂进行手眼标定（眼在手上）python代码

猜你喜欢