CitraNav: A Lightweight Navigation Method Using Spatiotemporal Information Voxel Mapping and Model Predictive Path Integral Control for Complex Orchards

Abstract

Canopy occlusion, dynamic vegetation, structural degeneracy, and implicit terrain risks make stable localization and task-adaptive planning difficult for resource-constrained orchard robots. This paper proposes CitraNav, a lightweight navigation method for global navigation satellite system (GNSS)-denied orchards. It separates stable geometric evidence for localization from short-lived semantic evidence for planning, preventing semantic observations from accumulating in the global map. For localization, a hierarchical voxel map selects its resolution according to local structure and light detection and ranging (LiDAR) sampling characteristics, while cross-frame reliability and observability constraints suppress updates from transient vegetation and weakly observable directions. For planning, synchronized color and depth observations form a local semantic risk point cloud. A model predictive path integral (MPPI) planner combines task-dependent semantic costs with exact-footprint collision checking against currently detected obstacles. In simulation, CitraNav achieved a mean translational localization root mean square error (RMSE) of 0.075 m. Compared with geometric point-cloud planning, semantic planning reduced the collision rate by 71.4% and increased weed coverage 4.72-fold. Across 14 real-world sequences spanning farm-road, lawn, forest, and orchard environments, CitraNav achieved mean translational and heading RMSEs of 0.151 m and 1.13°, respectively, while using 72.3–87.4% fewer geometric map cells than the comparison methods. The complete perception–planning pipeline operated at 20.3–32.7 frames per second on an edge platform. These results suggest that CitraNav offers a balanced approach to localization stability, task-adaptive planning, and computational efficiency in complex orchard navigation.