机器人强化学习入门笔记（四）

DogKing 项目与 legged_robot_competition 项目差异说明

一、项目概述

本文档详细说明 DogKing 项目（基于 Training-Quadruped-Robots-to-Traverse-Diverse-Complex-Terrains）与原始 legged_robot_competition 项目的差异。

二、目录结构对比

2.1 主要修改目录

legged_gym/legged_gym/envs/go2/：GO2 环境相关代码，包含所有核心改进

go2_config.py：配置文件，包含奖励权重、观测维度等配置
go2_robot.py：环境实现，包含奖励函数、观测计算等核心逻辑

2.2 其他目录

isaacgym/：NVIDIA Isaac Gym 仿真库

isaacgym/python/examples/terrain_creation.py：stone_distance=1. -> 0.1
isaacgym/python/isaacgym/terrain_utils.py: def stepping_stones_terrain()中增加specific_stones，从原先完全随机生成所有踏步石的高度，变为了可以精确控制某些特定位置的踏步石的高度。

rsl_rl/：强化学习算法库（PPO 等）

rsl_rl/rsl_rl/runners/on_policy_runner.py: 新增def log2file ()，保存结果到"rewards_results.txt"

三、核心代码差异

3.1 配置文件差异（`go2_config.py`）

3.1.1 观测空间配置

原始版本：

python 复制代码

num_observations = 235  # 基础观测维度

DogKing 版本：

python 复制代码

num_obs = 275  # 基础观测维度
history_len = 10  # 历史长度
n_proprio = 55  # 本体感受观测维度
num_observations = num_obs + history_len * n_proprio  # 825 维

差异说明：

基础观测从 235 维增加到 275 维（新增偏航角误差、地形类别、接触状态等）
新增历史观测编码，总维度达到 825 维

3.1.2 地形测量点配置

原始版本：

python 复制代码

measured_points_x = [-0.8, -0.7, ..., 0.7, 0.8]  # 17 个点

DogKing 版本：

python 复制代码

measured_points_x = [-0.6, -0.5, ..., 1.2, 1.3]  # 21 个点

差异说明 ：测量点向前延伸，从 [ − 0.8 , 0.8 ] [-0.8, 0.8] [−0.8,0.8] 扩展到 [ − 0.6 , 1.3 ] [-0.6, 1.3] [−0.6,1.3]，使机器狗能够提前感知前方地形。

3.1.3 奖励权重配置

主要差异：

奖励项	原始权重	DogKing 权重	说明
`tracking_lin_vel`	2.0	0.0	已禁用，改用 `tracking_goal_vel`
`tracking_ang_vel`	0.5	0.0	已禁用
`tracking_goal_vel`	不存在	2.0	新增：目标点追踪奖励
`tracking_yaw`	不存在	0.5	新增：偏航角追踪奖励
`reach_all_goal`	不存在	100.0	新增：完成所有目标点奖励
`feet_edge`	不存在	-1.5	新增：梅花桩边缘惩罚
`collision`	-1.0	-10.0	权重加强
`orientation`	-0.1	-1.0	权重加强
`lin_vel_z`	-0.0	-0.3	权重加强
`ang_vel_xy`	-0.01	-0.04	权重加强
`torques`	-0.0002	-0.00001	权重调整
`dof_vel`	-2.5e-7	-0.0	已禁用
`dof_acc`	-2.5e-7	-2.5e-7	保持不变
`action_rate`	-0.001	-0.0015	权重加强
`dof_pos_limits`	-0.01	-0.5	权重加强
`delta_torques`	不存在	-1.0e-7	新增：力矩变化惩罚
`hip_pos`	不存在	-0.6	新增：髋关节位置惩罚
`dof_error`	不存在	-0.001	新增：关节位置误差惩罚
`symmetry`	不存在	-0.5	新增：对称性惩罚
`stagnation`	不存在	-0.0	新增：停滞惩罚（当前禁用）
`limbo`	不存在	-0.0	新增：越界惩罚（当前禁用）
`goal_pos`	不存在	0.0	新增：位置奖励（当前禁用）
`out_mid`	不存在	-0.0	新增：偏离中线惩罚（当前禁用）
`move_back`	不存在	-0.0	新增：后退惩罚（当前禁用）
`feet_stumble`	-0.0	-1.0	权重加强
`edge_feet_up`	不存在	0.0	新增：边缘抬脚奖励（当前禁用）
`foot_above_knee`	不存在	0.0	新增：脚高于膝盖奖励（当前禁用）
`foot_above_hip`	不存在	0.0	新增：脚高于髋关节奖励（当前禁用）
`jump_up`	不存在	0.0	新增：跳跃奖励（当前禁用）
`air_taitou`	不存在	0.0	新增：空中抬头奖励（当前禁用）
`jump_lift_front_feet`	不存在	0.0	新增：跳跃抬前脚奖励（当前禁用）
`jump_pitch`	不存在	-0.0	新增：跳跃俯仰角惩罚（当前禁用）
`jump_preparation`	不存在	0.0	新增：跳跃准备奖励（当前禁用）
`air_foward`	不存在	0.0	新增：空中前进奖励（当前禁用）
`feet_height`	不存在	-0.0	新增：脚高度惩罚（当前禁用）

3.1.4 其他配置差异

配置项	原始值	DogKing 值	说明
`symmetric`	True	False	禁用对称观测
`episode_length_s`	25	200	回合长度延长
`measured_points_x`	17个点	20个点	测量点向前延伸
`lin_vel_x` 范围	[0.5, 2.0]	[1.0, 1.2]	速度指令范围缩小
`penalize_contacts_on`	["thigh", "calf"]	["thigh", "calf", "base"]	新增基座碰撞惩罚
`friction_range`	[0.2, 1.5]	[1.0, 1.5]	摩擦系数范围调整
`run_name`	''	'go2_rough'	日志目录名称
`experiment_name`	'rough_go2'	'rough_go2'	保持不变

3.2 环境实现差异（`go2_robot.py`）

3.2.1 新增方法

euler_from_quaternion()（第 24-44 行）
- 从四元数提取欧拉角（roll, pitch, yaw）
- 用于计算 IMU 观测
_update_goals()（第 93-110 行）
- 更新当前目标点和下一个目标点
- 计算目标方向向量和偏航角
compute_edge_mask()（第 371-392 行）
- 计算地形边缘掩码
- 用于检测梅花桩边缘
_gather_cur_goals()（第 567-568 行）
- 获取当前或未来目标点
- 支持 future 参数获取下一个目标点
_draw_goals()（第 1144-1194 行）
- 可视化目标点序列
- 用于调试和观察
_draw_terrain_edges()（第 1122-1142 行）
- 可视化地形边缘
- 用于调试
_draw_feet()（第 1196-1207 行）
- 可视化脚部位置和边缘状态
- 用于调试

3.2.2 新增奖励函数

目标点追踪相关：

_reward_tracking_goal_vel()（第 1424-1429 行）
- 目标点速度追踪奖励
- 奖励朝向目标点的速度分量
_reward_tracking_yaw()（第 1671-1673 行）
- 偏航角追踪奖励
- 奖励朝向目标点的偏航角
_reward_reach_all_goal()（第 1685-1689 行）
- 完成所有目标点奖励
- 大额奖励（100.0）

正则化奖励 ：

_reward_delta_torques()（第 1289-1290 行）

力矩变化惩罚
惩罚力矩的剧烈变化

_reward_hip_pos()（第 1297-1298 行）
- 髋关节位置惩罚
- 惩罚髋关节偏离默认位置
_reward_dof_error()（第 1300-1302 行）
- 关节位置误差惩罚
- 惩罚所有关节偏离默认位置
_reward_symmetry()（第 1744-1753 行）
- 对称性惩罚
- 惩罚左右不对称的关节位置

地形相关奖励 ：

_reward_feet_edge()（第 1471-1487 行）

梅花桩边缘惩罚
惩罚脚踩在边缘

_reward_edge_feet_up()（第 1490-1519 行）
- 边缘抬脚奖励（当前禁用）
- 奖励在边缘时抬脚
_reward_feet_height()（第 1691-1729 行）
- 脚高度惩罚（当前禁用）
- 惩罚脚陷入地面

跳跃相关奖励 （当前大部分禁用）：

_reward_jump_up() （第 1431-1436 行）

