Home / 机器人经典基础 / TOPP-RA：先算哪里还能刹住，再决定哪里可以加速

093｜TOPP-RA：先算哪里还能刹住，再决定哪里可以加速

论文：Hung Pham、Quang-Cuong Pham，A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis，2017 年预印本，2018 年期刊发表。原文 v2 · 官方代码。

先接住前作的问题

PRM或GPMP2可以提供几何路径，但路径存在并不回答何时加速和制动；TOPP-RA 固定路径，把未来可制动条件反向传播。与Cheetah MPC相比，这是时间参数化和在线接触力规划两种平行任务，二者不能互相替代动力学与接触假设。

五个前置概念

五个前置概念：

几何路径：只指定构型顺序，不指定时刻。
时间参数化：给路径进度分配速度和时间。
可控集：仍能合法达到终端目标的一组状态。
线性规划：在线性约束下求线性目标的极值。
离散误差：有限网格近似与连续约束之间的差异。

一、问题设定：固定几何路径怎样安排仍能在终点停住的速度

机械臂已经规划好一条绕开障碍的路径，却不能沿路径每处都开到最高速度。前方弯曲较大、某个关节加速度受限，或终点要求停住，都可能迫使它提前减速。

问题定义：路径与轨迹有什么不同

路径 $q(s)$ 描述机器人经过哪些关节构型， $s$ 只是沿路径的进度。轨迹 $q(s(t))$ 才说明每个时刻到达哪里。

本篇假设路径具有所需的分段光滑性，时间进度单调向前，并给定起止路径速度。输入是路径和运动约束；输出是速度、加速度及时间安排。不存在策略网络、奖励训练或在线环境交互预算。

原有数值积分方法需要处理加减速曲线与切换点，遇到特殊约束结构时可能不稳健。整体凸优化又可能引入较大的优化问题。作者利用一个更简单的结构：沿固定路径投影后，很多动力学约束可以用两个主要变量表达。

二、以速度平方线性化约束，反向求可控集

关键变换：为什么要用速度的平方

对路径连续求导：

$\dot q=q'(s)\dot s,\qquad \ddot q=q''(s)\dot s^2+q'(s)\ddot s.$

定义 $x=\dot s^2$ 、 $u=\ddot s$ ，第二项变成关于 $x,u$ 的仿射表达。将力矩、加速度等约束代入后，可以整理为：

$a_i u_i+b_i x_i+c_i\in\mathscr C_i.$

对于文中多面体约束，以上条件可以转成线性不等式。 $u$ 是路径加速度，不是单个关节力矩； $x$ 是路径速度平方，也不是机器人空间位置。

在路径网格 $s_i$ 到 $s_{i+1}$ 内取常路径加速度，有：

$x_{i+1}=x_i+2\Delta_i u_i,\qquad \Delta_i=s_{i+1}-s_i.$

这是高中运动学 $v_1^2=v_0^2+2a\Delta s$ 的同一结构。复杂关节系统沿固定路线被压缩成一个有约束的一维进度系统，这才是后续集合运算变得简洁的原因。

数学目标是最小化走完路径的时间 $T=\int ds/\sqrt{x(s)}$ 。算法机制是集合递推加小型优化；实验指标则包括计算时间、返回可行解的比例、运动时间和约束违反误差。这几项不能混称为“速度提升”。

可达集与可控集：过去和未来的两种问题

可达集问：从给定起点出发，哪些速度能合法到达这里？可控集问：从这里以什么速度出发，还能合法到达指定终点？它们描述的方向不同。

TOPP-RA 的主算法使用后者。设终点必须停下，则 $K_N=\{0\}$ 。反向递推：

$K_i=\{x\ge0:\exists u,\ (u,x)\in\Omega_i, \ x+2\Delta_i u\in K_{i+1}\}.$

$\Omega_i$ 表示当前所有局部约束。凸性使这些一维集合可用区间表示，求上下界可归结为线性规划。

