AGRO-Nav: Autonomous Graph-based Orchard Navigation

Ho Young Yun; Jaemin Yu; Duksu Kim

cs.ROAug 26, 2026

AGRO-Nav: Autonomous Graph-based Orchard Navigation

Authors: Ho Young Yun, Jaemin Yu, Duksu Kim

Organizations: School of Computer Engineering, Korea University of Technology and Education (KOREATECH), Cheonan 31253, South Korea

Abstract

Orchards form semi-structured environments in which parallel tree rows create natural driving corridors, yet narrow inter-row clearance and dense foliage lead geometry-agnostic grid planners to drift off the row center and risk trunk or canopy contact. We present AGRO-Nav, an automated framework for static graph-based global planning in orchards. From tree-row lines fitted to trunk clusters in a SLAM point cloud, it builds, without any manual waypoints, a sparse topological graph of intra- and inter-row connectivity; a global route is then found by Dijkstra search on this graph, connected to the start and goal by any-angle Theta* segments, and smoothed with a cubic B-spline. In real-orchard trials, AGRO-Nav follows the row center with a mean error of about 0.08 m, far below the A* (0.31 m) and Theta* (0.43 m) shortest-path baselines, while planning roughly four to five times faster. In Isaac Sim, it attains the lowest error among A*, Theta*, and a reproduced RANSAC midline baseline and remains stable as tree density drops to 70%, where the RANSAC baseline degrades. The resulting trajectories---straight row-centered segments joined by controlled turns---suit differential-drive and four-wheel-steering platforms.

Figures & tables

Fig. 1 : Overview of the AGRO-Nav pipeline. An offline stage, built once per orchard map, converts a 3D SLAM point cloud into a 2D map with a topological graph; an online stage then plans a row-centered trajectory over that graph for each query. Sec. III-A describes the stages.

Panel of Figure 2 — Fig. 2 : Isaac Sim orchard environment used for the simulation-based validation.

Simulation environment (full tree density)
Method	Mean ( $\pm$ Std) / Max Error [m]	Length [m]	Mean ( $\pm$ Std) / Min Clearance [m]	Time [ms]
A*	0.95 ( $\pm$ 0.43) / 1.94	509.66	1.96 ( $\pm$ 0.38) / 1.26	61.95
Theta*	0.79 ( $\pm$ 0.54) / 2.22	499.33	2.07 ( $\pm$ 0.42) / 1.01	69.16
RANSAC	0.18 ( $\pm$ 0.10) / 0.47	524.30	2.58 ( $\pm$ 0.27) / 1.86	33.74
AGRO-Nav	0.14 ( $\pm$ 0.10) / 0.42	527.29	2.57 ( $\pm$ 0.25) / 1.93	14.24
Simulation environment (70% tree density)

TABLE I : Quantitative comparison of path planning methods across the three evaluation settings. Error is reported as mean (

\pm

standard deviation) / max lateral deviation from the row center. Clearance is reported as mean (

\pm

standard deviation) / minimum distance from the planned path to the nearest tree trunk, where trunk positions are known (larger is safer). The RANSAC baseline requires exact 3D tree positions and is evaluated only in simulation. In each setting, bold marks the best value of each reported metric.

Panel of Figure 4 — Fig. 3 : Simulation results at full and 70% tree density. Each panel pairs the planned paths on the 2D Isaac Sim orchard map (left) with the trunk clearance along those paths (right; higher is safer). On the maps, green and blue mark the start and goal of each evaluated route and yellow the intermediate waypoints that select the rows to traverse.

Fig. 4 : Paths generated by the three planners in the real orchard. Green and blue mark the start and goal, and yellow the intermediate waypoints.

Robot Navigation

Robot navigation studies how robots perceive, localize, plan, and move safely toward goals in static or changing environments.

See all 557 papers

LeCropFollow: Latent Space Planning for Navigation in Unstructured Crop Fields
Jun 30, 2026Felipe Tommaselli, Francisco Affonso, Arthur Pompeu +4Robot Navigation Agricultural Robotics
Mobile Robotics Group, Center for Robotics (CRob), São Carlos School of Engineering (EESC), University of São Paulo, São Carlos, SP, Brazil · DASLab, Department of Computer Science, University of Illinois Urbana-Champaign, Champaign, IL, USA
STAG: A Sparse Traversability-Aware Graph Representation from Grid-Based Costmaps for Robotic Navigation
Oct 8, 2026Gabriel Manuel Garcia, Stéphanie Aravecchia, Miguel Angel Olivares-MendezRobot Navigation Graph-Based Planning
University of Luxembourg, Luxembourg · IRL Georgia Tech-CNRS, Metz, France
Learning Traversability for Long Horizon Off-Road Navigation
Jul 26, 2026Kasi Viswanath, Jason M. Gregory, Shaunak Kolhe +1Off-Road Robot Navigation Geospatial Representation Learning
J. Mike Walker ’66 Department of Mechanical Engineering, Texas A&M University, College Station, TX 77840, USA · DEVCOM Army Research laboratory, Adelphi, MD, USA

Jun 30, 2026J/K move · Enter open · S save

LeCropFollow: Latent Space Planning for Navigation in Unstructured Crop Fields

Felipe Tommaselli, Francisco Affonso, Arthur Pompeu, Gianluca Capezzuto+3

Mobile Robotics Group, Center for Robotics (CRob), São Carlos School of Engineering (EESC), University of São Paulo, São Carlos, SP, Brazil · DASLab, Department of Computer Science, University of Illinois Urbana-Champaign, Champaign, IL, USA

Robot Navigation Agricultural Robotics Vision-Based Robot Control Latent-Space Planning

Open paper AskPDF arXiv

Unstructured navigational features, such as irregular planting or discontinuities, remain the primary failure mode for under-canopy agricultural robots. Existing geometric approaches often fail in these scenarios because they compress high-dimensional visual data into deterministic spatial references, effectively discarding the uncertainty and semantic context required to navigate ambiguous terrain. To address this, we present LeCropFollow, a visual navigation framework that bypasses explicit geometric modeling in favor of a learned latent representation. By integrating a self-supervised semantic heatmap extractor with TD-MPC2, a Model-Based Reinforcement Learning (MBRL) planner, our system optimizes trajectories directly within a latent manifold. The framework operates over the uncompressed heatmap signal, preserving the semantic context that geometric reductions discard. We demonstrate that this representational shift enables zero-shot transfer from simplified simulation to the physical world without fine-tuning. Extensive field experiments in late-stage corn fields show that LeCropFollow matches state-of-the-art baselines in unstructured rows but significantly outperforms them in plantation gaps, achieving a 2.4x reduction in semantic failures compared to keypoint-based methods. These results suggest that latent planning offers a robust alternative to geometric estimation for operations in heterogeneous agricultural environments. Code, models, and data available: https://felipe-tommaselli.github.io/lecropfollow .

Abstract

Robot Navigation

Explore similar work

LeCropFollow: Latent Space Planning for Navigation in Unstructured Crop Fields

STAG: A Sparse Traversability-Aware Graph Representation from Grid-Based Costmaps for Robotic Navigation

Learning Traversability for Long Horizon Off-Road Navigation