强化学习 Policy 的 Tracking 能力全解析，以Legged_gym为例解说Policy的训练流程

概述：Policy 作为通用 Tracker

Tracking 的核心思想

Legged Gym 的强化学习 Policy 本质上是一个通用的多模态跟踪系统。通过设计不同的观测空间和奖励函数，Policy 可以学习跟踪各种目标：

🎯 Tracking = 观测 (Observation) + 目标 (Target) + 奖励 (Reward)

训练的本质是让 Policy 学会：

1. 感知当前状态 (Where am I?)
2. 理解目标 (Where should I be?)
3. 计算偏差 (How far am I from the goal?)
4. 输出控制 (What should I do?)

为什么 RL Policy 擅长 Tracking?

优势	传统控制器	RL Policy
非线性系统	需要精确建模	✅ 从数据学习
高维耦合	难以设计	✅ 端到端学习
扰动鲁棒性	需要显式设计	✅ 隐式学习
多目标平衡	需要手动调优	✅ 奖励函数自动平衡
泛化能力	局限于设计范围	✅ 可泛化到未见场景

Legged Gym 的 Tracking 框架

基于 Isaac Gym 和 PPO (Proximal Policy Optimization) 算法，Legged Gym 提供了一个统一的框架，可以训练 Policy 跟踪：

• ✅ 速度指令 (Velocity Commands)
• ✅ 参考轨迹 (Reference Trajectories)
• ✅ 关节状态 (Joint States)
• ✅ 身体姿态 (Body Pose)
• ✅ 足端位置 (Foot Positions)
• ✅ 运动风格 (Motion Style)
• ✅ 多模态组合 (Multi-modal Combinations)

---

Tracking 的本质与范式

Tracking 的数学定义

在强化学习中，Tracking 问题可以形式化为：

min⁡πEτ∼π[∑t=0T∥st−stref∥2] \min_{\pi} \mathbb{E}{\tau \sim \pi} \left[ \sum{t=0}^{T} \| s_t - s_t^{ref} \|^2 \right] πminEτ∼π[t=0∑T∥st−stref∥2]

其中：

• π\piπ: Policy 策略
• sts_tst: 当前状态
• strefs_t^{ref}stref: 参考目标
• τ\tauτ: 轨迹

Tracking 的三种范式

1. 目标跟踪 (Target Tracking)

给定固定/变化的目标 → Policy 持续跟踪
例：速度指令跟踪、姿态保持

2. 轨迹跟踪 (Trajectory Tracking)

给定时间序列轨迹 → Policy 按时间跟随
例：动作捕捉数据重现、预定义步态

3. 分布跟踪 (Distribution Tracking)

给定状态分布 → Policy 采样符合分布的行为
例：风格模仿、多样性生成

Legged Gym 中的 Tracking 实现

在 Legged Gym 中，所有 Tracking 任务都遵循统一的框架：

# 统一的 Tracking 框架
class TrackingPolicy:
    def __init__(self):
        self.target = None  # 跟踪目标
        self.current_state = None  # 当前状态
        
    def compute_tracking_error(self):
        """计算跟踪误差"""
        return self.target - self.current_state
    
    def compute_tracking_reward(self, error):
        """将误差转换为奖励"""
        return torch.exp(-error / sigma)  # 高斯核
    
    def step(self, obs):
        """执行控制"""
        action = self.policy(obs)
        return action

---

可跟踪的模态与量

1. 运动学量 (Kinematic Quantities)

量	维度	观测空间	典型范围
线速度	3D	身体坐标系	[-2, 2] m/s
角速度	3D	身体坐标系	[-2, 2] rad/s
位置	3D	世界坐标系	任意
朝向	4D (四元数)	世界坐标系	SO(3)
关节角度	12D	关节空间	机械限位
关节角速度	12D	关节空间	[-10, 10] rad/s

2. 动力学量 (Dynamic Quantities)

量	维度	观测空间	应用场景
关节力矩	12D	关节空间	力控制任务
接触力	4×3D	足端	力感知步态
动量	3D	身体坐标系	跳跃、冲击
能量消耗	1D	标量	效率优化

3. 几何量 (Geometric Quantities)

量	维度	观测空间	应用场景
足端位置	4×3D	身体坐标系	精确落脚
质心位置	3D	身体坐标系	平衡控制
支撑多边形	2D	水平面	稳定性
地形高度图	187D	局部栅格	地形适应

4. 时序量 (Temporal Quantities)

量	维度	观测空间	应用场景
相位	1D	[0, 2π]	周期步态
步频	1D	[0.5, 3] Hz	运动节奏
占空比	1D	[0, 1]	支撑/摆动
时序轨迹	T×D	时间窗口	动作复现

5. 高级语义量 (Semantic Quantities)

量	维度	观测空间	应用场景
步态类型	离散	{trot, pace, gallop}	步态切换
运动风格	连续	潜在空间	风格化运动
任务目标	混合	任务相关	多任务学习
地形类型	离散	{flat, rough, stairs}	地形识别

Tracking 可行性分析

不是所有量都适合直接跟踪，需要考虑：

