前面几篇讲了空间路径的平滑（最小化Jerk、B样条）。今天讲一个经常被忽视但极其重要的问题：时间参数化——给定一条空间路径，怎么分配时间让机器人跑得快又不超速。

这个问题听起来简单，实际上很有讲究。一条路径在曲率大的地方必须减速（否则离心力超标），在直线段可以加速。怎么在满足速度、加速度、力矩等多重约束的前提下，让执行时间最短？这就是TOPP（Time-Optimal Path Parameterization）要解决的问题。

一、TOPP问题的定义

给定一条空间路径p(s)，其中s是弧长参数（从0到L），找时间参数化函数s(t)，使得：

• 总时间T最小
• 速度约束：|dp/dt| ≤ v_max
• 加速度约束：|d²p/dt²| ≤ a_max
• 力矩约束（机械臂场景）：|τ| ≤ τ_max

# TOPP问题的核心
# 输入: 路径p(s), s∈[0, L]
# 输出: 时间函数s(t), t∈[0, T]
# 优化: minimize T
# 约束: 速度、加速度、力矩限制
# 关键变量: s'(t) = ds/dt (路径速度)

注意s'(t)和v(t)的区别：s'(t)是路径参数的变化速率，v(t)是实际速度。两者通过路径的导数关联：v(t) = |p'(s)| · s'(t)。如果路径参数化不均匀（比如弧长参数化），|p'(s)| = 1，则v(t) = s'(t)。

二、TOPP的经典解法——相平面法

TOPP最早由Bobrow、Dubowsky和Pfeiffer在1985年提出，用相平面法求解。相平面的两个轴是s（路径位置）和s'（路径速度）。

