📚 课程目录第3章仿真循环与控制

3. 仿真循环与控制：理解仿真步进机制，使用 Python 绑定控制关节，实现一个简单的 PD 控制器

好，咱们进入第三章。前两章我们把 MuJoCo 的环境搭好了，也学会了怎么加载模型、查看数据。但仿真器真正干活的地方，是那个循环——你推它一步，它算一步，你再推，它再算。今天我们就来拆开这个循环，看看里面到底发生了什么。

3.1 仿真步进机制：MuJoCo 的“心跳”

MuJoCo 的仿真不是连续的，它是离散的。你调用一次 mj_step，它就往前跳一个固定时间片。这个时间片叫 dt，默认是 0.002 秒，也就是 2 毫秒。

我个人习惯把 dt 理解成仿真器的“心跳间隔”。每次心跳，MuJoCo 会干三件事：

计算动力学：根据当前状态（位置、速度）和外力，算出加速度。
数值积分：用算出的加速度更新位置和速度。
检测碰撞：检查有没有新的接触，更新接触力。

这三步一气呵成，你完全不用操心中间细节。但有一点要记住：你必须在每次步进之前设置好控制量，否则 MuJoCo 会用上一帧的控制量继续算。

核心概念：mj_step 是 MuJoCo 的“单步执行”指令。你给它一个状态，它给你下一个状态。就这么简单。

我在项目中遇到过一个问题：仿真跑着跑着，机器人突然飞了。排查了半天，发现是 dt 设得太大，积分不稳定。你想想看，2 毫秒的步长对于大多数机械臂来说没问题，但如果你在跑高速碰撞或者柔性体，步长就得缩小到 0.5 毫秒甚至更小。

参数	默认值	说明
`dt`	0.002	仿真步长时间（秒）
`nsubsteps`	1	每帧内部子步数
`timestep`	0.002	等效于 `dt`

小技巧：如果你需要更高的控制频率，可以保持 dt 不变，增大 nsubsteps。比如 nsubsteps=2 时，MuJoCo 内部会走两步，但外部你只调用一次 mj_step。这样控制频率不变，但仿真精度更高。

3.2 Python 绑定：如何“拧”关节

好了，理论说完了，咱们上手。MuJoCo 的 Python 绑定提供了非常直观的接口来控制关节。核心就两个数组：

data.ctrl：控制量数组，长度等于模型中的 nu（控制自由度）。
data.qpos：关节位置数组，长度等于 nq。
data.qvel：关节速度数组，长度等于 nv。

你直接给 data.ctrl 赋值，然后调用 mj_step，MuJoCo 就会把这些控制量当成关节力矩（或者位置目标，取决于你模型里怎么配的）去算。

举个例子，假设我们有一个单关节摆锤模型：

import mujoco
import numpy as np

# 加载模型
model = mujoco.MjModel.from_xml_path('pendulum.xml')
data = mujoco.MjData(model)

# 设置控制量：给关节一个 1.0 Nm 的力矩
data.ctrl[0] = 1.0

# 步进仿真
mujoco.mj_step(model, data)

# 看看关节位置变了没
print(data.qpos[0])  # 输出新的角度

嗯，这里要注意：data.ctrl 的索引和模型中的关节顺序是对应的。如果你有多个关节，一定要搞清楚哪个索引对应哪个关节。我刚开始用的时候经常搞混，后来养成了一个习惯——每次加载模型后先打印 model.actuator_names 看一眼。

避坑指南：我曾经在调试六轴机械臂时，把第三个关节和第四个关节的控制量写反了。结果机械臂扭成了麻花。后来我加了一行断言：assert len(model.actuator_names) == len(data.ctrl)，确保数组长度匹配。

3.3 实现一个简单的 PD 控制器

光给固定力矩没意思，咱们得让关节动到我们想要的位置。这就用到了 PD 控制器。

PD 控制器的公式很简单：

力矩 = Kp × (目标位置 - 当前位置) - Kd × 当前速度

说白了，就是位置误差越大，你给的力越大；速度越快，你给的阻尼越大。这样既能快速到达目标，又不会冲过头。

下面是一个完整的 PD 控制循环：

import mujoco
import numpy as np
import time

# 加载模型
model = mujoco.MjModel.from_xml_path('pendulum.xml')
data = mujoco.MjData(model)

# PD 参数
Kp = 100.0   # 比例增益
Kd = 10.0    # 微分增益

# 目标位置：垂直向上（0 弧度）
target_pos = 0.0

# 仿真 1000 步
for i in range(1000):
    # 计算当前误差
    error = target_pos - data.qpos[0]
    
    # PD 控制律
    torque = Kp * error - Kd * data.qvel[0]
    
    # 施加控制
    data.ctrl[0] = torque
    
    # 步进
    mujoco.mj_step(model, data)
    
    # 每 100 步打印一次状态
    if i % 100 == 0:
        print(f"Step {i}: pos={data.qpos[0]:.3f}, vel={data.qvel[0]:.3f}")

你运行一下就会发现，摆锤从初始位置（比如水平方向）开始，慢慢摆向垂直向上，最后稳定在目标位置附近。这就是 PD 控制器的效果。

关键点：PD 控制器的两个参数 Kp 和 Kd 需要调优。Kp 太大，系统会震荡；Kd 太大，系统会反应迟钝。我一般先调 Kp 让系统能到达目标，再加 Kd 消除震荡。

3.4 知识结构图：仿真循环与控制流程

下面这张图把整个流程串起来了。你可以看到，从初始化到步进，再到控制更新，是一个闭环。

3.5 调参实战：Kp 和 Kd 怎么配？

调 PD 参数是个手艺活。我分享一个我常用的方法：

先调 Kp：把 Kd 设成 0，逐渐增大 Kp，直到系统出现轻微震荡。记下这个临界值。
再加 Kd：把 Kp 设为临界值的 60%-80%，然后加 Kd，直到震荡消失。
微调：观察稳态误差和响应速度，微调两个参数。

举个例子，对于上面那个摆锤模型，我试出来的参数是 Kp=80，Kd=8。系统能在 0.5 秒内到达目标，稳态误差小于 0.01 弧度。

Kp	Kd	响应速度	稳态误差	震荡情况
50	0	慢	大	无
100	0	快	小	持续震荡
80	5	较快	小	轻微
80	10	适中	很小	无

经验之谈：如果你发现系统怎么调都稳不住，先检查一下 dt 是不是太大了。我遇到过好几次，调了半天参数，最后发现是步长问题。把 dt 从 0.005 降到 0.002，问题就解决了。

3.6 小结

这一章我们干了三件事：理解了 mj_step 的步进机制，学会了用 Python 绑定控制关节，最后亲手实现了一个 PD 控制器。这些东西是 MuJoCo 仿真的基本功，后面所有的高级控制——比如逆动力学、MPC、强化学习——都建立在这个循环之上。

你可以在自己的模型上试试不同的 Kp 和 Kd，看看响应曲线怎么变。动手调一调，比看十遍理论都管用。