Kimi | 视觉抓取闭环实现

ur5_with_camera.urdf.xacro

XACRO 5.97 KB

ur5_camera_bringup.launch.py

PY 6.64 KB

package.xml

XML 878 Bytes

CMakeLists.txt

TXT 308 Bytes

🎉 第三阶段任务圆满完成！你已成功构建了完整的自主导航系统 —— 从Gazebo仿真、SLAM建图、AMCL定位到Nav2路径规划，这是机器人软件工程师的核心能力。简历上这个项目会让面试官眼前一亮！ 🎯 第四阶段任务：第10–12周｜视觉引导抓取项目（迈向算法）项目名称：vision_grasp_demo 目标：在Gazebo中搭建 UR5机械臂 + 摄像头系统，实现物体识别 → 位姿估计 → MoveIt!抓取闭环，首次接触机器人视觉算法！ 🌟 为什么这个项目是“算法转型”的关键跳板？能力维度本项目价值未来算法岗关联 ---------- ------------ ---------------- 工程能力集成Gazebo/MoveIt!/OpenCV 算法落地必备基础感知算法颜色/形状识别 + 位姿估计计算机视觉核心技能坐标变换手眼标定简化版机器人视觉必修课运动规划 MoveIt!路径规划与避障运动规划算法前置知识系统思维从感知到执行的完整闭环复杂算法系统设计基础 💡 企业视角： “懂工程的算法工程师”比“纯调参侠”更受青睐。你能说出“在实际抓取中，光照变化导致HSV阈值失效，我改用轮廓面积+长宽比双重过滤” —— 这就是差异化优势！ 📁 一、项目结构（交付物清单） vision_grasp_demo/ ├── config/ │ ├── camera.yaml # 摄像头参数 │ ├── moveit_config/ # MoveIt!配置文件 │ └── grasp_params.yaml # 抓取参数（物体尺寸/抓取高度） ├── launch/ │ ├── bringup.launch.py # 一键启动仿真+视觉+MoveIt │ └── grasp_demo.launch.py # 启动抓取任务 ├── src/ │ ├── object_detector.py # OpenCV物体识别 │ ├── pose_estimator.py # 位姿计算（手眼标定简化版） │ └── grasp_controller.py # MoveIt!抓取控制 ├── scripts/ │ └── calibrate_camera.py # （可选）简易相机标定 ├── rviz/ │ └── grasp_view.rviz # Rviz可视化配置 ├── demo_video.mp4 # 1分钟演示视频（识别→抓取全过程） └── report.md # 技术报告（难点/解决方案/改进方向） 🧠 二、技术栈与依赖安装核心组件组件作用 ------ ------ UR5机械臂工业级6轴机械臂（Gazebo模型） RGB摄像头 Gazebo中安装在机械臂末端的相机 OpenCV 4.x 物体识别（颜色/形状） MoveIt 2 ROS 2官方运动规划框架 cv_bridge ROS图像消息 ↔ OpenCV图像转换安装命令（Ubuntu 22.04 + ROS 2 Humble） bash UR5 Gazebo模型 sudo apt install ros-humble-universal-robot-gazebo MoveIt 2 sudo apt install ros-humble-moveit OpenCV + cv_bridge sudo apt install ros-humble-vision-opencv python3-opencv 创建工作空间 mkdir -p ~/vision_ws/src cd ~/vision_ws/src git clone https://github.com/ros-planning/moveit2_tutorials.git # 官方示例环境变量（添加到 ~/.bashrc） bash export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/usr/share/gazebo-11/models export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:/opt/ros/humble/lib source /opt/ros/humble/setup.bash 🛠️ 三、分周任务拆解（10-12周） ▶ 第10周：环境搭建 + 物体识别 1. 启动UR5+摄像头仿真 bash 终端1：启动Gazebo ros2 launch ur_gazebo ur5_bringup.launch.py 终端2：添加摄像头到机械臂末端（需修改URDF）修改 /opt/ros/humble/share/ur_description/urdf/ur5.urdf.xacro 在<joint name="tool0">前添加摄像头模型 2. OpenCV物体识别（颜色+形状） python src/object_detector.py import cv2 import numpy as np def detect_object(rgb_image): """识别红色方块（示例）""" hsv = cv2.cvtColor(rgb_image, cv2.COLOR_BGR2HSV) # 红色HSV范围（需根据实际光照调整） lower_red = np.array([0, 100, 100]) upper_red = np.array([10, 255, 255]) mask = cv2.inRange(hsv, lower_red, upper_red) contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) if contours: # 选最大轮廓 c = max(contours, key=cv2.contourArea) x, y, w, h = cv2.boundingRect(c) aspect_ratio = w / h # 过滤非方块（简化版） if 0.8 < aspect_ratio < 1.2: return { "center": (x + w//2, y + h//2), "size": (w, h), "color": "red" } return None 📝 交付：src/object_detector.py + 测试脚本（从摄像头读取→识别→显示框） ▶ 第11周：位姿估计 + 手眼标定简化版 1. 坐标系关系 camera_optical_frame → object ↓ camera_link → ur5_base (通过TF) ↓ base_link (机械臂基座) 2. 位姿计算（简化版） python src/pose_estimator.py import numpy as np import tf2_ros def estimate_object_pose(pixel_x, pixel_y, camera_info, tf_buffer): """计算物体在base_link坐标系下的位姿（简化版）""" # 1. 像素→相机坐标系 (Z=1m 假设) fx, fy = camera_info.k[0], camera_info.k[4] cx, cy = camera_info.k[2], camera_info.k[5] cam_x = (pixel_x - cx) 1.0 / fx cam_y = (pixel_y - cy) 1.0 / fy cam_z = 1.0 # 假设物体在1米处 # 2. 相机坐标系→base_link (通过TF) try: transform = tf_buffer.lookup_transform( "base_link", "camera_optical_frame", tf2_ros.Time() ) # 应用变换（简化：仅平移+旋转矩阵） rot = transform.transform.rotation trans = transform.transform.translation # 四元数→旋转矩阵 R = quat_to_rot_matrix(rot) obj_in_base = R @ np.array([cam_x, cam_y, cam_z]) + np.array([trans.x, trans.y, trans.z]) return { "position": obj_in_base, "orientation": [0, 0, 0, 1] # 默认朝上 } except Exception as e: print(f"TF lookup failed: {e}") return None 📌 简化技巧：完整手眼标定需9点法，但求职项目可用预设Z高度+TF关系替代：假设物体在桌面（Z=0.1m）用ros2 run tf2_ros static_transform_publisher发布相机到机械臂末端的固定TF ▶ 第12周：MoveIt!抓取 + 系统集成 1. MoveIt!配置UR5 bash 生成UR5的MoveIt配置包 ros2 run moveit_setup_assistant setup_assistant 1. Load URDF from /opt/ros/humble/share/ur_description/urdf/ur5.urdf 2. Generate collision matrix 3. Configure controllers (use demo controllers for simulation) 4. Save to ~/vision_ws/src/ur5_moveit_config 2. 抓取控制脚本 python src/grasp_controller.py from moveit.planning import PlanningSceneInterface from moveit_msgs.msg import CollisionObject from geometry_msgs.msg import Pose class GraspController: def __init__(self): self.move_group = MoveGroupCommander("manipulator") # UR5规划组 self.scene = PlanningSceneInterface() def add_table_collision(self): """添加桌子碰撞体""" table = CollisionObject() table.id = "table" table.header.frame_id = "base_link" table_pose = Pose() table_pose.position.z = -0.05 # 桌面高度 table.primitive_poses = [table_pose] table.primitives = [SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[1.0, 1.0, 0.1])] self.scene.add_collision_object(table) def grasp_object(self, object_pose): """抓取物体""" # 1. 规划到预抓取点（物体上方10cm） pre_grasp_pose = object_pose.copy() pre_grasp_pose.position.z += 0.1 self.move_group.set_pose_target(pre_grasp_pose) plan = self.move_group.plan() if plan[0]: # 规划成功 self.move_group.execute(plan[1]) # 2. 下降到抓取点 self.move_group.set_pose_target(object_pose) self.move_group.go(wait=True) # 3. 闭合夹爪（UR5无夹爪，用关节值模拟） self.gripper_close() 3. 一键启动脚本 python scripts/main_grasp_demo.py import rclpy from object_detector import detect_object from pose_estimator import estimate_object_pose from grasp_controller import GraspController def main(): rclpy.init() # 初始化模块 detector = ObjectDetector() estimator = PoseEstimator() controller = GraspController() controller.add_table_collision() # 主循环 while rclpy.ok(): rgb_image = get_camera_image() # 从ROS话题获取 obj = detector.detect_object(rgb_image) if obj: pose = estimator.estimate_pose(obj["center"]) if pose: controller.grasp_object(pose) break rclpy.shutdown() 📝 交付：完整闭环系统 + 1分钟演示视频 + 技术报告 🔍 四、关键难点与解决方案（提前预警）难点解决方案调试技巧 ------ ---------- ---------- Gazebo摄像头无图像检查URDF中camera_link的<sensor>标签 ros2 topic echo /camera/image_raw OpenCV识别不稳定用HSV代替BGR + 动态阈值 + 轮廓过滤单独测试识别脚本：python3 src/object_detector.py test.jpg 位姿计算错误用ros2 run tf2_tools view_frames验证TF树在Rviz中叠加显示识别框和坐标轴 MoveIt!规划失败降低速度/加速度限制 + 添加碰撞体 ros2 service call /plan_kinematic_path ... 单步调试抓取偏移在末端执行器添加tool0偏移 + Z轴补偿用示教模式手动抓取，记录位置 📚 五、核心资源清单类型资源链接 ------ ------ ------ MoveIt 2教程官方文档 https://moveit.ros.org/documentation/ UR5 Gazebo Universal Robots ROS2 Driver https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver OpenCV实战 OpenCV官方教程 https://docs.opencv.org/4.x/d6/d00/tutorial_py_root.html 手眼标定 ROS2手眼标定简化方案 https://github.com/ros-perception/image_pipeline/issues/720 代码参考 MoveIt2视觉抓取示例 https://github.com/moveit/moveit2_tutorials/tree/main/doc/pick_place 📝 六、技术报告模板（report.md） markdown 视觉引导抓取技术报告 1. 系统架构 ![架构图](system_architecture.png) 感知层：Gazebo摄像头 → OpenCV识别决策层：位姿计算 → 抓取点生成执行层：MoveIt!运动规划 → 关节控制 2. 关键技术实现 2.1 物体识别优化问题：初始HSV阈值在不同光照下失效方案：增加轮廓面积过滤 + 长宽比验证代码：object_detector.py#L45-60 2.2 位姿估计简化完整手眼标定需9点法，但求职项目采用： 1. 预设物体高度 Z=0.1m 2. 用静态TF发布 camera→tool0 关系 3. 通过TF树转换到 base_link 误差：±2cm（可接受） 3. 未解决问题与改进方向 [ ] 完整手眼标定（9点法） [ ] 使用YOLO替代传统CV提升鲁棒性 [ ] 力反馈抓取（Gazebo插件） ✅ 七、完成标准检查清单 [ ] Gazebo中UR5+摄像头正常启动，显示图像话题 [ ] OpenCV能稳定识别红色方块（在不同位置/角度） [ ] 位姿计算结果在Rviz中可视化（显示坐标轴） [ ] MoveIt!能规划到抓取点，无碰撞 [ ] 完整闭环：识别→规划→抓取→放置 [ ] 演示视频清晰展示全流程（含失败案例更佳） [ ] 技术报告包含至少2个技术难点与解决方案 💡 八、给面试官的“钩子” 在简历/面试中重点强调： “在视觉抓取项目中，我发现光照变化导致HSV阈值失效，通过增加轮廓几何特征验证（面积/长宽比）将识别率从70%提升至95%。这让我意识到：算法必须适配工程场景，这也是我想深入算法但坚持从软件岗切入的原因。” ✅ 请回复：“第四阶段任务已接收” 我将为你提供： 1. ur5_camera.urdf 预配置文件（含摄像头） 2. MoveIt!配置包快速生成脚本 3. 物体识别参数调优指南（光照/遮挡处理）你正在跨越“软件”与“算法”的边界 —— 12周后，你将拥有让面试官追问细节的硬核项目。下一步，直指Offer！ 🤖📸✨ 我已经完成1. 启动UR5+摄像头仿真，下一步是什么，要实现的内容有哪些，具体怎么做

