本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

M10 时间参数化——从几何路径到可执行轨迹的最后一公里¶

本章定位：OMPL 和 TrajOpt 输出的是**几何路径**——一串关节位置序列，没有时间信息。你不能直接把这串路径点发给关节控制器——控制器需要知道"每个时刻应该在哪里、以多快的速度、以多大的加速度"。时间参数化就是给几何路径附加**速度曲线**的过程，是从规划到执行的最后一公里。本章深入讲解三大时间参数化工具——Ruckig（在线 OTG）、TOPP-RA（离线时间最优）、TOTG（MoveIt2 默认）——的算法原理、C++ 工程设计和工业应用场景。

共享属性：⚪ 部分共享。Ruckig 也用于腿足 SEA 实时轨迹生成，但在机械臂领域是**主战场**。TOPP-RA 和 TOTG 主要服务机械臂的"几何路径 → 可执行轨迹"后处理。

前置依赖：M07/M08（运动规划——理解规划输出是几何路径）、v8 Ch12-13（模板基础——Ruckig 的模板设计）、M11（实时 C++，部分——Ruckig 的 RT-safe 特性）

下游章节：M12（ros2_control 硬件接口）、M14（MoveIt2 系统集成）

建议用时：1 周（理论 2 天 + Ruckig 精读 3 天 + TOPP-RA/TOTG 对比 2 天）

前置自测 ⭐¶

📋 答不出 >= 2 题 → 先回前置章节复习

编号	问题	答不出时回顾
1	几何路径 vs 轨迹：路径是 $\sigma(s)$（参数 $s \in [0,1]$），轨迹是 $q(t)$（参数 $t \in [0,T]$）。两者的本质区别是什么？	M07/M08 运动规划
2	加加速度（Jerk）：jerk 是加速度的时间导数 $j = d^3q/dt^3$。为什么工业机械臂控制中要限制 jerk？（提示：振动、电机电流突变、减速器磨损）	控制理论基础
3	C++ 模板编译期 vs 运行时：`Ruckig<7>` 和 `Ruckig<DynamicDOFs>` 的区别是什么？为什么编译期固定维度能避免堆分配？	v8 Ch12-13 模板
4	MoveIt2 规划管线：MoveIt2 的规划请求从 `PlanningScene` 到 `PlanningPipeline` 到 `PlanRequestAdapter` 的数据流是什么？时间参数化在哪个阶段介入？	M14 MoveIt2
5	实时控制约束：1 kHz 控制循环意味着每个周期只有 1 ms 时间预算。哪些操作可以在控制循环中做？哪些不可以？	M11 实时 C++

本章目标¶

学完本章后，你应该能够：

解释时间参数化问题的数学定义——给定几何路径和运动学/动力学约束，求时间最优速度曲线
区分 Ruckig（在线 OTG）、TOPP-RA（离线时间最优）、TOTG（离线迭代样条）的定位差异，根据应用场景做出正确选择
精读 Ruckig 的编译期 DOF 模板设计和零依赖架构，理解其如何实现 < 1 us 在线轨迹生成
集成 Ruckig 到 C++ 项目中，实现在线目标切换场景下的平滑轨迹生成
配置 MoveIt2 中的时间参数化适配器，在 TOTG 和 Ruckig Filter 之间切换
理解时间最优路径参数化（TOPP/TOPP-RA）的理论基础——Bobrow-Slotine-Shin 经典方法和 Pham 的可达集分析

1. 时间参数化问题——规划与执行之间的鸿沟 ⭐¶

1.1 动机：为什么规划器不直接输出轨迹¶

考虑一个典型的 pick-and-place 场景：

Step 1: 用户给定目标位姿 T_goal
Step 2: MoveIt2 调用 OMPL 规划器 (RRT-Connect / PRM)
Step 3: 规划器输出一条从 q_start 到 q_goal 的路径
Step 4: ??? → 把路径发给关节控制器
Step 5: 机械臂运动到目标

Step 3 到 Step 4 之间有一个鸿沟——OMPL 输出的是什么？

OMPL 输出 (几何路径):
  q_0 = [0.1, 0.3, -0.5, 1.2, 0.0, -0.7, 0.5]    ← 路径点 0
  q_1 = [0.2, 0.5, -0.3, 1.0, 0.1, -0.5, 0.3]    ← 路径点 1
  q_2 = [0.4, 0.7, -0.1, 0.8, 0.2, -0.3, 0.1]    ← 路径点 2
  ...                                              ← 没有时间!
  q_N = [1.0, 1.2, 0.5, 0.0, 0.8, 0.2, -0.3]     ← 路径点 N

  问题: 路径点之间隔多久? 关节速度是多少? 加速度呢?

关节控制器需要什么？

控制器输入 (时间轨迹):
  t = 0.000s: q = [...], dq = [...], ddq = [...]
  t = 0.001s: q = [...], dq = [...], ddq = [...]   ← 每 1ms 一个点
  t = 0.002s: q = [...], dq = [...], ddq = [...]
  ...

控制器需要**连续时间轨迹** $q(t)$，不仅有位置，还有速度 $\dot{q}(t)$ 和加速度 $\ddot{q}(t)$。时间参数化就是这个"从路径点到时间轨迹"的转换过程。

跨领域类比：几何路径之于时间轨迹，就像地图上的行驶路线之于导航仪的实时指令。地图路线告诉你"先走 A 路再走 B 路"（空间信息），但导航仪需要告诉你"现在加速到 60km/h、保持 3 秒、然后减速到 30km/h 准备转弯"（时空信息）。路径是空间的，轨迹是时空的。

1.2 数学定义¶

形式化表达：

给定： - 几何路径 $\sigma(s)$，$s \in [0, 1]$，描述关节从 $q_{start}$ 到 $q_{goal}$ 的空间轨迹 - 约束：每个关节的速度限 $\dot{q}_{max}$、加速度限 $\ddot{q}_{max}$、（可选）jerk 限 $\dddot{q}_{max}$ - （可选）力矩限：$\tau_{max}$（需要逆动力学计算）

求： - 时间函数 $s(t)$，$t \in [0, T]$，使总执行时间 $T$ 最短，且所有约束满足

关键关系：

\[q(t) = \sigma(s(t))$$ $$\dot{q}(t) = \sigma'(s) \cdot \dot{s}(t)$$ $$\ddot{q}(t) = \sigma''(s) \cdot \dot{s}^2(t) + \sigma'(s) \cdot \ddot{s}(t)\]

其中 $\sigma'(s) = d\sigma/ds$ 是路径的几何导数，$\dot{s}(t) = ds/dt$ 是沿路径的速度。时间参数化的核心就是求 $s(t)$——或等价地，求速度曲线 $\dot{s}(s)$。

1.3 时间最优性的数学刻画¶

优化问题的完整形式：

\[\min_{s(t)} T = \int_0^T dt = \int_0^1 \frac{ds}{\dot{s}(s)}\]

后一个等式来自 $dt = ds/\dot{s}$。目标是最小化总时间 $T$——等价于最大化沿路径的速度 $\dot{s}(s)$。

为什么这是一个非平凡的优化问题？ 因为约束不仅限制 $\dot{s}$（速度），还限制 $\ddot{s}$（加速度）。高 $\dot{s}$ 需要在之前更大的 $\ddot{s}$ 来加速，但 $\ddot{s}$ 本身受限。速度和加速度约束的耦合使问题无法逐点独立求解——必须全局考虑。

变量替换的关键技巧：定义 $x(s) = \dot{s}^2(s)$，则 $\ddot{s} = \frac{1}{2}\frac{dx}{ds}$。优化问题变为：

\[\min \int_0^1 \frac{ds}{\sqrt{x(s)}}, \quad \text{s.t. 约束关于 } (x, \frac{dx}{ds}) \text{ 线性}\]

这个变换让约束在离散网格上可以写成关于 $(x, u)$ 的线性不等式，进而用一系列小规模 LP 做可达集递推——这正是 TOPP-RA 的数学基础。

本质洞察：时间参数化问题的深层结构是：路径已固定（$\sigma(s)$ 给定），仅优化速度分配（$\dot{s}(s)$）。这把原本的高维轨迹优化问题（$q(t) \in \mathbb{R}^n$ 对所有 $t$）降维为一维函数优化问题（$\dot{s}(s) \in \mathbb{R}$ 对所有 $s$）。降维的代价是路径不变——如果路径本身不好（如绕了远路），时间参数化无法弥补。好的轨迹 = 好的路径 + 好的时间参数化。

1.4 三种工具解决三种变体¶

工具	输入	输出	约束	定位
Ruckig	当前(pos, vel, acc) + 目标(pos, vel, acc)	时间最优 jerk-limited 轨迹	vel/acc/jerk per joint	在线 OTG（< 1 us）
TOPP-RA	几何路径（路径点序列）	时间最优速度曲线	vel/acc/torque per joint	离线后处理
TOTG	几何路径（路径点序列）	迭代样条速度曲线	vel/acc per joint	离线后处理（MoveIt2 默认）

关键区分： - Ruckig：不需要预先计算的路径——给**当前状态**和**目标状态**，直接生成轨迹。适合**实时控制**（visual servoing、碰撞避让、在线目标切换） - TOPP-RA / TOTG：需要**完整的几何路径**（OMPL/TrajOpt 输出），做后处理。适合**离线规划后的执行**

本质洞察：Ruckig 和 TOPP-RA/TOTG 解决的是**不同的问题**。Ruckig 解决的是"从当前状态到目标状态的最优过渡"（没有预定义路径，轨迹形状由约束决定），TOPP-RA/TOTG 解决的是"沿着预定义路径的最优速度分配"（路径形状固定，只优化速度曲线）。这不是"哪个更好"的问题，而是"你的应用场景是什么"的问题。

1.5 为什么不直接用线性插值¶

最简单的"时间参数化"：在路径点之间做线性插值，每两个点匀速运动。

为什么这不行？

问题	后果
速度不连续	路径点处速度瞬间跳变 → 需要无穷大加速度 → 电机无法执行
加速度不连续	梯形速度曲线切换点处加速度跳变 → jerk 无穷大 → 机械振动
未利用关节极限	所有关节用同一速度 → 最慢关节决定整体速度 → 执行时间远大于最优
未考虑动力学	某些构型力矩需求更大（如水平伸展），但匀速不区分 → 可能超力矩限

反事实推理：如果你在工业焊接场景中使用线性插值（速度不连续），焊枪在路径点处会因速度跳变而产生焊接缺陷——焊缝在减速点堆积、在加速点稀薄。时间参数化不是"锦上添花"，而是工业应用的硬性要求。

⚠️ 常见陷阱¶

💡 概念误区：认为"时间最优"就是"越快越好"

新手想法："时间参数化的目标就是让机械臂尽可能快地完成运动"

实际上：工业应用中，"时间最优"往往要附加额外约束——焊接需要恒定末端速度（速度均匀性比时间最优更重要）、涂胶需要匀速曲线（否则胶量不均）、人机协作需要限速（ISO 10218 安全标准）。"最优"目标函数应根据应用场景定制

正确思维：时间参数化是约束优化问题，"时间最优"只是最常见的目标函数之一

⚠️ 编程陷阱：混淆路径参数 $s$ 和时间参数 $t$

错误做法：把 OMPL 路径点的序号当作时间

现象：路径点密集处运动慢、稀疏处运动快

根本原因：OMPL 的采样策略是空间均匀的，不是时间均匀的。路径点密集通常意味着空间中有障碍物需要绕行

正确做法：先沿路径做弧长参数化（使 $s$ 与弧长成正比），再做时间参数化

练习¶

⭐ 对一条 3 个路径点的 2-DOF 路径，分别画出线性插值、梯形速度曲线、S 形速度曲线的 $q(t)$、$\dot{q}(t)$、$\ddot{q}(t)$、$\dddot{q}(t)$ 曲线。标注哪些不满足 jerk 约束。
⭐⭐ 解释为什么 OMPL 的 RRT-Connect 不直接输出时间轨迹。如果 RRT 在树扩展时就考虑动力学约束，算法会变成什么？（提示：Kinodynamic RRT）
⭐⭐ 列举至少 3 个工业场景，"时间最优"不是正确的优化目标。说明每个场景的正确目标函数。

