Optimization-Based Terrain Analysis and Path Planning in Unstructured Environments

Accurate environment representation is one of the key challenges in autonomous ground vehicle navigation in unstructured environments. We propose a real-time optimization-based approach to terrain modeling and path planning in off-road and rough environments. Our method uses an irregular, hierarchical, graph-like environment model. A space-dividing tree is used to define a compact data structure capturing vertex positions and establishing connectivity. The same unique underlying data structure is used for both terrain modeling and path planning without memory reallocation. Local plans are generated by graph search algorithms and are continuously regenerated for on-the-fly obstacle avoidance inside the scope of the local terrain map. We show that implementing a hierarchical model over a regular space division reduces graph edge expansions by up to 84%. We illustrate the applicability of the method through experiments with an unmanned ground vehicle in both structured and unstructured environments.