# 线性代数：机器人变换矩阵详解

> 本文档详细介绍机器人学中的齐次变换矩阵，包括复合变换推导和实际例子。

---

## 1. 基础变换矩阵

### 1.1 平移矩阵 T

将点沿向量 `(tx, ty, tz)` 平移：

```
    | 1  0  0  tx |
T = | 0  1  0  ty |
    | 0  0  1  tz |
    | 0  0  0  1  |
```

### 1.2 旋转矩阵

#### 绕 Z 轴旋转 Rz(θ)

```
      | cosθ  -sinθ   0   0 |
Rz =  | sinθ   cosθ   0   0 |
      |  0      0     1   0 |
      |  0      0     0   1 |
```

#### 绕 X 轴旋转 Rx(θ)

```
      | 1    0     0    0 |
Rx =  | 0  cosθ  -sinθ  0 |
      | 0  sinθ   cosθ  0 |
      | 0    0     0    1 |
```

#### 绕 Y 轴旋转 Ry(θ)

```
      | cosθ   0   sinθ  0 |
Ry =  |  0     1    0    0 |
      | -sinθ  0   cosθ  0 |
      |  0     0    0    1 |
```

---

## 2. 复合变换

### 2.1 核心规则：右乘法则

**变换顺序非常重要！先进行的变换写在右边。**

```
P' = T × R × P
```

表示：先旋转 R，再平移 T。

### 2.2 推导过程

**问题：** 点 P 先绕 Z 轴旋转 θ 角度，再沿 X 轴平移 d 距离。

**第一步：旋转**
```
P₁ = Rz(θ) × P
```

**第二步：平移**
```
P' = T × P₁ = T × Rz(θ) × P
```

**复合矩阵：**
```
        | cosθ  -sinθ   0   d |
T×Rz =  | sinθ   cosθ   0   0 |
        |  0      0     1   0 |
        |  0      0     0   1 |
```

---

## 3. 实际例子

### 例子 1：2D 机械臂末端位置计算

**场景：** 一个 2D 机械臂
- 关节 1：长度 L₁ = 2，角度 θ₁ = 30°
- 关节 2：长度 L₂ = 1.5，角度 θ₂ = 45°

**求：** 末端执行器的位置

**Python 代码：**

```python
import numpy as np

# 定义变换矩阵函数
def rot_z(theta):
    """绕 Z 轴旋转 theta 弧度"""
    c, s = np.cos(theta), np.sin(theta)
    return np.array([
        [c, -s, 0, 0],
        [s,  c, 0, 0],
        [0,  0, 1, 0],
        [0,  0, 0, 1]
    ])

def translate(x, y, z=0):
    """平移变换"""
    return np.array([
        [1, 0, 0, x],
        [0, 1, 0, y],
        [0, 0, 1, z],
        [0, 0, 0, 1]
    ])

# 参数
L1, L2 = 2, 1.5
theta1, theta2 = np.radians(30), np.radians(45)

# 变换链：基座 → 关节1 → 关节2 → 末端
T_base = np.eye(4)  # 基座（单位矩阵）
T_joint1 = rot_z(theta1) @ translate(L1, 0)  # 旋转后平移
T_joint2 = rot_z(theta2) @ translate(L2, 0)  # 第二个关节

# 总变换（注意：从右到左乘）
T_total = T_base @ T_joint1 @ T_joint2

# 末端位置（齐次坐标）
end_effector = T_total @ np.array([0, 0, 0, 1])
print(f"末端位置: ({end_effector[0]:.3f}, {end_effector[1]:.3f})")
# 输出: 末端位置: (1.366, 2.299)
```

---

### 例子 2：无人机姿态变换

**场景：** 无人机需要依次进行：
1. 绕 Z 轴偏航（Yaw）45°
2. 绕 Y 轴俯仰（Pitch）30°
3. 绕 X 轴翻滚（Roll）15°
4. 向前飞行 10 米

**变换顺序（从右到左）：**
```
T_total = Translate(10, 0, 0) × Rx(15°) × Ry(30°) × Rz(45°)
```

**为什么是这个顺序？**

想象你自己是无人机：
- 先偏航（左右转）→ 你的"前"方向改变了
- 再俯仰（上下抬头）→ 是相对于新的"前"方向
- 再翻滚（侧翻）→ 是相对于当前的姿态
- 最后向前飞 → 沿着当前的"前"方向

**如果顺序反过来，结果完全不同！**

---

### 例子 3：相机标定中的欧拉角变换

在计算机视觉中，世界坐标系到相机坐标系的变换：

```
P_camera = T_cam × R × P_world
```

其中 R 是欧拉角旋转矩阵：

```
R = Rz(γ) × Ry(β) × Rx(α)

    | cγcβ  cγsβsα-sγcα  cγsβcα+sγsα |
  = | sγcβ  sγsβsα+cγcα  sγsβcα-cγsα |
    | -sβ      cβsα         cβcα     |
```

（其中 c=cos, s=sin）

---

## 4. 重要注意事项

### ⚠️ 顺序很重要！

```
T × R ≠ R × T
```

**平移后旋转** vs **旋转后平移**：

- **旋转后平移**：点绕原点转，然后移动
  ```
  [R  t]
  [0  1]
  ```

- **平移后旋转**：点先移动，再绕原点转（不是绕自身转！）
  ```
  [R  Rt]
  [0   1 ]
  ```

### ⚠️ 万向节死锁（Gimbal Lock）

当使用欧拉角时，如果中间轴旋转 90°，会导致失去一个自由度。

**解决方法：** 使用四元数（Quaternion）

---

## 5. 总结

| 操作 | 矩阵 | 说明 |
|------|------|------|
| 平移 | T(tx,ty,tz) | 沿向量移动 |
| 旋转 | Rx/Ry/Rz(θ) | 绕坐标轴旋转 |
| 复合 | T₂ × R₂ × T₁ × R₁ | 右乘法则，先右后左 |
| 逆变换 | T⁻¹, R⁻¹=Rᵀ | 旋转矩阵正交性质 |

---

## 附录：常用公式速查

### 旋转矩阵性质
- 正交性：Rᵀ × R = I
- 逆矩阵：R⁻¹ = Rᵀ
- 行列式：det(R) = 1

### 齐次坐标
- 3D 点：(x, y, z, 1)
- 3D 向量：(x, y, z, 0)  # 平移不影响向量

### 复合变换链
对于 n 个关节的机械臂：
```
T_total = T₁ × R₁ × T₂ × R₂ × ... × Tₙ × Rₙ
```

---

*文档生成时间：2026-04-09*