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

1. Introduction

The Food and Agriculture Organization (FAO) forecasts that by 2030, a 70% increase in agricultural productivity will be necessary to sustain the growing global population of nearly 9 billion people [1]. 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. Thus, 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 primary tasks such as tillage, watering, sowing, fertilizing, and spraying, as well as 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, and 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, quantity and quality of collected products. This challenging process, though operationally straightforward, 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 or vegetables, could jeopardize both the quantity and quality of the harvested product, entailing 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 to enhance 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, where either manual or autonomous vehicles attend to the crop harvest by quickly positioning themselves near these harvest points. 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 [9,10]. However, a combined local path planning is usually required to guide the autonomous vehicle between global harvesting points, as proposed in this work. Beyond TSP, various methods such as those based on the Vehicle Routing Problem (VRP) [11,12], ant colony optimization [13,14], genetic algorithms [15], and others using heuristics [16] have also been aimed at exploring ordered routes of minimum traveling distance and reduced time (see [17,18] [and references therein]). In practice, TSP-based methods [19], addressing further than route travel optimization, are able to impact on vehicle resource utilization, load motion capacity, and energy efficiency when integrated into local planners, as proposed here in this 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 [20,21,22,23]. However, there remains a lack of practical approaches to integrate the need for priority waypoints within harvesting maps, all 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 sampled-based approaches such as Dijkstra, A*, or Probabilistic Road Mapping (PRM) [24], to name a few. 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 proper of the inherent complexity of the environment. Therefore, these methods do not assure generating kinematically compatible paths with the robot’s motion dynamics. To address such limitations, probabilistic techniques focused on Rapidly-exploring Random Trees (RRT) [25,26,27,28] 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 [29]. However, only a few of its variants have demonstrated capabilities 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 this 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, underlying insights of 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 harvest points based on vehicle payload capacity constraints while considering closeness between crop harvest locations.
• A local path planning coordinated with a global one that explores scheduled harvesting points based on ${\mathrm{IRRT}}^{*}$ to achieve feasible and kinematically compatible paths with SSMR dynamics, accounting for obstacle avoidance and terrain traversability posed by low-traction surfaces.
• The proposed strategy can generate paths that avoid difficult-to-crossing regions typical of agricultural farms, intending to reduce robot resources and prevent situations where robots get stuck.
• The proposed planner can be used as a reference for motion controllers, enabling efficient driving over shortened and qualified paths, potentially reducing exposure times of products to the environment and thus the mistreatment of the agricultural harvest.

Beyond other planners that only consider the minimization of path length and disregard the agricultural production process, the proposed work also considered predicted crop yield maps with multiple harvesting locations between farm aisles 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 or unforeseen obstacles are detected around the SSMR 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, and then such an image is processed to provide a deployed representation proper for designing the planner. Then, taking 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 by maximizing 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 performance of the proposed strategy using metrics of path length, travel time, smoothness, proximity to obstacles, whereas metrics of cumulative tracking errors, controller effort, and total cumulative costs for the test controller.

This paper is organized as follows. The 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 of the SSMR dynamics and the 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. In Section 5, it is discussed several experimental results. Finally, conclusions of the work are given in Section 6. Nomenclature of variables used throughout this paper is summarized in Table 1.

2. Related Work

