Skip to content
RobotWorld
返回论文库

PAPER DEEP DIVE

足式机器人移动操作力位控制

足式机器人力位统一控制策略

本文提出首个足式机器人力位统一控制策略,无需力传感器,通过RL从历史状态估计外力并补偿位置速度,支持位置/力跟踪与柔顺交互,并在四足与双足机器人上验证。

Peiyuan Zhi, Peiyang Li, Jianqin Yin, Baoxiong Jia, Siyuan Huang2025年5月27日3 分钟阅读
EN

论文:Learning a Unified Policy for Position and Force Control in Legged Loco-Manipulation

作者:Peiyuan Zhi、Peiyang Li、Jianqin Yin、Baoxiong Jia、Siyuan Huang(BIGAI、北京邮电大学)

来源:CoRL 2025 Best Paper

链接arXiv:2505.20829 · 项目主页

代码:已公开,github.com/unified-force/UniFP,包含 B2-Z1 的 Isaac Gym 训练与仿真部署流程;ROS2、MuJoCo 与模仿学习采集管线尚在计划中。

一句话总结

UniFP 用一条不依赖力传感器的强化学习策略,同时建模末端执行器的位置命令和力命令,通过从历史状态估计外力并补偿位置、速度,从而在四足机械臂和双足机器人上实现位置跟踪、力控、力跟踪、阻抗控制和力感知模仿学习。

研究背景与动机

足式机器人近年来的进步集中在复杂地形运动与移动操作。B2、G1 这类平台既能跨过台阶,又能在身体姿态变化时扩大机械臂工作空间,因此“边走边操作”成为具身智能研究的重点。真正困难的不再只是运动学,而是与环境的持续接触。

接触富集任务里,机械臂末端被黑板、抽屉或柜门约束,位置轨迹只是任务的一面。另一个必要信息是接触力:擦黑板必须持续压紧,推柜门必须达到触发机械结构的阈值,开被遮挡的抽屉必须知道“已经碰到”。如果只有位置信号,策略无法判断接触何时建立,也无法决定该施加多少力。

工业界常用力传感器解决这类问题,但力传感器增加成本、重量和维护难度,在很多足式平台上并不普及。部分研究通过电流或阻抗模型间接估计力,但往往只能覆盖单一控制模式。另一部分强化学习工作能完成单点力控或位置控制,却很少把位置控制与力控制在同一条策略里统一训练。

作者观察到,现有视觉运动策略大多只输出关节或末端轨迹,数据集也只记录位置,接触信息天然缺失。论文在 4.2 节用一个擦黑板实验说明:即使任务简单,仅靠轨迹数据也难以维持持续接触。这个观察引出了核心问题:能否让学习到的策略在没有力传感器的情况下同时理解位置命令和力命令,并把这些力信息反哺到模仿学习中。

UniFP 的回答是:把力命令、位置命令和外部扰动一起放进强化学习任务,让策略在仿真中学会从机器人历史状态估计净外力,再通过末端位置补偿和基座速度补偿满足不同控制需求。它不做一套位置策略加一套力策略,而是用同一个网络覆盖多种控制模式。

UniFP 总览
图1:统一力位策略覆盖位置跟踪、力施加和柔顺交互,并通过内部力估计为模仿学习提供力感知演示。

预备知识

阻抗控制把机器人末端建模为弹簧-阻尼-质量系统,使末端既跟踪目标位置,又对外力呈现柔顺响应。式 (1) 给出经典形式:净力由位置误差、速度误差和加速度误差共同决定。UniFP 不直接求解逆动力学,而是把该关系作为奖励和命令合成的依据,让强化学习隐式学会执行这个模型。

Proximal Policy Optimization 是本文底层训练算法。它在 Isaac Gym 中并行运行 4096 个环境,用大量随机命令和扰动训练策略。域随机化用于缩小仿真到真实机器人的差距,包括摩擦、质量、质心、电机强度和外部推力等变化。