跳跃奖励
检测垂直速度 > 0.7 m/s

_reward_jump_lift_front_feet()（第 1438-1469 行）
- 跳跃抬前脚奖励
- 奖励前脚向上和向前运动
_reward_foot_above_knee()（第 1521-1540 行）
- 脚高于膝盖奖励
- 奖励跳跃时脚高于膝盖
_reward_foot_above_hip()（第 1542-1568 行）
- 脚高于髋关节奖励
- 奖励跳跃时脚高于髋关节
_reward_jump_pitch()（第 1570-1590 行）
- 跳跃俯仰角奖励
- 奖励跳跃时保持合适的俯仰角（20度）
_reward_jump_preparation()（第 1592-1627 行）
- 跳跃准备奖励
- 奖励前脚力大于后脚力5倍且后脚接触
_reward_air_taitou()（第 1631-1638 行）
- 空中抬头奖励
- 奖励跳跃时向上抬头
_reward_air_foward()（第 1642-1667 行）
- 空中前进奖励
- 奖励跳跃时朝向目标点前进

其他奖励 ：

_reward_stagnation() （第 1384-1386 行）

停滞惩罚（当前禁用）
惩罚速度接近零

_reward_limbo()（第 1388-1403 行）
- 越界惩罚（当前禁用）
- 惩罚超出指定区域
_reward_goal_pos()（第 1405-1410 行）
- 位置奖励（当前禁用）
- 奖励 x 坐标超过阈值
_reward_out_mid()（第 1412-1419 行）
- 偏离中线惩罚（当前禁用）
- 惩罚在波浪地形中偏离中线
_reward_move_back()（第 1421-1422 行）
- 后退惩罚（当前禁用）
- 惩罚向后移动

3.2.3 修改的方法

step()（第 56-91 行）
- 新增：self.global_counter += 1（第 68 行）
- 新增：self.total_env_steps_counter += 1（第 69 行）
- 新增：self.extras["delta_yaw_ok"] = self.delta_yaw < 0.6（第 89 行）
- 移除：噪声添加逻辑（原始版本在 compute_observations() 中添加噪声）

post_physics_step()（第 112-159 行）

新增：欧拉角计算（第 129 行）

python 复制代码

self.roll, self.pitch, self.yaw = euler_from_quaternion(self.base_quat)

新增：接触状态滤波（第 131-133 行）

python 复制代码

contact = torch.norm(self.contact_forces[:, self.feet_indices], dim=-1) > 2.
self.contact_filt = torch.logical_or(contact, self.last_contacts)
self.last_contacts = contact

新增：目标点更新（第 136 行）
python 复制代码
```
self._update_goals()
```

新增：地形类别计算（第 153 行）

python 复制代码

self.env_class = ((self.root_states[:, 0] > 83.5) & (self.root_states[:, 0] < 96.5)).float()

新增：目标点获取（第 145-146 行）

python 复制代码

self.cur_goals = self._gather_cur_goals()
self.next_goals = self._gather_cur_goals(future=1)

check_termination()（第 160-209 行）
- 新增：横滚角终止条件（第 168 行）
  python 复制代码
```
roll_cutoff = torch.abs(self.roll) > 1.5
```
- 新增：俯仰角终止条件（第 169 行）
  python 复制代码
```
pitch_cutoff = torch.abs(self.pitch) > 1.5
```
- 新增：脚部过低终止条件（第 177 行）
  python 复制代码
```
foot_low_cutoff = (torch.sum(foot_height, dim=1) < -1.0) * (self.root_states[:, 0] > 80)
```
- 新增：目标点完成终止条件（第 184 行）
  python 复制代码
```
reach_goal_cutoff = self.cur_goal_idx >= self.cfg.terrain.num_goals
```
- 修改：终止条件合并逻辑（第 196-209 行）
  - 原始版本只有 3 个终止条件
  - DogKing 版本有 6 个终止条件
reset_idx()（第 217-270 行）
- 新增：历史观测缓冲区重置（第 252 行）
  python 复制代码
```
self.obs_history_buf[env_ids, :, :] = 0.
```
- 新增：接触缓冲区重置（第 253 行）
  python 复制代码
```
self.contact_buf[env_ids, :, :] = 0.
```
- 新增：目标点索引重置（第 255-256 行）
  python 复制代码
```
self.cur_goal_idx[env_ids] = 0
self.reach_goal_timer[env_ids] = 0
```
- 新增：last_torques 重置（第 248 行）
  python 复制代码
```
self.last_torques[env_ids] = 0.
```
compute_observations()（第 291-368 行）
- 完全重写：观测结构完全改变
- 新增观测项 ：
  - IMU 观测（roll, pitch）：第 295 行
  - 偏航角误差（delta_yaw, delta_next_yaw）：第 296-298, 315-316 行
  - 地形类别编码：第 319-320 行
  - 接触状态：第 324 行
- 修改：指令观测只保留 x 方向（第 318 行）
  python 复制代码
```
self.commands[:, :1] * self.commands_scale  # 原始版本是 [:3]
```
- 新增：历史观测编码（第 351, 361-368 行）
  python 复制代码
```
self.obs_buf = torch.cat((obs_buf, heights, self.obs_history_buf.view(self.num_envs, -1)), dim=-1)
```
- 移除：噪声添加逻辑（原始版本在观测计算后添加噪声）

create_competition_map()（第 394-450 行）

新增：边缘掩码计算（第 419-420 行）

python 复制代码

self.edge_mask = self.compute_edge_mask(self.terrain.heightsamples, 100).to(self.device)

新增：地面高度张量（第 443-449 行）

python 复制代码

self.ground_height = self.height_samples.unsqueeze(0).repeat(self.num_envs, 1, 1)

修改：梅花桩参数（第 415-416 行）

python 复制代码

stepping_stones_terrain(..., stone_distance=0.25, ...)  # 原始版本是 1.0

_get_heights()（第 1226-1268 行）
- 新增：competition 地形类型处理（第 1249-1250 行）
  python 复制代码
```
if self.cfg.terrain.mesh_type == 'competition':
    points = (points/self.terrain.cfg.horizontal_scale).long()
```
- 修改：高度采样逻辑（第 1265-1266 行）
  - 使用 torch.min 取最小值，而不是平均值

_init_buffers()（第 720-805 行）

新增：历史观测缓冲区（第 768-769 行）

python 复制代码

if self.cfg.env.history_encoding:
    self.obs_history_buf = torch.zeros(self.num_envs, self.cfg.env.history_len, self.cfg.env.n_proprio, ...)

新增：接触缓冲区（第 771 行）

python 复制代码

self.contact_buf = torch.zeros(self.num_envs, self.cfg.env.contact_buf_len, 4, ...)

新增：目标点相关缓冲区（第 765 行）
python 复制代码
```
self.reach_goal_timer = torch.zeros(self.num_envs, ...)
```
新增：last_torques 缓冲区（第 762 行）
python 复制代码
```
self.last_torques = torch.zeros_like(self.torques)
```

_get_env_origins()（第 995-1080 行）
- 新增：目标点序列初始化（第 1002-1009 行）
  python 复制代码
```
tmp_goal = torch.tensor(self.cfg.terrain.coordinates)
tm = tmp_goal.unsqueeze(0).repeat(self.num_envs, 1, 1)
# ... 防止索引越界处理
self.env_goals = torch.cat([tm, last_element_repeated], dim=1)
```
- 新增：目标点索引初始化（第 1022, 1037 行）
  python 复制代码
```
self.cur_goal_idx = torch.zeros(self.num_envs, dtype=torch.long)
```
- 修改：competition 地形初始位置（第 1030-1067 行）
  - 原始版本：x ∈ [6, 48], y ∈ [4, 8]
  - DogKing 版本：x ∈ [1.0, 10.0], y ∈ [1.0, 11.0]
  - 新增波浪地形高度调整（第 1054-1057 行）
  - 新增下楼梯高度调整（第 1063-1064 行）
_draw_debug_vis()（第 1094-1120 行）
- 新增：目标点可视化（第 1103 行）
  python 复制代码
```
self._draw_goals()
```
- 注释：地形边缘可视化（第 1107 行，已注释）
  python 复制代码
```
# self._draw_terrain_edges()
```

四、功能改进详细说明

4.1 目标点追踪系统

