摘要：
在获取相机坐标系下的抓取位姿后，必须通过手眼标定将其转换到机器人基坐标系。本文采用Eye-in-Hand方式，基于AX=XB模型完成标定。随后基于MoveIt 2和状态机设计抓取管理器，实现预抓取、抓取、提升、返回等完整动作序列。最后给出系统延迟、资源分配等性能指标及开发中的关键经验。

1. 手眼标定

1.1 Eye-in-Hand 标定原理

• Eye-in-Hand（眼在手上）：摄像头安装在机械臂末端法兰上，随机械臂一起运动。
• 数学模型：A_i * X = X * B_i （AX=XB问题）
  • A_i：末端法兰从位姿i到位姿j的变换（从机械臂正运动学获取）
  • B_i：标定板从位姿i到位姿j的变换（从摄像头图像中检测获取）
  • X：摄像头到末端法兰的固定变换（待求解）
• 标定流程：
  1. 移动机械臂到15-20个不同位姿
  2. 每个位姿记录：
     • 机械臂末端法兰位姿（正运动学）
     • 摄像头看到的标定板位姿（PnP求解）
  3. 使用Tsai/Park/Horaud等方法求解X

1.2 标定实现

# grasp_calibration/hand_eye_calibration.py
# Eye-in-Hand 手眼标定

import numpy as np
import cv2
import json
from pathlib import Path
from typing import List, Tuple

class HandEyeCalibrator:
    """
    手眼标定工具
    使用棋盘格标定板，采集多组数据后求解 AX=XB
    """
    def __init__(self, board_size=(9, 6), square_size=0.025):
        self.board_size = board_size
        self.square_size = square_size

        self.objp = np.zeros((board_size[0] * board_size[1], 3), np.float32)
        self.objp[:, :2] = np.mgrid[0:board_size[0], 0:board_size[1]].T.reshape(-1, 2) * square_size

        self.gripper2base_rotations = []    # 法兰在base下的旋转矩阵
        self.gripper2base_translations = [] # 法兰在base下的平移向量
        self.target2cam_rotations = []     # 标定板在camera下的旋转矩阵
        self.target2cam_translations = []  # 标定板在camera下的平移向量

        self.camera_matrix = None
        self.dist_coeffs = None

    def set_camera_intrinsics(self, camera_matrix: np.ndarray, dist_coeffs: np.ndarray):
        self.camera_matrix = camera_matrix
        self.dist_coeffs = dist_coeffs

    def add_sample(self, image: np.ndarray, gripper_pose: np.ndarray) -> bool:
        """
        添加一组标定数据
        image: BGR图像
        gripper_pose: 4x4 齐次变换矩阵（末端法兰在base坐标系下）
        """
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        found, corners = cv2.findChessboardCorners(gray, self.board_size, None)
        if not found:
            return False

        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

        _, rvec, tvec = cv2.solvePnP(self.objp, corners, self.camera_matrix, self.dist_coeffs)

        R_target2cam, _ = cv2.Rodrigues(rvec)
        self.target2cam_rotations.append(R_target2cam)
        self.target2cam_translations.append(tvec.reshape(3, 1))

        R_gripper2base = gripper_pose[:3, :3]
        t_gripper2base = gripper_pose[:3, 3].reshape(3, 1)
        self.gripper2base_rotations.append(R_gripper2base)
        self.gripper2base_translations.append(t_gripper2base)

        return True

    def calibrate(self, method=cv2.CALIB_HAND_EYE_TSAI) -> Tuple[np.ndarray, np.ndarray]:
        """执行手眼标定，返回 (R_cam2gripper, t_cam2gripper)"""
        n = len(self.gripper2base_rotations)
        if n < 3:
            raise ValueError(f"Need at least 3 samples, got {n}")

        R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
            self.gripper2base_rotations,
            self.gripper2base_translations,
            self.target2cam_rotations,
            self.target2cam_translations,
            method=method
        )

        T = np.eye(4)
        T[:3, :3] = R_cam2gripper
        T[:3, 3] = t_cam2gripper.flatten()

        print(f"\n==== Hand-Eye Calibration Result ====")
        print(f"Sample used: {n}")
        print(f"cam2gripper transform:\n{T}")
        print(f"Translation: {t_cam2gripper.flatten()}")

        angles = cv2.Rodrigues(R_cam2gripper)[0].flatten()
        print(f"Rotation (rodrigues): {np.degrees(angles)}")

        return R_cam2gripper, t_cam2gripper

    def save_result(self, filepath: str, R: np.ndarray, t: np.ndarray):
        T = np.eye(4)
        T[:3, :3] = R
        T[:3, 3] = t.flatten()
        data = {"cam2gripper": T.tolist()}
        with open(filepath, 'w') as f:
            json.dump(data, f, indent=2)
        print(f"Calibration saved to {filepath}")

    @staticmethod
    def load_result(filepath: str) -> np.ndarray:
        with open(filepath, 'r') as f:
            data = json.load(f)
        return np.array(data["cam2gripper"])

