机器人重力补偿技术的 MuJoCo 实现解析

struct mjData_ { // ... mjtNum* qfrc_gravcomp; // 重力补偿扭矩向量 (nv x 1) mjtNum* qfrc_passive; // 总被动力向量 (nv x 1) // ... };

qfrc_gravcomp 存储独立计算的重力补偿扭矩，最终与弹簧力、阻尼力等被动力合并为 qfrc_passive，通过 mj_rne 函数在每个仿真步更新。

计算流程解析

MuJoCo 的重力补偿计算流程可分为三个阶段：

1. 模型解析阶段：从 XML 模型文件中读取连杆质量、惯性参数和关节结构，构建动力学计算所需的数据结构。以 model/humanoid/humanoid.xml 为例，其中 <geom> 标签的 mass 属性和 <joint> 标签的 armature 属性直接影响重力扭矩计算。
2. 动力学计算阶段：在每个仿真步，通过 src/engine/engine_forward.c 中的 mj_forward 函数触发重力补偿计算，具体通过调用 mj_rne 函数实现。该函数采用递归方式计算每个关节的重力载荷。
3. 控制集成阶段：用户可通过读取 qfrc_gravcomp 字段获取补偿扭矩，将其加入控制输入。MuJoCo 提供两种集成方式：手动叠加或通过 mjOption 设置自动补偿标志。

图 1：肌腱驱动系统中重力引起的非线性扭矩分布，红色线条表示不同姿态下的肌腱张力变化

实践层：从零构建重力补偿控制系统

基础实现：核心 API 调用与控制循环

如何在仿真环境中实现基础的重力补偿控制？以下 Python 代码展示了完整的控制链路，从模型加载到补偿扭矩应用：

import mujoco
import numpy as np

# 1. 加载模型与创建数据结构
model = mujoco.MjModel.from_xml_path("model/humanoid/humanoid.xml")
data = mujoco.MjData(model)

# 2. 配置仿真参数
model.opt.timestep = 0.002 # 500Hz 控制频率
model.opt.gravity = [0, 0, -9.81] # 设置重力方向

# 3. 初始化控制目标
target_pos = np.array([0.2, -0.5, 0.3, 1.0, 0.0, 0.0, 0.0]) # 关节空间目标位置
Kp = np.diag([500, 400, 300, 200, 100, 100, 50]) # 比例增益矩阵
Kd = np.diag([50, 40, 30, 20, 10, 10, 5]) # 微分增益矩阵

# 4. 控制循环
for _ in range(10000):
    # 计算关节空间误差
    pos_error = target_pos - data.qpos[:7]
    vel_error = -data.qvel[:7]
    
    # 计算 PD 控制输出
    pd_torque = Kp @ pos_error + Kd @ vel_error
    
    # 叠加重力补偿扭矩
    data.ctrl[:7] = pd_torque + data.qfrc_gravcomp[:7]
    
    # 执行仿真步
    mujoco.mj_step(model, data)
    