Beyond generating a navigation path either for manual or assisted guidance tasks [17], a path planning strategy requires a sequence of waypoints arranged to form a traversable route. In agriculture, such order is primarily driven by the need to prioritize navigation points, generally considering operative aspects such as closeness between harvesting locations, harvest product capacity, distribution of workers across rows, and product collection time [30]. For instance, in [29], 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 aimed to minimize path length and execution times by spatially separating multiple regions, it targeted 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 to incorporate capabilities from additional path-planning techniques. The path planning problem can also be formulated as a variant of the TSP [9,13,17], where travel costs between individual targets could 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), as a variant of the TSP, has been developed involving vehicles with limited payload capacity [31]. In particular, the CVRP aims to find an 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 [32,33]. Whereas global path-planning approaches are effective in many scenarios, they may be inadequate in environments where robots must navigate around obstacles [34]. Consequently, more adaptable path planners are required 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 [25] and their variants are commonly used for local path planning with obstacles, as proposed in unstructured layouts of orchard environments [35]. In that work, kinematic constraints were met and obstacles were avoided fusing RRT with artificial potential field techniques. Similarly, current works on sampled-based techniques and global planning methods integrate maneuverability criteria to optimize paths subject to speed constraints [36]. Considering terra-mechanical models of Hunt-Crossley in the design of path planners also allowed autonomous ground vehicles to adapt the maneuverability control to different uneven and flexible navigation surfaces [37]. Several agricultural applications using hybrid techniques based on RRT can be found in spraying, planting, and harvesting tasks [38,39,40,41,42]. However, to the best of the authors’ knowledge, there is a lack of integrated path-planning approaches that simultaneously consider sorted harvesting points according to the robot payload capacity and account for wheel-terrain interactions. Most path planning works consider static and dynamic obstacles surrounding the vehicle but 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 a reduced tractive force exertion and wheel slippage [43]. Therefore, path planners that involves terrain traversability criteria to overcome terrain obstacles are more than welcome.

In contrast to the previous routing approaches, typical path planning methods for solving the shortest path problem encompass a variety of methodologies, where each of them arises strengths and limitations. For example, grid-based algorithms as Dijkstra’s and A* [44], offer resolution completeness but could become computationally burdensome for high-dimensional and complex problems. Conversely, sampling-based approaches such as RRT[45], Probabilistic Roadmaps (PRM) [46] and their variants [47] demonstrate asymptotically optimality and efficiency in irregular environments, particularly in navigating high-dimensional spaces. 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 start and end point locations, search space size, sampling efficiency, and unexpected obstacles. To address those aspects, the work in [24] 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 environment exploration poses a challenge for RRT-based path construction, as highlighted in [48]. In [49], it was introduced a hybrid RRT approach to improve workspace exploration by incorporating probabilistic learning techniques. The proposed method concurrently generates growing trees to explore probable boundary samples that could reach the destination point increasingly faster. An extended work is proposed in [50], where the selection of samples is made by filtering waypoints through clustering methods. On the other hand, evolutionary algorithms such as genetic approaches and optimization-based methods [51] offer additional advantages for efficiently solving high-dimensional problems. Nevertheless, they may encounter challenges such as being trapped in local optima. Additionally, incorporating the robot dynamics into path planning could be achieved by formulating an optimal control problem [52]. Nevertheless, this approach may face similar challenges as grid-based methods when dealing with high-dimensional spaces. For this reason, a variant of ${\mathrm{RRT}}^{*}$ based on heuristics is used as a complementary tool for path planning integration.

3. Motion Models for Assisted Harvesting Tasks

Previous to the path-planning design, it is required to introduce a motion model of the test 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]. 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 is 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 nonholonomic constraints:

$$\begin{array}{cc}\hfill & -\dot{x}sin\theta +\dot{y}cos\theta -\left[v_y+\left(l\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 pose; $v_x$, $v_y$, and ${\omega }_z$ are the longitudinal, lateral, and angular speeds, respectively. The parameters l and a are length and width of the SSMR, respectively. The vector of model parameters $p=[p_1,...,p_8]$ is obtained according to robot responses against torque inputs, and they are previously estimated in [53]. For more comprehensive information regarding the model, readers are referred to [43].

3.2. Model of the Terrain Traversability

The Coulomb friction model for the wheel-terrain interactions, as outlined in [54], 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 [55], is employed to characterize tangential forces for each wheel, capturing the complex wheel-terrain behavior:

$$\begin{array}{cc}\hfill F_{ijx}={\mu }_x& \left({\sigma }_{sij}\right)F_{N,ijz},F_{ijy}={\mu }_y\left({\alpha }_{ij}\right)F_{N,ijz}\hfill \\ \hfill {\mu }_x\left({\sigma }_{sij}\right)& =D_{x}sin\left(C_{x}arctan\left(B_x{\sigma }_{sij}\right)\right)\hfill \\ \hfill {\mu }_y\left({\alpha }_{ij}\right)& =D_{y}sin\left(C_{y}arctan\left(B_y{\alpha }_{ij}\right)\right)\hfill \\ \hfill {\mu }_s& =∥({\mu }_x,{\mu }_y)∥\hfill \end{array}$$

(3)

where ${\left[B_{\left(·\right)}{C}_{\left(·\right)}{D}_{\left(·\right)}\right]}^T$ represent parameters of the wheel model obtained from [56]; ${\mu }_x$, ${\mu }_y$, and ${\mu }_s$ are coefficients of 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:

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

(4)

where $s_{ij}$ represents the difference between the linear speed ${\omega }_{ij}{r}_w$ and longitudinal speed $v_{ijx}$ 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 given in this work since 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) processing of the navigation map, ii) planning of the global path based on the TSP-CVRP approach, and iii) planning of the local path with Informed ${\mathrm{RRT}}^{*}$ a.k.a. ${\mathrm{IRRT}}^{*}$, as illustrated in Figure 1. The following Sections detail each of these scenarios.

