2. 数学基础回顾：空间描述与变换
做轨迹规划，说白了就是让机器人知道「自己在哪里，要去哪里，怎么过去」。这背后全是数学。我当年刚入行时，觉得这些公式太枯燥，结果第一次调机械臂就撞了限位——嗯，从那以后我老老实实把数学基础补了一遍。
这一章我们快速过一遍核心概念。你不需要成为数学家，但得理解它们怎么用。
2.1 空间描述与齐次坐标
机器人世界里，每个物体都有自己的坐标系。你要描述一个点，得先告诉它相对于谁。
核心思想： 一个空间点用 (x, y, z) 表示，但加上旋转和偏移后，我们需要 4×4 的齐次变换矩阵。
齐次坐标就是在三维坐标后面加一个 1：
// 三维点 (x, y, z) 的齐次形式
P = [x, y, z, 1]^T
// 平移向量 t = (tx, ty, tz) 的齐次变换矩阵
T = | 1 0 0 tx |
| 0 1 0 ty |
| 0 0 1 tz |
| 0 0 0 1 |为什么要加这个 1？因为这样平移和旋转就能用一个矩阵搞定。我在项目中遇到过有人直接用 3×3 矩阵做变换，结果平移算错了——其实加上齐次坐标，一切就顺了。
2.2 旋转矩阵
旋转矩阵是 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 | |
我的习惯： 写代码时先用单位矩阵初始化，再分别乘绕各轴的旋转。别试图手算复合旋转，容易出错。
2.3 欧拉角与四元数
欧拉角直观，但有万向锁问题。四元数没这毛病，但抽象。你想想看，飞机俯仰时如果同时偏航，角度就乱套了——这就是万向锁。
我建议这样选：
- 人机交互、可视化 → 用欧拉角（直观）
- 插值、控制、滤波 → 用四元数（稳定）
四元数长这样：
q = w + xi + yj + zk
其中 w² + x² + y² + z² = 1
// 绕单位轴 u = (ux, uy, uz) 旋转 θ 角
q = (cos(θ/2), ux·sin(θ/2), uy·sin(θ/2), uz·sin(θ/2))
我曾经踩过的坑： 四元数乘法不满足交换律！q1 * q2 和 q2 * q1 结果不同。写代码时一定要确认旋转顺序。
2.4 运动学基础：正解与逆解
正运动学：已知关节角度，求末端位姿。逆运动学：已知末端位姿，求关节角度。
正解简单，代入公式就行。逆解麻烦——可能有多个解、无解，甚至需要数值迭代。
一个典型六轴机器人正解流程：
- 建立各关节坐标系（DH 参数）
- 写出相邻关节的齐次变换矩阵 T₁, T₂, …, T₆
- 连乘得到末端变换矩阵 T = T₁·T₂·…·T₆
逆解呢？我一般先判断有没有解析解。如果有，直接算；如果没有，用迭代法（比如牛顿-拉夫森）。
// 伪代码：数值逆解
while (误差 > 阈值) {
J = 计算雅可比矩阵(当前关节角)
Δx = 目标位姿 - 当前位姿
Δq = J⁻¹ · Δx // 或用伪逆
关节角 += Δq
}
避坑指南： 数值逆解要加关节限位约束。我曾经没加，结果迭代出来的角度让机械臂撞了工作台——嗯，从那以后我每次都检查限位。
2.5 本章知识体系
下面这张图把本章的核心逻辑串起来了。你可以把它当作一张「数学地图」：
嗯，这张图你看懂了吗？从空间描述出发，分三条路：齐次坐标、旋转矩阵、欧拉角/四元数，最后汇到运动学。这就是我们做轨迹规划的数学工具箱。
我个人习惯把这张图贴在工位旁边。每次写代码前扫一眼，思路就清晰了。