An Integrated Route and Path Planning Strategy for Skid–Steer Mobile Robots in Assisted Harvesting Tasks with Terrain Traversability Constraints

Abstract

This article presents a combined route and path planning strategy to guide Skid–Steer Mobile Robots (SSMRs) in scheduled harvest tasks within expansive crop rows with complex terrain conditions. The proposed strategy integrates: (i) a global planning algorithm based on the Traveling Salesman Problem under the Capacitated Vehicle Routing approach and Optimization Routing (OR-tools from Google) to prioritize harvesting positions by minimum path length, unexplored harvest points, and vehicle payload capacity; and (ii) a local planning strategy using Informed Rapidly-exploring Random Tree (${\mathrm{IRRT}}^{*}$) to coordinate scheduled harvesting points while avoiding low-traction terrain obstacles. The global approach generates an ordered queue of harvesting locations, maximizing the crop yield in a workspace map. In the second stage, the ${\mathrm{IRRT}}^{*}$ planner avoids potential obstacles, including farm layout and slippery terrain. The path planning scheme incorporates a traversability model and a motion model of SSMRs to meet kinematic constraints. Experimental results in a generic fruit orchard demonstrate the effectiveness of the proposed strategy. In particular, the ${\mathrm{IRRT}}^{*}$ algorithm outperformed RRT and ${\mathrm{RRT}}^{*}$ with 96.1% and 97.6% smoother paths, respectively. The ${\mathrm{IRRT}}^{*}$ also showed improved navigation efficiency, avoiding obstacles and slippage zones, making it suitable for precision agriculture.

1. Introduction

The Food and Agriculture Organization (FAO) forecasts that a 70% increase in agricultural productivity by 2030 will be necessary to sustain the growing global population of nearly 9 billion people [1]. In order to meet these new demands while adhering to stringent quality standards in the global markets, digital farming has become the prevailing path for future agricultural development. Increasing agricultural productivity through the adoption of technological advances such as artificial intelligence and robotics is currently contributing to the fulfillment of such goals [2,3]. For instance, agricultural processes will require autonomous mobility to assist in primary tasks such as tillage, watering, sowing, fertilizing, and spraying as well as in central tasks such as supervising, hauling, and harvesting, among others. All of these tasks will require agricultural ground vehicles capable of autonomously planning their tasks and routes.

The process of manual harvesting in farmlands has typically relied on human labor and traditional farm equipment. Workers generally operate machines or tools to guide the harvesting of crops such as fruits, vegetables, or grains, then gather them for further processing. These processes are labor-intensive, time-consuming, and physically demanding [4]. Moreover, they rely on the expertise of farm workers to determine the optimal picking and collection times, preferred harvesting locations, and quantity and quality of collected products. Though operationally straightforward, this challenging process encompasses several concerns [5]. The first shortcoming is that crop production can vary significantly between rows due to varied soil quality, sunlight exposure, and other environmental aspects. This makes it difficult to plan the assignment of workers to the highest-yield harvest areas. Similarly, harvesting perishable and fragile products that are subject to unpredictable weather and environmental conditions, such as fruits and vegetables, can jeopardize both the quantity and quality of the harvested product, requiring timely collection of products. Hence, it is appealing to conceive routing and path planning techniques for autonomous ground vehicles that prioritize harvesting locations based on closeness to harvesting points of maximum crop yield [6,7].

One of the key aspects for enhancing the product collection process is the execution of planning strategies that consider the optimal sequence of multiple harvesting locations and navigation points [8]. Furthermore, optimizing product harvest in agriculture entails the efficient implementation of path planning strategies that set appropriate sequences for coordinating harvest locations, in which either manual or autonomous vehicles attend to the crop harvest by quickly positioning themselves near these harvest points. Such positioning in farmlands is key for the automatic motion of autonomous agricultural machinery, and is also the basis for controlling operational behaviors such as autonomous turning, driving, and harvesting [9]. For instance, global planners focused on solving the Traveling Salesman Problem (TSP) enable the computation of combination sequences ordered for the shortest route according to specific navigation objectives [10,11]. However, combined local path planning is usually required to guide the autonomous vehicle between global harvesting points, as proposed in this work. In addition to TSP, various methods such as those based on the Vehicle Routing Problem (VRP) [12,13], ant colony optimization [14,15], genetic algorithms [16], and others using heuristics [17] have been aimed at exploring ordered routes of minimum traveling distance and reduced time (see [18,19]). In practice, TSP-based methods [20] addressing more than route travel optimization are able to impact vehicle resource utilization, load motion capacity, and energy efficiency when integrated into local planners, as proposed in the present study.

The efficient planning of feasible and adaptable paths is crucial for navigating around environments with static and dynamic obstacles. Existing local path planning methods primarily target the avoidance of predicted obstacles or adhere to structural characteristics outlined in workspace maps [21,22,23,24]. However, there remains a lack of practical approaches to integrate the need for priority waypoints within harvesting maps while taking into account the presence of obstacles and terrain traversability criteria. This gap highlights a critical issue in the existing classical methods, as exemplified in sample-based approaches such as Dijkstra, A* and Probabilistic Road Mapping (PRM) [25], to name only a few. For instance, in [26] the authors developed a path planning approach based on the extraction of a 2D grid map to reduce the computation memory needed and reduce the path search space. In addition, they introduced an extension to the A* algorithm to ensure a safe path and a maximum distance from the vine trees. Although these methods aim to find the shortest path length toward reachable goals, they often neglect practical aspects such as motion dynamics feasibility, crop workspace layout, and terra-mechanical constraints that are part of the inherent complexity of the environment. Therefore, these methods do not assure the generation of paths that are kinematically compatible with the robot’s motion dynamics. To address such limitations, probabilistic techniques focused on Rapidly-exploring Random Trees (RRT) [27,28,29,30] are often used for path planning applications subject to robot’s kinematic constraints. These RRT-based techniques offer a potential solution to the challenge of exploring feasible paths adapted to complex search workspaces [31]. However, only a few variants have demonstrated the capability to integrate global path planners and generate feasible paths that consider uncertain motion dynamics and terrain traversability exploration criteria. This advantage of (RRT) comprises the main reason for using probabilistic approaches in the present study.

The main contribution of this study is an integrated path planning method composed of a global and a local planning strategy for assisted harvesting tasks in agricultural scenarios. The proposed global route planning approach is based on the TSP strategy for scheduling harvested locations, making use of underlying insights from the CVRP approach to account for navigation objectives and constraints, whereas the local path planning uses a designed Informed Rapidly-exploring Random Tree Star (${\mathrm{IRRT}}^{*}$) approach. The proposed path planning strategy is able to generate kinematically feasible paths for Skid–Steer Mobile Robot (SSMR) dynamics that further account for the vehicle payload capacity and terrain traversability criteria. The key aspects of this research are as follows:

• A global path planning strategy that integrates the Traveling Salesman Problem (TSP) and the Capacitated Vehicle Routing Problem (CVRP) methodologies, prioritizing the observance of payload capacity constraints while considering the distance between crop harvest locations.
• Coordination between local path planning and global path planning helps to explores scheduled harvesting points based on ${\mathrm{IRRT}}^{*}$ to achieve feasible and kinematically compatible paths with SSMR dynamics while accounting for obstacle avoidance and terrain traversability issues posed by low-traction surfaces.
• The proposed strategy can generate paths that avoid difficult-to-cross regions typical of agricultural farms, helping to reduce robot resource usage and prevent situations where robots can become stuck.
• The proposed planner can be used as a reference for motion controllers, enabling efficient driving over shortened and qualified paths, which can potentially reduce the exposure time of farm products to the environment and help to avoid the mistreatment of the agricultural harvest.