ROS2 OpenCV颜色形状识别节点

用户已经完成了第一阶段的任务——启动UR5+摄像头仿真。现在需要我给他下一步的明确指示，告诉他接下来要实现什么内容、具体怎么做。

让我回顾一下第四阶段任务的完整规划：

第10周：环境搭建 + 物体识别（已完成环境搭建部分）
第11周：位姿估计 + 手眼标定简化版
第12周：MoveIt!抓取 + 系统集成

根据用户提供的内容：

他们已经按照教程修改了URDF文件，成功启动了Gazebo仿真
这意味着摄像头已经能在Gazebo中工作，并能发布图像话题

现在需要：

确认用户已完成的工作
明确下一步要做什么
给出具体的、可执行的步骤
提供代码示例和操作命令
设置预期交付物

从第10周的任务拆解来看，下一步应该是： OpenCV物体识别（颜色+形状）

具体包括：

创建ROS 2节点订阅摄像头图像话题
使用cv_bridge将ROS图像转换为OpenCV格式
实现颜色阈值分割
轮廓检测与几何特征过滤
在图像上绘制检测结果
发布调试图像话题
准备测试用例

我还需要给出一个清晰的文件结构，让用户知道在哪里创建文件。

需要注意的是，用户之前提供的文件包括：

ur5_with_camera.urdf.xacro（URDF文件）
ur5_camera_bringup.launch.py（启动文件）
package.xml和CMakeLists.txt（包配置）

