cuRobo

大多数方法都是一条轨迹慢慢算， cuRobo 用 GPU 一次算很多条轨迹（多seed）, 流程可分三段：

  1. 并行求解多组碰撞可行 IK → 获取末端位姿可达的末关节解
  2. 从这些终点生成很多条初始轨迹: 种子轨迹生成 → 线性插值 / 回撤点 / 几何规划
  3. 每条轨迹在 GPU 上同时优化，选出最优的那条: 并行轨迹优化 → 粒子搜索快速跳到好位置 + 并行 L-BFGS 精修收敛
Goal Pose X_g
      │
      ▼
[Parallel Collision-Free IK]  --> 多个末端可达解
  - 每个线程块：一批种子位姿
  - 线程：并行 FK / 距离 / 雅可比
      │
      ▼
[Seed Generator: Linear / Retract / Geometric Plan]
  - 线性插值 / 回撤点 / 几何规划 (batched)
      │
      ▼
[Particle Optimization] --(few iters)--> 好的初始区
  - 多种子并行扰动 + 代价下降筛选
  - 内含：任务/平滑/连续碰撞代价与梯度
      │
      ▼
[Parallel L-BFGS + Noisy Line Search]
  - 每条轨迹一个“小优化器”
  - 批量两环递推 + 并行多步长评估
      │
      ▼
[Timestep Optimization (dt Scaling)]
  - 时间标定满足 v/a/jerk 限制
  - 少量迭代精修
      │
      ▼
Collision-Free Minimum-Jerk Trajectory

整个 pipeline 从输入目标位姿到输出平滑无碰轨迹，全程在 GPU 上批处理多个候选解, 这种“多种子并行搜索”让它在几十毫秒内就能找到可行解

优化目标

整个运动看作一系列离散的时间步：

\[q_0, q_1, ..., q_T\]

其中 $q_t$ 表示在$t$时刻的关节角度。

目标为：

\[\min_{q_t} \left ( C_{task} +C_{smooth} +C_{collision} \right )\]

其中 $ C_{task} $ 为末端执行器到达目标位置的误差；$ C_{smooth} $ 为轨迹平滑度（速度、加速度、jerk越小越好）；$ C_{collision} $ 为与障碍物的距离惩罚（越近代价越大）。

约束

碰撞约束

在运动规划中，要避免两种碰撞, $C_{collision}$可以进一步拆解为$C_{\text{self}} + C_{\text{world}}$：

  1. 自碰（机器人自己不同部件之间的碰撞）
  2. 外碰（机器人和环境障碍物之间的碰撞）

自碰撞

把机器人简化成一组球（Sphere Approximation），自碰就等价于球-球之间的间距小于半径和

\[C_{\text{self}}(q_t) = w_{\text{self}} \max_{(i,j) \in \mathcal{P}} \left[0,\; r_i + r_j - \|p_i(q_t) - p_j(q_t)\|\right]\]

其中 $ P $ 为要检查的球对集合（过滤掉不可能碰撞的）； $ r_i, r_j $ 为球半径； $ p_i\left( q_t \right) $ 为 $ t $ 时刻，第 $ i $ 个球的位置。

外碰撞

环境碰撞检测用的是点到障碍物的有符号距离 $d$，$d>0$在障碍外，$d<0$在障碍内。

为了让优化更平滑，引入缓冲距离$\epsilon$，在缓冲区内（靠近障碍）代价呈二次增长:

\[\phi(d) = \begin{cases} 0, & d \ge \epsilon, \\ \frac{1}{2\epsilon} (\epsilon - d)^2, & d < \epsilon \end{cases}\]

外碰撞代价：

\[C_{\text{world}}(q_t, \dot{q}_t) = \sum_{s \in \text{spheres}} \|v_s(q_t)\| \cdot \phi\big(d_s(q_t)\big)\]

其中 $v_s(q_t)$ 为球的速度, 速度因子能防止优化器“高速穿越”障碍物区域

连续碰撞检测

如果只在相邻离散时间步检查，物体可能在中间穿过障碍物而被漏检（比如细杆子）

连续检测：将物体从起点到终点的运动轨迹建模为连续函数，然后求解它与障碍物在区间内的最小距离或首次相交时刻

解决方法：

  1. 假设球心在两个时间点之间是直线运动，对相邻时间步的球心位置做线性插值, 形成“扫掠”轨迹
  2. 环境表示为有符号距离场（Signed Distance Field），可来自网格、体素或深度图。球心沿插值轨迹变化的连续轨迹在多个采样点上扫掠检查 SDF, , 判断是否进入缓冲区, 直到中点。
  3. 当距离小于安全缓冲$\epsilon$时，进入二次惩罚函数$\phi(d)$；积分所有采样点的惩罚值，形成该段的碰撞代价。提前发现并记录进入障碍的那一刻
  4. 同时计算代价对球心位置的梯度，再通过雅可比链式法则回传到关节变量，用于梯度下降 / L‑BFGS 优化

平滑约束

轨迹的平滑性惩罚基于速度、加速度、jerk：

\[C_{\text{smooth}} = w_v \sum_t \|\dot{q}_t\|^2 + w_a \sum_t \|\ddot{q}_t\|^2 + w_j \sum_t \|\dddot{q}_t\|^2\]

总的优化目标为

\[\min_{\{q_t\}} \; C_{\text{task}} + C_{\text{smooth}} + C_{\text{self}} + C_{\text{world}}\]

满足动力学约束

\[q_{\min} \le q_t \le q_{\max}, \|\dot{q}_t\| \le v_{\max},\; \|\ddot{q}_t\| \le a_{\max},\; \|\dddot{q}_t\| \le j_{\max}\]

优化

粒子优化

Particle-Based Optimization

每个种子轨迹视作“粒子”, 并行评估所有粒子的代价（任务 + 平滑 + 碰撞）, 每个粒子独立进行小范围随机扰动，测试是否能减少代价

优点：快速跳出劣质局部最优，把解送到更优的收敛区

\[q_t^{(k+1)} = q_t^{(k)} + \eta \cdot \delta_t\]

其中 $\delta_t$ 为带噪声的改进方向

并行L-BFGS

L-BFGS 属于拟牛顿法，用历史梯度-位移对来近似 Hessian 逆阵，内存占用小

在 GPU 上为每条轨迹（粒子优化产出的种子）单独跑一个 batched L-BFGS

每步迭代都用并行 noisy line search 选择步长，这个策略鲁棒且适合 SIMD 结构

几十上百条轨迹同时收敛，每条只需极少迭代