“眼于手上”的嵌入式实践：基在ROS2与RK3576的机械臂追随抓取方案

发布时间：2026-04-10 来历：转载 责任编纂：lily

【导读】于上一篇技能实践中，咱们基在米尔RK3576焦点板与ROS2 Humble框架，乐成实现了呆板人使用SLAM Toolbox与Nav2举行自立建图与导航，解决了“呆板人怎样走到哪里”的基础挪动问题。还有要“看到并操作”——辨认特定物体、自动追随、近间隔抓取。本文将于此基础上，集成深度摄像头，实现呆板人焦点功效。咱们将重点使用RK3576内置的6TOPS NPU算力，部署高机能的MixFormerV2方针跟踪算法，以替换传统鲁棒性差的OpenCV要领，进而实现对于特定方针的精准辨认、底盘动态追随以和终极的机械臂自立抓取，完成“辨认-追随-抓取”这一繁杂使命链。

名词预先相识：

手眼转换：将相机看到的物体坐标转换到机械臂可履行的坐标系下。

逆运动学：给定结尾方针位姿，反解出机械臂各枢纽关头应动弹的角度。

图：米尔基在RK3576焦点板开发板

第一章：体系整体架构与硬件毗连

1.1 硬件构成

主控平台：米尔基在RK3576焦点板开发板（内置6TOPS NPU）

深度摄像头：RGB-D深度相机（输出RGB、IR、深度三路数据）

机械臂：6轴轻量机械臂（串口节制）

挪动底盘：STM32节制，麦克纳姆轮

1.2 软件模块与数据流

整个体系分为五个焦点ROS2节点：

模块 功效 输入 输出

camera_driver 发布RGB及深度图 RGB-D相机 RGB图象、深度图

mixformer_tracker NPU加快跟踪 RGB图 + 初始方针框 方针2D框 + 3D坐标

object_follower 节制底盘挪动 方针3D坐标 /cmd_vel 速率指令

grasp_planner 抓取计划 方针3D坐标 + 深度 机械臂运动轨迹

arm_controller 履行抓取 轨迹 夹爪状况

总体数据流：

相机 → 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这一方案可广泛运用在办事呆板人、巡检呆板人、教诲竞赛等场景。下一步可摸索：

多方针切换追随；

动态避障与追随并行；

抓取后主动放置（联合上篇的导航回位功效）。