FOCI: Trajectory Optimization on Gaussian Splats
作者: Mario Gomez Andreu, Maximum Wilder-Smith, Victor Klemm, Vaishakh Patil, Jesus Tordesillas, Marco Hutter
分类: cs.RO
发布日期: 2025-05-13 (更新: 2025-07-30)
备注: 8 pages, 8 figures, Mario Gomez Andreu and Maximum Wilder-Smith contributed equally
💡 一句话要点
FOCI：提出基于高斯球的轨迹优化算法，解决机器人狭窄空间运动规划问题
🎯 匹配领域: 支柱一：机器人控制 (Robot Control) 支柱三：空间感知与语义 (Perception & Semantics)
关键词: 轨迹优化 高斯溅射 碰撞检测 机器人运动规划 四足机器人
📋 核心要点
- 现有方法使用保守边界框进行碰撞检测，低估了环境的可穿越性，限制了机器人在复杂环境中的运动能力。
- FOCI算法将环境和机器人表示为高斯球，利用高斯重叠积分进行碰撞检测，实现更精确的碰撞避免。
- 实验表明，FOCI算法能够在几秒钟内为ANYmal四足机器人生成无碰撞轨迹，即使在包含数十万个高斯的环境中。
📝 摘要（中文）
本文提出了一种名为FOCI（Field Overlap Collision Integral）的算法，该算法能够直接在高斯球上优化轨迹，利用3D高斯溅射（3DGS）中编码的空间信息。FOCI利用高斯之间的重叠积分概念，为3DGS提出了一种新颖且可解释的碰撞公式。与其他使用保守边界框低估环境可穿越性的方法不同，我们建议将环境和机器人表示为高斯球。这不仅具有理想的计算特性，而且允许感知方向的规划，使机器人能够通过非常狭窄的空间。我们在合成和真实的高斯球中广泛测试了我们的算法，展示了ANYmal四足机器人的无碰撞轨迹可以在几秒钟内计算出来，即使环境由数十万个高斯组成。
🔬 方法详解
问题定义：现有机器人运动规划方法，特别是针对复杂三维环境，通常采用保守的碰撞检测策略，例如使用边界框近似机器人和环境。这种方法的痛点在于，它低估了环境的可穿越性，导致规划出的轨迹可能过于保守，无法充分利用环境中的狭窄空间，限制了机器人的运动能力。尤其是在高动态、高复杂度的场景下，这种限制更为明显。
核心思路：FOCI的核心思路是将环境和机器人均表示为高斯球（Gaussian Splats），并利用高斯球之间的重叠积分来精确计算碰撞概率。通过优化轨迹，使得机器人高斯球与环境高斯球的重叠积分最小化，从而实现无碰撞的轨迹规划。这种方法避免了传统边界框近似带来的误差，能够更准确地评估碰撞风险。
技术框架：FOCI算法的整体流程如下：1) 使用3D高斯溅射（3DGS）重建环境，得到环境的高斯球表示。2) 将机器人也表示为一组高斯球。3) 定义基于高斯球重叠积分的碰撞代价函数。4) 使用优化算法（如梯度下降）优化机器人的轨迹，使得碰撞代价函数最小化。5) 输出无碰撞的机器人轨迹。
关键创新：FOCI最重要的技术创新点在于提出了基于高斯球重叠积分的碰撞检测方法。与传统的基于边界框的碰撞检测方法相比，该方法能够更精确地评估碰撞风险，允许机器人更安全地通过狭窄空间。此外，将环境和机器人统一表示为高斯球，简化了碰撞检测的计算，提高了规划效率。
关键设计：FOCI的关键设计包括：1) 高斯球的参数化表示，包括中心位置、协方差矩阵等。2) 高斯球重叠积分的计算公式，用于量化两个高斯球之间的重叠程度。3) 碰撞代价函数的定义，通常是重叠积分的加权和，权重可以根据高斯球的重要性进行调整。4) 优化算法的选择，例如使用梯度下降法迭代优化轨迹，直到碰撞代价函数收敛。
🖼️ 关键图片
📊 实验亮点
实验结果表明，FOCI算法能够在几秒钟内为ANYmal四足机器人生成无碰撞轨迹，即使在包含数十万个高斯的环境中。与传统的基于边界框的碰撞检测方法相比，FOCI算法能够更有效地利用环境中的狭窄空间，提高机器人的运动效率。此外，FOCI算法在真实环境中的实验也取得了良好的效果，验证了其在实际应用中的可行性。
🎯 应用场景
FOCI算法在机器人运动规划领域具有广泛的应用前景，尤其适用于需要在复杂、狭窄环境中进行操作的机器人，例如四足机器人、无人机、机械臂等。该算法可以应用于搜索救援、工业巡检、医疗手术等场景，提高机器人的自主性和安全性。未来，FOCI算法可以与强化学习等技术结合，实现更智能、更高效的机器人运动规划。
📄 摘要（原文）
3D Gaussian Splatting (3DGS) has recently gained popularity as a faster alternative to Neural Radiance Fields (NeRFs) in 3D reconstruction and view synthesis methods. Leveraging the spatial information encoded in 3DGS, this work proposes FOCI (Field Overlap Collision Integral), an algorithm that is able to optimize trajectories directly on the Gaussians themselves. FOCI leverages a novel and interpretable collision formulation for 3DGS using the notion of the overlap integral between Gaussians. Contrary to other approaches, which represent the robot with conservative bounding boxes that underestimate the traversability of the environment, we propose to represent the environment and the robot as Gaussian Splats. This not only has desirable computational properties, but also allows for orientation-aware planning, allowing the robot to pass through very tight and narrow spaces. We extensively test our algorithm in both synthetic and real Gaussian Splats, showcasing that collision-free trajectories for the ANYmal legged robot that can be computed in a few seconds, even with hundreds of thousands of Gaussians making up the environment. The project page and code are available at https://rffr.leggedrobotics.com/works/foci/