4、运动控制基础（三）：轨迹规划
好，咱们接着聊运动控制。前两章我们把坐标系、插补这些基础打牢了，今天要啃的这块骨头，叫轨迹规划。
说白了，轨迹规划就是回答一个问题：“电机从A点跑到B点，中间怎么走？”
你可能会想，直接给个速度跑过去不就行了？嗯，没那么简单。直接给阶跃速度，电机起步那一瞬间加速度无穷大，轻则抖动，重则丢步甚至烧驱动器。我早期做一个小型桌面机械臂时就吃过这个亏——电机一启动，整个平台都在晃，吓得我赶紧断电。
所以，轨迹规划的核心就是：让运动平滑、可控、不冲击机械结构。
核心知识点一览：
- 梯形速度曲线（T-curve）—— 简单粗暴，但加减速有冲击
- S形曲线（S-curve）—— 平滑，适合高精度场景
- 多项式插值 —— 灵活，可定制任意轨迹
- 时间最优路径规划 —— 在约束下跑得最快
梯形速度曲线
梯形曲线，名字很形象——速度-时间图像像个梯形。分为三段：匀加速、匀速、匀减速。
它的优点是简单，计算量极小。在资源受限的MCU上，我经常用。但缺点也很明显：加速度在拐点处突变，产生无穷大的加加速度（Jerk）。机械结构会感受到冲击。
我的经验：梯形曲线适合对成本敏感、负载轻、速度不高的场景。比如小型的传送带、简单的拾放机构。但如果你做的是精密平台或者机器人关节，我建议你跳过它，直接上S形曲线。
梯形曲线的核心公式其实就三个阶段：
// 梯形速度曲线 - 伪代码
// 已知：总位移 S，最大速度 Vmax，加速度 A，减速度 D（通常 A = D）
// 加速段：t ∈ [0, Ta]
v(t) = A * t
s(t) = 0.5 * A * t²
// 匀速段：t ∈ [Ta, Ta + Tc]
v(t) = Vmax
s(t) = 0.5 * A * Ta² + Vmax * (t - Ta)
// 减速段：t ∈ [Ta + Tc, T]
v(t) = Vmax - D * (t - Ta - Tc)
s(t) = 0.5 * A * Ta² + Vmax * Tc + Vmax*(t-Ta-Tc) - 0.5*D*(t-Ta-Tc)²
你想想看，如果位移很短，还没加速到Vmax就要减速了，梯形就变成了三角形。这个边界条件一定要判断，否则算出来的时间会是负数。我见过不少新手在这里翻车。
S形曲线
S形曲线，说白了就是给加速度也加了一个“缓启动”。速度-时间图像像个拉长的S。它把梯形曲线的加速度突变，变成了连续变化的加加速度。
S形曲线通常分为七段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。听着复杂，但实际用起来，你只需要关心三个参数：最大速度、最大加速度、最大加加速度。
为什么用S形？
因为机械系统最怕的就是“抖”。加加速度突变会引起共振。我在做一台高速点胶机时，梯形曲线跑到300mm/s时机器就开始尖叫，换成S形曲线后，跑到500mm/s都安安静静。这就是S形的价值。
S形曲线的计算比梯形复杂不少，但好在有成熟的查表法或分段解析法。我个人的习惯是：在离线规划时用解析法，在实时控制时用查表法。
// S形曲线 - 加加速度阶段（Jerk = J）
// 加加速段：t ∈ [0, T1]
a(t) = J * t
v(t) = 0.5 * J * t²
s(t) = (1/6) * J * t³
// 匀加速段：t ∈ [T1, T2]
a(t) = Amax
v(t) = V1 + Amax * (t - T1) // V1 = 0.5*J*T1²
s(t) = S1 + V1*(t-T1) + 0.5*Amax*(t-T1)²
// 减加速段：t ∈ [T2, T3]
a(t) = Amax - J*(t - T2)
v(t) = V2 + Amax*(t-T2) - 0.5*J*(t-T2)²
s(t) = S2 + V2*(t-T2) + 0.5*Amax*(t-T2)² - (1/6)*J*(t-T2)³
// ... 后面还有匀速、减速段，对称结构
注意：S形曲线虽然平滑，但会延长运动时间。同样的最大速度和加速度，S形比梯形多出约10%~20%的时间。如果你追求极致节拍，需要权衡。
多项式插值
梯形和S形都是“固定形状”的曲线。但有些场景，比如机器人末端走复杂路径，或者需要经过多个中间点，这时候就需要多项式插值。
多项式插值的思路很简单：给定起点和终点的位置、速度、加速度约束，用一个多项式函数去拟合。最常见的是三次多项式和五次多项式。
- 三次多项式：给定位置和速度约束，4个方程解4个系数。计算快，但加速度不连续。
- 五次多项式：给定位置、速度、加速度约束，6个方程解6个系数。加速度连续，更平滑。
我记得有一次做六轴机器人的轨迹规划，客户要求末端经过5个点，每个点都要速度为零。用三次多项式分段插值，结果在连接点处加速度突变，机器人抖得像筛糠。换成五次多项式后，问题迎刃而解。
// 五次多项式插值
// 已知：q0, v0, a0 (起点位置、速度、加速度)
// qf, vf, af (终点位置、速度、加速度)
// T (运动时间)
// 求解系数 a0~a5
// q(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵
// 边界条件：
// q(0) = q0, q(T) = qf
// v(0) = v0, v(T) = vf
// a(0) = a0, a(T) = af
// 解线性方程组（矩阵形式）
// [1 0 0 0 0 0 ] [a0] [q0]
// [0 1 0 0 0 0 ] [a1] [v0]
// [0 0 2 0 0 0 ] [a2] [a0]
// [1 T T² T³ T⁴ T⁵ ] [a3] = [qf]
// [0 1 2T 3T² 4T³ 5T⁴ ] [a4] [vf]
// [0 0 2 6T 12T² 20T³ ] [a5] [af]
避坑指南：我曾经在STM32上直接解这个6x6矩阵，结果浮点运算太慢，导致控制周期超时。后来改用离线计算系数，在线只做多项式求值，问题就解决了。记住：嵌入式里，能离线算的绝不实时算。
时间最优路径规划
好，最后一个概念——时间最优路径规划。这个名字听起来高大上，其实核心思想很简单：在满足电机和机械约束的前提下，让运动时间最短。
约束包括：
- 电机最大转速（速度上限）
- 电机最大扭矩（加速度上限）
- 机械结构允许的加加速度上限
- 驱动器电流限制
时间最优规划，本质上是一个带约束的优化问题。常用的方法有：
- Bang-Bang控制：全程以最大加速度加速，然后以最大减速度减速。时间最短，但冲击最大。
- 数值优化方法：比如用凸优化或动态规划，在约束边界上“贴边”跑。
- 启发式方法：比如先按梯形算，再在拐角处圆滑处理。
我的看法：实际工程中，很少追求理论上的“时间最优”。因为最优解往往在约束边界上，稍微有点扰动就超限。我一般会留5%~10%的余量，这叫“工程最优”。
举个例子，一个直线运动，位移100mm，最大速度500mm/s，最大加速度2000mm/s²。梯形曲线算出来理论时间0.3秒。但实际跑的时候，加上S形平滑和余量，最终设成0.35秒。虽然慢了16%，但系统稳定多了，不会动不动就报错。
重要提醒：时间最优规划一定要考虑实际电机的响应延迟。你算出来的加速度，电机不一定能立刻跟上。我建议在仿真时加入一阶低通滤波，模拟电机电流环的响应，这样算出来的时间才靠谱。
好了，轨迹规划这块，核心就是这些。梯形简单但粗糙，S形平滑但稍慢，多项式灵活但计算量大，时间最优追求极致但需要留余量。实际项目中，我通常是先定约束，再选曲线，最后调参试跑。没有银弹，只有最适合你当前场景的方案。