2025 ICRA Sim2Rea 獲獎隊伍給我們帶來MuJoCo+ROS2閉環控制與精準視覺抓取全流程解析

項目主要包括Communication、Envs、Navigation、PoseTransform、TaskUnderstanding、VisionUnderstanding這六個部分，功能如下：

核心亮點

我們重構了機器人的底層運動控制，解決了仿真到現實的遷移難題與開環控制的巨大誤差。 初始控制算法在從MuJoCo仿真遷移到ROS2真實環境時完全失效。為此，我們放棄了簡單的開環控制，通過引入關節狀態和里程計作為閉環反饋信號，并結合簡化的PID調節邏輯，重寫了驅動底層。特別地，我們設計了利用sigmoid函數的平滑控制算法，實現了機器人運動的平滑啟停與變速。最終，這套閉環控制系統使機器人的底盤移動精度達到0.01米，旋轉精度達到0.1度，為上層任務的穩定執行提供了高精度、高流暢度的運動能力基礎。

我們建立了一套從視覺感知到物理執行的完整技術管線，解決了復雜場景下的目標定位與精確操作難題。 在感知端，我們結合了快速的YOLO目標檢測與深度信息，并設計了“先觀察記錄，后移動執行”的策略，有效解決了因機器人視角變化（如抓取前的俯視）導致的識別失敗問題。在執行端，為將視覺系統獲得的目標世界坐標轉化為精確的機械臂動作，我們利用一個實時同步的MuJoCo仿真環境來處理復雜的逆運動學解算和多坐標系（相機、基座、世界）間的轉換。該仿真環境作為一個高效的計算模型，能快速求解出機器人所需的關節配置，從而精確地打通了從“看到”到“做到”的全過程。

項目亮點

項目結構清晰，各個功能模塊解耦，API命名和編寫符合規范，文檔完整清晰，易于開發和維護。

大部分算法同時提供了ROS2和MuJoCo環境的實現，支持ROS2分布式和MuJoCo本地調試。

比賽的入口文件是run.py，通過一個文件自動檢測比賽進程，切換不同的任務腳本（round_1~3）執行。在任務腳本中也按照步驟給出了完整的任務流程和實現。

在Envs的GraspAppleTask類中提供了一個蘋果抓取的環境，并在DISCOVERSE的mjcf中提供了對應的xml文件，主要用于測試MuJoCo環境中的坐標轉換算法。

在Communication的MMK2_Controller類中提供了豐富的MMK2機器人控制方法，并提供多次優化之后的_move_base底層驅動方法，實現機器人運動的精準、流暢控制，移動精度達到0.01m，轉動精度達到0.1°。

在Navigation的MoveToPoint類中針對比賽環境提供了精準導航方法，只需要目標位置的坐標和朝向，配合MMK2_Controller即可實現任意位置的精準到達。

在PoseTransform的PoseTransform類中基于MuJoCo構建實時同步數字孿生環境實現機器人的正逆運動學解算，并針對相機坐標系、基座坐標系、底盤坐標系和世界坐標系提供了彼此坐標轉換的方法。

在TaskUnderstanding的TaskParser類中提供了比賽任務指令的解析器，通過ROS2發布的topic信息獲取task_info和game_info，根據比賽輪數自動解析任務指令為格式化輸出。

在VisionUnderstanding的RosVisionModule類中提供了物體識別和位姿計算方法，視覺模型包括YOLO v11/v12的目標檢測模型和SAM2的實例分割模型，提供了基于RGB-D信息的快速位姿計算方法和基于點云匹配的精準位姿計算方法。針對比賽任務，提供了道具Box的精準位姿計算方法和Box放置坐標的計算方法。

開發時的踩坑與重構

