去年年底我们实验室接了个有点“折腾”的任务：用四台 Unitree G1 仿人机器人做协同搬箱，正儿八经要把箱子从 A 区搬到 B 区，中间还要绕开几个货架。一开始我以为就是给每台机器人写个“走过去、弯腰、抓起来、放下”的动作脚本，真上手才发现，单人单机好办，一旦牵扯到两台以上机器人抬一个箱子，难度直接翻了不止一倍。这篇文章我完整梳理一遍从仿真搭建到多机协同部署的整个过程，把我试过的方案、踩过的坑、最后保留下来的做法都写进来，给准备做人形机器人多机协同的朋友一个能直接参考的路线图。
我默认你至少跑通过 G1 的官方示例，对 ROS2、Gazebo 或者 MuJoCo 有基本概念。如果完全是零基础，建议先花一周把官方仓库里的 robot 模型和 SDK 示例过一遍，再回来看这篇，不然中间很多参数会看得一头雾水。
1. 为什么是 G1，为什么是协同搬箱：立项时的需求拆解
1.1 G1 的硬件底子决定了它适合什么样的任务
在选型阶段我们对比过几款人形机器人平台，最后留下 G1 的原因其实很朴素：中小型尺寸、全电驱关节、末端有手爪，且官方提供了相对完整的 ROS2 接口和仿真模型。
关键参数大家可以去官方页面核对，我这边只列几个和“搬箱”强相关的指标：
| 项目 | 对搬箱任务的影响 |
|---|---|
| 整机高度约 127cm，重量约 35kg | 重心低，搬运过程中不容易倒，车间地面通行也没压力 |
| 手臂末端自由度 | 能做小范围姿态调整，抓箱时不用把整个身子扭过去 |
| 关节峰值力矩 | 决定单臂能抬多重的箱子，我们实测单机单手抓取 3-5kg 以内的箱子比较稳 |
| 续航时间 | 长任务需要轮换充电，直接影响部署班次设计 |
说实话，G1 的负载能力跟工业机械臂比差得远，但搬箱任务的重点本来就不是“举重”，而是“协调”。一个 40cm × 30cm × 25cm、重量 4kg 左右的纸箱，人形机器人双手抱持或者单手抓取，姿态对了是完全可以胜任的。
1.2 “协同”二字的真实难度：刚性耦合是最大的麻烦
两台机器人抬同一个箱子，物理上是把两个独立浮动的基座通过一个刚性箱子耦合在一起。每台机器人腿部的微小位置偏差，都会通过箱子传递到另一台机器人身上，形成力闭环。说白了，A 机器人往前迈一步，B 机器人如果没跟上，箱子就会扭，两台机器人都会感受到额外力矩，走着走着就可能步态失衡倒地。
单机搬箱，你只管自己重心；多机搬箱，你还要管队友的位置误差、速度误差和加速度误差。这些误差在仿真里不明显，实机上会被地面材质、摩擦系数、电池电压波动无限放大。所以我们在立项时就定下原则：先把协同控制逻辑放在仿真里无限调优，再上实机。
2. 仿真环境搭建：把 G1 和箱子“塞”进物理引擎踩过的坑
2.1 选型：机器人仿真器选型对比与结论
人形机器人仿真这几年可选的无非是 MuJoCo、Gazebo、Isaac Sim、PhysX 这几个。我们最终选了MuJoCo + ROS2 桥接，原因很实际：
- MuJoCo 对接触动力学模拟快，稳定步长在 0.002s 下能跑实时性很好的闭环控制；
- G1 官方在 MuJoCo 里已经提供了 XML 模型描述文件，包括关节限位、摩擦参数、电机模型，导入手感很完整；
- Gazebo 虽然 ROS2 集成更顺，但对多刚体接触精度和仿真速度的折中不够好，跑四台机器人同时抬箱子，CPU 负载和实时率都不理想。
ISAAC Sim 画质好、物理精度高，但部署门槛高，团队里没人熟 Omniverse 那一套，放弃。我的建议是，如果你只有一台普通工作站，MuJoCo 是效率最高的起步选择。
2.2 环境安装和 G1 模型导入：三个低级但致命的坑
基础依赖按官方 README 装：Python 3.10、mujoco、numpy、scipy、ros2 humble 桥接包。如果用的是 Docker，记得把渲染相关的 Vulkan/OpenGL 库装齐，不然加载 xml 模型后窗口黑屏。
模型导入阶段我们踩过三个坑：
**坑一：模型缩放单位。**官方 XML 里尺寸单位是米，但如果你的场景文件或 mesh 文件是厘米，机器人会凭空放大一百倍，关节力矩不适配，站都站不起来。检查方式是量一下骨盆宽度，如果视觉上 G1 像个小巨人，就是单位错了。
**坑二：关节初始角度与真实零位不一致。**G1 的胳膊在零点位置是自然下垂的，但有些二次开发模型把肩关节零点设在水平伸展位，导致你的站立控制器一启动，手臂就飞起来。导入后第一件事永远是逐个关节打印角度，和官方零位表对比。
**坑三：摩擦系数默认值太理想。**MuJoCo 默认接触摩擦在塑料地板上近似光滑，实际仓库地砖糙很多。我们在仿真里把模型的手掌和脚底摩擦系数调到 1.0 以上，再跑行走测试，后面对比实机才知道要这样调。
2.3 搭箱子场景：不是放个长方体那么简单
搬箱场景里，箱子要建模成带质量的盒子，并在底面和侧面定义接触对。最简单的 XML 配置大致长这样：
<body name="box_cargo" pos="0 0 0.4"> <geom name="box_geom" type="box" size="0.2 0.15 0.12" mass="4.0" friction="0.8" rgba="0.8 0.6 0.2 1"/> </body>注意几个细节：
- **质量不能均匀分布。**实际纸箱里东西可能偏置，用默认均匀质心会过于理想。我们后来把质心故意偏移 5cm，测试两机协同扛箱时的姿态鲁棒性，结果非常有价值。
- **地面的摩擦系数至少分两种区域。**仓库入口是环氧地坪，里面是固化地砖，摩擦系数差 0.2 左右，我们直接在仿真里画了两块不同摩擦区，让机器人在跨越区域时保持步态稳定。
3. 单机技能包先行：走路、识别、抓箱要练成肌肉记忆
3.1 步态与上身稳定：让 G1 走得像人而不是像提线木偶
搬箱的第一步是单机能带着目标走稳。G1 自带 SDK 里有基础步态库，调用起来很简单，但真实项目中不能只靠默认参数。
我用了经典的LIPM（线性倒立摆）+ 摆动腿轨迹规划来做步态控制，在 SDK 基础上增加了步行速度、步幅、离地高度三个可调参数。手调经验值：
- 搬箱工况建议步行速度 0.4m/s 以下，超过 0.6m/s 末端箱子惯性会干扰重心。
- 步高 3-4cm 就够了，太高会让躯干起伏，手爪抓持平面跟着颤动。
- 骨盆高度动态调整，箱子越重，重心越低。
仿真里试 1.0m/s 跑得很欢，但放上 4kg 箱子后，上身会前后晃。后来我意识到，人形机器人搬东西不是腿的事，是全身协调发力的事。手臂承载后相当于给躯干增加了一个外部力矩源，必须在步态控制里前馈补偿。
3.2 视觉识别：用AprilTag 做箱体定位不丢精度
多机协作的第一前提是知道箱子在哪。我们试过纯点云识别、YOLO 检测，最后发现项目初期最稳的还是AprilTag。
在箱体侧面贴一张 12cm 的 AprilTag，用 RGB 相机检测角点，P3P 解算出箱子相对于相机坐标系的 6D 位姿。配合 G1 头部云台的标定参数，可以做到 0.5 米距离内位置误差小于 1cm，角度误差小于 1°。
关键提示：相机标定不要偷懒。先用棋盘格做完整的内参标定，再用已知大小的 Tag 做外参验证。我们第一次上实机抓空就是因为相机内参大概是官方默认值，导致深度方向偏了 4cm，手爪撞到箱子边缘。
3.3 抓取和抬箱姿态：箱子一离地就要切换控制策略
抓取动作我们用预定义轨迹 + 力控收尾：
- 手臂移动到预抓取位置（箱子中心上方 5cm）；
- 手爪张开到略大于箱宽；
- 下降到手接触箱体表面，检测关节电流突增；
- 切换到力位混合控制，以竖直方向的力控制为目标，保证双手抬箱时不把箱子压变形。
箱子离地后，手臂就不再是“跟随位置”了，而是“辅助躯干平衡”。控制模式上，双手末端位置要在箱子坐标系内保持不变，但允许微小的柔顺偏差，用来吸收躯干晃动带来的冲击。
4. 多机协同的系统搭建：任务分配、通信和负载均衡一个都不能少
4.1 架构总览：四台机器人的角色分工
我们把四台 G1 分成两组，每组两台抬一个箱子。每组内部分为主机和从机：
- 主机（Leader）：负责全局路径规划、状态发布，接收任务分配节点下发的目标点；
- 从机（Follower）：不关心全局路径，只跟随主机的目标速度和期望位置，但保留独立的安全急停。
简单来说，主机“带路”，从机“跟上”。箱子在两者之间形成了刚性约束，用位姿误差反馈把两者拉在一起。ROS2 的 Topic 结构大概是：
/leader_pose # 主机发布自身的位姿 /follower_target # 主发给从的期望跟随时位置 /box_state # 视觉系统发布的箱子位姿 /group_cmd # 任务调度节点下发的全局命令4.2 避碰策略：不是全局规划，而是局部速度场
四台机器人加两个箱子在仓库里走，全局路径规划想过用经典 A* + RRT*，但实际发现规划出来的路径精度根本没法在多机协同里直接用，两台机器人之间的最小间距时刻在变化。
最后用的是速度障碍法（Velocity Obstacles, VO）。每台机器人感知到周围其他机器人的当前位置和速度，在速度空间里构建一个“会碰撞的速度区域”，然后避开这个区域选最优速度。加上一点优先级规则：搬运组永远比空载组优先级高，空载机器人要让路。
VO 在仿真里跑得很顺，一个棘手问题是箱子末端摆动造成体积膨胀。箱子不是质点，它有长宽高，两机抬箱转弯时，箱子扫过的区域比机器人本体大得多。所以我们在 VO 里把每台机器人连同箱子的包络盒一起注册，相当于搬运组“变大了一圈”，这样规避更保守，换来了真正的安全。
4.3 力觉与负载均衡：从机不是傻子式跟随
最早版本从机只做位置跟随，一跑起来就出问题：只要主机位置突变，从机刚性跟随会把箱子掰歪。
正确的做法是加一个导纳控制外环：
- 把箱子视为弹簧-阻尼系统连接两端；
- 主机位置变化 → 箱子产生位移 → 从机感受到力；
- 从机不直接跟踪位置，而是通过调整自身关节力矩来维持目标交互力。
通俗讲：从机不是跟主机的位置，而是跟着主机“给箱子施加的力”。这样两台机器人之间哪怕有 2-3cm 的位姿误差，箱子内部的力也不会持续累积。
实际调参时，最看重导纳控制里的质量-阻尼-刚度系数。我们初期刚度过大，箱子震荡明显；刚度过小，跟随滞后，走 S 形路线时两机会“拉锯”。最后阻尼系数固定，刚度降到临界值的 70%，效果最好。
4.4 通信可靠性：不是话题通了就行
ROS2 DDS 默认实时性在多机 WiFi 下不够用。我们换了几种配置组合，最后稳定的是：
- 使用 CycloneDDS 作为 RMW 实现；
- 开启组播发现，关闭跨网段广播；
- 关键控制指令的 QoS 设成 Reliability Reliable + Deadline 20ms；
- 状态反馈消息允许 Best Effort，避免把带宽吃满。
实测在 5GHz WiFi 下，控制指令端到端延迟稳定在 7-12ms 左右，四台机器人加两个相机一共 30 多个 Topic，CPU 占用还能接受。
5. 仿真验收到实机部署：中间得经过怎样的转译
5.1 先在仿真里考一场“实操考试”
上实机前，我们在 MuJoCo 里跑了一套模拟考：
- 两台机从静止启动，抬箱直线走 10m；
- 途中 90° 转弯；
- 两箱交错会车；
- 模拟一台机电池电量骤降 20%（关节力上限打八折）；
- 人为给主机 5cm 的侧向位置干扰，观察从机能否纠正。
第五项测试相当关键。仿真里我们一推主机，从机能靠导纳控制逐步把力归零；但第一版从机规划里没有恢复期望力矩这一项，结果主机偏移后从机一直维持着错误方向的力，箱子被拧着走完全程。后来加上“期望交互力矩修正”才彻底解决，这个坑强烈建议大家在仿真里提前踩一遍。
5.2 实机部署的坐标系与基座标定
仿真里坐标系是理想值，实机上四台机器人各有一个局部坐标系，而且站立地基还有微小沉降，如果不对齐全局坐标系，协同就是空中楼阁。
我们用外部动捕系统作为全局参考：在每台机器人躯干贴四个反光点，动捕频率 100Hz，把全局位姿发布到 ROS2，机器人本地里程计和外部位姿做扩展卡尔曼滤波融合，最后全局定位精度能到 1cm 以内。
这个方案成本不低，但考虑到协同控制对位姿误差的敏感度远大于单机，这笔钱不能省。只有单机调试时我们用机载里程计就够了。
5.3 安全冗余：三层急停外加人工接管
多机协同的失控风险是单机的几何倍数，我们的安全设计做了三层：
| 层级 | 机制 | 触发条件 |
|---|---|---|
| 第一层 | 每台机器人本地力矩上限超限急停 | 关节电流超过设定阈值 500ms |
| 第二层 | 组间相对位姿超差急停 | 主从机位姿偏差 > 3cm 持续 1s |
| 第三层 | 远程监控面板人工急停 | 操作员观察到任何异常 |
值得一提的细节：远程急停指令走的是独立于 ROS2 的 UDP 专用通道，避免 ROS2 节点卡死导致控制指令与急停指令同链路全断。这在仿真里发现不了，但在实机上我们模拟过 ROS2 节点崩溃，发现系统还能安全停下，算是个很有成就感的时刻。
6. 复盘：整个项目我总结的六个高频坑
6.1 摩擦系数与仿真参数不一致导致步态偏移
仿真里我们把地面摩擦设成 1.2，实机地砖实际只有 0.9。步态一启动就出现轻微打滑，主机和从机距离越拉越大。解决办法是提前在实机场地做摩擦实测，然后用实测值回填仿真参数。别信“差不多”的参数，人形机器人对地面条件极其敏感。
6.2 箱子质心偏移被忽略
仿真初期箱体质心居中，所有试验一次过。直到某次模拟“箱内重物偏置”后才暴露问题：转弯时外侧机器人受到的额外力矩明显增大，步态紊乱概率上升。实机上我们用压舱物模拟了 5% 的质心偏移，在控制端增加前馈补偿，才真正稳住。
6.3 视觉检测帧率和控制帧率不匹配
相机 30Hz 输出位姿，控制器 500Hz 运行。箱子位置跳到控制器里被当成了噪声，直接影响导纳控制输出。解决方法是加一个低通滤波器并缓存最近 6 帧数据做稳定估计。
6.4 电池 SOC 对关节力矩的非线性影响
仿真是理想电源，实机跑 20 分钟后电池电压下降，关节最大力矩明显变小，原来能扛的箱子现在扛着余量不足。我们在控制层根据 SOC 动态调整步态参数和速度上限，最后 10 分钟自动降速 30%。
6.5 DDS 发现机制挤占带宽
四台机入网后，DDS 的发现协议在全网广播，高峰期 WiFi 带宽被挤到几乎不可用。CycloneDDS 配置里把发现流量限制到仅限同一网段的指定端口，带宽立刻恢复正常。
6.6 人形机器人协同的现实期望管理
最后一个坑可能不算技术，但同样重要。人形机器人多机协同的亮点在于“类人任务编排能力”，不是“超稳定举重能力”。搬箱这种任务，轮式 AGV 加机械臂成熟又便宜，人形机器人的价值在于复杂地形适应性、类人交互方式和柔性操作潜力。展示时候选好场景，别拿着人形机器人的短处去跟传统自动化设备的强项硬拼，项目立得住，后面资源才好争取。
我在实机稳定运行两周后回看整个项目，最庆幸的是我们在仿真里提前压榨了足够多的异常场景。人形机器人的多机协同，本质上是把人的协作默契翻译成机器人之间的控制协议，你越尊重物理世界的刚性约束和不确定性，系统就越稳。希望这篇从仿真到部署的完整拆解能让你少走几个月的弯路。
