🤖 告别“盲抓”！Orbbec 3D视觉+XELA触觉，定义机器人精密操作的“新标准”

> 🌟 项目来源：第四届3D视觉创新应用竞赛 | 闫腾 | 深圳技术大学

> 🔗 开源地址：[RobotV_Visual-tactile-fusion]

🧠 项目概述

在3C制造与半导体行业中，硅晶圆、芯片裸片等脆性材料的抓取一直是自动化领域的“深水区”——力量小了抓不稳，力量大了直接碎。传统的纯视觉方案能解决“在哪”的问题，却无法感知“多紧”的极限。

本项目打造了一套视-触觉融合感知的机器人灵巧抓取系统，核心思路是“视觉定位于宏观，触觉感知于微观”：

– 3D视觉（Orbbec Gemini 2）：精准捕捉目标的3D位姿、形状与表面特征，解决“看得到、看得准”的问题；

– 触觉反馈（XELA传感器）：弥补视觉在接触瞬间的盲区，感知材料硬度与质地，实现力控闭环；

– Sim2Real仿真：借助 Isaac Sim 的光线追踪与物理引擎，在虚拟空间完成策略训练与验证，实现虚实无缝迁移。

🏗️ 系统架构

系统采用感知-决策-执行的经典闭环架构，软硬解耦，模块化设计：

层级	核心组件	关键技术/硬件
👁️ 感知层	3D视觉 + 触觉	Orbbec Gemini 2 (RGB-D对齐)、XELA触觉阵列
🧠 决策层	YOLO检测 + IL策略	目标位姿估计、Sim2Real模仿学习
🦾 执行层	机械臂 + 灵巧手	Diana 7 (7-DOF力控机械臂)、Seed Robotics (多指灵巧手)

数据流向：

🧩 核心模块说明

1. 🎯 3D视觉与位姿估计模块

基于 pyorbbecsdk 深度定制，不局限于基础的点云获取：

– 硬同步对齐：采用硬件级（HW）深度与RGB对齐，确保边缘脆性材料的深度映射无重影；

– 2D到3D映射：通过相机内参矩阵 getxyz() 函数，将 YOLO 检测到的2D Bounding Box 中心点精准反投影为机械臂基坐标系下的3D空间坐标；

def getxyz(distance,u,v):
    cx = 955.01
    cy = 516.37
    fx = 1101.41
    fy = 1101.58

    z = distance
    x = ((u-cx)*z)/fx
    y = ((v-cy)*z)/fy -15

    return x,y,z

– 时域滤波：集成 TemporalFilter，在抓取逼近阶段平滑深度噪声，避免因深度跳变导致机械臂抖动。

class TemporalFilter:
    def __init__(self, alpha):
        self.alpha = alpha
        self.previous_frame = None

    def process(self, frame):
        if self.previous_frame is None:
            result = frame
        else:
            result = cv2.addWeighted(frame, self.alpha, self.previous_frame, 1 - self.alpha, 0)
        self.previous_frame = result
        return result

2. 🦾 机械臂力控与运动规划模块

封装了 DianaApi，从粗暴的关节运动迈向细腻的力位混合控制：

– 轨迹规划：moveJToTarget 与 moveTCP 结合，实现大范围快速趋近与末端微调的平滑过渡；

def moveJToTarget(joints, v, a, ipAddress=''):
    dianaJointPos = DIANA_JOINTS_POSITION()
    dianaJointPos.values = (c_double * 7)()
    for index in range(0, len(joints)):
        dianaJointPos.values[index] = joints[index]
    api_mod.moveJToTarget.argtypes = [
        POINTER(DIANA_JOINTS_POSITION), c_double, c_double, c_char_p]
    api_mod.moveJToTarget.restype = c_int
    ret = api_mod.moveJToTarget(
        dianaJointPos, v, a, bytes(ipAddress.encode('utf-8')))
    return message(ret)
def moveTCP(d, v, a, ipAddress=''):
    active_tcp = c_void_p(0)
    api_mod.moveTCP.argtypes = [c_int, c_double, c_double, c_void_p, c_char_p]
    api_mod.moveTCP.restype = c_int
    ret = api_mod.moveTCP(d.value, v, a, active_tcp,
                          bytes(ipAddress.encode('utf-8')))
    return message(ret)

– 柔顺控制：底层支持 setCartImpeda（笛卡尔阻抗）与 enterForceMode（力控模式），这是抓取脆性材料的核心，让机械臂在接触瞬间具备“柔性”，避免硬接触压碎晶圆；