4.1. Processing of the Navigation Map

A navigation map is assumed to acquire a top-view image of the harvesting area in the field, and then it is processed according to steps depicted in Figure 2. In the initial step, an RGB vision sensor captures an image I from the robot workspace (see Figure 2a). Subsequently, the background is removed from I to isolate structural features irrelevant to the navigation map [57], obtaining $I_n$. From $I_n$, a new image ${\mathcal{I}}_b$ is binarized to simplify the map representation and reduce noise (see Figure 2b). Then, a non-linear 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 $I_b$ is excluded from spurious readings, the navigation map is available for complementing with new information regarding the navigation terrain.

The structural map is augmented with information on geo-localized areas exposed to the potential slip phenomena, arising from previous navigation scenarios. Such information comprises friction coefficients computed with the wheel slip ratio and sideslip angles, being 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 2c. Then, adaptive thresholds are used to expand the blurred image $I_b$ with a contour area $b_s$ so that internal regions of structural obstacles could be filled to avoid unnecessary path searching (see Figure 2d).

By detecting contours and using artificial areas in the adaptive thresholding process, the method accurately identifies structural obstacles as shown in image ${\mathcal{I}}_{b_c}$ of Figure 2e. In particular, rectangular contours of the farm isles are also detected and filled to enclose and highlight structural obstacle regions in the occupancy grid. The crop rows detection 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 free or occupied. Finally, the structural and slippage maps are joined in a single navigation map, as shown in Figure 2f.

Map representation of the navigation environment, where 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$, where 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.2. 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, visiting the same location only once, prioritizing the maximum harvest for each row, ensuring vehicle payload capacity, and maximizing closeness between harvesting locations. Hence, to determine a path that fulfills the former objectives, the TSP-CVRP algorithm is selected. This choice is made 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 connection comprises P as the set of ordered harvesting locations $H_p=\{h_0,h_1,...,h_p\}$ with $h_0$ denoting the initial crop depot and $h_p$ as the crop end-point. 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}=\frac{D_{ij}\times f_w}{w_j}$$

(5)

where $D_{ij}$ is the Euclidean distance to quantify closeness between two any harvesting points; $f_w$ is a factor prioritizing reduced-distance connections, and $w_j$ is the weight for scheduling the order of travel points according to a distance $D_{ij}$ between harvesting locations in $H_p$. For each harvesting point $H_p$, it is assigned a specific demand $q_j$ and a single harvest depot denoted by $q_0$. The accumulated harvest product obtained until weight $w_{ij}$ in the arrival position in j along the route search is limited to not overload the maximum load capacity Q of the SSMR. Thus, as it is required to find a route connecting ordered harvesting points with minimal total distance and payload constraints, the proposed optimization problem is:

$$\begin{array}{cc}\hfill \mathrm{Minimize}\text{:}& \sum _{i=1}^n\sum _{j=1}^n{C}_{ij}{\delta }_{ij}\hfill \\ \hfill \mathrm{Subject}\mathrm{to}\text{:}& \hfill \end{array}$$

(6)

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

(7)

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

(8)

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

(9)

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

(10)

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

(11)

