强化学习观测项详解之——重力投影

重力投影 (Projected Gravity) 指的是将世界坐标系下的重力向量（通常为 ( 0 , 0 , − 1 ) (0, 0, -1) (0,0,−1) ）投影（变换）到机器人的基座坐标系下。

这在强化学习和机器人控制中非常重要，因为 IMU 能够感知相对于自身的重力方向，这直接反映了机器人的倾斜姿态（Roll 和 Pitch）。

1. 为什么要用重力投影

这是人形机器人 RL 中非常关键的设计哲学。相比于欧拉角（Euler Angles）或原始四元数（Quaternion），重力投影向量（Projected Gravity）胜出的原因主要集中在 解耦航向 、避免奇异性 以及 Sim2Real 的友好性。

以下是详细对比分析：

1. 解耦绝对航向------ 最重要的原因

• 问题 ：
  • 四元数包含了完整的旋转信息（Roll, Pitch, Yaw）。
  • 欧拉角也明确包含 Yaw（偏航角）。
• 为何重力投影更好 ：
  • 人形机器人的运动控制（Locomotion）通常是相对于自身的。无论机器人面朝东方还是北方，只要它的身体倾斜角度相同，它维持平衡的策略应该是一样的。
  • 重力投影向量 g b g_b gb 只包含 Roll 和 Pitch 的信息，天然丢失了 Yaw 的绝对信息。
  • 好处：这迫使 Policy 学习基于当前朝向的相对控制，而不是过拟合到某个特定的世界坐标系航向（例如：防止机器人学会"只有面朝北边才能走直线"的错误策略）。如果输入完整的四元数，网络往往需要浪费神经元去"遗忘"绝对 Yaw 的影响。

2. 避免"双重覆盖"问题------ 针对四元数

• 问题 ：四元数具有双重覆盖特性，即 q q q 和 − q -q −q 代表完全相同的旋转姿态。
• 后果 ：如果不做特殊处理，神经网络会面临输入空间的不连续性（虽然物理上连续，但数值上可能瞬间从 q q q 跳变到 − q -q −q），这会导致训练时的 Loss 震荡或难以收敛。虽然可以通过规范化（比如强制 w > 0 w > 0 w>0）来解决，但重力投影天然没有这个问题。

3. 避免万向节死锁与不连续性 ------ 针对欧拉角

• 问题 ：欧拉角在 ± π \pm \pi ±π 处存在数值跳变，且在 Pitch 接近 90 度时存在万向节死锁（Gimbal Lock）。
• 后果：神经网络非常讨厌不连续的输入数据。重力投影向量在三维空间中是连续变化的，不存在奇异点，对梯度下降非常友好。

4. Sim2Real 的真实性（物理意义）

• 原理 ：IMU 中的加速度计在静止或匀速运动时，测量的就是重力在机体坐标系下的分量。
• 好处 ：重力投影向量 g b g_b gb 本质上就是理想状态下的加速度计读数（忽略运动加速度）。直接使用 g b g_b gb 作为观测，使得仿真与真机的观测空间在物理意义上高度一致，减小了 Sim2Real Gap。

5. 降维与特征压缩

• 四元数：4维。
• 欧拉角：3维。
• 重力投影：3维（实际上模长为1，自由度为2）。
• 虽然维度差别不大，但 g b g_b gb 用 3 个数值极其紧凑地表达了"我现在有多歪"这一核心状态，没有冗余信息，有助于 Policy 更快收敛。

---

一句话总结：

在人形机器人强化学习中，使用重力投影是为了剔除与平衡无关的绝对航向信息 ，并提供一个数值连续、无奇异点且符合传感器物理特性的姿态描述，从而加速训练并提高策略的鲁棒性。

2. 计算理论

数学公式

假设：

• g w g_w gw : 世界坐标系下的重力向量（World Frame）。
• q w b q_{wb} qwb : 从基座坐标系到世界坐标系的旋转四元数（Body to World Quaternion）。
• R w b R_{wb} Rwb : 对应的旋转矩阵。