模仿学习部分采用行为克隆与扩散策略。传统流程只记录关节状态、末端位置和视觉图像;UniFP 额外记录低层策略估计出的接触力,让上层策略同时看到视觉信息和接触信息。这相当于给轨迹数据补上缺少的“任务物理状态”。

方法详解

UniFP 的建模从统一力位控制公式开始。给定相对机器人机体坐标系的末端位置命令 $\mathbf{x}^{\text{cmd}}$ 与力命令 $\bm{F}^{\text{cmd}}$,目标不是分别执行两个独立控制器,而是让策略在净力 $\bm{F}$ 作用下同时遵守两者。论文从阻抗控制式 (1) 出发:

$$\bm{F}=K(\mathbf{x}-\mathbf{x}^{\text{des}})+D(\dot{\mathbf{x}}-\dot{\mathbf{x}}^{\text{des}})+M(\ddot{\mathbf{x}}-\ddot{\mathbf{x}}^{\text{des}}),$$

其中 $\mathbf{x}$ 是末端实际位置,$\mathbf{x}^{\text{des}}$、$\dot{\mathbf{x}}^{\text{des}}$、$\ddot{\mathbf{x}}^{\text{des}}$ 是期望位置、速度和加速度,$K$、$D$、$M$ 分别是刚度、阻尼和等效惯性。对移动操作中的慢速接触任务,作者保留刚度主导项,得到 $F=K(\mathbf{x}-\mathbf{x}^{\text{des}})$。

末端净力由主动力命令、环境反作用力和外部扰动组成:$\bm{F}=\bm{F}^{\text{ext}}+(\bm{F}^{\text{cmd}}-\bm{F}^{\text{react}})$。将反作用力代入刚度模型,得到末端目标位置:

$$\mathbf{x}^{\text{target}}=\mathbf{x}^{\text{cmd}}+\frac{\bm{F}^{\text{ext}}+(\bm{F}^{\text{cmd}}-\bm{F}^{\text{react}})}{K}.$$

式 (2) 是本方法的关键。它把力命令转译成一个位置偏移:当需要施加力时,末端朝力方向压入环境,反作用力逐渐增大,直到与力命令平衡;当存在外部扰动时,末端相应偏移,表现柔顺;当没有力命令和扰动时,末端就退化为纯位置跟踪。这样,同一条策略可以通过不同的命令组合切换控制模式。

机器人基座也可以使用同一套思想。由于足式机器人的全局位置通常不可用,论文用基座速度命令 $\mathbf{v}_{\text{base}}^{\text{cmd}}$ 和基座力命令 $\bm{F}_{\text{base}}^{\text{cmd}}$ 描述任务。由式 (1) 保留阻尼项,得到基座力与速度误差的关系:

$$\bm{F}_{\text{base}}=D(\dot{\mathbf{x}}_{\text{base}}-\dot{\mathbf{x}}_{\text{base}}^{\text{des}})=D(\mathbf{v}_{\text{base}}-\mathbf{v}_{\text{base}}^{\text{des}}).$$

于是基座目标速度变为:

$$\mathbf{v}_{\text{base}}^{\text{target}}=\mathbf{v}_{\text{base}}^{\text{cmd}}+\frac{\bm{F}_{\text{base}}^{\text{ext}}+(\bm{F}_{\text{base}}^{\text{cmd}}-\bm{F}_{\text{base}}^{\text{react}})}{D}.$$

式 (3) 和式 (4) 解释了一个实验中可见的行为:被踢后即使速度命令和力命令都为零,四足机器人仍会向前走。外力被转换成基座速度补偿,使身体在外界推搡下保持任务意图,而不是僵硬抵抗。