Unlike other planners that only consider the minimization of path length and disregard the agricultural production process, the proposed work also considers predicted crop yield maps with multiple harvesting locations between farm aisles in order to determine priority between navigation points until reaching storage terminals, sorting areas, or packing zones. The proposed strategy is capable of planning and re-planning paths as full-load conditions or unforeseen obstacles are detected around the SSMR. This can help to avoid the harvest exposition to the environment, which is expected to improve product quality. The proposed strategy considers a map rendered as an RGB image; this image is processed to provide a deployed representation for designing the planner. Then, considering specific exploration areas in the map, information about the current harvest positions and product quantity to be harvested for each crop row is used in the TSP-CVRP strategy to schedule the navigation path. The path contains a queue list of prioritized harvest points, seeking to maximize load capacity. The resulting ordered list of harvesting points is used as input information for the local path planner (${\mathrm{IRRT}}^{*}$). The local path planning strategy is then used as a reference path for a motion controller of autonomous agricultural vehicles. A comparative analysis is conducted to assess the performance of the proposed strategy using the metrics of path length, travel time, smoothness, and proximity to obstacles along with the cumulative tracking error, controller effort, and total cumulative cost metrics for the test controller.

The rest of this paper is organized as follows. An analysis of works related to this paper is presented in Section 2. Section 3 details the terra-mechanical models used in the path planning design, including those for the SSMR dynamics and interaction with low-frictional terrains. Section 4 details the work methodology, incorporating the processing of the harvest map and the proposed integral path planning strategy. Section 5 discusses several experimental results. Finally, the conclusions of this work are provided in Section 6. The nomenclature of the variables used throughout this paper is summarized in

Table 1. Nomenclature of the variables used in the proposed probabilistic path planning algorithms.

Traversability model
$\mu$	Coefficient of static friction
$F_N$	Normal force
$F_f$	Frictional force
Navigation map
I	Image captured by a vision sensor
$O_g$	Occupancy grid representing the environment (pixels)
Global path planning
$H_p$	Harvest points
$L_p$	List of locations and weights of $H_p$
R	List of harvest rows for in each isle
Q	Maximum payload capacity of the vehicle
w	Harvest weight on each harvest point
q	Harvest demand on each harvest point
$L_{\mathrm{ord}}$	Route with ordered $H_p$ to be visited
Local path planning
$z_{\mathrm{start}}$	Initial path planning state
$\mathcal{T}$	Tree
$z_{\mathrm{rand}}$	New sample point in the $O_g$
$z_{\mathrm{best}}$	best node in terms of closeness to a neighbourhood
$z_{\mathrm{nearest}}$	The nearest node from $z_{\mathrm{rand}}$
$z_{\mathrm{new}}$	New node added to $\mathcal{T}$
$T_f$	Traction force
$\mathcal{N}\left(z_{\mathrm{new}}\right)$	Neighbourhood of nodes around $z_{\mathrm{new}}$

.

2. Related Work

In addition to generating a navigation path for either manual or assisted guidance tasks [18], a path planning strategy requires a sequence of waypoints arranged to form a traversable route. In agriculture, this order is primarily driven by the need to prioritize navigation points, generally while considering operative aspects such as closeness between harvesting locations, harvest product capacity, distribution of workers across rows, and product collection time [32]. For instance, in [31], traveling routes were obtained by optimizing travel costs through the solution of a Traveling Salesman Problem (TSP) to guarantee the vehicle’s return upon task completion in orchard fields. While this method aims to minimize path length and execution times by spatially separating multiple regions, it targets specific tasks for vehicles within predefined working schedules rather than considering all potential workspaces and locations. Adapting path planning strategies to the complexities of agricultural environments requires incorporating capabilities from additional path planning techniques. The path planning problem can also be formulated as a variant of the TSP [10,14,18] where travel costs between individual targets can be set. However, this approach does not account for scenarios where the vehicle has limited payload and has to transport products to different locations. To address this limitation, the Capacitated Vehicle Routing Problem (CVRP), a variant of the TSP, has been developed to include vehicles with limited payload capacity [33]. In particular, the CVRP aims to find the optimal set of routes that minimizes the total travel cost in terms of path length and travel time while ensuring that capacity constraints are met and all locations are visited [34,35]. While global path planning approaches are effective in many scenarios, they may be inadequate in environments in which robots must navigate around obstacles [36]. Consequently, more adaptable path planners are required in order to devise trajectories that avoid them while accounting for other navigation characteristics such as robot’s dynamics and terra-mechanical constraints. For instance, sampling-based path planners based on Rapidly-exploring Random Trees [27] and their variants are commonly used for local path planning with obstacles, as proposed in the unstructured layouts of orchard environments in [37]. In that work, kinematic constraints were met and obstacles were avoided by fusing RRT with artificial potential field techniques. Similarly, current works on sample-based techniques and global planning methods integrate maneuverability criteria to optimize paths subject to speed constraints [38]. Considering the terra-mechanical models of Hunt and Crossley in the design of path planners can allow autonomous ground vehicles to adapt their maneuverability control to different uneven and flexible navigation surfaces [39]. Several agricultural applications using hybrid techniques based on RRT can be found in spraying, planting, and harvesting tasks [40,41,42,43,44]. In [45], the authors proposed a pure pursuit algorithm based on the kinematics characteristics of tracked mobile machinery. The work addressed optimization objectives including job coverage, path length, and path quantity. Optimal path planning was achieved while ensuring job coverage; however, this resulted in loss of accuracy in local path planning. To the best of our knowledge, there is a lack of integrated path planning approaches that consider sorted harvesting points according to the robot payload capacity while simultaneously accounting for wheel–terrain interactions. While most path planning works consider static and dynamic obstacles surrounding the vehicle, few have taken into account the navigation terrain in assisted harvesting tasks, which could be considered as the primary terra-mechanical constraint that affects the navigation performance through reduced tractive force exertion and wheel slippage [46]. Therefore, path planners that involve terrain traversability criteria to overcome terrain obstacles are more than welcome.

Typical path planning methods for solving the shortest path problem encompass a variety of methodologies, each with its own strengths and limitations. For example, grid-based algorithms such as Dijkstra’s and A* [47] offer resolution completeness, but can become computationally burdensome for high-dimensional and complex problems. Conversely, sampling-based approaches such as RRT [48], Probabilistic Roadmaps (PRM) [49], and their variants [50] demonstrate asymptotic optimality and efficiency in irregular environments, particularly in navigating high-dimensional spaces. In addition, they ensure probabilistic completeness by guaranteeing a feasible solution when one exists. Improving the speed of path calculations in RRT is also a key concern, influenced by factors such as the start and end point locations, search space size, sampling efficiency, and unexpected obstacles. To address these aspects, in [25] the authors proposed fusing PRM and RRT, splitting the planning area in regions, and taking advantage of PRM. However, the integration disregards the robot kinematics, potentially affecting the path execution.

