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

各位同学，欢迎来到第二章。说实话，轨迹规划这个领域，看着花哨，底子就是数学。我见过不少同学一上来就猛啃算法，结果推导到一半卡在坐标变换上，代码跑出来全是奇点。这一章，咱们就把这些基础工具彻底捋一遍。

你想想看，机器人要运动，首先得知道自己在哪里、目标在哪里、中间怎么过去。这些问题的答案，全藏在向量、矩阵和刚体运动描述里。别怕，咱们不搞纯数学推导，我带你从工程视角过一遍。

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

向量是什么？说白了就是空间里的一个箭头。位置、速度、加速度，在机器人眼里全是向量。矩阵呢？就是给这个箭头做变换的“操作员”。

2.1.1 向量运算

先看最基本的。一个三维向量 v = [x, y, z]^T，表示从原点指向 (x, y, z) 的箭头。我们常用的运算就这几个：

• 点积：a · b = |a||b|cosθ。用来算夹角、算投影。我在做路径平滑时，经常用点积判断两个速度方向是否一致。
• 叉积：a × b = |a||b|sinθ · n。结果是一个垂直于 a、b 平面的向量。嗯，这里要注意，叉积不满足交换律，a×b = -b×a。
• 模长：||v|| = sqrt(x² + y² + z²)。就是箭头的长度，也就是距离。

2.1.2 矩阵运算

矩阵在轨迹规划里，主要干两件事：变换和求解。

运算	数学表示	轨迹规划中的应用
矩阵乘法	C = A × B	复合变换：先旋转再平移
矩阵转置	A^T	求逆的快捷方式（正交矩阵）
矩阵求逆	A^{-1}	从世界坐标回到局部坐标
特征值分解	A = QΛQ^{-1}	分析轨迹的稳定性

我个人习惯，写代码时尽量用现成的线性代数库（比如 Eigen、NumPy），但一定要理解背后的几何意义。矩阵乘法不满足交换律，AB ≠ BA，这个坑我踩过不止一次。

2.2 坐标系与齐次坐标

机器人世界里，坐标系多到你头疼。世界坐标系、基坐标系、工具坐标系、相机坐标系……每个物体都有自己的“小天地”。怎么在这些坐标系之间来回切换？答案就是齐次坐标。

2.2.1 为什么需要齐次坐标？

你想想看，一个三维空间里的点，用 (x, y, z) 表示。旋转可以用 3×3 矩阵搞定，但平移呢？你没法用 3×3 矩阵表示平移。怎么办？加一维。

齐次坐标把三维点变成四维：p = [x, y, z, 1]^T。方向向量则用 [x, y, z, 0]^T。这样一来，旋转和平移就能统一在一个 4×4 矩阵里了。

2.2.2 齐次变换矩阵

一个标准的齐次变换矩阵长这样：

T = [ R   t ]
    [ 0   1 ]

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

这个矩阵 T 可以同时完成旋转和平移。复合变换就是矩阵乘法：T_13 = T_12 × T_23。注意顺序，先乘的变换先发生。

2.3 刚体运动描述

刚体运动，说白了就是一个物体在空间里又转又移，但自身不变形。描述刚体姿态，主要有三种方式：旋转矩阵、欧拉角、四元数。我一个个说。

2.3.1 旋转矩阵

旋转矩阵 R 是一个 3×3 的正交矩阵，满足 R^T R = I，det(R) = 1。它把向量从一个坐标系旋转到另一个坐标系。

绕 X、Y、Z 轴旋转 θ 角的基本矩阵：

R_x(θ) = [1    0       0    ]
         [0  cosθ  -sinθ ]
         [0  sinθ   cosθ ]

R_y(θ) = [ cosθ  0  sinθ ]
         [ 0     1    0   ]
         [-sinθ  0  cosθ ]

R_z(θ) = [cosθ -sinθ  0 ]
         [sinθ  cosθ  0 ]
         [ 0     0    1 ]

复合旋转就是矩阵乘法。但这里有个大坑：万向锁。用欧拉角（比如 ZYX 顺序）描述姿态时，当第二个旋转角达到 ±90°，第一个和第三个旋转轴会重合，丢失一个自由度。

2.3.2 四元数

四元数是什么？说白了就是“带约束的复数在三维空间的推广”。一个四元数 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部，满足 i² = j² = k² = ijk = -1。

单位四元数（模长为1）可以用来表示旋转。绕单位轴 u 旋转 θ 角：

q = [cos(θ/2), u_x·sin(θ/2), u_y·sin(θ/2), u_z·sin(θ/2)]

四元数的最大好处：无奇点、可平滑插值。球面线性插值（SLERP）就是专门为四元数设计的。

特性	旋转矩阵	欧拉角	四元数
存储空间	9个元素	3个元素	4个元素
奇点问题	无	有（万向锁）	无
插值平滑性	差	差	好（SLERP）
复合运算	矩阵乘法	复杂	四元数乘法
直观性	一般	好	差

我个人习惯，在代码里用四元数做内部运算，只在用户界面显示时转成欧拉角。这样既避免了奇点，又方便调试。

2.4 知识体系总览

下面这张图，把本章的核心知识点串起来了。你可以把它当作一个“思维地图”，以后遇到具体问题，先定位到对应的模块。

2.5 本章小结

这一章的内容，说白了就是轨迹规划的“工具箱”。向量和矩阵是基础运算工具，齐次坐标和变换矩阵是坐标系转换工具，旋转矩阵和四元数是姿态描述工具。你不需要背下所有公式，但一定要理解每个工具的适用场景和局限性。