2. 时间最优路径参数化的理论基础——Bobrow-Slotine-Shin 方法 ⭐⭐⭐¶

2.1 经典理论的历史脉络¶

时间最优路径参数化（TOPP）问题的理论基础由三组独立的研究者在 1985-1986 年几乎同时建立：

Bobrow, Dubowsky, Gibson (1985)：最早提出相空间（$s$-$\dot{s}$ 平面）分析方法
Shin, McKay (1985, 1986)：建立了时间最优参数化的充要条件
Slotine, Yang (1989)：给出了切换点（switching points）的完整分类

他们的核心洞察是相同的：时间最优的速度曲线 $\dot{s}(s)$ 总是在约束边界上运行——要么是某个关节的速度/加速度/力矩达到极限，要么是在两个极限之间"切换"。

2.2 相空间分析¶

核心变换：将轨迹 $q(t)$ 用路径参数 $s$ 重新表达：

\[q(t) = \sigma(s(t))$$ $$\dot{q} = \sigma'(s) \dot{s}$$ $$\ddot{q} = \sigma''(s) \dot{s}^2 + \sigma'(s) \ddot{s}\]

代入力矩约束 $\tau_{min} \leq \tau \leq \tau_{max}$（其中 $\tau = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q)$）：

\[\tau_{min} \leq M(\sigma(s))\sigma'(s)\ddot{s} + \left[M(\sigma(s))\sigma''(s) + C(\sigma(s), \sigma'(s))\sigma'(s)\right]\dot{s}^2 + g(\sigma(s)) \leq \tau_{max}\]

这是关于 $(\dot{s}^2, \ddot{s})$ 的**线性不等式**。对于每个 $s$ 值，它定义了 $(\dot{s}, \ddot{s})$ 的可行域。

相空间表示：在 $(s, \dot{s})$ 平面上，每一点对应一个路径位置和速度。约束定义了每个 $s$ 处 $\dot{s}$ 的上界 $\dot{s}_{max}(s)$——这条曲线就是**最大速度曲线（Maximum Velocity Curve, MVC）**。

    ds/dt (速度)
    |
    |      /\      MVC (最大速度曲线)
    |     /  \    /\
    |    /    \  /  \
    |   / 时间最优轨迹  \
    |  /                 \
    | /                   \
    |/___________________\_______ s (路径位置)
    0                     1

时间最优轨迹在相空间中的特征：从 $(s=0, \dot{s}=0)$ 到 $(s=1, \dot{s}=0)$（起止速度为零），最大化 $\dot{s}$——即尽可能贴近 MVC 运行。

2.3 切换点与 bang-bang 控制¶

时间最优轨迹由三种段组成：

段类型	加速度	物理含义
最大加速段	$\ddot{s} = \ddot{s}_{max}(s, \dot{s})$	某个关节达到最大加速力矩
最大减速段	$\ddot{s} = \ddot{s}_{min}(s, \dot{s})$	某个关节达到最大减速力矩
极限弧段	沿 MVC 滑动	某个关节恰好在速度极限上

这是经典最优控制理论中的 bang-bang 控制——最优控制总是在约束边界上"开关"切换。

跨领域类比：时间最优路径参数化的 bang-bang 特性类似于赛车驾驶——赛车手在弯道入口全力刹车、弯道内以极限速度过弯、弯道出口全力加速。没有"半油门"——要么全力加速，要么全力刹车。这正是 Pontryagin 最大值原理的工程体现。

Pontryagin 最大值原理的直觉证明：

为什么时间最优控制一定是 bang-bang 的？假设在某个时刻控制量（加速度）不在约束边界上：$\ddot{s}_{min} < \ddot{s} < \ddot{s}_{max}$。这意味着还有"余量"——我们可以增大 $\ddot{s}$（加速更快）而不违反约束。更快的加速意味着到达同一速度所需时间更短，总执行时间减少。因此，最优解必须让控制量时刻在边界上——要么全力加速，要么全力减速。

切换点分类（Slotine 1989）：

类型	特征	物理含义
零惯量点（Zero-inertia）	$\sigma'_j(s) = 0$——路径切线与某关节轴正交	该关节的惯量在此方向为零，力矩对加速度的影响突变
切换点（Switch point）	$\ddot{s}_{max}$ 从正变负或反之	限制加速度的关节从一个切换到另一个
MVC 接触点	轨迹触碰速度上界	速度达极限，需减速避免超限

零惯量点是经典 TOPP 的最大数值挑战——在此点附近 MVC 的梯度趋近无穷，积分器步长必须极小。TOPP-RA 的离散化 LP 方法天然避免了这个问题。

2.4 Worked Example：2-DOF 系统的完整相空间分析 ⭐⭐⭐¶

理论不做 worked example 就是空谈。 下面用一个完整的 2-DOF 例子，手工走一遍相空间分析的全过程。

问题设定：

\[\text{路径: } \sigma(s) = \begin{bmatrix} s \\ \sin(\pi s) \end{bmatrix}, \quad s \in [0, 1]\]

这是一条从 $(0, 0)$ 到 $(1, 0)$ 的弓形路径——关节 1 线性增长，关节 2 先升后降。

Step 1: 计算路径导数

\[\sigma'(s) = \begin{bmatrix} 1 \\ \pi \cos(\pi s) \end{bmatrix}, \quad \sigma''(s) = \begin{bmatrix} 0 \\ -\pi^2 \sin(\pi s) \end{bmatrix}\]

Step 2: 代入关节速度/加速度表达式

\[\dot{q}_1 = 1 \cdot \dot{s}, \quad \dot{q}_2 = \pi\cos(\pi s) \cdot \dot{s}\]

\[\ddot{q}_1 = 1 \cdot \ddot{s}, \quad \ddot{q}_2 = -\pi^2\sin(\pi s) \cdot \dot{s}^2 + \pi\cos(\pi s) \cdot \ddot{s}\]

Step 3: 从速度约束推导 MVC

设速度限 $|\dot{q}_i| \leq v_{max}$：

关节 1：$|\dot{s}| \leq v_{max}$ → 常数上界
关节 2：$|\pi\cos(\pi s) \cdot \dot{s}| \leq v_{max}$ → $\dot{s} \leq \frac{v_{max}}{|\pi\cos(\pi s)|}$

注意：在 $s = 0.5$ 处，$\cos(\pi \cdot 0.5) = 0$，关节 2 的速度约束**消失**——此处 MVC 由关节 1 单独决定。这在物理上合理：路径中点处弓形曲线的切线方向与关节 2 轴正交，关节 2 的瞬时速度为零，无论 $\dot{s}$ 多大。

\[\dot{s}_{max}(s) = \min\left(v_{max}, \frac{v_{max}}{|\pi\cos(\pi s)|}\right)\]

在路径两端 $s \to 0$ 和 $s \to 1$，$\cos(\pi s) \to \pm 1$，上界为 $v_{max}/\pi$；在 $s = 0.5$，上界为 $v_{max}$。MVC 形状为"中间高、两端低"。

Step 4: 从加速度约束推导可行加速度范围

设加速度限 $|\ddot{q}_i| \leq a_{max}$：

\[|\ddot{s}| \leq a_{max} \quad \text{(关节 1)}\]

\[|-\pi^2\sin(\pi s) \dot{s}^2 + \pi\cos(\pi s) \ddot{s}| \leq a_{max} \quad \text{(关节 2)}\]

不要直接除以 $\cos(\pi s)$ 后写成一个固定方向的不等式，因为 $\cos(\pi s)$ 会变号，并且在 $s=0.5$ 处为零。更稳妥的写法是把每个关节都写成关于 $u=\ddot{s}$ 和 $x=\dot{s}^2$ 的线性约束：

\[-a_{max} \leq a_i(s)u + b_i(s)x \leq a_{max}\]

其中关节 1 有 $a_1=1, b_1=0$；关节 2 有：

\[a_2(s)=\pi\cos(\pi s), \quad b_2(s)=-\pi^2\sin(\pi s)\]

当 $|a_i(s)| > \epsilon$ 时，该关节给出一段 $u$ 的上下界：

\[\ell_i=\min\left(\frac{-a_{max}-b_i x}{a_i}, \frac{a_{max}-b_i x}{a_i}\right), \quad u_i=\max\left(\frac{-a_{max}-b_i x}{a_i}, \frac{a_{max}-b_i x}{a_i}\right)\]

所有关节合并为：

\[u_{min}(s,x)=\max_i \ell_i(s,x), \quad u_{max}(s,x)=\min_i u_i(s,x)\]

当 $|a_i(s)| \leq \epsilon$ 时，该关节此刻不再约束 $\ddot{s}$，但仍要求 $|b_i(s)x| \leq a_{max}$；否则当前 $(s,\dot{s})$ 本身就不可行。比如 $s=0.5$ 时，关节 2 的 $\ddot{s}$ 系数为零，但曲率项 $-\pi^2\dot{s}^2$ 仍会限制允许的路径速度。

Step 5: 构造时间最优轨迹

从 $(s=0, \dot{s}=0)$ 出发，以最大 $\ddot{s}$ 加速（贴着可行域上边界），直到接近 MVC 或需要减速以在 $s=1$ 处停下。从 $(s=1, \dot{s}=0)$ 反向以最大减速度积分，两条曲线的交点就是切换点。

    ds/dt
    |
    |     ___
    |    / . \         MVC
    |   /  .  \
    |  /   .   \       ← 切换点
    | / acc→.←dec\
    |/______._____\_____ s
    0      0.5     1

本质洞察：整个相空间分析的核心就是把**多关节耦合约束**化归为 $(s, \dot{s})$ 二维平面上的几何问题。无论机器人有多少个关节，相空间始终是二维的——这就是路径参数化的降维威力。多 DOF 系统的约束只是 MVC 曲线形状更复杂，但分析框架完全相同。

2.5 为什么经典方法在实际中不够用¶

Bobrow-Slotine-Shin 方法理论优美，但有几个实际问题：

问题	影响
数值稳定性差	MVC 上可能有奇异点，积分器在奇异点附近发散
不支持 jerk 约束	经典理论只处理位置/速度/加速度/力矩约束
切换点检测困难	需要精确定位加速/减速切换时刻，数值上易出错
约束耦合不灵活	难以同时处理运动学约束和力矩约束的耦合

这些问题催生了 TOPP-RA（2018）——用可达集分析替代切换点检测，数值上更稳定。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为经典 TOPP 方法已经过时

新手想法："TOPP-RA 已经取代了经典方法，直接学 TOPP-RA 就行"

实际上：TOPP-RA 的数学基础仍然是相空间分析（$s$-$\dot{s}$ 平面）。不理解相空间和 MVC，就无法理解 TOPP-RA 为什么在每个路径点求解 LP——LP 的约束正是从相空间分析推导出来的。经典理论提供了理解 TOPP-RA 的概念框架

练习¶

⭐⭐ 对 1-DOF 系统 $\ddot{q} = u$，$|u| \leq 1$，路径 $\sigma(s) = s$，手动画出 $(s, \dot{s})$ 相空间中的 MVC。从 $(0, 0)$ 到 $(1, 0)$ 画出时间最优轨迹。
⭐⭐⭐ 对 2-DOF 系统，路径 $\sigma(s) = [s, s^2]^T$，约束 $|\dot{q}_i| \leq 1$、$|\ddot{q}_i| \leq 2$，推导 $\dot{s}_{max}(s)$。画出 MVC。
⭐⭐⭐ 解释为什么 bang-bang 控制是时间最优的。（提示：Pontryagin 最大值原理——如果加速度不在极限上，说明还有余量可以更快。）

3. TOPP-RA——时间最优路径参数化的现代方法 ⭐⭐¶

3.1 从经典 TOPP 到 TOPP-RA¶

TOPP-RA（Time-Optimal Path Parameterization based on Reachability Analysis）由 Hung Pham 和 Quang-Cuong Pham 于 2018 年提出（IEEE T-RO），是对经典 Bobrow 方法的根本性改进。

核心创新：用**可达集分析**替代切换点检测。不再试图寻找加速/减速的切换时刻，而是对每个路径点，直接计算"在满足所有约束的前提下，下一个路径点的 $\dot{s}$ 能达到什么范围"。

算法流程：

TOPP-RA 两遍扫描:

Pass 1: 反向扫描 (s = s_N → s_0)
  从终点 (s_N, ds/dt = 0) 出发
  对每个路径点 s_i, 求解一个小规模 LP:
    "给定 s_{i+1} 处 ds/dt 的可行范围,
     当前 s_i 处 ds/dt 最大能是多少?"
  得到每个 s_i 处的 ds/dt 上界

Pass 2: 正向提取 (s = s_0 → s_N)
  从起点 (s_0, ds/dt = 0) 出发
  对每个路径点, 选择可行的最大 ds/dt (贪心策略)
  得到时间最优的 ds/dt(s) 曲线

后处理: 从 ds/dt(s) 积分得到 s(t), 进而得到 q(t)

每个路径点的 LP：

在路径点 $s_i$ 处，运动学/动力学约束变成关于 $u_i = \ddot{s}_i$ 的线性不等式。这是一个关于 $(u_i, x_{i+1})$ 的 LP（线性规划）——回顾 M05：LP 是 QP 的特例（目标函数线性），有极高效的求解算法。

3.2 可达集分析的数学细节 ⭐⭐⭐¶

TOPP-RA 的核心数学机制是**可达集（Reachability Set）** 的递推计算。下面详细推导。

离散化：将路径 $s \in [0, 1]$ 等分为 $N$ 段：$0 = s_0 < s_1 < \cdots < s_N = 1$。定义 $x_i = \dot{s}_i^2$（用 $\dot{s}^2$ 而非 $\dot{s}$ 是关键——使约束线性化）。

为什么用 $x = \dot{s}^2$？ 回顾加速度表达式 $\ddot{q} = \sigma'' \dot{s}^2 + \sigma' \ddot{s}$。如果用 $(\dot{s}, \ddot{s})$ 作变量，力矩约束关于 $\dot{s}^2$ 和 $\ddot{s}$ 分别线性——但关于 $\dot{s}$ 是二次的。定义 $x = \dot{s}^2$ 后，约束变为 $x$ 和 $u = \ddot{s}$ 的**线性函数**，完美适配 LP 框架。

状态转移：假设段 $[s_i, s_{i+1}]$ 内 $\ddot{s}$ 恒定（零阶保持），则：

\[x_{i+1} = x_i + 2(s_{i+1} - s_i) u_i\]

约束线性化：在路径点 $s_i$ 处，运动学/动力学约束可统一写为：

\[\mathbf{a}(s_i) u_i + \mathbf{b}(s_i) x_i + \mathbf{c}(s_i) \leq 0\]

其中系数向量 $\mathbf{a}, \mathbf{b}, \mathbf{c}$ 由路径几何 $\sigma', \sigma''$ 和机器人动力学参数决定。

具体来说：

速度约束 $|\dot{q}_j| \leq v_j^{max}$ 化为 $|\sigma'_j(s)| \sqrt{x} \leq v_j^{max}$，即 $\sigma'_j(s)^2 x \leq (v_j^{max})^2$——关于 $x$ 线性
加速度约束 $|\ddot{q}_j| \leq a_j^{max}$ 化为 $|\sigma''_j x + \sigma'_j u| \leq a_j^{max}$——关于 $(x, u)$ 线性
力矩约束 $\tau_{min} \leq M\sigma' u + [M\sigma'' + C(\sigma,\sigma')\sigma']x + g(\sigma) \leq \tau_{max}$——关于 $(x, u)$ 线性，其中重力项是常数项，不随 $x$ 缩放