    # 每 100 步打印状态
    if _ % 100 == 0:
        print(f"Position error: {np.linalg.norm(pos_error):.4f} m")

操作要点：在读取 qfrc_gravcomp 前必须确保已调用 mj_step 或 mj_forward 更新动力学状态，否则将使用过时的补偿值导致控制漂移。

场景适配：面向特定应用的补偿策略

不同机器人系统对重力补偿有不同需求，以下是三种典型场景的适配方案：

1. 固定基座机械臂：全补偿策略

工业机械臂通常采用全重力补偿，通过 model/robot/arm.xml 类型的模型实现：

def full_compensation_control(model, data, target_pos):
    # 更新动力学状态
    mujoco.mj_forward(model, data)
    # 计算关节空间 PD 控制
    pos_error = target_pos - data.qpos
    pd_torque = 300 * pos_error - 20 * data.qvel
    # 全重力补偿
    return pd_torque + data.qfrc_gravcomp

2. 移动机器人：选择性补偿

对于腿足机器人，可对腿部关节保留重力影响以模拟真实步态：

def selective_compensation(model, data):
    # 更新动力学
    mujoco.mj_forward(model, data)
    # 复制原始补偿扭矩
    compensation = data.qfrc_gravcomp.copy()
    # 对腿部关节（ID 0-5）禁用补偿
    compensation[:6] = 0.0
    return compensation

3. 柔性机器人：自适应补偿

柔性机器人需要考虑形变引起的质量分布变化，可通过 plugin/elasticity/elasticity.cc 中的弹性模型实现自适应补偿：

def adaptive_compensation(model, data, deformation):
    # 更新弹性形变
    mujoco.mj_forward(model, data)
    # 基础重力补偿
    base_comp = data.qfrc_gravcomp.copy()
    # 根据形变调整补偿
    adaptive_term = deformation @ model.opt.elastic_stiffness
    return base_comp + adaptive_term

性能调优：从算法到硬件的优化路径

如何将重力补偿计算时间从 1.2ms 降至 0.3ms，满足高频控制需求？以下是多层级优化策略：

算法优化：稀疏矩阵与并行计算

# 启用稀疏雅可比矩阵计算
model.opt.jacobian = mujoco.mjtJacobian.mjJAC_SPARSE
# 设置多线程计算
model.opt.threads = 4 # 使用 4 核并行计算
# 验证优化效果
before = time.time()
mujoco.mj_forward(model, data)
after = time.time()
print(f"优化后计算时间：{(after - before)*1000:.2f} ms")

数据结构优化：预计算与内存布局

通过 src/engine/engine_memory.c 中的内存池管理，优化数据访问模式：

// 预分配连续内存块存储重力补偿相关数据
mjtNum* grav_buffer = mj_malloc(model.nv * sizeof(mjtNum));
// 优化数据访问局部性
for (int i = 0; i < model.nv; i++) {
    grav_buffer[i] = data.qfrc_gravcomp[i];
}

硬件加速：AVX 指令与 GPU 计算

MuJoCo 通过 src/engine/engine_util_sparse_avx.h 提供 AVX 指令优化，可通过编译选项启用：

# 启用 AVX 优化编译
cmake -DENABLE_AVX=ON ..
make -j8

对于大规模模型，可使用 MJX（MuJoCo 的 GPU 加速版本）实现并行重力补偿计算：

import mujoco.mjx as mjx

# GPU 加速的重力补偿计算
mjx_model = mjx.put_model(model)
mjx_data = mjx.put_data(model, data)
mjx_data = mjx.forward(mjx_model, mjx_data)
grav_comp = mjx_data.qfrc_gravcomp

图 2：不同阻抗参数（pow 和 mid 值）下的力 - 位移关系曲线，影响重力补偿的平滑度与响应速度

跨场景应用对比：重力补偿的领域适配策略

不同机器人领域对重力补偿有不同要求，以下是四个典型领域的适配策略对比：

以医疗手术机器人为例，其实现代码需要特别关注补偿精度：

def surgical_robot_compensation(model, data, tool_mass):
    # 工具质量实时更新
    model.body("tool").mass = tool_mass
    # 高精度重力补偿
    mujoco.mj_forward(model, data)
    # 加入温度补偿项
    temp_factor = 1.0 + 0.002 * (data.sensor("temp").data[0] - 25)
    return data.qfrc_gravcomp * temp_factor

故障诊断与解决方案：基于故障树的问题分析

重力补偿系统常见故障树

重力补偿失效
├─ 动力学计算错误
│  ├─ 模型参数错误 → 检查<geom>质量属性
│  ├─ 关节限制未考虑 → 检查<limit>标签设置
│  └─ 惯性张量异常 → 验证<inertia>参数
├─ 数据访问问题
│  ├─ 未调用 mj_forward → 确保控制循环顺序正确
│  ├─ 数据指针错误 → 检查 mjData 结构体初始化
│  └─ 内存越界访问 → 启用数组边界检查
└─ 环境参数变化
   ├─ 重力方向设置错误 → 检查 model.opt.gravity
   ├─ 外部载荷变化 → 实现质量自适应算法
   └─ 温度漂移 → 加入温度补偿项

典型问题解决方案

问题 1：静态漂移

症状：机器人在目标位置出现缓慢漂移 根因：补偿扭矩与实际重力扭矩存在静态误差 解决方案：

def calibrated_compensation(model, data, calibration_params):
    # 基础补偿
    mujoco.mj_forward(model, data)
    base_comp = data.qfrc_gravcomp
    # 应用校准参数
    calibrated_comp = base_comp @ calibration_params['gain_matrix'] + calibration_params['offset']
    return calibrated_comp

问题 2：动态震荡

症状：快速运动时出现震荡 根因：补偿延迟与控制环路不匹配 解决方案：

class PredictiveCompensator:
    def __init__(self, model, horizon=5):
        self.model = model
        self.horizon = horizon
        self.buffer = []

