第2章 数学基础回顾：向量、矩阵与刚体运动

做轨迹规划，说白了就是跟数学打交道。你想想看，机器人要动，就得知道位置、速度、加速度，这些东西怎么表示？怎么从一个坐标系变到另一个坐标系？这就是本章要聊的内容。

我个人习惯，每次带新人做项目，第一件事就是让他们把数学基础再过一遍。别觉得枯燥，这些概念在后面的轨迹插值、避障、优化里，天天都要用。

2.1 向量与矩阵：轨迹规划的“语言”

向量是什么？就是既有大小又有方向的量。在三维空间里，一个位置点可以用向量 p = [x, y, z]^T 表示。一个速度、一个力，也都是向量。

矩阵呢？可以看作向量的“操作器”。旋转、缩放、投影，都能用矩阵乘法搞定。

我在项目中遇到过一个问题：两个坐标系之间的点坐标转换，直接用矩阵乘法算出来总是错的。后来发现，是矩阵乘法的顺序搞反了。记住：先旋转，再平移，顺序不能乱。

2.2 坐标系与变换：齐次坐标的妙用

为什么要有齐次坐标？因为普通的3D向量没法直接表示平移。你想想看，一个点 [x, y, z] 平移 [dx, dy, dz]，用加法就行。但旋转用乘法。旋转+平移混在一起，就麻烦了。

齐次坐标的解法很简单：给3D向量加一个维度，变成 [x, y, z, 1]^T。这样，旋转和平移就能统一成一个4×4矩阵了。

T = [ R   t ]
    [ 0^T 1 ]

举个例子：把点 p 从坐标系 {A} 变换到 {B}，公式就是 p_B = T_{B←A} · p_A。这里的 T 就是齐次变换矩阵。

2.3 刚体运动与姿态表示

刚体运动，就是物体在空间里又转又移，但自身不变形。描述刚体姿态，常用的有三种方法：旋转矩阵、欧拉角、四元数。

2.3.1 旋转矩阵

最直观，9个元素，但冗余。旋转矩阵是正交矩阵，行列式为+1。优点是能直接用于向量旋转，缺点是参数多，插值困难。

2.3.2 欧拉角

用三个角度表示旋转，比如 ZYX 顺序：先绕Z轴转γ，再绕Y轴转β，最后绕X轴转α。优点是直观，缺点是存在万向锁问题。

2.3.3 四元数

四元数 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。单位四元数（模长为1）可以表示任意旋转。

为什么用四元数？三个原因：

• 无万向锁：不会出现奇异性。
• 插值平滑：球面线性插值（SLERP）能生成平滑的旋转过渡。
• 计算高效：比旋转矩阵少用内存，组合旋转也快。

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²) ]]

2.4 知识体系总览

下面这张图，把本章的核心概念串起来了。你可以把它当作一个“地图”，后面每章都会用到这些基础。

2.5 实战对比：三种姿态表示怎么选？