Hierarchical planning and control for autonomous excavation using terrain feedback

Excavation robots are a promising solution to skilled labor shortages in construction, yet achieving autonomy over entire excavation processes remains a challenge due to evolving terrain geometry and real-time constraints. Existing approaches focus on automating individual digging cycles, without closing the feedback loop between terrain perception, planning, and control throughout excavation. The system uses a hierarchical control framework that processes point cloud data from sensors and generates control commands to realize a desired terrain profile. The hierarchy consists of a global planner, a trajectory planner, and a high-rate tracking controller. Given the current and target terrain profiles, the global planner uses an efficient weighted sampling strategy to select excavation points. The trajectory planner takes these points as input and outputs the best dig trajectory. Finally, a real-time tracking controller running at approximately 50Hz executes the planned trajectory while enforcing actuator and velocity constraints. Experiments demonstrate computational efficiency, excavated volume estimation accuracy, multi-constraint satisfaction, and real-time tracking performance of the proposed system. The robot autonomously reshapes four different terrain types to match desired profiles, achieving an average surface error of 4mm.