where ${\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 node i to j and ${\delta }_{ij}=0$ otherwise; $w_j$ denotes the product demand in weight assigned to a particular arriving node j; Q is the vehicle capacity for the maximum payload to haul along the overall planned route; $F_c$ is the set of unfeasible connections due to inaccessible workspace from opposite sides of the same harvesting row; n is the total number of nodes. The auxiliary variables $u_{i,j}$ are introduced to prevent the route from excluding the initial and endpoint harvest position, 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 computations of individual costs $C_{ij}$ for all connections between nodes $i,j$. The constraints ensure that each node is visited exactly once, whereas the total weight on any traversed route does not exceed the maximum payload limit Q (see Eq. 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). Such harvest map is augmented with a separate map $M_w$, which incorporates geo-localized weights associated with each point in $H_p$ (see code line 2). A list of unfeasible connections $F_c$ are previously identified with the binary constraint function and incorporated to $M_w$. Before the execution of the TSP-CVRP algorithm, it is also specified the maximum payload Q under which the robot is capable of hauling throughout the navigation route (see code line 4).

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 node connections   4: $Q\leftarrow$ Set maximum vehicle payload capacity   Ensure:  $L_{\mathrm{ord}}$   5: Initialize $M_{cij}\leftarrow \mathrm{calculate}\_\mathrm{calculate}(H_p,M_w)$   6: Initialize empty route: $L_{\mathrm{ord}}\leftarrow \varnothing$   7: while$\mathrm{unvisited}\_\mathrm{nodes}(M_{cij},L_{\mathrm{ord}})$do   8:     $C_{ij}\leftarrow \mathrm{node}\_\min \_\mathrm{cost}(M_{cij},F_c)$ with Eq. 6   9:     if $\mathrm{meet}\_\mathrm{constraints}(L_{\mathrm{ord}}\cup \left\{C_{ij}\right\},Q,F_c)$ with Eqs. 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}\_\mathrm{new}\_\mathrm{route}\left(C_{ij}\right)$   13:     end if  14: end while  15: return$L_{ord}$

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 node $C_{ij}$ to visit is selected by finding the one with the minimum incremental cost detailed in Eq. 6, subject to constraints imposed by $F_c$ (line 3). This process continues until all locations are visited, as determined by the unvisited_nodes function (code line 7). The selected node $C_{ij}$ is added to the planned route $L_{ord}$ if appending it to the current route meets constraints specified by Eqs. 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, the algorithm re-orders the route by removing the last node added and creating a new route with the current node $C_{ij}$. In particular, as shown in code line 12, if all constraints are not achieved, a new possible route is generated with a selected node $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).

4.3. Local Path Planning

Once the ordered sequence of harvesting points $L_{ord}$ is generated to guide the harvest task, it is required to generate the navigation path to connect such points while avoiding obstacles and potential slippage areas. To do so, the ${\mathrm{RRT}}^{*}$[58] algorithm is selected to be adapted, incorporating feasibility conditions to navigate in low friction terrain under dynamics constraints and enhancing adaptability in the path search progress. Specifically, the proposed local planner combines principles of ${\mathrm{RRT}}^{*}$ informed with probabilistic sampling for learning the efficient exploration of the environment with paths kinematically compatible with the robot dynamics [59]. 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 couple of given nodes assigned as start and goal points within the workspace. The steps for the path planning process are depicted in Figure 3, whereas lines of pseudo-code 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. Then, 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, connection feasibility is checked in step three to ensure that the next node meets kinematic ans slip constraints. Subsequently, a set of neighbour nodes around the potential new node is identified within a radius changing according to the nodes in the tree. Then, it is selected the neighbour node (i.e., parent) from such set so that minimizes the path distance to each neighbour. Once the best node (i.e., node for 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 3). A tree rewire process is finally carried out with the nearest neighbours to 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 required to initialize parameters, the environment map and a tree structure $\mathcal{T}$ (see code lines 1-9). Then, it is set the ordered harvest waypoints $L_{\mathrm{ord}}$ from the TSP-CVRP path planning in code line 1 but only consecutive pairs are used for searching local paths (see code lines 7-8). The planner parameters were obtained in practice by guaranteeing reachability and path completeness according to the methodology suggested by [58]. Also, such parameters were tuned based on balancing path planning accuracy and computational efficiency, thus maintaining 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 stearability is checked between the past and new nodes before expanding, according to the maximum turning ratio and holonomic speed constraints of the skid-steer model in Eq 2. According to code line 14, it is verified with the feasibilty_check function if 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 follows:

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

