CBS-HT: Prioritized Safe Interval Path-Planning Algorithm for Heterogeneous Agricultural Robot Team

Abstract

Existing multi-agent path-finding (MAPF) methods either assume homogeneous robots or require inter-robot communication, limiting their field deployment in large orchards. We introduce conflict-based search for heterogeneous tasks (CBS-HT), a fixed-priority path-planner tailored for unmanned ground vehicles (UGVs) that perform monitoring, harvesting, and transportation. By design, CBS-HT relaxes strict completeness and optimality guarantees in exchange for practical real-time performance on large robot teams. CBS-HT integrates priority-based resolution into a CBS-inspired search framework, resolving path clashes without on-line negotiation and thus avoiding radio congestion. Three evaluation tiers—simulation, lab-scale, and orchard—test scalability, mission-level efficiency, and real-world robustness. Compared with an uncoordinated baseline, CBS-HT shortens high-priority robots’ travel distance by up to 19.1 m and mission time by 28.4 s, while maintaining a 100% collision-free success rate for tested instances. These gains demonstrate that fast, priority-aware planning can enable practical, communication-free coordination of heterogeneous UGV fleets in commercial orchards.