Navigation with polytopes and B-spline path planner

This paper firstly presents our optimal path planning algorithm within a 2D non-convex, polytopic region defined as a sequence of connected convex polytopes. The path is a B-spline curve but being parametrized with its equivalent Bezier representation. By doing this, the local convexity bound of each curve's interval is significantly tighter. Thus, it allows many more possibilities for constraining the entire curve to remain inside the region by using only linear constraints on the control points of the curve. We further guarantee the existence of the valid path by pointing out an algebraic solution. We integrate the algorithm, together with our previously published results, into the Navigation with polytopes toolbox which can be used as a global path planner, compatible with ROS navigation tools. It provides a framework for constructing a polytope map from a standard occupancy gridmap, searching for an appropriate sequence of connected polytopes and finally, planning a minimal-length path with different options on B-spline or Bezier parametrizations. The validation and comparison with existing methods are done using gridmaps collected under Gazebo simulations and real experiments.