执行手眼标定(eye in hand)步骤:
-
收集数据:使用相机拍摄多张不同角度的标定板图像,并记录相机和机械臂的位姿数据。
-
提取标定板角点:使用OpenCV库中的函数cv2.findChessboardCorners()提取标定板图像中的角点坐标。
-
计算相机内参矩阵:使用OpenCV库中的函数cv2.calibrateCamera()计算相机的内参矩阵,包括焦距、主点和畸变系数等。
-
计算相机外参矩阵:对于每张标定板图像,使用OpenCV库中的函数cv2.solvePnP()计算相机的外参矩阵,即相机在世界坐标系中的位姿。
-
计算机械臂末端在世界坐标系中的位姿:对于每个机械臂的位姿数据,使用机械臂的运动学模型计算机械臂末端在世界坐标系中的位姿。
-
计算相机和机械臂之间的变换矩阵:将相机在世界坐标系中的位姿和机械臂末端在世界坐标系中的位姿作为输入,使用OpenCV库中的函数cv2.solvePnP()计算相机和机械臂之间的变换矩阵。文章来源:https://www.toymoban.com/news/detail-595698.html
-
应用手眼标定结果计算目标世界坐标:对于每个目标点的像素坐标,使用相机的内参矩阵将其转换为相机坐标系下的坐标,然后使用相机和机械臂之间的变换矩阵将其转换为机械臂末端坐标系下的坐标,最后使用机械臂的逆运动学模型计算机械臂需要运动到的关节角度,从而实现目标点的定位。文章来源地址https://www.toymoban.com/news/detail-595698.html
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代码的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!