🎯 **第10周任务确认完成！** 你已经实现了**物体识别节点**，能够从 `/camera/image_raw` 稳定提取红色方块的像素坐标。这是视觉系统的"眼睛"，下一步要赋予它"空间感知能力"——**将2D像素转换为3D空间位姿**。 --- ## 📍 **第11周任务：位姿估计 + 手眼标定简化版** ### **核心目标** 实现 `pose_estimator.py` 节点，完成 **像素坐标 → 相机坐标 → 机械臂基坐标** 的转换，输出物体在 `base_link` 系下的 (x, y, z, qx, qy, qz, qw)。 ### **关键挑战** ``` pixel (u,v) ↓ [相机内参 + 深度假设] camera_optical_frame (Xc, Yc, Zc) ↓ [TF2变换] base_link (Xb, Yb, Zb) ← MoveIt!需要这个！ ``` --- ### **1. 数学基础（必须理解）** #### **1.1 相机成像模型** ``` u = fx * Xc/Zc + cx v = fy * Yc/Zc + cy ``` **反推相机坐标系**（假设物体在桌面，Z已知）： ```python # 已知：像素坐标 (u,v)，相机内参 (fx, fy, cx, cy) # 假设：物体高度 Zc = 1.0m（桌面高度，后续会优化） Xc = (u - cx) * Zc / fx Yc = (v - cy) * Zc / fy Zc = 1.0 # 预设值 ``` #### **1.2 坐标系变换** ``` P_base = R_base_camera * P_camera + t_base_camera ``` - `R`: 旋转矩阵（3x3） - `t`: 平移向量（3x1） 在ROS中用 `tf2_ros.Buffer.lookup_transform()` 自动完成。 --- ### **2. 创建位姿估计节点** ```bash cd ~/vision_ws/src/vision_grasp_demo/src/ touch pose_estimator.py chmod +x pose_estimator.py ``` **完整代码实现**： ```python #!/usr/bin/env python3 import rclpy from rclpy.node import Node from geometry_msgs.msg import PoseStamped, Pose from sensor_msgs.msg import CameraInfo import tf2_ros import numpy as np class PoseEstimator(Node): def __init__(self): super().__init__('pose_estimator') # 参数：物体高度（桌面到物体中心的距离） self.declare_parameter('object_height', 0.05) # 5cm高的方块 self.declare_parameter('camera_frame', 'camera_link_optical') self.declare_parameter('target_frame', 'base_link') # TF2 self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) # 相机内参（从CameraInfo话题获取） self.camera_info = None self.create_subscription( CameraInfo, '/camera/camera_info', self.camera_info_callback, 10 ) # 订阅物体检测结果 self.create_subscription( PoseStamped, '/detection/object_pose_pixel', self.pixel_pose_callback, 10 ) # 发布3D位姿 self.pose_pub = self.create_publisher(PoseStamped, '/detection/object_pose_3d', 10) self.get_logger().info('PoseEstimator节点已启动 - 等待相机参数和物体像素坐标...') def camera_info_callback(self, msg): """缓存相机内参""" if self.camera_info is None: self.camera_info = msg self.get_logger().info(f'相机参数已获取: fx={msg.k[0]:.2f}, fy={msg.k[4]:.2f}') def pixel_pose_callback(self, msg): """核心：像素坐标 → 3D位姿""" if self.camera_info is None: self.get_logger().warn('等待相机参数中...') return # 1. 提取像素坐标 pixel_u = msg.pose.position.x # 复用x,y字段存储像素坐标 pixel_v = msg.pose.position.y # 2. 相机内参 fx, fy = self.camera_info.k[0], self.camera_info.k[4] cx, cy = self.camera_info.k[2], self.camera_info.k[5] # 3. 预设深度（关键简化：物体在桌面，高度已知） object_height = self.get_parameter('object_height').value # 相机离地高度约0.8m，物体高度0.05m，所以Zc≈0.75m Zc = 0.75 # 根据你的场景调整 # 4. 像素 → 相机坐标系 Xc = (pixel_u - cx) * Zc / fx Yc = (pixel_v - cy) * Zc / fy self.get_logger().debug(f'相机坐标: ({Xc:.3f}, {Yc:.3f}, {Zc:.3f})') # 5. 相机坐标系 → base_link try: # 获取 TF: base_link ← camera_optical_frame transform = self.tf_buffer.lookup_transform( self.get_parameter('target_frame').value, self.get_parameter('camera_frame').value, rclpy.time.Time() ) # 提取旋转和平移 t = transform.transform.translation q = transform.transform.rotation # 四元数 → 旋转矩阵 R = self.quaternion_to_rotation_matrix([q.x, q.y, q.z, q.w]) # 应用变换: P_base = R * P_camera + t P_camera = np.array([Xc, Yc, Zc]) P_base = R @ P_camera + np.array([t.x, t.y, t.z]) self.get_logger().info( f'物体在base_link下的位姿: x={P_base[0]:.3f}, y={P_base[1]:.3f}, z={P_base[2]:.3f}' ) # 6. 发布PoseStamped pose_msg = PoseStamped() pose_msg.header.frame_id = self.get_parameter('target_frame').value pose_msg.header.stamp = self.get_clock().now().to_msg() pose_msg.pose.position.x = P_base[0] pose_msg.pose.position.y = P_base[1] pose_msg.pose.position.z = object_height # 物体中心高度 # 朝向：默认朝上（可根据物体方向调整） pose_msg.pose.orientation.x = 0.0 pose_msg.pose.orientation.y = 0.0 pose_msg.pose.orientation.z = 0.0 pose_msg.pose.orientation.w = 1.0 self.pose_pub.publish(pose_msg) except tf2_ros.LookupException as e: self.get_logger().error(f'TF变换失败: {str(e)}') self.get_logger().warn('请确认: 1) URDF中定义了camera_link_optical 2) robot_state_publisher已运行') @staticmethod def quaternion_to_rotation_matrix(q): """四元数 → 3x3旋转矩阵""" x, y, z, w = q # 归一化 norm = np.sqrt(x*x + y*y + z*z + w*w) x, y, z, w = x/norm, y/norm, z/norm, w/norm # 计算旋转矩阵 R = np.array([ [1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w], [2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w], [2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y] ]) return R def main(args=None): rclpy.init(args=args) estimator = PoseEstimator() try: rclpy.spin(estimator) except KeyboardInterrupt: estimator.get_logger().info('节点已停止') finally: estimator.destroy_node() rclpy.shutdown() if __name__ == '__main__': main() ``` --- ### **3. 修改 `object_detector.py` 以发布像素坐标** 在 `object_detector.py` 的 `image_callback` 中，识别成功后发布： ```python from geometry_msgs.msg import PoseStamped, Pose # 在 __init__ 中添加 self.pixel_pose_pub = self.create_publisher(PoseStamped, '/detection/object_pose_pixel', 10) # 在 detect_object() 返回后，添加： if detection: # ... 原有绘制代码 ... # 新增：发布像素坐标 pixel_msg = PoseStamped() pixel_msg.header.frame_id = 'camera_optical_frame' pixel_msg.header.stamp = self.get_clock().now().to_msg() pixel_msg.pose.position.x = float(detection['center'][0]) pixel_msg.pose.position.y = float(detection['center'][1]) pixel_msg.pose.position.z = 0.0 # 像素坐标无深度 self.pixel_pose_pub.publish(pixel_msg) ``` --- ### **4. 更新 CMakeLists.txt** ```cmake install(PROGRAMS src/object_detector.py src/pose_estimator.py # 新增 DESTINATION lib/${PROJECT_NAME} ) ``` --- ### **5. 创建配置文件** `config/pose_estimation.yaml`: ```yaml pose_estimator: ros__parameters: object_height: 0.05 # 物体高度5cm camera_frame: 'camera_link_optical' target_frame: 'base_link' ``` --- ### **6. 更新启动文件** 在 `ur5_camera_bringup.launch.py` 中添加： ```python # 在 launch_setup 中添加 pose_estimator_node = Node( package='vision_grasp_demo', executable='pose_estimator.py', name='pose_estimator', output='screen', parameters=[PathJoinSubstitution([ FindPackageShare('vision_grasp_demo'), 'config', 'pose_estimation.yaml' ])] ) # 在 nodes_to_start 中追加 pose_estimator_node, ``` --- ### **7. 编译 & 测试** ```bash cd ~/vision_ws colcon build --packages-select vision_grasp_demo --symlink-install source install/setup.bash # 启动完整系统 ros2 launch vision_grasp_demo ur5_camera_bringup.launch.py # 新终端：验证位姿输出 ros2 topic echo /detection/object_pose_3d # 在Rviz中可视化 # 添加 PoseArray 或 Marker 显示 /detection/object_pose_3d ``` --- ### **8. 验证与调优** **验证TF树是否正确：** ```bash ros2 run tf2_tools view_frames.py # 确认存在: base_link → ... → camera_link_optical 的链路 ``` **手动测量验证：** ```bash # 在Gazebo中，右键点击红色方块 → View → Link Inspector # 记录真实位置 (x, y, z) # 对比你的 /detection/object_pose_3d 输出 # 误差应在 ±2cm 内 ``` **调参策略：** ```python # 如果X/Y方向偏差大：调整 Zc 值 Zc = 0.75 # 尝试 0.7 ~ 0.8 之间调整 # 如果深度方向偏差大：检查TF变换 # 确认 camera_link_optical 的朝向是否正确（Z轴向前） ``` --- ### **9. 本周交付物清单** - [ ] `src/pose_estimator.py` **核心位姿计算代码**（含四元数转旋转矩阵） - [ ] `config/pose_estimation.yaml` **参数配置文件** - [ ] **Rviz截图**（显示 `/detection/object_pose_3d` 的Marker在方块上） - [ ] **误差分析表**（至少3个不同位置，记录真实值 vs 估计值） --- ### **10. 技术亮点（简历素材）** ```markdown ## 技术报告摘录：位姿估计简化方案 **问题**：完整手眼标定需9点法，项目周期不允许。 **创新**：采用"预设高度+静态TF"的轻量化方案： 1. 利用Gazebo已知环境，假设物体在Z=0.75m平面 2. 通过tf2_ros自动获取相机到基座的变换 3. 四元数转旋转矩阵实现坐标变换 **效果**：定位误差±2cm（满足抓取容差） **代价**：无法适应物体高度变化（未来改进方向） ``` --- ## 🎯 **下周预告：第12周 · MoveIt!抓取闭环** 你将实现： - **MoveIt 2集成**：配置UR5运动规划组 - **碰撞场景**：添加桌面和障碍物 - **抓取策略**：预抓取 → 接近 → 闭合 → 提升 - **完整闭环**：识别 → 规划 → 执行 → 放置 **下周交付物将包含完整的视频演示**，这是你简历上最硬核的证明。 --- 💪 **现在，去实现坐标变换节点吧！记住：四元数转旋转矩阵的代码要理解透，面试99%会问。** **完成后请回复："第11周任务完成，位姿估计误差±2cm内"，并附上Rviz可视化截图。** 以上是我的教程，请给我讲解：1. 数学基础（必须理解）