前言

回顧上篇：基于RK3576 + ROS2 Humble + SLAM Toolbox + Nav2，我們實(shí)現(xiàn)了機(jī)器人的建圖與自主導(dǎo)航。機(jī)器人已經(jīng)能夠“走到哪里”。但真正的智能機(jī)器人不僅要“走到哪”，還要“看到并操作”——識(shí)別特定物體、主動(dòng)跟隨、近距離抓取。本文將在此基礎(chǔ)上，集成深度攝像頭，實(shí)現(xiàn)機(jī)器人核心功能：

· 使用米爾RK3576 NPU加速M(fèi)ixFormerV2進(jìn)行目標(biāo)跟蹤，替代傳統(tǒng)OpenCV算法;

· 移動(dòng)底盤跟隨目標(biāo)物體，保持安全距離;

· 機(jī)器人機(jī)械臂抓取物體，完成“識(shí)別-跟隨-抓取”閉環(huán)。

名詞預(yù)先了解：

· 手眼轉(zhuǎn)換：將相機(jī)看到的物體坐標(biāo)轉(zhuǎn)換到機(jī)械臂可執(zhí)行的坐標(biāo)系下。

· 逆運(yùn)動(dòng)學(xué)：給定末端目標(biāo)位姿，反解出機(jī)械臂各關(guān)節(jié)應(yīng)轉(zhuǎn)動(dòng)的角度。

圖：米爾基于RK3576核心板開發(fā)板

第一章：系統(tǒng)總體架構(gòu)與硬件連接

1.1 硬件組成

· 主控平臺(tái)：米爾基于RK3576核心板開發(fā)板(內(nèi)置6TOPS NPU)

· 深度攝像頭：RGB-D深度相機(jī)(輸出RGB、IR、深度三路數(shù)據(jù))

· 機(jī)械臂：6軸輕量機(jī)械臂(串口控制)

· 移動(dòng)底盤：STM32控制，麥克納姆輪

1.2 軟件模塊與數(shù)據(jù)流

整個(gè)系統(tǒng)分為五個(gè)核心ROS2節(jié)點(diǎn)：

整體數(shù)據(jù)流：

相機(jī) → MixFormerV2跟蹤器 → 手眼轉(zhuǎn)換 → 底盤跟隨節(jié)點(diǎn) → 靠近停止 → 機(jī)械臂逆運(yùn)動(dòng)學(xué)規(guī)劃 → 抓取執(zhí)行。

第二章：深度相機(jī)數(shù)據(jù)獲取

與普通USB攝像頭不同，深度相機(jī)在ROS2下通過(guò)標(biāo)準(zhǔn)驅(qū)動(dòng)節(jié)點(diǎn)發(fā)布話題數(shù)據(jù)。本文使用的RGB-D相機(jī)輸出三路信息：

· RGB圖像：用于目標(biāo)跟蹤的視覺(jué)輸入

· IR圖像：輔助深度計(jì)算(夜間或弱光可用)

· 深度圖像：每個(gè)像素的毫米級(jí)距離值

相機(jī)輸出格式為：640×400 NV12，幀率13~15 FPS。主控RK3576通過(guò)訂閱ROS話題(如 /camera/color/image_raw 和 /camera/depth/image_raw)即可獲取數(shù)據(jù)，無(wú)需直接操作 /dev/video* 節(jié)點(diǎn)。

關(guān)鍵點(diǎn)：深度圖像與RGB圖像需要時(shí)間對(duì)齊和空間對(duì)齊(通常相機(jī)驅(qū)動(dòng)已提供對(duì)齊后的深度圖)，以便后續(xù)將目標(biāo)2D框映射到3D坐標(biāo)。

第三章：NPU加速的MixFormerV2目標(biāo)跟蹤

3.1 為什么放棄OpenCV，改用NPU+MixFormerV2?

傳統(tǒng)OpenCV跟蹤算法(KCF、CSRT等)在光照變化、遮擋、快速運(yùn)動(dòng)下容易丟失目標(biāo)，且完全依賴CPU，幀率受限。而MixFormerV2是一種基于Transformer的端到端跟蹤器，精度高、魯棒性好。配合RK3576內(nèi)置的6TOPS NPU，可以：

? 推理速度提升：?jiǎn)螏评?0ms左右，實(shí)際跟蹤幀率可達(dá)15~20 FPS;

