技术方案

1. 系统定位

本项目采用 GPT-6 / Codex 辅助构建与迭代的分层机器人控制系统。自然语言描述提供任务顺序和动作要求；GPT 根据实验反馈设计控制逻辑、调整参数并组织仿真验证。运动执行层读取 MuJoCo 状态，通过状态机、预训练 locomotion policy、inverse kinematics（IK）、接触力分配和关节控制产生电机输入，由物理仿真计算运动与接触。

GPT 在开发与实验迭代中参与决策，当前完整表演运行时的动作选择由本地程序执行。系统结合预训练行走策略与显式控制程序，读取仿真状态真值生成动作，并通过视频展示与实验检查评估运动结果。

舞台模型包含 29 个身体转动关节和两个滑台夹爪，并保留机器人浮动基座与支架的自由运动。

2. 总体 pipeline

当前表演的输入输出链如下：

1. 任务输入：自然语言动作顺序、机器人模型、舞台与器材配置、已有实验轨迹。
2. Codex 开发与实验决策：设计控制逻辑与参数，启动仿真试验，根据运动轨迹、接触指标与画面反馈修正方案。
3. 场景初始化：建立机器人与支架的动力学模型，设置惯量、关节限制、控制参数、预训练策略和参考姿态。
4. 状态读取与阶段选择：读取关节、机身、支架位置与接触，判定当前阶段是否完成，选择下一阶段。
5. 运动参考生成：行走阶段输出腿部关节目标；拾取、支撑、俯卧撑等阶段输出全身或手臂目标；必要时估计接触支撑力。
6. 电机控制：根据目标与实际关节状态的偏差计算力矩或夹爪驱动力，并施加限幅。
7. 物理积分：MuJoCo 在接触、摩擦和惯性作用下推进状态，下一轮控制继续读取实际结果。
8. 记录、审计与可视化：保存状态和事件，计算是否满足动作要求，再用保存的状态渲染视频。

运动执行闭环由第 4–7 步构成：控制器根据当前状态生成电机输入，物理仿真推进状态，再将结果反馈给控制器。第 8 步记录运动结果并生成可视化，用于实验评估与下一轮开发。

3. 基本量与时钟

3.1 自由度、位置和速度

DoF（degree of freedom，自由度）表示一个可以独立运动的方向。转动关节以 rad 表示角度，滑台关节以 m 表示位移。浮动基座表示机器人身体可以整体平移和转动，没有被固定在世界上。

舞台模型具有以下维度：

q 为广义位置向量，包含机身和支架的位姿、身体关节角度及夹爪位移；v 为广义速度向量，包含相应的线速度、角速度和关节速度；u 为执行器输入向量，包含 29 个身体电机力矩和 2 个夹爪直线驱动力。u 的前 29 项单位为 N·m，最后两项单位为 N。

机身和支架的转动采用四元数表示，分量顺序为 w、x、y、z，范数为 1。身体关节与夹爪的状态由广义状态向量中相应的分量提取。

定义身体关节角度 θ、身体关节速度 θ̇、夹爪位移 g 与夹爪速度 ġ：

θ 和 θ̇ 分别表示从 q、v 中提取的身体关节角度与角速度；g 和 ġ 分别表示两只夹爪的位移与速度。

3.2 三种时间

仿真时间是物理世界中动作经过的时间；墙钟时间是计算机实际花费的时间；视频时间由帧数和帧率决定。三者不能混为一谈。

这里，T 的三个下标分别表示仿真时长、视频时长和墙钟时长，单位均为 s；N_phys 是实际积分步数，为整数；Δt_phys 是每步仿真时间；N_frame 是视频帧数；f_video 是帧率，单位为帧/s；ρ 是无量纲仿真速度比。ρ 大于 1 表示计算快于被模拟的时间流逝。

本项目舞台仿真的 Δt_phys 为 0.0001 s，即 10,000 Hz。频率属于仿真时钟，并不表示这台计算机可以实时控制真机。

4. GPT 层的输入与输出

开发闭环

用概念表达描述第 i 次开发决策：

L 为自然语言任务描述；P_i 为第 i 轮的控制程序；η_i 为该轮的控制参数集合，包含标量与数组，其维数随控制模块变化；E_i 为实验证据，包含状态轨迹、接触指标与图像。G 表示 GPT 对任务、控制方案和实验反馈的综合分析过程；c_i 表示启动下一轮实验的执行指令。下标 i 按开发迭代计数。

