如何用Python实现机器人坐标变换?

如何用Python实现机器人坐标变换？

在机器人学中，坐标变换是核心基础，它回答了“我在哪？”和“目标在哪？”这两个关键问题。通常，我们使用齐次变换矩阵来统一描述空间中的旋转和平移。

下面我将介绍几种用 Python 实现机器人坐标变换的常用方法，从基础原理到高级库的使用。

🧱 方法一：使用 NumPy 手动实现（理解原理）

这是最基础的方法，通过构建旋转矩阵和平移向量，然后组合成齐次变换矩阵。这有助于你深刻理解底层的数学原理。

python

编辑

import numpy as np

def rotation_matrix_z(theta):

    """绕Z轴旋转的旋转矩阵"""

    c = np.cos(theta)

    s = np.sin(theta)

    return np.array([

        [c, -s, 0],

        [s,  c, 0],

        [0,  0, 1]

    ])

def translation_vector(x, y, z):

    """平移向量"""

    return np.array([x, y, z])

def create_homogeneous_matrix(rotation, translation):

    """组合成4x4齐次变换矩阵"""

    T = np.eye(4) # 创建单位矩阵

    T[:3, :3] = rotation # 左上角3x3为旋转矩阵

    T[:3, 3] = translation # 右上角为平移向量

    return T

# --- 示例：机械臂末端从基座到目标的变换 ---

# 1. 绕Z轴旋转90度 (pi/2 弧度)

R = rotation_matrix_z(np.pi / 2)

# 2. 沿X轴平移2个单位，沿Y轴平移1个单位

t = translation_vector(2, 1, 0)

# 3. 创建齐次变换矩阵

T_base_to_end = create_homogeneous_matrix(R, t)

print("基座到末端的齐次变换矩阵:")

print(T_base_to_end)

⚙️ 方法二：使用 transforms3d 库（推荐）

transforms3d 是一个专门处理三维空间变换的库，它极大地简化了代码，避免了手动编写繁琐的三角函数公式。

安装库：

bash

编辑

pip install transforms3d

代码示例：

python

编辑

import numpy as np

import transforms3d as tfs

# --- 场景：已知旋转（欧拉角）和平移，构建齐次矩阵 ---

# 1. 定义平移向量 T (x, y, z)

T = np.array([1.0, 2.0, 3.0])

# 2. 定义旋转 (这里使用欧拉角，顺序为 'rxyz' 表示绕固定轴X、Y、Z旋转)

# 假设绕X轴转30度，Y轴转45度，Z轴转0度

euler_angles = np.radians([30, 45, 0]) 

R = tfs.euler.euler2mat(*euler_angles, axes='rxyz')

# 3. 使用 tfs 直接合成齐次变换矩阵

# tfs.affines.compose(平移, 旋转, 缩放) -> 注意：缩放通常为[1,1,1]

T_matrix = tfs.affines.compose(T, R, np.ones(3))

print("使用 transforms3d 生成的齐次矩阵:")

print(T_matrix)

# --- 场景：分解齐次矩阵，获取欧拉角和平移 ---

decomposed_euler = tfs.euler.mat2euler(T_matrix[:3, :3], axes='rxyz')

decomposed_translation = T_matrix[:3, 3]

print(f"分解得到的欧拉角 (弧度): {decomposed_euler}")

print(f"分解得到的平移向量: {decomposed_translation}")

🤖 方法三：在 ROS2 中发布与监听坐标变换

如果你在机器人操作系统 (ROS2) 环境中开发，通常会使用 tf2 库来管理和广播坐标变换树。Python 代码主要负责发布坐标关系。

代码示例：发布静态坐标变换（如相机相对于机械臂基座的位置）

python

编辑

import rclpy

from rclpy.node import Node

from tf2_ros import StaticTransformBroadcaster

from geometry_msgs.msg import TransformStamped

import tf_transformations as tfs  # 注意：ROS中常用这个，与transforms3d类似

class FramePublisher(Node):

    def __init__(self):

        super().__init__('static_frame_publisher')

        # 创建静态广播器

        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # 创建变换消息

        static_transformStamped = TransformStamped()

        static_transformStamped.header.stamp = self.get_clock().now().to_msg()

        static_transformStamped.header.frame_id = 'base_link'      # 父坐标系 (机械臂基座)

        static_transformStamped.child_frame_id = 'camera_link'    # 子坐标系 (相机)

        # 设置平移 (x, y, z)

        static_transformStamped.transform.translation.x = 0.1

        static_transformStamped.transform.translation.y = 0.0

        static_transformStamped.transform.translation.z = 0.2

        # 设置旋转 (四元数 w, x, y, z)

        # 例如：绕Z轴旋转90度

        quat = tfs.quaternion_from_euler(0, 0, np.pi/2) 

        static_transformStamped.transform.rotation.w = quat[0]

        static_transformStamped.transform.rotation.x = quat[1]

        static_transformStamped.transform.rotation.y = quat[2]

        static_transformStamped.transform.rotation.z = quat[3]

        # 发布变换        self.tf_static_broadcaster.sendTransform(static_transformStamped)

def main():

    rclpy.init()

    node = FramePublisher()

    try:

        rclpy.spin(node)

    except KeyboardInterrupt:

        pass

    finally:

        node.destroy_node()

        rclpy.shutdown()

if __name__ == '__main__':

    main()

总结与对比