(12)

Navigation feasibility is analogously verified in this same feasibility_check function, considering the terrain traversability by comparing traction forces $T_{f,ij}$ exerted by wheels and soil resistance forces $F_{f,ij}$ offered by the navigation surface. Such forces are obtained based on the slip ratios of the MF-wheel model in Eqs. 3 and 4 (see Algorithm 3). Once the connection is feasible, the best node among a neighbour set and radius r is found according to the minimum distance criterion included in the select_best_parent function. If the best node again meets feasible constraints and it is added to $\mathcal{T}$ (code line 19), the algorithm performs a rewiring process to connect all new nodes in line 20. The resulting tree may feature potentially unfeasible sharp turns and jagged edges, paricularly when changing new exploration nodes extracted from the harvest points of the global path planning strategy. Therefore, to address such concern, a piecewise polynomial interpolation strategy was applied for path smoothing in code line 21. Indeed, it involved 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, 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 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: foriin$L_{ord}$do     7:     $z_{\mathrm{start}}\leftarrow L_{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}\_\mathrm{node}(\mathcal{T},z_{\mathrm{rand}})$   13:         $z_{\mathrm{new}}\leftarrow \mathrm{expand}(z_{\mathrm{nearest}},z_{\mathrm{rand}})$   14:         if $\mathrm{feasibility}\_\mathrm{check}(z_{\mathrm{nearest}},z_{\mathrm{new}},O_g)$ then   15:            $r\leftarrow \mathrm{compute}\_\mathrm{extended}\_\mathrm{radius}\left(N_s\right)$   16:            $\mathcal{N}\left(z_{\mathrm{new}}\right)\leftarrow \mathrm{find}\_\mathrm{neighbours}(\mathcal{T},z_{\mathrm{new}},r)$   17:            $z_{\mathrm{best}}\leftarrow \mathrm{select}\_\mathrm{best}\_\mathrm{parent}(\mathcal{N}\left(z_{\mathrm{new}}\right),z_{\mathrm{new}})$   18:            if $\mathrm{feasibility}\_\mathrm{check}(z_{\mathrm{best}},z_{\mathrm{new}},O_g)$ then   19:                $\mathcal{T}\leftarrow \mathrm{insert}\_\mathrm{node}(\mathcal{T},z_{\mathrm{new}},z_{\mathrm{best}})$   20:                $\mathcal{T}\leftarrow \mathrm{rewire}\_\mathrm{tree}(\mathcal{T},z_{\mathrm{new}},\mathcal{N}\left(z_{\mathrm{new}}\right))$   21:                $\mathcal{T}\leftarrow \mathrm{smooth}\_\mathrm{tree}(\mathcal{T},z_{\mathrm{new}})$   22:            end if  23:         end if  24:         if $\mathrm{path}\_\mathrm{to}\_\mathrm{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

Algorithm 3 Traversability Checking

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

4.4. Motion Controller for the 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 so, the planned path that connects the navigation route was parameterized with a constant speed profile between each pair of consecutive sampling waypoints, considering a robot sampling time $\Delta t$. Thus, the parameterized path is used as a reference for a motion control strategy, and the robot position obtained from a localization system is used as feedback (see the general scheme of the proposed path planning strategy of Figure 1). For simplicity in the implementation of the motion controller, a Proportional-Integral (PI) control technique based on algebraic approaches was used [30,60]; however, other motion control techniques could be used instead. The control parameters were previously tuned following guidelines in [61]. For more details regarding the tracking controller, the reader is encouraged to refer to [30].

5. Experiments Results and Analysis

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

The code used for implementing and testing the proposed solution will be available as a GitHub repository1. 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 [63] to solve the optimization problem posed by the combinatorial search strategy of Eq. . The trials were performed on a Windows-based computer equipped with an Intel^® Core^™ i9-10870H CPU @2.2GHz (16CPUs) and 32GB of DDR4 RAM. This setup ensured all computations required to achieve the proposed path planning strategies. The performance metrics are presented in the following section.

5.1. Performance Metrics

Beyond the path length and travel time, for numerical analysis of the local path planning approaches, three performance metrics were introduced by following suggestions of previous works [53,64,65].

5.1.1. Obstacle Avoidance [65]

Path safety was assessed based on proximity of the path to obstacles. With 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 given by:

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

(13)

where $\mathbf{p}=(x_{p,i},y_{p,i})$, with $i=0,...,N-1$ is 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. Then, the assessment metrics for obstacle proximity are the average sum ${\overline{d}}_o$ of all distance values for each sample point on the path, as well as the distance variability $\sigma$ given by its standard deviation.

5.1.2. Path Smoothness [64]

This metric is aimed to measure path smoothness along the travel based on summing the acceleration components of the path such that:

$$S_{path}=\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$$

(14)

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_{\mathrm{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 [53]

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{T}_v^2\left(t_k\right)+k_{\omega }{T}_{\omega }^2\left(t_k\right)\hfill \\ \hfill & C_{Tot}=C_{x,y}+C_u\hfill \end{array}$$

(15)

where $k_x$, $k_y$, $k_v$, $k_w$ are positive constants that provide a consistent sum between costs along the path.

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 4 presents a set of six trials of route planning, depicting routes that connect locations prioritized to be visited according to the maximum weight of the harvested product with respect to the robot payload capacity and minimum travel costs represented by the interconnected-point distances. In particular, the experiments are performed 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 4a, for an SSMR payload capacity of 28 Kg, the planner bypasses two collection points (8 and 10 Kg) from the same allay, which could provide the shortest distance if harvesting 8 Kg of the second alley. However, in this case, the route planner prioritized the highest harvested weight by traveling to additional locations in the field. Such prioritization achieved the maximum harvest load of 28 Kg compared to the SSMR payload capacity. Furthermore, Figure 4b shows that 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 allays. In this case, it is prioritized intending to achieve the maximum payload capacity despite visiting all positions. Conversely, Figure 4c 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 Figs. Figure 4d-f, three additional tests feature several start and end harvest positions not matching among them, as well as varying vehicle payload capacities. Here it is shown that the planned route did not necessarily have to start and end at the product depot to maximize the harvested product (71.4%, 70.2%, and 80%), which is critical if the SSMR would have to re-start the harvest process or if it would need to supply a harvest point that is not necessarily the final depot. Moreover, all trials also evidenced that a single harvest point was visited only once, but also avoided back-and-forth travels between harvest allays.

5.3. Local Path Planning Evaluation

This section presents the results of evaluating the local planner in a harvesting environment, where the planner is a variant of the ${\mathrm{IRRT}}^{*}$ algorithm to incorporate SSMR dynamics and a terrain traversability model. To qualitatively assess performance, three local planners are tested, i.e., RRT, ${\mathrm{RRT}}^{*}$, and the proposed ${\mathrm{IRRT}}^{*}$. Table 2 shows the parameters used to test the path-planning approaches. Figure 5 illustrates three different trials conducted under equal execution parameters, including sample number, sampling rate, neighbour size, and start/end points. The three algorithms approached a planned path in such 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, wherein an uneven path is 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 5, the planned path presented a slightly smoother path. However, it still exhibits abrupt turns, even when the sampling process nearly covers the entire navigation map.

As shown in the right of Figure 5, it is observed that the proposed algorithm ${\mathrm{IRRT}}^{*}$ yielded a feasible solution for autonomous navigation, as well as a smoother path with respect to the other two path planners (i.e., RRT and ${\mathrm{RRT}}^{*}$). The ${\mathrm{IRRT}}^{*}$ outperforms the other two comparison path planners in terms of exploration since the proposed approach only searched for confined areas close to the endpoint (as shown in greenish-yellow lines of Figure 5). Furthermore, the ${\mathrm{IRRT}}^{*}$ operates on a safety navigation area delineated by 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}}^{*}$ because it ensures robust performance of the path planner under the presence of uncertain obstacles in the environment, thereby offering further flexibility and path adaptability.

Figure 6 shows the results of the three different test approaches, aimed at illustrating the ability 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 a whole harvesting task. It can be observed that all three methods effectively avoided the structural and box obstacles, and the proposed ${\mathrm{IRRT}}^{*}$ again generates smoother and safer paths with respect to the other two path planners (i.e., RRT and ${\mathrm{RRT}}^{*}$). Furthermore, unlike RRT and ${\mathrm{RRT}}^{*}$, the proposed algorithm avoids returning along counter-directions to prevent 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 6).