输出程序和命令启动本地仿真，得到下一批实验证据：

Run 表示执行程序，输出状态序列和日志；A 表示审计与渲染流程，将这些记录组织为可检查的证据。

例如，拾取前的接近动作需要同时满足位置、朝向、承重与速度条件。开发迭代依据停止误差和稳定性反馈调整这些条件，运动执行层在条件成立后转入站立恢复阶段。

5. 状态读取与状态机

5.1 状态真值

运行时控制输入可以写为：

k 表示一次外层控制更新；q_k、v_k 的维度见第 3 节；B 为需要观测的刚体或几何点数量，X_k 存储它们的世界位置；C 为当前接触点数量，F_k 存储世界坐标中的三维接触力。B、C 随场景与接触状态变化。控制器还读取各刚体的 3×3 旋转矩阵，并将接触力汇总为左右手与左右脚的承重量。

状态输入直接来自物理仿真，包括刚体位置、朝向、关节状态和接触力。控制器利用这些状态计算目标误差、判断支撑条件，并决定阶段转换。

5.2 阶段转换

s_k 是离散动作阶段，例如接近支架或闭合夹爪；t_k 是当前仿真时刻；t_s 是本阶段开始时刻；b 是返回 0 或 1 的完成条件函数；Next 表示动作序列中的下一阶段。部分阶段要求完成条件连续满足一段时间，以减少瞬时接触波动引起的提前切换；超时和失稳条件用于终止未完成的动作。

例如优化版退出接近阶段时，双足平均位置与目标的距离小于 0.04 m、双脚前后差小于 0.04 m、朝向误差小于 0.10 rad、双足各承重超过 80 N、基座速度小于 0.12 m/s；滤波转动速度小于 0.15 rad/s，并保留 0.4 rad/s 的瞬时上限。进入站立恢复后还需满足进一步稳定条件，才开始下蹲。

6. 行走分支：目标位置到 12 个腿部目标

6.1 位置误差到速度命令

p★∈R² 为目标位置；p̄_k∈R² 为导航参考位置，登台阶段使用基座位置，接近支架阶段使用双足中心的平均位置；δ_k∈R² 为位置误差；ψ_k 为 yaw 朝向标量；R_z(−ψ_k)∈R²ˣ² 将世界平面误差转到机身前后、左右方向；c_des,xy∈R² 为期望平面速度，单位 m/s。clip 表示逐元素截断；ℓ_c、u_c∈R² 分别为速度命令下限和上限。接近支架阶段为 (−0.15, −0.10) 与 (0.20, 0.10) m/s，登台前向上限为 0.35 m/s。

再加入朝向控制，形成包含前向速度、侧向速度和转动速度的三维命令。控制器对实际速度低通滤波，使用比例与积分修正得到最终命令 c_k∈R³：前两维单位 m/s，第三维单位 rad/s。滤波与修正的核心形式为：

w_k∈R³ 是实际机身平面速度和转动速度；w̄_k∈R³ 是滤波值；e_k∈R³ 是速度误差；I_k∈R³ 是积分修正状态，0.02 对应策略更新周期。命令主体采用期望值加 0.8 倍误差和积分项，随后限幅；yaw 通道另外使用朝向误差与转动阻尼。这些增益含单位换算意义，属于固定工程参数。

6.2 47 维观测与预训练策略

concat 表示首尾拼接；维数为 3+3+3+12+12+12+1+1=47。策略输出为：

π_ϑ 是 Unitree 预训练行走策略，ϑ 表示固定的网络参数；a_k∈R¹² 是无量纲动作输出；h_k 与 m_k 分别为 LSTM hidden state 和 cell state，形状均为 1×1×64。θ★,leg∈R¹² 是腿部关节角度目标，单位 rad。行走策略以 50 Hz 更新，更新之间保持最近一次目标。接近支架时，对落后的摆动腿施加有限前伸修正，并对踝关节目标限幅。

行走策略将本体状态与速度命令映射为 12 个腿部关节角增量，再由关节控制器执行目标跟踪。

7. 操作分支：姿态、IK 与接触支撑

7.1 参考姿态与平滑过渡

