具身智能机器人实战教程——模块2（4）

前几集我们已经完成了众擎 T800 人形机器人的模型加载，初步认识了关节（joint）与连杆（link）的物理结构，以及相对坐标与绝对坐标的转换逻辑。然而，要让机器人真正"动"起来并具备感知能力，仅有几何结构是远远不够的。本集"T800 的状态参数"正是连接静态模型与动态控制的关键桥梁：在具身智能仿真开发中，如何准确获取并理解机器人的实时状态，是后续实现 PD 控制、强化学习策略训练以及 AI 观测空间构建的核心前提。

第一章：理解T800的状态参数

1.1机器人仿真中的"状态"

在众擎机器人参数讲解这个课程中，我们已经介绍了众擎机器人的相关参数，包括有多少个连杆，关节等等信息。这次我们更进一步，分析这些参数是如何使用的，如何用来描述机器人状态。

在机器人学与物理仿真中，"状态（State）"并非一个模糊概念，而是描述机器人在某一确切时刻运动情况的一组最小变量集合，可以把它理解为机器人的"瞬时快照"。在 MuJoCo 物理引擎中，这些状态量被高度结构化地封装在 mjData 对象里，并随着仿真主循环每一次 step 而实时更新。

可能有些抽象，我通过一个例子来解释。首先我们先获取T800的所有参数信息。

提示语：帮我编写一个脚本叫09_T800_nq_nv_nu_qpos_qvel_ctrl.py，打印机器人的关键参数，把他们的数量打印出来，初始的内容也打印出来。

从打印出来的信息中，我们得知nq的数量是32，对应的是qpos的长度；nv的数量是31，对应的是qvel的长度；nu的数量是25，对应的是ctrl的长度。qpos描述的是机器人姿态，用32个数字来表示；qvel描述的是机器人的速度，用31个数字来表示；ctrl描述的是机器人的控制信号数量，需要给25个电机发送指令。通过控制qpos，qvel，ctrl这三个数组的数字，将实现对机器人姿态的控制和速度的控制。所以qpos，qvel，ctrl这三个数组就是描述机器人状态的瞬时快照。后续的机器人控制算法的核心原理就是实时读取这三组数据并修改这三组数据来控制机器人行动。

通俗来说：ctrl就相当于游戏手柄，有25个按键，我们根据qpos和qvel数据实时调整电机数据，实现对机器人的控制

理解状态参数不仅是为了打印几个数值，它在整个具身智能开发链路中承上启下：首先，它是 PD 控制的误差来源，PD 控制器需实时读取当前关节角度与目标角度的差值，而"当前角度"正是从 qpos 中提取；其次，它是 AI 策略的观测输入，强化学习中智能体无法直接"看"到机器人内部结构，只能通过 qpos、qvel 等状态向量感知自身姿态并输出动作；最后，它是仿真主循环的更新对象，每一帧 mj_step 中，物理引擎正是基于当前状态量、受力情况与控制指令，通过积分计算出下一时刻的新状态。

第二章：Mujoco主循环

通过加载了T800 人形机器人，我们认识了它的 joint、link 与坐标系，也理清了 qpos、qvel 等状态参数的含义。模型是静止的，状态只是被"读"出来的快照。真正让机器人"动起来"的关键，是 MuJoCo 的仿真主循环（simulation main loop）。本章以mujoco 仿真主循环为主题，讲清楚主循环到底在循环什么、一次 step 内部发生了什么，以及如何把它写成可控、可调试的代码骨架。

2.1mujoco.mj_step接口介绍

物理引擎的本质是一个"状态推进器"：给定当前时刻系统的状态，再给定此刻施加的控制量，它就能算出下一时刻系统的状态。现实世界的物理是连续变化的，而计算机只能离散地一步步算。于是我们必须用一个循环，把连续时间切成一格一格的仿真步，在一格一格之间反复执行"读取控制 → 计算物理 → 更新状态"，这就是仿真主循环。

对具身智能而言，主循环还是感知、决策、执行三者交汇的节拍器。机器人策略（无论是 PD 控制器还是神经网络 AI 策略）都是在主循环的每一个节拍上，读取当前状态、输出下一拍的控制指令。理解了主循环，才能真正理解读取到的状态参数从哪里来、又要被送到哪里去