4.1.1 问题分析

原始版本的问题：

使用固定速度追踪奖励（tracking_lin_vel），机器狗只需保持固定速度即可获得奖励
在波浪地形中，机器狗容易陷入局部最优：反复在某个区域来回移动刷分，而不是向前推进
缺乏明确的阶段性目标，难以完成长距离复杂地形任务

4.1.2 解决方案设计

核心思想：将长距离任务分解为多个阶段性目标点，机器狗需要依次到达每个目标点。

4.1.3 全局目标点序列定义

配置位置 ：go2_config.py 第 71-161 行

目标点格式 ：每个目标点是一个三元组 [x, y, z]，表示世界坐标系中的位置：

python 复制代码

coordinates = [
    [11.5, 6.0, 0.0],    # 目标点 0：平地起点
    [13.0, 6.0, -0.2],   # 目标点 1：金字塔地形
    [17.5, 6.0, -1.0],   # 目标点 2：金字塔地形
    # ... 共约 30+ 个目标点
    [72.0, 7.85, 4.3],   # 最后一个目标点
]

目标点分布：

平地：目标点 0（起点）
金字塔斜坡：目标点 1-4
随机均匀地形（森林）：目标点 5-8
离散障碍物（矩阵）：目标点 9-15
波浪地形：目标点 16-19
上楼梯：目标点 20-24
下楼梯：目标点 25-27
梅花桩：目标点 28+（当前配置中已注释，可根据需要启用）

目标点数量 ：num_goals = len(coordinates)（约 30 个）

4.1.4 目标点系统初始化

代码位置 ：go2_robot.py 第 995-1044 行，_get_env_origins() 方法

初始化流程：

加载目标点序列（第 1002 行）：

python 复制代码

tmp_goal = torch.tensor(self.cfg.terrain.coordinates)  # shape: (num_goals, 3)

扩展到所有环境（第 1005 行）：
python 复制代码
```
tm = tmp_goal.unsqueeze(0).repeat(self.num_envs, 1, 1)  # shape: (num_envs, num_goals, 3)
```
所有环境共享相同的目标点序列。

防止索引越界（第 1007-1009 行）：

python 复制代码

last_element = tm[:, -1:, :]  # 最后一个目标点
last_element_repeated = last_element.repeat(1, 3, 1)  # 重复3次
self.env_goals = torch.cat([tm, last_element_repeated], dim=1)  # 在末尾添加3个重复点

原因：当机器狗到达最后一个目标点时，cur_goal_idx + 1 或 cur_goal_idx + 2 可能越界，添加重复点可以安全访问。

初始化当前目标点索引（第 1022, 1037 行）：

python 复制代码

self.cur_goal_idx = torch.zeros(self.num_envs, dtype=torch.long)  # 所有环境从索引0开始

初始化目标点相关缓冲区 （第 765 行，_init_buffers()）：
python 复制代码
```
self.reach_goal_timer = torch.zeros(self.num_envs)  # 到达目标点的计时器
```

4.1.5 当前和下一个目标点的获取

代码位置 ：go2_robot.py 第 567-568 行，_gather_cur_goals() 方法

方法实现：

python 复制代码

def _gather_cur_goals(self, future=0):
    return self.env_goals.gather(
        1, 
        (self.cur_goal_idx[:, None, None] + future).expand(-1, -1, self.env_goals.shape[-1])
    ).squeeze(1)

参数说明：

future=0：获取当前目标点（cur_goal_idx 对应的目标点）
future=1：获取下一个目标点（cur_goal_idx + 1 对应的目标点）

调用位置（第 145-146 行）：

python 复制代码

self.cur_goals = self._gather_cur_goals()        # 当前目标点，shape: (num_envs, 3)
self.next_goals = self._gather_cur_goals(future=1)  # 下一个目标点，shape: (num_envs, 3)

数学表示：

当前目标点： p cur = env_goals [ env_id , cur_goal_idx [ env_id ] ] p_{\text{cur}} = \text{env\_goals}[\text{env\_id}, \text{cur\_goal\_idx}[\text{env\_id}]] pcur=env_goals[env_id,cur_goal_idx[env_id]]
下一个目标点： p next = env_goals [ env_id , cur_goal_idx [ env_id ] + 1 ] p_{\text{next}} = \text{env\_goals}[\text{env\_id}, \text{cur\_goal\_idx}[\text{env\_id}] + 1] pnext=env_goals[env_id,cur_goal_idx[env_id]+1]

4.1.6 目标点更新逻辑

代码位置 ：go2_robot.py 第 93-110 行，_update_goals() 方法

更新流程 （每步调用一次，在 post_physics_step() 中）：

检查是否切换到下一个目标点（第 94-96 行）：

python 复制代码

next_flag = self.reach_goal_timer > self.cfg.env.reach_goal_delay / self.dt
self.cur_goal_idx[next_flag] += 1  # 对于所有满足"切换目标"条件的机器人，将它们的目标点索引加1，也就是让它们开始朝向序列中的下一个目标点移动
self.reach_goal_timer[next_flag] = 0  # 对于那些刚刚切换了目标的机器人，将它们的"持续到达"计时器清零。这样，当它们开始接近新的目标点时，计时器就可以重新开始计时。

条件：reach_goal_timer > reach_goal_delay / dt

reach_goal_delay = 0.1 秒（配置：go2_config.py 第 39 行）
需要持续在目标点附近 0.1 秒才切换，避免频繁切换

self.reach_goal_timer: 这是一个计时器张量，记录了每个机器人已经持续"到达"当前目标点多久了。当机器人的位置与目标点的距离小于某个阈值时（这部分判断在后面的代码 self.reached_goal_ids 中），这个计时器就会累加。
self.cfg.env.reach_goal_delay: 这是一个从配置文件读取的参数，代表"确认到达"所需的延迟时间，根据注释来看，这个值通常是0.1秒。
self.dt: 这是每次仿真步长的时间（例如0.02秒）。
self.cfg.env.reach_goal_delay / self.dt: 这个除法计算出了要达到reach_goal_delay这个延迟时间，总共需要多少个仿真步长。例如，如果延迟是0.1秒，步长是0.02秒，那么结果就是5。
整行代码的含义是：判断每个机器人的"持续到达"计时器 (self.reach_goal_timer) 是否已经超过了预设延迟所需要的步数。如果超过了，就意味着这个机器人已经稳定地停留在目标点足够长的时间，可以切换到下一个目标了。next_flag 是一个布尔类型的张量（Tensor），对应位置为 True 的元素就代表该机器人可以切换目标了。

检测是否到达当前目标点（第 98-99 行）：
python 复制代码
```
self.reached_goal_ids = torch.norm(
    self.root_states[:, :2] - self.cur_goals[:, :2], 
    dim=1
) < self.cfg.env.next_goal_threshold
self.reach_goal_timer[self.reached_goal_ids] += 1
```
判断条件 ：机器狗基座位置与目标点的水平距离 < next_goal_threshold
- next_goal_threshold = 0.2 米（配置：go2_config.py 第 38 行）
- 只考虑 XY 平面距离，忽略 Z 轴高度
- 满足条件时，计时器加 1（每步加 1，直到超过延迟阈值）

计算目标方向向量（第 101-110 行）：

python 复制代码

# 当前目标点的相对位置
self.target_pos_rel = self.cur_goals[:, :2] - self.root_states[:, :2]  # shape: (num_envs, 2)
norm = torch.norm(self.target_pos_rel, dim=-1, keepdim=True)
target_vec_norm = self.target_pos_rel / (norm + 1e-5)  # 归一化单位向量
self.target_yaw = torch.atan2(target_vec_norm[:, 1], target_vec_norm[:, 0])  # 目标偏航角

# 下一个目标点的相对位置（用于观测）
self.next_target_pos_rel = self.next_goals[:, :2] - self.root_states[:, :2]
# ... 类似计算 next_target_yaw

数学表示：

相对位置： Δ p = p goal − p base = [ Δ x , Δ y ] \Delta p = p_{\text{goal}} - p_{\text{base}} = [\Delta x, \Delta y] Δp=pgoal−pbase=[Δx,Δy]
单位方向向量： d = Δ p ∥ Δ p ∥ + ϵ d = \frac{\Delta p}{\|\Delta p\| + \epsilon} d=∥Δp∥+ϵΔp
目标偏航角： θ target = atan2 ( Δ y , Δ x ) \theta_{\text{target}} = \text{atan2}(\Delta y, \Delta x) θtarget=atan2(Δy,Δx)