反向递推 LP：从 $i = N-1$ 到 $i = 0$，每一步求解：

\[\text{maximize } x_i$$ $$\text{subject to: } x_i \geq 0$$ $$\quad\quad\quad\quad x_{i+1} = x_i + 2\Delta s_i \cdot u_i$$ $$\quad\quad\quad\quad x_{i+1} \in [0, \bar{x}_{i+1}] \quad \text{(下一步的可行范围)}$$ $$\quad\quad\quad\quad \mathbf{a}(s_i) u_i + \mathbf{b}(s_i) x_i + \mathbf{c}(s_i) \leq 0\]

这是一个关于 $(x_i, u_i)$ 的 LP——两个变量，几十个约束（每个关节贡献 2-4 个不等式）。LP 的求解时间与关节数线性相关。

正向提取：从 $x_0 = 0$ 出发，贪心选择满足约束的最大 $u_i$（最大加速），得到 $x_1, x_2, \ldots$。

算法复杂度：$O(N \cdot m)$，$N$ 为路径点数，$m$ 为约束数。对典型 7-DOF 机械臂，$N = 100$ 个路径点，约束数 $m \approx 28$（每关节 4 个），总计 $\sim 2800$ 次 LP 迭代——毫秒级完成。

反事实推理：如果不用可达集分析而用经典切换点方法，需要对常微分方程做前向/后向积分，在 MVC 的奇异点（$\sigma'_j = 0$ 导致约束退化的点）附近积分器步长急剧缩小，甚至发散。TOPP-RA 的离散化 LP 框架天然避免了连续积分的数值问题——每个路径点独立求解 LP，不存在积分累积误差。

带条件的数学保证：在路径已离散化、路径足够光滑、约束在网格上可行，并且每个 LP 都被数值求解器成功求解的前提下，TOPP-RA 的反向扫描穷尽离散网格上的可达状态，正向提取保证在可达集内选择。因此它保证的是这个离散化问题的可行解和时间最优速度曲线；连续路径上的约束满足仍取决于网格密度、插值方式和数值容差。

3.3 TOPP-RA 的 C++ 和 Python 实现¶

GitHub: hungpham2511/toppra | Stars ~700 | License: MIT

Python 示例（推荐入门，概念骨架）：

TOPPRA 的 Python API 在版本间对约束数组形状、插值器构造、solver wrapper 参数有差异。下面展示数据流：路径点 → 样条路径 → 速度/加速度约束 → TOPPRA 求解；实际代码以目标版本文档和仓库示例为准。

import toppra as ta
import numpy as np

# 路径点 (N 个点, 每个 7-DOF)
way_pts = np.array([
    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    [0.5, 0.3, -0.2, 0.8, 0.1, -0.3, 0.2],
    [1.0, 0.6, -0.4, 0.4, 0.3, -0.6, 0.4],
])
ss = np.linspace(0, 1, len(way_pts))

# 构建路径 (三次样条插值)
path = ta.SplineInterpolator(ss, way_pts)

# 运动学约束。部分 TOPPRA 版本接受正上限向量，部分版本要求
# [[lower, upper], ...] 形式；按目标版本确认。
vmax = np.array([2.175, 2.175, 2.175, 2.175, 2.61, 2.61, 2.61])
amax = np.array([15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0])
vlim = np.column_stack((-vmax, vmax))
alim = np.column_stack((-amax, amax))

pc_vel = ta.constraint.JointVelocityConstraint(vlim)
pc_acc = ta.constraint.JointAccelerationConstraint(alim)

# 求解
instance = ta.algorithm.TOPPRA([pc_vel, pc_acc], path)
trajectory = instance.compute_trajectory()

# 采样时间轨迹
ts = np.linspace(0, trajectory.duration, 1000)
qs = trajectory(ts)        # 位置
qds = trajectory(ts, 1)    # 速度
qdds = trajectory(ts, 2)   # 加速度

3.4 TOPP-RA 的 Python 高级用法——力矩约束 ⭐⭐⭐¶

TOPP-RA 的独特优势是支持**力矩约束**——通过逆动力学计算每个构型下的力矩需求。

力矩约束的推导：

将 $\ddot{q} = \sigma''(s)\dot{s}^2 + \sigma'(s)\ddot{s}$ 代入运动方程 $\tau = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q)$：

因为 $\dot{q}=\sigma'(s)\dot{s}$，速度相关项满足 $C(q,\dot{q})\dot{q}=C(\sigma(s),\sigma'(s))\sigma'(s)\dot{s}^2$。于是：

\[\tau = M(\sigma(s))\sigma'(s)\ddot{s} + \left[M(\sigma(s))\sigma''(s) + C(\sigma(s),\sigma'(s))\sigma'(s)\right]\dot{s}^2 + g(\sigma(s))\]

整理为关于 $(x = \dot{s}^2, u = \ddot{s})$ 的形式：

\[\tau = \underbrace{[M\sigma' ]}_{=:\mathbf{a}(s)} u + \underbrace{[M\sigma'' + C\sigma']}_{=:\mathbf{b}(s)} x + \underbrace{g(s)}_{=:\mathbf{c}(s)}\]

力矩约束 $\tau_{min} \leq \tau \leq \tau_{max}$ 变为：

\[\tau_{min} \leq \mathbf{a}(s) u + \mathbf{b}(s) x + \mathbf{c}(s) \leq \tau_{max}\]

这是关于 $(u, x)$ 的**线性不等式**——完美符合 LP 框架。

带力矩约束的实现骨架（概念代码）：

TOPPRA 自定义二阶约束的类名、返回数组形状和注册方式在 Python/C++ 版本间差异较大；下面只保留数学接口：对每个网格点计算 $\mathbf{a}(s), \mathbf{b}(s), \mathbf{c}(s)$，再按目标版本 API 封装成 SecondOrderConstraint 或等价约束。

import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("panda.urdf")
data = model.createData()
tau_max = np.array([87, 87, 87, 87, 12, 12, 12])
tau_min = -tau_max

def torque_terms_for_toppra(path, gridpoints):
    A, B, C = [], [], []
    for s in gridpoints:
        q = path(s)        # sigma(s)
        qs = path(s, 1)    # sigma'(s)
        qss = path(s, 2)   # sigma''(s)

        # M(q)
        M = pin.crba(model, data, q).copy()

        # g(q)
        g = pin.computeGeneralizedGravity(model, data, q).copy()

        # C(q, sigma') sigma'：令路径速度标量 dot{s}=1，路径加速度 ddot{s}=0。
        tau_unit_speed = pin.rnea(model, data, q, qs, np.zeros(model.nv))
        coriolis_path = tau_unit_speed - g

        A.append(M @ qs)
        B.append(M @ qss + coriolis_path)
        C.append(g)

    return np.asarray(A), np.asarray(B), np.asarray(C)

# 目标约束形式：
#   tau_min <= A_i u + B_i x + C_i <= tau_max
# 其中 x = dot{s}^2, u = ddot{s}。
# 下一步按所用 TOPPRA 版本的 SecondOrderConstraint 接口封装，
# 再与 pc_vel、pc_acc 一起传入 ta.algorithm.TOPPRA。

何时需要力矩约束？

场景	是否需要	原因
轻载高速运动	❌ 通常不需要	运动学约束（速度/加速度限）先饱和
重载/水平伸展	✅	重力力矩在某些构型下接近电机极限
协作机器人	✅	力矩限低（安全要求），经常是瓶颈
高动态运动	✅	惯量力矩在高加速度时可能超限

反事实推理：如果在 Franka Panda（力矩限较低的协作机器人）上只用运动学约束（Ruckig/TOTG），不考虑力矩约束，会怎样？机器人可能在水平伸展姿态下尝试高加速度运动，力矩需求超过电机极限，触发驱动器硬件保护（立即停机 + 报错 communication_constraints_violation）。TOPP-RA 的力矩约束可以在规划阶段就避免这种情况。

3.5 TOPP-RA 的数值稳定性保证 ⭐⭐⭐¶

TOPP-RA 相比经典 TOPP 的另一个关键优势是**数值稳定性的理论保证**。

经典 TOPP 的数值问题：

经典方法通过对 ODE $\frac{d\dot{s}}{ds} = \frac{\ddot{s}}{\dot{s}}$ 做前向/后向积分来构造速度曲线。问题出在 $\dot{s} \to 0$ 时——分母趋近零，ODE 变为**奇异形式**。虽然通过 L'Hopital 规则可以消除奇异性，但数值积分器在此附近的步长必须极小，计算时间激增且结果不可靠。