因素	说明	示例
可观测性	能否实时测量？	✅ 关节角度 vs ❌ 肌肉激活
可控性	Policy 能否影响？	✅ 速度 vs ❌ 外部扰动
时间尺度	控制频率匹配？	✅ 50Hz 关节控制 vs ❌ 0.1Hz 规划
耦合度	是否强耦合？	⚠️ 位置+姿态耦合
物理约束	是否违反动力学？	❌ 突变速度指令

---

运动跟踪 (Motion Tracking)

基础：速度指令跟踪

这是 Legged Gym 的默认任务，也是最基础的 Tracking 形式。

跟踪目标

commands = [vel_x, vel_y, ang_vel_yaw, heading]  # 4D 向量
# vel_x:      前后线速度 (m/s)
# vel_y:      左右线速度 (m/s)  
# ang_vel_yaw: 偏航角速度 (rad/s)
# heading:    目标朝向 (rad) - 可选

观测空间

obs = [
    base_lin_vel,       # [3] 当前线速度
    base_ang_vel,       # [3] 当前角速度
    projected_gravity,  # [3] 重力投影
    commands[:3],       # [3] 速度指令
    dof_pos,           # [12] 关节位置
    dof_vel,           # [12] 关节速度
    actions_prev,      # [12] 上一动作
    # height_map        # [187] 地形高度（可选）
]  # Total: 48 或 235

跟踪奖励

def _reward_tracking_lin_vel(self):
    """线速度跟踪奖励"""
    error = torch.sum(
        torch.square(self.commands[:, :2] - self.base_lin_vel[:, :2]), 
        dim=1
    )
    return torch.exp(-error / 0.25)  # 高斯核，sigma=0.25

def _reward_tracking_ang_vel(self):
    """角速度跟踪奖励"""
    error = torch.square(self.commands[:, 2] - self.base_ang_vel[:, 2])
    return torch.exp(-error / 0.25)

奖励曲线：

误差 (m/s) | 奖励
-----------|------
0.0        | 1.00  ← 完美跟踪
0.1        | 0.85
0.25       | 0.37
0.5        | 0.14
1.0        | 0.02

训练配置

class rewards:
    class scales:
        tracking_lin_vel = 1.0   # 主要优化目标
        tracking_ang_vel = 0.5   # 次要目标
        lin_vel_z = -2.0         # 惩罚跳动
        ang_vel_xy = -0.05       # 惩罚翻滚

进阶：身体姿态跟踪

跟踪目标

target_pose = {
    'position': [x, y, z],           # 目标位置
    'orientation': [qw, qx, qy, qz], # 目标四元数
    'height': h                      # 目标高度
}

姿态跟踪奖励

def _reward_orientation(self):
    """惩罚非水平姿态"""
    # 使用重力投影向量
    # 理想值: [0, 0, -1] (完全水平)
    error = torch.sum(
        torch.square(self.projected_gravity[:, :2]), 
        dim=1
    )
    return torch.exp(-error / 0.25)

def _reward_base_height(self):
    """高度跟踪"""
    target_height = 0.52  # ANYmal-C 标称高度
    error = torch.square(
        self.root_states[:, 2] - target_height
    )
    return torch.exp(-error / 0.1)

应用场景

1. 平衡任务: 在动态扰动下保持姿态
2. 俯仰控制: 爬坡时调整身体角度
3. 滚转避免: 侧向行走时防止翻倒

---

轨迹跟踪 (Trajectory Tracking)

概述

轨迹跟踪要求 Policy 不仅跟踪目标值，还要按照时间顺序精确重现整条轨迹。

参考轨迹来源

1. Motion Capture 数据

# 从动捕系统获取参考轨迹
mocap_data = {
    'joint_pos': [T, 12],      # 关节角度轨迹
    'joint_vel': [T, 12],      # 关节速度轨迹
    'root_pos': [T, 3],        # 根节点位置
    'root_orient': [T, 4],     # 根节点朝向
    'time': [T]                # 时间戳
}

2. 动画/仿真数据

# 从物理仿真或动画软件导出
animation_data = load_animation("trot_cycle.npy")
# Shape: [100, 48]  # 100 帧，48 维状态

3. 专家策略生成

# 使用传统控制器生成示范
expert_trajectory = classical_controller.rollout(
    duration=5.0, 
    command=[1.0, 0.0, 0.0]
)

实现：时序轨迹跟踪

扩展观测空间

class LeggedRobotCfg:
    class env:
        num_observations = 48 + 12  # 原始观测 + 参考状态
        reference_state_dim = 12    # 参考关节角度
        
obs = torch.cat([
    standard_obs,           # [48] 标准观测
    reference_joint_pos     # [12] 当前时刻的参考关节角度
], dim=-1)

轨迹跟踪奖励

def _reward_joint_tracking(self):
    """关节轨迹跟踪"""
    # 获取当前时间对应的参考状态
    t = self.episode_length_buf * self.dt
    ref_pos = self.get_reference_at_time(t)
    
    # 计算跟踪误差
    error = torch.sum(
        torch.square(self.dof_pos - ref_pos), 
        dim=1
    )
    return torch.exp(-error / 0.1)

def _reward_joint_vel_tracking(self):
    """关节速度跟踪"""
    t = self.episode_length_buf * self.dt
    ref_vel = self.get_reference_vel_at_time(t)
    
    error = torch.sum(
        torch.square(self.dof_vel - ref_vel), 
        dim=1
    )
    return torch.exp(-error / 0.5)