4.1.7 目标点追踪奖励函数

代码位置 ：go2_robot.py 第 1424-1429 行

奖励公式 ：
R tracking_goal_vel = w ⋅ min ⁡ ( ⟨ v , d ⟩ , v c m d ) v c m d + ϵ R_{\text{tracking\goal\vel}} = w \cdot \frac{\min(\langle v, d \rangle, v{cmd})}{v{cmd} + \epsilon} Rtracking_goal_vel=w⋅vcmd+ϵmin(⟨v,d⟩,vcmd)

其中：

v = [ v x , v y ] v = [v_x, v_y] v=[vx,vy]：机器狗当前水平速度（世界坐标系）
d = p goal − p base ∥ p goal − p base ∥ d = \frac{p_{\text{goal}} - p_{\text{base}}}{\|p_{\text{goal}} - p_{\text{base}}\|} d=∥pgoal−pbase∥pgoal−pbase：朝向目标点的单位向量
⟨ v , d ⟩ = v x ⋅ d x + v y ⋅ d y \langle v, d \rangle = v_x \cdot d_x + v_y \cdot d_y ⟨v,d⟩=vx⋅dx+vy⋅dy：速度在目标方向上的投影（点积）
v c m d v_{cmd} vcmd：速度指令（通常为 1.0-1.2 m/s）
w = 2.0 w = 2.0 w=2.0：奖励权重

设计原理：

方向性奖励：只奖励朝向目标点的速度分量，不奖励垂直方向的速度
速度限制 ：奖励上限为 v c m d v_{cmd} vcmd，防止机器狗为了刷分而过度加速
归一化 ：除以 v c m d v_{cmd} vcmd 使奖励范围在 [ 0 , 1 ] [0, 1] [0,1]，便于调参

代码实现：

python 复制代码

def _reward_tracking_goal_vel(self):
    norm = torch.norm(self.target_pos_rel, dim=-1, keepdim=True)
    target_vec_norm = self.target_pos_rel / (norm + 1e-5)  # 单位向量
    cur_vel = self.root_states[:, 7:9]  # 当前水平速度
    rew = torch.minimum(
        torch.sum(target_vec_norm * cur_vel, dim=-1),  # 速度投影
        self.commands[:, 0]  # 速度指令
    ) / (self.commands[:, 0] + 1e-5)
    return rew

4.1.8 完成所有目标点奖励

代码位置 ：go2_robot.py 第 1685-1689 行

奖励公式 ：
R reach_all_goal = w ⋅ 1 cur_goal_idx ≥ num_goals ⋅ 1 reset R_{\text{reach\_all\goal}} = w \cdot \mathbf{1}{\text{cur\_goal\_idx} \geq \text{num\goals}} \cdot \mathbf{1}{\text{reset}} Rreach_all_goal=w⋅1cur_goal_idx≥num_goals⋅1reset

其中：

w = 100.0 w = 100.0 w=100.0：大额奖励权重
1 cur_goal_idx ≥ num_goals \mathbf{1}_{\text{cur\_goal\_idx} \geq \text{num\_goals}} 1cur_goal_idx≥num_goals：完成所有目标点的指示函数
1 reset \mathbf{1}_{\text{reset}} 1reset：回合结束的指示函数（只在回合结束时给予奖励）

设计原理：

只在回合结束时检查是否完成所有目标点，避免中途给予奖励导致训练不稳定
大额奖励（100.0）强化成功经验，鼓励机器狗完成整个任务序列

代码实现：

python 复制代码

def _reward_reach_all_goal(self):
    rew = (self.cur_goal_idx >= self.cfg.terrain.num_goals).float()
    return self.reset_buf * rew  # 只在重置时给予奖励

4.1.9 目标点系统总结

优势：

✅ 避免局部最优：通过阶段性目标引导机器狗向前推进
✅ 任务分解：将长距离任务分解为多个小目标，降低学习难度
✅ 方向性奖励：只奖励朝向目标的速度，避免来回刷分
✅ 阶段性训练：可以通过调整目标点数量实现课程学习

关键参数：

next_goal_threshold = 0.2 米：到达目标点的判定距离
reach_goal_delay = 0.1 秒：切换目标点的延迟时间
tracking_goal_vel = 2.0：目标点追踪奖励权重
reach_all_goal = 100.0：完成所有目标点奖励权重

代码位置总结：

配置：go2_config.py 第 38-39, 71-162, 253-255 行
初始化：go2_robot.py 第 765, 1002-1044 行
更新逻辑：go2_robot.py 第 93-110, 136, 145-146 行
奖励函数：go2_robot.py 第 1424-1429, 1685-1689 行

4.2 历史观测编码

4.2.1 问题分析

原始版本的问题：

观测空间只包含当前时刻的状态，机器狗缺乏记忆能力
难以完成需要时序规划的动作，如：
- 跨越梅花桩：需要提前准备跳跃姿态，在空中调整身体
- 翻越障碍物：需要提前减速、调整重心
- 复杂地形适应：需要根据历史接触信息调整步态

4.2.2 解决方案设计

核心思想 ：保存过去 T T T 步的观测历史，使机器狗能够学习时序依赖关系。

4.2.3 历史观测配置

代码位置 ：go2_config.py 第 7-10, 22 行

配置参数：

python 复制代码

num_obs = 275  # 基础观测维度（包含地形高度）
history_len = 10  # 历史长度：保存过去10步
n_proprio = 55  # 本体感受观测维度（不包括地形高度）
num_observations = num_obs + history_len * n_proprio  # 825 维
history_encoding = True  # 启用历史编码

维度计算：

基础观测：275 维（包含 231 维地形高度）
本体感受观测 ： 275 − 231 = 44 275 - 231 = 44 275−231=44 维（不包括地形高度）
历史观测 ： 10 × 55 = 550 10 \times 55 = 550 10×55=550 维
- 注意：n_proprio = 55 可能包含一些额外的观测项
总维度 ： 275 + 550 = 825 275 + 550 = 825 275+550=825 维

4.2.4 历史观测缓冲区初始化

代码位置 ：go2_robot.py 第 768-769 行，_init_buffers() 方法

初始化代码：

python 复制代码

if self.cfg.env.history_encoding:
    self.obs_history_buf = torch.zeros(
        self.num_envs, 
        self.cfg.env.history_len,  # 10
        self.cfg.env.n_proprio,    # 55
        device=self.device, 
        dtype=torch.float
    )

缓冲区形状 ：(num_envs, history_len, n_proprio) = (4096, 10, 55)

4.2.5 历史观测更新逻辑

代码位置 ：go2_robot.py 第 291-368 行，compute_observations() 方法

更新流程：

计算当前时刻的基础观测（第 312-330 行）：

python 复制代码

obs_buf = torch.cat((
    self.base_lin_vel * self.obs_scales.lin_vel,      # 3 维
    self.base_ang_vel * self.obs_scales.ang_vel,      # 3 维
    imu_obs,                                           # 2 维
    self.delta_yaw[:, None],                           # 1 维
    self.delta_next_yaw[:, None],                      # 1 维
    self.commands[:, :1] * self.commands_scale,       # 1 维
    (self.env_class != 0).float(),                    # 1 维
    (self.env_class == 0).float(),                     # 1 维
    (self.dof_pos - self.default_dof_pos) * ...,      # 12 维
    self.dof_vel * ...,                                # 12 维
    self.actions,                                      # 12 维
    self.contact_filt.float() - 0.5,                  # 4 维
), dim=-1)  # 基础观测：约 55 维（不包括地形高度）

添加地形高度（第 348-351 行）：

python 复制代码

if self.cfg.terrain.measure_heights:
    heights = ...  # 231 维地形高度
    obs_buf = torch.cat((obs_buf, heights, self.obs_history_buf.view(self.num_envs, -1)), dim=-1)

注意：历史观测中不包括地形高度，只包括本体感受信息。

更新历史缓冲区（第 361-368 行）：

python 复制代码

