# ANYmal-C 导航 ## 概述 本文档详细描述了基于 ANYmal-C 四足机器人的导航任务环境。该环境是 MotrixLab 项目中导航任务集的一部分，提供了使用强化学习训练四足机器人执行目标位置和朝向导航的完整实现。 ```{video} /_static/videos/anymal_c.mp4 :poster: _static/images/poster/anymal_c.jpg :nocontrols: :autoplay: :playsinline: :muted: :loop: :width: 100% ``` --- ## 环境描述 ANYmal-C 导航任务环境基于真实的 ANYmal-C 四足机器人构建，旨在训练机器人在平面环境中导航至指定的目标位置和朝向。该环境使用 MotrixSim 物理引擎进行仿真，提供高保真的动力学模拟。 ### 机器人结构 ANYmal-C 是一个四足机器人，由以下主要部分组成： - **基座（Base）**：机器人的核心躯干，包含 IMU 传感器、摄像头、激光雷达等传感器模块 - **四条腿**：每条腿包含三个关节 - HAA（Hip Abduction/Adduction）：髋部外展/内收关节 - HFE（Hip Flexion/Extension）：髋部屈伸关节 - KFE（Knee Flexion/Extension）：膝关节屈伸 - **四个足端（Feet）**：球形接触几何体，与地面产生摩擦接触 ### 任务目标 机器人需要完成以下导航目标： 1. **位置导航**：移动到指定的目标位置（XY 平面坐标） 2. **朝向控制**：调整机器人朝向至目标航向角（yaw 角） 3. **稳定停止**：到达目标后保持稳定站立，线速度和角速度接近零 环境提供可视化标记： - **绿色箭头**：指示目标位置和朝向 - **机器人上方绿色箭头**：当前实际运动方向 - **机器人上方蓝色箭头**：期望运动方向 --- ## 动作空间 动作空间为 `Box(-1.0, 1.0, (12,), float32)`，表示施加到 12 个关节的位置控制指令（相对于默认站立姿态的偏移量）。 ### 控制模式 环境使用位置控制模式，动作通过以下方式转换为关节目标位置： ``` 目标关节角度 = 默认关节角度 + (动作值 × 动作缩放系数) ``` 其中动作缩放系数由配置参数 `control_config.action_scale` 指定。 ### 动作维度详细说明 | 编号 | 动作说明 | 控制范围 | 对应关节名称 | 关节类型 | | ---- | ------------------- | ---------- | ------------ | -------- | | 0 | 左前腿髋部外展/内收 | -1.0 ~ 1.0 | LF_HAA | hinge | | 1 | 左前腿髋部屈伸 | -1.0 ~ 1.0 | LF_HFE | hinge | | 2 | 左前腿膝关节屈伸 | -1.0 ~ 1.0 | LF_KFE | hinge | | 3 | 右前腿髋部外展/内收 | -1.0 ~ 1.0 | RF_HAA | hinge | | 4 | 右前腿髋部屈伸 | -1.0 ~ 1.0 | RF_HFE | hinge | | 5 | 右前腿膝关节屈伸 | -1.0 ~ 1.0 | RF_KFE | hinge | | 6 | 左后腿髋部外展/内收 | -1.0 ~ 1.0 | LH_HAA | hinge | | 7 | 左后腿髋部屈伸 | -1.0 ~ 1.0 | LH_HFE | hinge | | 8 | 左后腿膝关节屈伸 | -1.0 ~ 1.0 | LH_KFE | hinge | | 9 | 右后腿髋部外展/内收 | -1.0 ~ 1.0 | RH_HAA | hinge | | 10 | 右后腿髋部屈伸 | -1.0 ~ 1.0 | RH_HFE | hinge | | 11 | 右后腿膝关节屈伸 | -1.0 ~ 1.0 | RH_KFE | hinge | ### PD 控制参数 底层使用位置执行器，PD 控制参数在 XML 文件中定义： - **kp（比例增益）**：200 - **kv（微分增益）**：1 - **力矩限制**：-140 N·m ~ 140 N·m --- ## 观测空间 观测空间为 `Box(-inf, inf, (54,), float32)`，包含机器人的本体感知信息、任务相关信息和历史动作。 ### 观测组成部分 观测向量由以下部分组成（按顺序）： 1. **本体感知状态（33 维）** - 基座线速度（3 维）：机器人基座在世界坐标系中的线速度 [vx, vy, vz] - 角速度（3 维）：从陀螺仪读取的角速度 [ωx, ωy, ωz] - 投影重力（3 维）：重力向量在机器人本体坐标系中的投影 - 关节角度（12 维）：12 个关节相对于默认站立姿态的角度偏移 - 关节速度（12 维）：12 个关节的角速度 2. **历史动作（12 维）** - 上一时刻执行的动作 3. **速度命令（3 维）** - 期望线速度 XY（2 维）：根据位置误差计算的期望线速度 - 期望角速度 Z（1 维）：根据朝向误差计算的期望角速度 4. **任务状态（6 维）** - 位置误差向量（2 维）：到目标位置的 XY 平面误差向量（归一化） - 朝向误差（1 维）：到目标朝向的角度差（归一化到[-1, 1]） - 距离（1 维）：到目标的欧氏距离（归一化） - 到达标志（1 维）：是否同时满足位置和朝向到达条件（0 或 1） - 停止就绪标志（1 维）：是否达到停止标准（到达且角速度接近零） ### 观测详细说明 | 编号 | 观测内容 | 最小值 | 最大值 | 归一化系数 | 单位 | | ----- | --------------------------------- | ------ | ------ | --------------------- | ---------- | | 0-2 | 基座线速度 (vx, vy, vz) | -inf | inf | normalization.lin_vel | m/s | | 3-5 | 角速度 (ωx, ωy, ωz) | -inf | inf | normalization.ang_vel | rad/s | | 6-8 | 投影重力 (gx, gy, gz) | -1 | 1 | 1.0 | 无量纲 | | 9-20 | 关节角度偏移（12 个关节） | -inf | inf | normalization.dof_pos | rad | | 21-32 | 关节角速度（12 个关节） | -inf | inf | normalization.dof_vel | rad/s | | 33-44 | 上一时刻动作 | -1 | 1 | 1.0 | 无量纲 | | 45-47 | 速度命令 (vx_cmd, vy_cmd, ωz_cmd) | -inf | inf | commands_scale | m/s, rad/s | | 48-49 | 位置误差向量 (Δx, Δy) | -inf | inf | 1/5.0 | m | | 50 | 朝向误差 | -1 | 1 | 1/π | rad | | 51 | 到目标距离 | 0 | 1 | 1/5.0 (截断) | m | | 52 | 到达标志 | 0 | 1 | 1.0 | 布尔值 | | 53 | 停止就绪标志 | 0 | 1 | 1.0 | 布尔值 | ### 传感器信息 环境使用以下传感器获取状态： - **framelinvel**（名称：base_linvel）：基座线速度传感器 - **gyro**（名称：base_gyro）：陀螺仪传感器，安装在 IMU 站点 --- ## 奖励函数 奖励函数采用复合设计，根据机器人是否到达目标使用不同的奖励策略。 ### 未到达目标时的奖励 总奖励 = 速度跟踪奖励 + 接近奖励 - 惩罚项 **主要奖励项：** 1. **线速度跟踪奖励**（权重：1.5） - 计算公式：`1.5 × exp(-||v_xy - v_cmd||² / 0.25)` - 激励机器人跟踪期望的 XY 平面线速度 2. **角速度跟踪奖励**（权重：0.3） - 计算公式：`0.3 × exp(-(ωz - ωz_cmd)² / 0.25)` - 激励机器人跟踪期望的偏航角速度 3. **接近奖励** - 计算公式：`clip((历史最小距离 - 当前距离) × 4.0, -1.0, 1.0)` - 奖励机器人每次距离目标更近时的进步 **惩罚项：** - Z 轴线速度惩罚（权重：2.0）：`-2.0 × vz²` - XY 轴角速度惩罚（权重：0.05）：`-0.05 × (ωx² + ωy²)` - 力矩惩罚（权重：0.00001）：`-0.00001 × ||τ||²` - 动作变化率惩罚（权重：0.001）：`-0.001 × ||Δa||²` ### 到达目标后的奖励 总奖励 = 停止奖励 + 首次到达奖励 - 惩罚项 **主要奖励项：** 1. **停止基础奖励** - 计算公式：`2 × [0.8 × exp(-(v_xy/0.2)²) + 1.2 × exp(-(ωz/0.1)⁴)]` - 激励机器人在到达后保持低速度和角速度 2. **零角速度奖励**（额外奖励：6.0） - 条件：到达目标且 |ωz| < 0.05 rad/s - 激励机器人完全停止旋转 3. **首次到达奖励**（一次性：10.0） - 条件：首次同时满足位置和朝向到达条件 - 提供到达目标的明确信号 **惩罚项：**（与未到达时相同） ### 终止条件惩罚 以下情况会额外施加 -20.0 的惩罚： - 关节速度超限（超过 `max_dof_vel` 配置值） - 关节速度出现 NaN 或 Inf - 机器人基座接触地面 - 机器人侧翻（倾斜角度超过 75°） ### 到达判定标准 - **位置到达**：距离目标 < 0.3 米 - **朝向到达**：朝向误差 < 15° - **完全到达**：同时满足位置到达和朝向到达 - **停止就绪**：完全到达 且 |ωz| < 0.05 rad/s ### info 返回内容 每步返回的 info 字典包含以下调试信息： - `pose_commands`：当前目标位置和朝向 [x, y, yaw] - `last_actions`：上一时刻的动作 - `current_actions`：当前时刻的动作 - `steps`：当前回合步数 - `ever_reached`：是否曾经到达过目标 - `min_distance`：历史最小距离（用于计算接近奖励） --- ## 初始状态 ### 机器人初始化 **位置初始化：** 机器人在世界坐标系中的初始位置由配置参数 `init_state.pos_randomization_range` 定义的范围内随机采样： - X 坐标：在 [x_min, x_max] 范围内均匀随机采样 - Y 坐标：在 [y_min, y_max] 范围内均匀随机采样 - Z 坐标：固定在 0.56 米（避免坠落感） **姿态初始化：** - 机器人朝向（四元数）：初始化为单位四元数 [0, 0, 0, 1]，表示正向朝向 - 无随机噪声添加到四元数（保证初始稳定性） **关节初始化：** 关节角度设置为默认站立姿态，由配置参数 `init_state.default_joint_angles` 定义。无随机噪声添加到关节角度（保证初始稳定站立）。 **速度初始化：** 所有线速度和角速度初始化为零，确保机器人从静止状态开始。 ### 目标生成 **目标位置：** 目标位置相对于机器人初始位置生成： ``` 目标位置 = 机器人初始位置 + 随机偏移 ``` 随机偏移在配置参数 `commands.pose_command_range` 定义的范围内采样： - X 方向偏移：[pose_command_range[0], pose_command_range[3]] - Y 方向偏移：[pose_command_range[1], pose_command_range[4]] **目标朝向：** 目标朝向（yaw 角）在绝对参考系中随机生成： - 朝向角度：[pose_command_range[2], pose_command_range[5]] ### 可视化标记初始化 - **目标标记**（绿色箭头）：设置为目标位置和朝向 - **运动方向箭头**：初始化在机器人上方 0.76 米高度处 --- ## 回合终止 ### 终止条件（Termination） 环境在以下任一条件满足时终止回合： 1. **超时终止** - 条件：回合达到 `max_episode_second` 配置值 - 说明：防止无限回合 2. **关节速度异常** - 条件：任一关节速度的绝对值超过 `max_dof_vel` - 条件：关节速度出现 NaN、Inf 或超过 1e6 - 说明：防止数值发散和物理不稳定 3. **基座接触地面** - 条件：机器人基座（配置参数 `terminate_after_contacts_on` 定义的几何体）与地面接触 - 说明：机器人跌倒或姿态失败 4. **侧翻倾覆** - 条件：机器人倾斜角度超过 75° - 计算：通过投影重力向量计算倾斜角 `arctan2(||g_xy||, |g_z|)` - 说明：机器人严重侧翻 ### 成功条件 虽然环境不会因成功而终止，但任务成功的定义为： - 机器人到达目标位置和朝向（同时满足位置阈值 < 0.3m 和朝向阈值 < 15°） - 机器人保持稳定停止（线速度 < 0.05 m/s，角速度 < 0.05 rad/s）