局部合法速度不一定属于 $K_i$ 。一辆车在当前位置没有超速，但前方离停车点太近，仍可能无法满足终点停车条件。反向递推把这类未来要求提前带回当前位置。

算法：有后路的贪心

这里的“最大化加速度”包含负数：如果必须制动，允许的最大 $u$ 也可能是负的。它不是一路猛踩油门，而是从仍可到达终点的动作中选择最快的推进。

对常加速度段，若端点路径速度之和非零，时间可按 $\Delta t_i=2\Delta_i/(\sqrt{x_i}+\sqrt{x_{i+1}})$ 恢复。两端均零且段长非零时，不能直接用该公式得到有限运动；离散表示与网格需要能够表达实际加减速过程。这是恢复轨迹时容易被忽略的细节。

4.1 理论究竟保证到哪一层

第 IV-B 节定理 1针对离散实例：存在解时返回可行参数化，否则判为不可行。连续路径上的约束仍需要误差分析。

第 IV-C 节区分无零惯性点与存在这类点的情况，并讨论网格细化时的最优性。附录说明贪心证明依赖最大状态转移的单调结构；不能省略条件，写成“任意网格、任意约束都全局最优”。零惯性点是某项路径投影约束的加速度系数为零，不是机器人真实质量变为零。

基础形式在每段做少量小型 LP；论文的 $O(mN)$ 复杂度讨论采用其给定的求解成本模型，不能提升为所有线性规划实现的无条件最坏情况保证。

4.2 两端都合法，中间为什么仍可能超限

TOPP-RA-intp 在每个区间的两端都检查约束，比只检查左端更强，但仍不是对连续区间逐点证明。令某一个标量约束的违反函数为 $\epsilon(s)$ ，约定 $\epsilon\le0$ 合法；对长度 $h$ 的区间 $0\le s\le h$ ，若 $|\epsilon''(s)|\le M$ 且两端非正，线性插值余项给出

$\epsilon(s)\le\frac{M}{2}s(h-s)\le\frac{Mh^2}{8}.$

$M$ 是这段约束沿路径的二阶导数界。端点约束没有把右端变成零，只把最大可能违反压到二阶小量。若拥有可信的 $M$ 并在端点保留 $Mh^2/8$ 裕量，可以进一步构造保守保证；这句话是由余项得到的教学分析，不是说论文默认实现已自动求出所有这样的界。

原创反例。 取速度平方 $x(s)=2$ 、路径加速度 $u=0$ ，连续速度上界为 $x(s)\le2-4s(h-s)$ ，并令 $0<h\le1$ 。两端都是 $2\le2$ ，完全通过检查；中点上界却为 $2-h^2$ ，违反恰为 $h^2$ 。这里 $\epsilon(s)=4s(h-s)$ 、 $M=8$ ，正好达到上述界。网格缩半后违反从 $h^2$ 变为 $h^2/4$ ，但在有限网格下仍非零。

原文附录D与网格实验区分配点的一阶误差与插值的二阶误差。读实验时因此应同时看计算耗时、运动时间和连续约束误差，不能仅凭离散 LP 返回成功就宣布实体执行必然满足全部限制。

反向可控集先留下能到达终点的速度，前向选择才有安全的贪心范围。下面用手算例子检查两遍顺序，再区分网格与连续保证。

三、在有后路的速度范围内前向选择

输入：固定路径 q(s)，路径网格，运动约束，起止速度 v0/vN
构造各网格的允许状态—控制集合 Omega[i]
K[N] = {vN^2} 与终点允许速度集合的交集
从 i=N-1 到 0：
    求所有能够进入 K[i+1] 的非负速度平方 x
    用两个线性规划得到区间 K[i] 的下界与上界
若 v0^2 不在 K[0]：返回当前离散问题不可行
x[0] = v0^2
从 i=0 到 N-1：
    最大化 u，约束 (u,x[i]) 属于 Omega[i]
    同时要求 x[i] + 2*Delta[i]*u 属于 K[i+1]
    保存 u 与 x[i+1]