参考轨迹插值

def get_reference_at_time(self, t):
    """根据时间插值参考轨迹"""
    # 循环轨迹
    t_normalized = t % self.traj_duration
    
    # 线性插值
    idx = (t_normalized / self.dt).long()
    alpha = (t_normalized / self.dt) - idx
    
    ref_pos = (1 - alpha) * self.trajectory[idx] + \
              alpha * self.trajectory[idx + 1]
    
    return ref_pos

相位跟踪 (Phase Tracking)

周期步态的相位表示

# 相位作为观测
phase = (t % T) / T * 2 * π  # [0, 2π]
obs = torch.cat([
    standard_obs,
    torch.sin(phase),  # 正弦编码
    torch.cos(phase)   # 余弦编码
], dim=-1)

相位条件奖励

def _reward_phase_based_foot_contact(self):
    """相位同步的接触奖励"""
    phase = self.get_phase()
    
    # 期望接触模式 (trot 步态)
    expected_contact = torch.tensor([
        [1, 0, 0, 1],  # 对角支撑 1
        [0, 1, 1, 0]   # 对角支撑 2
    ])
    
    phase_idx = (phase / (2*π) * 2).long() % 2
    target = expected_contact[phase_idx]
    
    actual_contact = self.contact_forces[:, self.feet_indices, 2] > 1.0
    
    # 匹配度奖励
    match = (actual_contact == target).float().mean(dim=1)
    return match

---

状态跟踪 (State Tracking)

多关节状态同步跟踪

挑战

同时跟踪 12 个关节的位置和速度，共 24 维状态：

target_state = {
    'dof_pos': [12],  # 关节角度
    'dof_vel': [12]   # 关节速度
}

加权跟踪奖励

def _reward_weighted_joint_tracking(self):
    """加权关节跟踪"""
    # 不同关节不同权重
    weights = torch.tensor([
        1.0, 1.0, 0.8,  # 前左腿 (HAA, HFE, KFE)
        1.0, 1.0, 0.8,  # 前右腿
        1.0, 1.0, 0.8,  # 后左腿
        1.0, 1.0, 0.8   # 后右腿
    ])
    
    error = torch.square(self.dof_pos - self.target_dof_pos)
    weighted_error = (error * weights).sum(dim=1)
    
    return torch.exp(-weighted_error / 0.5)

足端轨迹跟踪

正向运动学计算

def compute_foot_positions(self):
    """计算足端位置（身体坐标系）"""
    # 使用机器人运动学模型
    foot_pos = []
    for leg_id in range(4):
        joint_angles = self.dof_pos[:, leg_id*3:(leg_id+1)*3]
        pos = forward_kinematics(joint_angles, leg_id)
        foot_pos.append(pos)
    
    return torch.stack(foot_pos, dim=1)  # [N, 4, 3]

足端跟踪奖励

def _reward_foot_position_tracking(self):
    """足端位置跟踪"""
    current_foot_pos = self.compute_foot_positions()
    target_foot_pos = self.get_target_foot_positions()
    
    error = torch.sum(
        torch.square(current_foot_pos - target_foot_pos), 
        dim=[1, 2]
    )
    return torch.exp(-error / 0.05)

---

多模态融合跟踪

概述

实际应用中，往往需要同时跟踪多个目标，这需要在奖励函数中平衡多个 Tracking 目标。

多目标跟踪框架

class MultiModalTracking:
    def compute_reward(self):
        """多模态跟踪总奖励"""
        reward = 0
        
        # 1. 速度跟踪 (主要目标)
        reward += 1.0 * self.tracking_lin_vel()
        reward += 0.5 * self.tracking_ang_vel()
        
        # 2. 姿态跟踪 (次要目标)
        reward += 0.2 * self.orientation_tracking()
        reward += 0.1 * self.height_tracking()
        
        # 3. 步态跟踪 (风格约束)
        reward += 0.3 * self.phase_based_gait()
        
        # 4. 稳定性 (软约束)
        reward -= 2.0 * self.lin_vel_z_penalty()
        reward -= 0.05 * self.ang_vel_xy_penalty()
        
        return reward

典型组合场景

场景 1: 速度 + 姿态跟踪

# 应用：斜坡行走
class SlopeWalkingTracking:
    rewards = {
        'tracking_lin_vel': 1.0,      # 保持速度
        'tracking_ang_vel': 0.5,      # 转向能力
        'orientation': 0.5,           # 保持水平（抵抗斜坡倾斜）
        'base_height': 0.2,           # 高度稳定
        'torques': -0.0001,           # 能量效率
    }

场景 2: 轨迹 + 相位跟踪

# 应用：动作复现
class MotionImitationTracking:
    rewards = {
        'joint_pos_tracking': 2.0,    # 关节角度匹配
        'joint_vel_tracking': 1.0,    # 关节速度匹配
        'phase_tracking': 0.5,        # 时序对齐
        'foot_contact': 0.3,          # 接触时机
        'root_pose': 0.5,             # 身体位姿
    }

---

Policy 网络结构

Actor-Critic 架构

系统使用 Actor-Critic 双网络结构：

