Reachability-Based Planning of Time-Optimal Curvature-Constrained Path With Moving and Deforming Obstacles

Abstract

In this work, we develop a time-optimal path planning algorithm for a mobile robot constrained by a minimum turning radius in an environment cluttered with an arbitrary number of moving and deforming obstacles. The algorithm builds on our previous work and involves substantial extensions to handle the turning radius constraint by adding the heading angle of the robot to the state space in addition to its location in a 2-D plane. The developed planner involves two stages: 1) forward propagation of the reachable set in the state space to preset destination through a newly derived variational inequality (VI) which encodes the obstacle avoidance and 2) backtracking to obtain the waypoints of the optimal path (corresponding to optimal control of the turning rate and speed), solved through an ODE-based scheme or a new and more robust backward-set-based scheme. The planned path represents a rigorous global optimal solution (except numerical errors) to the problem that can be used as a benchmark for other simplified planners or implemented together with a receding horizon for path planning with limited perception ability. We demonstrate both applications in several test cases.