坐标系与空间变换：世界坐标系、机器人基坐标系、工具坐标系、齐次变换矩阵、欧拉角与四元数
做机器人轨迹规划，说白了就是回答三个问题：我在哪？我要去哪？怎么去？
这三个问题，全得靠坐标系和空间变换来解决。我刚开始接触这个领域时，觉得坐标系不就是个数学概念嘛，有啥好学的？直到第一次调试机械臂，发现末端执行器死活对不准目标点……嗯，从那以后我再也不敢小看坐标系了。
1. 世界坐标系与机器人基坐标系
世界坐标系，你可以理解为整个工作空间的“参考系”。它通常固定在地面或工作台上，是所有其他坐标系的最终参照。
机器人基坐标系，则固定在机器人底座上。它定义了机器人的“原点”和“姿态”。
关键点：基坐标系与世界坐标系之间，通常有一个固定的变换关系。你安装机器人时，这个关系就定下来了。
我在项目中遇到过一个问题：两台机器人协同工作，各自的世界坐标系定义不同。结果一运行，两台机器人“各说各话”，差点撞上。后来我统一用激光跟踪仪标定了它们的世界坐标系，才解决。
2. 工具坐标系
工具坐标系（TCP，Tool Center Point），定义在机器人末端执行器上。比如你装了一个焊枪，TCP就在焊丝尖端；装了一个夹爪，TCP就在夹爪中心。
为什么要单独定义它？因为机器人控制的是法兰盘，而真正干活的是工具。你想想看，如果工具长度变了，你还用原来的坐标去算轨迹，那肯定偏了。
我的习惯：每次更换工具，第一件事就是重新标定TCP。别偷懒，否则后面调试会花更多时间。
3. 齐次变换矩阵
齐次变换矩阵，是描述坐标系之间关系的“万能工具”。它长这样：
| R t |
| 0 1 |其中，R 是 3x3 的旋转矩阵，t 是 3x1 的平移向量。这个矩阵可以把一个点从一个坐标系变换到另一个坐标系。
举个例子：
# 假设有一个点 P 在工具坐标系下的坐标是 [0, 0, 100]
# 工具坐标系相对于基坐标系的变换矩阵为 T
# 那么 P 在基坐标系下的坐标就是 T * P
import numpy as np
T = np.array([[1, 0, 0, 200],
[0, 1, 0, 100],
[0, 0, 1, 50],
[0, 0, 0, 1]])
P_tool = np.array([0, 0, 100, 1])
P_base = T @ P_tool
print(P_base[:3]) # 输出 [200, 100, 150]注意：齐次变换矩阵的乘法不满足交换律。先旋转后平移，和先平移后旋转，结果完全不同。我曾经因为搞错顺序，让机器人画了一个歪歪扭扭的圆……
4. 欧拉角与四元数
旋转矩阵虽然好用，但9个参数太冗余了。实际工程中，我们常用欧拉角和四元数来描述旋转。
4.1 欧拉角
欧拉角用三个角度（绕X、Y、Z轴的旋转）来描述姿态。常见的顺序有 ZYX、ZYZ 等。
优点：直观，容易理解。
缺点：有万向锁问题。当第二个旋转角为 ±90° 时，会丢失一个自由度。
避坑指南：我曾经在规划一个球面轨迹时，用了欧拉角插值。结果在某个点附近，机器人突然“抽搐”了一下。后来发现是万向锁导致的。从那以后，我只要涉及连续轨迹规划，一律用四元数。
4.2 四元数
四元数用四个参数表示旋转：q = [w, x, y, z]，其中 w 是实部，x, y, z 是虚部。
优点：无万向锁，插值平滑，计算效率高。
缺点：不够直观，调试时不好想象姿态。
# 四元数插值示例（SLERP）
from scipy.spatial.transform import Rotation as R
q_start = R.from_euler('xyz', [0, 0, 0]).as_quat()
q_end = R.from_euler('xyz', [90, 45, 0], degrees=True).as_quat()
# 插值 0.5
q_mid = R.from_quat(q_start).slerp(0.5, R.from_quat(q_end)).as_quat()
print(q_mid)5. 知识体系总览
下面这张图，是我自己总结的坐标系与空间变换的核心逻辑。你把它理清了，后面学轨迹规划会轻松很多。
6. 实战建议
- 标定优先：每次安装或更换工具后，先标定坐标系。别问我为什么强调这个，问就是吃过亏。
- 四元数为主：涉及连续轨迹规划时，尽量用四元数。欧拉角只适合做简单的姿态设定。
- 验证变换：写完变换代码后，用已知点验证一下。比如把工具坐标系原点变换到基坐标系，看看是不是你期望的位置。
一个小技巧：调试时，可以在机器人末端贴一个反光标记点，用激光跟踪仪实时测量它的位置。然后对比你算出来的理论位置。如果偏差超过1mm，说明你的变换矩阵有问题。
好了，坐标系与空间变换就讲到这里。这些东西看着简单，但真正用好需要大量实践。你多动手写写代码，多调调机器人，慢慢就熟了。