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

TL;DR

This paper addresses real-time motion planning for mobile robots in dynamic environments with moving obstacles. It introduces RT-FMT, a real-time variant of FMT* that searches for the global path while simultaneously generating low-cost local paths for immediate execution, and it continuously rewires to avoid dynamic obstacles and keep the tree root near the robot. RT-FMT combines ideas from FMT* and RT-RRT*, enabling multiple-query planning and reuse of the search tree for different goals. In simulations in a Maze-like and Mine-like environment, RT-FMT outperforms RT-RRT* in plan time, execution cost, and arrival time, particularly under dynamic obstacles. The results highlight the value of integrating fast global search with concurrent local planning for real-time robotic navigation, with future work extending to OMPL and constrained robots.

Abstract

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.

Figures (5)

• Figure 1: Illustration of a mine-like space. The start and goal configurations are shown as $x_s$ and $x_g$, respectively. The solution is highlighted in red. The pink squares are the dynamic obstacles representing mining trucks and the dark solid obstacles are the mine pillars. The goal is to traverse the space with the least amount of time to maximize profit.
• Figure 2: Simulation environment with the Maze space for (a) RT-FMT and (b) RT-RRT*. The parameter $r_o$ represents the sensing range of the robot, and the distance $r_b$ represents the safety radius in which tree nodes are considered blocked if a dynamic obstacle is within $r_o$ from the robot.
• Figure 3: Simulation results for Experiment 1. Hollow squares represent a single simulation, solid circles the average on both axes for different sample counts, and the ellipses 90 % confidence level.
• Figure 4: Simulation results for the Maze in Experiment 2.
• Figure 6: Simulation results for the Maze in Experiment 3.