TOPP-RA 如何避免？

TOPP-RA 用 $x = \dot{s}^2$ 而非 $\dot{s}$ 作为状态变量。当 $\dot{s} \to 0$ 时，$x \to 0$——这只是一个普通的零值，不是奇异点。LP 在 $x = 0$ 处的求解与 $x > 0$ 处完全相同，没有数值退化。

TOPP-RA 的可达性证明（Pham & Pham 2018, Theorem 2）：

定理表述（针对离散化后的 TOPP-RA 问题）：如果路径在网格上可行、路径导数足够光滑、离散约束正确表达，并且 LP 数值求解成功，则 TOPP-RA 的反向递推会找到非空可达集，正向提取会生成该离散问题上的时间最优轨迹。

证明思路（简述）：

反向递推从终点 $x_N = 0$ 出发，在每一步通过 LP 计算可达的 $x_i$ 范围 $[x_i^{min}, x_i^{max}]$
如果在某步 $x_i^{max} < 0$，说明无法从 $s_i$ 到达终点——但这与"路径可行"的前提矛盾
因此 $x_i^{max} \geq 0$ 对所有 $i$ 成立——可达集非空
正向提取在可达集内选择最大 $x$ 即为时间最优（因为 $\dot{s}$ 越大，通过每段所需时间越短）

"不是X而是Y"句式：TOPP-RA 的可靠性不是经验观测，而是建立在可达集递推上的**条件性数学保证**。它不是"测了很多案例都成功"，而是"在离散路径可行、网格和约束表达足够准确、LP 求解成功时，能找到离散问题的最优解"。这是 TOPP-RA 相比经典 TOPP（可能因切换点和积分数值问题错过可行解）的本质优势。

3.6 TOPP-RA 算法总结：与 Bellman 动态规划的联系¶

TOPP-RA 的反向递推本质上是 **Bellman 动态规划**的特例：

\[V(s_i) = \max_{u_i} \{ x_i \mid x_{i+1} \in \mathcal{R}(s_{i+1}), \text{约束满足} \}\]

其中 $\mathcal{R}(s_{i+1})$ 是下一步的可达集。从终点开始反向计算每一步的可达集——这就是**后向归纳法**。

回顾 M05 中的 QP/LP 基础：TOPP-RA 的每步 LP 变量只有 2 个（$(x_i, u_i)$），约束数等于约束类型数 $\times$ 关节数。对 7-DOF 机械臂，典型约束数为 28-42（每关节 4-6 个不等式）。这是一个极小规模的 LP——用单纯形法或内点法都是微秒级。

3.7 与 MoveIt2 的集成¶

TOPP-RA 是外部库（hungpham2511/toppra），并非 MoveIt2 的标准适配器。如需在 MoveIt2 中使用 TOPP-RA，需要编写自定义插件或在规划后自行后处理。MoveIt2 的适配器字段存在发行版差异：

# Humble / 旧版 MoveIt2: request_adapters 字段，命名空间通常是
# default_planner_request_adapters。虽然名字叫 request adapter，
# 时间参数化实际发生在规划结果返回前的后处理阶段。
request_adapters: >-
  default_planner_request_adapters/AddTimeOptimalParameterization

# Rolling / Kilted / main: response_adapters 字段，命名空间通常是
# default_planning_response_adapters，更准确表达“规划后处理”语义。
# response_adapters:
#   - default_planning_response_adapters/AddTimeOptimalParameterization

# MoveIt2 不提供内置 TOPP-RA 适配器；需要自定义 adapter 插件，
# 或在 MoveIt2 plan() 之后离线调用 toppra 再送给控制器。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：TOPP-RA 的路径点间距不均匀导致数值问题

错误做法：直接把 OMPL 的路径点（密疏不均）传给 TOPP-RA

现象：TOPP-RA 在稀疏处产生不准确的速度曲线

根本原因：TOPP-RA 在每个路径点求解 LP。稀疏处 LP 离散化误差大，可能导致约束违反

正确做法：先对 OMPL 路径做均匀重采样（按弧长等间隔插值），再传给 TOPP-RA

💡 概念误区：认为 TOPP-RA 可以替代 Ruckig

新手想法："TOPP-RA 既然是时间最优的，应该比 Ruckig 更好"

实际上：TOPP-RA 是离线工具——需要完整路径，计算时间 ~ms 级。在 visual servoing 场景中，目标每帧更新（30-60 Hz），根本没有"完整路径"可供 TOPP-RA 处理。Ruckig 从当前状态到目标状态直接生成轨迹（< 1 us），才是实时场景的正确选择

练习¶

⭐ 用 toppra Python 包对一条 3 点路径做时间参数化。可视化 $q(t)$、$\dot{q}(t)$、$\ddot{q}(t)$。验证不超限。
⭐⭐ 同一路径分别用 TOPP-RA（只有运动学约束）和 TOPP-RA（加力矩约束）。对比执行时间 $T$。
⭐⭐⭐ 阅读 toppra C++ 版 cpp/src/TOPPRA.cpp。标注反向扫描 LP：每个路径点的 LP 有多少变量和约束？

4. Ruckig——在线 Jerk-limited OTG 的现代标杆 ⭐⭐¶

4.1 什么是在线轨迹生成（OTG）¶

TOPP-RA 和 TOTG 是**离线**工具——需要完整路径。但在许多实时场景中，路径事先未知或随时变化：

场景	特征	为什么离线工具不行
Visual servoing	目标位姿每帧更新	无法预计算完整路径
碰撞避让	障碍物突然出现	重新规划+重新参数化太慢
人机协作	人靠近时需减速	需要实时响应
拾放切换	目标从物体 A 切换到 B	需要从当前状态平滑过渡
PLC 实时控制	控制周期 250 us - 1 ms	离线工具计算时间不可接受

在线轨迹生成（OTG）：给定当前状态和目标状态，在一次函数调用内（< 1 us）计算出满足所有约束的平滑轨迹。

4.2 Ruckig 概览¶

GitHub: pantor/ruckig | Stars ~1.1k | License: MIT（Community Version）| C++17 | 论文: RSS 2021

指标	Ruckig	Reflexxes Type IV
单次计算时间	< 1 us	~2 us
最小控制周期	250 us	~500 us
代码行数（核心）	~2000 行	数万行
外部依赖	零	独立
许可证	MIT	被 KUKA 收购闭源

4.3 Ruckig 的算法创新与三阶段数学推导¶

Reflexxes（前身）的方法：用一棵巨大的决策树（4760+ 节点）枚举所有可能的运动阶段组合。每个叶节点对应一种特定的运动剖面。

Ruckig 的方法：先计算所有可能的**极值曲线**（extremal profiles），然后根据时间约束选匹配的。

核心思路：jerk-limited 轨迹的每一段只有三种可能的 jerk 值：$+j_{max}$、$0$、$-j_{max}$。一条完整轨迹最多由 7 段组成（加加速→匀加速→减加速→匀速→加减速→匀减速→减减速）。Ruckig 列举所有有效段组合，为每种组合推导闭式解，选择时间最短的。

4.3.1 三阶段（加速-匀速-减速）数学推导 ⭐⭐⭐¶

为理解 Ruckig 的闭式求解过程，从最简单的 1-DOF 情况开始推导。

问题：从状态 $(p_0, v_0, a_0)$ 到 $(p_f, v_f, 0)$，在 jerk 限制 $|j| \leq j_{max}$、加速度限制 $|a| \leq a_{max}$、速度限制 $|v| \leq v_{max}$ 下，求时间最短轨迹。

Phase 1: 加速阶段（将速度从 $v_0$ 提升到 $v_{max}$ 或某个目标速度 $v_{peak}$）

加速阶段自身由 3 个子段构成（jerk = $+j_{max}$, $0$, $-j_{max}$）：

子段	Jerk	持续时间	加速度变化	速度变化
1a	$+j_{max}$	$t_1$	$a_0 \to a_{max}$	$v_0 \to v_1$
1b	$0$	$t_2$	$a_{max}$（恒定）	$v_1 \to v_2$
1c	$-j_{max}$	$t_3$	$a_{max} \to 0$	$v_2 \to v_{peak}$

子段 1a 的解析解：

\[t_1 = \frac{a_{max} - a_0}{j_{max}}, \quad v_1 = v_0 + a_0 t_1 + \frac{1}{2} j_{max} t_1^2\]

\[p_1 = p_0 + v_0 t_1 + \frac{1}{2} a_0 t_1^2 + \frac{1}{6} j_{max} t_1^3\]

子段 1c 的解析解（对称）：

\[t_3 = \frac{a_{max}}{j_{max}}, \quad v_{peak} = v_2 + a_{max} t_3 - \frac{1}{2} j_{max} t_3^2\]

子段 1b 的时间 $t_2$ 由目标速度 $v_{peak}$ 反解：

\[t_2 = \frac{v_{peak} - v_1 - a_{max} t_3 + \frac{1}{2} j_{max} t_3^2}{a_{max}}\]

如果 $t_2 < 0$，说明无法达到 $a_{max}$——需要缩短 $t_1$ 和 $t_3$（加速度达不到极限的"三角形"轮廓）。

Phase 2: 匀速阶段（$v = v_{peak}$, $a = 0$, $j = 0$）

\[t_4 = \frac{\Delta p_{remaining}}{v_{peak}}\]

其中 $\Delta p_{remaining}$ 是去掉加速和减速阶段位移后的剩余距离。如果 $t_4 < 0$，说明无法达到 $v_{max}$——需要降低 $v_{peak}$。

Phase 3: 减速阶段（从 $v_{peak}$ 降到 $v_f$，与 Phase 1 镜像对称）

同理由 3 个子段构成，闭式解完全对称。

Ruckig 的关键洞察：每种段组合（7 段全用、省略匀速段变 6 段、省略恒加速段变 5 段...）对应一组解析方程组。Ruckig 预先为每种有效组合推导了闭式解，运行时只需代入参数计算——这就是 < 1 us 的来源。没有数值积分、没有迭代优化，纯解析计算。

跨领域类比：Ruckig 的段组合枚举类似于编译器的模式匹配——不是运行时"搜索"最优解，而是编译期（或设计期）已经穷举了所有可能的解结构，运行时只需"查表+代入"。这与 M05 中 QP 求解器的在线迭代形成鲜明对比。

"不是X而是Y"句式：Ruckig 的算法创新不是"更好的决策树"，而是"完全不同的问题分解方式"——从"穷举决策分支"转变为"解析极值曲线匹配"。这种问题重构使得代码从数万行缩减到 ~2000 行，同时性能提升 2 倍。这是**算法创新驱动软件简化**的教科书案例。

4.4 多 DOF 同步策略——时间最短 DOF 决定总时间 ⭐⭐¶

对于 7-DOF 机械臂，每个关节独立求解会得到不同的最短时间。但机器人需要所有关节**同时到达**目标。

Ruckig 的同步算法：

对每个 DOF 独立计算最短时间：$T_1, T_2, \ldots, T_7$
取最大值：$T_{sync} = \max(T_1, T_2, \ldots, T_7)$
对每个 $T_i < T_{sync}$ 的 DOF，重新计算——在时间 $T_{sync}$ 内完成运动

重新计算的方式：降低该 DOF 的 jerk/加速度使轨迹"拉长"到 $T_{sync}$。具体来说，保持路径端点不变，调整中间段的参数使总时间恰好等于 $T_{sync}$。这等价于求解：

\[\text{给定 } (p_0, v_0, a_0) \to (p_f, v_f, 0) \text{ 且总时间 } = T_{sync}$$ $$\text{求满足 } |j| \leq j_{max}, |a| \leq a_{max}, |v| \leq v_{max} \text{ 的轨迹}\]

这是一个带等式时间约束的问题——Ruckig 为此也推导了闭式解。

结果：所有关节同时出发、同时到达，但"快"关节的运动更平滑（jerk 更小、加速度更低），"慢"关节接近极限运行。

4.5 Jerk-limited 轨迹的物理意义¶

为什么限制 jerk 很重要？

