米爾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節點：
模塊 | 功能 | 輸入 | 輸出 |
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這一方案可廣泛應用于服務機器人、巡檢機器人、教育競賽等場景。下一步可探索：
? 多目標切換跟隨；
? 動態避障與跟隨并行；
? 抓取后自動放置（結合上篇的導航回位功能）。
評論