2025 ICRA Sim2Rea 獲獎(jiǎng)隊(duì)伍給我們帶來(lái)MuJoCo+ROS2閉環(huán)控制與精準(zhǔn)視覺抓取全流程解析

項(xiàng)目主要包括Communication、Envs、Navigation、PoseTransform、TaskUnderstanding、VisionUnderstanding這六個(gè)部分，功能如下：

核心亮點(diǎn)

我們重構(gòu)了機(jī)器人的底層運(yùn)動(dòng)控制，解決了仿真到現(xiàn)實(shí)的遷移難題與開環(huán)控制的巨大誤差。 初始控制算法在從MuJoCo仿真遷移到ROS2真實(shí)環(huán)境時(shí)完全失效。為此，我們放棄了簡(jiǎn)單的開環(huán)控制，通過引入關(guān)節(jié)狀態(tài)和里程計(jì)作為閉環(huán)反饋信號(hào)，并結(jié)合簡(jiǎn)化的PID調(diào)節(jié)邏輯，重寫了驅(qū)動(dòng)底層。特別地，我們?cè)O(shè)計(jì)了利用sigmoid函數(shù)的平滑控制算法，實(shí)現(xiàn)了機(jī)器人運(yùn)動(dòng)的平滑啟停與變速。最終，這套閉環(huán)控制系統(tǒng)使機(jī)器人的底盤移動(dòng)精度達(dá)到0.01米，旋轉(zhuǎn)精度達(dá)到0.1度，為上層任務(wù)的穩(wěn)定執(zhí)行提供了高精度、高流暢度的運(yùn)動(dòng)能力基礎(chǔ)。

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

項(xiàng)目亮點(diǎn)

項(xiàng)目結(jié)構(gòu)清晰，各個(gè)功能模塊解耦，API命名和編寫符合規(guī)范，文檔完整清晰，易于開發(fā)和維護(hù)。

大部分算法同時(shí)提供了ROS2和MuJoCo環(huán)境的實(shí)現(xiàn)，支持ROS2分布式和MuJoCo本地調(diào)試。

比賽的入口文件是run.py，通過一個(gè)文件自動(dòng)檢測(cè)比賽進(jìn)程，切換不同的任務(wù)腳本（round_1~3）執(zhí)行。在任務(wù)腳本中也按照步驟給出了完整的任務(wù)流程和實(shí)現(xiàn)。

在Envs的GraspAppleTask類中提供了一個(gè)蘋果抓取的環(huán)境，并在DISCOVERSE的mjcf中提供了對(duì)應(yīng)的xml文件，主要用于測(cè)試MuJoCo環(huán)境中的坐標(biāo)轉(zhuǎn)換算法。

在Communication的MMK2_Controller類中提供了豐富的MMK2機(jī)器人控制方法，并提供多次優(yōu)化之后的_move_base底層驅(qū)動(dòng)方法，實(shí)現(xiàn)機(jī)器人運(yùn)動(dòng)的精準(zhǔn)、流暢控制，移動(dòng)精度達(dá)到0.01m，轉(zhuǎn)動(dòng)精度達(dá)到0.1°。

在Navigation的MoveToPoint類中針對(duì)比賽環(huán)境提供了精準(zhǔn)導(dǎo)航方法，只需要目標(biāo)位置的坐標(biāo)和朝向，配合MMK2_Controller即可實(shí)現(xiàn)任意位置的精準(zhǔn)到達(dá)。

在PoseTransform的PoseTransform類中基于MuJoCo構(gòu)建實(shí)時(shí)同步數(shù)字孿生環(huán)境實(shí)現(xiàn)機(jī)器人的正逆運(yùn)動(dòng)學(xué)解算，并針對(duì)相機(jī)坐標(biāo)系、基座坐標(biāo)系、底盤坐標(biāo)系和世界坐標(biāo)系提供了彼此坐標(biāo)轉(zhuǎn)換的方法。

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

在VisionUnderstanding的RosVisionModule類中提供了物體識(shí)別和位姿計(jì)算方法，視覺模型包括YOLO v11/v12的目標(biāo)檢測(cè)模型和SAM2的實(shí)例分割模型，提供了基于RGB-D信息的快速位姿計(jì)算方法和基于點(diǎn)云匹配的精準(zhǔn)位姿計(jì)算方法。針對(duì)比賽任務(wù)，提供了道具Box的精準(zhǔn)位姿計(jì)算方法和Box放置坐標(biāo)的計(jì)算方法。

開發(fā)時(shí)的踩坑與重構(gòu)

