米尔RK3576_ROS2_MixFormerV2目标跟随与机械臂抓取实战

对于一台真正意义上的智能机器人而言，“会走”只是起点。更关键的能力在于“会看”和“会做”——即感知周围环境中的特定物体、主动跟随目标移动，并在近距离内完成抓取操作。这要求机器人不仅要有空间移动能力，还要具备视觉感知、目标识别、动态跟踪与运动规划协同的综合智能。

本文基于RK3576平台，结合ROS2 Humble，集成深度摄像头，系统性地为机器人添加三项核心功能，解决这三个问题：

1. 目标在哪里
2. 机器人怎样移动到合适位置
3. 机械臂怎样到达并夹住目标

可以在算法定义上说为：

目标初始化或选定—持续跟踪—三维定位—底盘跟随—机械臂抓取。

因为 MixFormerV2 本身主要是一个单目标跟踪器，通常需要先提供一个初始目标框；它并不天然等同于“自动识别所有物体的检测器”（要求输入“RGB 图像 + 初始目标框”）。

为了完成这个任务，我们将使用米尔RK3576核心板，使用传感器驱动配置、视觉识别算法部署，到与Nav2导航栈的行为融合，完整呈现整个实现过程。最终，机器人将从一个“只会走路”的移动平台，升级为具备“观察-决策-操作”闭环的智能体。

完整机械本体

实物如图

实物如图

这是一种典型的异构机器人架构：RK3576 擅长高计算量任务；STM32 擅长确定性较强的低层实时控制；内部NPU 专门处理神经网络；机械臂控制器处理舵机或关节执行；不建议直接让 ROS2 节点逐周期控制电机 PWM，更合理的是：

发布期望速度执行速度闭环电机驱动

ROS2 节点设计

目前这种多数据流交互的任务，都会选择 ROS，2026 年也早就都普及使用：

ROS2

ROS2

熟悉 ROS 的人都知道，我们很多的时间都花在节点设计上，所以本次任务规划了以下5个节点：

image-20260715160552773

image-20260715160552773

重新整理版将系统拆成六个主要节点。

camera_driver

负责：获取 RGB 图像；获取深度图像；发布相机内参；发布相机坐标系；必要时发布对齐后的深度图。

常见输出包括：

/camera/color/image_raw
/camera/depth/image_raw
/camera/color/camera_info

只是数据源，不应该承担目标跟踪、底盘控制等高层逻辑。

mixformer_tracker

输入：RGB 图像；初始目标框；可选的模板图。输出：目标二维边界框；跟踪置信度；对应图像时间戳。如：

/tracking_box

这个节点主要给出：

目标当前在图像中的哪个矩形区域？

原则上只负责二维视觉跟踪，不应该同时承担大量深度计算和机械臂规划，否则会导致节点耦合过重。

target_localizer

这是我单独拆出的三维定位节点。输入：目标二维框；对齐深度图；相机内参；图像时间戳。

输出：

/target_3d_position

完成：

二维框深度图相机坐标系三维点

拆出这个节点的好处是，可以独立判断问题究竟来自：跟踪框不准；深度图不准；RGB/深度没有对齐；相机内参错误；坐标变换错误。

object_follower

输入：目标在 base_link 坐标系下的位置；跟踪置信度；深度有效标志。

输出：

/cmd_vel
/grasp_trigger

根据目标的位置计算底盘运动速度。

grasp_planner

将目标转换到机械臂基座坐标系；生成预抓取位姿；设定夹爪姿态；求解逆运动学；检查工作空间和关节限位；输出的是关节目标值或完整抓取轨迹。

arm_controller

将关节目标转换成串口协议；下发六轴角度；控制夹爪；获取关节和夹爪反馈；判断动作是否完成。

系统中的数据流

系统中的数据流

RGB-D 相机

相机输出为：RGB；IR；深度；640×400；NV12；约 13～15 FPS。RGB 图像用于 MixFormerV2 观察目标外观，例如：颜色；纹理；边缘；形状；局部特征；其中目标跟踪器通常主要使用 RGB 数据。但IR 通常不是直接给 MixFormerV2 使用，而更多服务于深度相机自身：主动红外结构光；双目红外匹配；弱光深度获取。

深度图中的每个像素表示该方向上物体到相机的距离。

如：

depth(u,v) = 850

若单位为毫米，则表示：

但必须读取相机驱动对深度值的单位定义;有些相机使用毫米整数，有些使用米制浮点数，还有些需要乘以 depth_scale。

时间对齐

目标在运动，相机也可能随机械臂运动，这里可以假设：RGB 图像采集时刻是 ；深度图采集时刻是 ；那。

那么目标在 RGB 图中框出的区域，到了深度图采集时刻可能已经移动。

结果就是：

二维框对应的是目标，但读取到的深度可能是目标后面的墙。

因此 RGB 和深度必须尽量来自同一时刻。

ROS2 中一般使用：message_filters::TimeSynchronizer，ApproximateTime，相机硬件同步，驱动内部同步来实现。

空间对齐

因为RGB 相机和深度相机通常不是同一个光学中心。

可以看到是有间距的

可以看到是有间距的

它们存在：水平位置差；垂直位置差；旋转误差；焦距差；分辨率差。

因此 RGB 图中的像素：

不能天然直接索引深度图中相同的：

必须通过相机标定参数完成深度到彩色图的投影，而在工程上应优先使用相机驱动输出的：

aligned_depth_to_color

也就是已经对齐到 RGB 坐标系的深度图。

不能只读目标框中心一个深度点

假设目标框中心像素恰好落在：物体孔洞；透明区域；反光区域；深度边缘；无效深度点；背景缝隙，那么单像素深度会剧烈跳动。

因为深度图携带的数据还是有限的