Efficient environmental exploration poses a challenge for RRT-based path construction, as highlighted in [51]. In [52], a hybrid RRT approach was introduced to improve workspace exploration by incorporating probabilistic learning techniques. The method concurrently generates growing trees to explore probable boundary samples that can reach the destination point increasingly faster. An extended work was proposed in [53], where the selection of samples was accomplished by filtering waypoints through clustering methods. On the other hand, evolutionary algorithms such as genetic approaches and optimization-based methods [54] offer additional advantages for efficiently solving high-dimensional problems. Nevertheless, they may encounter challenges such as becoming trapped in local optima. Additionally, incorporating the robot dynamics into path planning could be achieved by formulating an optimal control problem [55]. Nevertheless, this approach may face similar challenges to grid-based methods when dealing with high-dimensional spaces. For this reason, a variant of ${\mathrm{RRT}}^{*}$ based on heuristics is used in this paper as a complementary tool for path planning integration.

3. Motion Models for Assisted Harvesting Tasks

Prior to the path planning design, it is necessary to introduce a motion model of the robot and terrain traversability according to the proposed methodology depicted in Figure 1.

3.1. Model of the Skid–Steer Mobile Robot

The motion model used in this work is an adapted version of a unicycle-type vehicle model for Skid–Steer Mobile Robots (SSMR), encompassing both lateral and longitudinal dynamics [2] (as shown in Figure 2). The SSMR model is used because the harvest payload can be maintained for a certain speed profile and uniformly distributed along its structure, as typically required in turns of farm headlines. This model integrates nonholonomic constraints, speeds, and control actions that allow kinematically compatible paths with the robot dynamics. The driveline model of the robot configures a four-wheel-drive scheme, with traction forces regulated by traction and turning torques. The forward dynamics of the robot model are as follows:

$$\begin{array}{cc}\hfill & \begin{array}{c}\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\\ {\dot{v}}_x\\ {\dot{v}}_y\\ {\dot{\omega }}_z\end{array}\right]=\left[\begin{array}{c}{v}_{x}cos\theta -a{\omega }_{z}sin\theta \\ v_{x}sin\theta +a{\omega }_{z}cos\theta \\ {\omega }_z\\ p_0{\omega }_z^2+p_1{v}_x\\ p_3{\omega }_z{v}_x+p_4{\omega }_z\\ p_6{\omega }_z{v}_x+p_7{\omega }_z\end{array}\right]+\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ p_2& 0\\ 0& p_5\\ 0& p_8\end{array}\right]\left[\begin{array}{c}{\tau }_v\\ {\tau }_{\omega }\end{array}\right]\end{array}\hfill \end{array}$$

(1)

subject to the following nonholonomic constraints:

$$\begin{array}{cc}\hfill & -\dot{x}sin\theta +\dot{y}cos\theta -\left[v_y+\left(d\right){\omega }_z-b{\omega }_z\right]=0\hfill \\ \hfill & v_y-a{\omega }_z=0\hfill \end{array}$$

(2)

where $(x,y,\theta )$ denotes the robot’s pose, while $v_x$, $v_y$, and ${\omega }_z$ are the longitudinal, lateral, and angular speeds, respectively. The variable ${\tau }_v$ denotes the control input effort related to translational torque, while ${\tau }_{\omega }$ represents the control input effort related to the rotational torque. The parameters d and a are the length and track width of the SSMR, respectively. The vector of the model parameters $p=[p_1,\dots ,p_8]$ is obtained according to the robot’s responses to torque inputs, and were previously estimated in [56]. For more comprehensive information regarding the model, readers are referred to [46].

3.2. Model of Terrain Traversability

The Coulomb friction model for wheel–terrain interactions, as outlined in [57], defines the frictional force $F_f$ between the wheel and the navigation terrain such that

$$F_f=\mu ·F_N,$$

where $\mu$ denotes the coefficient of static friction and $F_N$ refers to the normal force, associated here with the full weight of the harvesting vehicle. In particular, normal reaction forces represent the vertical projection of the vehicle’s weight onto the surface normal, subject to variations due to changes in terrain conditions or weight on the harvest vehicle. The wheel–terrain interaction model assumes that static and kinetic friction forces are equivalent until the applied force exceeds the friction force $\mu ·F_N$, potentially leading to slippage conditions or even wheel entrapment. For accurate path planning, representations of wheel–terrain interactions for each wheel of the vehicle are necessary. Consequently, the so-called MF-wheel model, based on Pacejka’s insights [58], is employed to characterize tangential forces for each wheel, capturing the complex wheel–terrain behavior:

$$\begin{array}{cc}\hfill F_{klx}=& {\mu }_x\left({\sigma }_{s,kl}\right)F_{N,klz},F_{kl,y}={\mu }_y\left({\alpha }_{kl}\right)F_{N,klz}\hfill \\ \hfill {\mu }_x\left({\sigma }_{skl}\right)& =D_{x}sin\left(C_{x}arctan\left(B_x{\sigma }_{s,kl}\right)\right)\hfill \\ \hfill {\mu }_y\left({\alpha }_{kl}\right)& =D_{y}sin\left(C_{y}arctan\left(B_y{\alpha }_{kl}\right)\right)\hfill \\ \hfill {\mu }_s& =∥({\mu }_x,{\mu }_y)∥\hfill \end{array}$$

(3)

where k denotes the front or rear position of the wheel in the robot and l stands for the left or right position of the wheel in the robot, x and y denote the longitudinal and lateral directions, respectively, ${\left[B_{\left(·\right)}{C}_{\left(·\right)}{D}_{\left(·\right)}\right]}^T$ represents the parameters of the wheel model obtained from [59], and ${\mu }_x$, ${\mu }_y$, and ${\mu }_s$ are the coefficients of the longitudinal, lateral, and resultant friction, respectively. The wheel slip ratio ${\sigma }_s\in [-1,1]$ and sideslip angle $\alpha \in [-\pi ,\pi ]/2$ represent wheel speeds varying with respect to the robot chassis. The slip ratio ${\sigma }_s$ for each wheel is as follows:

