📚 课程目录第2章机器人学基础（一）

2. 机器人学基础（一）

各位同学，欢迎来到机器人学基础的第一讲。说实话，这部分内容看着全是公式，但它是后面所有视觉定位和机械臂控制的基石。我当年刚接触时也觉得抽象，后来在做一个手眼标定项目时，因为旋转矩阵用错了顺序，机械臂直接怼到了相机上……嗯，从那以后我再也不敢轻视这些数学工具了。

1. 刚体变换：描述物体在空间中的位姿

什么叫刚体变换？说白了，就是一个物体在三维空间里又转又移，但形状和大小不变。你想想看，机械臂的末端夹爪从A点移动到B点，中间经历了旋转和平移，这就是一个典型的刚体变换。

刚体变换由两部分组成：

旋转：描述物体的朝向变化
平移：描述物体的位置变化

我个人习惯把刚体变换想象成“拧瓶盖+挪杯子”的组合动作。拧瓶盖是旋转，挪杯子是平移，两者合在一起就是刚体变换。

核心要点：刚体变换保持两点间的距离不变，也保持向量的夹角不变。这是它区别于仿射变换、透视变换的根本特征。

2. 齐次坐标：把旋转和平移统一起来

为什么要引入齐次坐标？因为旋转是矩阵乘法，平移是向量加法，两者混在一起写很麻烦。齐次坐标用一个4×4矩阵，把旋转和平移统一成一次矩阵乘法。

齐次坐标的核心思想：给三维向量加一个维度，变成四维向量。

// 三维点 (x, y, z) 的齐次坐标表示
// 普通坐标 → 齐次坐标
(x, y, z) → (x, y, z, 1)

// 齐次坐标 → 普通坐标（归一化）
(x, y, z, w) → (x/w, y/w, z/w)

齐次变换矩阵的标准形式：

| R   t |
| 0   1 |

其中：
R：3×3 旋转矩阵
t：3×1 平移向量
0：1×3 零向量
1：标量 1

我在项目中遇到过一个问题：用齐次坐标做连续变换时，矩阵乘法的顺序千万不能搞反。先旋转后平移，和先平移后旋转，结果完全不同。

避坑指南：我曾经在写机械臂正运动学代码时，把变换矩阵的乘法顺序写反了，结果仿真里机械臂直接穿模。记住：T = T1 * T2 表示先做 T2 变换，再做 T1 变换。矩阵乘法不满足交换律！

3. 旋转矩阵：描述三维空间中的旋转

旋转矩阵是一个3×3的正交矩阵，行列式为+1。它描述了坐标系绕某个轴旋转一定角度后的新朝向。

绕三个坐标轴的基本旋转矩阵：

旋转轴	旋转矩阵（绕θ角）
绕X轴	[[1, 0, 0], [0, cosθ, -sinθ], [0, sinθ, cosθ]]
绕Y轴	[[cosθ, 0, sinθ], [0, 1, 0], [-sinθ, 0, cosθ]]
绕Z轴	[[cosθ, -sinθ, 0], [sinθ, cosθ, 0], [0, 0, 1]]

旋转矩阵有几个重要性质：

正交性：R^T = R^-1，即转置等于逆
行列式为+1：保证是纯旋转，不包含反射
封闭性：两个旋转矩阵相乘，结果还是旋转矩阵

你想想看，如果旋转矩阵的行列式是-1，那意味着什么？意味着物体被镜像了，这在物理世界中是不可能发生的。所以检测旋转矩阵是否合法，先看行列式是不是+1。

4. 欧拉角：直观但容易出问题的表示方式

欧拉角用三个角度表示旋转：偏航（Yaw）、俯仰（Pitch）、滚转（Roll）。听起来很直观，对吧？但实际用起来坑不少。

常见的欧拉角顺序：

ZYX顺序：先绕Z轴转（偏航），再绕Y轴转（俯仰），最后绕X轴转（滚转）
ZYZ顺序：先绕Z轴转，再绕Y轴转，最后再绕Z轴转

