Dynamic Multi-Query Motion Planning with Differential Constraints and Moving Goals

Planning robot motions in complex environments is a fundamental research challenge and central to the autonomy, efficiency, and ultimately adoption of robots. While often the environment is assumed to be static, real-world settings, such as assembly lines, contain complex shaped, moving obstacles and changing target states. Therein robots must perform safe and efficient motions to achieve their tasks. In repetitive environments and multi-goal settings, reusable roadmaps can substantially reduce the overall query time. Most dynamic roadmap-based planners operate in state-time-space, which is computationally demanding. Interval-based methods store availabilities as node attributes and thereby circumvent the dimensionality increase. However, current approaches do not consider higher-order constraints, which can ultimately lead to collisions during execution. Furthermore, current approaches must replan when the goal changes. To this end, we propose a novel roadmap-based planner for systems with third-order differential constraints operating in dynamic environments with moving goals. We construct a roadmap with availabilities as node attributes. During the query phase, we use a Double-Integrator Minimum Time (DIMT) solver to recursively build feasible trajectories and accurately estimate arrival times. An exit node set in combination with a moving goal heuristic is used to efficiently find the fastest path through the roadmap to the moving goal. We evaluate our method with a simulated UAV operating in dynamic 2D environments and show that it also transfers to a 6-DoF manipulator. We show higher success rates than other state-of-the-art methods both in collision avoidance and reaching a moving goal.