MMK2機(jī)器人的底盤控制算法最開始是基于DISCOVERSE中的mmk2_base.py編寫的，只是簡(jiǎn)單地將對(duì)應(yīng)關(guān)節(jié)的數(shù)值綁定到對(duì)應(yīng)的關(guān)節(jié)上，因?yàn)殄e(cuò)誤地使用了位置變量qpos作為控制對(duì)象，所以在低速情況下，在MuJoCo環(huán)境中的效果還可以，但是遷移到ROS2中發(fā)現(xiàn)對(duì)應(yīng)的topic綁定的其實(shí)是驅(qū)動(dòng)變量ctrl，無(wú)法兼容。經(jīng)過第一輪重構(gòu)，通過實(shí)驗(yàn)得到穩(wěn)定運(yùn)行時(shí)各個(gè)關(guān)節(jié)的參數(shù)范圍，但是發(fā)現(xiàn)開環(huán)控制具有很大的誤差，因此引入關(guān)節(jié)位置參數(shù)joint_states和底盤里程計(jì)參數(shù)odom作為閉環(huán)反饋信號(hào)。經(jīng)過多輪迭代，引入了簡(jiǎn)化版本的PID調(diào)節(jié)，在最終的MMK2_Controller版本中，使用_move_base作為機(jī)器人底盤閉環(huán)運(yùn)動(dòng)的底層驅(qū)動(dòng)，并利用sigmoid函數(shù)在_smooth_control_step實(shí)現(xiàn)了更加平滑流暢的控制，最終實(shí)現(xiàn)移動(dòng)精度0.01m，轉(zhuǎn)動(dòng)精度0.1°。同時(shí)使用相同的想法完善了更加平滑連貫的set_arm_position方法實(shí)現(xiàn)機(jī)械臂的控制。

對(duì)于環(huán)境中的物體位姿計(jì)算，一開始采用了傳統(tǒng)的目標(biāo)檢測(cè)+深度點(diǎn)云重建+點(diǎn)云位姿匹配的流程，但是點(diǎn)云匹配的精度和速度都很難滿足要求，因此采用了和官方baseline中類似的方法，只使用目標(biāo)檢測(cè)+深度信息實(shí)現(xiàn)了較為精準(zhǔn)的位姿計(jì)算，實(shí)現(xiàn)了精度和速度的平衡。對(duì)于需要更加精準(zhǔn)定位的物體，比如cabinet上盛放三種prop的box，需要在移動(dòng)到物體前方后進(jìn)行二次定位和微調(diào)，具體通過get_Toc_box_from_yolo這一類方法，考慮到了檢測(cè)到不同數(shù)量的box的情況，增加了系統(tǒng)容錯(cuò)。

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

針對(duì)比賽任務(wù)，我們采集了大量的數(shù)據(jù)，根據(jù)比賽環(huán)境對(duì)任務(wù)流程做了深入的優(yōu)化。采集了1000條左右的目標(biāo)檢測(cè)數(shù)據(jù)進(jìn)行YOLO（YOLO v11/v12）訓(xùn)練，最終基本能實(shí)現(xiàn)對(duì)場(chǎng)景中全部物體（包括drawer的上下兩個(gè)把手）的精準(zhǔn)識(shí)別定位；采集了近萬(wàn)條抓取仿真數(shù)據(jù)進(jìn)行box和不同prop的ACT訓(xùn)練，完善了并行訓(xùn)練腳本實(shí)現(xiàn)了DDP多卡訓(xùn)練，并嘗試將base model更換為更大的resnet-50進(jìn)行訓(xùn)練以提高泛化性，最終在仿真環(huán)境中取得了不錯(cuò)的效果。

ROS2與ROS1的分布式通訊模式不同，ROS2支持在同一個(gè)局域網(wǎng)內(nèi)所有相同random_seed的node之間的topic通訊，這會(huì)導(dǎo)致兩臺(tái)同時(shí)進(jìn)行測(cè)試的client會(huì)彼此影響，導(dǎo)致server的不穩(wěn)定，產(chǎn)生“量子糾纏”，后續(xù)在每個(gè)新的版本的docker初始化時(shí)都會(huì)給配對(duì)的client和server設(shè)定唯一的random_seed，最終random_seed從99排到了91。

比賽規(guī)則里物體名稱與仿真環(huán)境提供不相符，需要在TaskParser完成相應(yīng)字段的映射。

采用ACT的方法解釋性比較差，訓(xùn)練周期較長(zhǎng)，改進(jìn)方法相對(duì)不明確，短時(shí)間內(nèi)無(wú)法取得較好的效果?；趥鹘y(tǒng)的目標(biāo)檢測(cè)和三維重建及硬編碼等方法，解釋性較強(qiáng)，改進(jìn)方向相對(duì)明確。

手臂運(yùn)動(dòng)和夾爪開合同步進(jìn)行，容易造成夾取prop后，尚未運(yùn)行到目標(biāo)點(diǎn)，夾爪就松開等出乎意料的情況。將二者運(yùn)動(dòng)獨(dú)立以后，即手臂運(yùn)動(dòng)期間，夾爪保持狀態(tài)不變，即可避免上述問題。

由于訓(xùn)練的數(shù)據(jù)中缺少對(duì)prop的俯視照片，所以在夾取前，俯視prop時(shí)，容易出現(xiàn)無(wú)法識(shí)別的情況。采用的解決辦法是，先將盒子放置在桌子上，隨后后退，以此時(shí)的相對(duì)位置獲得prop的世界坐標(biāo)，開始抓取時(shí)，再將剛才記錄的世界坐標(biāo)轉(zhuǎn)換為相對(duì)坐標(biāo)，指導(dǎo)機(jī)械手的抓取。

比賽任務(wù)流程設(shè)計(jì)

round_1

目標(biāo)是移動(dòng)到目標(biāo)box的前面固定位置，然后等待抓取任務(wù)

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

round_2

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

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

round_3

機(jī)器人需打開指定層級(jí)的儲(chǔ)物單元識(shí)別內(nèi)部物品(prop)，找到并抓取裝有該prop的盒子運(yùn)至目標(biāo)桌子旁，取出prop放置在參照物的指定方位

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