机器人学中的旋转表示方法全解析

在机器人学、计算机视觉、航空航天等领域，准确表示三维空间中的旋转是一个基础而关键的问题。本文将全面介绍常见的旋转表示方法，帮助你理解它们的概念、优劣势以及适用场景。

旋转矩阵 (Rotation Matrix)

概念

旋转矩阵是一个 3×3 的正交矩阵，用于描述三维空间中的旋转变换。

数学形式：

R ∈ SO(3) = {R ∈ ℝ³ˣ³ | RRᵀ = I, det(R) = 1}

基本性质：

• 正交矩阵：RRᵀ = I
• 行列式为 1：det(R) = 1
• 列向量是单位正交基

优点

✅ 数学直观 - 线性代数中最自然的表示方式 ✅ 变换简单 - 旋转点只需矩阵乘法：p’ = Rp ✅ 组合容易 - 连续旋转直接矩阵相乘：R₃ = R₁R₂ ✅ 无奇异性 - 可以表示任意旋转 ✅ 易于理解 - 列向量直接表示坐标系的轴方向

缺点

❌ 冗余度高 - 需要 9 个参数表示 3 个自由度 ❌ 存储开销大 - 相比其他方法占用更多内存 ❌ 数值漂移 - 多次运算后可能失去正交性，需要重新正交化 ❌ 插值困难 - 直接线性插值不能保证结果仍是旋转矩阵

使用场景

• 图形学变换管线
• 理论推导和证明
• 需要频繁进行点变换的场景
• 机器人运动学正逆解

代码示例

import numpy as np
 