self.obs_history_buf = torch.where(
    (self.episode_length_buf <= 1)[:, None, None],  # 如果是新回合
    torch.stack([obs_buf] * self.cfg.env.history_len, dim=1),  # 用当前观测填充所有历史
    torch.cat([
        self.obs_history_buf[:, 1:],      # 移除最旧的观测（第0步）
        obs_buf.unsqueeze(1)              # 添加当前观测（第9步）
    ], dim=1)
)

更新逻辑说明：

新回合：用当前观测填充所有历史位置（所有历史都是当前观测）
正常更新：滑动窗口，移除最旧的观测，添加最新的观测
历史顺序 ：obs_history_buf[:, 0] 是最旧的，obs_history_buf[:, 9] 是最新的

数学表示：

历史观测： H t = [ o t − 9 , o t − 8 , ... , o t − 1 ] H_t = [o_{t-9}, o_{t-8}, \ldots, o_{t-1}] Ht=[ot−9,ot−8,...,ot−1]（不包括地形高度）
当前观测： O t = [ o proprio , h terrain , H t ] O_t = [o_{\text{proprio}}, h_{\text{terrain}}, H_t] Ot=[oproprio,hterrain,Ht]（展平后的完整观测）

4.2.6 历史观测在观测空间中的位置

完整观测结构（第 351 行）：

python 复制代码

self.obs_buf = torch.cat((
    obs_buf,                              # 275 维：当前时刻完整观测（包括地形高度）
    self.obs_history_buf.view(self.num_envs, -1)  # 550 维：历史观测（展平）
), dim=-1)

观测维度分布：

当前时刻本体感受：~44 维（不包括地形高度）
当前时刻地形高度：231 维
历史观测：550 维（10 步 × 55 维，不包括地形高度）

总维度 ： 44 + 231 + 550 = 825 44 + 231 + 550 = 825 44+231+550=825 维

4.2.7 历史观测的作用

为什么历史观测不包括地形高度：

维度控制 ：地形高度有 231 维，如果包含在历史中，总维度会达到 275 × 10 = 2750 275 \times 10 = 2750 275×10=2750 维，计算开销过大
信息冗余：地形高度变化相对缓慢，当前时刻的地形信息已经足够
重点信息：历史观测主要关注机器狗自身的状态变化（速度、姿态、动作等）

历史观测帮助机器狗学习：

动作序列：过去 10 步的动作序列，帮助学习连贯的动作模式
速度变化：速度的历史变化，帮助预测和控制
接触模式：脚部接触的历史模式，帮助调整步态
姿态变化：身体姿态的历史变化，帮助完成复杂动作（如跳跃）

4.2.8 历史观测编码总结

优势：

✅ 记忆能力：使机器狗能够学习时序依赖关系
✅ 动作规划：帮助完成需要提前准备的动作（如跳跃）
✅ 步态协调：通过历史接触信息优化步态
✅ 维度平衡：通过排除地形高度，在记忆能力和计算效率之间取得平衡

关键参数：

history_len = 10：历史长度（约 0.2 秒，假设 dt=0.02s）
n_proprio = 55：本体感受观测维度
history_encoding = True：启用历史编码

代码位置总结：

配置：go2_config.py 第 7-10, 22 行
初始化：go2_robot.py 第 768-769 行
更新逻辑：go2_robot.py 第 351, 361-368 行
重置逻辑：go2_robot.py 第 252 行（重置时清零）

4.3 地形边缘检测

4.3.1 问题分析

原始版本的问题：

在梅花桩区域，机器狗会通过蹬梅花桩的边缘获得不当的跳跃速度
虽然这种方式可以获得速度，但：
- 不稳定：边缘接触面积小，容易滑落
- 不可控：蹬边缘产生的力方向不确定
- 不安全：可能导致机器狗失去平衡

4.3.2 解决方案设计

核心思想：检测地形边缘，当脚踩在边缘且处于接触状态时给予惩罚，鼓励机器狗踩在梅花桩中心。

4.3.3 边缘检测算法

代码位置 ：go2_robot.py 第 371-392 行，compute_edge_mask() 方法

检测原理 ：

通过计算高度场的梯度（高度差）来检测边缘。如果相邻像素的高度差超过阈值，则认为是边缘。

算法实现：

python 复制代码

def compute_edge_mask(self, height_field, threshold):
    # 计算 X 方向的高度差
    diff_x = np.abs(height_field[:, 1:] - height_field[:, :-1])  # shape: (rows, cols-1)
    
    # 计算 Y 方向的高度差
    diff_y = np.abs(height_field[1:, :] - height_field[:-1, :])  # shape: (rows-1, cols)
    
    # 初始化边缘掩码
    edge_mask = np.zeros_like(height_field, dtype=bool)
    
    # 标记 X 方向的边缘
    edge_mask[:, 1:] |= (diff_x > threshold)   # 右边缘
    edge_mask[:, :-1] |= (diff_x > threshold)  # 左边缘
    
    # 标记 Y 方向的边缘
    edge_mask[1:, :] |= (diff_y > threshold)   # 下边缘
    edge_mask[:-1, :] |= (diff_y > threshold)  # 上边缘
    
    return torch.tensor(edge_mask)

数学表示：

X 方向梯度： Δ x h i , j = ∣ h i , j + 1 − h i , j ∣ \Delta_x h_{i,j} = |h_{i,j+1} - h_{i,j}| Δxhi,j=∣hi,j+1−hi,j∣
Y 方向梯度： Δ y h i , j = ∣ h i + 1 , j − h i , j ∣ \Delta_y h_{i,j} = |h_{i+1,j} - h_{i,j}| Δyhi,j=∣hi+1,j−hi,j∣
边缘判定： edge_mask i , j = ( Δ x h i , j > θ ) ∨ ( Δ y h i , j > θ ) \text{edge\mask}{i,j} = (\Delta_x h_{i,j} > \theta) \lor (\Delta_y h_{i,j} > \theta) edge_maski,j=(Δxhi,j>θ)∨(Δyhi,j>θ)
其中 θ = 100 \theta = 100 θ=100（阈值，单位：高度场像素值）

阈值说明：

threshold = 100：高度场像素值的差异阈值
考虑到 vertical_scale = 0.005 米/像素，实际高度差为 100 × 0.005 = 0.5 100 \times 0.005 = 0.5 100×0.005=0.5 米
这个阈值可以检测梅花桩的边缘（高度差约 0.2 米）和台阶的边缘

4.3.4 边缘掩码的创建和应用

代码位置 ：go2_robot.py 第 419-420 行，create_competition_map() 方法

创建时机：在地形创建时计算一次边缘掩码，后续直接使用。

创建代码：

python 复制代码

# 计算 edge_mask
self.edge_mask = self.compute_edge_mask(
    self.terrain.heightsamples, 
    threshold=100
).to(self.device)  # shape: (tot_rows, tot_cols)

存储：self.edge_mask 是一个布尔张量，形状与高度场相同。

4.3.5 脚部边缘检测

代码位置 ：go2_robot.py 第 1471-1487 行，_reward_feet_edge() 方法

检测流程：

获取脚部位置（第 1472 行）：

python 复制代码

foot_pos_xy = self.rigid_body_states[:, self.feet_indices, :2]  # shape: (num_envs, 4, 2)

转换为高度场索引（第 1476-1480 行）：

python 复制代码

indices = (foot_pos_xy / 0.25).long()  # 除以 horizontal_scale
indices[:, :, 0] = indices[:, :, 0].clamp(0, self.height_samples.shape[0] - 1)  # 限制 X 索引
indices[:, :, 1] = indices[:, :, 1].clamp(0, self.height_samples.shape[1] - 1)  # 限制 Y 索引

坐标转换：世界坐标 → 高度场索引

horizontal_scale = 0.25 米/像素
例如：世界坐标 ( 10.0 , 6.0 ) (10.0, 6.0) (10.0,6.0) → 索引 ( 40 , 24 ) (40, 24) (40,24)

检查是否在边缘（第 1482 行）：
python 复制代码
```
feet_at_edge = self.edge_mask[indices[:, :, 0], indices[:, :, 1]]  # shape: (num_envs, 4)
```
对每只脚，检查其对应的高度场像素是否被标记为边缘。
结合接触状态（第 1484 行）：
python 复制代码
```
self.feet_at_edge = self.contact_filt & feet_at_edge
```
条件：脚必须同时满足：
- 处于接触状态（contact_filt = True）
- 踩在边缘上（feet_at_edge = True）