class ActorCritic(nn.Module):
    def __init__(self):
        # Actor: 策略网络，用于控制
        self.actor = MLP(
            input_dim=num_observations,      # 观测维度
            output_dim=num_actions,          # 动作维度 (12个关节)
            hidden_dims=[512, 256, 128],     # 隐藏层
            activation='elu'
        )
        
        # Critic: 价值网络，仅用于训练
        self.critic = MLP(
            input_dim=num_observations,      # 或 num_privileged_obs
            output_dim=1,                    # 输出状态价值
            hidden_dims=[512, 256, 128],
            activation='elu'
        )

网络配置示例

配置 1: 轻量级速度跟踪

class policy:
    actor_hidden_dims = [128, 64, 32]
    critic_hidden_dims = [128, 64, 32]
    activation = 'elu'
    
# 参数量：~50K
# 推理速度：<0.5ms
# 适用：简单平地行走

配置 2: 标准复杂地形跟踪

class policy:
    actor_hidden_dims = [512, 256, 128]
    critic_hidden_dims = [512, 256, 128]
    activation = 'elu'
    
# 参数量：~400K
# 推理速度：~1ms
# 适用：复杂地形、多目标跟踪

配置 3: 轨迹跟踪 (LSTM)

class policy:
    actor_hidden_dims = [256, 128]
    critic_hidden_dims = [256, 128]
    rnn_type = 'lstm'
    rnn_hidden_size = 256
    rnn_num_layers = 2
    
# 参数量：~600K
# 推理速度：~2ms
# 适用：动作模仿、相位跟踪

---

从训练到部署

Policy 的本质理解

训练完成后，Policy 是一个条件控制器：

Policy: (Observation, Target) → Action

关键理解：

1. ✅ Policy 需要外部提供跟踪目标 (target/command)
2. ✅ Policy 根据当前状态和目标输出控制动作
3. ❌ Policy 不会自己决定目标是什么
4. ❌ Policy 没有全局规划能力

训练流程

1. 环境配置

# train.py
def train(args):
    # 创建环境
    env, env_cfg = task_registry.make_env(name=args.task, args=args)
    
    # 创建 PPO Runner
    ppo_runner, train_cfg = task_registry.make_alg_runner(
        env=env, 
        name=args.task,
        args=args
    )
    
    # 开始训练
    ppo_runner.learn(
        num_learning_iterations=train_cfg.runner.max_iterations,
        init_at_random_ep_len=True
    )

2. 训练循环

# PPO 训练循环
for iteration in range(max_iterations):
    # 采集经验
    for step in range(num_steps_per_env):
        obs = env.get_observations()
        actions = policy(obs)
        obs_next, rewards, dones, _ = env.step(actions)
        
        buffer.add(obs, actions, rewards, dones)
    
    # 更新策略
    for epoch in range(num_learning_epochs):
        for mini_batch in buffer.iterate(num_mini_batches):
            loss = compute_ppo_loss(mini_batch)
            optimizer.zero_grad()
            loss.backward()
            optimizer.step()
    
    # 保存模型
    if iteration % save_interval == 0:
        save_model(f"model_{iteration}.pt")

部署流程

1. 加载模型

# play.py
def load_policy(model_path):
    # 加载训练配置
    env_cfg, train_cfg = task_registry.get_cfgs(name=args.task)
    
    # 创建环境
    env, _ = task_registry.make_env(name=args.task, env_cfg=env_cfg)
    
    # 加载 Policy
    train_cfg.runner.resume = True
    ppo_runner, _ = task_registry.make_alg_runner(env=env, train_cfg=train_cfg)
    policy = ppo_runner.get_inference_policy(device=env.device)
    
    return policy, env

2. 实时推理

---

训练算法：PPO

PPO 核心参数

# 来自 LeggedRobotCfgPPO.algorithm
class algorithm:
    # 价值函数损失
    value_loss_coef = 1.0              # 价值损失系数
    use_clipped_value_loss = True      # 使用裁剪的价值损失
    
    # PPO 特定参数
    clip_param = 0.2                   # PPO 裁剪参数 ε
    entropy_coef = 0.01                # 熵正则化系数
    
    # 优化参数
    num_learning_epochs = 5            # 每次迭代的 epoch 数
    num_mini_batches = 4               # mini-batch 数量
    learning_rate = 1.e-3              # 学习率
    schedule = 'adaptive'              # 学习率调度: adaptive/fixed
    
    # GAE (广义优势估计)
    gamma = 0.99                       # 折扣因子
    lam = 0.95                         # GAE λ 参数
    
    # KL 散度约束
    desired_kl = 0.01                  # 目标 KL 散度
    max_grad_norm = 1.0                # 梯度裁剪

训练循环参数

# 来自 LeggedRobotCfgPPO.runner
class runner:
    num_steps_per_env = 24             # 每个环境每次迭代采集的步数
    max_iterations = 1500              # 总迭代次数
    save_interval = 50                 # 每 50 次迭代保存一次模型
    
    # 并行环境数
    num_envs = 4096                    # 同时运行 4096 个环境

关键计算:

# 每次迭代采样总步数
total_steps = num_envs * num_steps_per_env = 4096 * 24 = 98,304 步

# Mini-batch 大小
mini_batch_size = total_steps / num_mini_batches = 98,304 / 4 = 24,576

