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

从路径投影、平方速度离散化到后向可控集和前向贪心提取，推导 TOPP-RA 如何在运动学和动力学约束下求最快时间律。

2026年8月10日机器人,运动规划3分钟阅读

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

假设上层规划器已经给出一条几何路径 $\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）推导算法。它讨论的是给定几何路径后的时间参数化，而不是路径搜索或在线状态到状态轨迹生成。

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 离线或准在线给出时间律，底层再做跟踪或局部平滑。

设机器人有 $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}$

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}$ 。

固定网格点 $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 的不同在于：它尽量避免构造整个二维投影，只在需要区间端点时做一维投影，而一维投影就是很小的线性规划。

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

给定起始平方速度集合 $\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)$ 仍是区间。它的两个端点分别是两个线性规划的最小值与最大值。

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 的关键简化：每个网格点只维护一个平方速度区间，而不是搜索高维轨迹空间。

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

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

终点平方速度已知：

$\mathcal K_N={\dot s_N^2}.$

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

$\dot s_0^2\notin\mathcal K_0,$

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

通过后向检查后，固定：

$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))]

求得 $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)$ ，代价是更多变量和不等式。因此网格数量不是越少越好：它直接影响约束保守性、轨迹质量与计算时间。

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

$O(mN).$

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

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

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

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

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 的推导链条很清楚：

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

它最重要的思想不是某个特定 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 项目主页与文档。

评论