$$\begin{array}{c}\hfill {\sigma }_{s,kl}=\left\{\begin{array}{c}{\displaystyle \frac{s_{kl}}{{\omega }_{kl}{r}_w}},\mathrm{if}{\omega }_{kl}{r}_w>v_x,{\omega }_{kl}{r}_w\ne 0,\mathrm{while}\mathrm{moving},\hfill \\ {\displaystyle \frac{s_{kl}}{v_x}},\mathrm{if}{\omega }_{kl}{r}_w<v_x,v_x\ne 0,\mathrm{while}\mathrm{braking},\hfill \\ \mathrm{with}{s_{kl}=\omega }_{kl}{r}_w-v_x,\hfill \end{array}\right.\end{array}$$

(4)

where $s_{kl}$ represents the difference between the linear speed ${\omega }_{kl}{r}_w$ and longitudinal speed $v_x$ along the robot’s axis. To implement this model, both the coefficient of friction $\mu$ and normal force $F_N$ are required, which are assumed to be given in this work, as the robot’s payload and terrain type are known a priori. It is worth noting that changes in surface characteristics are assumed to be minimal for the same testing area.

4. Integration of Route and Path Planning

The proposed path planning methodology was developed along three stages: (i) planning of the global path based on the TSP-CVRP approach, (ii) processing of the navigation map, and (iii) planning of the local path with Informed ${\mathrm{RRT}}^{*}$, also known as ${\mathrm{IRRT}}^{*}$, as illustrated in Figure 1. The following sections detail each of these scenarios.

4.1. Global Path Planning

This section presents the design of the global path-planning approach for assisted harvesting tasks. The path planning method aims to find an optimal route connecting harvesting points (i.e., nodes) while visiting the same location only once, prioritizing the maximum harvest for each row, ensuring vehicle payload capacity, and maximizing closeness between harvesting locations (see Figure 3a of the harvesting map). The TSP-CVRP algorithm is selected to determine a path that fulfills the former objectives due to its ability to schedule coordinated points while meeting connection constraints.

The TSP-CVRP approach considers a graph $G=(P,E)$ to represent potential connections between harvesting points. Such a connection comprises P as the set of ordered harvesting locations $H_p=\{h_0,h_1,\dots ,h_p\}$. Similarly, E is the set of effective connections that pair harvesting points, and each edge $e\{i,j\}\in E$ is associated with a non-negative cost $C_{ij}$, computed as follows:

$$C_{ij}={\displaystyle \frac{D_{ij}\times f_w}{w_j}}$$

(5)

where $D_{ij}$ is the Euclidean distance, used to quantify the closeness between two any harvesting points; $f_w$ is a factor prioritizing reduced-distance connections; and $w_j$ is the product demand expected to be harvested, expressed in weight. For each harvesting point from $H_p$, a specific product demand $q_j$ is assigned; however, the single harvest depot denoted by $h_0$ stands as the start and end point without associated demand (i.e., $q_0=0$). The accumulated harvest product obtained until weight $w_{ij}$ in the arrival position in j along the route search is limited in order to not overload the maximum load capacity Q of the SSMR. Thus, as it is necessary to find a route connecting the ordered harvesting points while minimizing the total distance and payload constraints, the proposed functional cost can be formulated as follows:

$$\begin{array}{cc}\hfill \underset{{\delta }_{ij}}{min}& \mathcal{C}=\sum _{i=1}^p\sum _{j=1}^p{C}_{ij}{\delta }_{ij}\hfill \\ \hfill \mathrm{Subject}\mathrm{to}:& \end{array}$$

(6)

$$\begin{array}{cc}\hfill & \sum _{i=1}^p{\delta }_{ij}=1,j=1,2,\dots ,p\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill & \sum _{j=1}^p{\delta }_{ij}=1,i=1,2,\dots ,p\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill & {\delta }_{ij}=0,\forall i,j\notin F_c\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill & \sum _{i=1}^p{w}_j{\delta }_{ij}\le q_j,\forall j=1,2,\dots ,p\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill & \sum _{j=1}{q}_j\le Q,q_0=0,\forall j=1,2,\dots ,p\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill & u_i-u_j+n{\delta }_{ij}\le n-1,\forall i\ne j,2\le i,j\le p\hfill \end{array}$$

(12)

where the objective is to minimize the total travel distance $\mathcal{C}$ over ${\delta }_{ij}$. This distance $\mathcal{C}$ is defined as the sum of distances $C_{ij}$ between harvesting points i and j, ensuring that each of them is visited only once. The variable ${\delta }_{ij}\in \left\{0,1\right\}$ is a binary constraint function, taking ${\delta }_{ij}=1$ if there is a possible route of connection from i to j and ${\delta }_{ij}=0$ otherwise; $w_j$ denotes the product demand in weight assigned to a particular arriving harvesting point j; Q is the vehicle capacity for the maximum payload to be hauled along the overall planned route; and $F_c$ is the set of unfeasible connections due to inaccessible workspace from opposite sides of the same harvesting row, which is detailed in the following subsection. The auxiliary variables $u_{i,j}$ are introduced to prevent the route from excluding the initial and endpoint harvest positions, ensuring that the linkage of two consecutive points is met and the planned points do not generate isolated route loops. The objective function minimizes the overall travel cost through the computation of individual costs $C_{ij}$ for all connections between harvesting points $i,j$. Moreover, the constraints ensure that each harvesting point in the route that does not result in the maximum payload limit Q being exceeded is visited exactly once (see Equation (10)).

In Algorithm 1, the TSP-CVRP method begins by building a map $M_p$ containing coordinates $(x_i,y_i)$ without any specified order of the harvesting points $H_p$ (see line 1). This harvest map is augmented with a separate map $M_w$, which incorporates geolocalized weights associated with each point in $H_p$ (see code line 2). A list of unfeasible connections $F_c$ are previously identified using the binary constraint function and incorporated to $M_w$. Prior to the execution of the TSP-CVRP algorithm, it is also necessary to specify the maximum payload Q that the robot is capable of hauling throughout the navigation route (see code line 4).

The TSP-CVRP algorithm calculates the distances between all pairs of harvesting points $\{h_i,h_j\}$ and arbitrary points on the map $M_w$, represented as a cost matrix $M_{cij}$ (line 5). For each iteration of the while-do loop in code lines 7–14, the next connection $C_{ij}$ to be performed is selected by finding the one with the minimum incremental cost, detailed in Equation (6), subject to the constraints imposed by $F_c$ (line 3). This process continues until all locations have been visited, as determined by the unvisited_nodes function (code line 7). The selected connection $C_{ij}$ is added to the planned route $L_{\mathrm{ord}}$ if appending it to the current route meets the constraints specified by Equations (8)–(10), as verified in the meet_constraints function of line 9. These constraints involve the vehicle payload capacity Q and the unfeasible connections $F_c$. If these constraints are violated, then the algorithm re-orders the route by removing the last node (i.e., harvest point) to be added and creating a new route with the current $C_{ij}$. In particular, as shown in code line 12, if all constraints are not achieved, then a new possible route is generated with a selected $C_{ij}$ using the create_new_route function. The route planning algorithm returns the harvest points in $L_{\mathrm{ord}}$ (see code code line 15).

Algorithm 1: Improved TSP-CVRP

Require:    1: $H_p\leftarrow$ Set harvesting points to visit 2: $M_w\leftarrow$ Build map of geo-localized weights for each $H_p$ 3: $F_c\leftarrow$ Set unfeasible harves point connections 4: $Q\leftarrow$ Set maximum vehicle payload capacity Ensure:  $L_{\mathrm{ord}}$ 5: Initialize $M_{cij}\leftarrow \mathrm{calculate\_distances}(H_p,M_w)$ 6: Initialize empty route: $L_{\mathrm{ord}}\leftarrow \varnothing$ 7: while  $\mathrm{unvisited\_nodes}(M_{cij},L_{\mathrm{ord}})$do 8:     $C_{ij}\leftarrow \mathrm{node\_min\_cost}(M_{cij},F_c)$ with Equation (6) 9:     if $\mathrm{meet\_constrains}(L_{\mathrm{ord}}\cup \left\{C_{ij}\right\},Q,F_c)$ with Equations (8)–(10). then 10:         $L_{\mathrm{ord}}\leftarrow L_{\mathrm{ord}}\cup \left\{C_{ij}\right\}$ 11:     else 12:         $L_{\mathrm{ord}}\leftarrow \mathrm{create\_new\_route}\left(C_{ij}\right)$ 13:     end if 14: end while 15: return  $L_{ord}$

4.2. Processing of the Navigation Map

The navigation map is assumed to be a top-view image of the harvesting area in the field. The map is processed according to the steps depicted in Figure 3. Prior to the environmental rendering and conditioning processes it is necessary to identify the structural layout of the agricultural environment (the green alleys in Figure 3a) and slip zones (the brown areas) by extracting and refining essential features of the navigation map that could represent obstacles to the robot’s maneuverability. The resulting map is used as raw input data representing free or occupied zones within the proposed path planning approach. As an initial step, an RGB vision sensor captures an image I from the robot workspace (see Figure 3a). Subsequently, the background is removed from I to isolate structural features irrelevant to the navigation map [60], resulting in $I_n$. From $I_n$, a new image ${\mathcal{I}}_b$ is binarized to simplify the map representation and reduce noise (see Figure 3b). Then, a nonlinear median blur filter is applied to $I_b$ for noise reduction and smoothing while preserving the contour edges and boundaries between obstacles and free space. After excluding spurious readings from $I_b$, the navigation map is available and can be complemented with new information regarding the navigation terrain.

The structural map is augmented with information arising from previous navigation scenarios on geolocalized areas exposed to potential slip phenomena. This information comprises friction coefficients computed with the wheel slip ratio and sideslip angles, which are assigned to each map location in $I_b$. Due to existing heterogeneous slipping areas, the associated slip region outlines are detected and enclosed in re-sizable rectangles (see Figure 3c). Then, adaptive thresholds are used to expand the blurred image $I_b$ with a contour area $b_s$. This allows the internal regions of structural obstacles to be filled in order to avoid unnecessary path searching (see Figure 3d).

The proposed method accurately identifies structural obstacles by detecting contours and using artificial areas in the adaptive thresholding process, as shown in image ${\mathcal{I}}_{b_c}$ of Figure 3e. In particular, the rectangular contours of the farm aisles are also detected and filled to enclose and highlight structural obstacle regions in the occupancy grid. The detection of crop rows effectively contributes to delineating the navigation space and further reducing noise. The bounding rectangles of the retained contours are then used to modify the occupancy grid $O_g$, setting the corresponding regions to be either free or occupied. Finally, the structural and slippage maps are joined in a single navigation map, as shown in Figure 3f.

In the map representation of the navigation environment, each cell (or pixel) corresponds to a relatively small region in the physical world. By classifying these cells as occupied or free, the proposed strategy can identify efficient navigation paths through complex scenarios involving structural obstacles, non-structural obstacles, and slippery zones. Specifically, the occupancy grid is defined as a 2D binary matrix $O_g$ in which each element $O_g(i,j)$ indicates whether the corresponding cell at row and column is free (0) or occupied (1). Thus, each element in $O_g$ directly maps to a corresponding element in the image representation $O_g$.

4.3. Local Path Planning

After the ordered sequence of harvesting points $L_{\mathrm{ord}}$ has been generated to guide the harvest task, it is necessary to generate a navigation path to connect these points while avoiding obstacles and potential slippage areas. In this paper, we select the ${\mathrm{RRT}}^{*}$ [61] algorithm, adapted to incorporating feasibility conditions allowing the robot to navigate in low-friction terrain under dynamics constraints and to enhance adaptability in the path search progress. Specifically, the proposed local planner combines the principles of ${\mathrm{RRT}}^{*}$ with probabilistic sampling to learn efficient exploration of the environment by paths that are kinematically compatible with the robot dynamics [62]. Furthermore, it integrates the dynamical model of the SSMR and the traversability terrain model to ensure that the generated paths are capable of avoiding slip situations.

The proposed ${\mathrm{IRRT}}^{*}$ algorithm is designed in six steps, considering a pair of given harvesting points assigned as start and goal points within the workspace. The steps for the path planning process are depicted in Figure 4, whereas lines of pseudocode are presented in Algorithm 2. In the first step, the proposed algorithm generates a random sample within a bounded region describing an adaptable area that guides the path search. A second step involves extending the path on a tree towards the nearest node that could connect to the next sample node. Before linking the previous nodes, the connection feasibility is checked in step three to ensure that the next node meets the kinematic and slip constraints. Subsequently, a set of neighbor nodes around the potential new node is identified within a radius that changes according to the number of nodes in the tree. Then, the neighbor node (i.e., parent) that minimizes the path distance to each neighbour is selected from this set. After the best node (i.e., the node with minimum cost) is selected, the new node is connected to the best parent and added to the tree, as illustrated in the parent node of step five (see Figure 4). A tree rewiring process is finally carried out with the nearest neighbors that achieve the lowest path cost, and path smoothing is carried out between nodes of the resulting tree.

The previous planning process is detailed in Algorithm 2 and explained as follows. Briefly, the proposed algorithm requires initializes parameters, the environment map, and a tree structure $\mathcal{T}$ (see code lines 1–9). Then, the ordered harvest waypoints $L_{\mathrm{ord}}$ from the TSP-CVRP path planning are set in code line 1; only consecutive pairs are used for searching local paths (see code lines 7–8). In practice, we obtained the planner parameters by guaranteeing reachability and path completeness according to the methodology suggested by [61]. In addition, the obtained parameters were tuned based on balancing path planning accuracy and computational efficiency in order to maintain the overall path planning performance. The path search process is performed through the while-loop in code lines 10–28. A start and goal node are assigned from the harvest points. Thus, for each iteration, a new node $z_{\mathrm{new}}$ is obtained by expanding the tree $\mathcal{T}$ from a random sample $z_{\mathrm{rand}}$ towards the nearest node $z_{\mathrm{nearest}}$ (see lines 11–13). Here, the robot’s steerability between the past and new nodes is checked before expanding, which uses the turning radius and holonomic speed constraints of the skid–steer model in Equation (2). It is essential to consider both the minimum and maximum turning radius to ensure the steering feasibility and safety of the robot’s navigation. For SSMRs, the minimum turning radius $r_{f,\min }\approx a/2$ is essential for ensuring the robot’s maneuverability in confined spaces or near obstacles requiring tight turns. The maximum turning radius, on the other hand, guarantees safety by preventing excessive deviations from the planned path. Specifically, $r_f$ can be adjusted using $r_{f,\max }=\max \left(r_{f,\min },{\displaystyle \frac{d_{\mathrm{obs}}}{f_{\mathrm{obs}}}}\right)$, where $f_{\mathrm{obs}}$ is the distance to the nearest obstacle and $f_{\mathrm{obs}}$ is a scaling factor used to tune sensitivity of the change in radius. According to code line 14, the feasibilty_check function verifies that the link segment between the new and nearest node does not intersect any obstacles in the map $O_g$, including structural obstacles and terrain traversability conditions on potential slip zones. In particular, the feasibility check function examines possible collisions across a navigation area rather than single points in a path, considering a circular region of radius $r_f$ and center $z_{\mathrm{new}}$, as shown below.

$$\forall z\in \left|\right|z_{\mathrm{nearest}}-z_{\mathrm{new}}\left|\right|\le r_f^2:O_g\left(z\right)\ne 0,r_{f,\min }\le r_f\le r_{f,\max }$$

(13)

Algorithm 2: Informed ${\mathrm{RRT}}^{*}$ (${\mathrm{IRRT}}^{*}$)

Require:    1: $L_{\mathrm{ord}}\leftarrow$ Set a list of ordered $H_p$ 2: $O_g\leftarrow$ Get a navigation map 3: $N\leftarrow$ Number of probable samples 4: $\gamma \leftarrow$ Sample rate 5: $N_s\leftarrow$ Neighbour size Ensure:  $path:\mathbf{p}$ 6: for  i  in  $L_{\mathrm{ord}}$  do 7:     $z_{\mathrm{start}}\leftarrow L_{\mathrm{ord}}\left[i\right]$ 8:     $z_{\mathrm{goal}}\leftarrow L_{\mathrm{ord}}[i+1]$ 9:     $\mathcal{T}\leftarrow$ initialize_tree$(\varnothing ,z_{\mathrm{start}},\mathcal{T})$; 10:     while $j\le N$ do 11:         $z_{\mathrm{rand}}\leftarrow$ sample_node($z_{\mathrm{start}}$,$\gamma$) 12:         $z_{\mathrm{nearest}}\leftarrow \mathrm{nearest\_node}(\mathcal{T},z_{\mathrm{rand}})$ 13:         $z_{\mathrm{new}}\leftarrow \mathrm{expand}(z_{\mathrm{nearest}},z_{\mathrm{rand}})$ 14:         if $\mathrm{feasibility\_check}(z_{\mathrm{nearest}},z_{\mathrm{new}},O_g)$ then 15:            $r_{neighbour}\leftarrow \mathrm{compute\_extended\_radius}\left(N_s\right)$ 16:            $\mathcal{N}\left(z_{\mathrm{new}}\right)\leftarrow \mathrm{find\_neighbours}(\mathcal{T},z_{\mathrm{new}},r_{neighbour})$ 17:            $z_{\mathrm{best}}\leftarrow \mathrm{select\_best\_parent}(\mathcal{N}\left(z_{\mathrm{new}}\right),z_{\mathrm{new}})$ 18:            if $\mathrm{feaibility\_check}(z_{\mathrm{best}},z_{\mathrm{new}},O_g)$ then 19:                $\mathcal{T}\leftarrow \mathrm{insert\_node}(\mathcal{T},z_{\mathrm{new}},z_{\mathrm{best}})$ 20:                $\mathcal{T}\leftarrow \mathrm{rewire\_tree}(\mathcal{T},z_{\mathrm{new}},\mathcal{N}\left(z_{\mathrm{new}}\right))$ 21:                $\mathcal{T}\leftarrow \mathrm{smooth\_tree}(\mathcal{T},z_{\mathrm{new}})$ 22:            end if 23:         end if 24:         if $\mathrm{path\_to\_goal}(\mathcal{T},z_{\mathrm{goal}})$ then 25:            $\mathbf{p}\leftarrow$ smooth_path($\mathcal{T}$); 26:            break 27:         end if 28:     end while 29: end for

Navigation feasibility is analogously verified in this same feasibility_check function, considering the terrain traversability by comparing traction forces $T_{f,kl}$ exerted by wheels and soil resistance forces $F_{f,kl}$ offered by the navigation surface (see Algorithm 3). These forces are obtained based on the slip ratios of the MF-wheel model in Equations (3) and (4) (see Algorithm 3). When the connection is feasible, the best node among a neighbor set and radius $r_{neighbour}$ is found according to the minimum distance criterion included in the select_best_parent function. If the best node again meets the feasibility constraints, it is added to $\mathcal{T}$ (code line 19) and the algorithm performs a rewiring process to connect all new nodes (line 20). The resulting tree may feature unfeasibly sharp turns and jagged edges, particularly when changing new exploration nodes extracted from the harvest points of the global path planning strategy. To address this concern, a piecewise polynomial interpolation strategy is applied for path smoothing (line 21). This involves fitting a cubic polynomial approximation between each pair of consecutive nodes, resulting in a smoothed tree. As a result, if the tree reaches the goal node in line 24, then the local path is set and the overall path $\mathbf{p}$ is connected in line 25 with all local paths after running the algorithm for all harvest points $L_{\mathrm{ord}}$.

Algorithm 3: Traversability checking

Require:    1: ${\sigma }_{s,kl}\leftarrow$ Slip ratios for $kl$-wheel 2: ${\mu }_x\left({\sigma }_{s,kl}\right)\leftarrow$ Coefficient of longitudinal static friction 3: ${\mu }_y\left({\sigma }_{s,kl}\right)\leftarrow$ Coefficient of lateral static friction 4: $F_{N,kl}\leftarrow$ Normal forces on each wheel surface 5: $T_{f,kl}\leftarrow$ Evenly allocated traction force on SSMR Ensure:  $\mathrm{Traversability}\mathrm{of}\mathrm{terrain}$ 6: $F_{f,kl}\leftarrow {\mu }_s·F_{N,kl}$ according to Equations (3) and (4) 7: return  $\mathrm{traversable}\leftarrow T_{f,kl}\ge F_{f,kl}$

4.4. Motion Controller for Assessment of Path Planning

Following one of the hypotheses of this work, which suggests that the proposed path planning is able to improve maneuverability in environments with obstacles and terrain constraints, the performance of the planned path is assessed in a motion control strategy for completeness as part of the trails. To do this, the planned path that connects the navigation route is parameterized with a constant speed profile between each pair of consecutive sampling waypoints, considering a robot sampling time $\Delta t$; thus, the parameterized path can be used as a reference for the motion control strategy, with the robot’s position obtained from a localization system used as feedback (see the general scheme of the proposed path planning strategy in Figure 1). For simplicity, in the implementation of the motion controller, a Proportional–Integral (PI) control technique based on algebraic approaches was used [32,63]; however, other motion control techniques could be used instead. The control parameters were previously tuned following the guidelines in [64]. For more details regarding the tracking controller, readers are encouraged to refer to [32].

5. Experimental Results and Analysis

This section presents the results of the proposed learning-based routing scheme for path planning of SSMRs in assisted harvesting tasks. The experiments were conducted in a multi-row crop environment designed to replicate structured farm fields. The test map was acquired from a geolocalized environment of a generic fruit orchard similar to the one located at the Lincoln Institute for Agri-Food Technology. To test motion control, we used the Coppelia V-Rep simulation software [65]. The experiment involved building a 60 × 60 m map with eight harvest alleys, each of which was distributed in 4 m × 3 m zones. In the environment, modifications were introduced to the navigation surface to change the conditions of the wheel–terrain interaction and intentionally produce slip phenomena. To explore the best path to transit waypoints between harvest alleys, a single waypoint was assigned to each alley under which the robot was tasked to navigate for crop harvesting.

The code used for implementing and testing the proposed solution is available as a GitHub repository (https://github.com/rickyurvina/path_planning_python_coppelia). The route planning framework for the global path planning method used the Path Cheapest Arc strategy under the Search Heuristics scheme available in the OR-Tools of Google [66] to solve the optimization problem composed by Equations (6)–(12). The trials were performed on a Windows-based computer equipped with an Intel^® Core^™ i9-10870H CPU @2.2 GHz (16 CPUs) and 32 GB of DDR4 RAM. This setup ensured that all computations required to achieve the proposed path planning strategies could be performed. The performance metrics are presented in the following section.

5.1. Performance Metrics

In addition to the path length and travel time, three performance metrics were introduced for numerical analysis of the local path planning approaches, following suggestions in previous works [56,67,68].

5.1.1. Obstacle Avoidance [68]

Path safety was assessed based on proximity of the path to obstacles. Using prior knowledge about the set of obstacle locations $\mathcal{O}$ in the map, the minimum distance d between each point on the path and the nearest obstacle for each point is provided by

$$d=\underset{o\in \mathcal{O}}{min}{\parallel \mathbf{p}-\mathbf{o}\parallel }_2,$$

(14)

where $\mathbf{p}=(x_{p,i},y_{p,i})$, with $i=0,\dots ,N-1$ being a given point on the planned path; $\mathbf{o}$ is the position of the nearest obstacle to the path; and $\parallel ·\parallel$ denotes the Euclidean norm. The assessment metrics for obstacle proximity are the average sum ${\overline{d}}_o$ of all distance values for each sample point on the path and the distance variability $\sigma$ provided by its standard deviation.

5.1.2. Path Smoothness [67]

This metric aims to measure path smoothness along the travel distance based on summing the acceleration components of the path such that

$$S_{path}={\displaystyle \frac{1}{{(\Delta t)}^2}}\sum _{t_k=0}^{t_{N-1}}{∥\left({\Delta }^2\left(x\left(t_k\right)\right),{\Delta }^2\left(y\left(t_k\right)\right)\right)∥}_2,$$

(15)

where $t_k$ is the time step of the parameterized path, N is the total number of points comprising the planned path, and $\Delta t$ is the time interval between steps. The path smoothness $S_{path}$ essentially quantifies the rate of curvature changes along the path, with relatively small values of $S_{path}$ indicating smooth paths.

5.1.3. Controller Effort [4]

The total motion controller effort can be computed as the contribution of the accumulative path tracking cost $C_{x,y}$ and control input $C_u$, as follows:

$$\begin{array}{cc}\hfill & C_{x,y}=\sum _{t_k=0}^{t_{N-1}}{k}_x{(x_{\mathrm{ref}}\left(t_k\right)-x\left(t_k\right))}^2+k_y{(y_{\mathrm{ref}}\left(t_k\right)-y\left(t_k\right))}^2\hfill \\ \hfill & C_u=\sum _{t_k=0}^{t_{N-1}}{k}_v{\tau }_v^2\left(t_k\right)+k_{\omega }{\tau }_{\omega }^2\left(t_k\right)\hfill \\ \hfill & C_{Tot}=C_{x,y}+C_u\hfill \end{array}$$

(16)

where $k_x$, $k_y$, $k_v$, and $k_{\omega }$ are positive constants. The constants are selected to weight and balance the magnitudes of different costs, enabling the assessment of control performance from different operating ranges of traction and turning control actions and tracking errors. Following [4], the values of these constants in the implementation were set as follows: $k_x=0.1$, $k_y=0.1$, $k_v=5$, and $k_w=10$.

5.2. Global Path Planning Evaluation

Several trials were carried out to evaluate the TSP-CVRP planner under varying vehicle load capacities and different route waypoints. Assessment criteria included route efficiency, resource utilization, and overall optimization of the harvesting process. Figure 5 presents a set of six route planning trials, showing routes that connect locations prioritized by the weight of the harvested product. These routes ensure that the total weight does not exceed the robot’s payload capacity while minimizing travel costs, which are represented by the distances between interconnected points. In particular, the experiments were performed while initializing different start/end points and several SSMR payload capacities to evaluate the total load harvested by the robot along the traveled route. For instance, in Figure 5a, for an SSMR payload capacity of 28 kg, the planner bypasses two collection points (8 and 10 kg) from the same alley, which could provide the shortest distance if harvesting 8 kg in the second alley. However, in this case the route planner prioritized the highest harvested weight by traveling to additional locations in the field. This prioritization achieved the maximum harvest load of 28 kg compared to the SSMR payload capacity. Furthermore, Figure 5b shows that when increasing the SSMR payload capacity to 42 kg, the planned route enables the robot to visit all harvest points, given a total product quantity of 41 kg (97.6%) distributed in the three alleys. In this case, achieving the maximum payload capacity is prioritized despite visiting all positions. Conversely, Figure 5c illustrates a scenario with a limited vehicle capacity of 13 kg, where the planner visits the nearest points of 8 kg and 5 kg and archives the maximum payload capacity before returning to the product depot. Subsequently, in Figure 5d–f, three additional tests feature several start and end harvest positions that do not match, as well as varying vehicle payload capacities. The results show that the planned route did not necessarily have to start and end at the product depot in order to maximize the harvested product (92.3%, 97.3%, and 91.1%), which is critical if the SSMR would have to restart the harvest process or to supply a harvest point that is not necessarily the final depot. The proposed algorithm is designed to initiate the search either from the depot or from any specified harvesting point. This flexibility allows the algorithm to start from different points, as demonstrated in Figure 5d–f, where the starting points are harvesting points instead of the depot. The algorithm prioritizes the load capacity up to the vehicle’s limit, ensuring that the vehicle does not exceed its maximum payload capacity while still achieving efficient harvesting routes. Moreover, all trials evidence that every single harvesting point in the routes was visited only once, avoiding back-and-forth travels between harvest alleys.

5.3. Local Path Planning Evaluation

This section presents the evaluation results of the local planner in a harvesting environment. The planner is a variant of the ${\mathrm{IRRT}}^{*}$ algorithm that incorporates the SSMR dynamics and a terrain traversability model. To qualitatively assess its performance, three local planners were tested, i.e., RRT, ${\mathrm{RRT}}^{*}$, and the proposed ${\mathrm{IRRT}}^{*}$.

Table 2. Parameters used in the path planning methods.

Parameter	RRT	RRT*	IRRT*
N Iterations	500	500	500
$r_f$ Safe navigation bounds	–	–	0.3 m
$N_s$ Neighbour size	2.5 m	2.5 m	2.5 m
$r_{neighbour}$ Extended ratio	1.2 m	1.2 m	1.2 m
$\gamma$ Sample rate	0.2	0.2	0.2
$F_{N,kl}$ Normal force per wheel	–	–	400 N

shows the parameters used to test these path planning approaches. Figure 6 illustrates three different trials conducted under equal execution parameters, including sample number, sampling rate, neighbor size, and start/end points. All three algorithms approached path planning in a way that completed the navigation task between two harvesting points and avoided the obstacles in the farm environment; however, they present different path planning characteristics. The left plot depicts the path obtained with the RRT algorithm, where an uneven path can be observed by simple inspection. The RRT path has abrupt turns and a wide distribution of sampled points across the navigation map. Conversely, when qualitatively assessing the performance of the ${\mathrm{RRT}}^{*}$ algorithm in the middle plot of Figure 6, the planned path is a slightly smoother; however, it still exhibits abrupt turns, even when the sampling process covers nearly the entire navigation map.

As shown on the right of Figure 6, the proposed algorithm ${\mathrm{IRRT}}^{*}$ yields a feasible solution for autonomous navigation as well as a smoother path when compared to the other two path planners, RRT and ${\mathrm{RRT}}^{*}$. ${\mathrm{IRRT}}^{*}$ outperforms the other two comparison path planners in terms of exploration, as the proposed approach only searched for confined areas close to the endpoint (as shown by green-yellow lines of Figure 6). Furthermore, ${\mathrm{IRRT}}^{*}$ operates within a safe navigation area, delineated by the black dotted lines. This constraint area was effectively extended around the path solution to avoid uncertain obstacles that may appear in the environment. This feature provides a significant advantage over RRT and ${\mathrm{RRT}}^{*}$, as it ensures robust performance of the path planner in the presence of uncertain obstacles in the environment, thereby offering further flexibility and path adaptability. It is important to note that this robustness can result in more conservative path planning outcomes.

Figure 7 shows the results of the three different test approaches, illustrating the ability of the path planners to avoid structural and randomly located box obstacles in a farm scenario. In this test, the visiting order of the harvest points was taken as a reference to connect different start and end points along the whole harvesting task. It can be observed that while all three methods effectively avoid the structural and box obstacles, the proposed ${\mathrm{IRRT}}^{*}$ again generates smoother and safer paths than the other two path planners. Furthermore, unlike RRT and ${\mathrm{RRT}}^{*}$, the proposed algorithm avoids returning along counter-directions, preventing kinematically incompatible turns that SSMR robots are unable to perform. Instead, the proposed algorithm maintains the same route or smoothly adjusts the path to continue navigating (see scenarios 2 and 3 for ${\mathrm{IRRT}}^{*}$ in Figure 7).

Figure 8 shows the path planning results for each path planner in three different scenarios under conditions with structural obstacles and variable-traction terrain. This test focused on evaluating the ability of the path planners to avoid areas that are prone to slip. Minimum and maximum friction coefficients were used to represent the widest range of slip conditions, indicating areas where a robot may experience low and high traction terrain. The three local path planners were capable of connecting all scheduled points provided by the TSP-CVRP planner, and each waypoint was visited only once. In particular, RRT was not able to present adaptive capabilities to slip conditions in any of the three scenarios. These results show that while the RRT and ${\mathrm{RRT}}^{*}$ methods effectively avoided structural obstacles along the planned route, they failed to navigate on low-traction terrain, although slip areas were also considered obstacles in the environment. Moreover, in most scenarios both RRT and ${\mathrm{RRT}}^{*}$ provided unfeasible paths for SSMRs that require them to encounter kinematic constraints. On the other hand, the proposed ${\mathrm{IRRT}}^{*}$ effectively connects the prioritized route points along off-slip travel, generating paths that are kinematically compatible with the robot’s dynamics. The ${\mathrm{IRRT}}^{*}$ algorithm prioritizes navigating in high-traction regions while generating obstacle-free paths. A particular case can be observed in the ${\mathrm{IRRT}}^{*}$ results for scenes 1 and 3, shown in Figure 8c–i, where the path had to visit the same harvest point twice. This result is reasonable for the particular scenario, as the prioritized harvesting point was enclosed by a low-traction terrain region and no other feasible solution was available that maintained the shortest local path length.

5.4. Evaluation of Path Tracking Performance

To perform a quantitative performance assessment, we conducted 150 motion control trials using preplanned paths (see Figure 9 and Figure 10). An example of the results is presented in Figure 9 and summarized in

Table 3. Performance metrics of the path planning approaches.

Method	Path Planning				Obstacle Avoidance
	Path Length [m]	Travel Time [s]	Execution Time [s]	Smoothness  $S_{path}$ [m/s2]	Obstacle Prox.  ${\overline{d}}_o$ [m]	Distance Variability  $\sigma$  [m]
RRT	47.4	1.8	7.8	1.97	2.0	±0.31
RRT*	41.7	41.6	8.3	1.18	2.2	±0.35
IRRT*	64.4	15.2	17.6	0.046	2.9	±0.28

. In all trials, all the three tested approaches consistently avoided structural obstacles in the farm environment; however, only ${\mathrm{IRRT}}^{*}$ successfully avoided low-traction surfaces with potential slip risks. The obstacle proximity for each approach was consistent, maintaining a safe collision distance; however, the greatest obstacle clearance was achieved with ${\mathrm{IRRT}}^{*}$ throughout the travel time, demonstrating adaptability to environmental obstacles (see Figure 10a,b). The obstacle avoidance capability of ${\mathrm{IRRT}}^{*}$ also facilitated suitable maneuverability on challenging terrain, although the total length of the reference path for ${\mathrm{IRRT}}^{*}$ (64.4 m) was longer than those obtained with ${\mathrm{RRT}}^{*}$ (47.4 m) and RRT (41.7 m). Similarly, ${\mathrm{IRRT}}^{*}$ required a longer time to navigate the path compared to the other methods, which is reasonable, as the proposed approach prioritized fully avoiding obstacles over taking the shortest path, resulting it its fully traveling around the harvesting row. The ${\mathrm{IRRT}}^{*}$ approach demonstrated longer execution times relative to other tested path planning techniques, which can be attributed to its incorporation of additional information during sampling and its consideration of wheel–terrain interaction in the exploration stage, contributing to more efficient guidance of the robot towards the goal. In addition, ${\mathrm{IRRT}}^{*}$ produced kinetically compatible paths with good smoothness, 97.63% smoother than RRT and 96.1% smoother than ${\mathrm{RRT}}^{*}$, as shown in Figure 10c.

The performance metrics of the motion controllers are presented in

Table 4. Performance metrics of the motion controllers using previously planned paths.

Method	${\mathit{C}}_{\mathit{x}}$ [m]	${\mathit{C}}_{\mathit{y}}$ [m]	${\mathit{C}}_{\mathit{u}}$ [${\mathit{m}}^2$]	${\mathit{C}}_{\mathit{T}\mathit{o}\mathit{t}}$
RRT	178.96	132.06	93.4	311.02
${\mathrm{RRT}}^{*}$	84.30	131.03	47.7	215.33
${\mathrm{IRRT}}^{*}$	28.21	173.71	22.3	201.03

. With ${\mathrm{IRRT}}^{*}$, the cumulative path tracking error for the trial, which is related to the ability to avoid obstacles, barely increases over RRT and ${\mathrm{RRT}}^{*}$. However, the control effort of ${\mathrm{IRRT}}^{*}$ is reduced by about 76.1% compared to RRT and 53.1% compared to ${\mathrm{RRT}}^{*}$, which is attributed to choosing qualified paths with greater smoothness than those selected by the other path planners.

6. Conclusions

This article has presented an integrated route and path planning strategy for Skid–Steer Mobile Robots (SSMRs) in assisted harvesting tasks within complex agricultural environments. The proposed approach effectively combines a global planning algorithm based on the Traveling Salesman Problem (TSP) and the Capacitated Vehicle Routing Problem (CVRP) with a local path planner based on the Informed Rapidly-exploring Random Tree Star (${\mathrm{IRRT}}^{*}$) algorithm. The global planning strategy prioritizes harvesting locations in such a way that all possible waypoints are connected with the minimum path length and without exceeding the maximum payload capacity of the vehicle. This approach is aimed at generating an ordered sequence of harvesting points that maximizes the harvested product yield within the workspace. The local ${\mathrm{IRRT}}^{*}$ planner is used to connect the scheduled harvesting points while simultaneously avoiding structural objects and obstacles characterized by low-traction terrain regions. In particular, ${\mathrm{IRRT}}^{*}$ generated smoother and safer routes compared to RRT and ${\mathrm{RRT}}^{*}$ while maintaining larger average distance to obstacles and reducing control effort in slippery zones.

The proposed path planning scheme incorporates the SSMR motion dynamics and a traversability model to ensure kinematically compatible paths that meet terrain constraints. By incorporating terrain traversability and kinematic constraints, the proposed strategy mitigates the risk of robot entrapment and enables efficient navigation over shortened routes and qualified smooth paths. Our experimental results demonstrate that the proposed ${\mathrm{IRRT}}^{*}$ outperforms the classical RRT and ${\mathrm{RRT}}^{*}$ alternatives by achieving 96.1% and 97.6% smoother paths, respectively, under terrain constraints. The ${\mathrm{IRRT}}^{*}$ approach also exhibits improved navigation efficiency, avoiding both obstacles and potential slippage zones. Future work might explore the integration of additional slip sensing systems and dynamic obstacle detection to further enhance robustness and path adaptability in real-world agricultural settings.

For reproducibility of this work, the code of the proposed algorithm and trial data will be made publicly available.