Many-RRT*: Robust Joint-Space Trajectory Planning for Serial Manipulators

本文提出了一种名为 Many-RRT* 的鲁棒关节空间轨迹规划算法，通过并行规划多个逆运动学解来克服高自由度机械臂因多解性导致的规划失败或次优问题，实验表明其在保持运行效率的同时显著提升了规划成功率与轨迹质量。

要点

这篇论文介绍了一种让机器人手臂更聪明、更灵活地规划动作的新方法，叫做 Many-RRT⋆。

为了让你轻松理解，我们可以把机器人手臂想象成一个在复杂迷宫里找路的盲人探险家，而这篇论文就是教它如何不再“撞墙”，而是能同时尝试多条路线，最终找到最完美的路径。

以下是用大白话和生动比喻对这篇论文的解读：

1. 核心难题：为什么机器人会“迷路”？

想象一下，你让机器人把手伸到桌子上的一个杯子（这是任务空间的目标）。

• 问题在于：机器人有很多关节（比如 6 个或 7 个），就像人的手臂有肩膀、手肘、手腕一样。
• 多解性：要把手放到杯子上，机器人可以有无数种姿势。它可以把手肘高高举起，也可以把手肘低垂，甚至可以把身体扭成麻花状。在数学上，这叫“正运动学不可逆”——一个目标位置，对应无数个关节姿势。

传统的做法（RRT⋆）：
以前的机器人就像是一个固执的赌徒。它随机猜一个姿势作为“终点”，然后开始画路。

• 风险：如果它运气不好，猜了一个“死胡同”的姿势（比如那个姿势被墙壁挡住了，或者需要扭断腰才能做到），它就会在那条死路上浪费大量时间，甚至直接放弃，明明旁边就有条好路，但它没看见。
• 比喻：就像你要去火车站，你只选了一条路走，结果那条路修路封了，你只能掉头重来，或者干脆迟到。

2. 新方案：Many-RRT⋆（“多臂”策略）

这篇论文提出的 Many-RRT⋆ 方法，核心思想是：不要只赌一条路，要同时开多条路！

• 多目标并行：
  当机器人要把手伸向杯子时，它不再只猜一个终点姿势，而是同时猜出 10 个、20 个甚至更多不同的终点姿势（只要它们都能碰到杯子）。
• 多树齐发：
  想象一下，机器人派出了10 个探险小分队。
  • 一个小分队从起点出发，往终点跑。
  • 另外 10 个小分队分别从 10 个不同的“终点姿势”出发，往起点跑。
  • 它们同时在迷宫里探索，互不干扰。
• 优胜劣汰：
  只要其中任何一个小分队成功找到了路，或者找到了最短的路，系统就立刻宣布：“任务完成！”
  • 比喻：就像你让 10 个朋友同时去不同的路口找路去火车站。只要有一个朋友说“我找到路了，而且很快”，你就成功了。你不需要每个人都找到路，也不需要每个人都走对路。

3. 为什么这个方法这么厉害？

论文通过实验证明了它的三大优势：

1. 成功率极高（不再轻易放弃）：

   • 旧方法：在复杂的迷宫（比如有很多障碍物的狭窄通道）里，如果猜错了终点姿势，成功率可能只有 1.6%（几乎必死）。
   • 新方法：因为同时试了很多姿势，成功率直接飙升到 100%。就像你派了 10 个人去探路，总有一个能活着回来。

2. 路径更优（走得更省力）：

   • 旧方法：即使找到了路，也可能是一条绕远路、动作很别扭的路。
   • 新方法：因为它比较了所有可能的路径，它能挑出最省力、最顺畅的那一条。实验显示，在同样的时间里，它找到的路径成本（比如动作幅度、能量消耗）降低了 44.5%。
   • 比喻：旧方法可能让你绕远路去上班，新方法直接给你规划了一条不堵车的最快路线。