# 总训练样本数
total_samples = max_iterations * total_steps = 1500 * 98,304 ≈ 1.47 亿步

PPO 损失函数

# 1. Policy Loss (Clipped Surrogate Objective)
ratio = π_new(a|s) / π_old(a|s)
L_policy = -min(ratio * A, clip(ratio, 1-ε, 1+ε) * A)

# 2. Value Loss
L_value = (V(s) - V_target)^2

# 3. Entropy Bonus
L_entropy = -entropy(π(·|s))

# 总损失
Loss = L_policy + value_loss_coef * L_value + entropy_coef * L_entropy

---

奖励函数设计

奖励函数的作用

通过精心设计的奖励函数组合，引导 Policy 学习期望的行为。

主要奖励项

# 来自 LeggedRobotCfg.rewards.scales
class scales:
    # === 核心跟踪奖励 ===
    tracking_lin_vel = 1.0      # 线速度跟踪
    tracking_ang_vel = 0.5      # 角速度跟踪
    
    # === 稳定性惩罚 ===
    lin_vel_z = -2.0            # 惩罚 z 轴速度（上下跳动）
    ang_vel_xy = -0.05          # 惩罚 x,y 轴角速度（翻滚/俯仰）
    orientation = -0.0          # 惩罚非水平姿态
    base_height = -0.0          # 惩罚偏离目标高度
    
    # === 能量与平滑性 ===
    torques = -0.00001          # 惩罚力矩（能量消耗）
    dof_vel = -0.0              # 惩罚关节速度
    dof_acc = -2.5e-7           # 惩罚关节加速度
    action_rate = -0.01         # 惩罚动作变化（平滑性）
    
    # === 步态奖励 ===
    feet_air_time = 1.0         # 奖励长步幅（脚在空中的时间）
    
    # === 碰撞惩罚 ===
    collision = -1.0            # 惩罚非脚部的碰撞
    feet_stumble = -0.0         # 惩罚脚部撞到垂直面
    
    # === 终止惩罚 ===
    termination = -0.0          # 跌倒时的惩罚
    
    # === 其他 ===
    stand_still = -0.0          # 零速度指令时惩罚运动

关键奖励函数实现

1. 速度跟踪奖励（最重要）

def _reward_tracking_lin_vel(self):
    """线速度跟踪 - 核心训练目标"""
    # 计算指令速度与实际速度的误差
    lin_vel_error = torch.sum(
        torch.square(self.commands[:, :2] - self.base_lin_vel[:, :2]), 
        dim=1
    )
    # 使用高斯核将误差转换为 [0, 1] 的奖励
    return torch.exp(-lin_vel_error / tracking_sigma)
    # tracking_sigma = 0.25

def _reward_tracking_ang_vel(self):
    """角速度跟踪"""
    ang_vel_error = torch.square(self.commands[:, 2] - self.base_ang_vel[:, 2])
    return torch.exp(-ang_vel_error / tracking_sigma)

奖励机制分析:

• 完美跟踪 (误差=0) → 奖励 = 1.0
• 误差 = 0.25 → 奖励 ≈ 0.37
• 误差 = 0.5 → 奖励 ≈ 0.14

2. 步态奖励

def _reward_feet_air_time(self):
    """奖励长步幅 - 促进自然步态"""
    # 检测脚的接触状态
    contact = self.contact_forces[:, self.feet_indices, 2] > 1.0
    contact_filt = torch.logical_or(contact, self.last_contacts)
    
    # 检测首次接触
    first_contact = (self.feet_air_time > 0.) * contact_filt
    
    # 累积空中时间
    self.feet_air_time += self.dt
    
    # 只在首次接触时给奖励
    rew_airTime = torch.sum((self.feet_air_time - 0.5) * first_contact, dim=1)
    
    # 仅在有运动指令时给奖励
    rew_airTime *= torch.norm(self.commands[:, :2], dim=1) > 0.1
    
    # 重置接触脚的空中时间
    self.feet_air_time *= ~contact_filt
    return rew_airTime

3. 关节限制惩罚

def _reward_dof_pos_limits(self):
    """惩罚关节位置接近极限"""
    # 计算超出软限制的量
    out_of_limits = -(self.dof_pos - self.dof_pos_limits[:, 0]).clip(max=0.)  # 下限
    out_of_limits += (self.dof_pos - self.dof_pos_limits[:, 1]).clip(min=0.)  # 上限
    return torch.sum(out_of_limits, dim=1)

奖励组合计算

def compute_reward(self):
    """计算总奖励"""
    self.rew_buf[:] = 0.
    
    # 遍历所有非零奖励项
    for i in range(len(self.reward_functions)):
        name = self.reward_names[i]
        # 调用奖励函数并乘以 scale 和 dt
        rew = self.reward_functions[i]() * self.reward_scales[name]
        self.rew_buf += rew
        self.episode_sums[name] += rew  # 记录累积奖励
    
    # 可选：只保留正奖励
    if self.cfg.rewards.only_positive_rewards:
        self.rew_buf[:] = torch.clip(self.rew_buf[:], min=0.)
    
    # 添加终止惩罚（不受 clipping 影响）
    if "termination" in self.reward_scales:
        rew = self._reward_termination() * self.reward_scales["termination"]
        self.rew_buf += rew