2. MoveIt 2 运动规划与抓取执行管理器

抓取任务管理器通过状态机驱动。

以下代码实现了状态机驱动的抓取流程：从接收抓取位姿开始，依次执行预抓取、精确抓取、闭合夹爪、提升物体、返回Home位姿、打开夹爪等步骤。

# grasp_execution/grasp_manager.py
# 抓取任务管理器
# 协调：视觉检测 -> 抓取规划 -> 运动执行 -> 夹爪控制

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from geometry_msgs.msg import PoseStamped, Pose
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import (
    MotionPlanRequest, Constraints, JointConstraint,
    PositionConstraint, OrientationConstraint,
    BoundingVolume, SolidPrimitive,
    RobotState, PlanningScene, CollisionObject
)
from shape_msgs.msg import SolidPrimitive as ShapeSolid
from std_msgs.msg import Bool, Float32
from sensor_msgs.msg import JointState
import numpy as np
import time
import json
from enum import Enum

class GraspState(Enum):
    IDLE = "idle"
    DETECTING = "detecting"
    PLANNING_APPROACH = "planning_approach"
    APPROACHING = "approaching"
    PLANNING_GRASP = "planning_grasp"
    GRASPING = "grasping"
    CLOSING_GRIPPER = "closing_gripper"
    LIFTING = "lifting"
    PLACING = "placing"
    OPENING_GRIPPER = "opening_gripper"
    RETURNING = "returning"
    ERROR = "error"