def setCartImpeda(arrStiff, arrDampRatio, ipAddress=''):
    cartStiff = DIANA_CART_STIFF()
    cartStiff.values = (c_double*6)()
    for index in range(0, len(cartStiff.values)):
        cartStiff.values[index] = arrStiff[index]
    api_mod.setCartImpeda.argtypes = [
        POINTER(DIANA_CART_STIFF), c_double, c_char_p]
    api_mod.setCartImpeda.restype = c_int
    ret = api_mod.setCartImpeda(
        cartStiff, arrDampRatio[0], bytes(ipAddress.encode('utf-8')))
    return message(ret)
def enterForceMode(frame_type, frame_matrix, force_direction, force_value, max_approach_velocity, max_allow_tcp_offset, ipAddress=''):
    dianaFrameMatrix = DIANA_FRAME_MATRIX()
    dianaFrameMatrix.values = (c_double * 16)()
    for index in range(0, len(dianaFrameMatrix.values)):
        dianaFrameMatrix.values[index] = frame_matrix[index]
    dianaForceDirection = DIANA_FORCE_DIRECTION()
    dianaForceDirection.values = (c_double * 3)()
    for index in range(0, len(dianaForceDirection.values)):
        dianaForceDirection.values[index] = force_direction[index]
    api_mod.enterForceMode.argtypes = [c_int, POINTER(DIANA_FRAME_MATRIX), POINTER(
        DIANA_FORCE_DIRECTION), c_double, c_double, c_double, c_char_p]
    api_mod.enterForceMode.restype = c_int
    ret = api_mod.enterForceMode(frame_type, byref(dianaFrameMatrix), byref(
        dianaForceDirection), force_value, max_approach_velocity, max_allow_tcp_offset, bytes(ipAddress.encode('utf-8')))
    return message(ret)

– 状态护航：完善的错误码字典（如碰撞检测 ERROR_CODE_COLLISION_ERROR、急停 ERROR_CODE_EMERGENCY_STOP），确保异常工况下设备安全。

errorCodeMessage = {
    0: 'NO_ERROR_CODE',
    -1000: 'ERROR_CODE_SOCKET_OTHER_ERROR',
    -1001: 'ERROR_CODE_WSASTART_FAIL',
    -1002: 'ERROR_CODE_CREATE_SOCKET_FAIL',
    -1003: 'ERROR_CODE_BIND_PORT_FAIL',
    -1004: 'ERROR_CODE_SOCKET_READ_FAIL',
    -1005: 'ERROR_CODE_SOCKET_TIMEOUT',
    -1006: 'ERROR_CODE_RECVFROM_FAIL',
    -1007: 'ERROR_CODE_SENDTO_FAIL',
    -1008: 'ERROR_CODE_LOST_HEARTBEAT',
    -1009: 'ERROR_CODE_LOST_ROBOTSTATE',
    -1010: 'ERROR_CODE_GET_DH_FAILED',
    -1011: 'ERROR_CODE_RELEASE_BRAKE_FAILED',
    -1012: 'ERROR_CODE_HOLD_BRAKE_FAILED',
    -2001: 'ERROR_CODE_JOINT_REGIST_ERROR',
    -2101: 'ERROR_CODE_COMMUNICATE_ERROR',
    -2201: 'ERROR_CODE_CALLING_CONFLICT_ERR',
    -2202: 'ERROR_CODE_COLLISION_ERROR',
    -2203: 'ERROR_CODE_NOT_FOLLOW_POSITION_CMD',
    -2204: 'ERROR_CODE_NOT_FOLLOW_TCP_CMD',
    -2205: 'ERROR_CODE_NOT_ALL_AT_OP_STATE',
    -2206: 'ERROR_CODE_OUT_RANGE_FEEDBACK',
    -2207: 'ERROR_CODE_EMERGENCY_STOP',
    -2208: 'ERROR_CODE_NO_INIT_PARAMETER',
    -2209: 'ECODE_NOT_MATCH_LOAD',
    -2210: 'ECODE_CANNOT_MOVE_WHILE_FREE_DRIVING', 
    -2211: 'ECODE_CANNOT_MOVE_WHILE_ZERO_SPACE_FREE_DRIVING',
    -2212: 'ECODE_PASSTHROUGH_FIFO_OVERFLOW_UP_ERROR',
    -2213: 'ECODE_PASSTHROUGH_FIFO_OVERFLOW_DOWN_ERROR',
    -2214: 'ERROR_CODE_ROBOT_IN_VIRTUAL_WALL',
    -2215: 'ERROR_CODE_CONFLICT_TASK_RUNNING',
    -2301: 'ERROR_CODE_PLAN_ERROR',
    -2302: 'ERROR_CODE_INTERPOLATE_ERROR',
    -2303: 'ERROR_CODE_INTERPOLATE_TORQUE_ERROR',
    -2304: 'ERROR_CODE_SINGULAR_VALUE_ERROR',
    -3001: 'ERROR_CODE_RESOURCE_UNAVAILABLE',
    -3002: 'ERROR_CODE_DUMP_LOG_TIMEOUT',
    -3003: 'ERROR_CODE_DUMP_LOG_FAILED',
    -3004: 'RESET_DH_FAILED',
}

