TOPPRA 轨迹规划学习文档

基于 src/core/motion_plan/src/topp/ 源码的完整解析涵盖算法原理、实现细节、常见疑问解答

一、整体流程#

1
输入：离散路径点 waypoints (关节空间)
2
       ↓
3
  [1] 三次 B 样条插值          → 连续几何路径 q(s), s ∈ [0,1]
4
       ↓
5
  [2] 碰撞/约束检验 + 路径加密  → 保证样条路径无碰撞
6
       ↓
7
  [3] 自适应网格生成            → 非均匀离散化网格 {s_i}
8
       ↓
9
  [4] TOPPRA 时间参数化         → 最优速度剖面 u(s) = σ(s)²
10
       ↓
11
输出：轨迹 {q(t), dq/dt(t), d²q/dt²(t)}，按 dt 采样

二、三次 B 样条插值#

文件： include/motion_plan/topp/cubic_bspline.hpp

2.1 目的#

路径规划输出的是离散路径点，TOPPRA 需要连续可微函数 q(s) 及其一、二阶导数，B 样条正是这个桥梁。

2.2 弦长参数化#

不用均匀参数 s_i = i/(n-1)，而是按段距离分配：

1
s[0] = 0
2
s[i] = s[i-1] + ||q[i] - q[i-1]||₂
3
s /= s[n-1]    →  s ∈ [0, 1]

为什么： 若路径点间距差异大（密集段与稀疏段混杂），均匀参数化会让 dq/ds 在密集段爆炸；弦长参数化保证导数量级均匀。

边界情况：所有点重合时退化到均匀参数。

2.3 Clamped 节点向量#

样条次数 k=3，节点向量长度 n+k+1：

1
knots = [0, 0, 0, 0,  s[1], s[2], ..., s[n-k-1],  1, 1, 1, 1]
2
         ← k+1 个 0 →    ← 内部节点 →              ← k+1 个 1 →

Clamped 设计保证样条精确通过首尾路径点，且端点导数连续。

2.4 预计算基函数系数（Init 核心）#

B 样条定义：

$q(s) = \sum_i B_{i,3}(s) \cdot q_i$

Cox-de Boor 递推：

1
B_{i,0}(s) = 1  if s ∈ [t_i, t_{i+1}),  else 0
2

3
B_{i,k}(s) = (s - t_i)/(t_{i+k} - t_i) · B_{i,k-1}(s)
4
           + (t_{i+k+1} - s)/(t_{i+k+1} - t_{i+1}) · B_{i+1,k-1}(s)

关键性质（局部支撑性）： 在任意区间 [t_j, t_{j+1}) 内，只有 4 个基函数非零。每个基函数在该区间内是 s 的三次多项式：

1
B_{i,3}(s)|_{[t_j, t_{j+1})} = c0 + c1·s + c2·s² + c3·s³

预计算的意义： Init 时一次性将所有基函数在其对应区间上的多项式系数算好，存入 coefs_（每个网格区间对应一个 4×4 矩阵）。

1
coefs_[indx]:  4×4 Matrix
2
  row 0: B_{indx,   3}(s) 在区间 indx 的系数 [c0,c1,c2,c3]
3
  row 1: B_{indx-1, 3}(s) 在区间 indx 的系数
4
  row 2: B_{indx-2, 3}(s) 在区间 indx 的系数
5
  row 3: B_{indx-3, 3}(s) 在区间 indx 的系数

查询时只需：

1
size_t indx = getIndex(t);         // 二分查找 O(log n)
2
for (i = 0; i < 4; i++)
3
    p += CubicValue(t, coefs_[indx].row(i), order) * way_pts_[indx - i];

将每次 eval_single 从 O(递推树) 降为 O(4)。

2.5 求导公式#

1
order=0:  c0 + c1·t + c2·t² + c3·t³        (位置)
2
order=1:  c1 + 2c2·t + 3c3·t²              (一阶导，速度方向)
3
order=2:  2c2 + 6c3·t                      (二阶导，曲率)

2.6 eval_single 的使用方#

调用方	参数	用途
碰撞检验循环	order=0	取路径位置验证碰撞
自适应网格生成	order=2	取二阶导估计曲率
TOPPRA 内部	order=1,2	构建约束矩阵

三、碰撞/约束检验 + 路径加密#

文件： src/topp/topp.cpp，第 186-255 行

3.1 为什么必须做？#

B 样条是插值曲线，经过给定路径点，但在路径点之间的弯曲由样条拟合决定，不保证无碰撞。两个无碰撞的路径点之间，样条可能弯出障碍物区域。

1
q₀ ●────────────────● q₁       ← 路径点本身 OK
2
         ↑
3
    样条弯曲穿过障碍物

