TOPP-RA 原理解析：用可达性分析做时间最优路径参数化

在机器人规划中，“找到一条无碰路径”和“让机器人沿它尽可能快地跑完”是两件不同的事。

假设上层规划器已经给出一条几何路径 $\mathbf q(s)$，例如关节空间的样条曲线。现在不能随意给它一个时间轴：关节速度、加速度、力矩，甚至接触摩擦约束都可能在某些位置限制速度。Time-Optimal Path Parameterization（TOPP）要做的，就是寻找最快的单调时间律 $s(t)$，让最终轨迹 $\mathbf q(s(t))$ 全程满足这些约束。

TOPP-RA 的全称是 Time-Optimal Path Parameterization via Reachability Analysis。它的核心不是沿速度上限曲线做复杂的正反向积分，而是把问题离散成一串很小的线性规划：后向计算“现在的哪些速度还能到终点”，再前向贪心选择“还能保证到终点的最大加速度”。

本文依据 Pham 与 Pham 的论文 A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis（IEEE T-RO，2018）推导算法。它讨论的是给定几何路径后的时间参数化，而不是路径搜索或在线状态到状态轨迹生成。

1. TOPP 和 Ruckig 解决的问题不同

Ruckig 的输入是当前状态与目标状态；TOPP-RA 的输入则是已经固定的路径：

\[\mathbf q(s),\qquad s\in[0,s_{\mathrm{end}}].\]

它不改变路径形状，只求一个严格递增的 $s(t)$，使

\[\mathbf q(t)=\mathbf q(s(t))\]

走完整条路径的时间

\[T=\int_0^{s_{\mathrm{end}}}\frac{1}{\dot s}\,ds\]

最小。

图 1：路径 $\mathbf q(s)$ 由上层给定；TOPP-RA 只计算如何随时间推进路径坐标 $s(t)$。

因此 TOPP-RA 适用于“路径已知”的任务：加工、焊接、3D 打印、沿无碰路径移动机械臂等。若传感器每毫秒都在改变目标，通常应优先考虑 Ruckig 一类在线轨迹生成器；两者也可以串联：上层先规划路径，TOPP-RA 离线或准在线给出时间律，底层再做跟踪或局部平滑。

2. 把机器人约束投影到一维路径坐标

设机器人有 $n$ 个自由度，路径 $\mathbf q(s)\in\mathbb R^n$ 至少分段二阶连续。对时间求导并使用链式法则：

\[\dot{\mathbf q}=\mathbf q'(s)\dot s,\] \[\ddot{\mathbf q}=\mathbf q''(s)\dot s^2+\mathbf q'(s)\ddot s.\]

注意两个标量：

• $\dot s$：沿既定路径的速度；
• $\ddot s$：沿既定路径的加速度。

原本 $n$ 维关节运动的时间分配问题，就被压缩到这两个标量。路径的几何信息藏在 $\mathbf q’(s)$ 和 $\mathbf q’‘(s)$ 中。

论文将二阶约束统一写成：

\[\mathbf A(\mathbf q)\ddot{\mathbf q} +\dot{\mathbf q}^{\mathsf T}\mathbf B(\mathbf q)\dot{\mathbf q} +\mathbf f(\mathbf q) \in\mathscr C(\mathbf q),\]

其中 $\mathscr C(\mathbf q)$ 是凸多面体。代入链式法则后，约束变为一维路径变量上的形式：

\[\mathbf a(s)\ddot s+\mathbf b(s)\dot s^2+\mathbf c(s) \in\mathscr C(s). \tag{1}\]

系数为：