4.3.6 边缘惩罚奖励函数

代码位置 ：go2_robot.py 第 1471-1487 行

奖励公式 ：
R feet_edge = w ⋅ 1 x > 83.5 ⋅ ∑ i = 1 4 1 foot i at edge ⋅ 1 contact i R_{\text{feet\edge}} = w \cdot \mathbf{1}{x > 83.5} \cdot \sum_{i=1}^{4} \mathbf{1}_{\text{foot}i \text{ at edge}} \cdot \mathbf{1}{\text{contact}_i} Rfeet_edge=w⋅1x>83.5⋅i=1∑41footi at edge⋅1contacti

其中：

w = − 1.5 w = -1.5 w=−1.5：惩罚权重
1 x > 83.5 \mathbf{1}_{x > 83.5} 1x>83.5：只在梅花桩区域生效（ x > 83.5 x > 83.5 x>83.5 米）
1 foot i at edge \mathbf{1}_{\text{foot}_i \text{ at edge}} 1footi at edge：第 i i i 只脚踩在边缘
1 contact i \mathbf{1}_{\text{contact}_i} 1contacti：第 i i i 只脚处于接触状态

设计原理：

区域限制：只在梅花桩区域生效，不影响其他地形的行为
接触条件：只惩罚实际接触边缘的脚，不惩罚悬空的脚
累积惩罚：多只脚踩边缘时，惩罚累加

代码实现：

python 复制代码

def _reward_feet_edge(self):
    foot_pos_xy = self.rigid_body_states[:, self.feet_indices, :2]
    meihuazhuang = self.root_states[:, 0] > 83.5  # 梅花桩区域
    
    # 转换为索引并检查边缘
    indices = (foot_pos_xy / 0.25).long()
    indices[:, :, 0] = indices[:, :, 0].clamp(0, self.height_samples.shape[0] - 1)
    indices[:, :, 1] = indices[:, :, 1].clamp(0, self.height_samples.shape[1] - 1)
    
    feet_at_edge = self.edge_mask[indices[:, :, 0], indices[:, :, 1]]
    self.feet_at_edge = self.contact_filt & feet_at_edge
    
    rew = meihuazhuang * torch.sum(self.feet_at_edge, dim=-1)  # 累加所有脚的惩罚
    return rew

4.3.7 边缘检测总结

优势：

✅ 防止不当行为：避免机器狗通过蹬边缘获得不当速度
✅ 提高稳定性：鼓励踩在梅花桩中心，提高接触稳定性
✅ 区域特定：只在梅花桩区域生效，不影响其他地形
✅ 计算高效：边缘掩码只需计算一次，后续直接查询

关键参数：

threshold = 100：边缘检测阈值（高度场像素值）
feet_edge = -1.5：边缘惩罚权重
梅花桩区域： x ∈ [ 83.5 , 96.5 ] x \in [83.5, 96.5] x∈[83.5,96.5] 米

代码位置总结：

边缘检测：go2_robot.py 第 371-392 行
掩码创建：go2_robot.py 第 419-420 行
奖励函数：go2_robot.py 第 1471-1487 行

4.4 观测空间扩展

4.4.1 观测空间对比

原始版本观测维度：235 维

基座线速度：3 维
基座角速度：3 维
投影重力：3 维
指令：3 维
关节位置误差：12 维
关节速度：12 维
上一时刻动作：12 维
地形高度：187 维

DogKing 版本观测维度：275 维（基础）+ 550 维（历史）= 825 维

4.4.2 新增观测项详解

代码位置 ：go2_robot.py 第 291-330 行，compute_observations() 方法

1. IMU 观测（2 维）

代码位置：第 295 行

实现：

python 复制代码

imu_obs = torch.stack((self.roll, self.pitch), dim=1)  # shape: (num_envs, 2)

计算方式 （第 24-44 行，euler_from_quaternion()）：

从基座四元数提取欧拉角：

roll：绕 X 轴旋转角度（横滚角）
pitch：绕 Y 轴旋转角度（俯仰角）
yaw：绕 Z 轴旋转角度（偏航角，不包含在 IMU 观测中）

数学表示：

四元数： q = [ q x , q y , q z , q w ] q = [q_x, q_y, q_z, q_w] q=[qx,qy,qz,qw]
Roll： ϕ = atan2 ( 2 ( w x + y z ) , 1 − 2 ( x 2 + y 2 ) ) \phi = \text{atan2}(2(wx + yz), 1 - 2(x^2 + y^2)) ϕ=atan2(2(wx+yz),1−2(x2+y2))
Pitch： θ = arcsin ⁡ ( clip ( 2 ( w y − z x ) , − 1 , 1 ) ) \theta = \arcsin(\text{clip}(2(wy - zx), -1, 1)) θ=arcsin(clip(2(wy−zx),−1,1))

作用：提供机器狗的姿态信息，帮助判断是否倾斜或翻倒。

2. 偏航角误差（2 维）

代码位置：第 296-298, 315-316 行

实现：

python 复制代码

if self.global_counter % 5 == 0:  # 每5步更新一次
    self.delta_yaw = self.target_yaw - self.yaw        # 当前目标点偏航角误差
    self.delta_next_yaw = self.next_target_yaw - self.yaw  # 下一个目标点偏航角误差

# 在观测中
self.delta_yaw[:, None],        # 1 维
self.delta_next_yaw[:, None],   # 1 维

计算方式：

target_yaw：朝向当前目标点的偏航角（第 106 行）
next_target_yaw：朝向下一个目标点的偏航角（第 110 行）
yaw：当前机器狗的偏航角（从四元数提取）
delta_yaw = target_yaw - yaw：需要调整的偏航角

数学表示：

目标偏航角： θ target = atan2 ( Δ y , Δ x ) \theta_{\text{target}} = \text{atan2}(\Delta y, \Delta x) θtarget=atan2(Δy,Δx)，其中 Δ p = p goal − p base \Delta p = p_{\text{goal}} - p_{\text{base}} Δp=pgoal−pbase
当前偏航角： θ current \theta_{\text{current}} θcurrent（从四元数提取）
偏航角误差： Δ θ = wrap_to_pi ( θ target − θ current ) \Delta \theta = \text{wrap\to\pi}(\theta{\text{target}} - \theta{\text{current}}) Δθ=wrap_to_pi(θtarget−θcurrent)

作用：

告诉机器狗需要调整多少偏航角才能朝向目标点
delta_next_yaw 帮助机器狗提前规划，在接近当前目标点时就开始转向下一个目标点

更新频率 ：每 5 步更新一次（global_counter % 5 == 0），减少计算开销。

3. 地形类别编码（2 维）

代码位置：第 153, 319-320 行

实现：

python 复制代码

# 计算地形类别（第 153 行）
self.env_class = ((self.root_states[:, 0] > 83.5) & 
                   (self.root_states[:, 0] < 96.5)).float().unsqueeze(0).reshape(self.num_envs, -1)

# 在观测中（第 319-320 行）
(self.env_class != 0).float(),  # 1 维：是否在梅花桩区域
(self.env_class == 0).float(),  # 1 维：是否不在梅花桩区域

编码方式：

梅花桩区域： x ∈ [ 83.5 , 96.5 ] x \in [83.5, 96.5] x∈[83.5,96.5] 米
(env_class != 0).float()：在梅花桩区域时为 1.0，否则为 0.0
(env_class == 0).float()：不在梅花桩区域时为 1.0，否则为 0.0

作用：

帮助机器狗识别当前地形类型
可以针对不同地形采用不同策略（如梅花桩区域需要跳跃，其他区域正常行走）

扩展性：可以扩展为多类别编码，标识更多地形类型。

4. 接触状态（4 维）

代码位置：第 131-133, 324 行

实现：

python 复制代码

# 检测接触（第 131-133 行）
contact = torch.norm(self.contact_forces[:, self.feet_indices], dim=-1) > 2.0
self.contact_filt = torch.logical_or(contact, self.last_contacts)  # 滤波后的接触状态
self.last_contacts = contact

# 在观测中（第 324 行）
self.contact_filt.float() - 0.5,  # shape: (num_envs, 4)

计算方式：

