从“抓瞎”到“精准到0.01m”：我们是怎么调教机器人不乱动的？

靖靖不靖靖 · 2025 年6 月 16 日 05:36

2025 ICRA Sim2Rea 获奖队伍给我们带来MuJoCo+ROS2闭环控制与精准视觉抓取全流程解析

项目主要包括Communication、Envs、Navigation、PoseTransform、TaskUnderstanding、VisionUnderstanding这六个部分，功能如下：

模块名称	主要功能	核心类或函数
Communication	MMK2的MuJoCo、ROS2通讯代码和驱动算法	TopicSubscriber、TopicPublisher、MMK2_Receiver、MMK2_Controller
Envs	用于调试的MuJoCo环境	GraspAppleTask
Navigation	计算并导航至目标位置	MoveToPoint
PoseTransform	MMK2的MuJoCo、ROS2坐标变换和逆运动学计算	PoseTransform、transform_position_wrt_camera_to_base、get_world_position_from_head_camera等
TaskUnderstanding	比赛任务解析和格式化输出	TaskParser
VisionUnderstanding	识别并计算物体位姿	RosVisionModule、get_Toc_box_from_yolo、find_space_for_box等

核心亮点

我们重构了机器人的底层 运动控制 ，解决了仿真到现实的迁移难题与开环控制的巨大误差。 初始控制算法在从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移动到目标放置点后松开