\[\mathbf a(s)=\mathbf A(\mathbf q(s))\mathbf q'(s),\] \[\mathbf b(s)=\mathbf A(\mathbf q(s))\mathbf q''(s) +\mathbf q'(s)^{\mathsf T}\mathbf B(\mathbf q(s))\mathbf q'(s),\] \[\mathbf c(s)=\mathbf f(\mathbf q(s)).\]

这不是抽象形式主义。它能覆盖：

• 关节速度与加速度边界；
• 完全驱动机械臂的关节力矩边界；
• 冗余驱动系统的可行力矩集合；
• 线性化摩擦锥下的接触稳定性约束。

例如，机械臂动力学

\[\mathbf M(\mathbf q)\ddot{\mathbf q} +\dot{\mathbf q}^{\mathsf T}\mathbf C(\mathbf q)\dot{\mathbf q} +\mathbf g(\mathbf q)=\boldsymbol\tau\]

在力矩盒约束 $\boldsymbol\tau_{\min}\le\boldsymbol\tau\le\boldsymbol\tau_{\max}$ 下，恰好能直接放入式 (1)。

一阶约束也可以投影。例如速度边界最终成为只包含 $\dot s$ 的可行范围：

\[\mathbf a^v(s)\dot s+\mathbf b^v(s)\in\mathscr C^v(s). \tag{2}\]

3. 为什么状态选用平方速度

TOPP-RA 沿路径取网格：

\[0=s_0<s_1<\cdots<s_N=s_{\mathrm{end}},\]

并在第 $i$ 个网格区间 $[s_i,s_{i+1}]$ 内假设路径加速度为常数：

\[u_i:=\ddot s.\]

若直接把 $\dot s_i$ 当状态，离散动力学会带有平方项。TOPP-RA 改用平方速度：

\[x_i:=\dot s_i^2.\]

因为

\[\frac{d(\dot s^2)}{ds} =\frac{2\dot s\ddot s}{\dot s} =2\ddot s,\]

所以对长度为 $\Delta_i=s_{i+1}-s_i$ 的区间积分得到：

\[x_{i+1}=x_i+2\Delta_i u_i. \tag{3}\]

这一步至关重要：状态转移对 $(x_i,u_i)$ 变成线性的。把式 (1) 在 $s_i$ 处配置（collocation）后也得到线性约束：

\[\mathbf a_i u_i+\mathbf b_i x_i+\mathbf c_i \in\mathscr C_i. \tag{4}\]

于是整个问题不再是非线性最优控制，而是一个一维离散线性系统：

\[\text{state: }x_i,\qquad \text{control: }u_i,\qquad \text{transition: }x_{i+1}=x_i+2\Delta_i u_i.\]

这也是算法名称中 Reachability Analysis 能够成立的原因。

$x_i$ 是平方速度而非速度本身，因此必须有 $x_i\ge0$；最后恢复路径速度时取 $\dot s_i=\sqrt{x_i}$。

4. 每个网格点的可行集合

固定网格点 $s_i$ 后，定义可行的“路径加速度—平方速度”对：

\[\Omega_i= \left\{ (u,x)\mid \mathbf a_i u+\mathbf b_i x+\mathbf c_i\in\mathscr C_i \right\}.\]

如果 $\mathscr C_i$ 是凸多面体，那么 $\Omega_i$ 是 $(u,x)$ 平面上的凸多边形。

由它可定义两个一维集合：

\[\mathcal X_i=\{x\mid\exists u:(u,x)\in\Omega_i\},\]

表示该点允许的平方速度区间；以及

\[\mathcal U_i(x)=\{u\mid(u,x)\in\Omega_i\},\]

表示当前平方速度为 $x$ 时允许采用的加速度区间。

传统 TOPP 数值积分方法往往需要显式构造 $(u,x)$ 平面的边界，例如最大速度曲线与最大/最小加速度场。TOPP-RA 的不同在于：它尽量避免构造整个二维投影，只在需要区间端点时做一维投影，而一维投影就是很小的线性规划。

5. 两个核心概念：可达与可控

可达性分析不是只问“当前能走多快”，而是同时关心“现在这样走，最后还能不能刹到目标速度”。

5.1 可达集：从起点能到哪里

给定起始平方速度集合 $\mathbb I_0$，第 $i$ 个网格点的可达集 $\mathcal L_i(\mathbb I_0)$ 定义为：从某个 $x_0\in\mathbb I_0$ 出发，存在一系列可行控制后能抵达的所有 $x_i$。

一步传播写为：

\[\mathcal R_i(\mathbb I)= \left\{ \tilde x+2\Delta_i u\;\middle|\; \tilde x\in\mathbb I,\, (u,\tilde x)\in\Omega_i \right\} \cap\mathcal X_{i+1}. \tag{5}\]

因为约束是凸的，若 $\mathbb I$ 是区间，$\mathcal R_i(\mathbb I)$ 仍是区间。它的两个端点分别是两个线性规划的最小值与最大值。

5.2 可控集：哪些状态还能到终点

TOPP-RA 实际采用这个对偶概念。给定终点要求 $\mathbb I_N$，第 $i$ 点的可控集 $\mathcal K_i(\mathbb I_N)$ 是所有满足以下条件的 $x_i$：存在一串可行控制，使系统最终到达某个 $x_N\in\mathbb I_N$。

从后往前递推：

\[\mathcal K_N(\mathbb I_N)=\mathbb I_N\cap\mathcal X_N,\] \[\mathcal K_i(\mathbb I_N)=\mathcal Q_i(\mathcal K_{i+1}(\mathbb I_N)), \tag{6}\]

其中一步前驱集为：

\[\mathcal Q_i(\mathbb I)= \left\{ x\in\mathcal X_i\;\middle|\; \exists u\in\mathcal U_i(x): x+2\Delta_i u\in\mathbb I \right\}. \tag{7}\]

计算 $\mathcal Q_i(\mathbb I)$ 的端点同样只需两个 LP：

\[\begin{aligned} \max/\min\quad &x \\ \text{s.t.}\quad& \mathbf a_i u+\mathbf b_i x+\mathbf c_i\in\mathscr C_i,\\ &x+2\Delta_i u\in\mathbb I. \end{aligned} \tag{8}\]

这就是 TOPP-RA 的关键简化：每个网格点只维护一个平方速度区间，而不是搜索高维轨迹空间。

6. TOPP-RA 的两遍算法

给定起末路径速度 $\dot s_0,\dot s_N$，算法分为严格的两遍。

图 2：红色竖线是各网格点的可控平方速度区间 $\mathcal K_i$。后向传播先保证“能刹到终点”；前向传播再选择允许的最大加速度，得到蓝色时间最优状态序列。

6.1 后向遍历：先证明能到终点

终点平方速度已知：

\[\mathcal K_N=\{\dot s_N^2\}.\]

从 $i=N-1$ 递减到 $0$，利用式 (6) 求每个 $\mathcal K_i$。若某个可控集为空，说明从更早的位置不可能满足全部约束并抵达终点；若

\[\dot s_0^2\notin\mathcal K_0,\]

说明给定起始速度本身无法安全到达指定终止速度，算法应报告 infeasible。

6.2 前向遍历：每一步尽可能加速

通过后向检查后，固定：

\[x_0^\ast=\dot s_0^2.\]

从 $i=0$ 到 $N-1$，每一步选择最大的可行路径加速度：

\[\begin{aligned} u_i^\ast=\max_u\quad \text{s.t.}\quad& (u,x_i^\ast)\in\Omega_i,\\ &x_i^\ast+2\Delta_i u\in\mathcal K_{i+1}. \end{aligned} \tag{9}\]

再用式 (3) 更新：

\[x_{i+1}^\ast=x_i^\ast+2\Delta_i u_i^\ast.\]

这里的“贪心取最大 $u$”之所以正确，是因为 $\mathcal K_{i+1}$ 已经编码了未来所有的刹车需求。只要下一状态留在可控集内，就仍保证能到终点；在这个前提下，当前越快越不会增加总时间。论文证明：对于离散化后的问题，该算法只要存在可行参数化就一定返回可行解，并且返回时间最优解。

完整流程可概括为：

flowchart TD
    A[输入：路径 q(s)、约束、起末路径速度] --> B[离散路径并计算 q' 与 q'']
    B --> C[投影约束：得到每点的 a_i、b_i、c_i]
    C --> D[令 K_N = {sdot_N²}]
    D --> E[后向：每点解两个 LP 得到 K_i]
    E --> F{K_0 非空且包含 sdot_0²？}
    F -- 否 --> G[报告不可行]
    F -- 是 --> H[令 x_0 = sdot_0²]
    H --> I[前向：在下一点可控集约束下最大化 u_i]
    I --> J[更新 x_i+1 = x_i + 2Δ_i u_i]
    J --> K{到达终点？}
    K -- 否 --> I
    K -- 是 --> L[恢复 sdot(s) 并构造 q(s(t))]

7. 如何把 ${x_i,u_i}$ 还原为时间轨迹

求得 $x_i=\dot s_i^2$ 后，单个区间的时间来自：

\[dt=\frac{ds}{\dot s}.\]

在 $u_i$ 为常数时，$x(s)$ 随 $s$ 线性变化。对于 $x_i,x_{i+1}>0$：

\[\Delta t_i= \frac{2\Delta_i}{\sqrt{x_i}+\sqrt{x_{i+1}}}. \tag{10}\]

累加所有 $\Delta t_i$ 得到总执行时间，随后可构造 $s(t)$，再代回 $\mathbf q(s(t))$ 获得关节位置、速度和加速度参考。

这里要注意：离散化约束只在网格点配置时，区间内部仍可能有小的约束误差。论文分析表明，基本 collocation 方案的误差阶为 $O(\Delta)$；一阶插值约束可以改善到 $O(\Delta^2)$，代价是更多变量和不等式。因此网格数量不是越少越好：它直接影响约束保守性、轨迹质量与计算时间。

8. 复杂度与鲁棒性来自哪里

设每个网格点有 $m$ 条不等式约束，路径分为 $N$ 段。TOPP-RA 在每一步主要做常数个小 LP，整体复杂度可写为：

\[O(mN).\]

这与将所有网格状态和控制一次性放进大规模凸优化相比更轻量；相较传统数值积分法，它又避开了寻找加速/减速切换点、处理速度边界陷阱点等容易失稳的几何分支。

不过“鲁棒”不代表不需要工程判断：

• 路径 $\mathbf q(s)$ 必须足够光滑，至少能可靠给出 $\mathbf q’$ 与 $\mathbf q’’$；
• 网格过粗会带来明显的区间内部约束误差；
• 接近动力学奇异点时，得到的加速度序列可能抖动，需要更细网格、插值约束或后处理；
• 标准 TOPP-RA 的核心约束是二阶的。jerk 限制不是其原生问题形式，不能把它当作 Ruckig 的直接替代品。

9. 一个最小使用方式

库的典型输入是插值路径和若干约束对象：关节速度、关节加速度、笛卡尔速度、力矩等。求解结果是时间参数化对象，而不是逐控制周期的下一点状态。

import toppra as ta
import toppra.constraint as constraint
import toppra.algorithm as algo

path = ...  # q(s)，通常由样条或路点插值得到
vlim = constraint.JointVelocityConstraint(v_limits)
alim = constraint.JointAccelerationConstraint(a_limits)

instance = algo.TOPPRA([vlim, alim], path)
trajectory = instance.compute_trajectory(0.0, 0.0)

需要注意当前项目 README 已说明 Python 支持将被移除，建议新项目优先采用 C++ 版本及其绑定。无论使用哪种 API，算法层面的输入始终是“固定路径 + 约束 + 起末路径速度”。

总结

TOPP-RA 的推导链条很清楚：

1. 固定几何路径 $\mathbf q(s)$，只优化时间律 $s(t)$；
2. 用链式法则把关节/动力学约束投影到 $(\ddot s,\dot s^2)$；
3. 用平方速度 $x=\dot s^2$ 让离散状态转移线性化；
4. 后向计算可控集，排除所有“当前虽快但未来无法到终点”的状态；
5. 前向在可控集约束下最大化加速度，从而得到时间最优参数化。

它最重要的思想不是某个特定 LP，而是“先把未来可行性压缩为每个位置的一维区间，再放心地在当前加速”。这使它能够在已知路径上同时兼顾速度、加速度、力矩与接触等约束，并保持较好的实现鲁棒性。

参考资料

• Hung Pham, Quang-Cuong Pham. A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis, IEEE Transactions on Robotics, 34(3), 2018.
• hungpham2511/toppra 项目主页与文档。