四元数详解

四元数 (Quaternion) 是一种用于表示三维空间旋转的数学工具，比欧拉角更稳定，没有万向锁问题。

四元数的定义

四元数由 4个分量 组成：

1
q = [qx, qy, qz, qw]  # 或写作 q = qw + qx*i + qy*j + qz*k
  • qx, qy, qz: 向量部分 (imaginary parts)
  • qw: 标量部分 (real part)

⚠️ 重要说明：没有单独的”前两个分量含义”

四元数的4个分量是一个整体，不能单独解释某一个或某两个分量的物理意义。它们共同描述旋转，必须作为整体来理解。

为什么不能单独解释？

四元数表示旋转的公式是：

1
q = cos(θ/2) + sin(θ/2) * (nx*i + ny*j + nz*k)

展开为：

1
2
3
4
qw = cos(θ/2)         # 标量部分
qx = sin(θ/2) * nx    # x轴分量
qy = sin(θ/2) * ny    # y轴分量  
qz = sin(θ/2) * nz    # z轴分量

其中：

  • θ: 旋转角度
  • [nx, ny, nz]: 旋转轴的单位向量

可以看到，每个分量都同时包含了角度和轴的信息，无法单独解释。

四元数表示旋转的原理

轴角表示法 → 四元数

假设绕某个轴旋转：

1
2
3
4
5
6
7
8
9
10
11
# 例1: 绕 Z轴 旋转 90度
θ = π/2  # 90度
axis = [0, 0, 1]  # Z轴

qw = cos(π/4) = 0.707
qx = sin(π/4) * 0 = 0
qy = sin(π/4) * 0 = 0
qz = sin(π/4) * 1 = 0.707

# 结果四元数
q = [0, 0, 0.707, 0.707]  # [qx, qy, qz, qw]
1
2
3
4
5
6
7
8
9
10
11
# 例2: 绕 X轴 旋转 180度
θ = π  # 180度
axis = [1, 0, 0]  # X轴

qw = cos(π/2) = 0
qx = sin(π/2) * 1 = 1
qy = sin(π/2) * 0 = 0
qz = sin(π/2) * 0 = 0

# 结果四元数
q = [1, 0, 0, 0]  # [qx, qy, qz, qw]

特殊四元数示例

1
2
3
4
5
6
7
8
9
10
11
12
13
14
# 无旋转（单位四元数）
q = [0, 0, 0, 1]  # 机器人直立，无任何旋转

# 绕 X轴旋转 90度（机器人向左侧倒 - Roll）
q = [0.707, 0, 0, 0.707]

# 绕 Y轴旋转 90度（机器人向前倾 - Pitch）
q = [0, 0.707, 0, 0.707]

# 绕 Z轴旋转 90度（机器人水平转向 - Yaw）
q = [0, 0, 0.707, 0.707]

# 绕 X轴旋转 180度（机器人完全翻倒）
q = [1, 0, 0, 0]

四元数的约束

重要: 四元数必须是单位四元数 (unit quaternion)，即：

1
qx² + qy² + qz² + qw² = 1

否则不能正确表示旋转。

在机器人仿真中的应用

从代码中提取旋转信息

1
2
3
4
5
6
7
8
9
10
# 你的代码
self.base_quat = self.rigid_body_param[:, self.imu_in_torso_indice, 3:7]
# shape: [num_envs, 4]，每一行是 [qx, qy, qz, qw]

# 例如某个环境的四元数
q = self.base_quat[0]  # [qx, qy, qz, qw]

# 检查是否是单位四元数
norm = torch.sqrt(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2)
print(f"Norm: {norm}")  # 应该等于 1.0

四元数 → 欧拉角（Roll, Pitch, Yaw）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
import torch

def quat_to_euler(quat):
    """
    将四元数转换为欧拉角 (roll, pitch, yaw)
    quat: [qx, qy, qz, qw]
    返回: [roll, pitch, yaw] (弧度)
    """
    qx, qy, qz, qw = quat[..., 0], quat[..., 1], quat[..., 2], quat[..., 3]
    
    # Roll (绕X轴旋转)
    sinr_cosp = 2 * (qw * qx + qy * qz)
    cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
    roll = torch.atan2(sinr_cosp, cosr_cosp)
    
    # Pitch (绕Y轴旋转)
    sinp = 2 * (qw * qy - qz * qx)
    pitch = torch.asin(torch.clamp(sinp, -1, 1))
    
    # Yaw (绕Z轴旋转)
    siny_cosp = 2 * (qw * qz + qx * qy)
    cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
    yaw = torch.atan2(siny_cosp, cosy_cosp)
    
    return torch.stack([roll, pitch, yaw], dim=-1)

