We consider navigation for a polygonal, holonomic robot in an obstacle filled environment in SE(2). We denote the configuration space of the robot as C. In order to determine the free space, obstacles are represented as point clouds then transformed into C. The point-wise Minkowski sum of the robot and obstacle points is then calculated in C by adding the vertices and points on the convex hull of robot to obstacle points for different robot configurations. We then find a seed path using either a graph search or sample based planner. This seed path is then used in our novel method to determine overlapping convex regions for each consecutive chord of the seed path. Our proposed method represents the collision free, traversable region by defining overlapping convex corridors defined by a set of linear constraints. Within these corridors we find feasible trajectories that optimize a specified cost functional. The generated corridors along with the initial and desired poses are then used to determine an optimal path that satisfies the specified objective within the same homotopy group as the seed path. The key contributions is the proposed methods' ability to easily generate a set of convex, overlapping poly-topes that effectively represent the traversable free space. This in turn lends itself to (a) efficient computation of optimal paths, and (b) extending these basic ideas to non-Euclidean spaces such as SE(2). We provide simulated examples and implement this algorithm on the KUKA youBot omni-directional base.
展开▼