让机器人“看懂”世界：华南理工团队打造精准智能抓取系统

来源：第四届 3D 视觉创新应用竞赛 | 华南理工大学 | 毛一非 项目名称：基于奥比中光 3D 相机的识别取物机器人

在智能制造、自动化分拣和移动机器人场景中，机器人的关键能力正在从“能动”走向“会看、会判断、会执行”。华南理工大学团队基于奥比中光 Gemini 2 双目结构光 3D 相机、NVIDIA Jetson Orin Nano 开发板和自研运动控制系统，搭建了一套面向颜色识别、距离测量与目标抓取的精准智能抓取系统。

这套系统能够通过 RGB-D 视觉数据识别红、蓝目标物体，并结合深度信息估算目标距离，再把识别结果转化为底盘移动、取球、升降、投放等动作。它不是单纯的视觉识别 Demo，而是一套从感知、计算到控制闭环都较完整的中大型机器人方案。

一、项目概述

随着工业自动化和智能制造快速发展，机器人已经成为提升生产效率、稳定产品质量和减少重复劳动的重要工具。尤其在电子组装、食品分拣、制药、资源回收等场景中，系统不仅需要“移动到位”，还需要准确识别目标、判断距离，并稳定完成抓取或搬运。

本项目面向这一类需求，开发了一套基于 3D 视觉的识别取物机器人。它通过双目结构光相机获取彩色图像和深度图像，利用颜色差分与局部深度采样算法识别目标球体，再由 Jetson Orin Nano 完成视觉处理，并通过 STM32 控制底盘、电机、舵机等执行机构。

相关资源

🔗 开源代码：RobotV_Identify-and-retrieve-items

📖奥比中光开发文档：Orbbec SDK for Python 文档

二、系统架构：视觉、计算与控制协同

整个系统采用分层架构：前端由 3D 相机完成环境感知，中间由 Jetson 进行图像处理和目标判断，底层由 STM32、CAN 总线、电机和舵机完成运动控制。这样的架构让视觉算法和执行控制相对解耦，也便于后续替换算法或扩展机械结构。

硬件组成

模块	设备 / 方案	作用
感知层	奥比中光 Gemini 2 双目结构光 3D 相机	同步获取彩色图像与深度图像，支持目标识别和距离测量
计算层	NVIDIA Jetson Orin Nano	运行 Orbbec SDK for Python 与 OpenCV 视觉处理程序
控制层	STM32F407 + FreeRTOS	负责电机控制、任务调度、串口通信和执行机构控制
执行层	全向舵轮底盘	实现前后、横移、斜向移动和原地旋转
执行层	3508 电机、2006 电机、DS3230 舵机	驱动底盘、取球机构和放球机构
反馈层	编码器、光电门等传感器	提供电机状态、舵轮角度和机构位置反馈

三、核心算法：RGB-D 融合目标识别

视觉识别部分的核心思路很直接：先用 RGB 图像进行颜色筛选，定位最可能的目标区域；再利用深度图在目标中心附近取样，得到更稳定的距离估计。这样既减少了全图深度处理的计算量，也避免只依赖颜色导致距离信息缺失。

算法主流程

原来的“算法流程表”更适合改成流程代码块。读者可以一眼看到系统每一轮识别如何运行：

while robot_is_running:
    1. 从 Gemini 2 获取一帧 RGB 图像和一帧深度图
    2. 将深度图从原始 Y16 数据转换为毫米单位
    3. 过滤无效深度值，并裁剪掉边缘干扰区域
    4. 把 1280 x 720 的 RGB 图像切成 5 x 4 个候选块
    5. 计算每个候选块的 B、G、R 平均值
    6. 通过 R-B 和 B-R 差分找出红球、蓝球候选块
    7. 在候选块中心附近取 3 x 3 深度网格
    8. 输出目标颜色、图像位置和平均深度

1. 深度数据获取