# 使用示例
euler = quat_to_euler(self.base_quat)  # [num_envs, 3]
roll = euler[:, 0]   # 侧倾角
pitch = euler[:, 1]  # 俯仰角
yaw = euler[:, 2]    # 偏航角

提取旋转角度（不考虑轴）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
def quat_to_angle(quat):
    """
    从四元数提取旋转角度
    quat: [qx, qy, qz, qw]
    返回: 旋转角度（弧度）
    """
    qw = quat[..., 3]
    angle = 2 * torch.acos(torch.clamp(qw, -1, 1))
    return angle

# 使用
rotation_angles = quat_to_angle(self.base_quat)  # [num_envs]
# 例如：检查机器人是否倾斜过大
large_tilt = rotation_angles > 0.5  # 超过约29度

提取旋转轴

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
def quat_to_axis_angle(quat):
    """
    从四元数提取旋转轴和角度
    quat: [qx, qy, qz, qw]
    返回: (axis, angle)
    """
    qx, qy, qz, qw = quat[..., 0], quat[..., 1], quat[..., 2], quat[..., 3]
    
    # 旋转角度
    angle = 2 * torch.acos(torch.clamp(qw, -1, 1))
    
    # 旋转轴
    sin_half_angle = torch.sin(angle / 2)
    # 避免除以0
    sin_half_angle = torch.clamp(sin_half_angle, min=1e-8)
    
    axis_x = qx / sin_half_angle
    axis_y = qy / sin_half_angle
    axis_z = qz / sin_half_angle
    
    axis = torch.stack([axis_x, axis_y, axis_z], dim=-1)
    
    return axis, angle

# 使用
axis, angle = quat_to_axis_angle(self.base_quat)
# axis: [num_envs, 3] - 旋转轴
# angle: [num_envs] - 旋转角度

在双足机器人中的实际应用

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
# 在 birl_task_stable.py 中可能的应用

# 1. 检查机器人是否倒下（通过欧拉角）
euler = quat_to_euler(self.base_quat)
roll = euler[:, 0]
pitch = euler[:, 1]

# 如果 roll 或 pitch 超过阈值，认为机器人倒下
fallen = (torch.abs(roll) > 0.5) | (torch.abs(pitch) > 0.5)

# 2. 奖励函数：惩罚倾斜
# 机器人应该保持直立，即四元数接近 [0,0,0,1]
orientation_error = 1 - self.base_quat[:, 3]**2  # qw应该接近1
reward = -orientation_error

# 3. 检查机器人朝向（yaw）
yaw = euler[:, 2]
# 奖励机器人朝向目标方向
target_yaw = self.commands[:, 2]  # 目标偏航角
yaw_error = torch.abs(yaw - target_yaw)

四元数的数学性质

四元数乘法（组合旋转）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
def quat_multiply(q1, q2):
    """
    四元数乘法，组合两个旋转
    先应用 q1，再应用 q2
    """
    w1, x1, y1, z1 = q1[..., 3], q1[..., 0], q1[..., 1], q1[..., 2]
    w2, x2, y2, z2 = q2[..., 3], q2[..., 0], q2[..., 1], q2[..., 2]
    
    w = w1*w2 - x1*x2 - y1*y2 - z1*z2
    x = w1*x2 + x1*w2 + y1*z2 - z1*y2
    y = w1*y2 - x1*z2 + y1*w2 + z1*x2
    z = w1*z2 + x1*y2 - y1*x2 + z1*w2
    
    return torch.stack([x, y, z, w], dim=-1)

四元数共轭（逆旋转）

1
2
3
4
5
def quat_conjugate(q):
    """
    四元数共轭（逆旋转）
    """
    return torch.cat([-q[..., :3], q[..., 3:4]], dim=-1)

总结

  1. 四元数的4个分量必须作为整体理解，不能单独解释前两个
  2. 每个分量都混合了旋转角度和旋转轴的信息
  3. 四元数 [qx, qy, qz, qw] 通过公式与旋转轴角对应
  4. 实际使用中，通常转换为欧拉角或旋转矩阵来理解
  5. 在机器人仿真中，四元数主要用于表示机器人基座的姿态

如果你想知道机器人的具体姿态（倾斜、俯仰等），建议将四元数转换为欧拉角！