class GraspManagerNode(Node):
    """
    抓取任务管理器
    状态机驱动的抓取流程管理
    """
    def __init__(self):
        super().__init__('grasp_manager')

        self.declare_parameter('cam2gripper_path', '/opt/robot/config/cam2gripper.json')
        self.declare_parameter('approach_distance', 0.10)
        self.declare_parameter('lift_height', 0.15)
        self.declare_parameter('gripper_close_force', 20.0)
        self.declare_parameter('planning_time', 5.0)

        self._load_calibration()

        self.state = GraspState.IDLE
        self._current_grasp_pose = None
        self._current_joint_state = None

        self.create_subscription(PoseStamped, '/grasp_pose', self._grasp_pose_callback, 10)
        self.create_subscription(JointState, '/joint_states', self._joint_state_callback, 10)

        self.gripper_pub = self.create_publisher(Float32, '/gripper/command', 10)
        self.state_pub = self.create_publisher(Bool, '/grasp/busy', 10)

        self._move_group_client = ActionClient(self, MoveGroup, '/move_action')

        self.create_timer(0.1, self._state_machine_loop)

        self.get_logger().info('Grasp manager ready')

    def _load_calibration(self):
        path = self.get_parameter('cam2gripper_path').value
        try:
            with open(path, 'r') as f:
                data = json.load(f)
            self.T_cam2gripper = np.array(data['cam2gripper'])
            self.get_logger().info(f'Hand-eye calibration loaded from {path}')
        except FileNotFoundError:
            self.T_cam2gripper = np.eye(4)
            self.get_logger().warning('No calibration found, using identity')

    def _grasp_pose_callback(self, msg):
        if self.state == GraspState.IDLE:
            grasp_in_cam = self._pose_to_matrix(msg.pose)
            grasp_in_base = self._transform_to_base(grasp_in_cam)
            self._current_grasp_pose = grasp_in_base
            self.state = GraspState.PLANNING_APPROACH
            self.get_logger().info('Grasp target received, planning...')

    def _joint_state_callback(self, msg):
        self._current_joint_state = msg

    def _transform_to_base(self, pose_in_cam: np.ndarray) -> np.ndarray:
        """将相机坐标系的位姿转换到机器人base坐标系"""
        T_gripper2base = self._get_current_gripper_pose()
        T_cam2base = T_gripper2base @ self.T_cam2gripper
        T_target_in_base = T_cam2base @ pose_in_cam
        return T_target_in_base

    def _get_current_gripper_pose(self) -> np.ndarray:
        """获取当前末端法兰在base下的位姿（从关节角正运动学）"""
        # 实际项目中通过 MoveIt 的 FK 服务获取
        return np.eye(4)

    def _state_machine_loop(self):
        """状态机主循环"""
        busy = self.state != GraspState.IDLE
        msg = Bool()
        msg.data = busy
        self.state_pub.publish(msg)

        if self.state == GraspState.PLANNING_APPROACH:
            self._plan_approach()
        elif self.state == GraspState.PLANNING_GRASP:
            self._plan_grasp()
        elif self.state == GraspState.CLOSING_GRIPPER:
            self._close_gripper()
        elif self.state == GraspState.LIFTING:
            self._plan_lift()
        elif self.state == GraspState.OPENING_GRIPPER:
            self._open_gripper()
        elif self.state == GraspState.RETURNING:
            self._plan_return_home()

    def _plan_approach(self):
        """规划到预抓取位姿（抓取点上方一段距离）"""
        if self._current_grasp_pose is None:
            self.state = GraspState.ERROR
            return
        approach_dist = self.get_parameter('approach_distance').value
        approach_pose = self._current_grasp_pose.copy()
        approach_pose[2, 3] += approach_dist
        success = self._execute_move(approach_pose)
        if success:
            self.state = GraspState.PLANNING_GRASP
        else:
            self.get_logger().error('Approach planning failed')
            self.state = GraspState.ERROR

    def _plan_grasp(self):
        """规划到精确抓取位姿（直线下降）"""
        success = self._execute_move(self._current_grasp_pose, cartesian=True)
        if success:
            self.state = GraspState.CLOSING_GRIPPER
        else:
            self.state = GraspState.ERROR

    def _close_gripper(self):
        """关闭夹爪"""
        force = self.get_parameter('gripper_close_force').value
        msg = Float32()
        msg.data = force
        self.gripper_pub.publish(msg)
        time.sleep(1.0)
        self.state = GraspState.LIFTING

    def _plan_lift(self):
        """提升物体"""
        lift_pose = self._current_grasp_pose.copy()
        lift_h = self.get_parameter('lift_height').value
        lift_pose[2, 3] += lift_h
        success = self._execute_move(lift_pose, cartesian=True)
        if success:
            self.state = GraspState.RETURNING
        else:
            self.state = GraspState.ERROR

    def _open_gripper(self):
        """打开夹爪"""
        msg = Float32()
        msg.data = 0.0
        self.gripper_pub.publish(msg)
        time.sleep(0.5)
        self.state = GraspState.IDLE

    def _plan_return_home(self):
        """返回Home位姿"""
        home_joints = [0.0, -1.57, 1.57, -1.57, -1.57, 0.0]
        success = self._execute_joint_move(home_joints)
        if success:
            self.state = GraspState.OPENING_GRIPPER
        else:
            self.state = GraspState.ERROR

    def _execute_move(self, target_matrix: np.ndarray, cartesian: bool = False) -> bool:
        """执行运动规划和移动"""
        self.get_logger().info(f'Planning move (cartesian={cartesian})')
        # 实际通过 MoveIt 2 MoveGroup Action 执行
        time.sleep(0.5)
        return True

    def _execute_joint_move(self, joint_values: list) -> bool:
        """执行关节空间移动"""
        self.get_logger().info(f'Moving to joint config')
        time.sleep(0.5)
        return True

    @staticmethod
    def _pose_to_matrix(pose: Pose) -> np.ndarray:
        T = np.eye(4)
        T[0, 3] = pose.position.x
        T[1, 3] = pose.position.y
        T[2, 3] = pose.position.z

        qx, qy, qz, qw = pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w
        T[0, 0] = 1 - 2*(qy*qy + qz*qz)
        T[0, 1] = 2*(qx*qy - qz*qw)
        T[0, 2] = 2*(qx*qz + qy*qw)
        T[1, 0] = 2*(qx*qy + qz*qw)
        T[1, 1] = 1 - 2*(qx*qx + qz*qz)
        T[1, 2] = 2*(qy*qz - qx*qw)
        T[2, 0] = 2*(qx*qz - qy*qw)
        T[2, 1] = 2*(qy*qz + qx*qw)
        T[2, 2] = 1 - 2*(qx*qx + qy*qy)
        return T

def main(args=None):
    rclpy.init(args=args)
    node = GraspManagerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

3. 系统性能指标与总结

3.1 系统性能

3.2 关键经验

1. 手眼标定精度：标定时至少采集15组以上不同位姿的数据，位姿变化要覆盖旋转和平移。
2. 深度图质量：RGB-D相机的深度图噪声直接影响抓取精度，建议使用时间滤波。
3. 碰撞场景：MoveIt 2需要实时更新碰撞场景（桌面、已抓物体），避免规划到碰撞轨迹。
4. 安全岛保护：机械臂运动时Safety Island持续监控关节力矩，超限立即停止。

3.3 参考资料

1. MoveIt 2 文档
2. OpenCV 手眼标定
3. GraspNet-1Billion
4. QIR SDK
5. HuggingFace 高通模型
6. 高通跃龙IQ-9100平台
7. 高通跃龙IQ-9100开发板
   

作者总结

视觉引导抓取是让机器人从“看得见”到“抓得住”的关键跨越。IQ-9100的双NPU可以同时运行目标检测和抓取姿态估计，CPU余力运行MoveIt 2，Safety Island保障运动安全，真正实现了一芯全栈。下一篇我们将探索端侧多模态大模型如何赋予机器人更高层次的理解和交互能力。

（全文完）