接触力阈值：2.0 N（如果脚部接触力 > 2.0 N，认为处于接触状态）
接触滤波：使用 logical_or(contact, last_contacts) 进行滤波，避免接触检测的抖动
编码：contact_filt - 0.5，接触时为 0.5，不接触时为 -0.5

作用：

告诉机器狗哪些脚正在接触地面
帮助机器狗在空中调整姿态（如跳跃时）
帮助优化步态（如确保至少两只脚接触地面）

4.4.3 观测空间结构总结

完整观测结构（按顺序）：

观测项	维度	说明
基座线速度	3	当前水平速度
基座角速度	3	当前角速度
IMU 观测	2	roll, pitch（新增）
偏航角误差	2	delta_yaw, delta_next_yaw（新增）
指令	1	速度指令（只保留 x 方向）
地形类别	2	是否在梅花桩区域（新增）
关节位置误差	12	相对于默认姿态的偏差
关节速度	12	关节角速度
上一时刻动作	12	用于动作平滑
接触状态	4	四条腿的接触状态（新增）
地形高度	231	前方地形高度测量
历史观测	550	过去 10 步的观测历史（新增）

总维度 ： 3 + 3 + 2 + 2 + 1 + 2 + 12 + 12 + 12 + 4 + 231 + 550 = 825 3 + 3 + 2 + 2 + 1 + 2 + 12 + 12 + 12 + 4 + 231 + 550 = 825 3+3+2+2+1+2+12+12+12+4+231+550=825 维

4.4.4 观测空间扩展的优势

✅ 姿态感知：IMU 观测提供姿态信息，帮助判断稳定性
✅ 方向引导：偏航角误差直接告诉机器狗需要调整的方向
✅ 地形识别：地形类别帮助机器狗针对不同地形采用不同策略
✅ 接触感知：接触状态帮助机器狗在空中调整姿态
✅ 时序记忆：历史观测使机器狗能够学习时序依赖关系

代码位置总结：

IMU 观测：go2_robot.py 第 24-44, 295 行
偏航角误差：go2_robot.py 第 296-298, 315-316 行
地形类别：go2_robot.py 第 153, 319-320 行
接触状态：go2_robot.py 第 131-133, 324 行

五、数学公式对比

5.1 速度追踪奖励

原始版本 ：
R track_lin_vel = w ⋅ exp ⁡ ( − ∥ v cmd − v base ∥ 2 σ 2 ) R_{\text{track\lin\vel}} = w \cdot \exp\left(-\frac{\|v{\text{cmd}} - v{\text{base}}\|^2}{\sigma^2}\right) Rtrack_lin_vel=w⋅exp(−σ2∥vcmd−vbase∥2)

DogKing 版本 ：
R tracking_goal_vel = w ⋅ min ⁡ ( ⟨ v , d ⟩ , v c m d ) v c m d + ϵ R_{\text{tracking\goal\vel}} = w \cdot \frac{\min(\langle v, d \rangle, v{cmd})}{v{cmd} + \epsilon} Rtracking_goal_vel=w⋅vcmd+ϵmin(⟨v,d⟩,vcmd)

其中 d = p goal − p base ∥ p goal − p base ∥ d = \frac{p_{\text{goal}} - p_{\text{base}}}{\|p_{\text{goal}} - p_{\text{base}}\|} d=∥pgoal−pbase∥pgoal−pbase 是朝向目标点的单位向量。

5.2 观测维度

原始版本 ：
obs_dim = 3 + 3 + 3 + 3 + 12 + 12 + 12 + 187 = 235 \text{obs\_dim} = 3 + 3 + 3 + 3 + 12 + 12 + 12 + 187 = 235 obs_dim=3+3+3+3+12+12+12+187=235

DogKing 版本 ：
obs_dim = 3 + 3 + 2 + 2 + 1 + 2 + 12 + 12 + 12 + 4 + 231 = 275 \text{obs\_dim} = 3 + 3 + 2 + 2 + 1 + 2 + 12 + 12 + 12 + 4 + 231 = 275 obs_dim=3+3+2+2+1+2+12+12+12+4+231=275
total_dim = 275 + 10 × 55 = 825 \text{total\_dim} = 275 + 10 \times 55 = 825 total_dim=275+10×55=825

六、训练策略改进

6.1 阶段性训练

原始版本：固定奖励函数，通过课程学习逐步增加难度。

DogKing 版本：

通过目标点序列控制训练难度
完成所有目标点后提前终止并给予大额奖励
逐步增加目标点数量和难度

6.2 回合长度

原始版本 ：episode_length_s = 25 秒

DogKing 版本 ：episode_length_s = 200 秒

原因：适应更长的任务序列和更复杂的地形。

七、奖励函数详细说明

8.1 正则化奖励函数

8.1.1 `_reward_delta_torques()`

代码位置 ：go2_robot.py 第 1289-1290 行

奖励公式 ：
R delta_torques = w ⋅ ∑ i = 1 12 ( τ i − τ i , last ) 2 R_{\text{delta\torques}} = w \cdot \sum{i=1}^{12} (\tau_i - \tau_{i,\text{last}})^2 Rdelta_torques=w⋅i=1∑12(τi−τi,last)2

其中：

τ i \tau_i τi：第 i i i 个关节的当前力矩
τ i , last \tau_{i,\text{last}} τi,last：第 i i i 个关节的上一步力矩
w = − 1.0 × 10 − 7 w = -1.0 \times 10^{-7} w=−1.0×10−7：惩罚权重

设计原理：惩罚力矩的剧烈变化，鼓励平滑控制，减少机械磨损和能量消耗。

8.1.2 `_reward_hip_pos()`

代码位置 ：go2_robot.py 第 1297-1298 行

奖励公式 ：
R hip_pos = w ⋅ ∑ i = 1 4 ( q hip , i − q hip , i , default ) 2 R_{\text{hip\pos}} = w \cdot \sum{i=1}^{4} (q_{\text{hip},i} - q_{\text{hip},i,\text{default}})^2 Rhip_pos=w⋅i=1∑4(qhip,i−qhip,i,default)2

其中：

q hip , i q_{\text{hip},i} qhip,i：第 i i i 个髋关节的当前角度
q hip , i , default q_{\text{hip},i,\text{default}} qhip,i,default：第 i i i 个髋关节的默认角度
w = − 0.6 w = -0.6 w=−0.6：惩罚权重

设计原理：鼓励髋关节保持在默认位置附近，避免过度外展或内收。

8.1.3 `_reward_dof_error()`

代码位置 ：go2_robot.py 第 1300-1302 行

奖励公式 ：
R dof_error = w ⋅ ∑ i = 1 12 ( q i − q i , default ) 2 R_{\text{dof\error}} = w \cdot \sum{i=1}^{12} (q_i - q_{i,\text{default}})^2 Rdof_error=w⋅i=1∑12(qi−qi,default)2

其中：

q i q_i qi：第 i i i 个关节的当前角度
q i , default q_{i,\text{default}} qi,default：第 i i i 个关节的默认角度
w = − 0.001 w = -0.001 w=−0.001：惩罚权重

设计原理：鼓励所有关节保持在默认位置附近，维持稳定的站立姿态。

8.1.4 `_reward_symmetry()`

代码位置 ：go2_robot.py 第 1744-1753 行

奖励公式 ：
R symmetry = w ⋅ [ ∑ j ∈ { hip , thigh , calf } ( ( q j , FR − q j , RL ) 2 + ( q j , FL − q j , RR ) 2 ) ] R_{\text{symmetry}} = w \cdot \left[\sum_{j \in \{\text{hip}, \text{thigh}, \text{calf}\}} \left((q_{j,\text{FR}} - q_{j,\text{RL}})^2 + (q_{j,\text{FL}} - q_{j,\text{RR}})^2\right)\right] Rsymmetry=w⋅ j∈{hip,thigh,calf}∑((qj,FR−qj,RL)2+(qj,FL−qj,RR)2)

其中：

q j , FR q_{j,\text{FR}} qj,FR：右前腿关节 j j j 的角度
q j , RL q_{j,\text{RL}} qj,RL：左后腿关节 j j j 的角度
w = − 0.5 w = -0.5 w=−0.5：惩罚权重

设计原理：鼓励左右对称的步态，提高行走稳定性和能量效率。

8.2 跳跃相关奖励函数（当前大部分禁用）

