📚 课程目录第2章基础数学预备

2. 基础数学预备

做路径规划与优化，说白了就是跟空间位置和姿态打交道。你想想看，机器人要动，首先得知道自己在哪、目标在哪、中间怎么转怎么走。这些问题的底层，全是数学。我刚开始接触这个领域时，觉得数学太抽象，后来踩了几个坑才明白——这些工具是拿来用的，不是拿来背的。

向量和矩阵，是描述空间最基础的语言。我个人习惯把向量看作“带方向和大小的箭头”，矩阵则是“对箭头进行变换的规则”。

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

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

避坑指南：我曾经在计算多个旋转矩阵的乘积时，忘记矩阵乘法是“右乘”还是“左乘”。记住：对向量左乘矩阵，表示在固定坐标系下变换；右乘矩阵，表示在自身坐标系下变换。这个搞反了，姿态全乱。

在三维空间中，一个物体从位置A到位置B，通常包含旋转和平移。如果分开处理，公式会变得很啰嗦。齐次坐标就是来解决这个问题的。

齐次坐标：给三维向量 (x, y, z) 加一个维度，变成 (x, y, z, 1)。这样，旋转和平移就可以用一个4×4矩阵统一表示。

// 齐次变换矩阵 T 的形式
T = [ R   t ]
    [ 0   1 ]

// 其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量
// 对点 p = (x, y, z, 1) 应用变换：
p' = T * p

这样做的好处很明显：多个变换可以连乘成一个矩阵。比如从世界坐标系到相机坐标系，再到机器人基座坐标系，只需要把对应的T矩阵乘起来就行。

我的经验：在实际项目中，我习惯把所有的变换都写成齐次矩阵形式。哪怕只有平移，也写成4×4矩阵。这样代码结构统一，调试时一眼就能看出问题。

说到旋转，你可能会问：用欧拉角不行吗？行，但欧拉角有万向锁问题。用旋转矩阵？矩阵有9个参数，但实际自由度只有3个，冗余且不便于优化。这时候，李群和李代数就登场了。

SO(3)：特殊正交群，就是所有3×3旋转矩阵的集合。它满足 RᵀR = I 且 det(R)=1。

so(3)：SO(3)对应的李代数，是3×3的反对称矩阵。每个反对称矩阵对应一个三维向量，也就是旋转轴和旋转角。

为什么需要李代数？因为旋转矩阵不能直接做加法（两个旋转矩阵相加结果不是旋转矩阵），但李代数可以。这样，我们就可以用常规的优化方法（比如高斯-牛顿法）来优化姿态了。

指数映射：从李代数 so(3) 映射到李群 SO(3)。说白了，就是把“旋转向量”变成“旋转矩阵”。

// 罗德里格斯公式：从旋转向量 ω 到旋转矩阵 R
R = I + sin(θ) * [ω]× + (1 - cos(θ)) * [ω]×²

// 其中 θ = ||ω||，[ω]× 是 ω 的反对称矩阵

SE(3)：特殊欧氏群，就是所有4×4齐次变换矩阵的集合。它对应的李代数是 se(3)，包含旋转和平移两部分。

注意：李代数到李群的指数映射，在 θ 接近0时会有数值不稳定性。我在做高精度路径优化时，遇到过因为角度太小导致计算发散的情况。解决方案是用泰勒展开近似，或者直接用 Sophus 这类库，它们已经处理好了。

下面这张图，是我自己总结的本章知识结构。你可以把它当作一个“地图”，学完后再回来看，会更有感觉。

我的建议：刚开始学李群李代数时，不要死磕数学推导。先会用 Sophus 或 Eigen 库做基本的指数映射、对数映射，跑几个小例子。等用熟了，再回头理解理论，会轻松很多。

好了，这一章的内容就到这里。数学是工具，不是目的。后面我们会反复用到这些知识，到时候你会觉得它们越来越自然。