aubo学习：realsense手眼标定

手眼标定到底在做什么？

手眼标定需要计算相机与机械臂末端之间的固定空间关系：

• 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_camera

2.启动相机

终端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:=true

3.检查相机话题

终端 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.py

2.发布静态 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_link

3.验证 TF 树是否连通

终端4：

rosrun tf tf_echo wrist3_Link camera_color_optical_frame
rosrun tf tf_echo base_link camera_color_optical_frame

正常应持续显示平移和四元数