getCurrentFrame() 负责从 Gemini 2 相机中获取彩色帧和深度帧。程序使用 Orbbec SDK 的 Pipeline 模式启用彩色流和深度流，其中深度流采用 Y16 格式，并通过 get_depth_scale() 将原始数据转换为毫米级深度值。

下面是根据项目脚本整理后的核心逻辑。为了便于阅读，省略了部分异常处理和重复判断：

def getCurrentFrame():
    pipeline = Pipeline()
    config = Config()

    # 1. 启用彩色流
    color_profiles = pipeline.get_stream_profile_list(OBSensorType.COLOR_SENSOR)
    color_profile = color_profiles.get_default_video_stream_profile()
    config.enable_stream(color_profile)

    # 2. 启用深度流：1280 宽、Y16 格式、30 fps
    depth_profiles = pipeline.get_stream_profile_list(OBSensorType.DEPTH_SENSOR)
    depth_profile = depth_profiles.get_video_stream_profile(
        1280, 0, OBFormat.Y16, 30
    )
    config.enable_stream(depth_profile)

    pipeline.start(config)

    frames = pipeline.wait_for_frames(100)
    depth_frame = frames.get_depth_frame()
    color_frame = frames.get_color_frame()

    # 3. 将深度数据转换成真实距离
    width = depth_frame.get_width()
    height = depth_frame.get_height()
    scale = depth_frame.get_depth_scale()

    depth_data = np.frombuffer(depth_frame.get_data(), dtype=np.uint16)
    depth_data = depth_data.reshape((height, width))
    depth_data = depth_data.astype(np.float32) * scale

    # 4. 过滤过近、过远和无效数据
    depth_data = np.where(
        (depth_data > MIN_DEPTH) & (depth_data < MAX_DEPTH),
        depth_data,
        0
    ).astype(np.uint16)

    # 5. 保存彩色帧，供颜色识别函数读取
    img = frame_to_bgr_image(color_frame)
    cv2.imwrite("color.jpg", img)

    # 6. 裁剪深度图上下边缘，减少无效区域干扰
    return depth_data[40:760][:]

这段逻辑的关键价值在于：它不是只拿 RGB 图片做颜色识别，而是同步保留了深度图，让后续算法能够知道目标“在哪里”和“离相机多远”。

2. 颜色识别：差分峰值判断

系统将彩色图像划分为 20 个区域，并计算每个区域的 BGR 平均值。相比直接与固定色卡做绝对匹配，项目采用红蓝通道的相对差值进行判断，例如红色区域更关注 R-B 的差异，蓝色区域更关注 B-R 的差异。

def split_image_to_grid(img):
    """将 1280 x 720 图像切成 5 x 4 个区域，每块 256 x 180 像素。"""
    regions = []
    for row in range(4):
        for col in range(5):
            x1, x2 = col * 256, (col + 1) * 256
            y1, y2 = row * 180, (row + 1) * 180
            regions.append(img[y1:y2, x1:x2])
    return regions


def calc_mean_bgr(region):
    """计算候选区域的平均 B、G、R 值。"""
    b, g, r = cv2.mean(region)[:3]
    return round(b), round(g), round(r)

红色与蓝色并不是靠“某个固定阈值”一刀切，而是在所有候选区域中寻找最明显的颜色差异：

max_red = {"idx": None, "score": -1}
max_blue = {"idx": None, "score": -1}

for idx, region in enumerate(regions):
    b, g, r = calc_mean_bgr(region)

    # 红色候选：红色通道足够高，或红蓝差值足够明显
    red_like = (
        (r >= 130 and g < 100 and b < 100)
        or r >= 200
        or (r - b) >= 80
    )
    if red_like and (r - b) > max_red["score"]:
        max_red = {"idx": idx, "score": r - b}

    # 蓝色候选：蓝色通道占优，并排除灰白区域误判
    gray_like = (
        abs(b - g) <= 5
        and abs(g - r) <= 5
        and abs(b - r) <= 5
    )
    blue_like = (
        (b >= 100 and r <= 150 and g <= 140)
        or b > r
    )
    if blue_like and not gray_like and (b - r) > max_blue["score"]:
        max_blue = {"idx": idx, "score": b - r}

