4. 欧拉角转旋转矩阵：数学推导与Python代码实现

好，咱们进入实战手册的第四个核心环节。前面我们聊了欧拉角的直观含义，也看了四元数的优雅结构。但说实话，在真正的工程代码里——尤其是做飞控、做机械臂运动学的时候——我们最常用的姿态表达方式，其实是旋转矩阵。

为什么？因为矩阵乘法可以直接叠加旋转，而且跟三维空间里的向量变换是天然匹配的。你想想看，一个三维向量乘以一个旋转矩阵，结果就是旋转后的新向量。这比用欧拉角去算要方便太多了。

所以这一节，我们就来彻底搞明白：给定一组欧拉角，怎么得到对应的旋转矩阵？ 数学推导我会讲清楚，但更重要的是——我会给你可以直接用的 Python 代码。

核心结论： 欧拉角转旋转矩阵，本质上就是三个基本旋转矩阵的乘积。顺序不同，结果天差地别。我当年在调试一个机械臂的逆解算法时，就因为搞错了旋转顺序，整整排查了两天……嗯，从那以后我再也不敢轻视这个“简单”的乘法了。

4.1 三个基本旋转矩阵

任何三维旋转，都可以分解成绕三个坐标轴的旋转。我们先定义绕 X、Y、Z 轴旋转角度 θ 的基本矩阵。

绕 X 轴旋转（Roll，滚转）：

R_x(θ) = [[1,       0,        0],
          [0,  cos(θ), -sin(θ)],
          [0,  sin(θ),  cos(θ)]]

绕 Y 轴旋转（Pitch，俯仰）：

R_y(θ) = [[ cos(θ), 0,  sin(θ)],
          [       0, 1,       0],
          [-sin(θ), 0,  cos(θ)]]

绕 Z 轴旋转（Yaw，偏航）：

R_z(θ) = [[cos(θ), -sin(θ), 0],
          [sin(θ),  cos(θ), 0],
          [       0,       0, 1]]

这里有个细节我提醒一下：绕 Y 轴的矩阵里，左下角的 sin(θ) 是负号，右上角是正号。 这个跟绕 X 和绕 Z 的符号分布不一样。我见过不少新手在这里把符号写反，结果旋转方向完全反了。

4.2 旋转顺序：ZYX 内旋（最常用）

在航空航天领域，最常用的欧拉角顺序是 ZYX 内旋。也就是：先绕 Z 轴转 Yaw，再绕新的 Y 轴转 Pitch，最后绕最新的 X 轴转 Roll。

用数学语言表达：

R = R_z(Yaw) * R_y(Pitch) * R_x(Roll)

注意乘法的顺序！矩阵乘法是从右往左生效的。 所以最右边的 R_x(Roll) 先作用，然后是 R_y(Pitch)，最后是 R_z(Yaw)。

把三个矩阵乘开，得到完整的旋转矩阵：

R = [[cos(Y)*cos(P), cos(Y)*sin(P)*sin(R) - sin(Y)*cos(R), cos(Y)*sin(P)*cos(R) + sin(Y)*sin(R)],
     [sin(Y)*cos(P), sin(Y)*sin(P)*sin(R) + cos(Y)*cos(R), sin(Y)*sin(P)*cos(R) - cos(Y)*sin(R)],
     [-sin(P),       cos(P)*sin(R),                        cos(P)*cos(R)]]

其中 R = Roll, P = Pitch, Y = Yaw。

我的个人习惯： 每次写完这个矩阵，我都会用几个特殊角度验证一下。比如 Roll=0, Pitch=0, Yaw=90°，结果应该是一个绕 Z 轴转 90° 的矩阵。如果不对，那肯定是符号或者顺序错了。这个小习惯帮我避免过好几次低级错误。

4.3 其他常见顺序

除了 ZYX，工程中还会用到其他顺序。我整理了一个表格，方便你查阅：

顺序 典型应用场景 矩阵乘法顺序
ZYX 航空航天（无人机、飞机） R_z * R_y * R_x
ZYZ 机器人学（尤拉角经典定义） R_z * R_y * R_z
XYZ 计算机图形学（固定轴） R_x * R_y * R_z
YXZ 某些惯性导航系统 R_y * R_x * R_z

你看，不同领域用的顺序都不一样。所以写代码的时候，一定要在注释里明确标注你用的是哪种顺序。 不然别人接手你的代码，或者你自己三个月后回来看，很容易搞混。

4.4 Python 代码实现

好了，理论讲完，上代码。我会用 NumPy 来实现，因为这是最常用的科学计算库。

import numpy as np

