机器人位姿转换实战：从欧拉角到RPY的5分钟快速指南（附Python代码）

在机器人控制和自动化工程领域，位姿数据的准确转换是确保机械臂精准运动的基础。想象一下，当你需要让机械臂从一个位置平滑移动到另一个位置时，不同的控制系统可能使用不同的姿态表示方法——有的偏好欧拉角，有的采用RPY角。这就好比两个人用不同语言描述同一个动作，需要一位"翻译官"来确保沟通无误。本文将扮演这个翻译官的角色，带你快速掌握两种主流位姿表示法的转换技巧。

1. 位姿表示法基础认知

机器人位姿包含位置和姿态两部分信息，通常表示为{X,Y,Z,A,B,C}或{X,Y,Z,Rx,Ry,Rz}。其中前三个参数代表三维空间坐标，后三个参数则描述姿态。虽然看起来相似，但欧拉角和RPY角的旋转顺序和轴定义存在关键差异：

• KUKA系欧拉角：采用Z-Y-X旋转顺序

  • A：绕Z轴旋转角度（偏航角Yaw）
  • B：绕Y轴旋转角度（俯仰角Pitch）
  • C：绕X轴旋转角度（横滚角Roll）

• 新松系RPY角：采用X-Y-Z旋转顺序

  • Rx：绕X轴旋转角度（横滚角Roll）
  • Ry：绕Y轴旋转角度（俯仰角Pitch）
  • Rz：绕Z轴旋转角度（偏航角Yaw）

这两种表示法的对比如下：

理解这个差异至关重要——就像知道摄氏度和华氏度的转换关系一样，这是后续所有计算的基础。

2. 欧拉角与RPY角的转换原理

两种表示法间的转换本质上是旋转顺序的调整。由于最终姿态相同，只是描述方式不同，我们可以通过旋转矩阵这个"中间人"来实现转换。具体步骤分为三个阶段：

1. 欧拉角→旋转矩阵：

   def euler_to_matrix(a, b, c):
       """Z-Y-X顺序欧拉角转旋转矩阵"""
       import math
       rz = [[math.cos(a), -math.sin(a), 0],
             [math.sin(a),  math.cos(a), 0],
             [0,           0,          1]]
       ry = [[math.cos(b),  0, math.sin(b)],
             [0,           1,          0],
             [-math.sin(b), 0, math.cos(b)]]
       rx = [[1,          0,           0],
             [0, math.cos(c), -math.sin(c)],
             [0, math.sin(c),  math.cos(c)]]
       # 矩阵乘法顺序Z→Y→X
       return np.dot(np.dot(rz, ry), rx)
   

2. 旋转矩阵→RPY角：

   def matrix_to_rpy(R):
       """旋转矩阵转X-Y-Z顺序RPY角"""
       import math
       ry = math.atan2(-R[2,0], math.sqrt(R[0,0]**2 + R[1,0]**2))
       rz = math.atan2(R[1,0]/math.cos(ry), R[0,0]/math.cos(ry))
       rx = math.atan2(R[2,1]/math.cos(ry), R[2,2]/math.cos(ry))
       return rx, ry, rz
   

3. 直接转换公式（适用于Z-Y-X欧拉角与X-Y-Z RPY角）：

   Rx = C
   Ry = B
   Rz = A
   

   这个简单关系成立的前提是确认旋转顺序匹配。在实际项目中，建议先用旋转矩阵验证转换逻辑。

3. Python实战：完整转换函数实现

下面给出可直接集成到机器人项目中的转换工具类：

import numpy as np
import math

