Efficient evaluation of collisions and costs on grid maps for autonomous vehicle motion planning

Collision checking is the major computational bottleneck for many robot path and motion planning applications, such as for autonomous vehicles, particularly with grid-based environment representations. Apart from collisions, many applications benefit from incorporating costs into planning; cost functions or cost maps are a common tool. Similar to checking a single configuration for collision, evaluating its cost using a grid-based cost map also requires examining every cell under the robot footprint. This work gives theoretical and practical insights on how to efficiently check a large number of configurations for collision and cost. As part of this work, configuration space costs are formulated, which can be seen as generalization of configuration space obstacles allowing a complete configuration check incorporating the robot geometry to be done using a single lookup. Furthermore, this paper presents two efficient algorithms for their calculation: FAMOD, an approximate method based on convolution, which is independent of the size and the shape of the robot mask, and vHGW-360, an exact method based on the van Herk-Gil-Werman morphological dilation algorithm, which can be used if the robot shape is rectangular. Both algorithms were implemented and evaluated on graphics hardware to demonstrate the applicability and benefit to real-time path and motion planning systems.