在附录 A 中,论文从式 (2) 推导出四种基本控制模式。无外力无主动力时是位置控制 $\mathbf{x}^{\text{target}}=\mathbf{x}^{\text{cmd}}$;接触中施加力命令时是力控制 $\mathbf{x}^{\text{target}}=\mathbf{x}^{\text{cmd}}+(\bm{F}^{\text{cmd}}-\bm{F}^{\text{react}})/K$;受外部扰动但未主动施力时是阻抗控制 $\mathbf{x}^{\text{target}}=\mathbf{x}^{\text{cmd}}+\bm{F}^{\text{ext}}/K$;动态调整位置命令 $\Delta\mathbf{x}^{\text{cmd}}=\bm{F}^{\text{ext}}/K$ 可实现力跟踪和重力补偿。这些公式不是装饰,而是训练奖励和命令生成的基础。

策略输入也按统一控制设计。观测包括基座姿态、角速度、关节位置、关节速度、上一动作、命令和足相时钟:

$$\mathbf{o}_t=[\mathbf{g}_t^{\text{base}},\bm{\omega}_t^{\text{base}},\mathbf{q}_t,\dot{\mathbf{q}}_t,\mathbf{a}_{t-1},\mathbf{c}_t^{\text{cmd}},\bm{\theta}_t^{\text{feet}}].$$

命令向量 $\mathbf{c}_t^{\text{cmd}}=[\mathbf{v}_{\text{base}}^{\text{cmd}},\mathbf{x}_{\text{ee}}^{\text{cmd}},\bm{F}_{\text{ee}}^{\text{cmd}},\bm{F}_{\text{base}}^{\text{cmd}}]$ 覆盖基座速度、末端位置、末端力和基座力。四足 B2-Z1 使用全部命令;G1 人形机器人因为没有夹爪,只使用基座运动命令和基座力命令。动作是残差关节位置,经动作缩放后加到默认姿态:

$$\mathbf{q}_t^{\text{target}}=\sigma_a\mathbf{a}_t+\mathbf{q}^{\text{default}}.$$

网络结构包含三个模块:观测编码器、状态估计器和动作网络。编码器处理 $H=32$ 步观测历史,输出隐特征;状态估计器从隐特征预测基座速度、末端位置、末端外力和基座外力;动作网络接收当前观测与隐特征,输出关节位置目标。代码中的自适应模块显式列出这四个预测目标:

UniFP 方法总览
图2:方法总览。(a) 策略架构;(b) 力感知模仿学习数据流;(c) 末端与基座的力位补偿;(d) 训练时采样的力命令与扰动。
# legged_gym/b2_gym_learn/ppo_cse_pf/actor_critic.py:13
adaptation_labels = [
    "base_velocity_loss",
    "gripper_pos_loss",
    "force_ee_loss",
    "force_base_loss",
]
adaptation_dims = [3, 3, 3, 3]
adaptation_weights = [.2, .2, 1.0, 1.0]

这里的 loss 结构对应论文的“状态估计器”。训练时使用特权信息构造监督目标,部署时只用 32 步历史观测计算同一隐特征,再由解码器输出估计值。力估计的权重是 1.0,说明论文刻意让网络把主要容量放在接触力上。

为了让策略见到的力场景足够多样,训练命令和外部扰动按随机时间表施加。论文在 C.1 节给出范围:末端位置在球坐标内采样,末端力和基座力在正负 60 N 内采样,基座速度在平面内采样,外部扰动同样覆盖末端和基座。代码中的力采样先随机生成目标力,再分阶段斜坡上升、保持、回落:

# legged_gym/envs/b2/legged_robot_b2z1_pos_force.py:1037
self.force_target_gripper_cmd[...] = torch_rand_float(
    min_force_cmd, max_force_cmd, ...
)
self.current_Fxyz_gripper_cmd[...] = (
    target / push_duration
    * torch.clamp(time_ramp, 0, push_duration)
)

训练采用两阶段课程。第一阶段只学习全身到达和行走,让身体平衡与末端稳定先收敛;第二阶段再加入随机力命令与外部扰动。代码配置中的 `force_start_step=8000` 和 `frame_stack=32` 支持了这个设计:早期步数不施加大力扰动,之后才进入统一力位场景。

奖励函数也直接来自式 (2) 和式 (4)。末端奖励计算当前末端位置与“外力+力命令偏移后的目标位置”的误差,基座奖励计算当前速度与“外力+力命令补偿后的目标速度”的误差。以末端奖励为例:

