手眼标定公式

相机像素 → 手眼矩阵 → 机器人 base_link 坐标 → 实时 Servo 控制全部公式

一、先明确当前的坐标系（完全对应）

• 手眼标定后统一坐标系：base_link
• Servo 接收指令坐标系：base_link
• 相机输出：通常是2D 像素 (u, v)或3D 相机坐标 (Xc, Yc, Zc)
• 最终发给机器人：base_link 下的微小偏移 dx, dy, dz

二、手眼标定核心变换公式

假设你已经做完手眼标定，得到：

• 相机内参矩阵K
• 手眼变换矩阵T_cam2base（相机 → base_link）或T_cam2gripper + T_gripper2base（眼在手上）

1. 相机 3D 点 → 机器人 base_link 点

plaintext

P_base = T_cam2base * P_cam

其中：

• P_cam = [Xc, Yc, Zc, 1] 相机坐标系下焊缝点
• P_base = [Xb, Yb, Zb, 1] 机器人基坐标系下点

2. 得到偏移量（给 Servo 用）

plaintext

dx = Xb - X_ref dy = Yb - Y_ref dz = Zb - Z_ref

• X_ref/Y_ref/Z_ref：示教基准点（base_link 下）
• dx/dy/dz：就是发给 Servo 的速度指令

三、完整可运行代码（视觉→Servo 全套）

相机数据填进去就能跑。

#!/usr/bin/env python3 import rospy import numpy as np from geometry_msgs.msg import Twist # ============================================== # 1. 填入你的手眼标定矩阵（相机 → base_link） # ============================================== # 示例：4x4 变换矩阵，单位：米 T_cam2base = np.array([ [1, 0, 0, 0.0], [0, 1, 0, 0.0], [0, 0, 1, 0.5], # 相机在Z方向0.5m高处 [0, 0, 0, 1.0] ]) # ============================================== # 2. 示教基准点（base_link下的标准焊缝点） # ============================================== X_ref = 0.5 # 基准X Y_ref = 0.0 # 基准Y Z_ref = 0.3 # 基准Z # ============================================== # 3. 安全速度限制（和你的servo配置匹配） # ============================================== MAX_LIN_VEL = 0.15 # m/s MAX_ANG_VEL = 0.2 # rad/s class VisionToServo: def __init__(self): rospy.init_node('vision_servo_bridge') self.pub = rospy.Publisher( '/servo_server/delta_twist_cmds', Twist, queue_size=1 ) self.rate = rospy.Rate(30) rospy.loginfo("✅ 视觉→Servo 节点启动完成") def cam_to_base(self, Xc, Yc, Zc): """相机3D坐标 → base_link坐标""" p_cam = np.array([Xc, Yc, Zc, 1.0]) p_base = T_cam2base @ p_cam return p_base[0], p_base[1], p_base[2] def limit(self, v, max_v): return max(-max_v, min(v, max_v)) def run(self, Xc, Yc, Zc): # 1. 相机坐标 → 机器人base_link Xb, Yb, Zb = self.cam_to_base(Xc, Yc, Zc) # 2. 计算偏移量 dx = Xb - X_ref dy = Yb - Y_ref dz = Zb - Z_ref # 3. 限幅 dx = self.limit(dx, MAX_LIN_VEL) dy = self.limit(dy, MAX_LIN_VEL) dz = self.limit(dz, MAX_LIN_VEL) # 4. 发布Twist指令 twist = Twist() twist.linear.x = dx twist.linear.y = dy twist.linear.z = dz twist.angular.x = 0.0 twist.angular.y = 0.0 twist.angular.z = 0.0 self.pub.publish(twist) self.rate.sleep() # 打印调试 print(f"偏移 dx={dx:.3f} dy={dy:.3f} dz={dz:.3f}") if __name__ == '__main__': vs = VisionToServo() while not rospy.is_shutdown(): # ========================================== # 这里替换成你的相机实时输出：Xc,Yc,Zc # ========================================== X_camera = 0.01 # 相机X Y_camera = 0.005 # 相机Y Z_camera = 0.0 # 相机Z vs.run(X_camera, Y_camera, Z_camera)

四、如果你只有 2D 相机（无深度）

用像素 → 基坐标简化公式：

plaintext

Xb = a * u + b Yb = c * v + d Zb = 固定高度

a,b,c,d是你做像素标定得到的比例系数。

五、和现有 Servo 配置的严格对应

YAML 里的关键项：

yaml

robot_link_command_frame: base_link cartesian_command_in_topic: ~/delta_twist_cmds command_in_type: "unitless" 或 speed_units 都兼容

• 代码发布话题：/servo_server/delta_twist_cmds✅
• 指令坐标系：base_link✅
• 单位：m/s✅
• 平滑、限幅、安全 ✅