Real-Time Fast Marching Tree for Mobile Robot Motion Planning in Dynamic Environments

📄 arXiv: 2502.09556v1 📥 PDF

作者: Jefferson Silveira, Kleber Cabral, Sidney Givigi, Joshua A. Marshall

分类: cs.RO, eess.SY

发布日期: 2025-02-13

备注: This is the preprint version of the paper published in 2023 IEEE International Conference on Robotics and Automation (ICRA). The final version is available at IEEE Xplore: https://doi.org/10.1109/ICRA48891.2023.10160595

期刊: Proc. IEEE ICRA, 2023, pp. 7837-7843

DOI: 10.1109/ICRA48891.2023.10160595

💡 一句话要点

提出RT-FMT算法，用于动态环境中移动机器人的实时运动规划

🎯 匹配领域: 支柱一：机器人控制 (Robot Control)

关键词: 运动规划 实时规划 动态环境 移动机器人 快速行进树

📋 核心要点

  1. 现有实时运动规划方法在动态环境中难以兼顾全局最优和快速响应，导致机器人行动迟缓或陷入局部最优。
  2. RT-FMT算法融合了FMT和RT-RRT的优点，通过局部路径快速启动和全局路径优化，实现动态环境下的高效规划。
  3. 仿真结果表明，RT-FMT在执行成本和到达时间上优于RT-RRT*，验证了局部路径优先策略的有效性。

📝 摘要（中文）

本文提出了一种名为实时快速行进树（RT-FMT）的实时规划算法，该算法具有局部和全局路径生成、多查询规划和动态避障功能。在搜索过程中，RT-FMT快速寻找全局解决方案，同时生成可供机器人更快启动执行的局部路径。此外，我们的算法不断重连树结构，以防止分支在动态障碍物内部形成，并保持树根靠近机器人，这使得该树可以多次重复使用于不同的目标。我们的算法基于快速行进树（FMT）和实时快速探索随机树（RT-RRT）规划器。通过仿真表明，在大多数情况下，RT-FMT在执行成本和到达时间方面均优于RT-RRT*。此外，我们还通过仿真证明，为了减少到达时间，即使存在采取次优路径的小概率，在全局路径可用之前采取局部路径也是值得的。

🔬 方法详解

问题定义：论文旨在解决动态环境中移动机器人的实时运动规划问题。现有方法，如RT-RRT*，虽然能进行实时规划，但在寻找全局最优解方面效率较低，导致机器人可能采取次优路径或花费较长时间才能到达目标。因此，需要在保证实时性的前提下，提高规划路径的质量和效率。

核心思路：RT-FMT的核心思路是结合局部快速规划和全局优化。算法首先生成局部路径，使机器人能够快速启动并移动，同时并行地搜索全局最优路径。通过不断重连树结构，算法能够适应动态环境的变化，并保持树根靠近机器人，从而实现多查询规划。

技术框架：RT-FMT算法的整体框架包含以下几个主要阶段：1) 初始化：构建初始树结构，树根位于机器人当前位置。2) 局部路径生成：基于当前树结构，快速生成一条局部路径，供机器人立即执行。3) 全局路径搜索：并行地进行全局路径搜索，优化树结构，寻找到达目标的更优路径。4) 动态障碍物处理：实时检测动态障碍物，并重连树结构，避免分支进入障碍物区域。5) 树结构维护：保持树根靠近机器人，以便在目标改变时快速进行重新规划。

关键创新：RT-FMT的关键创新在于融合了局部快速规划和全局优化，并采用动态重连树结构的方法来适应动态环境。与RT-RRT*相比，RT-FMT能够更快地找到全局最优解，并减少机器人的到达时间。此外，RT-FMT的多查询规划能力使其能够适应目标位置的变化。

关键设计：RT-FMT的关键设计包括：1) 局部路径生成策略：采用快速行进方法生成局部路径，保证实时性。2) 全局路径搜索策略：基于FMT*算法进行全局路径搜索，优化路径质量。3) 动态重连策略：根据动态障碍物的位置，实时重连树结构，避免碰撞。4) 树根维护策略：定期将树根移动到机器人当前位置，以便快速进行重新规划。

🖼️ 关键图片

fig_0
fig_1

📊 实验亮点

仿真结果表明，在大多数情况下，RT-FMT在执行成本和到达时间方面均优于RT-RRT*。具体来说，RT-FMT能够显著减少机器人的到达时间，尤其是在复杂动态环境中。此外，仿真还验证了局部路径优先策略的有效性，即使存在采取次优路径的小概率，也能有效减少整体到达时间。

🎯 应用场景

RT-FMT算法可应用于各种需要在动态环境中进行实时运动规划的移动机器人，例如自动驾驶汽车、无人机、服务机器人和仓储机器人。该算法能够提高机器人在复杂环境中的导航效率和安全性，并降低运营成本。未来，该算法可以进一步扩展到多机器人协同规划和人机协作等领域。

📄 摘要（原文）

This paper proposes the Real-Time Fast Marching Tree (RT-FMT), a real-time planning algorithm that features local and global path generation, multiple-query planning, and dynamic obstacle avoidance. During the search, RT-FMT quickly looks for the global solution and, in the meantime, generates local paths that can be used by the robot to start execution faster. In addition, our algorithm constantly rewires the tree to keep branches from forming inside the dynamic obstacles and to maintain the tree root near the robot, which allows the tree to be reused multiple times for different goals. Our algorithm is based on the planners Fast Marching Tree (FMT) and Real-time Rapidly-Exploring Random Tree (RT-RRT). We show via simulations that RT-FMT outperforms RT- RRT* in both execution cost and arrival time, in most cases. Moreover, we also demonstrate via simulation that it is worthwhile taking the local path before the global path is available in order to reduce arrival time, even though there is a small possibility of taking an inferior path.