# legged_gym/envs/b2/legged_robot_b2z1_pos_force.py:1891
forces_offset = (forces_global + forces_cmd_global)
curr_ee_goal_cart_world_offset = (
    forces_offset / self.gripper_force_kps
    + self.curr_ee_goal_cart_world
)
ee_pos_error = torch.sum(torch.abs(
    self.ee_pos - curr_ee_goal_cart_world_offset), dim=1)
rew = torch.exp(-ee_pos_error / self.cfg.rewards.tracking_ee_sigma * 2)

因此,策略不是直接最小化“末端是否到达命令位置”,而是最小化“是否到达考虑了接触力和力命令后的目标位置”。这是 UniFP 区别于普通位置跟踪策略的核心。

模仿学习阶段把上述能力用于数据采集。作者用无线遥控器同时向低层策略发送位置命令和力命令,记录关节状态、基座状态、命令、低层策略估计出的接触力和两路 RGB 图像。扩散策略以图像、机器状态和估计力为输入,输出位置命令与力命令,再交给低层力位策略执行。这样采集的数据天然包含接触信息,无需在硬件上安装力传感器。

flowchart TB
    A[Input commands
base vel / ee pos
ee force / base force] --> B[Observation history H=32] B --> C[Observation encoder] C --> D[Latent feature] D --> E[State estimator
base vel / gripper pos
ee force / base force] D --> F[Actor] E --> G[Estimated states and forces] G --> H[Force offset target] F --> I[Joint position target] I --> J[PD controller] J --> K[Robot / Isaac Gym] K -->|rewards and MSE| C G -.-> L[Force-aware IL data] L --> M[Diffusion policy] M --> A

实验结果

论文首先在仿真中评估位置跟踪。6000 步随机末端轨迹测试中,无外力和无主动力命令时,末端跟踪误差大部分保持在 0.1 m 以内,Y 轴误差略高,作者认为这与该方向可用自由度较少有关。状态估计器对末端位置的估计误差在各轴均小于 0.05 m。

在加入与外力匹配的力命令后,位置跟踪误差仍基本保持在 0.1 m 以内。这说明策略没有把力控制当作独立开关,而是能够在受力条件下继续维持末端目标。图 3 还展示了仿真曲线和真实机器人测力计结果。

真实力控评估使用 0 到 60 N 的力命令,在五个不同末端位置测量误差,平均误差在 10 N 以内;六个离散力等级的估计误差在 5 到 10 N 之间。由于 Unitree Z1 硬件限制,Y 轴和 Z 轴评估上限为 40 N。这个精度已经足够支撑论文选择的接触富集任务。

力与位置跟踪评估
图3:仿真中的位置与力跟踪误差评估,以及真实机器人测力计下的力控制结果。
命令或扰动训练范围含义
末端位置(球坐标)$r\in[0.35,0.85]\text{m}$,$\theta\in[-0.4\pi,0.4\pi]$,$\phi\in[-0.6\pi,0.6\pi]$机体坐标系中的到达目标
末端力命令$[-60,60]\text{N}$末端主动力指令
基座速度命令$v_x\in[-0.8,0.8]\text{m/s}$,$v_y\in[-0.6,0.6]\text{m/s}$,$\omega_z\in[-0.8,0.8]\text{rad/s}$行走与转向指令
基座力命令$[-60,60]\text{N}$基座主动力指令
外部净力末端与基座均为 $[-60,60]\text{N}$环境或人施加的扰动

模仿学习实验在四个真实任务上各执行 50 次,每次最多 1000 步,约 20 秒。基线策略在遥操作数据采集时排除力估计器和力命令,只使用位置控制低层策略。UniFP 的力感知策略成功率达到约 39.5% 的相对提升。

任务无力感知力感知 UniFP绝对提升
擦黑板0.220.58+0.36
开门柜0.360.70+0.34
关门柜0.300.72+0.42
遮挡开抽屉0.300.76+0.46
力感知模仿学习
图4:力感知模仿学习的命令输出、数据采集过程,以及四个接触富集任务的 50 次试验成功率对比。

