手眼标定到底在做什么？
手眼标定需要计算相机与机械臂末端之间的固定空间关系：
base_link：机械臂基座坐标系wrist3_Link：机械臂末端附近的坐标系camera_link：相机主体坐标系camera_color_optical_frame：相机彩色光学坐标系
最终需要建立一条连通的 TF 链：
base_link → wrist3_Link → camera_link → camera_color_optical_frame
采集过程是让机械臂处于多个不同姿态，从不同角度观察棋盘格，并记录机器人末端位姿和棋盘格在相机中的姿态。让算法通过多组不同姿态的数据，估计相机与机械臂末端之间固定不变的空间变换。采样姿态越有充分的旋转变化，通常越有利于标定。
一、启动真实机械臂
具体操作参考前一节
二、启动相机并发布图像话题
1.确认系统识别到相机
lsusb | grep -i -E 'intel|realsense'如果能看到 Intel RealSense 相关设备，继续检查 ROS 驱动包：
rospack find realsense2_camera2.启动相机
终端1（不可关）：
source /opt/ros/noetic/setup.bash source ~/aubo_ws/devel/setup.bash roslaunch realsense2_camera rs_camera.launch \ serial_no:=f0461386 \ camera:=camera \ enable_color:=true \ enable_depth:=true \ align_depth:=true \ enable_pointcloud:=true \ publish_tf:=true3.检查相机话题
终端 2：
检查相机节点是否启动
source /opt/ros/noetic/setup.bash source ~/aubo_ws/devel/setup.bash rosnode list检查相机图像话题
rostopic list | grep camera检查图像是否正常发布
rostopic hz /camera/color/image_raw rostopic hz /camera/depth/color/points查看实时画面
rosrun rqt_image_view rqt_image_view在弹出的窗口中选择：/camera/color/image_raw
检查深度图像和 TF
rosrun tf tf_echo camera_link camera_color_optical_frame如果能持续显示 Translation 和 Rotation，说明相机内部坐标变换已发布
三、棋盘格标定
1.创建采集脚本
mkdir -p ~/aubo_ws/scripts nano ~/aubo_ws/scripts/handeye_chessboard.py#!/usr/bin/env python3 # -*- coding: utf-8 -*- import os import json import threading import cv2 import numpy as np import rospy import tf from sensor_msgs.msg import Image, CameraInfo from cv_bridge import CvBridge # ===== 标定参数 ===== PATTERN_SIZE = (11, 8) # 内角点：11列、8行 SQUARE_SIZE = 0.020 # 方格边长：20 mm = 0.020 m IMAGE_TOPIC = "/camera/color/image_raw" INFO_TOPIC = "/camera/color/camera_info" BASE_FRAME = "base_link" GRIPPER_FRAME = "wrist3_Link" OUTPUT_DIR = os.path.expanduser("~/aubo_ws/handeye_data") OUTPUT_FILE = os.path.join(OUTPUT_DIR, "chessboard_samples.json") class HandEyeCollector: def __init__(self): rospy.init_node("handeye_chessboard_collector") self.bridge = CvBridge() self.listener = tf.TransformListener() self.lock = threading.Lock() self.image = None self.stamp = None self.K = None self.D = None self.samples = [] os.makedirs(OUTPUT_DIR, exist_ok=True) self.load_existing() rospy.Subscriber( INFO_TOPIC, CameraInfo, self.camera_info_cb, queue_size=1 ) rospy.Subscriber( IMAGE_TOPIC, Image, self.image_cb, queue_size=1 ) rospy.loginfo("棋盘格参数：%s，方格边长 %.3f m", PATTERN_SIZE, SQUARE_SIZE) rospy.loginfo("保存文件：%s", OUTPUT_FILE) rospy.loginfo("按 s 保存样本，按 q 退出。") def load_existing(self): if os.path.isfile(OUTPUT_FILE): try: with open(OUTPUT_FILE, "r") as f: data = json.load(f) self.samples = data.get("samples", []) rospy.loginfo("已读取 %d 个历史样本", len(self.samples)) except Exception as e: rospy.logwarn("无法读取旧数据：%s", e) def camera_info_cb(self, msg): with self.lock: self.K = np.array(msg.K, dtype=np.float64).reshape(3, 3) self.D = np.array(msg.D, dtype=np.float64) def image_cb(self, msg): try: image = self.bridge.imgmsg_to_cv2( msg, desired_encoding="bgr8" ) with self.lock: self.image = image self.stamp = msg.header.stamp except Exception as e: rospy.logerr_throttle(2.0, "图像转换失败：%s", e) def save_json(self): data = { "pattern_size": list(PATTERN_SIZE), "square_size_m": SQUARE_SIZE, "base_frame": BASE_FRAME, "gripper_frame": GRIPPER_FRAME, "camera_frame": "camera_color_optical_frame", "samples": self.samples } tmp_file = OUTPUT_FILE + ".tmp" with open(tmp_file, "w") as f: json.dump(data, f, indent=2) os.replace(tmp_file, OUTPUT_FILE) def process_frame(self, image): gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) found, corners = cv2.findChessboardCornersSB( gray, PATTERN_SIZE, flags=cv2.CALIB_CB_NORMALIZE_IMAGE ) if not found: return image, None cv2.drawChessboardCorners( image, PATTERN_SIZE, corners, found ) with self.lock: K = None if self.K is None else self.K.copy() D = None if self.D is None else self.D.copy() if K is None or D is None: cv2.putText( image, "Waiting for camera_info", (20, 35), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2 ) return image, None # 棋盘格坐标：左上角内部角点为原点，Z=0 objp = np.zeros( (PATTERN_SIZE[0] * PATTERN_SIZE[1], 3), dtype=np.float32 ) objp[:, :2] = np.mgrid[ 0:PATTERN_SIZE[0], 0:PATTERN_SIZE[1] ].T.reshape(-1, 2) objp *= SQUARE_SIZE ok, rvec, tvec = cv2.solvePnP( objp, corners.reshape(-1, 2), K, D, flags=cv2.SOLVEPNP_ITERATIVE ) if not ok: return image, None R_camera_board, _ = cv2.Rodrigues(rvec) cv2.putText( image, "Chessboard detected - press s to save", (20, 35), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 0), 2 ) return image, { "R_target2cam": R_camera_board, "t_target2cam": tvec.reshape(3, 1), } def save_sample(self, pose): if pose is None: rospy.logwarn("当前帧没有有效棋盘格位姿，未保存") return with self.lock: stamp = self.stamp if stamp is None or stamp == rospy.Time(0): rospy.logwarn("图像时间戳无效，未保存") return try: # 在图像时间戳处读取 base_link -> wrist3_Link self.listener.waitForTransform( BASE_FRAME, GRIPPER_FRAME, stamp, rospy.Duration(0.5) ) trans, quat = self.listener.lookupTransform( BASE_FRAME, GRIPPER_FRAME, stamp ) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: rospy.logwarn("该图像时刻的机器人 TF 不可用，未保存：%s", e) return R_base_gripper = tf.transformations.quaternion_matrix(quat)[:3, :3] t_base_gripper = np.array(trans, dtype=float).reshape(3, 1) sample = { "stamp_sec": stamp.to_sec(), # base_T_gripper：末端坐标系到基座坐标系的位姿 "R_gripper2base": R_base_gripper.tolist(), "t_gripper2base": t_base_gripper.tolist(), # camera_T_target：棋盘格坐标系到相机坐标系的位姿 "R_target2cam": pose["R_target2cam"].tolist(), "t_target2cam": pose["t_target2cam"].tolist() } self.samples.append(sample) try: self.save_json() except Exception as e: self.samples.pop() rospy.logerr("保存文件失败：%s", e) return rospy.loginfo( "已保存样本 %d；末端位置基座坐标 [%.3f, %.3f, %.3f] m", len(self.samples), trans[0], trans[1], trans[2] ) def run(self): last_pose = None while not rospy.is_shutdown(): with self.lock: frame = None if self.image is None else self.image.copy() if frame is None: cv2.waitKey(1) rospy.sleep(0.02) continue display, last_pose = self.process_frame(frame) cv2.putText( display, "Samples: {}".format(len(self.samples)), (20, 70), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 0), 2 ) cv2.imshow("L515 Eye-on-Hand Collector", display) key = cv2.waitKey(1) & 0xFF if key == ord("s"): self.save_sample(last_pose) elif key == ord("q"): break cv2.destroyAllWindows() rospy.loginfo( "采集结束：%d 个样本。数据文件：%s", len(self.samples), OUTPUT_FILE ) if __name__ == "__main__": try: HandEyeCollector().run() except rospy.ROSInterruptException: pass每组样本的核心信息包括：
机器人末端相对于基座的位姿。
棋盘格相对于相机的旋转和平移。
棋盘格参数和相关坐标系名称。
2.运行采集程序
chmod +x ~/aubo_ws/scripts/handeye_chessboard.py source /opt/ros/noetic/setup.bash source ~/aubo_ws/devel/setup.bash python3 ~/aubo_ws/scripts/handeye_chessboard.py数据保存到 ~/aubo_ws/handeye_data/chessboard_samples.json
3.创建求解程序
nano ~/aubo_ws/scripts/solve_handeye.py#!/usr/bin/env python3 # -*- coding: utf-8 -*- import os import json import cv2 import numpy as np input_file = os.path.expanduser( "~/aubo_ws/handeye_data/chessboard_samples.json" ) output_file = os.path.expanduser( "~/aubo_ws/handeye_data/handeye_result.json" ) with open(input_file, "r") as f: data = json.load(f) samples = data["samples"] if len(samples) < 10: raise RuntimeError( "有效样本不足 10 组，当前只有 {} 组。".format(len(samples)) ) R_gripper2base = [] t_gripper2base = [] R_target2cam = [] t_target2cam = [] for s in samples: R_gripper2base.append( np.asarray(s["R_gripper2base"], dtype=np.float64) ) t_gripper2base.append( np.asarray(s["t_gripper2base"], dtype=np.float64).reshape(3, 1) ) R_target2cam.append( np.asarray(s["R_target2cam"], dtype=np.float64) ) t_target2cam.append( np.asarray(s["t_target2cam"], dtype=np.float64).reshape(3, 1) ) R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye( R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, method=cv2.CALIB_HAND_EYE_PARK ) # OpenCV 输出的是 gripper_T_camera： # 将 camera_color_optical_frame 中的点变换到 wrist3_Link T_gripper_camera = np.eye(4) T_gripper_camera[:3, :3] = R_cam2gripper T_gripper_camera[:3, 3] = t_cam2gripper.reshape(3) print("样本数量:", len(samples)) print("wrist3_Link -> camera_color_optical_frame") print("旋转矩阵 R:") print(R_cam2gripper) print("平移向量 t (m):") print(t_cam2gripper.reshape(3)) print("齐次变换矩阵 T_gripper_camera:") print(T_gripper_camera) result = { "sample_count": len(samples), "parent_frame": "wrist3_Link", "child_frame": "camera_color_optical_frame", "rotation_matrix": R_cam2gripper.tolist(), "translation_m": t_cam2gripper.reshape(3).tolist(), "transform_matrix": T_gripper_camera.tolist() } with open(output_file, "w") as f: json.dump(result, f, indent=2) print("结果已保存到:", output_file)其输入主要是两组位姿数据：
机器人末端相对于基座的位姿。
棋盘格相对于相机的位姿。
输出是相机和机械臂末端之间的固定旋转矩阵与平移向量
运行
chmod +x ~/aubo_ws/scripts/solve_handeye.py python3 ~/aubo_ws/scripts/solve_handeye.py数据保存在 ~/aubo_ws/handeye_data/handeye_result.json
4.验证标定结果
nano ~/aubo_ws/scripts/validate_handeye.py#!/usr/bin/env python3 # -*- coding: utf-8 -*- import json import numpy as np from pathlib import Path data_dir = Path.home() / "aubo_ws/handeye_data" with open(data_dir / "chessboard_samples.json") as f: data = json.load(f) with open(data_dir / "handeye_result.json") as f: result = json.load(f) X = np.asarray(result["transform_matrix"], dtype=float) samples = data["samples"] def make_T(R, t): T = np.eye(4) T[:3, :3] = np.asarray(R, dtype=float).reshape(3, 3) T[:3, 3] = np.asarray(t, dtype=float).reshape(3) return T def rotation_error_deg(R1, R2): R = R1.T @ R2 c = np.clip((np.trace(R) - 1.0) / 2.0, -1.0, 1.0) return np.degrees(np.arccos(c)) # 检查旋转矩阵是否有效 R = X[:3, :3] print("样本数:", len(samples)) print("旋转矩阵行列式:", np.linalg.det(R)) print("旋转矩阵正交误差:", np.linalg.norm(R.T @ R - np.eye(3))) print("相机到末端的平移距离: %.2f cm" % (100 * np.linalg.norm(X[:3, 3]))) # 每组数据计算棋盘格在 base_link 下的位姿 poses = [] for i, s in enumerate(samples): T_base_gripper = make_T( s["R_gripper2base"], s["t_gripper2base"] ) T_camera_target = make_T( s["R_target2cam"], s["t_target2cam"] ) T_base_target = T_base_gripper @ X @ T_camera_target poses.append(T_base_target) # 以旋转矩阵平均值的 SVD 投影作为参考姿态 Rs = np.stack([T[:3, :3] for T in poses]) U, _, Vt = np.linalg.svd(Rs.mean(axis=0)) R_mean = U @ Vt if np.linalg.det(R_mean) < 0: U[:, -1] *= -1 R_mean = U @ Vt ts = np.stack([T[:3, 3] for T in poses]) t_mean = ts.mean(axis=0) pos_err_mm = np.linalg.norm(ts - t_mean, axis=1) * 1000 rot_err_deg = np.array([ rotation_error_deg(R_mean, Ri) for Ri in Rs ]) print("\n棋盘格基座坐标估计的离散程度：") print("位置误差中位数: %.2f mm" % np.median(pos_err_mm)) print("位置误差最大值: %.2f mm" % np.max(pos_err_mm)) print("姿态误差中位数: %.2f deg" % np.median(rot_err_deg)) print("姿态误差最大值: %.2f deg" % np.max(rot_err_deg)) print("\n注意：以上是整体一致性检查，不是独立的精度认证。") print("如果误差很大，应检查采集同步、角点检测、棋盘格是否移动，") print("以及机械臂位姿和相机位姿的变换方向。")python3 ~/aubo_ws/scripts/validate_handeye.py四、将手眼标定结果接入 TF
1.自动计算wrist3_Link→camera_link
source /opt/ros/noetic/setup.bash source ~/aubo_ws/devel/setup.bash cat > ~/aubo_ws/scripts/compute_camera_link_tf.py <<'PY' #!/usr/bin/env python3 import json import numpy as np import rospy import tf from pathlib import Path result_file = Path.home() / "aubo_ws/handeye_data/handeye_result.json" result = json.loads(result_file.read_text()) # 标定结果：T_wrist_optical T_wrist_optical = np.array(result["transform_matrix"], dtype=float) rospy.init_node("compute_camera_link_tf", anonymous=True) listener = tf.TransformListener() parent = "camera_link" child = "camera_color_optical_frame" print("等待相机内部 TF:", parent, "->", child) listener.waitForTransform(parent, child, rospy.Time(0), rospy.Duration(10.0)) trans, quat = listener.lookupTransform(parent, child, rospy.Time(0)) # lookupTransform(target, source) 返回 T_target_source T_camera_optical = tf.transformations.quaternion_matrix(quat) T_camera_optical[:3, 3] = trans # T_wrist_camera_link = T_wrist_optical * inverse(T_camera_link_optical) T_wrist_camera_link = T_wrist_optical @ np.linalg.inv(T_camera_optical) translation = T_wrist_camera_link[:3, 3] quaternion = tf.transformations.quaternion_from_matrix(T_wrist_camera_link) print("\n计算结果：wrist3_Link -> camera_link") print("平移 (m):", translation) print("四元数 (x y z w):", quaternion) print("\n请检查后再执行以下命令：") print( "rosrun tf2_ros static_transform_publisher " f"{translation[0]:.9f} {translation[1]:.9f} {translation[2]:.9f} " f"{quaternion[0]:.9f} {quaternion[1]:.9f} " f"{quaternion[2]:.9f} {quaternion[3]:.9f} " "wrist3_Link camera_link" ) PY python3 ~/aubo_ws/scripts/compute_camera_link_tf.py2.发布静态 TF
终端3（不可关）：
source /opt/ros/noetic/setup.bash source ~/aubo_ws/devel/setup.bash rosrun tf2_ros static_transform_publisher \ 0.001775997 0.101285283 0.027796307 \ -0.499906345 -0.495186915 -0.494087099 0.510648122 \ wrist3_Link camera_link3.验证 TF 树是否连通
终端4：
rosrun tf tf_echo wrist3_Link camera_color_optical_frame rosrun tf tf_echo base_link camera_color_optical_frame正常应持续显示平移和四元数