Figure 7 shows the path planning results for each path planner in three different scenarios under structural obstacles and variable-traction terrain conditions. This test focused on evaluating the path planning ability to avoid areas 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 given 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. It is shown that the RRT and ${\mathrm{RRT}}^{*}$ methods effectively avoided structural obstacles along the planned route but 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}}^{*}$ provide unfeasible paths for SSMRs that require them to meet kinematic constraints. On the other hand, it was observed that the proposed ${\mathrm{IRRT}}^{*}$ effectively connects the prioritized route points along off-slip travels, generating paths kinematically compatible with the robot’s dynamics. Indeed, the ${\mathrm{IRRT}}^{*}$ algorithm prioritizes navigating in high-traction regions while generating obstacle-free paths. Furthermore, a particular case was observed in ${\mathrm{IRRT}}^{*}$ for scenes 1 and 3 as shown in Figure 7c-i, where the path had to visit the same harvest point twice. Such a particular result was reasonable since the prioritized harvesting point was enclosed by a low-traction terrain region, and no other feasible solution that maintained the shortest local path length was available.

5.4. Evaluation of Path Tracking Performance

To quantitatively assess performance, it was conducted 150 motion control trials using pre-planned paths (see Figure 9). An example of the results is presented in Figure 8 and summarized in Table 3. It was demonstrated that all trials across the three approaches consistently avoided structural obstacles in the farm environment; however, only ${\mathrm{IRRT}}^{*}$ successfully avoided low-traction surfaces with potential slip risk. 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, demonstrating adaptability to environmental obstacles (see Figure 9a-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.4m) was longer than those obtained with ${\mathrm{RRT}}^{*}$ (47.4m) and RRT (41.7m). Similarly, ${\mathrm{IRRT}}^{*}$ required longer time to navigate the path compared to the other methods, which is reasonable since the proposed approach prioritized avoiding obstacles fully over taking the shortest path, thereby traveling around the harvesting row. In addition, ${\mathrm{IRRT}}^{*}$ produced a kinetically compatible path with smoothness, being 97.63% smoother than RRT and 96.1% smoother than ${\mathrm{RRT}}^{*}$, as shown in Figure 9c.

