2. 坐标系与位姿表示：世界坐标系、机器人坐标系、齐次变换矩阵、欧拉角与四元数

好，咱们正式开始聊轨迹规划里最基础、也最绕不开的话题——坐标系与位姿表示。

说实话，我刚开始做机器人那会儿，觉得坐标系这东西太简单了。不就是三个轴嘛，谁不会？结果第一次调试机械臂，发现末端执行器死活对不准目标点。折腾了一下午，最后发现是坐标系搞反了。嗯，从那以后，我再也不敢小看这部分内容了。

2.1 世界坐标系 vs 机器人坐标系

先说说这两个最常用的坐标系。

世界坐标系，说白了就是整个场景的“参考系”。你可以把它想象成房间里的一个固定墙角——原点在那，X轴指向东，Y轴指向北，Z轴指向天。不管机器人怎么动，这个坐标系纹丝不动。

机器人坐标系，也叫基坐标系或工具坐标系，是跟着机器人走的。比如机械臂的基座有一个坐标系，末端夹爪也有一个坐标系。我习惯把机器人坐标系想象成“机器人自己的小世界”。

为什么会需要两套坐标系？你想想看：

• 你要告诉机器人“去抓桌子上的杯子”——这是世界坐标系的任务
• 你要告诉机器人“手腕旋转30度”——这是机器人坐标系的事

我在项目里遇到过最典型的坑：用世界坐标系去规划机械臂的关节运动，结果机械臂差点撞到旁边的设备。因为世界坐标系下的直线路径，在机器人关节空间里可能是个大弧线。

2.2 齐次变换矩阵——位姿的“瑞士军刀”

聊到位姿表示，就绕不开齐次变换矩阵。这东西看起来就是个4x4的矩阵，但用起来是真方便。

# 齐次变换矩阵的标准形式
T = [[R, t],
     [0, 1]]

# 其中 R 是 3x3 旋转矩阵
# t 是 3x1 平移向量
# 最后一行是 [0, 0, 0, 1]

我个人习惯把齐次变换矩阵看作“坐标转换器”。你有一个点在世界坐标系下的坐标，乘以这个矩阵，就能得到它在机器人坐标系下的坐标。

举个例子：

import numpy as np

# 定义旋转矩阵（绕Z轴转45度）
theta = np.pi/4
R = np.array([[np.cos(theta), -np.sin(theta), 0],
              [np.sin(theta),  np.cos(theta), 0],
              [0,              0,             1]])

# 定义平移向量（沿X轴移动1米）
t = np.array([[1], [0], [0]])

# 构建齐次变换矩阵
T = np.vstack([np.hstack([R, t]), [0, 0, 0, 1]])

print("齐次变换矩阵：")
print(T)

输出结果：

齐次变换矩阵：
[[ 0.7071 -0.7071  0.      1.    ]
 [ 0.7071  0.7071  0.      0.    ]
 [ 0.      0.      1.      0.    ]
 [ 0.      0.      0.      1.    ]]

你看，一个矩阵同时包含了旋转和平移信息。而且多个变换可以连乘——比如从世界到基座，再从基座到末端，最后从末端到工具，一串矩阵乘下来，就得到了从世界到工具的完整变换。

2.3 欧拉角——直观但暗藏玄机

欧拉角可能是最直观的旋转表示方法了。绕X轴转叫横滚，绕Y轴转叫俯仰，绕Z轴转叫偏航。飞机、无人机、机器人手臂，到处都在用。

但欧拉角有个大问题——万向锁。当俯仰角达到±90度时，横滚和偏航就分不清了。我调试六轴机械臂时就遇到过：明明只给了两个轴的旋转指令，结果第三个轴莫名其妙也跟着转。

表示方式	优点	缺点
欧拉角 (RPY)	直观，容易理解	万向锁，插值不平滑
旋转矩阵	无奇异性，可连乘	9个参数，冗余
四元数	无奇异性，插值平滑	不够直观

所以我的建议是：人机交互时用欧拉角（方便理解），内部计算时用四元数或旋转矩阵（避免问题）。

2.4 四元数——平滑旋转的利器

四元数听起来高大上，其实没那么玄乎。它用四个数表示旋转：一个实部和三个虚部。

q = w + xi + yj + zk

# 其中 w 是实部
# x, y, z 是虚部
# 且满足 i² = j² = k² = ijk = -1

为什么用四元数？因为它做旋转插值时特别平滑。比如你要让机械臂从姿态A平滑过渡到姿态B，用欧拉角插值可能会出现“抖动”，但四元数的球面线性插值（SLERP）能保证路径最短、最平滑。

from scipy.spatial.transform import Rotation as R
import numpy as np

# 定义两个姿态（用欧拉角）
euler_a = [0, 0, 0]      # 初始姿态
euler_b = [90, 0, 0]     # 绕X轴转90度

# 转成四元数
quat_a = R.from_euler('xyz', euler_a, degrees=True).as_quat()
quat_b = R.from_euler('xyz', euler_b, degrees=True).as_quat()

# SLERP插值（取中间点）
t = 0.5
quat_mid = R.from_quat(quat_a).slerp(t, R.from_quat(quat_b)).as_quat()

print(f"初始四元数: {quat_a}")
print(f"中间四元数: {quat_mid}")
print(f"目标四元数: {quat_b}")

2.5 知识体系总览

下面这张图是我自己总结的坐标系与位姿表示的知识结构，帮你理清思路：

总结一下：坐标系定义了“在哪里”，位姿表示定义了“怎么转”。齐次变换矩阵是通用工具，欧拉角适合人看，四元数适合机器算。做轨迹规划时，我建议你：

• 用世界坐标系定义任务目标
• 用机器人坐标系执行运动
• 用齐次变换矩阵做坐标转换
• 用四元数做旋转插值
• 用欧拉角做调试和显示

嗯，这套组合拳打下来，坐标系和位姿这块基本就不会出问题了。