3. 🧠 模仿学习与策略迁移模块

项目没有停留在硬编码阶段，而是探索了基于 IL（Imitation Learning）的智能抓取：

– PyBullet 仿真：构建 Diana7 与目标的物理仿真环境，利用逆运动学（IK）生成专家演示数据；

– 策略网络：采用 Keras 搭建 MLP 网络，以状态为输入，预测各关节目标角度；

– Sim2Real：通过 Isaac Sim 的高级物理仿真弥补 Sim-to-Real 的 Gap，让网络在真机上具备鲁棒性。

📑 设计要点

通过提炼 Diana7 API 官方文档，本项目在工程实现上重点挖掘了以下“神级”接口：

1. 力矩与阻抗控制：

setJointImpeda / setCartImpeda：关节/笛卡尔空间阻抗设置，抓取脆性材料时的“弹簧”效应全靠它；

getTcpExternalForce：实时获取末端外力，结合 XELA 触觉做双重力觉闭环。

2. 高阶轨迹控制：

servoJ / servoL：实时关节/笛卡尔伺服控制，为后续接入强化学习（RL）的高频输出铺平道路；

createComplexPath：支持多段 MoveJ/MoveL/MoveC 复合轨迹，实现“趋近-减速-接触-抬起”的流水线编排。

3. 安全机制：

setCartCollision / isCollision：设置笛卡尔空间碰撞边界与检测，防止脆性材料被过度挤压。

🚀 使用示例

以下展示系统从“视觉定位”到“抓取抬起”的核心逻辑：

# 1. 视觉系统初始化与目标定位
pipeline = Pipeline()
config.enable_stream(color_profile) # 开启RGB-D流
frames = pipeline.wait_for_frames(100)
color_image = frame_to_bgr_image(frames.get_color_frame())

# YOLO 检测与 3D 坐标转换
result, is_label = yolo.predict(color_image)
if is_label:
    u, v = get_target_center(result) # 获取像素中心
    distance = depth_frame.get_distance(u, v)
    x, y, z = getxyz(distance, u, v) # 像素坐标转3D空间坐标

# 2. 机械臂运动与力控抓取
DianaApi.releaseBrake() # 释放抱闸
DianaApi.moveJToTarget(pose_ori, vel, acc, ip) # 运动至目标正上方
DianaApi.moveTCP(T_MOVE_Z_UP, vel, acc, ip)   # 沿TCP Z轴缓慢下降

# 开启阻抗控制，确保接触安全
DianaApi.setCartImpeda(stiffness=[...], damping=[...]) 
DianaApi.enterForceMode(...)

# 3. 灵巧手闭合与触觉反馈微调
motionhub.fork_grab() # Seed灵巧手执行包覆抓取
# (此处可接入 XELA 触觉反馈，若检测到滑移则增加握力)
DianaApi.moveTCP(T_MOVE_Z_UP, vel, acc, ip) # 安全抬起

🧭 后续规划

目前系统已打通“视-触-动”全链路，但在泛化性与智能水平上仍有广阔空间：

– 🧪 触觉闭环强化：深度整合 XELA 传感器数据，构建基于触觉图像的滑移检测网络，实现“感知即调整”的动态力控；

– 🤖 端到端策略升级：从当前的模仿学习（IL）向视觉-动作（VLA）大模型演进，减少对显式坐标转换的依赖，实现“所见即所抓”；

– 🏥 场景泛化拓展：将验证成熟的“脆性抓取”范式迁移至医疗手术器械递送、危险品排爆等对力控要求极苛刻的场景；

– 🔄 Isaac Sim 全量仿真：完善 Sim2Real 流程，通过域随机化提升策略在复杂光照、遮挡环境下的鲁棒性。

> 💡 “给机器人一双不碎的眼睛和懂得温柔的双手，是具身智能走向精密制造的第一步。"