我们需要求 g b g_b gb（基座坐标系下的重力向量）。

根据坐标变换原理：
g b = R b w ⋅ g w = R w b T ⋅ g w g_b = R_{bw} \cdot g_w = R_{wb}^T \cdot g_w gb=Rbw⋅gw=RwbT⋅gw

使用四元数运算，向量 v v v 经四元数 q q q 旋转得到 v ′ v' v′ 的公式为 v ′ = q v q − 1 v' = q v q^{-1} v′=qvq−1 (或 q v q ∗ q v q^* qvq∗，取决于定义)。在这里我们需要进行逆变换 （从世界到基座），即使用 q w b q_{wb} qwb 的共轭（或逆）来旋转 g w g_w gw：

g b = q w b − 1 ⊗ g w ⊗ q w b g_b = q_{wb}^{-1} \otimes g_w \otimes q_{wb} gb=qwb−1⊗gw⊗qwb

---

2. IsaacLab 实现分析

代码位置

• 文件 : source/isaaclab/isaaclab/assets/articulation/articulation_data.py

代码实现

@property
def projected_gravity_b(self):
    """Projection of the gravity direction on base frame. Shape is (num_instances, 3)."""
    return math_utils.quat_apply_inverse(self.root_link_quat_w, self.GRAVITY_VEC_W)

变量说明:

• self.root_link_quat_w: 基座在世界坐标系下的姿态四元数 ( q w b q_{wb} qwb)，格式为 (w, x, y, z)。
• self.GRAVITY_VEC_W: 世界坐标系下的重力方向向量，在 __init__ 中初始化，归一化为 (0, 0, -1)。

核心函数 math_utils.quat_apply_inverse，位于 source/isaaclab/isaaclab/utils/math.py：

def quat_apply_inverse(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor:
    # ... (省略形状处理)
    # 提取虚部 xyz
    xyz = quat[:, 1:]
    # 计算中间变量 t
    t = xyz.cross(vec, dim=-1) * 2
    # 应用逆旋转公式 (对应于 q^-1 * v * q)
    return (vec - quat[:, 0:1] * t + xyz.cross(t, dim=-1)).view(shape)

分析 :

IsaacLab 使用了 PyTorch 进行批量化 (Batch) 计算。quat_apply_inverse 实现了四元数逆旋转的高效代数形式（避免了构建旋转矩阵的开销）。它本质上就是计算 g b = q w b − 1 ⋅ g w g_b = q_{wb}^{-1} \cdot g_w gb=qwb−1⋅gw。

---

3. 宇树的实现分析

代码位置

• 文件 : unitree_articulation.h （仓库地址）
• 依赖: Eigen 库

代码实现

// 1. 构建四元数 (Eigen库 w在首位)
data.root_quat_w = Eigen::Quaternionf(
    lowstate->msg_.imu_state().quaternion()[0], // w
    lowstate->msg_.imu_state().quaternion()[1], // x
    lowstate->msg_.imu_state().quaternion()[2], // y
    lowstate->msg_.imu_state().quaternion()[3]  // z
);

// 2. 计算投影重力
data.projected_gravity_b = data.root_quat_w.conjugate() * data.GRAVITY_VEC_W;

分析:

1. 数据源 : 直接读取真实的 IMU 数据 (lowstate->msg_.imu_state())。
2. 四元数构建 : 使用 Eigen::Quaternionf 构建 q w b q_{wb} qwb。
3. 计算逻辑 : data.root_quat_w.conjugate() 得到 q w b ∗ q_{wb}^* qwb∗ (即 q w b − 1 q_{wb}^{-1} qwb−1，假设是单位四元数)。
4. 旋转操作 : 在 Eigen 中，q * v 运算符重载表示用四元数 q q q 旋转向量 v v v。因此 q.conjugate() * v 等价于将向量从世界系旋转回基座系。

注意，Isaaclab和宇树仓库两者的计算逻辑在数学上是等价的。