3. 速度不慢（并行计算）：

   • 你可能会问：“同时跑 10 条路，会不会更慢？”
   • 答案：不会。现在的电脑芯片有很多核心（就像有很多个大脑），新方法利用这些核心并行工作。虽然它在算 10 条路，但因为它是在同一时间内算完的，所以总耗时并没有比算 1 条路多太多，甚至因为不用反复试错，反而更快。

4. 总结

Many-RRT⋆ 就像是一个拥有“分身术”的超级机器人规划师。

• 以前：机器人像个单线程的傻瓜，撞了南墙才回头，容易在复杂环境里卡死。
• 现在：机器人学会了“广撒网”，同时尝试多种可能的姿势和路径。它利用现代电脑的多核能力，瞬间比较所有可能性，确保既能找到路（成功率高），又能找到最好的路（质量高），而且速度还很快。

这项技术对于未来让机器人手臂在工厂、医院或灾难现场等复杂环境中灵活工作，具有非常重要的意义。它解决了机器人“手长脚长”但“脑子容易转不过弯”的痛点。

技术摘要

论文技术总结：Many-RRT⋆：用于串联机械臂的鲁棒关节空间轨迹规划

1. 研究背景与问题陈述 (Problem Statement)

随着高自由度（DoF）串联机械臂的快速发展，需要在高维空间中使用快速、基于采样的运动规划器。尽管像快速探索随机树（RRT）及其变体（如 RRT*）这样的采样规划器已被广泛应用，但在**关节空间（Configuration Space）**进行规划时面临重大挑战：

• 正向运动学的不可逆性：串联机械臂的正向运动学函数 $f(q)$ 是非线性的且不可逆。这意味着任务空间中的一个单一末端执行器位姿（Pose）可能对应关节空间中的多个构型（Configurations）。
• 多臂老虎机问题（Multi-armed Bandit Problem）：由于上述特性，规划器在选择目标关节构型时面临“多臂老虎机”困境。如果规划器错误地选择了一个次优的或不可达的目标关节构型（即使该构型在任务空间上是正确的），可能导致：
  • 规划失败（无法找到可行路径）。
  • 生成的轨迹次优（成本过高）。
• 现有方法的局限性：
  • 传统的 RRT* 或 RRT*-Connect 通常只针对单个目标关节构型进行规划。
  • 穷举所有可能的逆运动学（IK）解在计算上是不可行的（对于冗余机械臂，解空间可能是连续流形）。
  • 简单的 IK 求解器（如基于雅可比矩阵伪逆的方法）容易陷入局部极小值，且严重依赖初始种子点，导致生成的单一目标构型可能位于被障碍物隔离的连通分量中（即从起点无法到达）。

2. 方法论：Many-RRT⋆ (Methodology)

为了解决上述问题，作者提出了 Many-RRT⋆，这是一种扩展了 RRT*-Connect 的算法，旨在并行规划多个目标构型。

核心思想

将运动规划问题重新表述为在配置空间和**正向运动学原像（Preimage）**上的搜索。不再寻找单一的目标点 $q_N$，而是寻找一组目标构型 $Q_g = \{q \in Q \mid g = f(q)\}$ 中的最优解。

算法流程

1. 多 IK 解采样 (Sampling IK Solutions)：

   • 不直接求解单一 IK，而是从自由空间 $C_{free}$ 中通过拒绝采样生成大量无标签的关节构型集合 $M$。
   • 构建 KD-Tree 以加速查询。
   • 利用这些构型作为种子，并行求解 $K$ 个约束优化问题（公式 5），生成 $K$ 个不同的 IK 解。
   • 对解进行下采样（Down-sampling），去除距离过近的冗余解，得到一组多样化的目标构型 $\{q'_0, ..., q'_N\}$。