擦黑板任务最能说明位置控制不足。基线策略常常失去接触,导致墨迹擦不干净,或者压得过重,增加机械应力。UniFP 的低层策略根据估计力持续补偿末端位置,使扩散策略输出的力命令能稳定转换为接触压力。

开柜和关柜任务中,push-to-open 机构的触发行程只有约 3 mm,视觉几乎无法判断是否已经按到位。基线策略因为看不到接触而失败;UniFP 通过估计力准确感知是否触发了机构。遮挡开抽屉任务中,夹爪在操作过程中逐渐被遮挡,纯视觉策略成功率降到 0.30,而 UniFP 仍达到 0.76,说明力信息在视觉不可用时有独立价值。

基本操作技能方面,2.5 kg 哑铃挂在末端时,25 N 的力命令能让机械臂维持平衡;没有该命令则末端因重力下沉。零力命令下,末端受外力会顺着力方向移动,外力撤去后保持新位置,这是力跟踪行为。人与机器人拔河和掰手腕中,末端离目标越远,机器人的抵抗越强,体现阻抗特性。

多样化操作技能
图5:力控制、基座力跟踪、力跟踪和阻抗控制等由同一条策略实现的操作技能。

跨本体实验验证了公式的统一性。B2-Z1 上外力直接补偿到末端;G1 上因为无人形末端夹爪,外力补偿到基座速度。论文报告,当补偿速度与速度命令大小相等方向相反时,人形机器人会停住并倾斜身体维持平衡,而不是被推倒。

硬件系统包括 29 自由度的 Unitree G1、由 12 自由度 B2 和 6 自由度 Z1 组成的四足机械臂、两个 RealSense 相机以及运行推理的 RTX 3090 桌面主机。位置遥操作使用 iPhone 上的 MuJoCo AR;混合力位任务使用 B2 自带无线遥控器,按住 L1 可将末端命令从位置切换为力。论文承认这种采集方式较慢,擦黑板单次约 20 秒,柜门任务约 10 秒。

局限性

作者自述的第一项局限是高频交互和末端工作空间边缘的力估计精度下降。当前模型从历史状态推断力,对动态快速接触的响应不足;作者提出引入速度与加速度项,并把它作为未来工作。

第二项作者自述局限是仿真到真实的差异,尤其是不同坐标轴上的力精度。论文将差异归因于执行器动力学和接触建模的不匹配,并提出域随机化和 real-to-sim 校正。真实评估中 Y 轴误差更明显,说明这一方向仍需专门处理。

第三项局限是当前框架只估计单一交互点。复杂任务往往需要身体和末端同时接触,例如四足机器人用身体顶门、同时用机械臂按门把手。论文把多点接触与全身力交互列为未来方向,目前基座和末端被独立建模。

本文的准静态简化也值得注意。式 (1) 中省略速度与加速度项,适用于擦、推、拔河等慢速任务,但不适用于高速动态操作。作者在附录中明确承认这一点。对于需要快速响应或惯性补偿的场景,统一的纯位置残差策略可能不够。

总结与展望

UniFP 的核心贡献不是新增一个控制器,而是把“位置控制”和“力控制”放进同一个强化学习框架,并用外力估计把二者连接起来。式 (2) 和式 (4) 提供的目标位置与目标速度补偿,使一条策略能覆盖位置跟踪、力控、力跟踪、阻抗和混合力位模式。

该工作还展示了一条数据飞轮:低层策略既完成任务,又作为力感知遥操作底座,产出含接触信息的模仿学习数据。这些数据再训练扩散策略,形成“仿真学到力模型,真实任务复用力模型”的流程。未来如果加入多点力估计、动态项和 real-to-sim 微调,UniFP 有潜力从单一交互点扩展到全身接触操作。

金句:真正的柔顺不是放弃位置,而是让策略知道目标位置应该被外力改到哪里。