MMK2機器人的底盤控制算法最開始是基于DISCOVERSE中的mmk2_base.py編寫的，只是簡單地將對應關節的數值綁定到對應的關節上，因為錯誤地使用了位置變量qpos作為控制對象，所以在低速情況下，在MuJoCo環境中的效果還可以，但是遷移到ROS2中發現對應的topic綁定的其實是驅動變量ctrl，無法兼容。經過第一輪重構，通過實驗得到穩定運行時各個關節的參數范圍，但是發現開環控制具有很大的誤差，因此引入關節位置參數joint_states和底盤里程計參數odom作為閉環反饋信號。經過多輪迭代，引入了簡化版本的PID調節，在最終的MMK2_Controller版本中，使用_move_base作為機器人底盤閉環運動的底層驅動，并利用sigmoid函數在_smooth_control_step實現了更加平滑流暢的控制，最終實現移動精度0.01m，轉動精度0.1°。同時使用相同的想法完善了更加平滑連貫的set_arm_position方法實現機械臂的控制。

對于環境中的物體位姿計算，一開始采用了傳統的目標檢測+深度點云重建+點云位姿匹配的流程，但是點云匹配的精度和速度都很難滿足要求，因此采用了和官方baseline中類似的方法，只使用目標檢測+深度信息實現了較為精準的位姿計算，實現了精度和速度的平衡。對于需要更加精準定位的物體，比如cabinet上盛放三種prop的box，需要在移動到物體前方后進行二次定位和微調，具體通過get_Toc_box_from_yolo這一類方法，考慮到了檢測到不同數量的box的情況，增加了系統容錯。

項目的ACT訓練腳本的配置文件比較分散，為了實現命令行參數的簡潔，在命名上的耦合比較嚴重，完全搞懂的確花了些功夫。為了更加多樣化的數據，在仿真數據生成的腳本中添加了額外的變化，包括轉動角度、初始化位置等；為了實現更加高效的訓練，重構了DDP并行訓練腳本；同時修改了evaluate腳本中的一些bug，使其能夠兼容不同長度的action。不過最終不知道是因為over fitting還是數據多樣性不足，ACT算法幾乎自始至終沒有work，當然這也是模仿學習中不得不品嘗的一環:)

針對比賽任務，我們采集了大量的數據，根據比賽環境對任務流程做了深入的優化。采集了1000條左右的目標檢測數據進行YOLO（YOLO v11/v12）訓練，最終基本能實現對場景中全部物體（包括drawer的上下兩個把手）的精準識別定位；采集了近萬條抓取仿真數據進行box和不同prop的ACT訓練，完善了并行訓練腳本實現了DDP多卡訓練，并嘗試將base model更換為更大的resnet-50進行訓練以提高泛化性，最終在仿真環境中取得了不錯的效果。

ROS2與ROS1的分布式通訊模式不同，ROS2支持在同一個局域網內所有相同random_seed的node之間的topic通訊，這會導致兩臺同時進行測試的client會彼此影響，導致server的不穩定，產生“量子糾纏”，后續在每個新的版本的docker初始化時都會給配對的client和server設定唯一的random_seed，最終random_seed從99排到了91。

比賽規則里物體名稱與仿真環境提供不相符，需要在TaskParser完成相應字段的映射。

采用ACT的方法解釋性比較差，訓練周期較長，改進方法相對不明確，短時間內無法取得較好的效果。基于傳統的目標檢測和三維重建及硬編碼等方法，解釋性較強，改進方向相對明確。

手臂運動和夾爪開合同步進行，容易造成夾取prop后，尚未運行到目標點，夾爪就松開等出乎意料的情況。將二者運動獨立以后，即手臂運動期間，夾爪保持狀態不變，即可避免上述問題。

由于訓練的數據中缺少對prop的俯視照片，所以在夾取前，俯視prop時，容易出現無法識別的情況。采用的解決辦法是，先將盒子放置在桌子上，隨后后退，以此時的相對位置獲得prop的世界坐標，開始抓取時，再將剛才記錄的世界坐標轉換為相對坐標，指導機械手的抓取。

比賽任務流程設計

round_1

目標是移動到目標box的前面固定位置，然后等待抓取任務

