Source-linked AI summary
AGRO-Nav: Autonomous Graph-based Orchard Navigation
Ho Young Yun, Jaemin Yu, Duksu Kim
TL;DR
Orchard corridors are structurally regular but difficult for planners that ignore row geometry, motivating a reusable global representation. AGRO-Nav automatically builds and routes over a row graph, then connects and smooths the route; it achieves lower row-center error and faster planning than the reported baselines in real and simulated evaluations.
Problem
Narrow, cluttered, and irregular orchard rows challenge planners that treat the environment as undifferentiated free space, while existing pipelines often lack an automatic reusable global topology.
Method
AGRO-Nav fits tree-row lines from a SLAM point cloud, automatically constructs an intra- and inter-row graph, routes with Dijkstra and Theta*, and smooths with a cubic B-spline.
Results
0.08 m mean row-center error versus 0.31 m for A* and 0.43 m for Theta*, with planning about four to five times faster in real-orchard trials; simulation reports the lowest error at both tested densities.
Takeaways & Limitations
The resulting trajectories provide repeatable, efficient, structure-aligned global planning with straight in-row segments and controlled turns suitable for differential-drive and 4WS platforms.
Takeaways & Limitations
Evaluation is limited to static, pre-mapped planning and a single commercial orchard site; dynamic obstacles and broader multi-site validation remain future work.
Abstract
from arXiv · showhide
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.
I. INTRODUCTION
AGRO-Nav addresses orchard navigation by converting SLAM-derived tree-row geometry into a reusable global topological representation. It combines automatic graph construction with row-aligned planning and evaluates the approach against local, manual, and geometry-agnostic alternatives.
- Geometry-agnostic A* and Theta* planners optimize distance rather than row adherence, while RANSAC midline methods rely on locally visible trunks.These alternatives can cut corners, enter tree rows, or degrade when rows are sparse.
- Orchard navigation combines repetitive corridor structure with narrow clearance, dense foliage, missing trees, and uneven spacing.These conditions make the row geometry only loosely defined and challenge generic planning.
- Prior systems commonly follow rows locally, use manually built or implicit graphs, or expose no routable topology from their mapping pipeline.The closest graph-based approach still manually specifies per-tree access nodes and plans shortest paths.
- AGRO-Nav automatically builds a sparse, connected orchard graph from PCA-fitted tree-row lines without manual waypoints.The graph represents intra-row and inter-row connectivity for global routing.
B. Graph-Based Global Planning
AGRO-Nav uses a problem-specific row graph to constrain global search to meaningful orchard routes. Its design contrasts with learned, manually specified, or distance-only graph-planning approaches.
- A row graph bounds search to within-row travel and controlled cross-row transitions through semantically meaningful corridors.This exploits orchard structure while reducing branching compared with general sampling-based planners.
- Dijkstra provides deterministic global routing, Theta* shortens any-angle junction connectors, and inflation-based costs preserve vegetation clearance.
- AGRO-Nav constructs its graph automatically from non-learned PCA-fitted trunk geometry, avoiding labeled data and manual per-tree access nodes.Node spacing and cross-row thresholds remain tunable.
- Unlike the closest related method, AGRO-Nav explicitly plans for row-center adherence rather than minimizing travel distance.The related method uses learned semantic segmentation and manually specified access nodes.
A. System Overview
The framework separates reusable offline map construction from online query planning. It converts a SLAM point cloud into a compact row graph, then connects and smooths a graph route for each start–goal request.
- Tree trunks are segmented, PCA fits lines representing orchard rows, and nodes are sampled along centerlines between adjacent tree lines.Edges connect nodes along rows and across headlands.
- Dijkstra finds a row-aligned graph route after the start and goal are snapped to their nearest graph nodes.
- Offline map construction converts a 3D SLAM point cloud into a 2D map and topological graph, while online planning serves each query.
- Theta* connects off-graph start and goal poses on a static cost map, after which the combined path is smoothed with a cubic B-spline.The resulting trajectory is uniformly resampled for output.
B. Problem Formulation and Notation
AGRO-Nav formulates orchard planning as producing one smooth, row-centered trajectory between poses through a topological abstraction. The objective balances row adherence and planning time under static-clearance and driving-region constraints.
- The planner takes a SLAM-generated 3D point cloud, a derived 2D occupancy/cost map, detected tree lines, and start and goal poses.An optional polygon bounds the permissible driving region.
- The output is a smooth, uniformly resampled trajectory generated from a graph representing intra-row corridors and cross-row headland transitions.
- The objective minimizes lateral deviation from inter-row centerlines and planning time while maintaining static clearance and remaining within the driving region.
- Edges are constrained by the driving region, line-of-sight, lateral separation below τrow, and a maximum length Lmax.The trajectory must also be trackable by differential-drive and four-wheel-steering platforms.
- The evaluated planner assumes a static, pre-mapped orchard and does not reactively avoid dynamic or moving obstacles at runtime.Dynamic avoidance is left to future work or downstream control.
C. Topological Graph Construction
AGRO-Nav constructs a sparse orchard graph from paired tree-row lines, placing nodes along row center axes and connecting them longitudinally and laterally under geometric constraints.
- Node generation: Adjacent tree lines form traversable rows when their orientations differ by less than 25° and their perpendicular separation is within 3–5 m.
- Node generation: For each paired row, the seed point is the geometric center of the four line endpoints, and nodes are sampled bidirectionally along the row direction.
- Node generation: Generated nodes are retained only inside the permissible driving region Ω, defined by a manual region or an expanded row-pair bounding box.
- Edge creation: Longitudinal edges connect consecutive nodes within each tree row, while lateral edges connect nodes in adjacent rows.
- Edge creation: Lateral edges require line-of-sight without detected tree-line intersections and a perpendicular separation below τrow.
- Edge creation: Edges longer than Lmax are recursively bisected, producing uniform graph resolution while discretizing routes rather than the entire orchard area.
D. Static Graph-based Global Planning
AGRO-Nav plans a static route by combining graph-based row travel with any-angle entry and exit connections, then smooths the complete path into a trackable trajectory.
- Entry and exit path planning: A linear distance-based cost decreases from occupied cells to zero within the maximum influence distance dmax.
- Graph-level path planning: The planner snaps start and goal poses to nearest graph nodes and finds the row-aligned route between them with Dijkstra’s shortest-path search.
- Entry and exit path planning: Theta* computes entry and exit segments from off-graph poses to the graph route on a static 2D cost map.
- Entry and exit path planning: Theta* is preferred over grid-aligned planners because its any-angle line-of-sight search produces smoother paths.
- Path smoothing and resampling: The concatenated entry, graph, and exit paths are smoothed with a cubic B-spline and uniformly resampled for consistent waypoint density.
- Path smoothing and resampling: The resulting trajectory contains straight in-row segments and controlled turns at row transitions, supporting differential-drive and 4WS platforms.
A. Experimental Setup
The evaluation compares AGRO-Nav with grid-based and structural baselines on repeated real-world and simulated orchard routes, using row-center adherence and planning time as key measures.
- Evaluation protocol: Each planner runs ten times per start–goal pair under identical 2D occupancy-grid conditions, with path-quality metrics averaged across evaluated pairs.
- Metrics: Accuracy is mean lateral deviation from sampled orchard row centers, computed as each reference point’s minimum Euclidean distance to the generated path.
- Metrics: The row-center reference acts as pseudoground-truth because ground-truth trajectories are unavailable, measuring row adherence rather than path optimality.
- Metrics: The row-center metric structurally favors AGRO-Nav over shortest-path baselines, so reported errors require interpretation with this bias in mind.
- Simulation setup: Isaac Sim evaluation uses six start–goal pairs and 66 sampled points across six representative rows in a controlled orchard model.
- Real-world setup: Real-world evaluation uses three start–goal pairs from ROS2 datasets collected with a 4WS orchard robot in a commercial orchard in Daegu, South Korea.
B. Simulation-based Validation
AGRO-Nav is evaluated in Isaac Sim at full and 70% tree density against A*, Theta*, and a reproduced RANSAC structural baseline. It maintains accurate, consistent row-centered paths while planning faster than the alternatives.
- Full-density results: AGRO-Nav achieved the lowest full-density simulation errors, with 0.14 m mean and 0.42 m maximum lateral deviation.Its 0.10 m standard deviation was also the smallest among the compared methods.
- Planning efficiency: 14.24 ms and 13.48 ms were AGRO-Nav’s planning times at full and 70% density, making it roughly four to five times faster than grid-based planners.It was also 2.4–2.6× faster than RANSAC in the two simulation conditions.
- 70% density results: AGRO-Nav maintained a 0.14 m mean error at 70% tree density, essentially unchanged from full density.Maximum error and path length increased only marginally, while grid-based planners remained substantially less accurate.
- Method interpretation: AGRO-Nav’s stability follows from constructing the graph from global extracted row geometry rather than isolated tree observations.The graph remains close to the full-density structure when remaining trunks provide enough evidence.
- 70% density results: Theta* penetrated a tree row at 70% density, while RANSAC mean error increased from 0.18 m to 0.24 m as density decreased.These results contrast with AGRO-Nav’s stable row-centered error under partial sparsity.
- Clearance behavior: AGRO-Nav held trunk-clearance profiles near 2.5 m across routes, whereas A* and Theta* repeatedly fell near 1 m at row transitions and off-row segments.RANSAC followed a similar band but dipped more at transitions.
C. Real-world Orchard Evaluation
In the real orchard, AGRO-Nav follows row centerlines more accurately and plans faster than the baseline planners. Its slightly longer routes trade shortest-path behavior for centered segments and controlled headland turns that preserve clearance.
- Accuracy: 0.08 m was AGRO-Nav’s real-orchard mean lateral error, confirming precise row-centerline following.The field evaluation reports this as the mean error across the real-orchard block.
- Planning efficiency: 11.52 ms versus 51.24 ms and 58.48 ms made AGRO-Nav approximately four to five times faster than the baselines.The speedup follows from searching a small precomputed topological graph rather than many occupancy-grid cells.
- Trajectory structure: 218.03 m versus 214.09 m for A* and 212.70 m for Theta* shows that AGRO-Nav accepts slightly longer trajectories than shortest-path baselines.Its route uses straight row-centered segments joined by controlled turns at row transitions.
- Operational safety: Turning only at headlands keeps AGRO-Nav’s trajectory consistently distant from trunks and canopy, providing a margin that absorbs ordinary tracking error.The stated consequence applies to both differential-drive and four-wheel-steering platforms.
V. CONCLUSION
AGRO-Nav automatically constructs a connected orchard topological graph from SLAM-derived tree-row lines, then combines graph routing, any-angle search, and spline smoothing for row-aligned trajectories. Real-orchard and Isaac Sim evaluations show lower row-center error and faster planning than the compared baselines, while the current evaluation remains limited to static planning and one orchard site.
- Conclusion: AGRO-Nav automatically builds a connected topological graph from PCA-fitted tree-row lines recovered from a SLAM-generated 3D point cloud, eliminating manual waypoint design.The graph supports static global planning through Dijkstra routing over sparse row connectivity.
- Conclusion: Cubic B-spline smoothing and uniform resampling produce continuous trajectories with straight in-row segments and controlled turns, suited to differential-drive and 4WS platforms.
- Conclusion: 0.08 m mean row-center error was achieved in real-orchard trials, compared with 0.31 m for A* and 0.43 m for Theta*, while planning took 11.52 ms versus 51.24 ms and 58.48 ms.Isaac Sim experiments also reported the lowest mean and maximum row-center error among compared methods at full and 70% tree density, with 0.14 m mean error at both densities.
- Conclusion: Static global planning is evaluated in a pre-mapped environment, while dynamic-obstacle avoidance is left for future work; real-world testing used a single commercial orchard site.Future work includes a runtime local obstacle layer, physical-platform trajectory and kinematic validation, and evaluation across broader layouts, seasons, and sites.