米尔RK3576 + ROS2 进阶：NPU加速MixFormerV2目标跟随与机械臂抓取实战

前言

回顾上篇：基于RK3576 + ROS2 Humble + SLAM Toolbox + Nav2，我们实现了机器人的建图与自主导航。机器人已经能够“走到哪里”。但真正的智能机器人不仅要“走到哪”，还要“看到并操作”——识别特定物体、主动跟随、近距离抓取。本文将在此基础上，集成深度摄像头，实现机器人核心功能：

• • 使用米尔RK3576 NPU加速MixFormerV2进行目标跟踪，替代传统OpenCV算法；
• • 移动底盘跟随目标物体，保持安全距离；
• • 机器人机械臂抓取物体，完成“识别-跟随-抓取”闭环。

名词预先了解：

• • 手眼转换：将相机看到的物体坐标转换到机械臂可执行的坐标系下。
• • 逆运动学：给定末端目标位姿，反解出机械臂各关节应转动的角度。

图：米尔基于RK3576核心板开发板

第一章：系统总体架构与硬件连接

1.1 硬件组成

• • 主控平台：米尔基于RK3576核心板开发板（内置6TOPS NPU）
• • 深度摄像头：RGB-D深度相机（输出RGB、IR、深度三路数据）
• • 机械臂：6轴轻量机械臂（串口控制）
• • 移动底盘：STM32控制，麦克纳姆轮

1.2 软件模块与数据流

整个系统分为五个核心ROS2节点：

整体数据流：
相机 → MixFormerV2跟踪器 → 手眼转换 → 底盘跟随节点 → 靠近停止 → 机械臂逆运动学规划 → 抓取执行。

第二章：深度相机数据获取

与普通USB摄像头不同，深度相机在ROS2下通过标准驱动节点发布话题数据。本文使用的RGB-D相机输出三路信息：

• • RGB图像：用于目标跟踪的视觉输入
• • IR图像：辅助深度计算（夜间或弱光可用）
• • 深度图像：每个像素的毫米级距离值

相机输出格式为：640×400 NV12，帧率13~15 FPS。主控RK3576通过订阅ROS话题（如 /camera/color/image_raw 和 /camera/depth/image_raw）即可获取数据，无需直接操作 /dev/video* 节点。

关键点：深度图像与RGB图像需要时间对齐和空间对齐（通常相机驱动已提供对齐后的深度图），以便后续将目标2D框映射到3D坐标。

第三章：NPU加速的MixFormerV2目标跟踪

3.1 为什么放弃OpenCV，改用NPU+MixFormerV2？

传统OpenCV跟踪算法（KCF、CSRT等）在光照变化、遮挡、快速运动下容易丢失目标，且完全依赖CPU，帧率受限。而MixFormerV2是一种基于Transformer的端到端跟踪器，精度高、鲁棒性好。配合RK3576内置的6TOPS NPU，可以：

• • 推理速度提升：单帧推理30ms左右，实际跟踪帧率可达15~20 FPS；
• • CPU占用大幅降低：NPU独立处理视觉任务，CPU可专注ROS2通信与运动控制；
• • 功耗更低，适合嵌入式移动机器人。

3.2 模型转换与部署流程

1. • 模型转换：下载MixFormerV2的PyTorch权重，使用RKNN-Toolkit2工具转换为 .rknn 格式，并做INT8量化。
2. • ROS2节点实现：
   • • 订阅RGB图像话题；
   • • 将图像缩放至模型输入尺寸（如224×224），进行预处理；
   • • 调用NPU推理，输出目标边界框；
   • • 结合深度图中对应区域的有效深度值，通过手眼转换得到目标在机器人坐标系下的3D坐标（X, Y, Z）；
   • • 发布 /target_3d_position 和 /tracking_box 话题。
3. • 性能匹配：相机帧率约15 FPS，MixFormerV2采用累计3帧一起推理的方式（batch size=3），单次耗时约70ms，整体匹配流畅。

3.3 手眼转换

本文采用 “眼在手上” 的配置：深度相机固定在机械臂末端，随机械臂一起运动。此时，相机到机械臂末端（camera_link → end_effector_link）的变换是固定的（可通过标定获得），而机械臂末端到基座（end_effector_link → arm_base_link）的变换则随关节角度实时变化。

