1. 轨迹优化概述：从定义到数学建模

大家好，我是你们的老朋友。今天咱们正式开讲《轨迹优化与平滑处理方法》这门课。第一节课，我想先聊聊轨迹优化的基本概念、它到底用在哪儿，以及怎么把它变成一个数学问题。

说实话，我刚开始接触这个领域时，也觉得“轨迹优化”四个字挺唬人的。但干久了你会发现，它没那么神秘。说白了，就是给机器人、自动驾驶车或者无人机找一条“好”的路。这个“好”字，就是优化的核心。

1.1 轨迹优化的定义

轨迹优化，简单讲就是：在满足各种约束的前提下，找一条从起点到终点的最优路径。这里的“最优”可以是时间最短、能耗最低、运动最平滑，或者综合多个目标。

我习惯把轨迹优化分成两个层次：

• 路径规划：只关心几何路径，不考虑时间。比如A点到B点怎么走，不撞墙就行。
• 轨迹优化：在路径基础上加上时间信息，考虑速度、加速度、甚至 jerk（加加速度）。这才是真正的“运动规划”。

举个例子。我在做移动机器人项目时，一开始只做了路径规划，结果机器人走起来一顿一顿的，像在跳机械舞。后来加了轨迹优化，运动就流畅多了。嗯，这就是优化的价值。

1.2 应用领域：机器人、自动驾驶、无人机

轨迹优化不是纸上谈兵，它在三个领域用得最多。我分别说说。

1.2.1 机器人领域

工业机械臂、服务机器人、人形机器人，都离不开轨迹优化。你想想看，一个机械臂要从A点抓取工件放到B点，如果轨迹不优化，要么抖动大，要么关节超限，要么能耗高。

我记得有一次做焊接机器人项目，客户要求焊缝平滑。单纯用插值法根本不行，最后用了带 jerk 约束的优化，效果立竿见影。

1.2.2 自动驾驶

自动驾驶的轨迹优化更复杂。因为车是在动态环境中行驶，要考虑交通规则、障碍物、乘客舒适度。

我曾经参与过一个园区无人车项目。最头疼的是“换道轨迹”。如果只优化路径，换道会像“甩尾”一样。后来加了横向加速度约束，乘客才觉得舒服。

1.2.3 无人机

无人机轨迹优化，核心是“快”和“稳”。尤其是航拍、巡检、编队飞行，轨迹不平滑会导致画面抖动，甚至炸机。

我有个朋友做无人机编队表演，几百架无人机同时飞。轨迹优化没做好，两架差点撞上。从那以后，他对约束条件特别敏感。

1.3 优化问题的数学建模

好了，聊完应用，咱们来点硬核的。轨迹优化怎么变成数学问题？

一个标准的轨迹优化问题，包含三个要素：

• 状态变量：比如位置、速度、加速度
• 控制变量：比如力、力矩、油门
• 目标函数：要最小化的东西
• 约束条件：必须满足的限制

数学上可以写成：

minimize    J = ∫ L(x(t), u(t), t) dt + Φ(x(T))
subject to  ẋ(t) = f(x(t), u(t))      // 动力学约束
            g(x(t), u(t)) ≤ 0         // 不等式约束
            h(x(t), u(t)) = 0         // 等式约束
            x(0) = x_start            // 初始状态
            x(T) = x_goal             // 终端状态

别被公式吓到。我解释一下：

• J 是总代价，包括运行过程中的代价（比如能耗）和终端代价（比如到达精度）
• L 是瞬时代价，比如加速度平方，代表平滑性
• f 是动力学模型，描述系统怎么运动
• g 和 h 是约束，比如速度上限、障碍物距离

1.3.1 目标函数的设计

目标函数怎么选？我常用的几种：

目标类型	数学形式	适用场景
最小时间	∫ 1 dt	时间最优控制
最小能量	∫ u² dt	节能轨迹
最小 jerk	∫ (d³x/dt³)² dt	平滑轨迹
混合目标	w1*时间 + w2*能量	多目标权衡

我个人习惯用“加权和”的方式。但要注意，权重怎么调？我一般先跑一次仿真，看各个目标的量级，再调整权重让它们平衡。

1.3.2 约束条件的分类

约束条件分几类，我列一下：

• 边界约束：起点、终点固定
• 路径约束：不能碰障碍物
• 动力学约束：加速度不能太大，电机有扭矩上限
• 运动学约束：速度、角速度限制
• 输入约束：控制量有上下限

1.4 小结

这一章我们讲了轨迹优化的定义、应用和数学建模。你想想看，其实核心就三件事：定义目标、设置约束、求解最优。

下一章，我会讲具体的求解方法。比如怎么把连续问题离散化，怎么用数值优化器求解。到时候我会带一个完整的代码示例，手把手教大家。

嗯，今天就到这儿。有什么问题，欢迎课后交流。