θ_A、θ_B∈R²⁹ 是两组身体关节目标；t、t₀、T 是标量秒数，分别为当前时间、过渡开始时间与过渡时长，T>0；r 和 α 是无量纲标量；θ★(t)∈R²⁹ 是插值目标。该式采用 cosine easing 构造平滑过渡，部分动作也沿多个离散 IK 节点插值。插值结果作为关节控制目标，机器人实际状态由电机驱动和物理积分产生。

参考姿态由预先求解的 IK 轨迹、当前场景中的在线几何求解及预定义动作曲线共同构成。搬运下蹲沿已验证的身体姿态参考执行，同时根据支架实际位置修正手臂目标；持物起身依据足端和手掌的实际位置重新求解。逐腿转换采用经过动力学试验验证的运动参考，其适用性取决于场景几何和起始状态。

秀肌肉动作在双臂抬至峰值后向保持姿态平滑过渡。末段下顿将这两组双臂目标之间的差值放大至 1.6 倍，过渡时长为 0.3 s；腿部稳定与跳跃参考由相应控制分支生成。

7.2 手臂 IK

手臂 IK 根据目标手掌位置与朝向求解肩、肘、腕的关节角度。每侧手臂具有 7 个关节，采用以下带关节限位的加权最小二乘形式：

x∈R⁷ 为待求的一侧手臂关节角；x_min、x_max∈R⁷ 为保留余量的关节限位；x₀∈R⁷ 为初始参考；p(x)、p★∈R³ 为实际模型计算出的手掌位置和目标位置；R(x)、R★∈R³ˣ³ 为末端朝向矩阵；vec 将矩阵展平为 9 维向量。拼接后的残差维数为 3+9+7=19，二范数平方为标量代价。前两项追踪位置与朝向，最后一项减少无必要的关节改动；80、2、0.01 是固定权重，不代表概率。

有界非线性最小二乘求解得到最优关节角 x★。求解过程中通过正运动学评估候选姿态，得到的手臂目标交由电机控制器跟踪。站立与逐腿转换采用更大规模的 IK，进一步考虑足端位置、质心、身体俯仰、关节范围和碰撞距离；不同阶段依据支撑条件构造相应的残差项。

7.3 波比跳的接触力分配

波比跳控制选择机身 6 个速度自由度与身体 29 个关节，形成 35 维活动子空间。设当前选定接触点数为 n_c，每点用三维力表示：

f 为各接触点期望力的拼接，单位 N；J 为接触点速度对活动速度的 Jacobian；J_b 是 J 的浮动基座六列。这里的 n_c 随支撑脚、脚跟抬起状态和是否握杆改变。由姿态误差和速度反馈构造机身六维目标 wrench：

w_b∈R⁶ 为基座广义力需求，包含平移与转动分量；b_b∈R⁶ 来自偏置广义力减去被动力的基座部分；M_bb∈R⁶ˣ⁶ 为质量矩阵的基座子块；a_b★∈R⁶ 为基座位置、姿态误差与速度反馈构造的目标加速度。该式对应实现中的近似基座 wrench 匹配，不是完整全身逆动力学等式约束优化。

P(f) 是切向摩擦违规的二次惩罚项，输出非负标量。采用最多 4 轮有界最小二乘逐步加入违反摩擦条件的惩罚项，每行权重为 100；单点世界竖直力范围为 0–500 N，两个水平分量范围为 −200–200 N，并对超过 0.6 倍竖直力的水平分量施加惩罚。竖直力非负和分量范围为硬边界，摩擦条件属于软惩罚，不能声称所有摩擦约束都得到严格满足。

关节支撑前馈为：

b_j∈R²⁹ 为身体关节的偏置力减去被动力；J_j∈R^(3n_c×29) 为 J 的关节列。移动脚还根据三维足端位置和速度误差，经过足端 Jacobian 转换成对应腿部的力矩修正。求得的 f★只用于计算电机力矩；实际手脚接触力由 MuJoCo 计算，没有直接把 f★作为外部托举力加到身体上。

搬运阶段采用重力补偿与支撑 Jacobian 前馈；波比跳的承重转换采用接触力分配；跳跃阶段采用屈膝、摆臂、蹬伸、落地参考与平衡反馈。不同动作阶段按受力特点选择控制形式。

8. 电机控制与物理输出

身体关节的主要控制形式为：