不限 jerk（梯形速度）	限制 jerk（S 形速度）
加速度可以瞬间跳变	加速度连续变化
$\tau \propto \ddot{q}$ 也跳变 → 电机电流突变	电流平滑变化
减速器内部冲击力大	冲击力小，延长齿轮寿命
末端执行器高频振动	振动被抑制
适用：低速、非精密任务	适用：高速、高精度、长寿命

7 段 jerk-limited 轨迹的形状：

jerk:    +j  0  -j  0  +j  0  -j
          ___      ___      ___
         |   |    |   |    |   |
    _____|   |____|   |____|   |_____
         t1  t2  t3  t4  t5  t6  t7

加速度: 线性增→恒定→线性减→0→线性增（负）→恒定（负）→线性减至0

速度:   S 形加速 → 匀速 → S 形减速

位置:   平滑曲线

不是所有轨迹都需要 7 段——如果距离短、速度限低，某些段可能时长为零。Ruckig 自动选择最短的段组合。

4.6 C++ 工程亮点精读¶

4.6.1 编译期/运行时 DOF 的双模态模板¶

// 编译期固定 DOF: 零堆分配, 栈分配 std::array<double, 7>
ruckig::Ruckig<7> otg(0.001);  // 7-DOF, 1 ms 控制周期

// 运行时动态 DOF: 用 std::vector
ruckig::Ruckig<ruckig::DynamicDOFs> otg(dof, 0.001);

内部实现：

template <size_t DOFs = 0>
class Ruckig {
    // DOFs > 0: std::array (栈)
    // DOFs == 0: std::vector (堆)
    using Vector = std::conditional_t<(DOFs > 0),
                      std::array<double, DOFs>,
                      std::vector<double>>;
    // ...
};

这与 Eigen 的 Matrix<double, N, 1> vs VectorXd 是**完全相同的设计思想**——两个独立的库面对同一个设计问题，得到了相同的解决方案。编译期版本的优势：

std::array<double, 7> 全部在栈上——零堆分配
编译器可以做循环展开和 SIMD 向量化
完美适合 M11 讲的 1 kHz 实时控制（EIGEN_RUNTIME_NO_MALLOC 不会被触发）

4.6.2 零外部依赖¶

Ruckig 不依赖 Eigen、不依赖 Boost、不依赖 ROS——整个库自包含。

为什么这重要？ struckig 项目把整个 Ruckig 移植到 IEC 61131-3 Structured Text（工业 PLC 编程语言），运行在 Beckhoff TwinCAT 3 上。如果 Ruckig 依赖了 Eigen，这种跨语言移植根本不可能。零依赖是工业级可移植性的前提。

4.6.3 50 亿随机轨迹的数值测试¶

验证项	精度阈值	含义
位置到达精度	1e-8	目标位置误差 < 10 nm
速度到达精度	1e-8	目标速度误差 < 10 nm/s
加速度到达精度	1e-10	极高精度
Jerk 上限违反	1e-12	接近机器精度

这种工业级数值质量保证在开源项目中非常罕见。值得学习的测试方法论：不是测几十个手写用例，而是用随机测试覆盖整个参数空间。

4.6.4 nanobind Python 绑定¶

Ruckig 从 pybind11 迁移到了 nanobind：编译速度提升 2.7-4.4 倍、二进制体积缩小 3-5 倍。与 VAMP 并列为 nanobind 在机器人库的标杆采用者。

4.7 典型使用流程¶

#include <ruckig/ruckig.hpp>
#include <iostream>

int main() {
    // 创建 OTG 实例: 7-DOF, 1 ms 控制周期
    ruckig::Ruckig<7> otg(0.001);
    ruckig::InputParameter<7> input;
    ruckig::OutputParameter<7> output;

    // === 设置当前状态 ===
    input.current_position = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    input.current_velocity = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    input.current_acceleration = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

    // === 设置目标状态 ===
    input.target_position = {1.0, 0.5, -0.3, 0.8, 0.2, -0.5, 0.3};
    input.target_velocity = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

    // === 设置约束 (Franka Panda 的典型值) ===
    input.max_velocity = {2.175, 2.175, 2.175, 2.175, 2.61, 2.61, 2.61};
    input.max_acceleration = {15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0};
    input.max_jerk = {7500, 3750, 5000, 6250, 7500, 10000, 10000};

    // === 在线控制循环 ===
    ruckig::Result result = ruckig::Result::Working;
    while (result == ruckig::Result::Working) {
        result = otg.update(input, output);

        // output.new_position/velocity/acceleration
        // → 直接发送给关节控制器

        // 关键: 把输出反馈为下一周期的输入
        output.pass_to_input(input);

        std::cout << "t=" << output.time
                  << " q0=" << output.new_position[0]
                  << " dq0=" << output.new_velocity[0] << std::endl;
    }

    if (result == ruckig::Result::Finished) {
        std::cout << "Duration: "
                  << output.trajectory.get_duration() << " s" << std::endl;
    }
}

4.8 在线目标切换——Ruckig 的杀手级特性¶

Ruckig 最强大的特性是**随时切换目标**——在运动过程中改变目标位置/速度，Ruckig 会从当前状态平滑过渡到新目标：

// 模拟 visual servoing: 目标每 100ms 更新一次
for (int frame = 0; frame < 10000; ++frame) {
    if (frame % 100 == 0) {
        // 目标更新! (来自视觉系统)
        input.target_position = get_new_target_from_camera();
    }
    // Ruckig 自动从当前运动状态平滑过渡到新目标
    auto result = otg.update(input, output);
    send_to_controller(output.new_position,
                       output.new_velocity,
                       output.new_acceleration);
    output.pass_to_input(input);
}

这就是 Ruckig 与 TOPP-RA/TOTG 的根本区别：TOPP-RA 目标变了就要重新规划+重新参数化（10-100 ms），Ruckig 直接从当前状态过渡到新目标（< 1 us）。

4.9 MoveIt2 集成¶

# Rolling / Kilted / main: MoveIt2 pipeline 配置为 Ruckig 平滑器
response_adapters:
  - default_planning_response_adapters/AddRuckigTrajectorySmoothing

# Humble / 旧版 MoveIt2 常见写法:
# request_adapters: >-
#   default_planner_request_adapters/AddRuckigTrajectorySmoothing

注意这里的 Ruckig Filter / RuckigSmoothing 是对 MoveIt2 已有轨迹做 jerk-limited 后处理，输入仍是 OMPL/CHOMP/STOMP 生成的完整路径。它不同于前面控制循环里每 1 ms 调一次 otg.update() 的在线 OTG 模式；Filter 不能替代运行时碰撞监控、路径约束检查或安全停机逻辑。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：忘记调用 output.pass_to_input(input) 导致轨迹断裂

错误做法：控制循环中只调 otg.update(input, output) 但不更新 input

现象：Ruckig 每次都从初始状态重新计算，产生不连续轨迹

根本原因：pass_to_input 把输出的 new_position/velocity/acceleration 拷贝到输入的 current_position/velocity/acceleration，形成闭环

正确做法：每次 update 后立即调用 output.pass_to_input(input)

💡 概念误区：认为 Ruckig 只能做点对点运动

新手想法："Ruckig 只能从 A 到 B，不能跟踪轨迹"

实际上：在线 OTG 模式下，Ruckig 可以在控制循环中不断接收新的目标状态，从当前状态平滑过渡到新目标。但这不是 MoveIt2 Ruckig Filter 的工作方式；MoveIt2 Filter 是离线/后处理 smoothing，对整条已规划轨迹重新分配时间并限制 jerk。在线目标更新还必须配合碰撞监控、路径约束检查和必要时的重新规划，否则可能平滑地驶向已经失效的目标。

🧠 思维陷阱：认为"jerk-limited"只是理论概念

新手想法："jerk 限制是学术论文的概念，工业上用不到"

实际上：jerk 不连续的物理后果：1) 电机电流突变（$\tau = M\ddot{q}$）；2) 减速器冲击力增大，缩短齿轮寿命；3) 末端振动，在精密装配和焊接中导致质量问题。高速、高精度、长寿命场景中 jerk-limited 是必需的

练习¶

⭐ 用 Ruckig<7> 为 Franka Panda 生成一条轨迹。可视化 pos/vel/acc/jerk——验证 jerk 不超限。
⭐⭐ 分别用 Ruckig<7> 和 Ruckig<DynamicDOFs>(7, ...) 做同一规划。Google Benchmark 对比性能。
⭐⭐ 模拟在线目标切换：运动到一半改变目标。验证切换点处位置、速度、加速度连续。
⭐⭐ 在 MoveIt2 中从 TOTG 切换到 Ruckig Filter。对比关节电流曲线平滑度。
⭐⭐ 模拟人机协作场景：运动中途降低 max_velocity。验证减速过程加速度连续、jerk 始终受限；观察 jerk 在切换点可能出现的跳变。

5. TOTG——MoveIt2 的默认时间参数化 ⭐⭐¶

5.1 定位：够用且稳定的基线¶

代码位置：moveit_core/trajectory_processing/time_optimal_trajectory_generation.*

算法：Kunz & Stilman 2012 论文的实现。迭代样条方法——约 1000 行 C++，可在一天内通读。

特性： - MoveIt2 大部分规划管线的默认时间参数化 - 数值稳定（不依赖 LP 求解器） - 不支持 jerk 限制——加速度在段边界处跳变 - 不支持力矩约束 - "够用且稳定"——不是最优的，但可靠

5.2 TOTG 算法原理——迭代样条拟合 ⭐⭐¶

TOTG 的算法（Kunz & Stilman 2012）与 TOPP-RA 不同——它不使用 LP 求解器，而是直接在路径上迭代构建速度曲线。

核心思想：

对每个路径点，计算当前约束下的最大速度（类似 MVC）
从起点前向积分（最大加速），从终点后向积分（最大减速）
取两条曲线的下界作为可行速度曲线
对速度曲线做三次样条拟合，保证位置连续到二阶导数

与 TOPP-RA 的算法区别：

维度	TOTG	TOPP-RA
求解方式	前后向积分 + 样条拟合	反向 LP + 正向贪心
需要求解器	不需要（纯数值积分）	需要 LP 求解器
时间最优保证	近似（样条拟合引入偏差）	离散化问题上精确（LP 全局最优）
数值稳定性	好（无矩阵运算）	好（LP 有理论保证）
代码复杂度	~1000 行	~3000 行

为什么 MoveIt2 选 TOTG 作默认？ 三个原因：1) 无外部依赖（不需要 LP 求解器库）；2) 数值稳定（不会因 LP 退化而失败）；3) 够用——对大多数 pick-and-place 场景，近似时间最优与精确时间最优的差距 < 5%。

跨领域类比：TOTG 之于 TOPP-RA，就像梯形积分之于 Gauss 积分——前者简单、鲁棒、"够用"，后者精确但需要更多数学机制。在工程中，"够用且可靠"往往比"最优但复杂"更有价值。

5.3 MoveIt2 三种时间参数化器的代码对比 ⭐⭐¶

在 MoveIt2 中，时间参数化器的插件入口随发行版演进：Humble/旧版多通过 PlanningRequestAdapter 和 request_adapters 配置；Rolling/Kilted/main 开始使用 PlanningResponseAdapter 和 response_adapters 表达规划后处理。下面展示代码层面的直接调用，以及两类 YAML 配置。

TOTG（默认）的配置与调用：

// === TOTG: MoveIt2 默认时间参数化 ===
// 位于 moveit_core/trajectory_processing/
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>

// 创建 TOTG 实例
trajectory_processing::TimeOptimalTrajectoryGeneration totg(
    0.1,    // path_tolerance: 路径偏差容忍度
    0.01    // resample_dt: 重采样时间步长
);

// 应用到 MoveIt 轨迹
robot_trajectory::RobotTrajectory trajectory(robot_model, "panda_arm");
// ... 填充路径点 ...

// 计算时间参数化
bool success = totg.computeTimeStamps(
    trajectory,
    1.0,   // max_velocity_scaling_factor (0-1)
    1.0    // max_acceleration_scaling_factor (0-1)
);
// 注意: TOTG 不支持 jerk scaling — 加速度在段边界处跳变

