Asymptotically Optimal Lifelong Planning With Lazy Edge Evaluation Under Expensive Collision Checks

Robotic systems operating in dynamic and uncertain environments require motion planners that can rapidly adapt to environmental changes while maintaining safety and efficiency. Frequent replanning is inevitable in such scenarios as obstacle configurations evolve and previously feasible trajectories become invalid. However, real-time replanning remains challenging for many applications due to the high computational cost of collision checking and graph maintenance, especially when edge evaluations are expensive or when the environment changes continuously. The paper introduces an asymptotically optimal lifelong sampling-based path planning algorithm that combines the merits of lifelong planning algorithms and lazy search algorithms for rapid replanning in dynamic environments where edge evaluation is expensive. The algorithm maintains an incremental search graph which is reused throughout the entire navigation process. By evaluating only sub-path candidates for the optimal solution, the algorithm saves considerable evaluation time and reduces the overall planning cost. It employs a novel informed rewiring cascade to efficiently repair the search tree when the underlying search graph changes. Theoretical analysis indicates that the proposed algorithm converges to the optimal solution as long as sufficient planning time is given. Planning results on robotic systems with SE(3) and R⁷ state spaces in challenging environments highlight the superior performance of the proposed algorithm over various state-of-the-art sampling-based planners in both static and dynamic motion planning tasks. The experiment of planning for a Turtlebot 4 operating in a dynamic environment with several moving pedestrians further verifies the feasibility and advantages of the proposed algorithm.