MuJoCo 中最常用的推进函数是 mj_step(model, data)。每调用一次，引擎就向前推进一个仿真步长（timestep）。它内部大致完成两件事：先用 mj_forward 根据当前状态与控制量计算出所有广义加速度，再用 mj_Euler 等积分器把加速度积分成新的 qvel 与 qpos，从而把状态推进到下一时刻。

2.2案例演示mujoco.mj_step接口

第一步，获取T800的模型并将其用viewer接口打印到屏幕上。这个内容在MuJoCo viewer 可视化 T800 机器人这个链接中已经存在。

第二步，在可视化代码的基础上进行仿真过程姿态打印操作。解释一下rpy，r表示机器人横滚，p表示俯仰，y表示偏航（原地转身）。

提示语：参考可视化T800机器人这个代码，基于这个代码帮我生成一个脚本叫10_T800_pos_rpy.py,打印出mujoco仿真过程，输出关键数据，包含机器人的pos，rpy。每隔100毫秒打印一次一共打印6秒钟。

"""
10_T800_pos_rpy.py
在 MuJoCo 仿真过程中打印机器人基座(body=LINK_BASE)的位置 (pos) 和姿态 (roll-pitch-yaw)
- 每 0.5 秒仿真时间打印一次
- 同时启动 viewer 可视化
运行: D:/miniconda3/envs/t800/python.exe 10_T800_pos_rpy.py
"""
import math
import time
import mujoco
import mujoco.viewer
import numpy as np
model = mujoco.MjModel.from_xml_path("t800.xml")
data = mujoco.MjData(model)
基座 body id (LINK_BASE)
BASE_ID = model.body("LINK_BASE").id
PRINT_INTERVAL = 0.5  # 打印间隔(仿真秒)
next_print = 0.0
def quat2rpy(quat):
"""四元数 (w, x, y, z) -> roll, pitch, yaw (rad), 使用 ZYX (内蕴) 约定"""
w, x, y, z = quat
# roll (x-axis rotation)
sinr = 2.0 * (w * x + y * z)
cosr = 1.0 - 2.0 * (x * x + y * y)
roll = math.atan2(sinr, cosr)
# pitch (y-axis rotation)
sinp = 2.0 * (w * y - z * x)
sinp = max(-1.0, min(1.0, sinp))
pitch = math.asin(sinp)
# yaw (z-axis rotation)
siny = 2.0 * (w * z + x * y)
cosy = 1.0 - 2.0 * (y * y + z * z)
yaw = math.atan2(siny, cosy)
return roll, pitch, yaw
def print_state():
pos = data.xpos[BASE_ID]  # 世界坐标 (x, y, z)
quat = data.xquat[BASE_ID]  # (w, x, y, z)
roll, pitch, yaw = quat2rpy(quat)
print(f"t={data.time:6.3f}s | "
f"pos=({pos[0]:7.4f},{pos[1]:7.4f},{pos[2]:7.4f}) | "
f"rpy=({roll:+7.4f},{pitch:+7.4f},{yaw:+7.4f}) rad "
f"({math.degrees(roll):+6.2f},{math.degrees(pitch):+6.2f},{math.degrees(yaw):+6.2f}) deg")
print("初始状态:")
mujoco.mj_forward(model, data)
print_state()
print("-" * 80)
print("开始仿真 (关闭 viewer 窗口退出)...")
print(f"{'时间':>8} {'位置 (x,y,z)':<32} {'姿态 (roll,pitch,yaw) rad':<32} {'姿态 (deg)':<24}")
print("-" * 80)
with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running() and data.time < 10.0:
mujoco.mj_step(model, data)
if data.time >= next_print:
print_state()
next_print += PRINT_INTERVAL
viewer.sync()
print("-" * 80)
print("仿真结束")
print_state()

执行代码，打印数据

没有通电的T800在重力的作用下瘫倒在地，从打印的数据中我们能看出pos的值，例如高度z一直在降低，rpy的值，一开始角度全为0，后面也在变化。直到趋近于某一个数静止。

第三章：PD控制