TOPP-RA 的调用（通过外部 toppra 库，非 MoveIt2 内置）：

# === TOPP-RA: 时间最优 + 可选力矩约束（概念骨架） ===
# TOPP-RA 不是 MoveIt2 的标准适配器，需通过外部 toppra 库调用。
# 约束类名、limit 数组形状和 solver 参数按目标 toppra 版本确认。
import toppra as ta
import toppra.constraint as constraint
import numpy as np

# 从 MoveIt2 规划结果提取路径点（关节空间）
way_pts = np.array([...])  # (N, n_dof)
ss = np.linspace(0, 1, len(way_pts))  # 路径参数

path = ta.SplineInterpolator(ss, way_pts)

# 关节速度/加速度约束：部分版本要求 [[lower, upper], ...]，
# 部分示例接受正上限向量。这里用区间形式表达概念。
vmax = np.array([...])
amax = np.array([...])
vlim = np.column_stack((-vmax, vmax))
alim = np.column_stack((-amax, amax))
vel_lim = constraint.JointVelocityConstraint(vlim)
acc_lim = constraint.JointAccelerationConstraint(alim)

# 可选: 力矩约束需要按目标版本自定义二阶约束，
# 形式为 tau_min <= A(s) u + B(s) x + C(s) <= tau_max。
# torque_lim = make_torque_constraint_for_this_toppra_version(...)

instance = ta.algorithm.TOPPRA(
    [vel_lim, acc_lim], path, parametrizer="ParametrizeConstAccel"
)
traj = instance.compute_trajectory()
# 优势: 支持力矩约束；在离散路径可行、LP 求解成功时保证离散问题时间最优
# 劣势: 需要额外集成工作, 不支持 jerk 约束

Ruckig Filter 的配置与调用：

// === Ruckig: Jerk-limited 平滑 ===
// 位于 moveit_core/trajectory_processing/
#if __has_include(<moveit/trajectory_processing/ruckig_traj_smoothing.hpp>)
#include <moveit/trajectory_processing/ruckig_traj_smoothing.hpp>
#else
#include <moveit/trajectory_processing/ruckig_traj_smoothing.h>  // Humble 等旧版
#endif

trajectory_processing::RuckigSmoothing ruckig;

// Humble 常用 API: applySmoothing(trajectory, velocity_scale, acceleration_scale)；
// 也可传 velocity/acceleration/jerk 的 unordered_map。
// Rolling / Kilted / MoveIt2 main: .hpp 为主，并增加了带
// moveit_msgs::msg::JointLimits 列表的 overload，可直接传 jerk limits。
// URDF 原生 <limit> 只有 lower/upper/velocity/effort，不包含 jerk 字段。
// Ruckig Filter 的 jerk 限制应来自 joint_limits.yaml 或 MoveIt 的 joint_limits 配置；
// 未配置时的默认行为按目标 MoveIt2 版本确认。
bool success = ruckig.applySmoothing(
    trajectory,
    1.0,   // max_velocity_scaling_factor
    1.0    // max_acceleration_scaling_factor
);
// 优势: 加速度连续, jerk 有界/分段常值
// 注意: Ruckig Filter 模式是对已有轨迹做 jerk 平滑,
//       不是在线 OTG 模式

通过 YAML 配置切换（推荐方式）：

# === planning_pipeline.yaml ===
# Humble / 旧版 MoveIt2
request_adapters: >-
  default_planner_request_adapters/AddTimeOptimalParameterization

# 若切换到 Ruckig smoothing:
# request_adapters: >-
#   default_planner_request_adapters/AddRuckigTrajectorySmoothing

# Rolling / Kilted / main
# response_adapters:
#   - default_planning_response_adapters/AddTimeOptimalParameterization
#
# 若切换到 Ruckig smoothing:
# response_adapters:
#   - default_planning_response_adapters/AddRuckigTrajectorySmoothing

# 方案 C: TOPP-RA (需要力矩约束时)
# MoveIt2 不内置 TOPP-RA adapter。
# 需要按目标发行版自定义 RequestAdapter / ResponseAdapter 插件，
# 或在 MoveIt2 规划后做离线后处理，再把结果送给控制器。

"不是X而是Y"句式：在 MoveIt2 中切换时间参数化器不是"更换算法"那么简单——它改变的是轨迹的**物理特性**。TOTG 产生加速度跳变（高频振动源），Ruckig 产生加速度连续、jerk 有界的轨迹（jerk 通常分段常值，可在切换点跳变），TOPP-RA 产生力矩可行轨迹（避免驱动器过载）。选择哪个取决于你的末端执行器对振动的敏感度和驱动器的力矩裕度。

5.4 三种工具综合对比¶

维度	TOTG	TOPP-RA	Ruckig
类型	离线后处理	离线后处理	在线 OTG；MoveIt2 中也可作后处理 Filter
输入	完整路径	完整路径	当前状态 + 目标；Filter 输入完整轨迹
约束	vel/acc	vel/acc/torque	vel/acc/jerk
时间最优性	近似	离散问题精确	在线 OTG 对给定起止状态精确；Filter 只做后处理平滑
Jerk 限制	❌	❌	✅
力矩约束	❌	✅	❌
计算时间	~ms	~ms	在线 OTG < 1 us；Filter 随轨迹点数通常为 ms 级
MoveIt2	默认	需自定义集成	Ruckig Filter / Smoothing 后处理
代码规模	~1000 行	~3000 行	~2000 行
教学价值	★★★☆☆	★★★★☆	★★★★★

5.5 选型决策树¶

你的应用场景？
│
├── 实时在线(visual servoing、碰撞避让、目标切换)
│   └─→ Ruckig (< 1 us, 在线, jerk-limited)
│
├── 给定完整路径的离线后处理
│   ├── 需要力矩约束(动力学可行性) → TOPP-RA
│   └── 只需运动学约束(vel/acc)
│       ├── 需要 jerk-limited → Ruckig (作为路径平滑器)
│       └── 不需要 jerk → TOTG (最简单)
│
├── MoveIt2 默认
│   └─→ TOTG (开箱即用) 或 Ruckig Filter (推荐升级)
│
├── 嵌入式 PLC 部署
│   └─→ Ruckig (零依赖 + struckig PLC 版本)
│
└── 工业应用: 焊接/涂胶(需要恒定速度)
    └─→ 弧长参数化 + 标量路径速度控制；若要求精确速度范数，
        用非线性轨迹优化或工艺专用规划器

⚠️ 常见陷阱¶

💡 概念误区：认为 TOTG 不好因为"不支持 jerk"

新手想法："TOTG 应该被淘汰了"

实际上：很多场景不需要 jerk 限制——低速 pick-and-place、教学演示、仿真实验。TOTG 的优势是**简单可靠**。只有当你观察到末端振动或电机电流突变时，才需要升级到 Ruckig

正确思维：TOTG 是"够用"的基线，Ruckig 和 TOPP-RA 是"需要时"的升级

练习¶

⭐ 阅读 time_optimal_trajectory_generation.cpp 的主循环。标注速度和加速度约束检查逻辑。
⭐⭐ 用 TOTG 和 TOPP-RA 分别处理同一条 OMPL 路径，对比执行时间 $T$。
⭐⭐ 画出 TOTG 处理的轨迹的 jerk 曲线。路径点处应能看到 jerk 跳变。

6. 工业应用场景 ⭐⭐¶

6.1 焊接速度均匀性——具体参数与工程实现 ⭐⭐¶

弧焊应用中，焊接速度必须均匀——速度不均导致焊缝质量不一致（减速处堆积、加速处稀薄）。但规划路径在转角处曲率不同，简单的时间参数化会在转角处降速。

典型焊接参数（MIG 焊接碳钢板 3-6mm 厚度）：

参数	典型值	允许偏差	超差后果
焊接速度	8-15 mm/s	$\pm 5\%$	焊缝宽度不均、强度下降
送丝速度	3-8 m/min	与行走速度匹配	过多→堆积；过少→未熔合
焊枪姿态偏差	-	$\pm 3°$	焊缝偏移、气孔
弧长（电压）	18-26 V	$\pm 1$ V	飞溅增多、焊缝成型差

速度均匀性的量化指标：

\[\text{速度均匀性} = 1 - \frac{v_{max} - v_{min}}{v_{target}} \times 100\%\]

工业标准要求速度均匀性 $\geq 95\%$，即末端速度在目标值 $\pm 5\%$ 范围内。

在路径转角处的挑战：

在直线段之间的转角处（如 L 形焊缝），路径曲率突变。如果机器人严格沿着尖角路径运动，必须在转角处减速到零再加速——速度均匀性被破坏。

解决方案对比：

方案	速度均匀性	路径精度	实现复杂度
圆弧过渡（blending radius）	高（95%+）	偏离原始路径	低
弧长参数化 + 标量路径速度	高（95%+）	取决于路径/IK	中
非线性轨迹优化（笛卡尔速度范数约束）	高（98%+）	精确	高
Ruckig 在线跟踪 + 预瞄	中高（90%+）	实时跟踪	中
梯形速度曲线	低（70-80%）	精确但有停顿	低

弧长参数化 + TOPP-RA 的实现思路：

# 焊接场景: 目标不是时间最优, 而是末端 feed rate 稳定。
# 不要把 v_min <= ||J(q) qdot|| <= v_max 直接塞进标准 TOPP-RA LP：
# 笛卡尔速度范数的下界/等式不是标准 TOPP-RA 的线性二阶约束。
# 工程上先用焊缝弧长 l 作为路径参数，让标量路径速度 dl/dt
# 近似等于末端行走速度，再用关节 vel/acc/torque 约束检查可行性。
import toppra as ta
import numpy as np

# 1. 笛卡尔焊缝按弧长重采样，做 IK 得到关节路径 way_pts。
# arc_lengths 单位为 m，严格单调递增。
arc_lengths = compute_cartesian_arclength(weld_points)
ss = arc_lengths / arc_lengths[-1]
way_pts = solve_ik_along_weld_path(weld_points)
path = ta.SplineInterpolator(ss, way_pts)

# 2. 目标 feed rate 10 mm/s。若 s 已归一化，则 ds/dt 的目标上界
# 约为 v_feed / total_length。标准 TOPP-RA 可表达“不要超过这个速度”
# 和关节速度/加速度/力矩约束；精确恒速通常由工艺层按时间采样执行。
v_feed = 0.010  # m/s
sdot_max = v_feed / arc_lengths[-1] * 1.05

pc_path_speed = make_scalar_path_speed_upper_bound(sdot_max)

# 组合约束: 标量路径速度上界 + 关节限/力矩限
constraints = [pc_path_speed, pc_vel, pc_acc]
instance = ta.algorithm.TOPPRA(constraints, path)
trajectory = instance.compute_trajectory()

如果必须严格满足 $\|J(q)\dot{q}\| \in [v_{target}-\epsilon, v_{target}+\epsilon]$，尤其还要同时优化路径形状、姿态和转角 blending，就应建成非线性轨迹优化问题（如 direct collocation / Drake / Crocoddyl / Tesseract TrajOpt），而不是把它伪装成标准 TOPP-RA LP。

反事实推理：如果在焊接中使用标准时间最优参数化（不加恒速约束），末端速度在转角处可能降至目标值的 30-50%。以 10 mm/s 焊接速度为例，转角处降至 3-5 mm/s，焊接电流保持不变的情况下，热输入密度增加 2-3 倍——焊缝在转角处过宽、变形增大、甚至可能烧穿薄板。这就是为什么焊接应用中"速度均匀性"比"时间最优"更重要。

6.2 涂胶轨迹¶

涂胶中，胶量与速度倒数成正比。要求速度恒定或按预定义曲线变化。TOPP-RA 的自定义约束可以精确控制末端速度曲线。

6.3 人机协作减速¶

ISO 10218 和 ISO/TS 15066 要求人进入协作区域时机器人限速（< 250 mm/s）。Ruckig 的在线 OTG 最自然：

