6.2 轨迹优化与平滑：时间-加加速度最优与硬约束

6.2 轨迹优化与平滑：时间-加加速度最优与硬约束

运动规划给的是「一串路径点」，但机器人不能瞬间从一点跳到另一点。轨迹优化的任务是把路径点变成「随时间变化的位置/速度/加速度曲线」，让机器人能平稳、快速、安全地执行。

6.2.1 路径 vs 轨迹

先澄清两个概念：

• 路径（Path）：纯几何，只有形状，无时间。\sigma: [0, 1] \to \mathcal{C}。
• 轨迹（Trajectory）：路径 + 时间，含速度、加速度信息。\sigma: [0, T] \to \mathcal{C}。

轨迹优化就是把路径 \sigma(s) 「时间参数化」为 \sigma(t)，并选择最优的时间分配。

6.2.2 轨迹平滑的目标

不平滑的轨迹会导致：

• 机械冲击：加加速度（Jerk，加速度的导数）突变损坏机械结构。
• 振动：激发机械臂共振。
• 能耗高：频繁加减速。
• 跟踪误差大：控制器跟不上突变。

所以轨迹优化追求 平滑性：位置、速度、加速度、加加速度都连续，最好加加速度最小。

6.2.3 最小 Jerk 轨迹

最小 Jerk 轨迹（Minimum Jerk Trajectory） 是经典的平滑轨迹。目标函数：

最小化加加速度平方积分。在固定起点终点、固定起止速度加速度为零的约束下，闭式解是 5 次多项式：

这个 5 次多项式轨迹在机器人学中极为常用——简单、平滑、有解析解。

def min_jerk(start, end, T, num_steps):
    """5 次多项式最小 Jerk 轨迹"""
    traj = []
    for i in range(num_steps):
        tau = i / (num_steps - 1)
        s = 10 * tau**3 - 15 * tau**4 + 6 * tau**5
        x = start + (end - start) * s
        traj.append(x)
    return traj
​

6.2.4 梯形速度规划

工业机器人更常用的是梯形速度规划（Trapezoidal Velocity Profile）——简单、可解析、易于实时计算：

速度 v(t)
  ^
  |     ____________  ← 恒速段
  |    /            \
  |   /              \
  |  / 加速           \ 减速
  | /                  \
  |/____________________\___→ 时间 t
   0   t1            t2   T
​

三个阶段：

1. 加速段 [0, t1]：恒定加速度 a，速度从 0 升到 v_max。
2. 恒速段 [t1, t2]：保持 v_max。
3. 减速段 [t2, T]：恒定减速 -a，速度从 v_max 降到 0。

优点：计算简单、易于硬件实现。
缺点：加速度不连续（在 t1, t2 处突变），引发冲击。

升级版 S 曲线（S-curve） 用 7 段（含加加速段恒定的过渡），加速度也连续，更平滑。工业机器人（UR、Kuka）默认用 S 曲线。

6.2.5 时间最优化

固定形状的路径（如最小 Jerk），还有 时间参数 T 怎么选？这涉及时间最优轨迹规划（TOTP，Time-Optimal Trajectory Planning）：

约束：

• 关节速度上限 \dot{q} \leq \dot{q}_{max}
• 关节加速度上限 \ddot{q} \leq \ddot{q}_{max}
• 关节力矩上限 \tau \leq \tau_{max}（考虑动力学）

目标：最小化总时间 T。

经典算法是 动态规划（DP）在「位置-速度」相平面求解：

1. 把路径参数化为弧长 s。
2. 在 (s, \dot{s}) 相平面上找可行区域（受速度、加速度、力矩约束）。
3. 用 DP 找到时间最短的切换曲线。

代表实现如 Khalil-Luce 方法、TOPP（Time-Optimal Path Parameterization） 库。

6.2.6 TOPP-RA：现代时间最优

TOPP-RA（TOPP via Reachability Analysis） 是当前最先进的时间最优算法：

• 用可达性分析（reachability analysis）替代数值积分。
• 数值稳定性好，能处理复杂约束。
• 开源实现（Python），与 MoveIt、Pinocchio 集成。

工业级机器人轨迹优化的标准工具之一。

6.2.7 轨迹优化中的硬约束

除了平滑和时间，轨迹还要满足各种约束：