在ROS中，我们需要：

• • 标定相机到机械臂末端的静态TF。
• • 机械臂驱动节点根据当前关节角度实时发布 end_effector_link → arm_base_link 的动态TF。
• • 通过 tf2 监听完整变换链，将物体坐标从相机系转换到机械臂基座系。

第四章：底盘移动跟随目标

当跟踪节点输出目标在机器人底盘坐标系下的3D位置后，底盘跟随节点 object_follower 执行以下逻辑：

• • 计算相对位置：得到目标相对于机器人中心的水平距离和角度偏差。
• • 优先调整方向：先原地旋转，使机器人正对目标（角度偏差 < 5°）。
• • 前进至抓取距离：保持正对，以线速度向前移动，直到距离目标约0.5米（安全抓取范围）。
• • 停止并通知抓取：到达抓取范围后，发布速度零指令，并触发抓取标志。

第五章：机械臂抓取物体

当底盘停止在抓取距离（约0.5米）后，抓取节点启动。本系统不依赖MoveIt 2，所有机械臂控制通过串口直接下发各关节角度，逆运动学由我们自行实现。

5.1 手眼转换（眼在手上）

相机固定在机械臂末端，因此手眼转换分为两部分：

• • 静态部分：相机到机械臂末端的变换（camera_link → end_effector_link），通过一次标定得到固定值。
• • 动态部分：机械臂末端到基座的变换（end_effector_link → arm_base_link），由机械臂当前关节角度实时决定。

在抓取流程中，目标物体在相机图像中被检测到后，首先得到物体在相机坐标系下的3D坐标，然后通过ROS的tf2监听完整的变换链：camera_link → end_effector_link → arm_base_link，自动转换到机械臂基座坐标系。这一过程无需手动干预，只要机械臂驱动节点正确发布关节状态和TF即可。

5.2 逆运动学解算

六轴机械臂通过串口接收角度指令（每个轴一个角度值）。为了抓取目标点，需要求解逆运动学：已知末端夹爪在机械臂基座下的目标位置（以及期望的姿态，例如垂直向下抓取），反算出6个关节的角度。

• • 实现方式：针对具体机械臂的几何参数（D-H参数），编写解析解或数值迭代解（如雅可比伪逆法）。解析解速度快，适合固定构型；数值法通用但需注意收敛。
• • 输出：6个关节角度（单位：度或弧度），通过串口逐条发送（可同时发送或按顺序移动）。

5.3 抓取流程

• • 获取目标坐标：从跟踪节点读取底盘停止瞬间的目标3D点（已转换到arm_base_link坐标系）。
• • 设定抓取姿态：根据物体形状和相机视角，设定夹爪的期望方向（例如让夹爪水平或垂直接近）。这一步需结合经验预设。
• • 逆运动学求解：输入末端目标位姿，计算出各关节角度。若求解失败（如目标超出工作空间），则调整底盘位置重新跟随。
• • 发送关节角度：通过串口依次发送6个关节的角度指令，等待机械臂运动到位（可简单延时或读取状态反馈）。
• • 夹取：发送夹爪闭合指令（串口另一命令），通过电流反馈或限位开关判断是否夹住物体。
• • 完成：抓取成功后，机械臂保持闭合，底盘可原地等待下一步指令。

第六章：总结与展望

本文在上篇“建图与导航”的基础上，为米尔RK3576机器人增加了“视觉跟随与抓取”能力，实现了完整的“识别-跟随-抓取”闭环。关键技术包括：

• • MixFormerV2 + NPU 实现高能效目标跟踪；
• • 手眼转换：将相机看到的物体坐标转换到机械臂可执行的坐标系下。本文采用“眼在手上”配置（相机固定在机械臂末端），需同时考虑固定偏移和关节运动。
• • 自研逆运动学 控制6轴机械臂精准抓取（不依赖MoveIt 2）。

米尔RK3576这一方案可广泛应用于服务机器人、巡检机器人、教育竞赛等场景。下一步可探索：

• • 多目标切换跟随；
• • 动态避障与跟随并行；
• • 抓取后自动放置（结合上篇的导航回位功能）。

插入视频

演示视频：米尔RK3576机器人方案机械臂抓取演示