一、轨迹规划概述

1.1 什么是轨迹规划

轨迹规划，说白了就是回答三个问题：去哪儿、怎么去、什么时候到。

我刚开始接触这个领域时，觉得它不就是路径规划嘛。后来踩了坑才明白——路径规划只关心空间上的点，而轨迹规划还要管时间和速度。你想想看，一个机械臂从A点走到B点，路径规划告诉你走哪条线，轨迹规划则告诉你每个时刻手臂应该在什么位置、以多快的速度运动。

用公式表达就是：

轨迹 = 路径 + 时间律
即：q(t) = [x(t), y(t), z(t), θ(t), ...]

这里的 q(t) 是随时间变化的位姿向量。它包含了位置、姿态，甚至关节角度。嗯，这里要注意——轨迹规划的输出不是一条几何线，而是一条带时间戳的状态序列。

1.2 轨迹规划在机器人系统中的位置

我习惯把机器人软件架构分成三层：

• 决策层：任务分配、行为选择（比如“去抓杯子”）
• 规划层：轨迹规划、路径搜索（比如“怎么抓”）
• 控制层：伺服控制、PID调节（比如“电机转多少”）

轨迹规划正好卡在中间。它上接任务指令，下连底层控制。我在做AGV项目时遇到过一个问题——规划层输出的轨迹太粗糙，控制层拼命追都追不上，结果车子一路抖。后来我才意识到，轨迹规划必须考虑控制器的能力边界。

来看一张我常用的系统架构图：

从这张图你能看到，轨迹规划不是孤立存在的。它接收决策层的目标，输出给控制层去执行。同时，控制层的状态反馈也会影响规划——比如遇到障碍物需要重新规划。

1.3 核心评价指标

做轨迹规划这么多年，我总结出三个绕不开的指标。任何一个实际项目，你都得在这三者之间找平衡。

1.3.1 平滑性

平滑性差，机器人就会抖。抖不仅影响精度，还会加速机械磨损。我在做协作机器人项目时，就因为轨迹不够平滑，导致末端执行器在抓取时反复震荡——零件都飞出去了。

平滑性通常用以下指标衡量：

指标	数学表达	物理含义
位置连续	q(t) ∈ C⁰	轨迹没有断点
速度连续	q̇(t) ∈ C⁰	速度没有突变
加速度连续	q̈(t) ∈ C⁰	加速度没有跳变
加加速度连续	q⃛(t) ∈ C⁰	冲击最小化

1.3.2 安全性

安全性是底线。轨迹规划必须保证机器人不与障碍物碰撞，同时自身关节不超限。

安全性约束通常包括：

• 碰撞避免：轨迹与障碍物的最小距离 > 安全阈值
• 关节限位：每个关节角度在 [θ_min, θ_max] 内
• 速度/加速度限幅：不超过电机和减速器的承受范围
• 奇异点规避：远离雅可比矩阵奇异的位形

我记得有一次做移动机器人导航，规划出的轨迹贴着墙边走。理论上是安全的，但实际运行时传感器噪声导致定位偏差，机器人直接蹭上了墙。从那以后，我习惯在安全距离上加一个冗余系数——说白了就是留余量。

1.3.3 时间最优

时间最优，就是让机器人跑得尽可能快。但快不是无脑加速——你得在平滑性和安全性之间找最优解。

时间最优轨迹规划本质上是一个优化问题：

min  T = ∫₀ᵀ dt
s.t. q(0) = q_start, q(T) = q_goal
     |q̇(t)| ≤ v_max
     |q̈(t)| ≤ a_max
     |q⃛(t)| ≤ j_max
     碰撞距离 ≥ d_safe

这个问题的难点在于约束是非线性的，而且维度高。我常用的解法有两种：

方法	原理	适用场景	计算量
梯形速度规划	加速-匀速-减速三段	简单点对点运动	极低
S形速度规划	加加速-匀加速-减加速-匀速-加减速-匀减速-减减速七段	对平滑性有要求的场景	低
数值优化（如TOPP）	在路径上离散化，求解时间最优参数化	复杂约束下的最优轨迹	中等
直接配点法	将轨迹离散为节点，用非线性规划求解	高维、强约束场景	高

你可能会问：为什么不直接用最快的？因为时间最优往往意味着加速度和加加速度接近极限，这会带来两个问题：一是电机容易过热，二是轨迹对扰动非常敏感。我在一个高速分拣项目里就吃过这个亏——为了追求节拍时间，把加速度推到极限，结果稍微有点负载波动，轨迹就跟踪不上。

1.4 三个指标的权衡

平滑性、安全性、时间最优，这三者不可能同时做到极致。你想想看：

• 要绝对平滑 → 就得降低速度 → 时间变长
• 要绝对安全 → 障碍物距离留大 → 路径绕远 → 时间变长
• 要时间最优 → 加速度逼近极限 → 平滑性下降

我个人的经验是：先保安全，再求平滑，最后优化时间。安全是红线，碰不得。平滑性影响执行质量，时间最优影响生产效率。具体怎么权衡，取决于你的应用场景。