2. 运动学核心概念与坐标变换
上一讲直接给出了 FK 公式，但没有解释为什么第二项是 cos(θ1 + θ2) 而不是 cos(θ2)。这一讲从这个问题出发，推导坐标系变换，最终让 FK 公式从矩阵连乘里自然推出来。
前言：lesson_01 留下的一个问题
仔细看上面这张图：θ1 是连杆 L1 相对全局坐标系 F0（x 轴）的角度，θ2 是连杆 L2 相对连杆 L1 延长线的角度——注意图中虚线，θ2 的参考方向不是 x 轴，而是 L1 的方向。
在上一讲，我们直接给出了两连杆机械臂的正运动学公式：
x = L1 * cos(θ1) + L2 * cos(θ1 + θ2)
y = L1 * sin(θ1) + L2 * sin(θ1 + θ2)
你可能当时就有一个疑问：
为什么第二项是
cos(θ1 + θ2)，而不是cos(θ2)？
答案就藏在图里：θ2 是相对 L1 量的，不是相对全局 x 轴量的。要算末端在全局坐标系中的位置，就必须把这个"局部角度"转换到全局参考系——这个转换过程，就是本讲的核心。
理解了这一点，你就理解了：
- 为什么机器人需要多个坐标系
- 齐次变换矩阵是什么，为什么要用它
- FK 公式不是"背下来的"，而是从坐标系变换一步步推出来的
2.1 为什么 θ2 不是全局角度？
2.1.1 用手臂做类比
伸出你的右臂，想象：
- 你的肩膀是关节 1，大臂是连杆 1
- 你的肘部是关节 2，小臂是连杆 2
现在做一个动作：
- 大臂向前抬起 45°（θ1 = 45°，相对地面）
- 小臂相对大臂再弯曲 30°（θ2 = 30°，相对大臂）
问：小臂在全局空间里的方向是多少度？
答案是 75°，也就是 θ1 + θ2 = 45° + 30°。
╱ 小臂（全局方向 75°）
╱
* 肘部（关节 2） ← θ2 = 30° 是相对大臂量的
╱
╱ 大臂
╱
────*──────── 地面
肩部 ↑ θ1 = 45° 是相对地面量的
（关节 1）
这就是为什么公式里是 θ1 + θ2：θ2 是相对大臂的角度，不是相对地面的角度。
2.1.2 这就是"坐标系"问题的本质
θ2 是在大臂坐标系里定义的，不是在全局坐标系里定义的。
机器人的每个关节都只知道"自己相对上一级转了多少"，要算末端在全局的位置，就必须把这些局部角度一层层传递、累加。这棵层级结构，就是下一节要讲的坐标系树。
2.2 为什么机器人需要多个坐标系
2.2.1 生活类比：同一个地方，不同的描述
假设你在北京，朋友在上海。你们同时描述天安门广场的位置：
- 你说："在我北边 2 公里"
- 朋友说："在我北边 1200 公里"
两个描述都是正确的，只是参考点不同。
机器人里也一样：
- 相机说："杯子在我前方 0.3 米"（相机坐标系）
- 机械臂说："杯子在我右边 0.5 米、前方 0.2 米"（机械臂基座坐标系）
同一个杯子，在不同坐标系下，数字完全不同。
2.2.2 机械臂的坐标系树
一个两连杆机械臂有这样的坐标系结构：
world（世界坐标系）
└── base（机械臂基座坐标系）
└── link1（第一根连杆坐标系）
└── link2（第二根连杆坐标系）
└── end_effector（末端执行器坐标系）
每个坐标系都"附着"在对应的物理部件上，随着关节转动而转动。
为什么要这样设计？
因为每个关节只知道"自己相对于上一级转了多少角度"，不知道全局位置。把这些局部信息一层层传递，最终才能算出末端在全局的位置。
这棵坐标系树，在 ROS2 里由 tf2 负责管理。第 8 讲会详细讲 tf2，现在只需要知道"有这么一棵树"就够了。
2.2.3 坐标系之间的关系
相邻两个坐标系之间的关系，可以用两个量描述：
- 平移：子坐标系的原点，相对父坐标系原点偏移了多少
- 旋转：子坐标系的轴方向，相对父坐标系旋转了多少角度
父坐标系
│
│ 平移 (dx, dy)
│ 旋转 θ
▼
子坐标系
用数学表达旋转：
2.3 旋转矩阵——从直觉到数值
2.3.1 一个具体问题
有一个点 P，坐标是 (1, 0)。
现在把整个坐标系绕原点旋转 30°（或者等价地，把点 P 反向旋转 30°）。
旋转后，P 的新坐标是多少？
y
│
│
│ · P'(0.866, 0.500)
│ ·
│ ·
│ ·
│ ·
│ · ⌒30°
O──────────────────────── x
P(1,0)
用三角函数可以算出：
P'_x = cos(30°) = 0.866
P'_y = sin(30°) = 0.500
2.3.2 推广到任意点
如果点 P 的坐标是 (x, y)，绕原点旋转 θ 角后，新坐标是：
x' = x * cos(θ) - y * sin(θ)
y' = x * sin(θ) + y * cos(θ)
这个公式怎么来的？
把点 P 用极坐标表示：P = (r*cos(α), r*sin(α))，旋转 θ 后变成 (r*cos(α+θ), r*sin(α+θ))，展开用和角公式化简，就得到上面的公式。
2.3.3 写成矩阵形式
上面的公式可以写成矩阵乘法：
[x'] [cos(θ) -sin(θ)] [x]
[y'] = [sin(θ) cos(θ)] [y]
右边那个 2×2 矩阵，就叫做 2D 旋转矩阵，记作 R(θ)：
R(θ) = [cos(θ) -sin(θ)]
[sin(θ) cos(θ)]
2.3.4 数值验证
用 Python 验证一下：点 (1, 0) 旋转 30° 后的坐标。
import numpy as np
def rotation_matrix_2d(theta_deg):
"""
生成 2D 旋转矩阵
参数：theta_deg 旋转角度（度）
返回：2x2 旋转矩阵
"""
theta = np.radians(theta_deg)
R = np.array([
[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]
])
return R
# 点 P = (1, 0)，旋转 30°
R = rotation_matrix_2d(30)
P = np.array([1, 0])
P_new = R @ P # @ 是矩阵乘法运算符
print(f"旋转矩阵 R(30°):")
print(R)
print(f"\n旋转后的点: ({P_new[0]:.4f}, {P_new[1]:.4f})")
# 预期输出: (0.8660, 0.5000)
运行结果：
旋转矩阵 R(30°):
[[ 0.866 -0.5 ]
[ 0.5 0.866]]
旋转后的点: (0.8660, 0.5000)
和我们手算的结果完全一致。
2.3.5 旋转矩阵的直觉理解
旋转矩阵的两列，其实就是旋转后的坐标轴方向：
R(θ) = [cos(θ) -sin(θ)]
[sin(θ) cos(θ)]
↑列1 ↑列2
旋转后的x轴 旋转后的y轴
- 第 1 列
[cos(θ), sin(θ)]：原来的 x 轴，旋转 θ 后指向哪里 - 第 2 列
[-sin(θ), cos(θ)]：原来的 y 轴，旋转 θ 后指向哪里
这个理解方式在后面看 tf2 的输出时会很有用。
2.4 平移 + 旋转分开写的麻烦
2.4.1 只有旋转还不够
机器人的每个关节，不只是旋转，还有平移（连杆有长度）。
比如第一根连杆：
- 关节 1 旋转了 θ1
- 连杆 1 的末端（也就是关节 2 的位置）相对关节 1 平移了 (L1, 0)
如果分开写，一次变换需要两步：
# 第一步：旋转
P_rotated = R @ P
# 第二步：平移
P_final = P_rotated + translation
2.4.2 连续变换时的麻烦
如果有两段连杆，就需要做两次这样的操作：
# 第一段连杆的变换
P1 = R1 @ P0 + t1
# 第二段连杆的变换
P2 = R2 @ P1 + t2
# 展开后：
P2 = R2 @ (R1 @ P0 + t1) + t2
= R2 @ R1 @ P0 + R2 @ t1 + t2
三段连杆就更复杂了。而且旋转和平移混在一起，很容易出错。
有没有办法把旋转和平移合并成一个操作？
有，这就是齐次变换矩阵。
2.5 齐次变换矩阵——统一旋转和平移
2.5.1 核心思想：升维
齐次变换矩阵的思路是：给坐标加一个维度。
2D 的点 (x, y) 变成 (x, y, 1)，这个多出来的 1 是固定的，叫做齐次坐标。
然后用一个 3×3 矩阵，同时完成旋转和平移：
[x'] [cos(θ) -sin(θ) tx] [x]
[y'] = [sin(θ) cos(θ) ty] [y]
[1 ] [0 0 1] [1]
这个 3×3 矩阵就是 2D 齐次变换矩阵，记作 T：
T = [R t] = [cos(θ) -sin(θ) tx]
[0 1] [sin(θ) cos(θ) ty]
[0 0 1]
其中：
- R 是 2×2 旋转矩阵
- t = (tx, ty) 是平移向量
- 最后一行 [0, 0, 1] 是固定的
2.5.2 为什么这样设计能合并旋转和平移？
展开矩阵乘法验证一下：
[cos(θ) -sin(θ) tx] [x] [x*cos(θ) - y*sin(θ) + tx]
[sin(θ) cos(θ) ty] [y] = [x*sin(θ) + y*cos(θ) + ty]
[0 0 1] [1] [1 ]
结果的前两行正好是：先旋转，再平移。完美。
2.5.3 完整数值例子
问题：点 P = (2, 0)，先绕原点旋转 45°，再沿 x 方向平移 1，沿 y 方向平移 0.5。求变换后的坐标。
Python 验证：
import numpy as np
def homogeneous_transform_2d(theta_deg, tx, ty):
"""
生成 2D 齐次变换矩阵
参数：
theta_deg: 旋转角度（度）
tx, ty: 平移量
返回：3x3 齐次变换矩阵
"""
theta = np.radians(theta_deg)
T = np.array([
[np.cos(theta), -np.sin(theta), tx],
[np.sin(theta), np.cos(theta), ty],
[0, 0, 1]
])
return T
# 点 P = (2, 0)，旋转 45°，平移 (1, 0.5)
T = homogeneous_transform_2d(45, 1, 0.5)
P = np.array([2, 0, 1]) # 注意：齐次坐标，最后加 1
P_new = T @ P
print(f"变换矩阵 T:")
print(np.round(T, 4))
print(f"\n变换后的点: ({P_new[0]:.4f}, {P_new[1]:.4f})")
# 预期输出: (2.4142, 1.9142)
运行结果：
变换矩阵 T:
[[ 0.7071 -0.7071 1. ]
[ 0.7071 0.7071 0.5 ]
[ 0. 0. 1. ]]
变换后的点: (2.4142, 1.9142)
和手算结果一致。
2.6 矩阵连乘 = 坐标系链（核心）
2.6.1 连续变换变成矩阵连乘
有了齐次变换矩阵，连续变换就变成了矩阵连乘：
T_total = T1 * T2 * T3 * ...
这比之前的"旋转 + 平移分开写"简洁多了，而且不容易出错。
2.6.2 两连杆机械臂的变换链
对于两连杆机械臂，坐标系链是：
base → link1 → end_effector
对应的变换矩阵：
T_b_1：link1 坐标系相对 base 坐标系的变换（由 θ1 和 L1 决定）T_1_e：end_effector 坐标系相对 link1 坐标系的变换（由 θ2 和 L2 决定）
末端相对 base 坐标系的齐次变换：
T_b_e = T_b_1 * T_1_e
T_b_e 是一个 3×3 齐次变换矩阵，包含旋转和平移两部分信息。末端位置从其第三列提取：x = T_b_e[0,2]，y = T_b_e[1,2]。
2.6.3 写出每个变换矩阵
T_b_1（base → link1）：
关节 1 旋转了 θ1，连杆 1 长度为 L1，所以 link1 的末端（关节 2 的位置）相对 base 的变换是：
T_b_1 = [cos(θ1) -sin(θ1) L1*cos(θ1)]
[sin(θ1) cos(θ1) L1*sin(θ1)]
[0 0 1 ]
注意：平移量 (L1*cos(θ1), L1*sin(θ1)) 就是关节 2 在 base 坐标系中的位置。
T_1_e（link1 → end_effector）：
关节 2 相对 link1 旋转了 θ2，连杆 2 长度为 L2：
T_1_e = [cos(θ2) -sin(θ2) L2*cos(θ2)]
[sin(θ2) cos(θ2) L2*sin(θ2)]
[0 0 1 ]
2.6.4 矩阵连乘推导 FK 公式
矩阵连乘展开过程：
T_b_e = T_b_1 · T_1_e
[cos θ1 -sin θ1 L1·cos θ1] [cos θ2 -sin θ2 L2·cos θ2]
= [sin θ1 cos θ1 L1·sin θ1] · [sin θ2 cos θ2 L2·sin θ2]
[0 0 1 ] [0 0 1 ]
展开第三列（平移部分，即末端位置）：
x = cos θ1 · L2·cos θ2 + (-sin θ1) · L2·sin θ2 + L1·cos θ1
= L1·cos θ1 + L2·(cos θ1·cos θ2 - sin θ1·sin θ2)
= L1·cos θ1 + L2·cos(θ1 + θ2) ← 和差化积
y = sin θ1 · L2·cos θ2 + cos θ1 · L2·sin θ2 + L1·sin θ1
= L1·sin θ1 + L2·(sin θ1·cos θ2 + cos θ1·sin θ2)
= L1·sin θ1 + L2·sin(θ1 + θ2) ← 和差化积
这就是 FK 公式的来源——矩阵连乘第三列展开后，用三角和差化积化简，直接得到我们在第 1 讲里给出的公式。
import numpy as np
def fk_via_matrix(L1, L2, theta1_deg, theta2_deg):
"""
用矩阵连乘推导两连杆机械臂正运动学
"""
theta1 = np.radians(theta1_deg)
theta2 = np.radians(theta2_deg)
# base → link1 的变换矩阵
T_b_1 = np.array([
[np.cos(theta1), -np.sin(theta1), L1 * np.cos(theta1)],
[np.sin(theta1), np.cos(theta1), L1 * np.sin(theta1)],
[0, 0, 1 ]
])
# link1 → end_effector 的变换矩阵
T_1_e = np.array([
[np.cos(theta2), -np.sin(theta2), L2 * np.cos(theta2)],
[np.sin(theta2), np.cos(theta2), L2 * np.sin(theta2)],
[0, 0, 1 ]
])
# 连乘得到 base → end_effector 的变换
T_b_e = T_b_1 @ T_1_e
# 末端位置就是变换矩阵最后一列的前两个元素
x = T_b_e[0, 2]
y = T_b_e[1, 2]
return x, y
# 测试：L1=1, L2=0.8, θ1=30°, θ2=45°
x, y = fk_via_matrix(1.0, 0.8, 30, 45)
print(f"矩阵连乘结果: x = {x:.4f}, y = {y:.4f}")
# 用直接公式验证
theta1 = np.radians(30)
theta2 = np.radians(45)
L1, L2 = 1.0, 0.8
x2 = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
y2 = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
print(f"直接公式结果: x = {x2:.4f}, y = {y2:.4f}")
print(f"两种方法结果一致: {np.isclose(x, x2) and np.isclose(y, y2)}")
运行结果：
矩阵连乘结果: x = 1.3428, y = 1.2071
直接公式结果: x = 1.3428, y = 1.2071
两种方法结果一致: True
这就是 FK 公式的来源：它不是凭空背下来的，而是矩阵连乘展开后的结果。
2.6.5 用 ASCII 图理解每一步
步骤 1：base 坐标系
y
│
│
└──── x
O(base)
步骤 2：关节 1 旋转 θ1，连杆 1 延伸 L1
y
│ * ← 关节2的位置 (L1*cos(θ1), L1*sin(θ1))
│ ╱
│ ╱ L1
│ ╱ θ1
└──── x
O(base)
步骤 3：在关节2处建立 link1 坐标系，关节 2 再旋转 θ2，连杆 2 延伸 L2
y
│ * ← 关节2
│ ╱ ╲
│ ╱ ╲ L2
│ ╱ θ1 ╲ (θ1+θ2 方向)
└──── x * ← 末端位置
O(base)
末端位置 = 关节2位置 + 连杆2在全局方向上的投影：
x = L1*cos(θ1) + L2*cos(θ1+θ2)
y = L1*sin(θ1) + L2*sin(θ1+θ2)
现在你知道这个公式为什么是这样了。
2.7 常见误区
2.7.1 误区一：以为 θ2 是全局角度
错误理解：θ2 = 30° 意味着第二根连杆在全局坐标系里指向 30°。
正确理解：θ2 = 30° 意味着第二根连杆相对第一根连杆旋转了 30°。第二根连杆在全局坐标系里的方向是 θ1 + θ2。
怎么避免：每次看到关节角，都问自己"这个角度是相对谁的？"
2.7.2 误区二：以为矩阵乘法顺序可以随意交换
矩阵乘法不满足交换律：T1 * T2 ≠ T2 * T1。
import numpy as np
# 先旋转 45°，再平移 (1, 0)
T_rotate_then_translate = (
homogeneous_transform_2d(0, 1, 0) @ # 平移
homogeneous_transform_2d(45, 0, 0) # 旋转
)
# 先平移 (1, 0)，再旋转 45°
T_translate_then_rotate = (
homogeneous_transform_2d(45, 0, 0) @ # 旋转
homogeneous_transform_2d(0, 1, 0) # 平移
)
P = np.array([0, 0, 1]) # 原点
print("先旋转再平移:", (T_rotate_then_translate @ P)[:2].round(4))
print("先平移再旋转:", (T_translate_then_rotate @ P)[:2].round(4))
# 两个结果不同！
运行结果：
先旋转再平移: [1. 0.]
先平移再旋转: [0.7071 0.7071]
结论：变换的顺序很重要，矩阵乘法不满足交换律。
这里有两种常见的写法，规则不同，注意区分：
- 对点施加连续变换：
T_final · P = T2 · T1 · P，从右往左读，T1 先执行 - 坐标系链：
T_b_e = T_b_1 · T_1_e，从左往右读，和坐标系链方向一致（base → link1 → end_effector）
本讲 2.6.2 节用的是坐标系链写法，两者本质等价，只是视角不同。
2.7.3 误区三：以为齐次变换矩阵只是"数学技巧"
齐次变换矩阵不只是数学上的便利，它在工程里直接对应实际系统：
- tf2 内部就是用变换矩阵（实际上是四元数 + 平移向量，等价于齐次变换矩阵）来存储坐标系关系的
- URDF 里每个
<joint>标签定义的就是相邻坐标系之间的变换 - MoveIt 2 的 IK 求解器，本质上就是在求"给定末端变换矩阵，反推各关节角"
理解了齐次变换矩阵，你看 tf2 的输出、URDF 的结构、MoveIt 2 的 API 都会更清晰。
2.8 本讲完整代码汇总
把本讲所有函数整理到一个文件，可以直接运行：
# lesson_02_transforms.py
# 运行方式：python3 lesson_02_transforms.py
import numpy as np
def rotation_matrix_2d(theta_deg):
"""生成 2D 旋转矩阵"""
theta = np.radians(theta_deg)
return np.array([
[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]
])
def homogeneous_transform_2d(theta_deg, tx, ty):
"""生成 2D 齐次变换矩阵"""
theta = np.radians(theta_deg)
return np.array([
[np.cos(theta), -np.sin(theta), tx],
[np.sin(theta), np.cos(theta), ty],
[0, 0, 1]
])
def fk_via_matrix(L1, L2, theta1_deg, theta2_deg):
"""用矩阵连乘计算两连杆机械臂正运动学"""
theta1 = np.radians(theta1_deg)
theta2 = np.radians(theta2_deg)
T_b_1 = np.array([
[np.cos(theta1), -np.sin(theta1), L1 * np.cos(theta1)],
[np.sin(theta1), np.cos(theta1), L1 * np.sin(theta1)],
[0, 0, 1]
])
T_1_e = np.array([
[np.cos(theta2), -np.sin(theta2), L2 * np.cos(theta2)],
[np.sin(theta2), np.cos(theta2), L2 * np.sin(theta2)],
[0, 0, 1]
])
T_b_e = T_b_1 @ T_1_e
return T_b_e[0, 2], T_b_e[1, 2]
if __name__ == "__main__":
print("=== 旋转矩阵演示 ===")
R = rotation_matrix_2d(30)
P = np.array([1, 0])
print(f"点 (1,0) 旋转 30° 后: {(R @ P).round(4)}")
print("\n=== 齐次变换矩阵演示 ===")
T = homogeneous_transform_2d(45, 1, 0.5)
P_h = np.array([2, 0, 1])
result = T @ P_h
print(f"点 (2,0) 旋转 45° 再平移 (1,0.5) 后: ({result[0]:.4f}, {result[1]:.4f})")
print("\n=== 矩阵连乘 FK 演示 ===")
configs = [
(1.0, 0.8, 0, 0, "两杆伸直"),
(1.0, 0.8, 0, 90, "第二杆向上弯 90°"),
(1.0, 0.8, 30, 45, "θ1=30°, θ2=45°"),
(1.0, 0.8, 90, -90, "θ1=90°, θ2=-90°"),
]
for L1, L2, t1, t2, desc in configs:
x, y = fk_via_matrix(L1, L2, t1, t2)
print(f" {desc}: 末端 = ({x:.4f}, {y:.4f})")
运行结果：
=== 旋转矩阵演示 ===
点 (1,0) 旋转 30° 后: [0.866 0.5 ]
=== 齐次变换矩阵演示 ===
点 (2,0) 旋转 45° 再平移 (1,0.5) 后: (2.4142, 1.9142)
=== 矩阵连乘 FK 演示 ===
两杆伸直: 末端 = (1.8000, 0.0000)
第二杆向上弯 90°: 末端 = (1.0000, 0.8000)
θ1=30°, θ2=45°: 末端 = (1.3428, 1.2071)
θ1=90°, θ2=-90°: 末端 = (0.8000, 1.0000)
试着改变 θ1 和 θ2 的值，观察末端位置如何变化，建立直觉。
本讲核心总结
| 概念 | 一句话理解 |
|---|---|
| θ2 是相对角度 | θ2 是相对上一段连杆的角度，全局方向是 θ1 + θ2 |
| 坐标系树 | 机器人每个部件有自己的坐标系，形成父子树结构 |
| 旋转矩阵 | 2×2 矩阵，描述坐标系旋转，两列是旋转后的坐标轴方向 |
| 齐次变换矩阵 | 3×3 矩阵，把旋转和平移合并成一个操作 |
| 矩阵连乘 | 多段变换串联，顺序不能交换，坐标系链从左往右读 |
| FK 公式的来源 | 矩阵连乘展开后的结果，不是凭空背下来的 |
参考代码
本讲对应的完整参考代码位于：
ros_ws/scripts/lesson_02_transforms.py
文件内容：旋转矩阵、齐次变换矩阵、矩阵连乘 FK，含详细原理注释。
直接运行（无需 ROS2 环境）：
python3 $ROS_WS/scripts/lesson_02_transforms.py
代码注释中标注了 [原理]、[注意]、[对比] 等标记，与本讲教程内容互补。
下一讲预告
3. 正运动学与逆运动学实现
现在你已经理解了 FK 公式从哪里来。下一讲我们会：
- 完整推导两连杆 IK 的解析解（用余弦定理）
- 处理双解（肘上 / 肘下）
- 判断目标点是否可达
- 写出完整的 FK + IK Python 实现
- 用 FK 验证 IK 的正确性（round-trip 测试）
这些函数后面会直接接入 ROS2 节点，成为整个系统的算法核心。