3、坐标系与变换基础：二维/三维坐标系、齐次坐标、旋转矩阵与欧拉角、使用NumPy实现坐标变换

各位同学，欢迎来到坐标系与变换这一章。

说实话，这部分内容在机器人领域里，就像地基一样。你后面做的路径规划、运动学解算、传感器融合，哪一步都离不开它。我当年刚入行时，就因为搞混了旋转顺序，让一台机械臂在调试现场差点撞了人。嗯，从那以后，我对坐标系变换就再也不敢马虎了。

3.1 二维与三维坐标系

我们先从最基础的聊起。坐标系，说白了就是给空间里的点一个「身份证号」。

• 二维坐标系：通常用 (x, y) 表示。比如一张图纸上的点，或者你手机屏幕上的像素位置。
• 三维坐标系：用 (x, y, z) 表示。我们生活的物理空间就是三维的。

这里有个关键点：右手定则。我建议你养成习惯，所有三维坐标系都按右手系来建立。拇指指向X轴，食指指向Y轴，中指指向Z轴。为什么？因为大部分机器人库（比如ROS、PyBullet）都默认右手系。你非要用左手系，后面跟别人联调时，数据对不上，那可就头疼了。

3.2 齐次坐标：为什么需要它？

你想想看，如果我们只用 (x, y, z) 表示一个点，那旋转可以用矩阵乘法搞定。但平移呢？平移是加法。旋转是乘法。在一个公式里又加又乘，写起来很麻烦。

齐次坐标就是来解决这个问题的。它给三维点加了一个维度 w，变成 (x, y, z, w)。

• 当 w = 1 时，表示一个点。
• 当 w = 0 时，表示一个方向向量（平移对它无效）。

这样一来，旋转和平移就可以统一写成一个 4x4 的矩阵乘法。我在项目中遇到过很多新手，写代码时忘了把 w 设成 1，结果点跑到无穷远去了。这个坑，你一定要注意。

3.3 旋转矩阵与欧拉角

旋转矩阵是一个 3x3 的正交矩阵，行列式为 +1。它描述了从一个坐标系到另一个坐标系的旋转关系。

但旋转矩阵有9个元素，人脑很难直观理解。所以就有了欧拉角。

欧拉角用三个角度（比如绕X轴转 α，绕Y轴转 β，绕Z轴转 γ）来描述旋转。听起来很直观，对吧？

但是！ 欧拉角有个大坑——万向锁。当中间那个轴旋转到 ±90° 时，会丢失一个自由度。我记得有一次做无人机仿真，就是因为欧拉角导致姿态解算突然跳变，飞机直接翻了个跟头。

3.4 使用NumPy实现坐标变换

理论说完了，我们上手写代码。我习惯用 NumPy 来操作，因为它矩阵运算快，而且生态好。

import numpy as np

# 定义一个三维点
point = np.array([1.0, 2.0, 3.0, 1.0])  # 齐次坐标

# 定义绕Z轴旋转30度的旋转矩阵
theta = np.radians(30)
cos_t = np.cos(theta)
sin_t = np.sin(theta)

R_z = np.array([
    [cos_t, -sin_t, 0, 0],
    [sin_t,  cos_t, 0, 0],
    [0,       0,    1, 0],
    [0,       0,    0, 1]
])

# 定义平移向量 (沿X轴移动5个单位)
T = np.array([
    [1, 0, 0, 5],
    [0, 1, 0, 0],
    [0, 0, 1, 0],
    [0, 0, 0, 1]
])

# 组合变换：先旋转，再平移
transform = T @ R_z

# 应用变换
point_transformed = transform @ point

print("原始点:", point[:3])
print("变换后点:", point_transformed[:3])

你看，代码很简洁。但这里有个细节：transform = T @ R_z 表示先应用 R_z 旋转，再应用 T 平移。顺序不能搞反。矩阵乘法不满足交换律，这个你肯定知道。

3.5 知识体系总览

为了让你对整个章节有个全局认识，我画了一张图。它把坐标系、齐次坐标、旋转表示方法以及代码实现串在了一起。

3.6 总结与个人经验

这一章的内容，说白了就是三件事：

1. 坐标系：选右手系，别搞特殊。
2. 齐次坐标：把平移和旋转统一成矩阵乘法。
3. 旋转表示：内部用矩阵，界面用欧拉角，小心万向锁。

我个人习惯在写任何变换代码之前，先在纸上画一下坐标系关系图。画清楚了，代码基本不会错。你也不妨试试。

好了，坐标系与变换基础就讲到这里。这部分内容虽然基础，但真的值得你多花时间吃透。后面我们讲运动学、路径规划时，你会感谢现在认真学坐标变换的自己。