1. 机器人运动学基础：刚体运动描述、齐次变换矩阵、旋转矩阵的几何意义
各位同学，欢迎来到《机器人运动学与力控算法联调》的第一章。
说实话，我见过不少刚入行的工程师，一上来就急着调PID、写轨迹规划代码，结果机器人动起来完全不是那么回事——要么末端位置算错了，要么姿态飞了。为什么？因为运动学基础没打牢。你想想看，连机器人“现在在哪儿、朝哪个方向”都描述不清楚，后面的控制全是空中楼阁。
这一章，我们就来啃下运动学最核心的三块基石：刚体运动描述、旋转矩阵的几何意义、齐次变换矩阵。这些东西，我几乎每天都会在代码里跟它们打交道。
本章核心脉络： 刚体在空间中的位置 + 姿态 → 旋转矩阵描述姿态 → 齐次变换矩阵统一描述位置与姿态 → 为后续正/逆运动学求解打下数学基础。
1.1 刚体运动描述——先搞清楚“在哪儿”和“朝哪儿”
什么叫刚体？说白了，就是受力不变形的物体。机器人每个连杆都可以近似看作刚体。描述一个刚体在空间中的状态，需要两样东西：位置和姿态。合起来叫位姿。
- 位置：刚体上某一点（通常取质心或坐标系原点）在参考坐标系下的坐标。用三维向量
p = [x, y, z]^T表示。 - 姿态：刚体相对于参考坐标系的朝向。用旋转矩阵
R表示。
我记得刚入行时，带我的老工程师跟我说过一句话，我一直记到现在：“没有参考系的位置，就是耍流氓。” 你报一个坐标 (1, 2, 3)，不说是相对于哪个坐标系，别人根本不知道你在说哪儿。所以，描述位姿的第一步——明确参考坐标系。
个人习惯： 我在代码里给每个坐标系都取一个明确的名字，比如 world_frame、base_frame、tool0_frame。变量名里带上坐标系后缀，比如 pos_in_world。这习惯帮我省了无数调试时间。
1.2 旋转矩阵的几何意义——不只是9个数字
旋转矩阵 R 是一个 3×3 的正交矩阵，行列式为 +1。它属于特殊正交群 SO(3)。
但别被这些数学名词吓到。你只需要记住旋转矩阵的几何意义：
- 列向量是旋转后的基向量：
R的三列，分别是被旋转坐标系{B}的三个单位主矢量在参考坐标系{A}下的投影。 - 行向量是参考系的基向量在旋转系下的投影：反过来看也行。
- 它描述了一个刚体从参考系到当前姿态的旋转操作。
举个例子，绕 Z 轴旋转 θ 角的旋转矩阵长这样：
R_z(θ) = | cosθ -sinθ 0 |
| sinθ cosθ 0 |
| 0 0 1 |你看，第一列 [cosθ, sinθ, 0]^T 就是旋转后的 X 轴在原始坐标系下的方向。是不是很直观？
避坑指南： 我曾经在项目里直接用欧拉角做插值，结果机器人末端走了一条奇怪的弧线，差点撞到工件。后来才意识到——欧拉角有万向锁问题。从那以后，我内部计算一律用旋转矩阵或四元数，只在人机界面显示时才转成欧拉角。
1.3 齐次变换矩阵——把旋转和平移装进一个“盒子”
有了旋转矩阵描述姿态，加上位置向量描述平移，我们能不能把两者统一起来？
能。齐次变换矩阵 T 就是干这个的。它是一个 4×4 的矩阵：
T = | R p |
| 0 1 |其中 R 是 3×3 旋转矩阵，p 是 3×1 平移向量，最后一行是 [0 0 0 1]。
为什么要加这一行？为了能用矩阵乘法把多个变换串联起来。你想想看，如果只有旋转矩阵和平移向量，你要先旋转再平移，得写两步。用齐次变换矩阵，一步到位：
p' = T * p这里的 p 和 p' 都是齐次坐标（4维，最后一维是1）。
我的经验： 在写运动学代码时，我习惯把所有连杆变换都写成齐次变换矩阵。这样从基座到末端执行器，只需要把一串 T 矩阵乘起来：T_0_n = T_0_1 * T_1_2 * ... * T_{n-1}_n。代码结构清晰，调试也方便。
1.4 三种常见旋转表示法对比
实际工程中，我们不止用旋转矩阵。这里我列个表，把几种常见表示法放在一起对比，你一看就明白各自的应用场景了。
| 表示法 | 维度 | 优点 | 缺点 | 常用场景 |
|---|---|---|---|---|
| 旋转矩阵 | 3×3 (9个参数) | 无奇异性，组合方便 | 参数冗余，需满足正交约束 | 内部计算、运动学推导 |
| 欧拉角 (RPY) | 3个角度 | 直观，易理解 | 万向锁，插值不平滑 | 人机交互、示教器显示 |
| 四元数 | 4个参数 | 无奇异性，插值平滑 | 不够直观 | 姿态插值、滤波、SLAM |
我个人在写运动学算法时，内部统一用旋转矩阵和齐次变换矩阵。为什么？因为矩阵乘法可以直接用线性代数库优化，计算效率高。只有在需要跟用户交互或者做可视化时，我才转成欧拉角。
1.5 齐次变换矩阵的复合——串联机器人的“通关密码”
串联机器人，说白了就是一系列连杆通过关节串起来。每个关节都有一个变换矩阵描述相邻连杆之间的位姿关系。
假设机器人有 n 个关节，从基座到末端执行器的总变换就是：
T_0_n = T_0_1(q1) * T_1_2(q2) * ... * T_{n-1}_n(qn)这里的 qi 是关节变量（旋转关节是角度，移动关节是位移）。每个 T_{i-1}_i 都是 qi 的函数。
这就是正运动学的核心——给定关节角度，求末端位姿。
注意： 矩阵乘法不满足交换律！T_0_1 * T_1_2 和 T_1_2 * T_0_1 的结果完全不同。我在刚学的时候犯过这个错，把变换顺序搞反了，结果机器人末端跑到天上去。记住：左乘表示相对于固定参考系变换，右乘表示相对于当前坐标系变换。
1.6 代码实战：用 Python 实现齐次变换矩阵
光说不练假把式。我们直接上代码。这里我用 NumPy 实现一个齐次变换矩阵的构建和复合操作。
import numpy as np
def rotation_x(theta):
"""绕X轴旋转theta弧度"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[1, 0, 0],
[0, c, -s],
[0, s, c]
])
def rotation_y(theta):
"""绕Y轴旋转theta弧度"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[ c, 0, s],
[ 0, 1, 0],
[-s, 0, c]
])
def rotation_z(theta):
"""绕Z轴旋转theta弧度"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[c, -s, 0],
[s, c, 0],
[0, 0, 1]
])
def homogeneous_transform(R, p):
"""由旋转矩阵R和平移向量p构建齐次变换矩阵"""
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = p
return T
# 示例：绕Z轴旋转30度，再沿X轴平移1米
theta = np.deg2rad(30)
R = rotation_z(theta)
p = np.array([1.0, 0.0, 0.0])
T = homogeneous_transform(R, p)
print("齐次变换矩阵 T：")
print(T)
# 复合变换：连续两次变换
T1 = homogeneous_transform(rotation_z(np.deg2rad(45)), np.array([0.5, 0, 0]))
T2 = homogeneous_transform(rotation_x(np.deg2rad(30)), np.array([0, 0.3, 0]))
T_composite = T1 @ T2 # 先T2后T1（左乘）
print("\n复合变换 T1 * T2：")
print(T_composite)小技巧： 我在调试时经常用 np.allclose(T @ T_inv, np.eye(4)) 来验证变换矩阵的逆是否正确。齐次变换矩阵的逆有解析形式，不需要真的去求4×4矩阵的逆——T_inv = [[R^T, -R^T * p], [0, 1]]，这样算又快又准。
1.7 本章小结
好了，这一章的内容就到这里。我们走完了运动学的第一公里：
- 刚体运动需要位置 + 姿态才能完整描述
- 旋转矩阵的列向量就是旋转后的基向量，几何意义非常直观
- 齐次变换矩阵把旋转和平移统一成4×4矩阵，方便串联复合
- 三种旋转表示法各有优劣，我推荐内部计算用旋转矩阵
- 正运动学就是齐次变换矩阵的连乘
这些内容看起来是纯数学，但相信我——后面每一章都会用到它们。你花时间把这一章吃透，后面的路会顺很多。