这段算法的优势在于：现实环境中的光照并不稳定，绝对 RGB 值容易漂移，而通道之间的相对差值通常更能反映目标颜色本身。换句话说，系统关注的不是“这个像素是不是标准红色”，而是“它是不是当前画面里最像红色目标的区域”。

3. 深度采样：3 x 3 局部均值

识别到目标区域后，程序会根据区域编号计算中心点，并在中心点附近取 3 x 3 的小窗口进行深度采样。只要该窗口中存在有效深度点，就计算有效点的平均值；如果全部无效，则返回错误码，提醒上层逻辑该位置深度不可靠。

def get_center_pos(idx):
    """根据 5 x 4 网格编号，计算区域中心点坐标。"""
    row = idx // 5
    col = idx % 5
    pos_y = row * 180 + 90
    pos_x = col * 256 + 128
    return pos_y, pos_x


def get_depth(depth_data, pos_y, pos_x):
    """在中心点附近取 3 x 3 有效深度均值。"""
    valid_depths = []

    for y in range(pos_y - 1, pos_y + 2):
        for x in range(pos_x - 1, pos_x + 2):
            depth = depth_data[y][x]
            if depth != 0:
                valid_depths.append(depth)

    if not valid_depths:
        return -110000

    return sum(valid_depths) / len(valid_depths)

从实现上看，3 x 3 采样虽然很小，但它同时解决了三个问题：过滤单点噪声、跳过无效深度值、避免全图深度遍历带来的算力浪费。这也是系统能在边缘计算平台上保持实时性的关键之一。

四、运动控制：全向移动与闭环执行

识别目标只是第一步，机器人还需要稳定移动到目标位置，并完成取球、升降和投放动作。项目的控制部分基于 STM32F407、FreeRTOS、CAN 总线和多电机 PID 控制模型构建，整体强调实时性和稳定性。

全向舵轮底盘

传统轮式底盘通常需要先转向再前进，而全向舵轮底盘可以在水平面内实现任意方向移动。项目中的 motorctrl.c 根据 Vx、Vy 和 omega 计算四个舵轮模块的目标角度与速度，并通过方向优化减少舵轮旋转幅度。

用伪代码可以概括为：

void ctrlmotor(double Vx, double Vy, double omega) {
    // 1. 设置死区，避免摇杆轻微抖动造成底盘漂移
    if (fabs(Vx) < 100) Vx = 0;
    if (fabs(Vy) < 100) Vy = 0;
    if (fabs(omega) < 100) omega = 0;

    // 2. 根据底盘平动速度与旋转速度，计算四个舵轮目标角度
    wheel[0].target_angle = atan2(Vy - omega * cos(alpha),
                                  Vx - omega * sin(alpha));
    wheel[1].target_angle = atan2(Vy - omega * cos(alpha),
                                  Vx + omega * sin(alpha));
    wheel[2].target_angle = atan2(Vy + omega * cos(alpha),
                                  Vx + omega * sin(alpha));
    wheel[3].target_angle = atan2(Vy + omega * cos(alpha),
                                  Vx - omega * sin(alpha));

    // 3. 选择最短转向路径，必要时反转轮速，减少舵轮旋转幅度
    for (int i = 0; i < 4; i++) {
        optimize_steering_direction(&wheel[i]);
    }

    // 4. 将目标速度与目标角度写入 PID 控制模型
    update_pid_targets(wheel);
}

这段控制逻辑让机器人具备前后移动、左右横移、斜向移动和原地旋转能力。对于抓取任务来说，横移和斜移尤其重要，因为机器人可以在不大幅改变朝向的情况下完成精细对位。

PID 闭环控制