因为深度图携带的数据还是有限的

更合理的方法是，在目标框中心取一个较小区域，例如：

然后：删除 0、NaN 和超出范围的值；删除深度边缘异常值；计算中位数；检查有效深度比例；对连续帧做限幅或轻度平滑。

例如目标框为：

可以只取中心 30%～50% 区域，减少背景像素混入；中位数通常比平均值更稳健，因为少量极端深度不会明显影响中位数。

二维像素转换到三维坐标

目标跟踪输出的是像素位置，例如：

深度图提供：

相机内参提供：

：水平焦距； ：垂直焦距； ：主点横坐标； ：主点纵坐标。

针孔相机模型为：

于是得到：

例如：

则：

因此目标相对于相机大约位于：横向 12.8 cm；纵向 −3.2 cm；前方 80 cm。需要特别注意 ROS 光学坐标系通常约定：：向前；：向右；：向下。而机器人 base_link 常见约定是：：向前；：向左；：向上。

两个坐标系轴定义不同，不能直接把相机的 当成底盘坐标使用。

MixFormerV2 与 NPU 部署

传统 KCF、CSRT 等算法通常具有以下特点：部署简单；CPU 可以直接运行；对普通场景有效；遮挡和快速运动时容易漂移；外观变化时容易失效。

MixFormerV2 使用 Transformer 建模模板目标与当前搜索区域之间的关系，通常对以下问题更稳健：目标尺度变化；局部遮挡；快速位移；背景干扰；外观变化。

性能可以做到单帧推理约 30 ms；实际跟踪约 15～20 FPS；使用 RK3576 的片上 NPU。

模型转换

典型过程为：

MixFormer 类跟踪器并非普通的单输入分类模型，而是包含模板图与搜索图双路输入、不同输入尺寸及多个输出张量的复杂架构，因此绝不能简单地将整幅图统一缩放至 224×224 就视为完成输入；在部署时，必须严格保证训练与推理阶段的预处理细节完全一致，涵盖色彩空间（RGB/BGR）、像素范围（0~1或0~255）、均值与标准差归一化、数据排布（NCHW/NHWC）、缩放策略（拉伸或等比例）、是否加入 Padding 填充以及量化零点与缩放系数等，任何一项微小的偏差都会导致模型虽能正常运行但跟踪框完全错误；此外，在输出端还需针对模型可能生成的置信度图、中心位置、宽高、偏移量或多个候选框等复杂张量进行精准解码，才能最终还原出正确的跟踪结果。

（另外这些是算法训练内容，此处就不班门弄斧了）

另外必须将模型坐标重新映射到原始图像：

若存在 letterbox padding，还要先减去 padding。

手眼转换

相机固定在机械臂末端，随机械臂运动，这称为：

可以接近目标后可以近距离观察；机械臂改变视角；适合精细抓取。但是由于相机不断运动；TF 必须实时更新；时间同步要求高；机械臂运动时画面容易模糊；相机运动会造成视觉背景整体变化。

需要两个变换；首先是静态变换，其中机械臂末端到相机：

相机安装完成后，这个关系固定，包含：相机相对末端的平移；相机相对末端的旋转，这个关系可以通过手眼标定获得。

对于动态变换来说，机械臂基座到机械臂末端：

它随六个关节角 改变，这由机械臂正运动学计算。

完整坐标变换

目标在相机坐标系中为：

转换到机械臂基座：

表示目标先从相机坐标系转换到末端坐标系；再从末端坐标系转换到机械臂基座坐标系，这里的矩阵乘法顺序不能反。

底盘跟随控制

当目标转换到 base_link 后，设其水平位置为：

：目标在机器人前方的距离；

：目标在机器人左侧或右侧的偏移。

计算距离：

计算方向误差：

控制分为两个阶段，阶段一先对准，当：

令：

即只旋转，不前进，二阶段再靠近，当：

并且：

令：

同时继续小幅调整方向，最后停车

当：

令：

并触发抓取状态。

机械臂抓取

首先目标位置不等于目标位姿，使用的 RGB-D 相机最初得到的通常只是目标点：

但六轴机械臂需要的是完整末端位姿：

是位置；

是末端姿态。

也就是说，除了“夹爪中心到哪里”，还要明确：夹爪朝向；接近方向；开口方向；是否垂直向下；是否水平侧抓。

设置预抓取点

不能让机械臂直接高速运动到物体表面，一般设置：

：最终抓取点；

：接近方向；

：安全距离。

例如垂直向下抓取，预抓取点取目标上方 10 cm，而抓取点在目标上方 1～2 cm 或夹爪中心补偿位置

逆运动学

正运动学是：

给定六个关节角，求末端在哪里。

逆运动学是：

给定目标末端位姿，求六个关节角。

当前应用不使用 MoveIt 2，而是自行求解 IK，然后通过串口发送各关节角。

后记

RK3576 在这里承担的是系统集成中心：ROS2 节点管理；图像输入；NPU 推理；深度融合；TF 变换；底盘控制；机械臂规划；状态机，NPU 只是其中一个计算单元。RGB-D 相机解决二维视觉与三维距离；MixFormerV2 解决目标持续锁定；ROS2 TF 解决多坐标系变换；STM32 底盘负责接近目标；六轴机械臂通过自研 IK 和串口完成抓取。

米尔RK3576方案凭借其出色的性能，可广泛应用于服务机器人、智能巡检机器人及教育竞赛等多元化场景。

在未来的技术探索中，该方案可进一步向更复杂的自主交互方向演进，例如实现多目标的智能切换跟随、动态避障与持续跟随的并行处理，以及结合前期的导航回位功能，完成物体抓取后的自动放置任务，从而全面提升机器人在复杂环境下的综合智能化作业能力。