第2章：数学基础回顾：向量与矩阵运算、雅可比矩阵与海森矩阵、凸优化与非凸优化概念

各位同学，欢迎来到轨迹规划的第二课。

说实话，很多做机器人控制的朋友，一听到「数学基础」四个字就想关页面。我当年也一样，觉得搞算法嘛，调调参数就行了。直到有一次，我写的一个轨迹平滑算法在仿真里跑得飞起，一上真机就抖成筛子——后来才发现，是雅可比矩阵的伪逆计算出了问题。嗯，从那以后，我再也不敢小看这些基础概念了。

今天这一章，咱们就把轨迹优化里最常用的数学工具捋一遍。不搞纯理论推导，重点讲清楚「这东西是干嘛的」以及「我实际用的时候要注意什么」。

2.1 向量与矩阵运算：轨迹优化的「砖瓦」

轨迹优化，说白了就是在高维空间里找一个点（或者一条曲线）。这个点用向量表示，点与点之间的变换用矩阵描述。

我个人习惯把向量运算分成三类：

• 线性组合：说白了就是「加权求和」。比如轨迹上的插值点，本质上就是起点和终点的线性组合。
• 内积与范数：内积衡量两个向量的「相似度」，范数衡量向量的「长度」。在轨迹优化里，我们经常用范数来构造代价函数——比如让关节速度的平方和最小，就是最小化速度向量的 L2 范数。
• 矩阵乘法与转置：这个太常用了。雅可比矩阵乘以关节速度，得到末端速度，这就是矩阵乘法的物理意义。

2.2 雅可比矩阵与海森矩阵：一阶与二阶的「眼睛」

雅可比矩阵，我把它叫做「一阶敏感度矩阵」。它告诉你：输入变化一点点，输出会变化多少。

举个例子。你控制机械臂的关节转动 0.01 弧度，末端执行器会移动多少毫米？雅可比矩阵就是干这个的。在轨迹优化里，我们经常用雅可比矩阵把关节空间的约束映射到任务空间。

// 伪代码示例：计算雅可比矩阵并求伪逆
Matrix J = computeJacobian(q);  // 当前关节角 q 下的雅可比
Matrix J_pinv = J.transpose() * (J * J.transpose()).inverse();  // 伪逆
Vector delta_q = J_pinv * delta_x;  // 将末端误差映射回关节空间修正量

海森矩阵呢？它是二阶的「曲率」信息。你想想看，如果雅可比告诉你「这条路是下坡」，海森矩阵就告诉你「这个坡有多陡，弯有多急」。

在轨迹优化里，海森矩阵主要出现在牛顿法及其变体中。不过说实话，我很少直接计算完整海森矩阵——计算量太大了。实际项目中，我更喜欢用 BFGS 或 L-BFGS 这类拟牛顿法，它们用梯度信息近似海森矩阵，效果已经很好了。

2.3 凸优化与非凸优化：选对工具，事半功倍

凸优化，说白了就是「只有一个坑」的问题。你从任何起点出发，沿着梯度往下走，最终都会掉进同一个坑里——那就是全局最优解。

非凸优化呢？到处都是坑。你从不同起点出发，可能掉进不同的坑里。这些坑叫「局部最优」。

特性	凸优化	非凸优化
全局最优	保证找到	不保证
求解难度	容易，多项式时间	困难，NP-hard 常见
典型算法	梯度下降、内点法	随机搜索、进化算法、SQP
轨迹规划应用	QP 求解器（二次规划）	带避障的轨迹优化

你可能会问：那轨迹优化到底是凸的还是非凸的？

答案是：看情况。如果你只优化平滑性（比如最小化 jerk），加上线性约束（比如关节限位），那这个问题是凸的，用 QP 求解器几毫秒就搞定。但如果你加上避障约束（比如不能撞到桌子），那问题就变成非凸的了——因为避障约束在几何空间里是「挖掉一块区域」，这破坏了凸性。

2.4 本章小结

这一章的内容，说白了就是三句话：

• 向量和矩阵是轨迹优化的「语言」，你得会读写。
• 雅可比和海森矩阵是优化的「眼睛」，帮你看到方向和曲率。
• 凸优化和非凸优化是「工具箱」，选对了工具，问题就解决了一半。

下一章，咱们开始讲具体的轨迹表示方法。到时候你会发现，今天这些数学工具全都会用上。