A method and system for navigation of a robot along a goal path and avoiding obstacles. The method includes receiving goal pose for one or more robots and determining a goal path for a first robot while avoiding moving and fixed obstacles of a received obstacle map. A first objective function is evaluated to select a preferred velocity from a generated set of candidate velocities, the selecting based on one or more weighted cost functions. A set of velocity obstacles created based on the poses of the one or more robots and the preferred velocity is used in evaluating a second objective function to determine the motion of the robot in the next time cycle. Creating the set of velocity objects includes converting the preferred velocity from a non-holonomic to a holonomic velocity.
展开▼