这些文件已经配置好了，所以下一步是创建实际的Python节点代码。

让我组织一个清晰的回复：

首先确认用户进度
明确下一步目标
提供详细的实现步骤
给出代码模板
说明测试方法
列出交付物清单

🎯 第四阶段任务进展确认 很好！你已经完成了视觉抓取系统最关键的基础设施搭建。现在你的Gazebo环境中，UR5机械臂末端应该已经挂载了摄像头，并成功发布图像话题 /camera/image_raw。

这正是分水岭——从"调参工程师"迈向"算法实现者"的第一步。接下来将直面真实世界的混乱：图像噪声、光照不均、遮挡干扰...

📍 下一步任务：第10周 · OpenCV物体识别节点

目标

创建ROS 2节点，订阅摄像头话题，实现稳定可靠的红色方块识别，输出物体像素坐标和几何特征。

1. 创建节点文件

在你的 vision_grasp_demo/src/ 目录下创建 object_detector.py：

bash

Copy

cd ~/vision_ws/src/vision_grasp_demo/src/
touch object_detector.py
chmod +x object_detector.py

完整代码实现：

Python

Copy

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np

class ObjectDetector(Node):
    def __init__(self):
        super().__init__('object_detector')
        
        # 参数配置
        self.declare_parameter('target_color', 'red')
        self.declare_parameter('min_contour_area', 500)  # 最小轮廓面积（过滤噪点）
        self.declare_parameter('aspect_ratio_tolerance', 0.2)  # 长宽比容差
        
        # CvBridge
        self.bridge = CvBridge()
        
        # 订阅摄像头图像
        self.image_sub = self.create_subscription(
            Image, '/camera/image_raw', self.image_callback, 10
        )
        
        # 发布调试图像（带检测框）
        self.debug_pub = self.create_publisher(Image, '/debug/detection', 10)
        
        self.get_logger().info('ObjectDetector节点已启动 - 等待图像输入...')
    
    def get_color_range(self, color_name):
        """获取HSV颜色范围（ROS参数化版本）"""
        # 注意：Gazebo仿真光照稳定，真实场景需动态调整
        color_ranges = {
            'red': {
                'lower1': np.array([0, 120, 100]),
                'upper1': np.array([10, 255, 255]),
                'lower2': np.array([170, 120, 100]),  # 红色跨区间
                'upper2': np.array([180, 255, 255])
            },
            'blue': {
                'lower': np.array([100, 150, 0]),
                'upper': np.array([140, 255, 255])
            }
        }
        return color_ranges.get(color_name, color_ranges['red'])
    
    def image_callback(self, msg):
        """主处理循环：每帧图像调用一次"""
        try:
            # ROS Image → OpenCV
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            
            # 识别物体
            detection = self.detect_object(cv_image)
            
            if detection:
                # 绘制检测框
                self.draw_detection(cv_image, detection)
                
                # 输出结果
                self.get_logger().info(
                    f"检测到物体: 中心={detection['center']}, "
                    f"尺寸={detection['size']}, 置信度={detection['confidence']:.2f}"
                )
            
            # 发布调试图像
            debug_msg = self.bridge.cv2_to_imgmsg(cv_image, encoding="bgr8")
            self.debug_pub.publish(debug_msg)
            
        except Exception as e:
            self.get_logger().error(f'图像处理失败: {str(e)}')
    
    def detect_object(self, image):
        """
        核心识别算法：颜色阈值 + 几何过滤
        返回值: dict或None
        """
        # 1. 颜色空间转换
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        
        # 2. 颜色阈值分割
        color_name = self.get_parameter('target_color').value
        ranges = self.get_color_range(color_name)
        
        if color_name == 'red':
            # 红色需要合并两个区间
            mask1 = cv2.inRange(hsv, ranges['lower1'], ranges['upper1'])
            mask2 = cv2.inRange(hsv, ranges['lower2'], ranges['upper2'])
            mask = cv1 + cv2.bitwise_or(mask1, mask2)
        else:
            mask = cv2.inRange(hsv, ranges['lower'], ranges['upper'])
        
        # 3. 形态学去噪
        kernel = np.ones((5, 5), np.uint8)
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)  # 开运算去噪
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)  # 闭运算填洞
        
        # 4. 轮廓检测
        contours, _ = cv2.findContours(
            mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE
        )
        
        if not contours:
            return None
        
        # 5. 选择最大轮廓（假设目标最近/最大）
        main_contour = max(contours, key=cv2.contourArea)
        area = cv2.contourArea(main_contour)
        
        # 6. 面积过滤（去掉小噪点）
        min_area = self.get_parameter('min_contour_area').value
        if area < min_area:
            return None
        
        # 7. 形状验证（长宽比）
        x, y, w, h = cv2.boundingRect(main_contour)
        aspect_ratio = w / h if h > 0 else 0
        tolerance = self.get_parameter('aspect_ratio_tolerance').value
        
        if not (1 - tolerance < aspect_ratio < 1 + tolerance):
            # 不是正方形
            return None
        
        # 8. 计算置信度（面积越大越可信）
        confidence = min(area / 5000.0, 1.0)  # 归一化
        
        return {
            "center": (x + w//2, y + h//2),
            "size": (w, h),
            "color": color_name,
            "confidence": confidence,
            "contour": main_contour
        }
    
    def draw_detection(self, image, detection):
        """在图像上绘制检测结果"""
        center = detection['center']
        size = detection['size']
        contour = detection['contour']
        
        # 绘制轮廓
        cv2.drawContours(image, [contour], 0, (0, 255, 0), 2)
        
        # 绘制中心点
        cv2.circle(image, center, 5, (0, 0, 255), -1)
        
        # 绘制信息文本
        text = f"Color: {detection['color']} | Conf: {detection['confidence']:.2f}"
        cv2.putText(image, text, (10, 30), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)


def main(args=None):
    rclpy.init(args=args)
    detector = ObjectDetector()
    
    try:
        rclpy.spin(detector)
    except KeyboardInterrupt:
        detector.get_logger().info('节点已停止')
    finally:
        detector.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

2. 注册为ROS 2可执行节点

修改 CMakeLists.txt，在 ament_package() 前添加：

cmake

Copy

# 安装Python节点
ament_python_install_package(${PROJECT_NAME})

# 创建可执行脚本
install(PROGRAMS
  src/object_detector.py
  DESTINATION lib/${PROJECT_NAME}
)

修改 package.xml，添加依赖：

xml

Copy

<depend>sensor_msgs</depend>
<depend>cv_bridge</depend>
<exec_depend>python3-opencv</exec_depend>

3. 编译并测试

bash

Copy

# 回到工作空间根目录
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 run vision_grasp_demo object_detector.py

# 再开一个终端：查看调试图像
ros2 run rqt_image_view rqt_image_view /debug/detection

4. 创建测试场景

在Gazebo中放置红色方块：

bash

Copy

# 方法一：手动添加
在Gazebo界面左侧 → Insert → 选择立方体 → 设置颜色为红色

# 方法二：使用Spawn Entity（推荐）
ros2 run gazebo_ros spawn_entity.py -entity red_box -database cube -x 0.3 -y 0.1 -z 0.05

# 修改立方体颜色为红色
# 在Gazebo的模型编辑器中，设置材质颜色 rgba="1 0 0 1"

5. 参数调优（关键！）

创建配置文件 config/detection_params.yaml：

yaml

Copy

object_detector:
  ros__parameters:
    target_color: "red"
    min_contour_area: 300  # Gazebo中可适当调小
    aspect_ratio_tolerance: 0.3  # 对形状要求宽松些

启动时加载参数：

bash

Copy

ros2 run vision_grasp_demo object_detector.py --ros-args --params-file config/detection_params.yaml

6. 本周交付物清单

[ ] src/object_detector.py 核心识别代码（需包含完整的HSV过滤、形态学处理、几何验证）
[ ] config/detection_params.yaml 参数配置文件（至少包含颜色、面积阈值、长宽比容差）
[ ] 测试截图/录屏（显示 /debug/detection 话题，能清晰看到红框和中心点）
[ ] 调优日志（记录至少3次参数调整过程：初始值 → 问题 → 调整 → 效果）

7. 本周调试技巧

常见问题排查指南：

Table

现象	可能原因	排查命令
终端无输出	未订阅到图像	`ros2 topic hz /camera/image_raw`
全黑调试图像	CvBridge转换失败	检查 `encoding="bgr8"` 是否匹配
识别率低	HSV阈值不准	用 `rqt_image_view` 实时调阈值
噪点太多	形态学核太小	增大 `kernel = np.ones((7,7), np.uint8)`
只能识别近处物体	面积阈值过大	调小 `min_contour_area`

高级调试技巧：

bash

Copy

# 实时查看HSV阈值效果（在rqt中动态调参）
ros2 run rqt_reconfigure rqt_reconfigure

# 录制rosbag用于离线调试
ros2 bag record /camera/image_raw -o detection_test

🎯 下一预告：第11周任务

当你能稳定输出像素坐标后，我们将进入最硬核的环节：

手眼标定简化版：从像素坐标 → 相机坐标 → 机械臂基座坐标
TF2坐标变换：用 tf2_ros 实现坐标系转换
误差分析：量化位姿估计误差（控制在±2cm内）

下周交付物将包含数学推导（旋转平移矩阵），这是简历上"具备机器人算法基础"的硬证据。

💪 现在，去实现你的第一个感知算法节点吧！记住：参数调优过程本身就是算法工程能力的体现。遇到任何问题，随时回来提问。

完成后请回复："第10周任务完成，已稳定识别红色方块"，并附上调试截图。