? CPU占用大幅降低：NPU獨(dú)立處理視覺(jué)任務(wù)，CPU可專注ROS2通信與運(yùn)動(dòng)控制;

? 功耗更低，適合嵌入式移動(dòng)機(jī)器人。

3.2 模型轉(zhuǎn)換與部署流程

1. 模型轉(zhuǎn)換：下載MixFormerV2的PyTorch權(quán)重，使用RKNN-Toolkit2工具轉(zhuǎn)換為 .rknn 格式，并做INT8量化。

2. ROS2節(jié)點(diǎn)實(shí)現(xiàn)：

? 訂閱RGB圖像話題;

? 將圖像縮放至模型輸入尺寸(如224×224)，進(jìn)行預(yù)處理;

? 調(diào)用NPU推理，輸出目標(biāo)邊界框;

? 結(jié)合深度圖中對(duì)應(yīng)區(qū)域的有效深度值，通過(guò)手眼轉(zhuǎn)換得到目標(biāo)在機(jī)器人坐標(biāo)系下的3D坐標(biāo)(X, Y, Z);

? 發(fā)布 /target_3d_position 和 /tracking_box 話題。

3. 性能匹配：相機(jī)幀率約15 FPS，MixFormerV2采用累計(jì)3幀一起推理的方式(batch size=3)，單次耗時(shí)約70ms，整體匹配流暢。

3.3 手眼轉(zhuǎn)換

本文采用 “眼在手上” 的配置：深度相機(jī)固定在機(jī)械臂末端，隨機(jī)械臂一起運(yùn)動(dòng)。此時(shí)，相機(jī)到機(jī)械臂末端(camera_link → end_effector_link)的變換是固定的(可通過(guò)標(biāo)定獲得)，而機(jī)械臂末端到基座(end_effector_link → arm_base_link)的變換則隨關(guān)節(jié)角度實(shí)時(shí)變化。

在ROS中，我們需要：

? 標(biāo)定相機(jī)到機(jī)械臂末端的靜態(tài)TF。

? 機(jī)械臂驅(qū)動(dòng)節(jié)點(diǎn)根據(jù)當(dāng)前關(guān)節(jié)角度實(shí)時(shí)發(fā)布 end_effector_link → arm_base_link 的動(dòng)態(tài)TF。

? 通過(guò) tf2 監(jiān)聽完整變換鏈，將物體坐標(biāo)從相機(jī)系轉(zhuǎn)換到機(jī)械臂基座系。

第四章：底盤移動(dòng)跟隨目標(biāo)

當(dāng)跟蹤節(jié)點(diǎn)輸出目標(biāo)在機(jī)器人底盤坐標(biāo)系下的3D位置后，底盤跟隨節(jié)點(diǎn) object_follower 執(zhí)行以下邏輯：

? 計(jì)算相對(duì)位置：得到目標(biāo)相對(duì)于機(jī)器人中心的水平距離和角度偏差。

? 優(yōu)先調(diào)整方向：先原地旋轉(zhuǎn)，使機(jī)器人正對(duì)目標(biāo)(角度偏差 < 5°)。

? 前進(jìn)至抓取距離：保持正對(duì)，以線速度向前移動(dòng)，直到距離目標(biāo)約0.5米(安全抓取范圍)。

? 停止并通知抓取：到達(dá)抓取范圍后，發(fā)布速度零指令，并觸發(fā)抓取標(biāo)志。

第五章：機(jī)械臂抓取物體

當(dāng)?shù)妆P停止在抓取距離(約0.5米)后，抓取節(jié)點(diǎn)啟動(dòng)。本系統(tǒng)不依賴MoveIt 2，所有機(jī)械臂控制通過(guò)串口直接下發(fā)各關(guān)節(jié)角度，逆運(yùn)動(dòng)學(xué)由我們自行實(shí)現(xiàn)。

5.1 手眼轉(zhuǎn)換(眼在手上)

相機(jī)固定在機(jī)械臂末端，因此手眼轉(zhuǎn)換分為兩部分：

? 靜態(tài)部分：相機(jī)到機(jī)械臂末端的變換(camera_link → end_effector_link)，通過(guò)一次標(biāo)定得到固定值。

? 動(dòng)態(tài)部分：機(jī)械臂末端到基座的變換(end_effector_link → arm_base_link)，由機(jī)械臂當(dāng)前關(guān)節(jié)角度實(shí)時(shí)決定。