注意 : 所有奖励 scale 会自动乘以 dt（控制频率），这样奖励值不依赖于控制频率。

---

训练流程

整体架构

train.py → TaskRegistry → Environment + PPO Runner → 训练循环

训练脚本 (train.py)

def train(args):
    # 1. 创建环境
    env, env_cfg = task_registry.make_env(name=args.task, args=args)
    
    # 2. 创建 PPO Runner
    ppo_runner, train_cfg = task_registry.make_alg_runner(
        env=env, 
        name=args.task, 
        args=args
    )
    
    # 3. 开始训练
    ppo_runner.learn(
        num_learning_iterations=train_cfg.runner.max_iterations,
        init_at_random_ep_len=True
    )

if __name__ == '__main__':
    args = get_args()
    train(args)

环境主循环 (step 函数)

def step(self, actions):
    """单步仿真"""
    # 1. 限幅动作
    self.actions = torch.clip(actions, -clip_actions, clip_actions)
    
    # 2. 渲染
    self.render()
    
    # 3. 物理仿真（重复 decimation 次）
    for _ in range(self.cfg.control.decimation):  # 默认 4 次
        # 计算力矩 (PD 控制)
        self.torques = self._compute_torques(self.actions)
        
        # 应用力矩并仿真
        self.gym.set_dof_actuation_force_tensor(self.sim, self.torques)
        self.gym.simulate(self.sim)
        
        # 更新状态张量
        self.gym.refresh_dof_state_tensor(self.sim)
    
    # 4. 后处理
    self.post_physics_step()
    
    # 5. 限幅观测
    self.obs_buf = torch.clip(self.obs_buf, -clip_obs, clip_obs)
    
    # 6. 返回
    return self.obs_buf, self.privileged_obs_buf, self.rew_buf, \
           self.reset_buf, self.extras

PD 控制器

def _compute_torques(self, actions):
    """从动作计算关节力矩"""
    # 缩放动作
    actions_scaled = actions * self.cfg.control.action_scale
    
    # PD 控制 (位置模式)
    if control_type == "P":
        torques = self.p_gains * (actions_scaled + self.default_dof_pos - self.dof_pos) \
                  - self.d_gains * self.dof_vel
    
    # PD 控制 (速度模式)
    elif control_type == "V":
        torques = self.p_gains * (actions_scaled - self.dof_vel) \
                  - self.d_gains * (self.dof_vel - self.last_dof_vel) / self.dt
    
    # 直接力矩控制
    elif control_type == "T":
        torques = actions_scaled
    
    # 限幅力矩
    return torch.clip(torques, -self.torque_limits, self.torque_limits)

PD 增益配置:

class control:
    stiffness = {  # P 增益 [N·m/rad]
        'HAA': 80.0,   # 髋部外展/内收
        'HFE': 80.0,   # 髋部屈伸
        'KFE': 80.0    # 膝部屈伸
    }
    damping = {    # D 增益 [N·m·s/rad]
        'HAA': 2.0,
        'HFE': 2.0,
        'KFE': 2.0
    }

后处理流程

def post_physics_step(self):
    """物理步后的处理"""
    # 1. 刷新状态
    self.gym.refresh_actor_root_state_tensor(self.sim)
    self.gym.refresh_net_contact_force_tensor(self.sim)
    
    # 2. 更新计数器
    self.episode_length_buf += 1
    self.common_step_counter += 1
    
    # 3. 计算派生量
    self.base_lin_vel = quat_rotate_inverse(self.base_quat, self.root_states[:, 7:10])
    self.base_ang_vel = quat_rotate_inverse(self.base_quat, self.root_states[:, 10:13])
    self.projected_gravity = quat_rotate_inverse(self.base_quat, self.gravity_vec)
    
    # 4. 回调（重采样指令、推机器人等）
    self._post_physics_step_callback()
    
    # 5. 检查终止条件
    self.check_termination()
    
    # 6. 计算奖励
    self.compute_reward()
    
    # 7. 重置终止的环境
    env_ids = self.reset_buf.nonzero(as_tuple=False).flatten()
    self.reset_idx(env_ids)
    
    # 8. 计算观测
    self.compute_observations()
    
    # 9. 保存历史
    self.last_actions[:] = self.actions[:]
    self.last_dof_vel[:] = self.dof_vel[:]

终止条件

def check_termination(self):
    """检查是否需要重置环境"""
    # 1. 碰撞终止：关键部位接触地面
    self.reset_buf = torch.any(
        torch.norm(
            self.contact_forces[:, self.termination_contact_indices, :], 
            dim=-1
        ) > 1.0, 
        dim=1
    )
    
    # 2. 超时终止
    self.time_out_buf = self.episode_length_buf > self.max_episode_length
    
    # 3. 合并
    self.reset_buf |= self.time_out_buf

---

指令跟踪机制

指令的生成与更新

