3. 机器人仿真环境

各位同学，欢迎来到第三章。前两章我们聊了强化学习的整体图景，今天要落地了——咱们得让机械臂真的动起来。

你可能会问：强化学习不是直接学策略吗？为什么还要先搞力矩控制？

我个人的经验是：一个连位置都稳不住的机械臂，学出来的策略全是空中楼阁。我在做第一个机器人项目时就吃过这个亏——策略网络在仿真里跑得飞起，一上真机就抖成筛子。后来才发现，底层的力矩控制根本没调好。

所以今天，咱们先把地基打牢。

什么是力矩控制？

说白了，力矩控制就是告诉电机：「你出多大力」。不是告诉它「你转到哪个角度」，而是直接控制力的大小。

想象一下你用手臂托着一杯水：

• 位置控制：你命令手臂「停在30度位置」——不管杯子多重，硬顶到那个角度。
• 力矩控制：你命令手臂「输出2牛米的力」——杯子轻就抬得快，杯子重就抬得慢，但力是恒定的。

在机器人学里，力矩控制的数学表达很简单：

τ = M(q)q̈ + C(q, q̇)q̇ + g(q) + τ_ext

其中：

• τ：关节力矩（我们要控制的量）
• M(q)：惯性矩阵
• C(q, q̇)：科里奥利力和离心力
• g(q)：重力项
• τ_ext：外部力矩（比如碰撞）

嗯，这个公式你不需要背，但要知道：力矩控制本质上是「力」的闭环，不是「位置」的闭环。

PD控制器：最简单也最实用的力矩控制

PD控制器，比例-微分控制，是力矩控制里最经典的方法。为什么先学它？因为：

• 它不需要系统的动力学模型（省事）
• 调两个参数就能工作（直观）
• 它是很多高级控制器的基础（重要）

PD控制器的公式长这样：

τ = K_p * (q_d - q) + K_d * (q̇_d - q̇)

拆开来看：

• K_p (比例增益)：位置误差越大，输出的力矩越大。相当于一个弹簧，把机械臂往目标位置拉。
• K_d (微分增益)：速度误差越大，输出的阻尼力越大。相当于一个减震器，防止机械臂冲过头。
• q_d, q̇_d：目标位置和目标速度
• q, q̇：实际位置和实际速度

为什么会这样设计？你想想看：

如果只有比例项（P控制），机械臂靠近目标时会因为惯性冲过去，然后在目标附近来回震荡。加上微分项（D控制）后，它检测到「速度太快了」，就会提前刹车。

我在项目中遇到过最典型的场景：调一个六轴机械臂的抓取动作。P给大了，末端抖得像帕金森；D给大了，动作慢得像树懒。最后花了整整一个下午才找到平衡点。

在MuJoCo中实现PD控制器

好，理论讲完了，咱们动手。MuJoCo里实现PD控制器其实很简单——它甚至内置了相关接口。

先看一个完整的例子。假设我们有一个单关节机械臂，目标位置是1.0弧度：

import mujoco
import numpy as np

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

# PD参数
Kp = 100.0
Kd = 10.0
target_pos = 1.0  # 目标位置（弧度）
target_vel = 0.0  # 目标速度

# 仿真循环
for i in range(1000):
    # 获取当前状态
    q = data.qpos[0]      # 当前位置
    q_dot = data.qvel[0]  # 当前速度
    
    # 计算PD控制力矩
    error_pos = target_pos - q
    error_vel = target_vel - q_dot
    tau = Kp * error_pos + Kd * error_vel
    
    # 施加力矩
    data.ctrl[0] = tau
    
    # 步进仿真
    mujoco.mj_step(model, data)
    
    # 打印状态
    if i % 100 == 0:
        print(f"Step {i}: pos={q:.3f}, vel={q_dot:.3f}, tau={tau:.3f}")

这段代码的核心逻辑就三步：

1. 读状态：从data.qpos和data.qvel拿到当前位置和速度
2. 算力矩：用PD公式算出需要的力矩
3. 发指令：把力矩写入data.ctrl，让MuJoCo执行

运行后你会看到，机械臂从初始位置（假设是0）快速逼近目标位置1.0，最终稳定在目标附近，误差在0.01弧度以内。

多关节PD控制

单关节会了，多关节其实一样——对每个关节独立算PD就行。来看一个六轴机械臂的例子：

# 六轴机械臂PD控制
Kp = np.array([100, 100, 80, 50, 30, 20])
Kd = np.array([10, 10, 8, 5, 3, 2])
target_pos = np.array([0.5, -0.3, 1.2, 0.0, 0.8, 0.0])

for i in range(2000):
    # 当前状态
    q = data.qpos[:6].copy()
    q_dot = data.qvel[:6].copy()
    
    # 计算每个关节的力矩
    error_pos = target_pos - q
    error_vel = -q_dot  # 目标速度=0
    tau = Kp * error_pos + Kd * error_vel
    
    # 施加力矩
    data.ctrl[:6] = tau
    
    mujoco.mj_step(model, data)

注意这里每个关节的Kp和Kd不一样。为什么？因为：

• 靠近基座的关节（关节1、2）承受的负载大，需要更大的增益
• 靠近末端的关节（关节5、6）负载小，增益可以小一些
• 每个关节的惯性、摩擦力都不同

我个人的习惯是：先让所有关节用同一组参数跑起来，然后逐个关节微调。这样能快速定位问题关节。

PD控制器的局限性

PD控制器虽然好用，但不是万能的。来看一个对比表格：

场景	PD控制器表现	原因
静止位置保持	优秀（稳态误差小）	重力被比例项补偿
低速轨迹跟踪	良好	微分项能跟上速度变化
高速轨迹跟踪	较差（有滞后）	没有前馈项，响应慢
变负载场景	一般（需要重新调参）	固定增益无法适应变化
外力扰动	中等（会反弹但恢复慢）	没有积分项消除稳态误差

你看，PD控制器在「静态保持」和「低速运动」场景下表现很好，但一旦涉及高速、变负载，就力不从心了。这也是为什么后面我们要学更高级的控制方法——比如计算力矩控制、阻抗控制，甚至用强化学习来学控制器。

本章知识结构

为了帮你理清思路，我画了一张图：

这张图把本章的核心脉络串起来了：从力矩控制的定义出发，到PD控制器的原理和参数，再到MuJoCo中的具体实现，最后指出局限性——为后续章节的进阶内容埋下伏笔。

小结

今天的内容就到这里。总结三个你必须记住的点：

1. 力矩控制是「力」的闭环，不是「位置」的闭环——这是理解所有机器人控制的基础
2. PD控制器 = 弹簧 + 阻尼，Kp决定刚度，Kd决定阻尼，两个参数配合好就能稳住位置
3. MuJoCo里实现PD控制器就三步：读状态、算力矩、发指令——代码不超过10行

嗯，说实话，PD控制器看起来简单，但真正调好需要耐心。我当年第一次调六轴机械臂时，光调一个关节就花了两个小时。别急，多调几次就有手感了。