class PoseConverter:
    @staticmethod
    def euler_to_rpy(a, b, c, degrees=True):
        """KUKA欧拉角(Z-Y-X)转新松RPY角(X-Y-Z)
        参数：
            a,b,c: 欧拉角ABC分量
            degrees: 输入是否为角度制
        返回：
            rx, ry, rz: RPY角三个分量
        """
        if degrees:
            a, b, c = math.radians(a), math.radians(b), math.radians(c)
        
        # 直接转换关系
        rx = c  # X轴旋转对应欧拉角的C分量
        ry = b  # Y轴旋转对应欧拉角的B分量
        rz = a  # Z轴旋转对应欧拉角的A分量
        
        if degrees:
            return math.degrees(rx), math.degrees(ry), math.degrees(rz)
        return rx, ry, rz

    @staticmethod
    def rpy_to_euler(rx, ry, rz, degrees=True):
        """新松RPY角(X-Y-Z)转KUKA欧拉角(Z-Y-X)"""
        return PoseConverter.euler_to_rpy(rz, ry, rx, degrees)

    @staticmethod
    def verify_conversion(a, b, c, degrees=True):
        """验证转换正确性：欧拉角→RPY→欧拉角应得到原始值"""
        rx, ry, rz = PoseConverter.euler_to_rpy(a, b, c, degrees)
        a2, b2, c2 = PoseConverter.rpy_to_euler(rx, ry, rz, degrees)
        return np.allclose([a,b,c], [a2,b2,c2])

使用示例：

# 将KUKA机器人位姿{100,200,300,30,20,10}转换为新松格式
x, y, z = 100, 200, 300
a, b, c = 30, 20, 10

# 转换姿态部分
rx, ry, rz = PoseConverter.euler_to_rpy(a, b, c)
print(f"新松位姿: {x}, {y}, {z}, {rx:.2f}, {ry:.2f}, {rz:.2f}")

# 验证转换正确性
assert PoseConverter.verify_conversion(a, b, c)

4. 工程应用中的注意事项

在实际机器人项目中应用这些转换时，有几个关键点需要特别注意：

1. 旋转顺序一致性：

   • 确认你的欧拉角定义确实是Z-Y-X顺序
   • ABB机器人使用Z-Y-Z顺序，此时转换公式完全不同
   • 当不确定时，通过小角度值（如90°）进行验证

2. 万向节锁问题：

   • 当俯仰角B=±90°时会出现万向节锁
   • 此时A和C角度会耦合，导致解不唯一
   • 解决方案：切换到四元数表示法或限制机械臂运动范围

3. 单位统一原则：

   # 错误示例：混合使用弧度和角度
   rx = math.sin(30)  # 30是角度还是弧度？
   
   # 正确做法：明确单位
   angle = math.radians(30) if degrees else 30
   rx = math.sin(angle)
   

4. 工业机器人特殊处理：

   • KUKA的EulerZYX与标准定义可能有符号差异
   • Fanuc机器人使用RPY但旋转顺序可能是Z-Y-X
   • 建议通过机器人厂商的文档确认具体定义

下表总结了常见机器人品牌的位姿表示特点：

5. 性能优化与扩展应用

对于需要高频调用转换算法的场景，如实时轨迹规划，可以考虑以下优化策略：

1. 预计算三角函数：

   # 普通计算
   rz = [[cos(a), -sin(a), 0], [sin(a), cos(a), 0], [0,0,1]]
   
   # 优化版本：预先计算sin/cos值
   ca, sa = math.cos(a), math.sin(a)
   rz = [[ca, -sa, 0], [sa, ca, 0], [0,0,1]]
   

2. 使用NumPy向量化运算：

   def batch_convert(euler_angles):
       """批量转换欧拉角到RPY"""
       angles = np.radians(euler_angles)
       rpy = angles[:, [2,1,0]]  # 利用numpy数组高效重排
       return np.degrees(rpy)
   

3. Cython加速关键代码：

   # 编译为C扩展提升性能
   # pose_converter.pyx
   import numpy as np
   cimport numpy as np
   
   def cython_euler_to_rpy(double a, double b, double c):
       cdef double rx = c
       cdef double ry = b
       cdef double rz = a
       return rx, ry, rz
   

4. ROS集成方案： 对于使用Robot Operating System的开发者，可以直接利用tf库：

   from tf.transformations import euler_from_matrix, quaternion_from_euler
   
   def ros_euler_to_rpy(a, b, c):
       q = quaternion_from_euler(a, b, c, axes='rzyx')
       return euler_from_quaternion(q, axes='sxyz')
   

在机械臂轨迹平滑处理中，正确的位姿转换可以避免"跳跃"现象。我曾在一个焊接机器人项目中遇到这样的情况：当机械臂接近奇异点时，由于欧拉角突变导致轨迹不平滑。通过切换到RPY表示并添加过渡点，最终实现了毫米级精度的连续焊接路径。