def _resample_commands(self, env_ids):
    """随机采样速度指令"""
    # 线速度 x (前后)
    self.commands[env_ids, 0] = torch_rand_float(
        self.command_ranges["lin_vel_x"][0],  # -1.0 m/s
        self.command_ranges["lin_vel_x"][1],  # +1.0 m/s
        (len(env_ids), 1), 
        device=self.device
    ).squeeze(1)
    
    # 线速度 y (左右)
    self.commands[env_ids, 1] = torch_rand_float(
        self.command_ranges["lin_vel_y"][0],  # -1.0 m/s
        self.command_ranges["lin_vel_y"][1],  # +1.0 m/s
        (len(env_ids), 1),
        device=self.device
    ).squeeze(1)
    
    # 角速度 yaw (旋转) 或 heading (朝向)
    if self.cfg.commands.heading_command:
        # Heading 模式：指定目标朝向
        self.commands[env_ids, 3] = torch_rand_float(
            self.command_ranges["heading"][0],   # -π
            self.command_ranges["heading"][1],   # +π
            (len(env_ids), 1),
            device=self.device
        ).squeeze(1)
    else:
        # 直接角速度模式
        self.commands[env_ids, 2] = torch_rand_float(
            self.command_ranges["ang_vel_yaw"][0],  # -1 rad/s
            self.command_ranges["ang_vel_yaw"][1],  # +1 rad/s
            (len(env_ids), 1),
            device=self.device
        ).squeeze(1)
    
    # 过滤小速度指令（避免不稳定）
    self.commands[env_ids, :2] *= (
        torch.norm(self.commands[env_ids, :2], dim=1) > 0.2
    ).unsqueeze(1)

指令重采样时机

def _post_physics_step_callback(self):
    """每个物理步后的回调"""
    # 定期重采样指令 (默认 10 秒)
    env_ids = (
        self.episode_length_buf % int(self.cfg.commands.resampling_time / self.dt) == 0
    ).nonzero(as_tuple=False).flatten()
    self._resample_commands(env_ids)
    
    # Heading 模式：计算角速度指令
    if self.cfg.commands.heading_command:
        forward = quat_apply(self.base_quat, self.forward_vec)
        heading = torch.atan2(forward[:, 1], forward[:, 0])
        # 将 heading 误差转换为角速度指令
        self.commands[:, 2] = torch.clip(
            0.5 * wrap_to_pi(self.commands[:, 3] - heading), 
            -1.0, 
            1.0
        )

指令课程学习 (Curriculum)

def update_command_curriculum(self, env_ids):
    """根据表现逐步增加指令难度"""
    # 如果线速度跟踪奖励 > 80% 最大值
    if torch.mean(self.episode_sums["tracking_lin_vel"][env_ids]) / \
       self.max_episode_length > 0.8 * self.reward_scales["tracking_lin_vel"]:
        
        # 扩大速度指令范围
        self.command_ranges["lin_vel_x"][0] = np.clip(
            self.command_ranges["lin_vel_x"][0] - 0.5,  # 减小下限
            -self.cfg.commands.max_curriculum,           # 最低 -max_curriculum
            0.
        )
        self.command_ranges["lin_vel_x"][1] = np.clip(
            self.command_ranges["lin_vel_x"][1] + 0.5,  # 增大上限
            0.,
            self.cfg.commands.max_curriculum            # 最高 +max_curriculum
        )

---

训练后的 Policy 使用方式

Policy 的本质

训练完成后，Policy 是一个映射函数：

f: Observations → Actions

关键理解:

1. ✅ Policy 需要外部提供速度指令 (commands)
2. ✅ Policy 根据指令和当前状态输出关节动作
3. ❌ Policy 不会自己决定往哪里走
4. ❌ Policy 没有全局路径规划能力

推理/部署流程

# 来自 play.py
def play(args):
    # 1. 加载环境和配置
    env_cfg, train_cfg = task_registry.get_cfgs(name=args.task)
    
    # 2. 创建环境
    env, _ = task_registry.make_env(name=args.task, args=args, env_cfg=env_cfg)
    obs = env.get_observations()
    
    # 3. 加载训练好的 Policy
    train_cfg.runner.resume = True
    ppo_runner, _ = task_registry.make_alg_runner(
        env=env, 
        name=args.task, 
        args=args, 
        train_cfg=train_cfg
    )
    policy = ppo_runner.get_inference_policy(device=env.device)
    
    # 4. 推理循环
    for i in range(10 * int(env.max_episode_length)):
        # 获取动作
        actions = policy(obs.detach())
        
        # 执行动作
        obs, _, rews, dones, infos = env.step(actions.detach())

Policy 调用示例

# 伪代码：实际机器人部署
class RobotController:
    def __init__(self):
        self.policy = load_trained_policy("model.pt")
        self.commands = [0.0, 0.0, 0.0, 0.0]  # [vx, vy, yaw, heading]
    
    def set_velocity_command(self, vx, vy, yaw):
        """外部设置速度指令"""
        self.commands = [vx, vy, yaw, 0.0]
    
    def control_step(self):
        """单步控制"""
        # 1. 读取传感器获取观测
        obs = self.get_observations()  # [48] 或 [235]
        # obs 中已包含 self.commands
        
        # 2. Policy 推理
        actions = self.policy(obs)  # [12] 关节目标
        
        # 3. 发送到低层控制器
        self.send_joint_targets(actions)
    
    def move_forward(self):
        """示例：向前走"""
        self.set_velocity_command(vx=0.5, vy=0.0, yaw=0.0)
        for _ in range(100):
            self.control_step()
    
    def turn_left(self):
        """示例：左转"""
        self.set_velocity_command(vx=0.0, vy=0.0, yaw=0.5)
        for _ in range(50):
            self.control_step()

