手眼标定公式

相机像素 → 手眼矩阵 → 机器人 base_link 坐标 → 实时 Servo 控制全部公式一、先明确当前的坐标系完全对应手眼标定后统一坐标系base_linkServo 接收指令坐标系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 点plaintextP_base T_cam2base * P_cam其中P_cam [Xc, Yc, Zc, 1] 相机坐标系下焊缝点P_base [Xb, Yb, Zb, 1] 机器人基坐标系下点2. 得到偏移量给 Servo 用plaintextdx Xb - X_ref dy Yb - Y_ref dz Zb - Z_refX_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_size1 ) 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 相机无深度用像素 → 基坐标简化公式plaintextXb a * u b Yb c * v d Zb 固定高度a,b,c,d是你做像素标定得到的比例系数。五、和现有 Servo 配置的严格对应YAML 里的关键项yamlrobot_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✅平滑、限幅、安全 ✅