第二章 数学基础回顾（一）：向量与坐标系、刚体的位姿描述、齐次变换矩阵
各位同学，欢迎来到第二章。
说实话，很多初学者一上来就啃轨迹规划算法，结果被各种矩阵变换搞得晕头转向。我当年刚入行时也踩过这个坑——花了两周调一个机械臂的抓取位姿，最后发现是坐标系搞反了。所以这一章，咱们把数学地基打牢。
你想想看，机器人要运动，首先得知道“我在哪”、“我要去哪”。这背后全靠向量、坐标系和变换矩阵撑着。别怕，这些东西没那么玄乎。
2.1 向量：描述空间中的点与方向
向量是啥？说白了就是带箭头的一个量。它有大小，有方向。在轨迹规划里，我们用向量来表示位置、速度、加速度，甚至是力的方向。
2.1.1 向量的表示
在三维空间中，一个向量通常写成：
v = [vx, vy, vz]^T右上角的 T 表示转置，其实就是列向量。我个人习惯用列向量，因为后面做矩阵乘法时更顺手。
2.1.2 向量的基本运算
你需要掌握这几个：
- 加法/减法：对应分量相加减。比如两个速度向量相加，得到合速度。
- 数乘：向量乘以一个标量，只改变大小，不改变方向（除非乘负数）。
- 点积：结果是一个数。公式是 v·w = |v||w|cosθ。点积为零说明两个向量垂直。我在做避障时经常用点积判断障碍物是否在运动方向前方。
- 叉积：结果是一个向量。公式是 v×w，大小是 |v||w|sinθ，方向垂直于 v 和 w 所在的平面。叉积在计算力矩、旋转轴时特别有用。
2.2 坐标系：给空间一个参考系
向量本身没有绝对意义，必须放在坐标系里才有意义。就像你说“往前走5米”，得先定义“前”是哪个方向。
2.2.1 世界坐标系与局部坐标系
在机器人系统里，至少有两个坐标系：
- 世界坐标系（全局坐标系）：固定在地面或工作空间的一个基准。通常用 {W} 表示。
- 局部坐标系（工具坐标系、基坐标系等）：附着在机器人连杆或末端执行器上。通常用 {B}、{T} 等表示。
我记得有一次做移动机器人导航，激光雷达的数据是在机器人坐标系下的，但地图是世界坐标系。如果不做坐标变换，机器人会直接撞墙。嗯，这就是坐标系的重要性。
2.2.2 右手定则
机器人学里几乎都用右手坐标系。伸出右手，拇指指向 X 轴，食指指向 Y 轴，中指指向 Z 轴。三个手指互相垂直。这个规则贯穿始终，别搞反了。
2.3 刚体的位姿描述：位置 + 姿态
刚体在空间中有6个自由度：3个平移（位置） + 3个旋转（姿态）。我们用一个向量描述位置，用一个旋转矩阵描述姿态。
2.3.1 位置描述
很简单，就是一个三维向量：
p = [x, y, z]^T表示刚体上某个参考点（比如质心）在坐标系中的坐标。
2.3.2 姿态描述
姿态稍微复杂一点。我们用旋转矩阵 R 来表示。R 是一个 3×3 的矩阵，它的三个列向量分别是刚体局部坐标系的 X、Y、Z 轴在世界坐标系中的投影。
旋转矩阵有一个重要性质：它是正交矩阵，即 R^T R = I，且行列式为 +1。
2.4 齐次变换矩阵：把旋转和平移统一起来
为什么要用齐次变换矩阵？因为如果分开处理旋转和平移，公式会变得很啰嗦。齐次变换矩阵把两者合并成一个 4×4 的矩阵，这样连续变换就可以用矩阵乘法串联起来。
2.4.1 齐次变换矩阵的结构
T = [ R p ]
[ 0 1 ]其中 R 是 3×3 旋转矩阵，p 是 3×1 位置向量，最后一行是 [0 0 0 1]。
2.4.2 齐次变换矩阵的逆
求逆在轨迹规划里很常见。比如已知末端在基坐标系下的位姿，想求基坐标系在末端坐标系下的位姿，就需要求逆。
T_inv = [ R^T -R^T * p ]
[ 0 1 ]注意，不是简单的转置。我曾经在项目里直接对 T 求逆，结果算出来的位置全错了，排查了半天才发现问题。
2.4.3 连续变换
假设从坐标系 A 到 B 的变换是 T_AB，从 B 到 C 是 T_BC，那么从 A 到 C 的变换就是：
T_AC = T_AB * T_BC注意乘法的顺序。左乘和右乘意义不同：左乘是相对于固定坐标系变换，右乘是相对于当前坐标系变换。这个细节非常容易出错。
2.5 本章知识体系总览
下面这张图帮你理清本章的核心逻辑。从向量出发，到坐标系，到位姿描述，最后统一到齐次变换矩阵。每一步都是下一步的基础。
2.6 本章小结
这一章我们干了三件事：
- 复习了向量的基本运算，这是描述一切运动的基础。
- 理解了坐标系的概念，知道世界坐标系和局部坐标系的关系。
- 掌握了齐次变换矩阵，这是连接不同坐标系、描述刚体位姿的核心工具。
这些内容看起来是纯数学，但后面每一章都会用到。尤其是齐次变换矩阵，在正运动学、逆运动学、轨迹插值里，它就像空气一样无处不在。别急，下一章我们会用这些工具去解决一个实际问题——如何描述一条连续的轨迹。