3.2 检验点数的计算#

按最坏关节变化量而非均匀数量：

1
对每段 [q[i], q[i+1]]:
2
    max_delta = max_j |q[i+1][j] - q[i][j]|   // 变化最大的关节
3
    segment_samples = ceil(max_delta / 0.033)  // 0.033 rad ≈ 1.9°
4
    total_samples += segment_samples

保证相邻检验点之间每个关节变化不超过 1.9°（工业标准碰撞检测粒度），最终 clamp 到 [2, 500]。

3.3 路径加密循环#

1
while (路径验证失败 AND dense_times < max_dense_times=4):
2
    1. 用 waypoints_aligned 构造 CubicBSpline
3
    2. 在 check_num 个均匀 s 点评估路径位置 q(s_i)
4
    3. 对每个点调用 isStateValid(q)
5
    4. 全部通过 → 成功退出
6
    5. 有失败点 → densePath(waypoints_aligned, 2)  # 插入中点，点数翻倍
7
    6. dense_times++

densePath：在相邻路径点之间均匀插入中间点：

1
dense_path[i*2 + j] = q[i] + (q[i+1] - q[i]) * (j/2),  j=1,2

加密为何有效： B 样条随控制点增多趋向折线（弯曲度减小）。原始折线是无碰撞的，控制点足够密时样条几乎贴着折线走，弯出障碍物的现象消失。

四、自适应网格生成#

文件： src/topp/gridpoint_adaptive.cpp

4.1 为什么不用均匀网格？#

TOPPRA 在每段内用线性约束近似非线性动力学。均匀网格的问题：

高曲率段：线性近似误差大，可能导致约束违反
平直段：过度采样，浪费计算

4.2 Drake 风格自适应细分算法#

初始状态：gridpts = {s₀=0, s₁=1}

主循环（最多 max_iteration=10 轮）：

遍历所有相邻网格点对 [a, b]，判断是否需要在中点 mid = (a+b)/2 插入新点：

1
条件1（段太长）：
2
    if (b - a) > max_seg_length=0.05:
3
        插入中点
4

5
条件2（线性化误差超标）：
6
    pdd = path->eval_single(mid, 2)          // 中点二阶导
7
    curvature = max_j |pdd_j|                // 最大分量绝对值
8
    err = 0.5 · (b-a)² · curvature          // 截断误差估计
9
    if err > max_err_threshold=1e-3:
10
        插入中点

误差估计推导： 在 [a,b] 内用线性插值近似 q'(s)，泰勒展开截断误差为 O(h² · max|q''|)，其中 h = b-a。

每轮若无新增点则提前退出。

保底最少点数：

1
while (num_grid_points < min_nb_points=50):
2
    对每隔一段插入中点（强制增密）

4.3 与均匀网格对比#

指标	均匀 setN(N)	自适应
点分配	等距	按曲率和段长
高曲率段	可能欠采样	自动加密
平直段	浪费点数	稀疏
总点数	固定	动态，保证 ≥50

五、TOPPRA 时间参数化#

文件： src/topp/topp.cpp，第 260-419 行

5.1 变量替换：为什么用 u = σ²？#

设 σ = ds/dt（路径速度），令 u = σ²，a = du/ds（控制输入）。

关节运动学：

1
关节位置：   q = q(s)
2
关节速度：   dq/dt = q'(s) · σ = q'(s) · √u
3
关节加速度： d²q/dt² = q''(s) · u  +  q'(s) · (a/2)
4

5
其中 a/2 = σ̇ 的推导：
6
  u = σ²  →  du/dt = 2σ·σ̇
7
  du/ds = (du/dt)/σ = 2σ̇ = a  →  σ̇ = a/2

5.2 约束线性化#

速度约束：

1
原始：  |dq_j/dt| ≤ v_max_j
2
代入：  |q'_j(s)| · √u ≤ v_max_j
3
平方：  q'_j(s)² · u ≤ v_max_j²     ← 关于 u 是线性的（u ≥ 0）

加速度约束：

1
原始：  |d²q_j/dt²| ≤ a_max_j
2
代入：  |q''_j(s)·u + q'_j(s)·(a/2)| ≤ a_max_j

这是关于 (u, a) 的联合线性不等式。

线性化为何关键：

1
若约束是 σ 的非线性函数  →  NLP（非线性规划）
2
    有局部最优，不保证全局最优，求解慢 O(m²~m³)
3

4
代换 u = σ² 后  →  2 变量 LP（线性规划）
5
    全局最优，Seidel 算法 O(m) 时间
6
    整体复杂度：O(N·m)，N=网格点数，m=约束数

其中 u 是路径级变量（标量），不是每个关节各有一个。σ 是路径速度，所有关节共享同一个 σ，因此对 u 的约束取的是所有关节中最严的一条。