if (distance_to_human < safety_threshold) {
    // 降低速度限制
    input.max_velocity = {0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5};
    // Ruckig 下一控制周期自动平滑减速, 无需重新规划!
}

ISO/TS 15066 安全距离计算：

\[S_{min} = v_{human} \cdot T_{react} + v_{robot} \cdot T_{stop} + C\]

其中 $v_{human} \approx 1.6$ m/s（步行速度），$T_{react}$ 是传感器+软件反应时间，$T_{stop}$ 是 Ruckig 从当前速度减速到零的时间（由 $a_{max}$ 和 $j_{max}$ 决定），$C$ 是安全余量。

Ruckig 的优势：$T_{stop}$ 是解析可算的（闭式解），不需要仿真。可以在每个控制周期精确计算当前状态下的停止距离，实时判断是否需要减速。

6.4 动态环境中的在线避障¶

障碍物突然出现时，立即切换到安全点：

if (obstacle_detected) {
    input.target_position = safe_waypoint;
    // Ruckig 从当前运动状态平滑过渡, < 1 us
}

6.5 多关节同步与时间缩放 ⭐⭐¶

工业应用中常需要动态调整执行速度（如操作员旋转速度旋钮）：

// 速度缩放: 0.0 (暂停) ~ 1.0 (全速)
double speed_scale = read_speed_slider();  // 来自操作面板

// 方案 A: 重新设置约束 (Ruckig 自动适配)
// 若希望近似保持同一轨迹形状并只做时间伸缩，速度/加速度/jerk
// 应按 k, k^2, k^3 缩放。工程上也可按安全策略选更保守的比例。
double k = std::clamp(speed_scale, 0.0, 1.0);
for (int i = 0; i < 7; ++i) {
    input.max_velocity[i] = nominal_velocity[i] * k;
    input.max_acceleration[i] = nominal_acceleration[i] * k * k;
    input.max_jerk[i] = nominal_jerk[i] * k * k * k;
}
// Ruckig 下一周期自动生成符合新约束的轨迹

// 方案 B: 用 Ruckig 的时间缩放 API
// ruckig 0.13+ 支持 min_duration 参数
// if (k > 0.0) input.min_duration = nominal_duration / k;

关键细节：纯时间伸缩不是把 velocity、acceleration、jerk 都乘同一个系数。若时间放慢为原来的 $1/k$，则速度上限按 $k$ 缩放，加速度按 $k^2$ 缩放，jerk 按 $k^3$ 缩放。只缩放 velocity 不缩放 acceleration/jerk，会改变加减速段形状。

反事实推理：如果操作员突然把速度旋钮从 100% 拧到 0%（紧急停机），且只用了 TOTG 而非 Ruckig，会怎样？TOTG 是离线工具——无法响应运行时速度变化。机器人要么继续按原速运动（危险），要么必须触发硬件急停（E-Stop）导致加速度不连续冲击。Ruckig 能在 < 1 us 内重新计算制动轨迹，平滑减速到零。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为工业场景都是"规划一次、执行一次"

新手想法："OMPL 规划路径 → TOTG 参数化 → 执行完成，就这么简单"

实际上：真实工业场景充满动态变化——操作员调速、人员接近、障碍物出现、视觉更新位姿、力传感器触发。静态离线管线在这些场景中要么无法响应，要么响应太慢。现代工业机械臂系统通常是**离线规划 + 在线实时调整**的混合架构——OMPL 负责初始路径，Ruckig 负责实时执行和在线修正

正确思维：时间参数化不是"一次性后处理"，而是贯穿整个运动执行过程的持续功能

练习¶

⭐⭐ 模拟焊接场景：设计直线+圆弧路径。分别用"时间最优"和"恒定速度"参数化，画出末端速度曲线对比。
⭐⭐ 模拟人机协作：Ruckig 在运动中途降低 max_velocity。验证减速平滑，计算过渡时间。
⭐⭐ 实现速度旋钮模拟：用键盘输入 0-100% 速度，实时调整 Ruckig 约束。验证加速度连续性。

7. 加加速度（Jerk）约束的物理意义与工程影响 ⭐⭐¶

7.1 Jerk 的物理来源¶

Jerk $j = d^3q/dt^3$ 是加速度的变化率。在机械臂系统中，jerk 通过以下物理链条产生实际影响：

\[j = \dddot{q} \xrightarrow{\text{动力学}} \dot{\tau} = M\dddot{q} + \ldots \xrightarrow{\text{电气}} \frac{dI}{dt} = \frac{\dot{\tau}}{K_t} \xrightarrow{\text{机械}} F_{impact} \propto \frac{dI}{dt}\]

力矩变化率 $\dot{\tau}$ 决定了电机电流的变化率 $dI/dt$。电流急变产生两个后果：

后果	物理机制	工程影响
电机发热增加	高 $dI/dt$ 产生涡流损耗	缩短电机寿命、需要降额运行
减速器冲击	齿轮啮合力突变	齿面磨损加速、游隙增大、精度下降
结构振动	弹性变形波沿连杆传播	末端位置精度下降
噪声	冲击力激发机械共振	工作环境噪声超标

7.2 Jerk 限制值的工程选取¶

经验规则：jerk 限值通常取加速度限值的 5-20 倍除以期望加速时间：

\[j_{max} \approx \frac{a_{max}}{t_{ramp}} \times k_{safety}\]

其中 $t_{ramp}$ 是期望的加速度爬升时间（通常 10-50 ms），$k_{safety}$ 是安全系数（2-5）。

典型值参考：

机器人型号	关节	$a_{max}$ (rad/s$^2$)	$j_{max}$ (rad/s$^3$)	$j_{max}/a_{max}$
Franka Panda J1	肩部	15.0	7500	500
Franka Panda J7	腕部	20.0	10000	500
UR5e J1	肩部	40.0	20000	500
KUKA LBR iiwa J1	肩部	10.0	5000	500

本质洞察：$j_{max}/a_{max}$ 的比值 $\approx 500$ s$^{-1}$ 意味着加速度从 0 爬升到 $a_{max}$ 的最短时间约 2 ms。这不是任意选择——2 ms 对应减速器的典型应力波传播时间。更快的加速度变化（更大的 jerk）会产生冲击波效应，而非准静态加载。

7.3 Jerk 限制对执行时间的影响¶

引入 jerk 限制总是会增加执行时间（相比无 jerk 限制的梯形轮廓）。增加多少取决于运动距离和约束比值：

运动距离	无 jerk 限制	有 jerk 限制	时间增加
短距离（< 10 deg）	0.15 s	0.20 s	+33%
中距离（30 deg）	0.35 s	0.40 s	+14%
长距离（90 deg）	0.80 s	0.85 s	+6%

规律：运动距离越长，jerk 限制对总时间的影响越小——因为匀速段不受 jerk 影响，jerk 只影响加减速过渡段。

练习¶

⭐⭐ 计算：Franka Panda J1 的加速度从 0 升到 $a_{max} = 15$ rad/s$^2$ 的最短时间（给定 $j_{max} = 7500$）。这个时间内关节转过多少角度？
⭐⭐ 用 Ruckig 分别设置 $j_{max} = 5000, 10000, 20000$ 生成同一条轨迹。对比执行时间和加速度平滑度。画出三条 $\ddot{q}(t)$ 曲线。

8. 性能 Benchmark：TOPP-RA vs TOTG vs Ruckig ⭐⭐¶

8.1 测试方法论¶

公平对比三种工具需要统一的测试框架：

测试环境： - 机器人模型：Franka Panda (7-DOF) - 路径来源：OMPL RRT-Connect 规划的 20 条路径（简单+复杂+奇异附近） - 约束：使用相同的关节速度/加速度限（Ruckig 额外加 jerk 限）

评测指标：

指标	含义	单位
$T$	总执行时间	s
$t_{compute}$	参数化计算时间	us
$\max	j_i	$
$\sigma_v$	末端速度标准差（均匀性）	mm/s
$\Delta\tau_{peak}$	力矩变化率峰值	Nm/s

8.2 典型结果¶

指标	TOTG	TOPP-RA	Ruckig Filter
执行时间 $T$	2.35 s	2.28 s	2.51 s
计算时间 $t_{compute}$	0.8 ms	1.2 ms	轨迹后处理为 ms 级，随路径点数变化
最大 jerk	$\infty$（跳变）	$\infty$（跳变）	8500 rad/s$^3$
力矩变化率峰值	高（冲击）	高（冲击）	低（平滑）
抗扰动能力	无（离线）	无（离线）	无（MoveIt2 后处理）；在线 OTG 模式才支持实时重算

解读：

TOPP-RA 执行时间最短——因为它在 $(s, \dot{s})$ 空间做全局优化，利用了所有约束的"余量"
Ruckig 执行时间略长——jerk 限制牺牲了约 10% 的时间最优性，但换来了加速度连续性
TOTG 介于两者之间——近似时间最优，但没有 TOPP-RA 在离散化问题上的全局最优保证
Ruckig Filter 的价值是 jerk 平滑——不要把在线 OTG 单周期 < 1 us 的结论直接套到 MoveIt2 后处理器；后处理耗时取决于轨迹点数和适配器链路

时间最优性:  TOPP-RA ≻ TOTG ≻ Ruckig
平滑度:     Ruckig ≻≻≻ TOTG ≈ TOPP-RA
计算速度:   在线 OTG Ruckig ≻≻≻ 离线后处理；MoveIt2 Filter 需按轨迹长度实测
在线能力:   在线 OTG Ruckig 有；Ruckig Filter/TOTG/TOPP-RA 无

8.3 选型总结¶

如果你只能选一个：Ruckig——在线 OTG 覆盖实时目标切换，MoveIt2 Filter 覆盖离线路径的 jerk 平滑；但 < 1 us 和精确 OTG 只对应在线 OTG 调用，不对应整条轨迹的后处理。

如果你需要两个：Ruckig（实时场景）+ TOPP-RA（离线规划+力矩约束场景）。

如果你什么都不想配置：TOTG（MoveIt2 默认，零配置开箱即用）。

⚠️ 常见陷阱¶

🧠 思维陷阱：根据 benchmark 数字盲目选择

新手想法："TOPP-RA 执行时间最短，所以总是选 TOPP-RA"

实际上：benchmark 中执行时间差异通常 < 15%。但实时能力、jerk 平滑度、力矩可行性的差异是质的。焊接应用优先看弧长参数化、转角 blending 和速度均匀性；重载应用看 TOPP-RA 力矩约束；高速装配看 Ruckig jerk 平滑。选型应根据**应用需求**，不是 benchmark 排名

练习¶

⭐⭐ 在你的 MoveIt2 环境中，对同一条 OMPL 路径分别用 TOTG 和 Ruckig Filter 参数化。用 ros2 bag record 录制关节命令，用 Python 画出 jerk 对比图。
⭐⭐⭐ 设计 benchmark 脚本：生成 50 条随机路径，三种方法全跑一遍，输出表格。分析哪些路径特征（长度、曲率、奇异接近度）影响各方法的相对优势。

9. Ruckig 源码精读指南 ⭐⭐⭐¶

Ruckig 的源码是学习现代 C++ 库设计的极佳教材——零依赖、模板双模态、闭式算法、50 亿测试。核心代码约 2000 行，精读 3 天可完整理解。以下是精读路线图。

9.1 精读路线图¶

文件	内容	精读重点
`include/ruckig/ruckig.hpp`	主类	`DOFs` 模板的编译期/运行时分支
`include/ruckig/input_parameter.hpp`	输入结构体	`std::array` vs `std::vector`
`include/ruckig/output_parameter.hpp`	输出结构体	`pass_to_input()` 实现
`src/ruckig/brake.cpp`	制动预轨迹	数值精度最高的部分
`src/ruckig/position.cpp`	位置模式求解	极值曲线匹配核心逻辑
`examples/10_eigen_library.cpp`	Eigen 集成	与 Eigen 生态的接口
`test/otg_test.cpp`	测试框架	50 亿随机轨迹测试

9.2 模板双模态的实现细节¶

template <size_t DOFs = 0>
class Ruckig {
    using Vector = std::conditional_t<(DOFs > 0),
                      std::array<double, DOFs>,   // 编译期: 栈
                      std::vector<double>>;        // 运行时: 堆

