🔢 mathematics

Optimized and kinematically feasible multi-agent motion planning

本文提出了一种用于优化且运动学可行的多智能体运动规划的两步框架，该框架结合了冲突基搜索等算法生成的初始可行解与后续的多阶段最优控制改进步骤，并在拖拉机 - 拖车系统上证明了其有效性，其中冲突基搜索优于基于优先级的搜索，而基于晶格的规划器则超越了安全区间路径规划。

原作者： Anja Hellander, Kristoffer Bergman, Daniel Axehill

发布于 2026-05-05

📖 1 分钟阅读🧠 深度阅读

CC BY 4.0

原作者： Anja Hellander, Kristoffer Bergman, Daniel Axehill

原始论文采用 CC BY 4.0 许可（http://creativecommons.org/licenses/by/4.0/）。 ✨ 这是对下方论文的AI生成解释。它不是由作者撰写或认可的。如需技术准确性，请参阅原始论文。阅读完整免责声明

想象一下，你是一家停满巨型铰接式卡车（类似拖拉机牵引长挂车）的繁忙停车场的交通指挥员。你的任务是指示每辆卡车如何从起点精确移动到目的地，同时避免撞墙或相互碰撞。

这是一个难题，因为这些卡车的移动方式不像网格上的简单点；它们具有复杂的物理特性。它们无法瞬间停止，无法原地急转，而且如果挂车撞到墙壁，整辆卡车都会被困住。

本文的作者提出了一种两步“规划与优化”策略，以高效地解决这一问题。

第一步：草稿（“草图”）

首先，计算机需要一个快速且安全的计划。它无法立即求解完美的物理方程，因为那太耗时。相反，它采用了一种“离散化”方法。

这就像玩棋盘游戏。计算机不是让卡车在任何方向上平滑移动，而是强制它们仅沿着特定的、预先计算好的“步法”移动（就像国际象棋中的马）。

工具：他们使用了一种“基于晶格的规划器”。想象一下由隐形踏脚石组成的网格。计算机通过从一个踏脚石跳到另一个踏脚石来寻找路径。
冲突：当多辆卡车在棋盘上时，它们可能会试图同时踏上同一块踏脚石。为了解决这个问题，本文比较了两种决定谁先行的方法：
- CBS（基于冲突的搜索）：就像一名裁判，它观察比赛，发现碰撞，并说：“你们两个不能同时在这里；其中一个必须等待或走另一条路。”它会持续这样做，直到所有人都安全。
- PBS（基于优先级的搜索）：就像咖啡店的排队。计算机选择一个优先级顺序（卡车 A 先走，然后是卡车 B）。后面的卡车将前面的卡车视为移动障碍物，并围绕它们进行规划。

令人惊讶的发现：
作者原本预期一种更复杂的算法SIPP-IP（它在“安全区间”内处理时间）会是最佳方案。然而，对于这些大型卡车而言，简单的基于晶格的规划器实际上效果更好。

为什么？ SIPP-IP 过于谨慎。它就像一个保安，会说：“如果你的卡车的任何部分可能碰到墙壁，你就不能走。”而晶格规划器稍微宽松一些，它检查卡车是否实际*与墙壁重叠，从而允许更平滑、更快速的路径。

第二步：优化（“平滑”）

第一步生成的“草稿”是安全的，但看起来很生硬。就像机器人因为被迫在网格踏脚石上跳跃，而进行一系列尖锐的 90 度转弯。

现在，计算机将这条粗糙的路径输入到一个数学优化器（最优控制问题求解器）中。

类比：想象你用锯齿状的蜡笔画了一幅粗糙的道路草图。第二步利用高科技平滑工具，将这幅草图变成一条完美、流畅的高速公路。
技巧：计算机将粗糙的草图作为“热启动”。它不是从头开始，而是仅微调现有路径，使其更平滑、更快、更省油，同时确保卡车仍遵守物理定律。

“时间同步”的秘诀

为了让第一步有效工作，作者必须发明一种创建这些“踏脚石”（运动基元）的新方法。

通常，一个动作可能耗时 1.2 秒，另一个耗时 1.7 秒。这使得很难检查两辆卡车是否会相撞。
作者强制所有动作都进行时间同步。每个动作都是微小固定时间片（如 0.1 秒）的倍数。
类比：想象一支行进乐队。不是每个人按自己的速度行进，而是每个人都严格踩着节拍迈步。这使得判断两名乐队成员是否即将相撞变得极其容易。

他们的发现

他们在计算机模拟中测试了该方法，场景是在 200x200 米的区域内有 2 到 5 套拖拉机 - 挂车系统。

规划器：简单的“晶格”规划器比复杂的"SIPP-IP"方法更快，并且找到了更多成功的路径，特别是在存在障碍物的情况下。
冲突求解器：
- 在空旷房间中，“优先级”方法（PBS）比“裁判”方法（CBS）解决了更多问题。
- 在充满障碍物的房间中，“裁判”方法（CBS）更快且更成功。
结果：经过“优化”步骤后，两种方法产生的路径质量非常相似。粗糙的草稿不如最终的平滑步骤重要。

总结

本文提出了一种系统，首先利用基于网格的游戏方法找到一条安全、粗糙的路径（这对大型卡车而言效果优于预期），然后利用高级数学将其平滑化。这就像先聘请一位速写艺术家绘制路线草图，再聘请一位大师级雕塑家将该草图 refinement 成一条完美、无碰撞的轨迹。

技术摘要：优化且运动学可行的多智能体运动规划

问题表述