    def predict_gravity(self, data, qpos_future):
        # 预测未来状态的重力补偿
        pred_data = mujoco.MjData(self.model)
        pred_data.qpos = qpos_future
        mujoco.mj_forward(self.model, pred_data)
        return pred_data.qfrc_gravcomp

    def get_compensation(self, data, current_qpos, qvel):
        # 预测未来位置
        qpos_future = current_qpos + qvel * self.model.opt.timestep * self.horizon
        # 获取预测补偿
        pred_comp = self.predict_gravity(data, qpos_future)
        # 缓存并平滑
        self.buffer.append(pred_comp)
        if len(self.buffer) > 5:
            self.buffer.pop(0)
        return np.mean(self.buffer, axis=0)

# 使用预测补偿器
compensator = PredictiveCompensator(model)
data.ctrl[:] = pd_torque + compensator.get_compensation(data, data.qpos, data.qvel)

图 3：不同接触状态下的力分布仿真，蓝色虚线表示未补偿重力时的接触力偏差，红色实线表示补偿后的理想分布

技术演进：重力补偿的未来发展方向

融合学习的智能补偿

随着强化学习技术的发展，未来重力补偿系统将结合数据驱动方法，通过 python/mujoco/sysid/中的系统辨识工具，实现自适应补偿：

from mujoco.sysid import IdentificationPipeline

# 系统辨识获取动力学模型
pipeline = IdentificationPipeline(model)
data = pipeline.collect_data(controller=random_controller)
dyn_model = pipeline.identify(data)

# 基于学习模型的重力补偿
def learning_based_compensation(dyn_model, qpos):
    return dyn_model.predict_gravity(qpos)

多物理场耦合补偿

未来的补偿系统将不仅考虑重力，还会融合流体阻力、电磁效应等多物理场因素，通过 plugin/扩展架构实现：

// 多物理场补偿插件示例
void ComputeMultiphysicsCompensation(const mjModel* m, mjData* d) {
    // 计算重力补偿
    mjtNum* grav_comp = d->qfrc_gravcomp;
    // 计算流体阻力补偿
    mjtNum* fluid_comp = ComputeFluidResistance(m, d);
    // 计算电磁效应补偿
    mjtNum* em_comp = ComputeEMEffects(m, d);
    // 合成总补偿
    for (int i = 0; i < m->nv; i++) {
        d->qfrc_passive[i] = grav_comp[i] + fluid_comp[i] + em_comp[i];
    }
}

实时硬件在环补偿

随着边缘计算能力的提升，重力补偿将在 FPGA 或专用 ASIC 上实现，通过 wasm/中的 WebAssembly 技术实现低延迟控制：

// WASM 重力补偿实现
#[wasm_bindgen]
pub fn compute_gravity_compensation(qpos: &[f64], mass: &[f64], inertia: &[f64]) -> Vec<f64> {
    let nv = qpos.len() / 7; // 假设每个关节 7 个参数
    let mut comp = vec![0.0; nv];
    // 高效计算补偿扭矩
    for i in 0..nv {
        comp[i] = compute_joint_gravity(qpos[i*7..(i+1)*7], mass[i], inertia[i*3..(i+1)*3]);
    }
    comp
}

总结：构建稳健的重力补偿系统

重力补偿作为机器人控制的基础技术，其实现质量直接决定了系统的控制精度和能源效率。通过 MuJoCo 提供的 qfrc_gravcomp 接口和底层动力学计算引擎，开发者可以构建从简单到复杂的各类补偿策略。从基础的 PD+ 重力补偿架构，到先进的自适应学习补偿系统，MuJoCo 的模块化设计为技术演进提供了灵活的扩展路径。

未来，随着机器人向更高精度、更高动态响应方向发展，重力补偿技术将与多物理场建模、智能学习算法深度融合，在 model/flex/等复杂模型上实现亚毫米级控制精度。掌握本文介绍的补偿原理与实现方法，将为机器人系统开发提供坚实的技术基础。