5.3 向后传播：计算可达集#

可达集定义：

1
K_i = { u_i ≥ 0 : 从 (s_i, u_i) 出发，存在可行轨迹到达终点 s_N }

关键结论：K_i 始终是一个区间 [lo_i, hi_i]（约束线性，可行域凸集，投影到 u_i 轴仍是区间）。

初始化（终点条件）：

1
K_N = {0}     （终点要求停止，σ=0 即 u=0）

向后传播一步（K_{i+1} → K_i）：

1
Step 1：加速度约束给出 u_{i+1} 关于 u_i 的可达范围
2
  -a_max_j ≤ q''_j·u_i + q'_j·(a/2) ≤ a_max_j
3
  令 a = (u_{i+1} - u_i)/Δs，解出：
4
    u_{i+1} ∈ [F_lo(u_i), F_hi(u_i)]   （u_i 的线性函数）
5

6
Step 2：u_{i+1} 必须落入 K_{i+1}
7
  非空条件：  F_lo(u_i) ≤ hi_{i+1}
8
             F_hi(u_i) ≥ lo_{i+1}
9
  （均是 u_i 的线性不等式）
10

11
Step 3：速度约束限制 u_i 上界
12
  u_i ≤ (v_max_j / |q'_j(s_i)|)²  对所有 j
13
  取最严：u_i ≤ min_j (v_max_j / |q'_j(s_i)|)²
14

15
Step 4：取交集
16
  K_i = [lo_i, hi_i]

5.4 向后传播有解的证明#

命题： 在只有速度上限和加速度上限约束的条件下，0 ∈ K_i 对所有 i 成立，即向后传播不产生空集。

前提： v_max_j > 0，a_max_j ≥ 0，0 ∈ K_N

归纳证明：

1
基础情形：0 ∈ K_N = {0}  ✓
2

3
归纳步骤：假设 0 ∈ K_{i+1}，取 u_i = 0, u_{i+1} = 0, a = 0：
4

5
  速度约束：|q'_j(s_i)| · √0 = 0 ≤ v_max_j  ✓
6
  加速度约束：q''_j·0 + q'_j·(0/2) = 0 ≤ a_max_j  ✓
7
  u_{i+1}=0 ∈ K_{i+1}（归纳假设）  ✓
8

9
  → 0 ∈ K_i  ✓
10

11
由归纳原理：∀i, 0 ∈ K_i，K_i 非空

物理含义： “全程以无穷慢的速度爬行（u≡0）“是平凡可行解，只要允许这个极端情况，可行集永不为空。

实际中 TOPPRA 失败的真正原因： 不是真正的几何不可行，而是：

浮点病态： u=0 是 LP 边界点，舍入误差可能让求解器误判
路径导数退化： q'_j(s) ≈ 0 导致约束矩阵系数接近 0/0（代码注释直接引用 toppra GitHub issues #15, #17）

5.5 向前传播：贪心最小时间#

1
u_0* = hi_0                              （起点取可行范围最大值）
2
u_{i+1}* = min(hi_{i+1}, F_hi(u_i*))   （在 K_{i+1} 内尽量快）

物理含义：T = ∫(1/√u)ds，u 越大时间越短，贪心取最大 u 是全局最优（LP 凸性保证）。

起点不一定取 v_max，而是取 hi_0（已被所有约束限制过的最大可行值）。

5.6 无解在哪个阶段发生？#

1
向后传播阶段：
2
  若某 K_i = ∅  →  立即失败，返回错误
3
  此时尚未进行向前传播
4

5
向前传播阶段：
6
  所有 K_i 已确认非空
7
  贪心策略在已知可行集内选值
8
  → 贪心永远不失败
9

10
结论：无解只在向后传播阶段发生，与贪心策略无关。

5.7 ConstAccel 参数化#

TOPPRA 求得 {u_i*} 后，用 ConstAccel 从路径参数域恢复时间域：

在每段 [s_i, s_{i+1}] 内假设 σ̇ 恒定：

1
σ(t) = σ_i* + a_i · (t - t_i)   (线性)
2
s(t) 是 t 的二次函数              (抛物线)

按 output_dt 采样输出 {q(t), dq/dt(t), d²q/dt²(t)}。

5.8 求解器策略#

1
主求解器：Seidel
2
  专为 TOPPRA 的 2D LP 子问题设计
3
  每步 O(m)，m=约束数（关节数×2）
4
  对病态约束（约束面近乎平行）数值不稳定
5

6
Fallback：qpOASES
7
  有效集法 QP 求解器，更鲁棒
8
  代价：比 Seidel 慢约 5-10 倍
9

10
代码逻辑：Seidel 失败 → 自动切换 qpOASES

5.9 结果验证与 Fallback#

1
TOPPRA 求解成功后：
2

3
  total_duration = NaN 或 ≤ 0?   → 物理估计时长 + 线性插值
4
  total_duration < 0.2s?         → 数值异常，同上
5
  轨迹含 NaN/Inf?                → 线性插值兜底
6
  否则                           → 按 output_dt 正常采样
7

8
物理时长估计（estimateReasonableDuration）：
9
  短路径（来不及加速到巡航速度）→ 三角剖面：T = 2√(L/a_max)
10
  长路径                        → 梯形剖面：T = 2·t_accel + L_cruise/v_avg

六、配置参数汇总#

碰撞检验#

参数	值	含义
`CHECK_DELTA_Q_MAX`	0.033 rad	相邻检验点最大关节变化量
`CHECK_NUM_MAX`	500	检验点数上限
`CHECK_NUM_MIN`	2	检验点数下限
`max_dense_times`	4	最大路径加密次数

自适应网格#

参数	值	含义
`max_err_threshold`	1e-3	线性化误差上限
`max_seg_length`	0.05	段长上限（路径参数）
`min_nb_points`	50	最少网格点数
`max_iteration`	10	最大细分轮数

TOPPRA#

参数	值	含义
`MIN_DURATION_THRESHOLD`	0.2s	最短合理轨迹时长
`EXTREMELY_SHORT_PATH_THRESHOLD`	1e-8	路径长度下限

七、错误处理流程#

1
路径点数 < 2           → 直接失败
2

3
路径长度 < 1e-8        → 直接失败（防 NaN）
4

5
碰撞检验
6
  加密次数超限         → ITERP_FAILED
7

8
TOPPRA 求解
9
  Seidel 失败          → 切换 qpOASES
10
  qpOASES 也失败       → TOPP_FAILED
11

12
结果验证
13
  时长异常或过短       → 使用物理估计时长
14
  含 NaN/Inf           → 退回线性插值

八、关键问答#

Q1：u 对所有关节的约束为何取 min_j？#

u = σ² 是路径级标量，所有关节共享同一个 σ = ds/dt。u 只有一个值，但必须同时满足所有关节的速度限制，因此取最严的那个关节的约束（即 min_j）。这不是”最保守关节限制所有人”，而是”同一个 u 必须让每个关节都达标”。

Q2：B 样条预计算的系数是什么？#

Cox-de Boor 递推在查询时是递归树，开销大。Init 一次性将每段上 4 个非零基函数的多项式展开系数 [c0,c1,c2,c3] 算好存入矩阵。查询时只做 4 次多项式求值，从 O(递推树) 降到 O(4)。

Q3：路径加密的意义？#

输入路径点本身是无碰撞的，但 B 样条在路径点之间的弯曲可能穿越障碍物。加密路径点使样条越来越贴近原始折线（B 样条的凸包性质），从而消除弯曲引起的碰撞。本质是用更多控制点抑制样条的”弯曲自由度”。

Q4：TOPPRA 约束为何对 u 是线性的？#

速度约束平方后变为 q'²·u ≤ v_max²（u 的线性式）；加速度约束代入 a = du/ds 后变为 q''·u + q'·(a/2) 的上下界（(u,a) 的联合线性式）。线性化的价值：每步子问题从 NLP 退化为 2D LP，Seidel 算法 O(m) 解决，整体 O(N·m)。

Q5：向后传播有解的证明#

取平凡解 u_i = 0（全程静止）：速度约束变为 0 ≤ v_max（恒成立），加速度约束变为 0 ≤ a_max（恒成立），则由归纳法可证 0 ∈ K_i 对所有 i 成立，可行集永不为空。实际失败只来自浮点病态，不是真正的几何不可行。

Q6：刹车距离不足是真实失败吗？#

理论上不是。若全程 u ≈ 0，则所需制动量 a = Δu/Δs ≈ 0，永远不会超限。“刹车距离不足”只在离散化网格过粗时作为数值问题出现，而非物理上不可行。

Q7：向后/向前传播各自的职责？#

向后传播：计算每点的可行区间 K_i = [lo_i, hi_i]，回答”能不能走”
向前传播：在可行集内贪心取最大 u，回答”怎么走最快”
无解发生在向后传播，与贪心策略无关；若向后传播通过，向前传播必然成功

Q8：a = du/ds 吗？#

是的。在 TOPPRA 的参数化中，u = σ² 是状态，a = du/ds 是控制输入（u 对路径参数的导数），它直接决定 u_{i+1} = u_i + a·Δs，即”从当前速度平方跳到下一个速度平方”。加速度约束 |q''·u + q'·a/2| ≤ a_max 就是对 (u, a) 的线性限制。