def euler_to_rotation_matrix(roll, pitch, yaw, order='zyx'):
    """
    将欧拉角转换为旋转矩阵
    
    参数:
        roll  : float - 滚转角 (弧度)
        pitch : float - 俯仰角 (弧度)
        yaw   : float - 偏航角 (弧度)
        order : str   - 旋转顺序，默认 'zyx' (内旋)
    
    返回:
        R : numpy.ndarray - 3x3 旋转矩阵
    """
    # 计算三角函数值
    cr = np.cos(roll)
    sr = np.sin(roll)
    cp = np.cos(pitch)
    sp = np.sin(pitch)
    cy = np.cos(yaw)
    sy = np.sin(yaw)
    
    if order.lower() == 'zyx':
        # ZYX 内旋：R = R_z(yaw) * R_y(pitch) * R_x(roll)
        R = 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]
        ])
    elif order.lower() == 'zyz':
        # ZYZ 内旋：R = R_z(yaw) * R_y(pitch) * R_z(roll)
        R = np.array([
            [cy*cp*cr - sy*sr,  -cy*cp*sr - sy*cr,  cy*sp],
            [sy*cp*cr + cy*sr,  -sy*cp*sr + cy*cr,  sy*sp],
            [-sp*cr,            sp*sr,              cp]
        ])
    elif order.lower() == 'xyz':
        # XYZ 外旋（固定轴）：R = R_x(roll) * R_y(pitch) * R_z(yaw)
        R = np.array([
            [cp*cy,  -cp*sy,  sp],
            [sr*sp*cy + cr*sy,  -sr*sp*sy + cr*cy,  -sr*cp],
            [-cr*sp*cy + sr*sy,  cr*sp*sy + sr*cy,  cr*cp]
        ])
    else:
        raise ValueError(f"不支持的旋转顺序: {order}")
    
    return R

# 示例：Roll=30°, Pitch=45°, Yaw=60° (转为弧度)
roll_deg = 30
pitch_deg = 45
yaw_deg = 60

roll_rad = np.deg2rad(roll_deg)
pitch_rad = np.deg2rad(pitch_deg)
yaw_rad = np.deg2rad(yaw_deg)

R = euler_to_rotation_matrix(roll_rad, pitch_rad, yaw_rad, order='zyx')
print("旋转矩阵 (ZYX 内旋):")
print(np.round(R, 4))

运行这段代码，你会得到：

旋转矩阵 (ZYX 内旋):
[[ 0.3536 -0.5732  0.7392]
 [ 0.6124  0.7392  0.2803]
 [-0.7071  0.3536  0.6124]]

注意： 上面的代码默认输入是弧度。如果你习惯用角度，记得用 np.deg2rad() 转换。我曾经在写一个实时飞控程序时，忘了做这个转换，结果无人机起飞后直接翻了180°……还好是在仿真环境里测试的。

4.5 验证你的旋转矩阵

写完了代码，怎么知道算出来的矩阵对不对？我一般用三个方法验证：

  1. 行列式检查： 旋转矩阵的行列式必须等于 1（或者非常接近 1，考虑浮点误差）。如果行列式是 0 或者负数，那肯定错了。
  2. 正交性检查： R * R^T 应该等于单位矩阵。也就是矩阵的每一行（或每一列）都是单位向量，且互相垂直。
  3. 特殊角度测试： 比如 Roll=0, Pitch=0, Yaw=90°，结果应该是一个绕 Z 轴转 90° 的矩阵。

这里我写了一个验证函数，你可以直接拿去用：

def validate_rotation_matrix(R):
    """验证旋转矩阵的有效性"""
    # 检查行列式
    det = np.linalg.det(R)
    if not np.isclose(det, 1.0, atol=1e-6):
        print(f"警告：行列式 = {det:.6f}，应为 1.0")
        return False
    
    # 检查正交性
    I = R @ R.T
    if not np.allclose(I, np.eye(3), atol=1e-6):
        print("警告：矩阵不是正交的")
        return False
    
    print("验证通过：矩阵是有效的旋转矩阵")
    return True

# 验证上面算出来的矩阵
validate_rotation_matrix(R)

4.6 知识体系图

为了让你更直观地理解这一节的内容，我画了一张流程图：

欧拉角 → 旋转矩阵 知识体系 欧拉角 (Roll, Pitch, Yaw) R_x(θ) - 绕X轴 (Roll) R_y(θ) - 绕Y轴 (Pitch) R_z(θ) - 绕Z轴 (Yaw) R = R_z * R_y * R_x (ZYX内旋) 3×3 旋转矩阵

这张图清晰地展示了整个流程：输入欧拉角 → 分别计算三个基本旋转矩阵 → 按照指定的顺序相乘 → 输出最终的旋转矩阵。

避坑指南： 如果你在做无人机飞控，我强烈建议你使用 ZYX 内旋顺序。这是航空领域的标准，几乎所有飞控库（如 PX4、ArduPilot）都默认用这个顺序。如果你用别的顺序，跟其他模块对接时很容易出问题。

好了，这一节的内容就到这里。代码可以直接复制到你的项目里用，验证函数也一并附上了。记住：旋转矩阵是姿态计算的基石，搞懂它，后面的四元数、姿态插值、卡尔曼滤波都会轻松很多。

公众号:蓝海资料掘金营，微信deep3321

← 上一章 📖 返回目录 下一章 →