本文针对具有复杂运动学的自主系统（特别是牵引车 - 挂车系统）的多智能体运动规划（MAMP）问题展开研究。与通常假设离散网格上的点状智能体及简化动力学的标准多智能体路径搜索（MAPF）不同，本工作将智能体视为由 $\dot{x}_i(t) = f_i(x_i(t), u_i(t))$ 描述的非线性动力学系统。其目标是在包含障碍物 $O$ 的工作空间 $W$ 内，为 $k$ 个智能体寻找一组从初始状态 $x_{i,0}$ 到最终状态 $x_{i,f}$ 的可行、无碰撞且优化的轨迹。

该优化问题旨在最小化个体代价函数之和 $J_i = \int_{t_0}^{t_f} l(x_i(t), u_i(t)) dt$ ，并受限于以下条件：

系统动力学约束。
状态约束（避免障碍物和工作空间边界）。
控制约束。
智能体间防碰撞约束（ $R_i(x_i) \cap R_j(x_j) = \emptyset$ ）。

作者指出，由于路线选择组合的复杂性、系统的非线性以及智能体间的优先级冲突，寻找全局最优解是 NP 完全问题。

方法论

所提出的框架采用两步法，旨在平衡计算可行性、解的质量以及运动学可行性。

第一步：初始可行解

第一步利用离散搜索方法生成初始可行且无碰撞的解。

多智能体搜索：该框架利用基于冲突的搜索（CBS）或基于优先级的搜索（PBS）。这些方法将 MAMP 问题分解为单智能体规划问题，通过添加约束（CBS）或探索优先级顺序（PBS）来解决冲突。
单智能体规划器：作者评估了两种单智能体规划器：
1. 扩展的 SIPP-IP（带区间投影的安全间隔路径规划）：经过修改以处理通用代价函数和更大的智能体（占据多个网格单元）。它使用时间间隔来表示安全状态。
2. 基于晶格的规划器：在离散化且附加时间维度的状态空间上使用图搜索（A*）。
运动基元：一个关键组件是生成时间同步的运动基元。作者提出了一种生成最优运动学可行基元的方法，其中每个基元的持续时间都是固定采样时间 $t_s$ 的整数倍。这种同步对于 SIPP-IP 和基于晶格的规划器中高效的碰撞检测是必需的。

第二步：连续优化

第二步利用数值最优控制对离散解进行细化。

多阶段最优控制问题（OCP）：初始离散解被用于“热启动”多阶段最优控制问题（OCP）的数值求解器。
阶段结构：为了处理具有不同到达时间（ $t_{i,f}$ ）的智能体，问题被划分为 $M=k$ 个阶段。第 $m$ 个阶段覆盖智能体 $m-1$ 和智能体 $m$ 达成目标之间的时间间隔。这种结构允许在每个阶段内对碰撞检测进行一致的时间离散化，同时允许不同智能体具有不同的总持续时间。
优化：求解器优化连续轨迹和控制输入，消除离散化伪影，并确保严格的运动学可行性。

主要贡献

两步 MAMP 框架：一种混合方法，结合离散搜索（CBS/PBS）以实现全局可行性，并结合连续最优控制（OCP）以实现局部优化和运动学平滑。
时间同步基元生成：一种新颖算法（算法 1），用于生成持续时间被约束为公共采样时间整数倍的最优运动基元，从而促进同步的碰撞检测。
SIPP-IP 的扩展：对 SIPP-IP 算法进行了修改，以支持通用代价函数以及具有复杂几何形状（占据多个网格单元）的智能体，而不仅仅是单点。
多阶段 OCP 公式：一种特定的 MAMP 改进步骤公式，通过将问题划分为顺序时间阶段来处理异构智能体的到达时间。

实验结果

该框架在牵引车 - 挂车系统（2 挂车配置）上进行了评估，环境包括无障碍物和无障碍丰富的场景。

单智能体性能：
- 在无障碍物环境中，基于晶格的规划器速度显著更快（0.008 秒对比 0.12 秒），且产生的代价略低于扩展的 SIPP-IP。
- 在包含障碍物的环境中，基于晶格的规划器仍然更快，并产生了代价降低约 14% 的解。作者将 SIPP-IP 较高的代价和较低的成算率归因于其“保守”的碰撞检测（基于网格单元），这导致其丢弃了基于晶格的规划器（使用圆对圆检测）所接受的可行路径。
多智能体性能（CBS 对比 PBS）：
- 成功率：在无障碍物环境中，PBS 解决的 problem 数量多于 CBS。然而，在包含障碍物的环境中，CBS 实现了更高的成功率和更低的平均运行时间。
- 解的质量：在优化步骤之后，两种规划器产生的解质量相似。虽然 CBS 最初找到的解代价略低（得益于其最优性保证），但连续优化步骤往往消除了这种优势，甚至略微逆转了这一局面。
- 可扩展性：随着智能体数量的增加，两种规划器成功解决的问题数量均有所下降，但在复杂且充满障碍物的场景中，CBS 保持了性能优势。

意义与主张

本文主张，对于像牵引车 - 挂车这样复杂的运动学系统，结合 CBS 或 PBS 的基于晶格的规划器优于理论上更稳健的 SIPP-IP 方法，这主要归功于其较不保守的碰撞检测。所提出的两步框架成功弥合了在合理时间内找到可行解与获得运动学可行且优化的轨迹之间的差距。

作者谦逊地指出，虽然 PBS 可以在开阔空间中解决更多问题，但 CBS 在杂乱环境中更为优越。他们得出结论，初始离散解的质量并不严格决定最终优化后的代价，因为连续优化步骤可以显著改变结果。未来的工作建议侧重于通过分布式优化提高优化步骤的计算效率，并纳入交通规则。