这些奖励函数主要用于训练机器狗在梅花桩区域的跳跃能力，但当前配置中权重都设为 0.0，表示未启用。

8.2.1 `_reward_jump_up()`

代码位置 ：go2_robot.py 第 1431-1436 行

奖励公式 ：
R jump_up = w ⋅ 1 v z > 0.7 ⋅ 1 x > 83.5 R_{\text{jump\up}} = w \cdot \mathbf{1}{v_z > 0.7} \cdot \mathbf{1}_{x > 83.5} Rjump_up=w⋅1vz>0.7⋅1x>83.5

其中：

v z v_z vz：基座垂直速度
x > 83.5 x > 83.5 x>83.5：梅花桩区域
w = 0.0 w = 0.0 w=0.0（当前禁用）

设计原理：奖励垂直速度大于 0.7 m/s 的跳跃动作。

8.2.2 `_reward_jump_preparation()`

代码位置 ：go2_robot.py 第 1592-1627 行

奖励公式 ：
R jump_prep = w ⋅ 1 F front > 5 F rear ⋅ 1 rear_contact ⋅ 1 v x > 0.1 ⋅ 1 ω y > 0.1 ⋅ 1 x > 83.5 R_{\text{jump\prep}} = w \cdot \mathbf{1}{F_{\text{front}} > 5F_{\text{rear}}} \cdot \mathbf{1}{\text{rear\contact}} \cdot \mathbf{1}{v_x > 0.1} \cdot \mathbf{1}{\omega_y > 0.1} \cdot \mathbf{1}_{x > 83.5} Rjump_prep=w⋅1Ffront>5Frear⋅1rear_contact⋅1vx>0.1⋅1ωy>0.1⋅1x>83.5

其中：

F front F_{\text{front}} Ffront：前脚平均接触力
F rear F_{\text{rear}} Frear：后脚平均接触力
w = 0.0 w = 0.0 w=0.0（当前禁用）

设计原理：奖励跳跃准备动作：前脚用力、后脚接触、向前移动、向上抬头。

8.3 其他奖励函数

8.3.1 `_reward_stagnation()`

代码位置 ：go2_robot.py 第 1384-1386 行

奖励公式 ：
R stagnation = w ⋅ 1 v ˉ < 5 × 10 − 4 R_{\text{stagnation}} = w \cdot \mathbf{1}_{\bar{v} < 5 \times 10^{-4}} Rstagnation=w⋅1vˉ<5×10−4

其中：

v ˉ = 1 3 ∑ i = 1 3 v i 2 \bar{v} = \frac{1}{3}\sum_{i=1}^{3} v_i^2 vˉ=31∑i=13vi2：平均速度平方
w = − 0.0 w = -0.0 w=−0.0（当前禁用）

设计原理：惩罚速度接近零的停滞状态。

8.3.2 `_reward_limbo()`

代码位置 ：go2_robot.py 第 1388-1403 行

奖励公式 ：
R limbo = w ⋅ [ 1 y > 11.5 ∨ y < 0.5 ∨ 1 ( 51 < x < 53 ∨ 57 < x < 59 ) ∧ ( ( 2 < y < 4 ) ∨ ( 8 < y < 10 ) ) ] R_{\text{limbo}} = w \cdot \left[\mathbf{1}{y > 11.5 \lor y < 0.5} \lor \mathbf{1}{(51 < x < 53 \lor 57 < x < 59) \land ((2 < y < 4) \lor (8 < y < 10))}\right] Rlimbo=w⋅[1y>11.5∨y<0.5∨1(51<x<53∨57<x<59)∧((2<y<4)∨(8<y<10))]

其中：

w = − 0.0 w = -0.0 w=−0.0（当前禁用）

设计原理：惩罚超出指定区域的行为。

九、总结

9.1 主要改进点

✅ 目标点追踪系统：替代固定速度追踪，避免局部最优
✅ 历史观测编码：增加记忆能力，提升复杂动作完成度
✅ 地形边缘检测：防止不当行为，提高稳定性
✅ 观测空间扩展：新增偏航角误差、地形类别、接触状态等
✅ 阶段性训练：通过目标点序列实现渐进式训练
✅ 奖励函数扩展：新增 20+ 个奖励函数，覆盖更多行为模式

9.2 修改范围

主要修改 ：legged_gym/legged_gym/envs/go2/ 目录下的所有文件
次要修改 ：
- isaacgym/python/examples/terrain_creation.py
- isaacgym/python/isaacgym/terrain_utils.py
- rsl_rl/rsl_rl/runners/on_policy_runner.py

9.3 关键参数总结

参数类别	关键参数	原始值	DogKing 值
观测空间	`num_observations`	235	825
历史编码	`history_len`	-	10
回合长度	`episode_length_s`	25	200
速度指令	`lin_vel_x`	[0.5, 2.0]	[1.0, 1.2]
目标点	`num_goals`	-	~30
测量点	`measured_points_x`	17个	20个

9.4 建议

根据实际训练效果调整奖励权重，特别是当前禁用的跳跃相关奖励
可以通过调整 num_goals 实现课程学习，逐步增加目标点数量
历史观测长度 history_len 可以根据任务复杂度调整（当前为 10 步）
边缘检测阈值可以根据实际地形调整（当前为 100 像素值）

机器人强化学习入门笔记（四）

DogKing 项目与 legged_robot_competition 项目差异说明

一、项目概述

二、目录结构对比

2.1 主要修改目录

2.2 其他目录

三、核心代码差异

3.1 配置文件差异（go2_config.py）

3.1.1 观测空间配置

3.1.2 地形测量点配置

3.1.3 奖励权重配置

3.1.4 其他配置差异

3.2 环境实现差异（go2_robot.py）

3.2.1 新增方法

3.2.2 新增奖励函数

3.2.3 修改的方法

四、功能改进详细说明

4.1 目标点追踪系统

4.1.1 问题分析

4.1.2 解决方案设计

4.1.3 全局目标点序列定义

4.1.4 目标点系统初始化

4.1.5 当前和下一个目标点的获取

4.1.6 目标点更新逻辑

4.1.7 目标点追踪奖励函数

4.1.8 完成所有目标点奖励

4.1.9 目标点系统总结

4.2 历史观测编码

4.2.1 问题分析

4.2.2 解决方案设计

4.2.3 历史观测配置

4.2.4 历史观测缓冲区初始化

4.2.5 历史观测更新逻辑

4.2.6 历史观测在观测空间中的位置

4.2.7 历史观测的作用

4.2.8 历史观测编码总结

4.3 地形边缘检测

4.3.1 问题分析

4.3.2 解决方案设计

4.3.3 边缘检测算法

4.3.4 边缘掩码的创建和应用

4.3.5 脚部边缘检测

4.3.6 边缘惩罚奖励函数

4.3.7 边缘检测总结

4.4 观测空间扩展

4.4.1 观测空间对比

4.4.2 新增观测项详解

1. IMU 观测（2 维）

2. 偏航角误差（2 维）

3. 地形类别编码（2 维）

4. 接触状态（4 维）

4.4.3 观测空间结构总结

4.4.4 观测空间扩展的优势

五、数学公式对比

5.1 速度追踪奖励

5.2 观测维度

六、训练策略改进

6.1 阶段性训练

6.2 回合长度

七、奖励函数详细说明

8.1 正则化奖励函数

8.1.1 _reward_delta_torques()

8.1.2 _reward_hip_pos()

8.1.3 _reward_dof_error()

8.1.4 _reward_symmetry()

8.2 跳跃相关奖励函数（当前大部分禁用）

8.2.1 _reward_jump_up()

8.2.2 _reward_jump_preparation()

8.3 其他奖励函数

8.3.1 _reward_stagnation()

8.3.2 _reward_limbo()

九、总结

9.1 主要改进点

9.2 修改范围

9.3 关键参数总结

9.4 建议

3.1 配置文件差异（`go2_config.py`）

3.2 环境实现差异（`go2_robot.py`）

8.1.1 `_reward_delta_torques()`

8.1.2 `_reward_hip_pos()`

8.1.3 `_reward_dof_error()`

8.1.4 `_reward_symmetry()`

8.2.1 `_reward_jump_up()`

8.2.2 `_reward_jump_preparation()`

8.3.1 `_reward_stagnation()`

8.3.2 `_reward_limbo()`