# 绕Z轴旋转θ角度的旋转矩阵
def rotation_matrix_z(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([
        [c, -s, 0],
        [s,  c, 0],
        [0,  0, 1]
    ])
 
# 旋转组合
R1 = rotation_matrix_z(np.pi/4)
R2 = rotation_matrix_z(np.pi/6)
R_combined = R1 @ R2  # 简单矩阵乘法
 
# 点的旋转
point = np.array([1, 0, 0])
rotated_point = R_combined @ point

---

欧拉角 (Euler Angles)

概念

欧拉角通过三次绕固定轴或动轴的旋转来描述方向，是最符合人类直觉的表示方法。

常见约定：

• XYZ 欧拉角（Roll-Pitch-Yaw）: 先绕 X 轴滚转，再绕 Y 轴俯仰，最后绕 Z 轴偏航
• ZYX 欧拉角: 航空航天常用
• ZYZ 欧拉角: 经典力学常用

数学形式：

R = Rz(ψ) · Ry(θ) · Rx(φ)

优点

✅ 直观易懂 - 符合人类对旋转的直觉理解 ✅ 参数少 - 只需 3 个角度参数 ✅ 易于调试 - 工程师可以直接理解每个角度的物理意义 ✅ 易于输入 - 适合用户界面输入（如相机姿态调整）

缺点

❌ 万向锁（Gimbal Lock） - 某些姿态下损失一个自由度

• 例如：当 pitch = ±90° 时，roll 和 yaw 效果重合 ❌ 插值不平滑 - 线性插值可能经过不合理的路径 ❌ 不唯一性 - 同一旋转可能有多种欧拉角表示 ❌ 顺序依赖 - 旋转顺序不同结果完全不同

万向锁详解

当中间旋转轴达到±90° 时：

假设采用ZYX顺序，当pitch = 90°时：
R = Rz(yaw) · Ry(90°) · Rx(roll)
此时yaw和roll的旋转效果叠加，无法独立控制

使用场景

• 人机交互界面（相机控制、物体摆放）
• 机械臂各关节角度表示
• 航空航天姿态显示
• 配置文件中的姿态参数
• 不适合需要平滑插值或频繁旋转组合的场景

代码示例

import numpy as np
 
def euler_to_rotation_matrix(roll, pitch, yaw):
    """ZYX欧拉角转旋转矩阵"""
    cr, sr = np.cos(roll), np.sin(roll)
    cp, sp = np.cos(pitch), np.sin(pitch)
    cy, sy = np.cos(yaw), np.sin(yaw)
    
    return np.array([
        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
        [-sp,   cp*sr,            cp*cr           ]
    ])
 
# 万向锁示例
R_gimbal = euler_to_rotation_matrix(0.1, np.pi/2, 0.1)  # pitch = 90°
# 此时roll和yaw的影响会混合

---

四元数 (Quaternion)

概念

四元数是一种扩展的复数系统，用于表示三维空间的旋转。单位四元数构成三维球面 S³。

数学形式：

q = w + xi + yj + zk = [w, x, y, z]
其中 i² = j² = k² = ijk = -1

单位四元数约束：

||q|| = √(w² + x² + y² + z²) = 1

与轴角的关系：

q = [cos(θ/2), sin(θ/2)·kx, sin(θ/2)·ky, sin(θ/2)·kz]
其中 k = [kx, ky, kz] 是旋转轴，θ是旋转角

优点

✅ 无奇异性 - 不存在万向锁问题 ✅ 紧凑高效 - 4 个参数（实际 3 自由度） ✅ 插值平滑 - SLERP 球面线性插值效果优秀 ✅ 组合高效 - 旋转组合只需四元数乘法 ✅ 数值稳定 - 在所有角度下都稳定 ✅ 易于求逆 - q⁻¹ = [w, -x, -y, -z]（单位四元数）

缺点

❌ 不直观 - 难以直接理解其物理意义 ❌ 双重覆盖 - q 和 -q 表示同一旋转 ❌ 需要归一化 - 多次运算后需要重新归一化保持单位长度 ❌ 学习曲线陡 - 需要理解四元数代数

四元数运算

乘法（旋转组合）：

q₁ ⊗ q₂ = [w₁w₂ - v₁·v₂, w₁v₂ + w₂v₁ + v₁×v₂]
其中 v = [x, y, z]

旋转点：

p' = q ⊗ p ⊗ q*
其中 p = [0, px, py, pz]，q* 是q的共轭

SLERP 插值：

slerp(q₁, q₂, t) = (sin((1-t)θ)/sin(θ))q₁ + (sin(tθ)/sin(θ))q₂
其中 cos(θ) = q₁·q₂

使用场景

• 游戏引擎 - Unity、Unreal 等广泛使用
• 飞行控制 - 无人机姿态估计
• 动画系统 - 骨骼动画插值
• 惯性导航 - IMU 姿态融合
• 实时姿态跟踪 - VR/AR 设备
• 机器人路径规划 - 姿态平滑过渡

代码示例

import numpy as np
 
class Quaternion:
    def __init__(self, w, x, y, z):
        self.w, self.x, self.y, self.z = w, x, y, z
    
    def normalize(self):
        """归一化"""
        norm = np.sqrt(self.w**2 + self.x**2 + self.y**2 + self.z**2)
        self.w /= norm
        self.x /= norm
        self.y /= norm
        self.z /= norm
        return self
    
    def multiply(self, other):
        """四元数乘法"""
        w = self.w*other.w - self.x*other.x - self.y*other.y - self.z*other.z
        x = self.w*other.x + self.x*other.w + self.y*other.z - self.z*other.y
        y = self.w*other.y - self.x*other.z + self.y*other.w + self.z*other.x
        z = self.w*other.z + self.x*other.y - self.y*other.x + self.z*other.w
        return Quaternion(w, x, y, z)
    
    @staticmethod
    def from_axis_angle(axis, angle):
        """从轴角创建四元数"""
        axis = axis / np.linalg.norm(axis)
        half_angle = angle / 2
        w = np.cos(half_angle)
        x, y, z = np.sin(half_angle) * axis
        return Quaternion(w, x, y, z)
    
    @staticmethod
    def slerp(q1, q2, t):
        """球面线性插值"""
        dot = q1.w*q2.w + q1.x*q2.x + q1.y*q2.y + q1.z*q2.z
        
        # 确保最短路径
        if dot < 0:
            q2.w, q2.x, q2.y, q2.z = -q2.w, -q2.x, -q2.y, -q2.z
            dot = -dot
        
        theta = np.arccos(np.clip(dot, -1, 1))
        
        if abs(theta) < 1e-6:  # 角度太小，线性插值
            return Quaternion(
                (1-t)*q1.w + t*q2.w,
                (1-t)*q1.x + t*q2.x,
                (1-t)*q1.y + t*q2.y,
                (1-t)*q1.z + t*q2.z
            ).normalize()
        
        sin_theta = np.sin(theta)
        a = np.sin((1-t)*theta) / sin_theta
        b = np.sin(t*theta) / sin_theta
        
        return Quaternion(
            a*q1.w + b*q2.w,
            a*q1.x + b*q2.x,
            a*q1.y + b*q2.y,
            a*q1.z + b*q2.z
        )
 
# 使用示例
q1 = Quaternion.from_axis_angle(np.array([0, 0, 1]), np.pi/4)
q2 = Quaternion.from_axis_angle(np.array([0, 1, 0]), np.pi/3)
q_combined = q1.multiply(q2)
 
# 插值
q_interpolated = Quaternion.slerp(q1, q2, 0.5)

---

旋转向量 (Rotation Vector / Axis-Angle)

概念

旋转向量将旋转轴和旋转角度编码在一个三维向量中，向量的方向表示旋转轴，长度表示旋转角度。

数学形式：

ω = θ · k
其中：
- θ ∈ [0, π] 是旋转角度
- k 是单位旋转轴
- ||ω|| = θ

李群李代数关系：

SO(3) ←exp→ so(3)
R = exp(ω∧)  # 指数映射
ω = log(R)ᵛ  # 对数映射

优点

✅ 极简表示 - 最少参数（3 个） ✅ 无奇异性 - 可表示任意旋转 ✅ 适合优化 - 在李代数框架下，更新是向量加法 ✅ 物理直观 - 旋转轴和角度明确 ✅ 无约束 - 不需要归一化维护

缺点

❌ 小角度不稳定 - θ→0 时，k 的方向不确定 ❌ 大角度周期性 - θ > π需要特殊处理 ❌ 组合复杂 - 两个旋转向量的组合需要通过矩阵 ❌ 插值非线性 - 不能直接线性插值

与四元数的关系

从旋转向量到四元数：

θ = ||ω||
k = ω / θ
q = [cos(θ/2), sin(θ/2)·k]

从四元数到旋转向量：

θ = 2·arccos(w)
k = [x, y, z] / sin(θ/2)
ω = θ·k

本质联系：

• 旋转向量是 SO(3) 的李代数 so(3) 中的元素
• 四元数是 SO(3) 的双重覆盖 S³中的元素
• 通过指数/对数映射相互转换
• 信息等价，但代数结构和计算方式不同

使用场景

• SLAM（同步定位与地图构建） - 位姿优化
• 视觉里程计 - 相机运动估计
• 非线性优化 - 如 Ceres、g2o 中的旋转参数化
• 机器人学理论 - 李群李代数框架
• 扰动分析 - 小角度扰动建模

代码示例

import numpy as np
 
def rotation_vector_to_matrix(omega):
    """旋转向量转旋转矩阵（罗德里格斯公式）"""
    theta = np.linalg.norm(omega)
    
    if theta < 1e-6:  # 小角度近似
        return np.eye(3) + skew_symmetric(omega)
    
    k = omega / theta
    K = skew_symmetric(k)
    
    # 罗德里格斯公式: R = I + sin(θ)K + (1-cos(θ))K²
    R = np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * (K @ K)
    return R
 
def skew_symmetric(v):
    """向量的反对称矩阵"""
    return np.array([
        [0,    -v[2],  v[1]],
        [v[2],  0,    -v[0]],
        [-v[1], v[0],  0   ]
    ])
 
def matrix_to_rotation_vector(R):
    """旋转矩阵转旋转向量"""
    theta = np.arccos((np.trace(R) - 1) / 2)
    
    if abs(theta) < 1e-6:  # 小角度
        return np.array([R[2,1] - R[1,2], 
                        R[0,2] - R[2,0], 
                        R[1,0] - R[0,1]]) / 2
    
    # 提取旋转轴
    k = 1 / (2 * np.sin(theta)) * np.array([
        R[2,1] - R[1,2],
        R[0,2] - R[2,0],
        R[1,0] - R[0,1]
    ])
    
    return theta * k
 
# 使用示例
omega1 = np.array([0.1, 0.2, 0.3])
R1 = rotation_vector_to_matrix(omega1)
 
# 组合旋转（需要通过矩阵）
omega2 = np.array([0.2, -0.1, 0.15])
R2 = rotation_vector_to_matrix(omega2)
R_combined = R1 @ R2
omega_combined = matrix_to_rotation_vector(R_combined)
 
print(f"组合后的旋转向量: {omega_combined}")

在优化中的应用

# SLAM中的位姿优化示例（伪代码）
def optimize_pose(initial_pose, observations):
    """使用旋转向量优化位姿"""
    # 位姿参数化：[tx, ty, tz, ωx, ωy, ωz]
    params = np.array([
        initial_pose.translation,
        initial_pose.rotation_vector
    ]).flatten()
    
    def residual(params):
        t = params[0:3]
        omega = params[3:6]
        R = rotation_vector_to_matrix(omega)
        
        # 计算重投影误差
        errors = []
        for obs in observations:
            predicted = project(R, t, obs.point_3d)
            errors.append(predicted - obs.point_2d)
        return np.array(errors).flatten()
    
    # 使用Levenberg-Marquardt等算法优化
    # 更新直接是向量加法: params += delta
    result = optimize(residual, params)
    
    return result

---

其他旋转表示方法

1. 轴角表示 (Axis-Angle)

旋转向量的解耦形式，分别存储轴和角。

形式： (k, θ) 其中 k ∈ ℝ³, ||k|| = 1, θ ∈ ℝ

优点： 非常直观，物理意义明确

缺点： 4 个参数存储，k 的归一化约束

使用： 教学、可视化、用户输入界面

2. 罗德里格斯参数 (Rodrigues Parameters)

形式： r = k · tan(θ/2)

特点：

• 3 个参数
• 在小角度时数值稳定
• θ→π时奇异

使用： 航天器姿态控制、卫星导航

3. 修正罗德里格斯参数 (Modified Rodrigues Parameters, MRP)

形式： r = k · tan(θ/4)

特点：

• 扩展了罗德里格斯参数的有效范围
• 在更大的角度范围内保持稳定
• 适合姿态控制

使用： 航天器姿态动力学

4. 对偶四元数 (Dual Quaternions)

形式： q̂ = qr + ε·qd

其中 qr 是实部四元数（旋转），qd 是对偶部（平移）

特点：

• 同时表示旋转和平移
• 统一表示刚体运动
• 插值平滑且高效

使用：

• 机器人运动学
• 角色动画（blend shapes）
• 手眼标定

代码示例：

class DualQuaternion:
    def __init__(self, real, dual):
        self.real = real  # 旋转四元数
        self.dual = dual  # 平移部分：0.5 * t_quat * real
    
    def transform_point(self, point):
        """变换点（同时旋转和平移）"""
        # p̂ = q̂ ⊗ p̂ ⊗ q̂*
        # 其中 p̂ = 1 + ε·[0, px, py, pz]
        pass
    
    @staticmethod
    def from_rotation_translation(quat, translation):
        """从旋转和平移创建对偶四元数"""
        t_quat = Quaternion(0, *translation)
        dual = 0.5 * t_quat.multiply(quat)
        return DualQuaternion(quat, dual)

5. 单位正切四元数 (Unit Tangent Quaternions)

四元数的变体，用于某些特殊优化问题。

6. Cayley-Klein 参数

基于复数的旋转表示，在量子力学中有应用。

7. 方向余弦矩阵 (Direction Cosine Matrix, DCM)

本质上就是旋转矩阵，在航空航天领域的特定称呼。

---

对比总结

参数对比表

表示方法	参数个数	自由度	奇异性	数值稳定性	插值性能	组合复杂度
旋转矩阵	9	3	无	⚠️ 需正交化	差	O(n³)
欧拉角	3	3	万向锁	好	差	中等
四元数	4	3	无	好	优秀 (SLERP)	O(1)
旋转向量	3	3	无	⚠️ 小/大角度	差	需转换
轴角	4	3	无	中等	中等	需转换
对偶四元数	8	6	无	好	优秀	O(1)

性能对比

计算效率（旋转组合）：

1. 四元数 > 旋转矩阵 > 欧拉角 > 旋转向量

存储效率：

1. 旋转向量 = 欧拉角 > 四元数 > 轴角 > 旋转矩阵

插值质量：

1. 四元数 (SLERP) > 对偶四元数 > 旋转矩阵 > 旋转向量 > 欧拉角

转换关系图

           ┌─────────────┐
           │  欧拉角      │
           └──────┬──────┘
                  │
    ┌─────────────┼─────────────┐
    │             │             │
    ▼             ▼             ▼
┌────────┐   ┌────────┐   ┌──────────┐
│ 四元数  │◄─►│旋转矩阵 │◄─►│旋转向量   │
└────┬───┘   └───┬────┘   └────┬─────┘
     │           │             │
     │      ┌────▼─────┐       │
     └─────►│  轴角     │◄──────┘
            └──────────┘
                 │
            ┌────▼────────┐
            │对偶四元数    │
            │(包含平移)    │
            └─────────────┘

适用领域总结

实时系统（游戏、VR、无人机） → 首选四元数

• 计算高效
• 插值平滑
• 无奇异性

用户交互界面 → 首选欧拉角

• 直观易懂
• 易于调节
• 可以限制角度范围避免万向锁

优化问题（SLAM、视觉里程计） → 首选旋转向量

• 参数最少
• 无约束优化
• 适合李代数框架

图形变换管线 → 首选旋转矩阵

• 与其他变换统一
• 硬件加速支持好
• 变换点最直接

刚体运动（机械臂、动画） → 考虑对偶四元数

• 统一旋转和平移
• 插值质量高

---

实际应用建议

1. 游戏引擎中的角色旋转

class Character:
    def __init__(self):
        self.orientation = Quaternion(1, 0, 0, 0)  # 用四元数存储
        self.target_orientation = None
        self.rotation_speed = 2.0  # rad/s
    
    def rotate_to(self, target_direction, dt):
        """平滑旋转到目标方向"""
        # 计算目标四元数
        self.target_orientation = Quaternion.from_direction(target_direction)
        
        # SLERP插值
        t = min(1.0, self.rotation_speed * dt)
        self.orientation = Quaternion.slerp(
            self.orientation, 
            self.target_orientation, 
            t
        )
    
    def get_euler_for_display(self):
        """仅用于UI显示"""
        return self.orientation.to_euler()

建议：

• 内部用四元数存储和计算
• UI 显示转换为欧拉角
• 避免用欧拉角做插值

2. SLAM 位姿优化

class PoseOptimizer:
    def __init__(self):
        # 使用旋转向量参数化
        self.params = np.zeros(6)  # [tx, ty, tz, ωx, ωy, ωz]
    
    def optimize(self, observations):
        """优化位姿"""
        def cost_function(params):
            t = params[0:3]
            omega = params[3:6]
            R = rotation_vector_to_matrix(omega)
            
            # 计算重投影误差
            error = compute_reprojection_error(R, t, observations)
            return error
        
        # 使用梯度下降或LM算法
        # 更新是向量加法：简单高效
        result = scipy.optimize.least_squares(
            cost_function, 
            self.params,
            method='lm'
        )
        
        return result.x

建议：

• 优化用旋转向量
• 需要变换点时转为矩阵
• 避免频繁转换

3. 无人机姿态估计

class IMU_Filter:
    def __init__(self):
        self.orientation = Quaternion(1, 0, 0, 0)
    
    def update