3D Off-road Terrain Mapping for Autonomous Ground Vehicle Energy-Optimal Path Planning
Matt Nguyen · 2025
Autonomous ground vehicle (AGV) applications have significantly increased in the last several decade in various fields, including agriculture, logistics, military and planetary explorations applications. This growth can be attributed to their ability to independently navigate with high payloads and perform tasks in hard-to-reach or dangerous environments. However, navigating unknown terrain may presents challenges for AGVs since unstructured paths, obstacles, and inconsistencies in ground properties can directly affect navigation efficiency and energy consumption. These factors are particularly critical for AGVs with a finite battery capacity that restricts the operational duration. This paper examines the challenge of creating a local terrain elevation map from GPS measurements collected by an AGV to enhance navigation efficiency. A methodology was developed that allows a commercially available Jackal AGV from Clearpath Robotics to collect onboard sensor data that is then interpolated to create a digital elevation map (DEM) based on the vehicle’s path. The platform was configured to collect a combination of IMU, gyroscope, and GPS data from sensors gathered by the Robot Operating System (ROS). An automated data processing pipeline was developed to convert raw data into readable formats (e.g., CSV tables), filter out irrelevant information, and organize the data efficiently to reduce computational overhead. The path traversed by the AGV was then reconstructed, and a linear regression model was implemented to interpolate between GPS points to create a 3D map that models the elevation of the terrain. Various sampling approaches were applied to position the grid points representing the terrain surface, generating a graphical representation of the surface. This graph, composed of nodes, provides both visual and numerical data and serves as a environmental model for path planning. An energy-optimal path planner based on A* (A star) algorithm was then developed by defining a cost function between nodes related to changes in terrain height and a planar distance heuristic to to guide the search toward the optimal path efficiently. Our approach demonstrated that the AGV could successfully avoid steep terrain, finding paths around difficult areas rather than attempting to traverse them directly. Both gridded sampling and random sampling methods were used to form a roadmap for planning and their relative performance was evaluated and compared. Simulated energy optimal paths over the experimentally determined elevation model are used to illustrate the approach.