【導(dǎo)讀】在上一篇技術(shù)實踐中，我們基于米爾RK3576核心板與ROS2 Humble框架，成功實現(xiàn)了機器人利用SLAM Toolbox與Nav2進(jìn)行自主建圖與導(dǎo)航，解決了“機器人如何走到哪里”的基礎(chǔ)移動問題。還要“看到并操作”——識別特定物體、主動跟隨、近距離抓取。本文將在此基礎(chǔ)上，集成深度攝像頭，實現(xiàn)機器人核心功能。我們將重點利用RK3576內(nèi)置的6TOPS NPU算力，部署高性能的MixFormerV2目標(biāo)跟蹤算法，以替代傳統(tǒng)魯棒性差的OpenCV方法，進(jìn)而實現(xiàn)對特定目標(biāo)的精準(zhǔn)識別、底盤動態(tài)跟隨以及最終的機械臂自主抓取，完成“識別-跟隨-抓取”這一復(fù)雜任務(wù)鏈。
名詞預(yù)先了解：
手眼轉(zhuǎn)換：將相機看到的物體坐標(biāo)轉(zhuǎn)換到機械臂可執(zhí)行的坐標(biāo)系下。
逆運動學(xué)：給定末端目標(biāo)位姿，反解出機械臂各關(guān)節(jié)應(yīng)轉(zhuǎn)動的角度。
圖：米爾基于RK3576核心板開發(fā)板
第一章：系統(tǒng)總體架構(gòu)與硬件連接
1.1 硬件組成
主控平臺：米爾基于RK3576核心板開發(fā)板（內(nèi)置6TOPS NPU）
深度攝像頭：RGB-D深度相機（輸出RGB、IR、深度三路數(shù)據(jù)）
機械臂：6軸輕量機械臂（串口控制）
移動底盤：STM32控制，麥克納姆輪
1.2 軟件模塊與數(shù)據(jù)流
整個系統(tǒng)分為五個核心ROS2節(jié)點：
模塊 功能 輸入 輸出
camera_driver 發(fā)布RGB和深度圖 RGB-D相機 RGB圖像、深度圖
mixformer_tracker NPU加速跟蹤 RGB圖 + 初始目標(biāo)框 目標(biāo)2D框 + 3D坐標(biāo)
object_follower 控制底盤移動 目標(biāo)3D坐標(biāo) /cmd_vel 速度指令
grasp_planner 抓取規(guī)劃 目標(biāo)3D坐標(biāo) + 深度 機械臂運動軌跡
arm_controller 執(zhí)行抓取 軌跡 夾爪狀態(tài)
整體數(shù)據(jù)流：
相機 → MixFormerV2跟蹤器 → 手眼轉(zhuǎn)換 → 底盤跟隨節(jié)點 → 靠近停止 → 機械臂逆運動學(xué)規(guī)劃 → 抓取執(zhí)行。
第二章：深度相機數(shù)據(jù)獲取
與普通USB攝像頭不同，深度相機在ROS2下通過標(biāo)準(zhǔn)驅(qū)動節(jié)點發(fā)布話題數(shù)據(jù)。本文使用的RGB-D相機輸出三路信息：
RGB圖像：用于目標(biāo)跟蹤的視覺輸入
IR圖像：輔助深度計算（夜間或弱光可用）
深度圖像：每個像素的毫米級距離值
相機輸出格式為：640×400 NV12，幀率13~15 FPS。主控RK3576通過訂閱ROS話題（如 /camera/color/image_raw 和 /camera/depth/image_raw）即可獲取數(shù)據(jù)，無需直接操作 /dev/video* 節(jié)點。
關(guān)鍵點：深度圖像與RGB圖像需要時間對齊和空間對齊（通常相機驅(qū)動已提供對齊后的深度圖），以便后續(xù)將目標(biāo)2D框映射到3D坐標(biāo)。
第三章：NPU加速的MixFormerV2目標(biāo)跟蹤
3.1 為什么放棄OpenCV，改用NPU+MixFormerV2？
傳統(tǒng)OpenCV跟蹤算法（KCF、CSRT等）在光照變化、遮擋、快速運動下容易丟失目標(biāo)，且完全依賴CPU，幀率受限。而MixFormerV2是一種基于Transformer的端到端跟蹤器，精度高、魯棒性好。配合RK3576內(nèi)置的6TOPS NPU，可以：
推理速度提升：單幀推理30ms左右，實際跟蹤幀率可達(dá)15~20 FPS；
CPU占用大幅降低：NPU獨立處理視覺任務(wù)，CPU可專注ROS2通信與運動控制；
功耗更低，適合嵌入式移動機器人。
3.2 模型轉(zhuǎn)換與部署流程
1.模型轉(zhuǎn)換：下載MixFormerV2的PyTorch權(quán)重，使用RKNN-Toolkit2工具轉(zhuǎn)換為 .rknn 格式，并做INT8量化。
2.ROS2節(jié)點實現(xiàn)：
訂閱RGB圖像話題；
將圖像縮放至模型輸入尺寸（如224×224），進(jìn)行預(yù)處理；
調(diào)用NPU推理，輸出目標(biāo)邊界框；
結(jié)合深度圖中對應(yīng)區(qū)域的有效深度值，通過手眼轉(zhuǎn)換得到目標(biāo)在機器人坐標(biāo)系下的3D坐標(biāo)（X, Y, Z）；
發(fā)布 /target_3d_position 和 /tracking_box 話題。
3.性能匹配：相機幀率約15 FPS，MixFormerV2采用累計3幀一起推理的方式（batch size=3），單次耗時約70ms，整體匹配流暢。
3.3 手眼轉(zhuǎn)換
本文采用 “眼在手上” 的配置：深度相機固定在機械臂末端，隨機械臂一起運動。此時，相機到機械臂末端（camera_link → end_effector_link）的變換是固定的（可通過標(biāo)定獲得），而機械臂末端到基座（end_effector_link → arm_base_link）的變換則隨關(guān)節(jié)角度實時變化。
在ROS中，我們需要：
標(biāo)定相機到機械臂末端的靜態(tài)TF。
機械臂驅(qū)動節(jié)點根據(jù)當(dāng)前關(guān)節(jié)角度實時發(fā)布 end_effector_link → arm_base_link 的動態(tài)TF。
通過 tf2 監(jiān)聽完整變換鏈，將物體坐標(biāo)從相機系轉(zhuǎn)換到機械臂基座系。
第四章：底盤移動跟隨目標(biāo)
當(dāng)跟蹤節(jié)點輸出目標(biāo)在機器人底盤坐標(biāo)系下的3D位置后，底盤跟隨節(jié)點 object_follower 執(zhí)行以下邏輯：
計算相對位置：得到目標(biāo)相對于機器人中心的水平距離和角度偏差。
優(yōu)先調(diào)整方向：先原地旋轉(zhuǎn)，使機器人正對目標(biāo)（角度偏差 < 5°）。
前進(jìn)至抓取距離：保持正對，以線速度向前移動，直到距離目標(biāo)約0.5米（安全抓取范圍）。
停止并通知抓?。旱竭_(dá)抓取范圍后，發(fā)布速度零指令，并觸發(fā)抓取標(biāo)志。
第五章：機械臂抓取物體
當(dāng)?shù)妆P停止在抓取距離（約0.5米）后，抓取節(jié)點啟動。本系統(tǒng)不依賴MoveIt 2，所有機械臂控制通過串口直接下發(fā)各關(guān)節(jié)角度，逆運動學(xué)由我們自行實現(xiàn)。
5.1 手眼轉(zhuǎn)換（眼在手上）
相機固定在機械臂末端，因此手眼轉(zhuǎn)換分為兩部分：
靜態(tài)部分：相機到機械臂末端的變換（camera_link → end_effector_link），通過一次標(biāo)定得到固定值。
動態(tài)部分：機械臂末端到基座的變換（end_effector_link → arm_base_link），由機械臂當(dāng)前關(guān)節(jié)角度實時決定。
在抓取流程中，目標(biāo)物體在相機圖像中被檢測到后，首先得到物體在相機坐標(biāo)系下的3D坐標(biāo)，然后通過ROS的tf2監(jiān)聽完整的變換鏈：camera_link → end_effector_link → arm_base_link，自動轉(zhuǎn)換到機械臂基座坐標(biāo)系。這一過程無需手動干預(yù)，只要機械臂驅(qū)動節(jié)點正確發(fā)布關(guān)節(jié)狀態(tài)和TF即可。
5.2 逆運動學(xué)解算
六軸機械臂通過串口接收角度指令（每個軸一個角度值）。為了抓取目標(biāo)點，需要求解逆運動學(xué)：已知末端夾爪在機械臂基座下的目標(biāo)位置（以及期望的姿態(tài)，例如垂直向下抓?。?，反算出6個關(guān)節(jié)的角度。
實現(xiàn)方式：針對具體機械臂的幾何參數(shù)（D-H參數(shù)），編寫解析解或數(shù)值迭代解（如雅可比偽逆法）。解析解速度快，適合固定構(gòu)型；數(shù)值法通用但需注意收斂。
輸出：6個關(guān)節(jié)角度（單位：度或弧度），通過串口逐條發(fā)送（可同時發(fā)送或按順序移動）。
5.3 抓取流程
獲取目標(biāo)坐標(biāo)：從跟蹤節(jié)點讀取底盤停止瞬間的目標(biāo)3D點（已轉(zhuǎn)換到arm_base_link坐標(biāo)系）。
設(shè)定抓取姿態(tài)：根據(jù)物體形狀和相機視角，設(shè)定夾爪的期望方向（例如讓夾爪水平或垂直接近）。這一步需結(jié)合經(jīng)驗預(yù)設(shè)。
逆運動學(xué)求解：輸入末端目標(biāo)位姿，計算出各關(guān)節(jié)角度。若求解失?。ㄈ缒繕?biāo)超出工作空間），則調(diào)整底盤位置重新跟隨。
發(fā)送關(guān)節(jié)角度：通過串口依次發(fā)送6個關(guān)節(jié)的角度指令，等待機械臂運動到位（可簡單延時或讀取狀態(tài)反饋）。
夾?。喊l(fā)送夾爪閉合指令（串口另一命令），通過電流反饋或限位開關(guān)判斷是否夾住物體。
完成：抓取成功后，機械臂保持閉合，底盤可原地等待下一步指令。
第六章：總結(jié)與展望
本文在上篇“建圖與導(dǎo)航”的基礎(chǔ)上，為米爾RK3576機器人增加了“視覺跟隨與抓取”能力，實現(xiàn)了完整的“識別-跟隨-抓取”閉環(huán)。關(guān)鍵技術(shù)包括：
MixFormerV2 + NPU 實現(xiàn)高能效目標(biāo)跟蹤；
手眼轉(zhuǎn)換：將相機看到的物體坐標(biāo)轉(zhuǎn)換到機械臂可執(zhí)行的坐標(biāo)系下。本文采用“眼在手上”配置（相機固定在機械臂末端），需同時考慮固定偏移和關(guān)節(jié)運動。
自研逆運動學(xué) 控制6軸機械臂精準(zhǔn)抓?。ú灰蕾嘙oveIt 2）。
米爾RK3576這一方案可廣泛應(yīng)用于服務(wù)機器人、巡檢機器人、教育競賽等場景。下一步可探索：
多目標(biāo)切換跟隨；
動態(tài)避障與跟隨并行；
抓取后自動放置（結(jié)合上篇的導(dǎo)航回位功能）。