项目使用由 MATLAB Simulink 生成的 PID 控制模型，覆盖 3508 与 2006 电机的速度环和角度环。控制模型中加入了输出限幅、滤波、死区判断和积分处理，能够在复杂机械负载下保持更稳定的响应。

下面的伪代码展示了速度环控制的主要思想：

float speed_pid_step(float target_rpm, float current_rpm) {
    float error = target_rpm - current_rpm;

    // 比例项：快速响应当前误差
    float p_out = Kp * error;

    // 积分项：消除长期稳态误差
    integral += Ki * error * dt;

    // 微分项：抑制变化过快带来的震荡
    float derivative = (error - last_error) / dt;
    float d_out = Kd * derivative;

    float output = p_out + integral + d_out;

    // 输出限幅，保护电机和驱动器
    output = clamp(output, -16384, 16384);

    last_error = error;
    return output;
}

在实际工程中，PID 输出并不会停留在软件内部，而是会被写入 CAN 控制指令，最终驱动电机：

void assign_output(void) {
    for (int i = 0; i < 16; i++) {
        output[i] = (rtU.yaw_status[i] == VEL)
            ? rtY.yaw_SPD_OUT[i]
            : rtY.yaw_ANG_OUT[i];
    }

    CAN1_cmd_chassis(output[0], output[1], output[2], output[3],
                     output[4], output[5], output[6], output[7]);

    CAN2_cmd_chassis(output[8], output[9], output[10], output[11],
                     output[12], output[13], output[14], output[15]);
}

FreeRTOS 任务调度

FreeRTOS 中创建了默认任务、取球任务和升降任务。默认任务持续调整舵轮状态；取球任务根据按键或控制条件启动取球动作；升降任务结合位置反馈控制上升、投放和复位。

void MX_FREERTOS_Init(void) {
    defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
    ThrowBallHandle   = osThreadNew(Throw,            NULL, &ThrowBall_attributes);
    RiseHandle        = osThreadNew(RiseBall,         NULL, &Rise_attributes);
}

void Throw(void *argument) {
    for (;;) {
        if (condition[0] == 1) {
            set_getball(3390);  // 启动取球电机
            movement_take();    // 执行取球机构动作
            motor_off();        // 动作完成后关闭电机
        }
    }
}

void RiseBall(void *argument) {
    for (;;) {
        if (condition[1] == 1) {
            rtU.yaw_target4 = -1500;  // 升降机构上升

            if (rise_position_is_ready()) {
                throw_ball();         // 到位后投放
                rtU.yaw_target4 = 2000;
            }
        }
        osDelay(1);
    }
}

定时中断则承担“心跳”角色：更新遥控输入、刷新看门狗、读取电机反馈、计算 PID、发送 CAN 控制量。它把视觉决策和执行动作连接成连续闭环。

TIM10 interrupt every control cycle:
    read joystick values -> Vx / Vy / omega
    if no signal for too long -> stop chassis
    refresh watchdog
    read encoder and motor feedback
    set speed / angle mode for each motor
    calculate wheel targets
    run pidctl_step()
    send CAN outputs
    update motor state

五、开发与测试过程

项目部署在 NVIDIA Jetson Orin Nano 上，系统环境为 Linux 20.04，视觉程序主要使用 Python、OpenCV 和 Orbbec SDK for Python。相机调试阶段通过 Orbbec Viewer 观察彩色流、深度流和红外流，再将官方示例迁移到项目识别程序中。

Jetson 与相机部署流程

部署步骤也比表格更适合用命令块展示。下面这段整理自项目中的 Orin 启动说明：

# 1. 进入 Orbbec Python SDK 目录
cd pyorbbecsdk/pyorbbecsdk

# 2. 设置 PYTHONPATH，让 Python 能找到 SDK 库文件
export PYTHONPATH=$PYTHONPATH:$(pwd)/install/lib/