在抓取流程中，目標(biāo)物體在相機(jī)圖像中被檢測(cè)到后，首先得到物體在相機(jī)坐標(biāo)系下的3D坐標(biāo)，然后通過(guò)ROS的tf2監(jiān)聽完整的變換鏈：camera_link → end_effector_link → arm_base_link，自動(dòng)轉(zhuǎn)換到機(jī)械臂基座坐標(biāo)系。這一過(guò)程無(wú)需手動(dòng)干預(yù)，只要機(jī)械臂驅(qū)動(dòng)節(jié)點(diǎn)正確發(fā)布關(guān)節(jié)狀態(tài)和TF即可。

5.2 逆運(yùn)動(dòng)學(xué)解算

六軸機(jī)械臂通過(guò)串口接收角度指令(每個(gè)軸一個(gè)角度值)。為了抓取目標(biāo)點(diǎn)，需要求解逆運(yùn)動(dòng)學(xué)：已知末端夾爪在機(jī)械臂基座下的目標(biāo)位置(以及期望的姿態(tài)，例如垂直向下抓取)，反算出6個(gè)關(guān)節(jié)的角度。

? 實(shí)現(xiàn)方式：針對(duì)具體機(jī)械臂的幾何參數(shù)(D-H參數(shù))，編寫解析解或數(shù)值迭代解(如雅可比偽逆法)。解析解速度快，適合固定構(gòu)型;數(shù)值法通用但需注意收斂。

? 輸出：6個(gè)關(guān)節(jié)角度(單位：度或弧度)，通過(guò)串口逐條發(fā)送(可同時(shí)發(fā)送或按順序移動(dòng))。

5.3 抓取流程

? 獲取目標(biāo)坐標(biāo)：從跟蹤節(jié)點(diǎn)讀取底盤停止瞬間的目標(biāo)3D點(diǎn)(已轉(zhuǎn)換到arm_base_link坐標(biāo)系)。

? 設(shè)定抓取姿態(tài)：根據(jù)物體形狀和相機(jī)視角，設(shè)定夾爪的期望方向(例如讓夾爪水平或垂直接近)。這一步需結(jié)合經(jīng)驗(yàn)預(yù)設(shè)。

? 逆運(yùn)動(dòng)學(xué)求解：輸入末端目標(biāo)位姿，計(jì)算出各關(guān)節(jié)角度。若求解失敗(如目標(biāo)超出工作空間)，則調(diào)整底盤位置重新跟隨。

? 發(fā)送關(guān)節(jié)角度：通過(guò)串口依次發(fā)送6個(gè)關(guān)節(jié)的角度指令，等待機(jī)械臂運(yùn)動(dòng)到位(可簡(jiǎn)單延時(shí)或讀取狀態(tài)反饋)。

? 夾?。喊l(fā)送夾爪閉合指令(串口另一命令)，通過(guò)電流反饋或限位開關(guān)判斷是否夾住物體。

? 完成：抓取成功后，機(jī)械臂保持閉合，底盤可原地等待下一步指令。

第六章：總結(jié)與展望

本文在上篇“建圖與導(dǎo)航”的基礎(chǔ)上，為米爾RK3576機(jī)器人增加了“視覺(jué)跟隨與抓取”能力，實(shí)現(xiàn)了完整的“識(shí)別-跟隨-抓取”閉環(huán)。關(guān)鍵技術(shù)包括：

? MixFormerV2 + NPU 實(shí)現(xiàn)高能效目標(biāo)跟蹤;

? 手眼轉(zhuǎn)換：將相機(jī)看到的物體坐標(biāo)轉(zhuǎn)換到機(jī)械臂可執(zhí)行的坐標(biāo)系下。本文采用“眼在手上”配置(相機(jī)固定在機(jī)械臂末端)，需同時(shí)考慮固定偏移和關(guān)節(jié)運(yùn)動(dòng)。

? 自研逆運(yùn)動(dòng)學(xué) 控制6軸機(jī)械臂精準(zhǔn)抓取(不依賴MoveIt 2)。

米爾RK3576這一方案可廣泛應(yīng)用于服務(wù)機(jī)器人、巡檢機(jī)器人、教育競(jìng)賽等場(chǎng)景。下一步可探索：

? 多目標(biāo)切換跟隨;

? 動(dòng)態(tài)避障與跟隨并行;

? 抓取后自動(dòng)放置(結(jié)合上篇的導(dǎo)航回位功能)。