关键代码解析

1. 环境初始化

class LeggedRobot(BaseTask):
    def __init__(self, cfg, sim_params, physics_engine, sim_device, headless):
        self.cfg = cfg
        self._parse_cfg(self.cfg)
        
        # 调用父类初始化（创建仿真）
        super().__init__(self.cfg, sim_params, physics_engine, sim_device, headless)
        
        # 设置相机
        if not self.headless:
            self.set_camera(self.cfg.viewer.pos, self.cfg.viewer.lookat)
        
        # 初始化缓冲区
        self._init_buffers()
        
        # 准备奖励函数
        self._prepare_reward_function()
        
        self.init_done = True

2. 缓冲区初始化

def _init_buffers(self):
    """初始化训练所需的张量"""
    # 获取 Isaac Gym 的状态张量
    actor_root_state = self.gym.acquire_actor_root_state_tensor(self.sim)
    dof_state_tensor = self.gym.acquire_dof_state_tensor(self.sim)
    net_contact_forces = self.gym.acquire_net_contact_force_tensor(self.sim)
    
    # 包装为 PyTorch 张量
    self.root_states = gymtorch.wrap_tensor(actor_root_state)
    self.dof_state = gymtorch.wrap_tensor(dof_state_tensor)
    self.contact_forces = gymtorch.wrap_tensor(net_contact_forces)
    
    # 创建视图
    self.dof_pos = self.dof_state.view(self.num_envs, self.num_dof, 2)[..., 0]
    self.dof_vel = self.dof_state.view(self.num_envs, self.num_dof, 2)[..., 1]
    
    # 初始化其他缓冲区
    self.actions = torch.zeros(self.num_envs, self.num_actions, device=self.device)
    self.commands = torch.zeros(self.num_envs, 4, device=self.device)
    # ... 更多缓冲区

3. 奖励函数准备

def _prepare_reward_function(self):
    """动态构建奖励函数列表"""
    # 移除零权重的奖励项，并将非零项乘以 dt
    for key in list(self.reward_scales.keys()):
        scale = self.reward_scales[key]
        if scale == 0:
            self.reward_scales.pop(key)
        else:
            self.reward_scales[key] *= self.dt
    
    # 构建函数列表
    self.reward_functions = []
    self.reward_names = []
    for name, scale in self.reward_scales.items():
        if name == "termination":
            continue
        self.reward_names.append(name)
        # 通过名称获取方法: '_reward_' + name
        name = '_reward_' + name
        self.reward_functions.append(getattr(self, name))
    
    # 初始化累积奖励
    self.episode_sums = {
        name: torch.zeros(self.num_envs, device=self.device)
        for name in self.reward_scales.keys()
    }

4. 地形课程学习

def _update_terrain_curriculum(self, env_ids):
    """根据表现调整地形难度"""
    if not self.init_done:
        return
    
    # 计算机器人移动距离
    distance = torch.norm(
        self.root_states[env_ids, :2] - self.env_origins[env_ids, :2], 
        dim=1
    )
    
    # 走得远 → 升级地形
    move_up = distance > self.terrain.env_length / 2
    
    # 走得近 → 降级地形
    expected_distance = torch.norm(self.commands[env_ids, :2], dim=1) * \
                        self.max_episode_length_s * 0.5
    move_down = (distance < expected_distance) * ~move_up
    
    # 更新地形等级
    self.terrain_levels[env_ids] += 1 * move_up - 1 * move_down
    
    # 解决最高难度的机器人随机重置
    self.terrain_levels[env_ids] = torch.where(
        self.terrain_levels[env_ids] >= self.max_terrain_level,
        torch.randint_like(self.terrain_levels[env_ids], self.max_terrain_level),
        torch.clip(self.terrain_levels[env_ids], 0)
    )
    
    # 更新起始位置
    self.env_origins[env_ids] = self.terrain_origins[
        self.terrain_levels[env_ids], 
        self.terrain_types[env_ids]
    ]

5. 域随机化

def _process_rigid_shape_props(self, props, env_id):
    """随机化摩擦系数"""
    if self.cfg.domain_rand.randomize_friction:
        if env_id == 0:
            # 创建摩擦系数桶
            num_buckets = 64
            bucket_ids = torch.randint(0, num_buckets, (self.num_envs, 1))
            friction_buckets = torch_rand_float(
                friction_range[0], 
                friction_range[1], 
                (num_buckets, 1), 
                device='cpu'
            )
            self.friction_coeffs = friction_buckets[bucket_ids]
        
        # 应用到每个形状
        for s in range(len(props)):
            props[s].friction = self.friction_coeffs[env_id]
    
    return props

def _push_robots(self):
    """随机推动机器人（测试鲁棒性）"""
    max_vel = self.cfg.domain_rand.max_push_vel_xy
    self.root_states[:, 7:9] = torch_rand_float(
        -max_vel, max_vel, (self.num_envs, 2), device=self.device
    )
    self.gym.set_actor_root_state_tensor(self.sim, self.root_states)

---