恢复每段时间及 q(s(t))；检查连续轨迹的约束误差

一个手算例子

自构例子，未运行：路径长 2，网格为 $0,1,2$ ，路径加速度限制 $-1\le u\le1$ ，起止速度均为零。

从最后一段倒推， $x_2=0=x_1+2u_1$ ，因此要能停车，必须 $0\le x_1\le2$ 。从起点向前选最快可行输入 $u_0=1$ ，得到 $x_1=2$ ；最后选 $u_1=-1$ ，回到 $x_2=0$ 。峰值速度是 $\sqrt2$ ，不是 2，因为内部状态存的是速度平方。

若去掉“下一状态必须在可控集内”，算法就可能持续加速，直到终点才发现停不下来。这正是反向阶段不可省略的原因。

四、对照可行性、时间和有限网格误差

Fig. 3：横轴增加约束不等式数量，纵轴是每网格点的平均求解时间，不含设置与解提取 — 原论文结果图／表摘引：Hung Pham、Quang-Cuong Pham（2017），Fig. 3，PDF 第 7 页；核对原文。原版权保留。

读图：横轴增加约束不等式数量，纵轴是每网格点的平均求解时间，不含设置与解提取。实线与线性、二次拟合虚线说明该实现的扩展性，不能直接替代整个时间参数化管线的总耗时。

实验如何验证

离散网格点满足限制不等于网格间全部满足；参数化图与计时表还需保留插值处理、网格大小和计时阶段。

实验	数据与规模	基线与预算	指标和问题
随机路径与约束数量	随机路点经样条插值，改变自由度与约束数；固定网格	TOPP-NI 与 TOPP-RA，同机求解	约束更多时耗时和数值失败如何变化
网格影响	多种网格规模；每个规模 100 个随机实例	增加 TOPP-RA-intp 插值变体	成功率、运动时间与连续约束误差
多接触腿式模型	50 自由度模型，双脚位于不平齐台阶，给定摆动路径	含接触力和力矩限制；比较投影加 TOPP-NI	高维约束投影是否成为瓶颈
计算平台	第 V 节给出 Intel i7-4770、8 GB 内存、Python 和 qpOASES	未训练策略	算法与实现的计算开销

随机路径实验特意生成可行问题，因此求解失败衡量的是数值求解可靠性。它不代表随机现实任务都有可行路径，更不是机器人执行成功率。

多接触实验是轨迹计算案例，不应改写为真实机器人完成大量试验。本文未把硬件执行、安全余量或模型失配统计补进原文。

五组证据怎样读

图 2。 两算法同一路径的速度与加速度；算出的时间安排接近；不能据此推出：所有问题精确相同。

原论文图2：TOPP-RA与TOPP-NI的关节速度、加速度和路径速度六面板 — Hung Pham、Quang-Cuong Pham（2017），图 2，PDF 第 7 页；原文与图注。原版权保留。

读图：A、B、C列依次展示不同运动量；比较红色限位与曲线，而不是只比较完成时间。

图 3、4。 约束数增加后的成本与成功率；该实现中计算扩展性和稳健性优势；不能据此推出：任何小问题都更快。

原论文图4：约束不等式数变化时TOPP-RA与TOPP-NI的成功率 — Hung Pham、Quang-Cuong Pham（2017），图 4，PDF 第 7 页；原文与图注。原版权保留。

读图：横轴增加约束数量；接近100%的柱形仅覆盖图中这组测试实例与实现。

表 I。 准备、核心求解、提取轨迹分开计时；总成本优势来源不只核心求解；不能据此推出：将总加速比称为 LP 求解加速比。

图 5。 网格及插值变体；网格越细、插值更严可改善约束误差；不能据此推出：网格点合法即处处合法。

原论文图5：网格大小对成功率、约束误差及运动代价差的三个面板 — Hung Pham、Quang-Cuong Pham（2017），图 5，PDF 第 7 页；原文与图注。原版权保留。

读图：三个面板依次报告成功率、相对约束误差、相对近似最优运动代价的差；后两幅采用对数纵轴，代价差不是计算耗时。