第二章我们拆开了 MuJoCo 的仿真主循环：在每一拍里，程序把控制量写入 mjData.ctrl，再调用 mj_step 让物理引擎把状态向前推进一小步。当时留了一个关键问题没有回答——写进 ctrl 的那个数字，究竟该怎么算，才能让关节从当前位置稳稳地停到我们想要的角度上？这正是本集「PD 控制」要解决的核心。可以说，PD 控制器是连接「AI 的意图」与「电机的力矩」之间的那座桥：无论上层的控制算法还是强化学习策略，最终都要落到一套 PD 参数上，才能把抽象的目标角度翻译成真实可执行的关节力矩。

3.1PD控制原理

当我们指挥机器人，最朴素的直觉是：想让关节停在某个角度，干脆直接把它定到那里。但这在物理仿真里行不通，因为它违背了动力学——机器人是一个有质量、有惯量、受重力与碰撞约束的系统，真实世界里电机能输出的也只是力矩，而不是"瞬移"指令。如果我们忽略动力学，强行改写位置，仿真结果既不符合物理规律，也无法迁移到真机。

正确的思路是"反馈控制"：不直接规定关节在哪，而是持续测量它现在在哪、离目标还差多少，再根据这个差距实时计算该施加多大的力矩，让关节像被一根有弹性的绳子牵引一样，自己收敛到目标。PD（Proportional-Integral 中去掉积分项的 Proportional-Derivative，比例—微分）控制就是这种思想最简单也最经典的一种实现。

PD 控制器把"该施加多大力矩"拆成了两项之和：一项负责消除位置误差，一项负责抑制运动过快带来的超调与振荡。用公式表达就是： $\tau = K_p \left(q_{\text{des}} - q\right) + K_d \left(\dot{q}_{\text{des}} - \dot{q}\right)$ 。其中 $q$ 是关节当前角度、 $q_{des}$ 是期望角度， $\dot{q}$ 是当前角速度、 $\dot{q}_{\text{des}}$ 是期望角速度， $K_p$ 、 $K_d$ 分别是比例增益与微分增益， $\tau$ 即最终写入 ctrl 的控制量。

比例项 $K_p \left(q_{\text{des}} - q\right)$ 只关心"误差有多大"：关节离目标越远，误差越大，输出的力矩就越大，于是关节被越用力地往目标方向拉；一旦到达目标，误差归零，这一项也归零。它好比一根理想弹簧——形变量越大，回弹力越强。但只有 P 项会带来一个副作用：关节冲过目标后误差反向，力矩又把它拉回来，如此往复形成振荡，始终在目标附近来回摆动而难以静止。

(二)微分项 D：给运动加上阻尼

微分项 $K_d \left(\dot{q}_{\text{des}} - \dot{q}\right)$ 关心的不是位置，而是"运动得有多快"，本质上起到阻尼（刹车）的作用。当关节高速冲向目标时，这一项产生一个反向的力矩，提前给它减速，从而压制掉纯 P 控制带来的超调与振荡，让关节平稳地"贴"到目标上。D 项与 P 项一柔一刚配合：P 负责"到位"，D 负责"稳住"。对于像 T800 这种人形机器人，腿部、髋部等关节惯量大、容易晃，合适的 D 项几乎是稳定站立与行走的前提。

3.2案例演示PD控制

提示语：帮我写一个脚本叫02_T800_pd.py，我需要使用pd控制器，让T800机器人的关节都保持初始的位置。