2. 并行树生长 (Parallel Tree Generation)：

   • 目标树：为每个采样得到的目标构型 $q'_i$ 启动一个独立的 RRT* 树，从目标向起点生长。
   • 起点树：同时启动一个从起点 $q_0$ 生长的 RRT* 树。
   • 异步并行执行：所有树在多线程上异步独立生长。
   • 连接策略：起点树不仅探索自由空间，还以一定概率尝试连接到各个目标树中的节点或目标构型本身（公式 10）。

3. 重布线与优化 (Rewiring)：

   • 所有树均遵循 RRT* 的重布线逻辑，确保渐近最优性。
   • 算法持续运行直到找到连接，并保留当前找到的最佳路径。

理论保证

• 全局渐近最优性：随着从正向运动学原像中采样的样本数量增加，Many-RRT⋆ 从局部渐近最优转变为全局渐近最优。
• 并行性：由于各目标树相互独立，该算法在现代硬件（多核 CPU/GPU）上具有天然的并行性，且不会显著增加规划时间。

3. 主要贡献 (Key Contributions)

1. 形式化全局最优运动规划：

   • 明确了基于采样的规划器（如 RRT*）在配置空间规划中无法保证全局最优的原因（正向运动学的多对一映射）。
   • 提出了在配置空间原像上进行搜索以实现全局最优的框架。

2. 解决“一对多”运动规划问题：

   • 提出了一种从正向运动学原像中采样并独立优化每条路径的方法。
   • 证明了随着样本量增加，规划结果趋近于全局最优。
   • 实现了高度并行化，在保持与现有最先进实现（SOTA）相当的规划时间的同时，显著提升了成功率。

3. 实验验证：

   • 在多种机器人形态（6-DoF UR10e, 7-DoF Franka Panda）和复杂环境（表格、墙壁、狭窄通道、随机障碍物）下进行了广泛测试。

4. 实验结果 (Results)

实验在 500 次试验中对比了 RRT*、RRT*-Connect 和 Many-RRT⋆。

• 成功率 (Success Rate)：
  • 在复杂环境（如“随机 Random"环境）中，传统 RRT* 和 RRT*-Connect 的成功率极低（分别为 1.4% 和 1.6%）。
  • Many-RRT⋆ 实现了 100% 的成功率（在 6-DoF 和 7-DoF 的多数测试中），显著优于基线。
• 路径成本 (Path Cost)：
  • 在相同的运行时间内，Many-RRT⋆ 生成的轨迹成本平均降低了 44.5%。
  • 在“随机”环境中，Many-RRT⋆ 收敛到可行解所需的迭代次数远少于基线（7-DoF 机械臂减少了 6 倍）。
• 运行时间 (Runtime)：
  • 尽管并行计算了多个树，但由于利用了多线程，Many-RRT⋆ 的总运行时间与单线程或双树算法相比没有显著增加，甚至在某些情况下更优。
• 收敛性：
  • 如图 4 所示，Many-RRT⋆ 不仅更快找到可行路径，而且随着迭代次数增加，其路径成本持续下降并显著低于其他算法。

5. 意义与结论 (Significance & Conclusion)

• 解决高维规划痛点：Many-RRT⋆ 有效解决了高自由度机械臂在配置空间规划中因正向运动学不可逆导致的“选错目标”问题。
• 鲁棒性与最优性：该方法在不牺牲计算效率的前提下，同时实现了高成功率和全局渐近最优性。
• 工程应用价值：对于需要在复杂、狭窄或高度约束环境中工作的工业机械臂（如装配、搬运），该算法能显著提高规划系统的鲁棒性，避免因规划失败导致的任务中断。
• 未来方向：该方法易于扩展到 GPU 或 NPU 加速，且其并行架构非常适合现代计算硬件。

总结：Many-RRT⋆ 通过将“寻找单一目标”转变为“并行探索多个潜在目标”，巧妙地规避了传统规划器在配置空间中的局部最优陷阱，为高自由度串联机械臂提供了一种高效、鲁棒且理论上最优的运动规划解决方案。