# 3. 安装并刷新 udev 规则，让系统正确识别深度相机
sudo bash ./scripts/install_udev_rules.sh
sudo udevadm control --reload-rules && sudo udevadm trigger

# 4. 运行 RGB-D 识别程序
python3 /home/orin/pyorbbecsdk/RGBD_detection.py

# 5. 如需单独测试图像采集，可运行测试脚本
python3 /home/orin/pyorbbecsdk/imgtest.py

在实际调试中，团队还通过 arp -a 查找 Jetson IP，使用 SSH、串口或 MobaXterm 登录开发板，并借助 Orbbec Viewer 查看彩色流、深度流和红外流，逐步确认相机、网络与 Python 环境都处于可用状态。

关键技术难点与应对

1. 光照与背景噪声
   challenge: 复杂光照会改变颜色表现，导致误判。
   solution : 使用 R-B、B-R 这样的通道差分，而不是只依赖绝对 RGB 值。

2. 实时数据处理
   challenge: 高分辨率 RGB-D 数据会带来较大计算压力。
   solution : 先用 RGB 区域粗筛，再只在目标中心附近采样深度。

3. 深度数据可靠性
   challenge: 遮挡、反光或目标表面不均会影响深度。
   solution : 过滤深度为 0 的无效点，并对 3 x 3 局部区域求均值。

4. 仿真场景建设
   challenge: 需要在 Isaac Sim 中复现机器人和使用场地。
   solution : 使用 USD、STEP、Blend 等模型资源构建仿真环境。

5. 机械取放球
   challenge: 成功率依赖机械结构、传感器反馈和控制参数协同。
   solution : 结合 PID、光电门、编码器和任务动作序列联合优化。

六、项目优势与应用前景

项目的价值不只在于“识别一个颜色”，而在于把 3D 视觉、实时计算、底盘运动和机械执行连接成完整系统。相比单一传感器或单一视觉算法，这种多传感器方案更适合面向真实场景迭代。

核心优势

优势	体现
识别稳定	颜色差分算法降低光照变化影响
定位更完整	RGB 识别与深度测量结合，既知道“是什么”，也知道“有多远”
运动灵活	全向舵轮底盘支持横移、斜移和原地旋转
控制可靠	Simulink 生成 PID 模型，结合编码器与 CAN 控制
易于调试	Orbbec Viewer、Python 示例和模块化工程便于开发验证
成本可控	相比部分激光雷达方案，双目结构光相机兼具视觉输出与距离感知

应用场景

应用方向	典型场景	系统价值
工业制造	自动化生产线分拣、色彩质量检测、物料搬运	提升分拣效率与一致性
资源回收	按颜色或类别识别可回收物	提高回收分类效率
环境监测	识别水质、植被或污染事件中的颜色变化	辅助快速巡检与数据采集
教育科研	机器人视觉、运动控制、嵌入式开发实验	适合作为综合实践平台
交互展示	识别彩色物体并触发机械动作	增强展陈互动性

七、技术栈总结

类型	技术 / 工具
3D 视觉	奥比中光 Gemini 2、Orbbec SDK for Python、RGB-D 数据采集
图像处理	Python、OpenCV、BGR 均值统计、颜色差分判断
边缘计算	NVIDIA Jetson Orin Nano、Linux 20.04
嵌入式控制	STM32F407、FreeRTOS、HAL、DMA、UART
电机控制	CAN 总线、3508 电机、2006 电机、PID 速度环与角度环
机械执行	全向舵轮底盘、升降机构、取球机构、舵机投放机构
建模仿真	Isaac Sim、USD、STEP、Blender

结语

这套识别取物机器人展示了 3D 视觉在机器人系统中的实际价值：相机负责感知世界，算法负责理解目标，控制系统负责把判断转化为动作。通过 RGB-D 融合识别、局部深度采样、全向移动和闭环 PID 控制，项目为面向真实场景的智能抓取机器人提供了一条清晰、可复用的实现路径。