In terms of controller effort, the performance metrics of the motion controller are presented in Table 4. With ${\mathrm{IRRT}}^{*}$, the cumulative path tracking errors along the trial barely increased with ${\mathrm{RRT}}^{*}$ regarding RRT and ${\mathrm{RRT}}^{*}$, which is related to the capability of avoiding obstacles. However, the controller effort of ${\mathrm{IRRT}}^{*}$ reduced by about 76.1% compared to RRT and 53.1% compared to ${\mathrm{RRT}}^{*}$, which was attributed to qualified paths with smoothness obtained in the previous path planning process.

6. Conclusions

This article 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 combined 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 prioritized harvesting locations according to criteria of minimum path length, unexplored harvest points, and vehicle payload capacity constraints. This approach aimed to generate an ordered sequence of harvesting locations that maximized the harvested product yield within the workspace. The local ${\mathrm{IRRT}}^{*}$ planner coordinated the scheduled harvesting points and simultaneously avoided obstacles characterized by low-traction terrain regions. In particular, ${\mathrm{IRRT}}^{*}$ generated smoother and safer routes, maintaining larger average distance to obstacles and reducing control effort compared to (RRT) and (${\mathrm{RRT}}^{*}$).

The path planning scheme incorporated the SSMR’s 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 mitigated the risk of robot entrapment and enabled efficient navigation over shortened and qualified smooth paths, potentially maintaining the quality of the harvested product. Experimental results demonstrated that the proposed ${\mathrm{IRRT}}^{*}$ outperformed the classical RRT and ${\mathrm{RRT}}^{*}$ by achieving 96.1% and 97.6% smoother paths under terrain constraints, respectively. The ${\mathrm{IRRT}}^{*}$ approach also exhibited improved navigation efficiency, avoiding obstacles and potential slippage zones. Future work may 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.