Fast Path Planning Through Large Collections of Safe Boxes

TL;DR

This work presents a fast algorithm for the design of smooth paths (or trajectories) that are constrained to lie in a collection of axis-aligned boxes, and provides an efficient open-source software implementation, fastpathplanning.

Abstract

We present a fast algorithm for the design of smooth paths (or trajectories) that are constrained to lie in a collection of axis-aligned boxes. We consider the case where the number of these safe boxes is large, and basic preprocessing of them (such as finding their intersections) can be done offline. At runtime we quickly generate a smooth path between given initial and terminal positions. Our algorithm designs trajectories that are guaranteed to be safe at all times, and detects infeasibility whenever such a trajectory does not exist. Our algorithm is based on two subproblems that we can solve very efficiently: finding a shortest path in a weighted graph, and solving (multiple) convex optimal-control problems. We demonstrate the proposed path planner on large-scale numerical examples, and we provide an efficient open-source software implementation, fastpathplanning.

Figures (9)

• Figure 1: Path planning for a quadrotor flying through a simulated village. Top. The village, composed of buildings, trees, and bushes. The free space is decomposed using more than ten thousand safe boxes. Bottom. A snapshot of the quadrotor flight. The smooth path connects two opposite corners of the village and is guaranteed to be collision free at all times. The online planning time is only a few seconds.
• Figure 2: Offline preprocessing of the safe boxes. Left. Safe boxes. Center left. Pairwise intersections of the safe boxes. Center right. Line graph, with vertices in the box intersections and edges connecting intersections that share a box. Right. Line graph with optimized representative points.
• Figure 3: Shortening of the polygonal curve $\mathcal{C}$ through the insertion of a new box ($\mathcal{B}_k$ in green) in the current box sequence.
• Figure 4: Polygonal phase of the algorithm. Left. Line graph augmented with $p^\mathrm{init}$ and $p^\mathrm{term}$, shown as black disks. Center left. Shortest path from $p^\mathrm{init}$ to $p^\mathrm{term}$. Center right. The safe box sequence is fixed and the polygonal curve is shortened via convex optimization. Right. A new box (shown in green) is inserted in the sequence and the curve is shortened a second time. Since no further shortening is possible, the polygonal phase converges in one iteration.
• Figure 5: Bézier curve with control points $\gamma_0,\ldots, \gamma_M$, $M=4$. The curve starts at $\gamma(a) = \gamma_0$, ends at $\gamma(b) =\gamma_M$, and is entirely contained in the convex hull of the control points, shown shaded.