"""
11_T800_pd.py
使用 PD 控制器让 T800 机器人的所有关节保持初始位置
- 25 个电机均为 motor 类型 (gear=1), ctrl 即力矩 (Nm)
- PD 公式: tau = Kp * (q_des - q) + Kd * (dq_des - dq)
- 初始关节角度全为 0, 即目标为保持初始姿态
- 每 0.5 秒打印一次各关节的偏差最大值
运行: D:/miniconda3/envs/t800/python.exe 11_T800_pd.py
"""
import numpy as np
import mujoco
import mujoco.viewer
model = mujoco.MjModel.from_xml_path("t800.xml")
data = mujoco.MjData(model)
nu = model.nu  # 25
收集每个 actuator 对应的关节 qpos/qvel 地址
qposadr = np.zeros(nu, dtype=int)
dofadr = np.zeros(nu, dtype=int)
for i in range(nu):
jid = int(model.actuator_trnid[i, 0])  # 关节 id
qposadr[i] = int(model.jnt_qposadr[jid])
dofadr[i] = int(model.jnt_dofadr[jid])
目标位置 = 初始位置 (全 0)
q_des = np.zeros(nu)
dq_des = np.zeros(nu)
PD 增益
Kp = np.full(nu, 150.0)   # 比例增益
Kd = np.full(nu, 5.0)     # 微分增益
力矩限幅 (从模型读取 ctrlrange)
ctrl_range = model.actuator_ctrlrange.copy()  # shape (nu, 2)
PRINT_INTERVAL = 0.5
next_print = 0.0
mujoco.mj_forward(model, data)
print(f"PD 控制器: Kp={Kp[0]}, Kd={Kd[0]}, 关节数={nu}")
print(f"目标: 保持初始关节位置 (全 0)")
print("-" * 60)
with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running() and data.time < 10.0:
# 读取当前关节位置和速度
q = data.qpos[qposadr]
dq = data.qvel[dofadr]
    # PD 控制
    tau = Kp * (q_des - q) + Kd * (dq_des - dq)

    # 力矩限幅
    tau = np.clip(tau, ctrl_range[:, 0], ctrl_range[:, 1])

    # 写入控制
    data.ctrl[:] = tau

    mujoco.mj_step(model, data)

    if data.time &gt;= next_print:
        err = np.abs(q - q_des)
        max_err = err.max()
        max_idx = err.argmax()
        jname = mujoco.mj_id2name(
            model, mujoco.mjtObj.mjOBJ_JOINT,
            int(model.actuator_trnid[max_idx, 0])
        )
        print(f"t={data.time:5.2f}s | max_err={max_err:.4f} rad "
              f"({np.degrees(max_err):.2f} deg) @ {jname} | "
              f"tau_max={np.abs(tau).max():.1f} Nm")
        next_print += PRINT_INTERVAL

    viewer.sync()
print("-" * 60)
print("仿真结束")
q = data.qpos[qposadr]
err = np.abs(q - q_des)
print(f"最终最大关节偏差: {err.max():.4f} rad ({np.degrees(err.max()):.2f} deg)")

运行效果如下：

机器人会直挺挺的倒下去，不会像之前那样瘫倒下去。要控制机器人站立，不仅是要pd控制，还需要考虑重心，摩檫力，全身关节还要进行动态平衡。大小脑协调才能保持机器人战力

总结

本集围绕众擎 T800 人形机器人的仿真开发，沿着「状态参数 → 仿真主循环 → PD 控制」这条主线，完成了从静态模型到动态控制的关键跨越。

第一章我们理清了机器人仿真中的「状态」概念：在 MuJoCo 中，状态被封装在 mjData 对象里，核心是 qpos（32 个数字描述姿态）、qvel（31 个数字描述速度）和 ctrl（25 个电机控制信号）三组数组。它们共同构成机器人的「瞬时快照」，也是后续 PD 控制的误差来源、AI 策略的观测输入以及仿真主循环的更新对象。

第二章拆开了 MuJoCo 的仿真主循环：物理引擎本质是一个「状态推进器」，通过反复执行「读取控制 → 计算物理 → 更新状态」把连续时间切成离散的仿真步。核心接口 mj_step(model, data) 每调用一次就推进一个 timestep，内部先用 mj_forward 计算广义加速度，再用积分器更新 qvel 与 qpos。通过 10_T800_pos_rpy.py 案例，我们看到未通电的 T800 在重力作用下瘫倒，pos 与 rpy 数据随之变化直至静止。

第三章引入 PD 控制来解决「写进 ctrl 的数字该怎么算」这一核心问题。PD 控制器把力矩拆成两项：比例项 Kp 负责消除位置误差、像弹簧一样把关节拉向目标；微分项 Kd 负责给运动加阻尼、抑制超调与振荡。二者一柔一刚配合，P 负责「到位」、D 负责「稳住」。通过 11_T800_pd.py 案例，机器人能直挺挺地倒下而非瘫倒，说明 PD 已能维持关节位置，但要真正站立还需考虑重心、摩擦力与全身动态平衡。

总的来说，理解状态参数是感知的基础，仿真主循环是驱动的节拍器，而 PD 控制则是把「AI 的意图」翻译成「电机力矩」的桥梁。这三者环环相扣，构成了具身智能仿真开发的核心链路，也为后续强化学习策略训练与 AI 观测空间构建打下了基础。