图 6、表 II。 多接触约束与投影开销；避免完整多面体投影能节省时间；不能据此推出：几何路径与接触序列也被共同优化。

原论文图6：腿式机器人接触受力约束下的动作快照、路径速度和接触力 — Hung Pham、Quang-Cuong Pham（2017），图 6，PDF 第 9 页；原文与图注。原版权保留。

读图：A列把时刻对应到姿态，B列比较路径速度与接触力；接触约束使可行域随路径变化，接触力不是关节力矩。

表 I 的示例中，总时间为 29.8 ms 对 455.3 ms，但核心求解是 26.1 ms 对 28.3 ms。自行相除得到约 15.3 倍的总流程耗时比；不能说核心求解快了 15.3 倍。

表 II 的多接触例中，TOPP-RA 总时间 451.6 ms，比较管线 4497.8 ms；其中后者的多面体投影约 3671.8 ms。若只引用 TOPP-RA 的 267 ms，应注明不含动力学量计算等环节。

图 5 的“最优参考”通过很细网格的 TOPP-RA-intp 近似得到，不是独立解析真值。更严的插值约束可能产生稍长运动时间，却更少违反连续约束；不能把这个时间增加简单判为算法退步。

五、区分离散最优、连续安全与路径选择

“顺便得到力”不代表任意平滑、能耗最小的力分配免费得到。若另加二次目标优化力，目标与问题已发生变化，应检查对时间最优性的影响。

作者报告插值变体约束误差更好但更慢，基础形式在某些动力学奇异位置附近仍有抖动。本文的进一步分析是：实际执行还需要考虑跟踪器误差、摩擦不确定性、离散控制以及可能的 jerk 限制。二阶约束时间参数化不是通用的 jerk 受限规划器。

固定路径本身若绕了远路，本方法也不会另找捷径。路径规划、时间参数化和反馈控制是串联的不同问题；解决中间一层，不能宣称整个任务全局最优。

六、追问：终点速度要求改变但路径不变，为什么不能只修改最后一个速度点？

终点速度要求改变但路径不变，为什么不能只修改最后一个速度点？

参考答案：末端要求会改变可控集，并向前限制哪些速度还有合法的后续减速方案。应重新进行相应反向传播，再前向选择。局部修改末端数值可能破坏中间加速度或力矩约束，路径相同不意味着时间安排可直接复用。

链式法则给出 $\dot q=q_s\dot s$ 、 $\ddot q=q_s\ddot s+q_{ss}\dot s^2$ 。因此前方曲率项 $q_{ss}$ 能在当前速度尚可行时，限制后续的可行速度范围。这个解释沿Modern Robotics时间缩放推导，本文新增的可控集递推仍以原论文为准。

七、区分路径定时与在线接触力规划

平行路线：Cheetah 凸 MPC：用简化模型提前安排四足机器人的支撑力。在线滚动接触力规划不固定同一条路径，问题和可行性条件不同。

八、自测与参考答案

问题一：为什么前向阶段看起来贪心，却不会忽略终点停车？

答案：因为每次选择都要求下一速度属于预先反向计算的可控集。这个集合已经编码后续所有阶段与终点速度限制。贪心只发生在仍保留合法后续方案的动作中；省掉反向阶段就失去了这一性质。

问题二：所有网格点都满足力矩界，能否认为整条连续轨迹满足？

答案：不能。路径导数、动力学系数和速度在网格之间变化，只检查离散位置可能漏掉中间峰值。论文专门分析离散误差并提供插值变体；实际使用还需细化网格、连续验证或采用适当的保守裕量。

问题三：为什么表 I 不能证明 TOPP-RA 的核心求解比 TOPP-NI 快十五倍？

答案：约十五倍来自准备、核心求解和轨迹提取的总时间比。表中的核心求解时间接近，较大差异来自其他环节。完整流程优势有实际价值，但必须按对应计时范围命名，不能把它归到单个算法内核。