Hybrid Theta$^*$: Motion Planning for Dubins Vehicles With Integral Constraints

Abstract

We consider a motion planning problem for vehicles with curvature constraints, such as minimum turn radius, and a secondary cost such as resource cost. Traditional motion planning problems address the secondary cost as a soft constraint within the cost function. In the current paper, we take a different approach and treat this as a constraint, separate from the primary objective cost. Specifically, the integrated resource cost along the vehicle's path is constrained to be within a pre-specified limit, which is separate from the main travel cost being optimized. This approach is suitable for applications such as fire fighting, where finding the paths of minimum cost or time is essential while limiting exposure to the high heat areas. To address the resource constraints, we introduce the Hybrid Theta $^*$ (H $\boldsymbol{\Theta }^*$ ) algorithm. This is an incremental sampling based search algorithm and draws inspiration from labeling algorithms used in resource constrained shortest path problems. We present two versions of the (H $\boldsymbol{\Theta }^*$ ) algorithm, label-select and focal-select; these variants differs in how labels to be expanded are selected from the processing queue. The proposed algorithms significantly outperform the baseline methods that uses the traditional motion planning algorithms in terms of both the success rate and the solution cost. We validate the algorithms through computational experiments and real-world flight testing with on-board computation.