别再死记公式了！用NumPy和SymPy玩转RPY角与旋转矩阵（可视化理解万向节锁）

用Python可视化理解RPY角与旋转矩阵：从基础到万向节锁实战

在机器人控制和3D视觉领域，RPY角（Roll-Pitch-Yaw）与旋转矩阵的相互转换是基础却容易让人困惑的知识点。传统教材往往堆砌公式推导，而本文将带你用Python工具链（NumPy+SymPy+Matplotlib）通过可视化手段直观理解这一转换过程，特别是万向节锁这一关键现象。

1. 基础概念与Python工具准备

RPY角是描述物体在三维空间中姿态的一种方式，分别代表绕固定坐标系的X（Roll）、Y（Pitch）、Z（Yaw）轴的旋转角度。与欧拉角不同，RPY角始终相对于固定坐标系进行旋转。

必备工具安装：

pip install numpy sympy matplotlib ipympl

在Jupyter Notebook中启用交互式绘图：

%matplotlib widget

为什么选择这个工具组合？

• NumPy：高效的矩阵运算基础库
• SymPy：符号计算验证公式正确性
• Matplotlib：直观展示三维旋转效果

2. RPY到旋转矩阵的Python实现

ZYX顺序的RPY角转换是最常见的工业标准。让我们分解这个转换过程：

import numpy as np
from math import cos, sin

def rpy_to_matrix(roll, pitch, yaw):
    """ZYX顺序的RPY角转旋转矩阵"""
    Rx = np.array([[1, 0, 0],
                   [0, cos(roll), -sin(roll)],
                   [0, sin(roll), cos(roll)]])
    
    Ry = np.array([[cos(pitch), 0, sin(pitch)],
                   [0, 1, 0],
                   [-sin(pitch), 0, cos(pitch)]])
    
    Rz = np.array([[cos(yaw), -sin(yaw), 0],
                   [sin(yaw), cos(yaw), 0],
                   [0, 0, 1]])
    
    return Rz @ Ry @ Rx  # 注意矩阵乘法顺序

验证转换正确性的SymPy代码：

from sympy import symbols, Matrix, simplify

a, b, c = symbols('a