第二讲：数学基础回顾——向量与矩阵、坐标系与变换、欧拉角与四元数、李群与李代数初步

各位同学，欢迎来到《多约束条件下的轨迹优化实战》的第二讲。

说实话，做轨迹优化这么多年，我最大的体会就是：数学基础决定上层建筑。你想想看，如果连旋转矩阵都写不对，那后面谈什么避障、平滑、时间最优？

这一讲，我们快速过一遍后面会反复用到的数学工具。别怕，我不讲枯燥的推导，只讲你写代码时真正用得上的东西。

2.1 向量与矩阵：你每天都在用的线性代数

向量和矩阵，说白了就是描述空间状态和变换的「语言」。

  • 向量：表示位置、速度、加速度。在三维空间中，一个位置向量 p = [x, y, z]^T 就搞定了。
  • 矩阵：表示变换。比如旋转矩阵 R，能把一个向量从机体坐标系转到世界坐标系。

我个人习惯用 列向量 表示点，用 行向量 表示法线或梯度。为什么？因为矩阵乘法默认是左乘，列向量更符合直觉。

核心操作速查：
  • 点积：a·b = a^T b，用于计算投影、夹角。
  • 叉积：a × b，用于计算法向量、旋转轴。
  • 矩阵乘法：C = A B，注意维度匹配（m×n 乘以 n×p）。
  • 转置：A^T，交换行列。
  • 逆矩阵：A^{-1}，用于「撤销」变换。
避坑指南： 我曾经在写轨迹插值算法时，把矩阵乘法的顺序搞反了，结果机器人直接往墙上撞。记住：R * p 是先旋转后平移，而 p * R 是反的。在代码里，左乘 vs 右乘 一定要分清。

2.2 坐标系与变换：从世界到机体

做轨迹优化，你至少需要搞懂三个坐标系：

  1. 世界坐标系（W）：固定在地面，通常 Z 轴向上。
  2. 机体坐标系（B）：固定在机器人上，通常 X 轴向前。
  3. 工具坐标系（T）：固定在末端执行器上，比如机械臂的夹爪。

变换的核心公式就一个：

p_W = R_WB * p_B + t_WB

其中 R_WB 是旋转矩阵，t_WB 是平移向量。

嗯，这里要注意：齐次变换矩阵 可以把旋转和平移写在一起：

T_WB = [ R_WB  t_WB ]
       [ 0^T    1   ]

这样，p_W = T_WB * p_B 一步到位。我在做多机器人协同规划时，全靠齐次矩阵来统一不同机器人的位姿表达。

常见错误： 很多人把 T_WB 和 T_BW 搞混。记住：T_WB 表示「从 B 系到 W 系的变换」，下标顺序是「目标_源」。我曾经因为这个搞反了，导致两台机器人差点撞上。

2.3 欧拉角与四元数：旋转的两种表达

旋转矩阵有9个参数，但自由度只有3个。所以我们需要更紧凑的表达方式。

欧拉角

欧拉角用三个角度描述旋转：偏航（Yaw）、俯仰（Pitch）、滚转（Roll）。

  • 优点：直观，容易理解。
  • 缺点：万向锁（Gimbal Lock）。当 Pitch 接近 ±90° 时，Yaw 和 Roll 会失去一个自由度。

我建议：只在人机交互界面用欧拉角，算法内部尽量别用。

四元数

四元数用四个参数表示旋转：q = [w, x, y, z]^T，其中 w 是实部，(x, y, z) 是虚部。

  • 优点：无万向锁，插值平滑（球面线性插值 Slerp）。
  • 缺点：不够直观，需要习惯。

四元数乘法：

q1 * q2 = [w1*w2 - x1*x2 - y1*y2 - z1*z2,
           w1*x2 + x1*w2 + y1*z2 - z1*y2,
           w1*y2 - x1*z2 + y1*w2 + z1*x2,
           w1*z2 + x1*y2 - y1*x2 + z1*w2]^T
我的经验： 在轨迹优化中，我通常用四元数做姿态插值。比如从 q_start 到 q_end，用 Slerp 插值可以保证旋转路径最短。但要注意：四元数必须归一化，否则插值结果会变形。

2.4 李群与李代数初步：优化问题的核心工具

为什么需要李群和李代数？因为旋转矩阵 SO(3) 和齐次变换矩阵 SE(3) 不是向量空间，不能直接做加减法。

  • 李群：连续的、光滑的群。比如 SO(3)（旋转矩阵）和 SE(3)（齐次变换矩阵）。
  • 李代数：李群在单位元处的切空间。比如 so(3)（三维旋转向量）和 se(3)（六维运动旋量）。

核心关系：

R = exp(θ * [ω]×)   // 从李代数到李群（指数映射）
θ * [ω]× = log(R)    // 从李群到李代数（对数映射）

其中 [ω]× 是 ω 的反对称矩阵。

说白了，李代数允许我们把旋转「线性化」，这样就能用常规的优化算法（比如高斯-牛顿法）来求解。

为什么轨迹优化离不开李代数？
  • 在优化过程中，我们需要对位姿求导。李代数提供了 右扰动 和 左扰动 两种求导方式。
  • 比如，你想知道「旋转矩阵 R 对时间 t 的导数」，用李代数可以写成 dR/dt = R * [ω]×。
  • 在 因子图优化 和 非线性最小二乘 中，李代数是标准工具。
注意： 李代数的指数映射和对数映射在数值上可能不稳定。当旋转角度接近 0 或 π 时，建议用 泰勒展开 近似。我曾经在优化过程中因为没处理这个，导致雅可比矩阵奇异，优化直接发散。

知识体系总览

下面这张图总结了本章的核心逻辑：从基础数学工具到轨迹优化的应用链路。

数学基础回顾：知识体系 向量与矩阵 坐标系与变换 欧拉角与四元数 旋转的多种表达方式 李群 (SO(3), SE(3)) 与 李代数 (so(3), se(3)) 轨迹优化：位姿插值、雅可比计算、非线性最小二乘 核心工具链

好了，这一讲的内容就到这里。数学基础是枯燥的，但它是你后面写优化代码的「底气」。下一讲我们会进入正题，开始搭建轨迹优化的基本框架。

课后练习：
  1. 写一个函数，把欧拉角转成四元数，再转回欧拉角，验证是否一致。
  2. 用李代数实现一个简单的旋转矩阵求导，验证数值梯度与解析梯度是否一致。

可可精品资料库

← 上一章 📖 返回目录 下一章 →