在相平面上，每个点(s, s')对应路径上某个位置的速度。约束条件在相平面上画出一个可行区域——速度上限是一条水平线，加速度约束是一条曲线。最优解是在可行区域的边界上走——尽可能快地走但不违反约束。

最优速度曲线（也叫Maximum Velocity Curve, MVC）是相平面上可行区域的上边界。时间最优的s'(s)曲线紧贴MVC——在曲率大的地方速度低（接近MVC下边界），在直线段速度高（接近MVC上边界）。

Bang-Bang结构：最优解的加速度是bang-bang的——要么最大加速，要么最大减速。这和控制论中的Pontryagin极大值原理一致。

三、TOPP-RA：考虑奇异点和鲁棒性

经典TOPP有个问题：在某些路径位置（奇异点），加速度约束可能不连续，导致数值求解不稳定。Pham和Phung在2013年提出了TOPP-RA（Robust Algorithm），解决了这个问题。

TOPP-RA的核心改进：

• 用二分法处理奇异点，保证数值稳定性
• 支持任意线性约束（不只是速度和加速度）
• 算法复杂度O(N·M)，N是离散点数，M是约束数量

# TOPP-RA的简化流程
def topp_ra(path, constraints, num_points=100):
    # 1. 路径离散化
    s_values = linspace(0, path_length, num_points)
    # 2. 计算每个离散点的约束上界
    for s in s_values:
        max_speed[s] = compute_speed_limit(path, s, constraints)
    # 3. 前向扫描: 从起点开始，按最大加速度递增速度
    # 4. 后向扫描: 从终点开始，按最大减速度递增速度
    # 5. 取前向和后向的逐点最小值
    velocity_profile = min(forward_pass, backward_pass)
    return velocity_profile

前向扫描和后向扫描的组合是TOPP的精髓——前向扫描保证速度不超过加速能力允许的上限，后向扫描保证速度不超过减速能力允许的上限。两者取最小值就是最优速度曲线。

四、工程实现与开源工具

TOPP-RA有多个开源实现：

toppra（Python）：Pham本人维护的库，支持机械臂的力矩约束和关节速度/加速度约束。安装简单，pip install toppra。

OMPL的TOPP模块：集成在OMPL中，适合和RRT等路径规划器配合使用。

Tesseract的TOTG：Time-Optimal Trajectory Generation，ROS2生态中常用的TOPP实现。

工程中的简化方法——S曲线速度规划：如果路径是简单的直线段，不需要TOPP这么复杂的方法。工业上常用S曲线速度规划：速度从0平滑加速到v_max，匀速段，再平滑减速到0。S曲线本质上是加速度也连续的梯形速度规划——用jerk限制让加速度变化平滑。S曲线规划计算量几乎为零（微秒级），在直线运动场景中完全够用。TOPP的优势在于处理复杂路径（有弯道、有曲率变化）的场景。

实际使用时的注意事项：

• 路径离散化精度影响结果质量。通常每1-5cm一个离散点——太粗可能漏掉约束最紧的点。
• TOPP只优化时间参数化，不改变路径形状。如果路径本身不好（曲率太大），TOPP也没办法让速度高起来。
• TOPP输出的速度曲线可能有加速度不连续点（bang-bang切换处）。如果需要平滑的加速度，用TOPP结果做初始化，再用最小化Jerk做平滑。

五、面试实战

Q：TOPP和轨迹优化有什么区别？ A：TOPP不改变路径形状，只优化路径上的速度分配。轨迹优化同时改变路径形状和时间分配。TOPP更轻量，适合路径已经确定的场景（比如轨道机器人、固定路线的AGV）。

Q：TOPP的前向扫描和后向扫描具体怎么做？ A：前向扫描从起点开始，用最大允许加速度逐步递增速度，但不超过当前位置的速度上限。后向扫描从终点开始，用最大允许减速度反向递增速度。两者逐点取最小值。直觉上，前向扫描处理"加速能力限制"，后向扫描处理"减速能力限制"。

Q：TOPP怎么处理机械臂的力矩约束？ A：机械臂的动力学方程τ = M(q)q'' + C(q,q')q' + g(q)把关节力矩和关节加速度/速度关联起来。TOPP把这个方程代入加速度约束中——在每个路径位置，根据当前位姿q和路径曲率计算最大允许的路径加速度。力矩约束变成了相平面上的一条曲线。

Q：你在项目中用过TOPP吗？ A：用过。做AGV的轨迹规划时，路径由A*生成，用TOPP-RA做时间参数化。速度上限1.5m/s，加速度上限0.8m/s²。TOPP算出来的时间比匀速走完全程短30%——因为在直线段加速到1.5m/s，在弯道减速到0.5m/s。计算时间<5ms。

Q：TOPP和速度规划（Speed Profile）是一回事吗？ A：本质上是一回事——都是在路径上分配速度。但TOPP特指时间最优的速度规划（最小化总时间），速度规划更广义——可能考虑舒适度、能耗等其他目标。自动驾驶领域通常说"speed profile planning"，机器人领域说"TOPP"或"time parameterization"，其实解决的是同一类问题。

Q：TOPP能处理多个约束同时起作用的情况吗？ A：能。TOPP-RA支持任意数量的线性约束——在每个路径位置上，所有约束取交集得到可行域。如果某个位置上力矩约束和速度约束同时起作用，取最紧的那个。这就是TOPP-RA比经典TOPP强的地方——经典TOPP只处理两个约束（速度和加速度），TOPP-RA能处理任意多个。

小结

TOPP解决"给定路径，怎么分配时间最快"的问题。最优解是bang-bang结构，用前向扫描+后向扫描的相平面法求解。TOPP-RA是鲁棒的数值实现，有成熟的开源库。

TOPP在机器人领域应用广泛——机械臂的点到点运动、AGV的路线执行、自动驾驶的速度规划都用得到。面试时能讲清楚前向/后向扫描的原理和bang-bang结构就够了。

下一篇讲机械臂运动规划——关节空间和笛卡尔空间的规划策略。

---

如果这篇文章对你有帮助，欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。

「机器人软件开发面试·从入门到精通」连载系列 

上一篇：第228篇 B样条曲线——平滑轨迹的参数化表示

下一篇预告：第230篇 机械臂运动规划——关节空间和笛卡尔空间的规划策略

有任何问题欢迎评论区留言，我会尽量回复。