第2章 数学基础回顾：向量与矩阵、李群与李代数、旋转矩阵与四元数、刚体运动学

各位同学，欢迎来到《复杂环境下轨迹优化实战宝典》的第二讲。

说实话，每次开课讲到数学基础，我都怕大家觉得枯燥。但这一章，我真心建议你沉下心来看完。为什么？因为后面所有轨迹优化、状态估计、SLAM后端优化的代码，底层全是这些数学工具在撑腰。我自己当年在项目里踩过的坑，十有八九都是因为某个矩阵性质没搞懂，或者四元数更新算错了方向。

好，我们直接进入正题。

2.1 向量与矩阵：机器人学的“砖瓦”

向量和矩阵，说白了就是用来描述空间中的点和变换的。一个三维向量 p = [x, y, z]^T 可以表示一个位置，一个3x3矩阵可以表示一个旋转。

向量运算：点积（内积）和叉积（外积）是两大核心。

• 点积：a · b = |a||b|cosθ，用于判断两个向量是否垂直（点积为0），或者投影长度计算。
• 叉积：a × b 生成一个垂直于a和b的向量，方向由右手定则决定。模长等于 |a||b|sinθ。

矩阵运算：矩阵乘法、转置、逆、行列式。

• 矩阵乘法：不满足交换律，即 AB ≠ BA。这一点在组合旋转时尤其要小心。
• 转置：交换行和列。旋转矩阵的转置等于它的逆，这是一个非常重要的性质。
• 逆矩阵：如果矩阵可逆，则 A * A^{-1} = I。在求解线性方程组时经常用到。
• 行列式：表示矩阵所代表的线性变换对空间体积的缩放比例。旋转矩阵的行列式为+1，表示纯旋转，不改变体积。

2.2 旋转矩阵与四元数：姿态的两种“语言”

在机器人学里，描述一个物体的朝向（姿态），最常用的就是旋转矩阵和四元数。它们各有优缺点，我分别说说。

2.2.1 旋转矩阵

旋转矩阵是一个3x3的正交矩阵，满足 R^T R = I 且 det(R) = 1。它最大的优点是直观——矩阵的每一列就是旋转后的坐标轴在原始坐标系下的表示。

但缺点也很明显：

• 冗余：9个参数描述3个自由度，存在约束，数值优化时容易不满足正交性。
• 插值困难：直接对矩阵元素做线性插值，得到的矩阵大概率不是旋转矩阵。

2.2.2 四元数

四元数 q = [w, x, y, z]^T 是一种紧凑、无奇异的姿态表示方法。它本质上是一个单位四元数，满足 w^2 + x^2 + y^2 + z^2 = 1。

四元数的乘法可以组合旋转，而且比矩阵乘法快（只需要16次乘法和12次加法）。

四元数转旋转矩阵：

R = [[1-2(y^2+z^2), 2(xy-wz), 2(xz+wy)],
     [2(xy+wz), 1-2(x^2+z^2), 2(yz-wx)],
     [2(xz-wy), 2(yz+wx), 1-2(x^2+y^2)]]

旋转矩阵转四元数：

w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2
x = (R[2][1] - R[1][2]) / (4 * w)
y = (R[0][2] - R[2][0]) / (4 * w)
z = (R[1][0] - R[0][1]) / (4 * w)

2.3 李群与李代数：连续优化的“瑞士军刀”

李群和李代数，听起来很高大上，其实核心思想很简单：在流形上做优化。

旋转矩阵构成的集合 SO(3) 是一个李群，它是一个连续的、光滑的流形。而它的李代数 so(3) 是流形在单位元处的切空间，由三维向量 φ = [φ1, φ2, φ3]^T 组成。

为什么需要李代数？

• 旋转矩阵有约束（正交、行列式为1），直接优化很难。
• 四元数也有单位约束。
• 李代数是无约束的向量空间，可以在上面做常规的加法、梯度下降。

指数映射：将李代数映射到李群。对于 so(3)，指数映射就是罗德里格斯公式：

R = exp(φ^) = I + sin(θ) * (φ^/θ) + (1 - cos(θ)) * (φ^/θ)^2

其中 θ = ||φ||，φ^ 是 φ 的反对称矩阵。

对数映射：将李群映射回李代数，即从旋转矩阵中提取旋转向量。

2.4 刚体运动学：位置与姿态的“时间演化”

刚体运动学描述的是物体位置和姿态随时间的变化关系。对于轨迹规划来说，我们关心的是：给定速度、角速度，如何更新位姿。

位置更新：

p(t + dt) = p(t) + v(t) * dt

很简单，就是速度乘以时间。

姿态更新：这里就有讲究了。如果用旋转矩阵：

R(t + dt) = R(t) * exp(ω^ * dt)

其中 ω 是角速度向量，ω^ 是它的反对称矩阵。

如果用四元数：

q(t + dt) = q(t) ⊗ [cos(||ω|| * dt / 2), (ω/||ω||) * sin(||ω|| * dt / 2)]^T

其中 ⊗ 表示四元数乘法。

2.5 知识体系总览

为了让你更直观地理解这些数学工具之间的关系，我画了一张图：

从图中你可以看到：向量与矩阵是基础，旋转矩阵和四元数是描述姿态的两种具体形式，它们之间可以互相转换。而李群与李代数则提供了在流形上做优化的数学框架，最终服务于刚体运动学——也就是我们轨迹规划中实际要用的位置和姿态更新公式。

2.6 本章小结

这一章的内容，说白了就是为后续的轨迹优化打地基。你不需要一次性全部记住，但一定要理解它们之间的逻辑关系：

• 用向量和矩阵描述位置和旋转。
• 用旋转矩阵或四元数表示姿态。
• 用李代数做无约束优化。
• 用刚体运动学公式做状态更新。

嗯，这些工具在后面的章节里会反复出现。尤其是李代数，它在轨迹优化中几乎是标配。我个人建议你花点时间把指数映射和对数映射的代码写一遍，这样印象会深很多。

---