3. 坐标系与变换：世界坐标系、机体坐标系、齐次变换矩阵、坐标变换链
好，我们直接进入正题。坐标系这东西，说白了就是给机器人一个「定位」的基准。你想想看，一个六轴机械臂在空间里乱舞，如果没有坐标系，你根本不知道它每个关节到底在哪儿。我刚开始做机器人控制时，就吃过这个亏——坐标系搞反了，结果仿真里看着挺正常，一上真机，直接撞到工作台上。嗯，从那以后，我对坐标系这件事就特别较真。
3.1 世界坐标系与机体坐标系
先讲两个最基础的坐标系。
世界坐标系，也叫全局坐标系。它是固定不动的，通常定义在机器人的基座或者工作空间的某个角落。你可以把它想象成整个场景的「绝对参考系」。所有物体的位置，最终都要换算到这个坐标系下才有意义。
机体坐标系，也叫局部坐标系。它是跟着机器人每个连杆一起动的。比如末端执行器上有一个坐标系，它的原点就在夹爪中心，轴方向跟着夹爪的朝向走。我习惯把机体坐标系想象成「贴在零件上的标签」——你动，它也动。
为什么要区分这两个？因为机器人的运动学计算，本质上就是在不同坐标系之间来回「翻译」。你在末端坐标系里规划了一条直线轨迹，但电机要转的角度得换算到世界坐标系下才能算出来。
核心概念：世界坐标系是「不动的地图」，机体坐标系是「会动的路标」。
3.2 齐次变换矩阵
好，现在问题来了：怎么把一个点从机体坐标系转换到世界坐标系？
答案就是齐次变换矩阵。它是个 4x4 的矩阵，长这样：
| R t |
| 0 1 |其中 R 是 3x3 的旋转矩阵，t 是 3x1 的平移向量。最后一行是 [0 0 0 1]，这是齐次坐标的固定格式。
为什么非要用 4x4？直接用 3x3 旋转加 3x1 平移不行吗？
嗯，这里有个坑。如果你只用 3x3 矩阵，旋转和平移是分开算的：先旋转，再平移。但如果你想把多个变换串起来，比如「先绕 X 轴转 30 度，再沿 Y 轴移动 5 厘米，再绕 Z 轴转 45 度」，用 3x3 矩阵你得写好几步，而且容易出错。齐次变换矩阵把旋转和平移统一到一个矩阵里，连续变换就是矩阵乘法，干净利落。
我曾经在项目里用纯 3x3 矩阵做链式变换，结果调试了三天，发现是平移顺序搞反了。后来全部换成齐次矩阵，半小时搞定。所以我的建议是：从一开始就用齐次变换矩阵，别偷懒。
个人技巧：写代码时，我习惯把齐次变换矩阵封装成一个类，包含旋转和平移的 set/get 方法。这样后续做运动学正解、逆解时，代码复用率极高。
3.3 坐标变换链
六自由度机器人有六个关节，每个关节都有一个机体坐标系。从基座到末端执行器，中间经过六个变换。这一串连续的变换，就叫坐标变换链。
举个例子：
- 基座坐标系 → 关节1坐标系 → 关节2坐标系 → ... → 末端坐标系
每个箭头对应一个齐次变换矩阵。把所有这些矩阵乘起来，就得到从基座到末端的总变换矩阵。这就是运动学正解的核心。
数学上写出来就是：
T_0_n = T_0_1 * T_1_2 * T_2_3 * ... * T_{n-1}_n注意顺序！矩阵乘法不满足交换律。先转再移和先移再转，结果完全不同。我见过不少新手在这里栽跟头——明明公式写对了，但代码里乘反了，结果末端位置飞到了十万八千里外。
避坑指南：我曾经在调试一个六轴协作机器人时，发现末端位置总是差一个固定偏移。查了两天，最后发现是变换矩阵的乘法顺序写反了——应该左乘新变换，我写成了右乘。记住：从基座往末端走，新变换要左乘到当前矩阵的左边。
3.4 知识体系结构图
下面这张图，是我自己总结的坐标系与变换的知识脉络。你可以把它当作本章的「地图」：
3.5 代码实现要点
讲完理论，咱们看看代码怎么写。我用 Python 和 NumPy 来演示，因为这是机器人学里最常用的组合。
import numpy as np
def build_homogeneous_matrix(R, t):
"""
构建齐次变换矩阵
R: 3x3 旋转矩阵
t: 3x1 平移向量
"""
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t.flatten()
return T
def transform_point(T, point):
"""
用齐次变换矩阵变换一个点
T: 4x4 齐次矩阵
point: 3x1 点坐标 (x, y, z)
"""
# 转为齐次坐标
p_homo = np.append(point, 1)
# 变换
p_transformed = T @ p_homo
# 转回笛卡尔坐标
return p_transformed[:3] / p_transformed[3]
# 示例：绕Z轴旋转30度，再沿X轴平移5
theta = np.radians(30)
R_z = np.array([
[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]
])
t = np.array([[5], [0], [0]])
T = build_homogeneous_matrix(R_z, t)
point = np.array([1, 0, 0])
result = transform_point(T, point)
print(f"变换后的点: {result}")这段代码我用了很多项目，基本没改过。你注意看 transform_point 函数里最后除以 p_transformed[3] 那一步——这是齐次坐标转回笛卡尔坐标的关键。如果平移向量是零，这一步不影响结果；但如果涉及缩放或投影变换，这一步就必不可少。
调试技巧：写变换矩阵时，我习惯先验证一下：把单位矩阵当作输入，看变换后点坐标是否不变。如果变了，说明矩阵构造有误。这个小测试能帮你省下大量排查时间。
3.6 常见错误与避坑
最后，我总结几个自己踩过的坑：
- 矩阵乘法顺序：前面说了，左乘和右乘结果不同。写代码时，我习惯用
@运算符，并明确注释当前是左乘还是右乘。 - 角度单位：NumPy 的三角函数默认用弧度。我见过有人直接传角度进去，结果旋转矩阵完全不对。我的做法是：在函数入口统一转弧度，内部全用弧度计算。
- 齐次坐标的最后一维：变换后一定要归一化，否则后续计算会出问题。尤其是做连续变换时，误差会累积。
- 坐标系定义一致性：整个项目里，所有坐标系的轴方向必须统一。比如 Z 轴向上还是 Y 轴向上？一旦混用，调试起来非常痛苦。
嗯，坐标系与变换这部分，说白了就是「定义清楚、计算准确、验证到位」。你只要把这三个原则记住，后面学运动学、动力学都会轻松很多。
公众号:蓝海资料掘金营，微信deep3321