τ、τ_ff、τ_max∈R²⁹，分别是请求力矩、前馈力矩和关节力矩上限，单位 N·m；θ★、θ∈R²⁹ 为目标与实际角度；θ̇★、θ̇∈R²⁹ 为目标与实际角速度；K_p、K_d∈R²⁹ˣ²⁹ 为对角增益矩阵。PD 可以理解为“位置偏差产生拉回作用，速度项减少振荡”。不同阶段使用不同增益；行走的 12 个腿部力矩由策略目标对应的 PD 分支覆盖，不能把非行走前馈原样套到它们上面。

两个夹爪的驱动力为：

u_g 是两只夹爪的直线驱动力，单位 N；g★∈R² 为夹爪目标，g 与 ġ 见第 3 节。常用张开目标为 0.06 m，闭合目标为 −0.015 m；这是控制参考，实际位移由关节限制和接触共同决定。夹爪与支架通过碰撞几何及摩擦接触，没有抓住后焊接绑定器材。

将身体力矩与夹爪力组成 u 后，MuJoCo 推进实际物理状态。概念形式为：

M∈R⁴⁹ˣ⁴⁹ 是质量矩阵；v̇∈R⁴⁹ 为广义加速度；b∈R⁴⁹ 为重力、科里奥利力等偏置项；B∈R⁴⁹ˣ³¹ 为执行器到广义力的映射；J_c∈R^(r_c×49) 为当前接触约束 Jacobian；λ∈R^(r_c) 为接触约束力，r_c 为求解器约束维数，随接触变化；f_passive∈R⁴⁹ 为被动力。该式是动力学概念说明，实际积分和摩擦接触求解由 MuJoCo 执行。

ξ_n 表示完整积分状态，除了 q、v，还可包含求解器 warmstart 等内部状态，其组成由动力学模型与积分器确定；n 为物理积分步编号；Φ_Δt 表示一个物理步。q 的四元数需要按转动规律积分，不能把 49 维 v 直接与 52 维 q 相加。

外层控制周期内仍在每个物理步重新计算 PD 误差和限幅电机输入；外层目标与部分前馈保持不变。因此“外层 500 Hz”和“PD 计算 10 kHz”可以同时成立。

9. 轨迹记录与可视化

9.1 轨迹与审计

N 是保存的采样数；Q、V、U 的每一行分别对应某个采样时刻的广义位置、广义速度和执行器输入；t 为相应的仿真时间向量。记录还包括初始状态、目标轨迹、动作阶段、接触力和速度，用于评价跟踪误差、支撑稳定性及阶段完成情况。

轨迹通常以 50 Hz 采样保存，部分约束指标在控制循环中累计。离散采样结果用于运动分析，控制循环内的累计检查用于补充采样间隔内的约束信息；两类结果按各自时间分辨率解释。

状态恢复需要区分运动状态初始化与完整积分状态恢复。前者以保存的位置、速度和执行器输入初始化续段；后者还包含求解器 warmstart 等内部状态，用于复现中断前的积分过程。仅保存运动状态时，恢复边界的数值连续性需要独立检查。视频片段的交叉淡化属于画面编辑，其视觉平滑性不能证明动力学连续性。

9.2 第一视角

第一视角采用附着于机器人躯干、位于头部前方的虚拟相机。头部与躯干保持固定相对关系，相机世界位姿由躯干位姿与固定安装变换共同确定：

W 表示世界坐标系，B 表示躯干坐标系，C 表示相机坐标系；p_C^W、p_B^W、p_C^B 均为三维位置向量；R_B^W、R_C^B、R_C^W 均为 3×3 旋转矩阵，上标为表达坐标系，下标为被表达的坐标系。p_C^B 固定为 (0.095, 0, 0.40) m，位于头部前方。展示版光轴朝机身前方并向下倾斜 25°，垂直视场角 90°；另保留水平朝前的 75° 视场试验记录。

这些参数是虚拟观察位，不是硬件标定参数。下倾版本便于看到近处双手和器材；随着机身俯仰，画面也会转动，没有额外头部追踪或相机稳定控制。图像输出为 480×848×3 的 RGB 数组，再编码为 15 fps 视频。

虚拟相机输出透视 RGB 图像，用于观察手部、器材与身体运动。相机参数描述仿真中的观察位置与视场，图像不包含真实传感器的噪声与畸变。

10. 控制频率