欧拉角最大的问题——万向锁。当俯仰角达到±90°时，偏航和滚转的旋转轴重合，丢失一个自由度。

个人经验：我在做无人机姿态估计时，就遇到过万向锁问题。当时用欧拉角做卡尔曼滤波，飞到某个角度时姿态突然跳变。后来换成四元数，问题就解决了。所以，如果你要做连续旋转或插值，建议直接上四元数。

5. 四元数：高效且无奇异的旋转表示

四元数可以理解为复数的推广。一个四元数 q 包含一个实部和三个虚部：

q = w + xi + yj + zk

其中：
w：实部（标量部分）
x, y, z：虚部（向量部分）
i² = j² = k² = ijk = -1

四元数的优势：

无万向锁：不会出现奇异性
插值平滑：可以用球面线性插值（SLERP）做平滑旋转
计算高效：比旋转矩阵少用4个参数，计算量更小

四元数与旋转矩阵的转换：

// 四元数 → 旋转矩阵
R = [[1-2(y²+z²), 2(xy-wz), 2(xz+wy)],
     [2(xy+wz), 1-2(x²+z²), 2(yz-wx)],
     [2(xz-wy), 2(yz+wx), 1-2(x²+y²)]]

// 旋转矩阵 → 四元数
w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2
x = (R[2][1] - R[1][2]) / (4w)
y = (R[0][2] - R[2][0]) / (4w)
z = (R[1][0] - R[0][1]) / (4w)

注意：四元数必须归一化，即 w² + x² + y² + z² = 1。否则表示的旋转会带有缩放效果。我见过有人忘记归一化，结果机械臂末端位置越跑越偏。

6. 平移与旋转的数学表达：从理论到代码

在实际编程中，我们通常用齐次变换矩阵来统一表达平移和旋转。下面是一个Python示例：

import numpy as np

def homogeneous_transform(R, t):
    """
    构建齐次变换矩阵
    R: 3x3 旋转矩阵
    t: 3x1 平移向量
    """
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

# 示例：绕Z轴旋转45度，再沿X轴平移10个单位
theta = np.pi / 4
R_z = np.array([[np.cos(theta), -np.sin(theta), 0],
                [np.sin(theta),  np.cos(theta), 0],
                [0,              0,             1]])
t = np.array([10, 0, 0])

T = homogeneous_transform(R_z, t)
print("齐次变换矩阵：\n", T)

# 对点进行变换
point = np.array([1, 0, 0, 1])  # 齐次坐标
transformed_point = T @ point
print("变换后的点：", transformed_point[:3])

这段代码展示了如何用齐次变换矩阵对点进行刚体变换。注意，点的齐次坐标最后一维是1，而向量的齐次坐标最后一维是0（因为向量只旋转不平移）。

关键区别：点的齐次坐标末尾是1，向量的齐次坐标末尾是0。这样设计的好处是，平移变换对向量不起作用，符合物理直觉。

7. 知识体系总览

下面这张图总结了本章的核心知识点和它们之间的关系：

从这张图可以看出，刚体变换的核心是旋转和平移。旋转有三种主流表示方式，最终都统一到齐次变换矩阵中，用于实际工程应用。

8. 本章小结

这一章我们覆盖了机器人学中最基础的数学工具：

刚体变换：旋转+平移，保持形状不变
齐次坐标：用4×4矩阵统一表达变换
旋转矩阵：正交矩阵，行列式为+1
欧拉角：直观但有万向锁问题
四元数：无奇异，适合插值
平移与旋转的数学表达：齐次变换矩阵的构建与应用

这些内容看起来是纯数学，但后面每一章都会用到。我个人建议你把代码示例跑一遍，亲手感受一下不同旋转表示之间的转换。嗯，等你真正在项目中遇到问题时，就会感谢今天打下的基础了。