把这些约束写成不等式，用 QP（二次规划） 求解器（如 OSQP、qpOASES）求解：

每条轨迹由 N 个构型点构成，优化变量是 N \times n（n 个关节）维向量。

6.2.8 MPPI：随机采样轨迹优化

MPPI（Model Predictive Path Integral） 是另一种思路——用随机采样 + 蒙特卡洛优化轨迹，特别适合高维非线性系统（自动驾驶、人形机器人）：

def MPPI(current_state, dynamics, cost, horizon, num_samples):
    # 1. 生成 num_samples 条候选控制序列
    nominal_control = previous_best  # 上一时刻的最优
    samples = []
    for _ in range(num_samples):
        noise = sample_gaussian(horizon, control_dim)
        samples.append(nominal_control + noise)
    
    # 2. 对每条样本前向模拟，计算代价
    costs = []
    for control_seq in samples:
        state = current_state
        total_cost = 0
        for u in control_seq:
            state = dynamics(state, u)
            total_cost += cost(state, u)
        costs.append(total_cost)
    
    # 3. 用代价加权平均得到最优控制
    weights = softmax(-costs / lambda)
    optimal = sum(w * s for w, s in zip(weights, samples))
    
    # 4. 只执行最优控制的第一步，下一时刻重规划（MPC 思路）
    return optimal[0]
​

MPPI 的优势：

• 不需要可微的代价函数（可以处理硬约束、离散事件）。
• 高度并行（GPU 同时模拟 num_samples 条轨迹）。
• 适合高度非线性、多模态代价（如「既要避障又要快速到达」）。

MPPI 已成为 自动驾驶轨迹规划 和 人形机器人 MPC 的主流方法之一。

6.2.9 轨迹优化 vs 运动规划

典型流程：

起点终点 → RRT*/CHOMP → 路径 → TOPP-RA → 时间最优轨迹 → 控制器
​

6.2.10 学习型轨迹优化

最近用神经网络做轨迹优化的工作增加：

• Neural MPPI：用神经网络提供初始控制猜测，缩小 MPPI 搜索范围。
• Diffusion Planner：用扩散模型直接生成多模态轨迹。
• VLA + 后处理：VLA 输出动作，再用轨迹优化平滑（如 OpenVLA + MPC）。

这些方法的优势是推理快，但牺牲了部分可解释性和安全保证。

6.2.11 整体平滑的工程实践

实际系统中，轨迹平滑通常这样组合：

每一层都对上一层的结果做精修，构成多层平滑栈。

💡 生产箴言：永远不要把运动规划的原始输出直接送给控制器。RRT 路径充满尖角，控制器跟踪时会引发剧烈抖动。一定要经过路径平滑（B-spline）+ 轨迹优化（TOPP）+ 控制器（MPC）三层处理。

6.2.12 VLA 输出动作的轨迹后处理

第 5 章 VLA 输出的动作 chunk（如未来 8-16 步）本质也是粗糙轨迹，需要后处理：

VLA 输出 chunk → 平滑滤波（如 EMA） → 轨迹插值（如三次样条） → 控制器
​

或者更激进：

VLA 输出 → 作为 MPC 的参考轨迹 → MPC 优化（含动力学约束） → 控制器
​

后者让 MPC 同时考虑 VLA 的「意图」和真实动力学，是当前 VLA + 传统控制融合的主流方案。

本节小结

• 轨迹 = 路径 + 时间；轨迹优化把路径转化为含时间的连续曲线。
• 最小 Jerk 轨迹用 5 次多项式，闭式解，是经典平滑方案。
• 梯形速度规划简单但加速度不连续；S 曲线平滑；工业多用 S 曲线。
• 时间最优化（TOTP/TOPP/TOPP-RA）在速度、加速度、力矩约束下最小化总时间。
• 硬约束轨迹优化用 QP（OSQP/qpOASES）求解，约束包括关节、速度、力矩、碰撞、任务。
• MPPI 用随机采样 + 蒙特卡洛，适合高维非线性、多模态代价。
• 典型流程：RRT* → B-spline 平滑 → TOPP-RA → MPC → 控制器。
• VLA 输出 chunk 也需要后处理：平滑滤波 + MPC 优化。

下一节《6.3 底层控制与逆动力学》将深入从关节指令到电机力矩的映射。