米尔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这一方案可广泛应用于服务机器人、巡检机器人、教育竞赛等场景。下一步可探索：

Ø 多目标切换跟随;

Ø 动态避障与跟随并行;

Ø 抓取后自动放置(结合上篇的导航回位功能)。