1. 解析任務 -> Target: cabinet_index + floor_index + prop_name 2. 走到中間點,調整高度到對應層數 -> [0.4, 0.4, CABINET_POSITION[cabinet_index][-1]] + SLIDE_POSITION[floor_index] 3. YOLO識別box + prop_name -> 識別到目標box/沒識別到則選擇離畫面中心點最近的box 4. YOLO -> 識別到目標box的中心點pos(左右) 5. left_cabinet調整CABINET_POSITION[cabinet_index][0](x+ -> 右), right_cabinet調整CABINET_POSITION[cabinet_index][1](y+ -> 左) 6. 設定目標點 -> 根據5調整CABINET_POSITION對應的位置 7. move_to_point -> 移動到目標點

round_2

目標是找到帶有指定紋理的prop(prop_name，目前只有類型限定)，然后放到指定參照物(object_name)的指定方位

1. 解析任務 -> prop_name, texture_name, target_object_name, target_direction 2. 找到指定參照物,獲取其相對于base的position,table_index,target_position_wrt_base 2.1 先觀察左桌子,再觀察右桌子,通過宏定義其各自的尋找位姿(參考round1_run中的observation) 2.2 當yolo檢測到target_object_name時,is_target_object_finded置為True,結束尋找,獲取其相對于base的position,同時返回table_index(left/right) 2.3 根據target_direction計算target_position_wrt_base,作為prop放置的目標位置 3. 走到中心點找到目標prop,返回floor_index和cabinet_index 3.1 走到中心點,首先觀察左柜子,再觀察右柜子,通過宏定義其各自的尋找位姿 3.2 當yolo檢測到prop_name時,is_prop_finded置為True,結束尋找,返回floor_index和cabinet_index 4. 參考round1_run中的流程,抓取包含prop的box,走到目標table之前 4.1 參考_step_3~_step_9,走到目標table之前,通過宏定義確定目標table前的世界坐標,使用move_to_point移動 5. 微調位置,走到target_object前面,放下box,抓取prop,將prop放置在target_position_wrt_base

round_3

機器人需打開指定層級的儲物單元識別內部物品(prop)，找到并抓取裝有該prop的盒子運至目標桌子旁，取出prop放置在參照物的指定方位

1. 解析任務 -> target_object, layer_index, target_direction 1.1 解析任務指令，確定目標參照物(target_object)、儲物單元層級(layer_index: bottom/top)、放置方位(target_direction) 2. 尋找目標參照物 -> table_index, target_object_position_wrt_world 2.1 依次觀察左右兩張桌子 2.2 當yolo檢測到target_object時，記錄其世界坐標(target_object_position_wrt_world)和所在桌子(table_index) 3. 打開儲物單元并識別Prop -> self.prop_name 3.1 導航至指定儲物單元前 3.2 根據layer_index執行操作： 3.2.1 若為bottom：調用_drawer_open()定位把手拉開抽屜 3.2.2 若為top：調用_cabinet_door_open()定位把手打開柜門 3.3 視覺識別儲物單元內部物品，確定prop名稱(self.prop_name) 4. 定位Prop Box -> cabinet_index, floor_index 4.1 移動至場地中心點，面向中央柜區 4.2 依次觀察左右柜子的各層(second/third/fourth) 4.3 當yolo檢測到包含self.prop_name的box時，記錄柜號(cabinet_index)和層數(floor_index) 5. 抓取Prop Box并移至目標桌子 5.1 導航至cabinet_index和floor_index指定的柜層前 5.2 調用_hug_box()： 5.2.1 視覺微調位置對準目標box 5.2.2 雙臂協同抓取box并取出 5.3 調用_move_to_target_table()： 5.3.1 根據table_index移動到目標桌子前 5.4 放置box并調整姿態： 5.4.1 控制手臂將box放置桌面 5.4.2 后退并調整頭部姿態，視覺精確定位box位置 6. 抓取Prop并放置 6.1 調用_grasp_prop_via_yolo()： 6.1.1 視覺定位box內的self.prop_name 6.1.2 左臂抓取prop并取出 6.2 調用_get_target_position_wrt_base()： 6.2.1 將target_object_position_wrt_world轉換為基坐標系 6.2.2 結合target_direction計算最終放置點 6.3 控制左臂將prop移動到目標放置點后松開