    double control_cycle;
    // ...

    Result update(InputParameter<DOFs>& input,
                  OutputParameter<DOFs>& output) {
        // 核心计算: 对每个 DOF 独立求解极值曲线
        // 然后取最长时间做同步
        // ...
    }
};

同步策略：多 DOF 的时间最优轨迹不是简单的"每个 DOF 独立最优"——需要所有 DOF 同时到达。Ruckig 先计算每个 DOF 独立的最短时间，取最大值 $T_{sync}$，然后让其他 DOF "放慢"以匹配 $T_{sync}$。放慢的方式是降低 jerk/加速度使得轨迹变长但更平滑。

9.3 制动轨迹（brake.cpp）的关键性¶

为什么制动计算需要最高精度？考虑以下场景：机械臂以速度 $v = 2$ rad/s 运行，突然需要停止（目标速度 = 0）。制动轨迹决定了停止距离和停止时间。如果数值误差导致停止距离计算偏大 0.01 rad，关节可能超过限位 0.01 rad——这在精密装配中是不可接受的。

练习¶

⭐⭐ 精读 ruckig.hpp 中的模板选择逻辑。DOFs = 7 时容器类型是什么？DOFs = 0 呢？
⭐⭐⭐ 阅读 brake.cpp 的制动预轨迹计算。标注数值精度最敏感的代码行。
⭐⭐⭐ 分析 test/otg_test.cpp 的测试组织方式。如何用随机测试覆盖参数空间？精度阈值如何分层？

本章小结¶

知识点	核心内容	难度	关键收获
时间参数化问题	几何路径 → 时间轨迹	⭐	规划到执行的鸿沟
TOPP 经典理论	相空间、MVC、bang-bang	⭐⭐⭐	理论基础
TOPP-RA	可达集分析 + LP + 力矩约束	⭐⭐	离线时间最优现代方法
Ruckig OTG	在线、jerk-limited、< 1 us	⭐⭐	实时控制首选
Ruckig 工程设计	模板双模态、零依赖、50 亿测试	⭐⭐	现代 C++ 库设计典范
TOTG	MoveIt2 默认、1000 行、简单稳定	⭐⭐	够用的基线
工业应用	焊接、涂胶、人机协作	⭐⭐	理论到工程映射

累积项目：本章新增模块¶

项目名称：从零构建 7-DOF 机械臂控制栈

章节	新增模块	功能
M01	URDF 加载 + FK/Jacobian	Pinocchio 基础设施
M05	QP 求解器层	瞬态 IK QP
M08	轨迹优化	NLP 轨迹规划
M10（本章）	时间参数化层	几何路径 → 可执行轨迹
M11	实时 C++ 工程	1 kHz 控制循环

延伸阅读¶

资源	内容	难度
Berscheid & Kroeger, Jerk-limited Real-time Trajectory Generation, RSS 2021	Ruckig 论文——闭式 OTG 的完整数学推导	⭐⭐
Pham & Pham, A New Approach to TOPP Based on Reachability Analysis, T-RO 2018	TOPP-RA 论文——可达集分析的理论基础和正确性证明	⭐⭐⭐
Kunz & Stilman, Time-Optimal Trajectory Generation for Path Following with Bounded Velocities and Accelerations, RSS 2012	TOTG 论文——迭代样条方法	⭐⭐
Bobrow et al., Time-Optimal Control of Robotic Manipulators Along Specified Paths, IJRR 1985	经典 TOPP——相空间分析的开创性工作	⭐⭐⭐⭐
Slotine & Yang, Improving the Efficiency of Time-Optimal Path-Following Algorithms, T-RA 1989	切换点分类的完整理论	⭐⭐⭐⭐
Kroeger, Opening the Door to New Sensor-Based Robot Applications, ICRA 2011	Reflexxes OTG 背景——理解 Ruckig 改进了什么	⭐⭐
Ruckig GitHub: `pantor/ruckig`	源码 + 示例——重点读 brake.cpp 和 position.cpp	⭐
toppra GitHub: `hungpham2511/toppra`	Python + C++ 实现——重点读 TOPPRA.compute_trajectory()	⭐⭐
struckig GitHub	Ruckig 的 IEC 61131-3 PLC 移植——工业部署参考	⭐⭐

故障排查手册¶

症状	可能原因	排查步骤	相关章节
Ruckig 返回 `ErrorSynchronizationCalculation`	约束不兼容	1. 检查 max_jerk 是否合理 2. 增大 max_jerk 测试 3. 检查初始状态是否违反约束	M10.4
TOTG 后轨迹有末端振动	jerk 不连续激励共振	1. 画 jerk 曲线 2. 切换到 Ruckig Filter 3. 降低加速度限	M10.5
TOPP-RA 返回 infeasible	路径某点约束不可行	1. 检查是否有奇异构型 2. 放宽力矩约束 3. 增加路径点密度	M10.3
实际执行偏差大	控制器跟踪误差	1. 对比规划 vs 实际 q(t) 2. 检查控制器增益 3. 减小控制周期	M10, M11
MoveIt2 执行时关节抖动	TOTG 加速度跳变	1. 切换到 Ruckig Filter 2. 增加 path_tolerance 3. 降低速度	M10.5
Ruckig 轨迹时间异常长	max_jerk 设置过小	1. 检查 jerk 值是否合理 2. 参照表 7.2 的典型值 3. $j_{max}/a_{max}$ 应 $\approx 500$	M10.7
TOPP-RA 计算时间过长	路径点过密	1. 减少路径点到 50-200 2. 先做弧长重采样 3. 检查 LP 求解器配置	M10.3
焊接场景速度不均匀	转角处 MVC 降速	1. 按焊缝弧长参数化 2. 加圆弧过渡 3. 必要时用非线性轨迹优化约束速度范数	M10.6

故障排查的通用流程：

轨迹问题?
│
├── 静态分析 (先看曲线)
│   ├── 画 q(t), dq(t), ddq(t), dddq(t) 四层曲线
│   ├── 检查是否有 NaN/Inf
│   └── 检查是否超限 (对比关节极限)
│
├── 约束检查 (是否满足物理限制)
│   ├── 速度超限? → 降低 max_velocity_scaling
│   ├── 加速度超限? → 降低 max_acceleration_scaling
│   ├── 力矩超限? → 切换到 TOPP-RA 加力矩约束
│   └── Jerk 超限? → 切换到 Ruckig
│
└── 执行检查 (规划正确但执行偏差大)
    ├── 控制器增益不匹配 → 调 PD 增益
    ├── 通信延迟 → 检查 EtherCAT/UDP
    └── 采样率不匹配 → 确认控制周期 = 参数化周期

跨章综合练习 ⭐⭐⭐¶

题目：综合 M08（轨迹优化）+ M10（时间参数化）+ M11（实时 C++），实现"规划→参数化→执行"完整管线：

用 OMPL RRT-Connect 为 Franka Panda 规划绕障碍物路径（M07）
分别用 TOTG、TOPP-RA、Ruckig Filter 做时间参数化（M10）
在 1 kHz 实时控制循环中执行轨迹（M11）
画出三种方法的 $q(t)$、$\dot{q}(t)$、$\ddot{q}(t)$、$\dddot{q}(t)$ 对比图
测量执行时间 $T$、最大 jerk、关节电流峰值

评分标准：TOPP-RA 应给出最短 $T$，Ruckig 应给出最平滑 jerk 曲线，TOTG 应在 jerk 图上显示明显跳变。

进阶挑战：在上述管线基础上，加入末端力传感器反馈——当检测到接触力 > 5 N 时，Ruckig 实时切换目标为当前位置（柔顺停止）。验证停止过程的加速度连续性。

子段	Jerk	持续时间	加速度变化	速度变化
1a	\(+j_{max}\)	\(t_1\)	\(a_0 \to a_{max}\)	\(v_0 \to v_1\)
1b	\(0\)	\(t_2\)	\(a_{max}\)（恒定）	\(v_1 \to v_2\)
1c	\(-j_{max}\)	\(t_3\)	\(a_{max} \to 0\)	\(v_2 \to v_{peak}\)

编号	问题	答不出时回顾
1	几何路径 vs 轨迹：路径是 \(\sigma(s)\)（参数 \(s \in [0,1]\)），轨迹是 \(q(t)\)（参数 \(t \in [0,T]\)）。两者的本质区别是什么？	M07/M08 运动规划
2	加加速度（Jerk）：jerk 是加速度的时间导数 \(j = d^3q/dt^3\)。为什么工业机械臂控制中要限制 jerk？（提示：振动、电机电流突变、减速器磨损）	控制理论基础
3	C++ 模板编译期 vs 运行时：`Ruckig<7>` 和 `Ruckig<DynamicDOFs>` 的区别是什么？为什么编译期固定维度能避免堆分配？	v8 Ch12-13 模板
4	MoveIt2 规划管线：MoveIt2 的规划请求从 `PlanningScene` 到 `PlanningPipeline` 到 `PlanRequestAdapter` 的数据流是什么？时间参数化在哪个阶段介入？	M14 MoveIt2
5	实时控制约束：1 kHz 控制循环意味着每个周期只有 1 ms 时间预算。哪些操作可以在控制循环中做？哪些不可以？	M11 实时 C++

段类型	加速度	物理含义
最大加速段	\(\ddot{s} = \ddot{s}_{max}(s, \dot{s})\)	某个关节达到最大加速力矩
最大减速段	\(\ddot{s} = \ddot{s}_{min}(s, \dot{s})\)	某个关节达到最大减速力矩
极限弧段	沿 MVC 滑动	某个关节恰好在速度极限上

类型	特征	物理含义
零惯量点（Zero-inertia）	\(\sigma'_j(s) = 0\)——路径切线与某关节轴正交	该关节的惯量在此方向为零，力矩对加速度的影响突变
切换点（Switch point）	\(\ddot{s}_{max}\) 从正变负或反之	限制加速度的关节从一个切换到另一个
MVC 接触点	轨迹触碰速度上界	速度达极限，需减速避免超限

不限 jerk（梯形速度）	限制 jerk（S 形速度）
加速度可以瞬间跳变	加速度连续变化
\(\tau \propto \ddot{q}\) 也跳变 → 电机电流突变	电流平滑变化
减速器内部冲击力大	冲击力小，延长齿轮寿命
末端执行器高频振动	振动被抑制
适用：低速、非精密任务	适用：高速、高精度、长寿命

参数	典型值	允许偏差	超差后果
焊接速度	8-15 mm/s	\(\pm 5\%\)	焊缝宽度不均、强度下降
送丝速度	3-8 m/min	与行走速度匹配	过多→堆积；过少→未熔合
焊枪姿态偏差	-	\(\pm 3°\)	焊缝偏移、气孔
弧长（电压）	18-26 V	\(\pm 1\) V	飞溅增多、焊缝成型差

后果	物理机制	工程影响
电机发热增加	高 \(dI/dt\) 产生涡流损耗	缩短电机寿命、需要降额运行
减速器冲击	齿轮啮合力突变	齿面磨损加速、游隙增大、精度下降
结构振动	弹性变形波沿连杆传播	末端位置精度下降
噪声	冲击力激发机械共振	工作环境噪声超标

指标	含义	单位
\(T\)	总执行时间	s
\(t_{compute}\)	参数化计算时间	us
$\max	j_i	$
\(\sigma_v\)	末端速度标准差（均匀性）	mm/s
\(\Delta\tau_{peak}\)	力矩变化率峰值	Nm/s

指标	TOTG	TOPP-RA	Ruckig Filter
执行时间 \(T\)	2.35 s	2.28 s	2.51 s
计算时间 \(t_{compute}\)	0.8 ms	1.2 ms	轨迹后处理为 ms 级，随路径点数变化
最大 jerk	\(\infty\)（跳变）	\(\infty\)（跳变）	8500 rad/s\(^3\)
力矩变化率峰值	高（冲击）	高（冲击）	低（平滑）
抗扰动能力	无（离线）	无（离线）	无（MoveIt2 后处理）；在线 OTG 模式才支持实时重算