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

现在做一个动作：

1. 大臂向前抬起 45°（θ1 = 45°，相对地面）
2. 小臂相对大臂再弯曲 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 写成矩阵形式

上面的公式可以写成矩阵乘法：

2D 旋转矩阵可视化

[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 节点，成为整个系统的算法核心。