Multi-Strategy Fusion RRT-Based Algorithm for Optimizing Path Planning in Continuous Cherry Picking

Abstract

Automated cherry harvesting presents a significant opportunity to overcome the high costs and inefficiencies of manual labor in modern agriculture. However, robotic harvesting in dense canopies requires sophisticated path planning to navigate cluttered branches and selectively pick target fruits. This paper introduces a complete robotic harvesting solution centered on a novel path-planning algorithm: the Multi-Strategy Integrated RRT for Continuous Harvesting Path (MSI-RRTCHP) algorithm. Our system first employs a machine vision system to identify and locate mature cherries, distinguishing them from unripe fruits, leaves, and branches, which are treated as obstacles. Based on this visual data, the MSI-RRTCHP algorithm generates an optimal picking trajectory. Its core innovation is a synergistic strategy that enables intelligent navigation by combining probability-guided exploration, goal-oriented sampling, and adaptive step size adjustments based on the obstacle’s density. To optimize the picking sequence for multiple targets, we introduce an enhanced traversal algorithm ($\sigma$-TSP) that accounts for obstacle interference. Field experiments demonstrate that our integrated system achieved a 90% picking success rate. Compared with established algorithms, the MSI-RRTCHP algorithm reduced the path length by up to 25.47% and the planning time by up to 39.06%. This work provides a practical and efficient framework for robotic cherry harvesting, showcasing a significant step toward intelligent agricultural automation.

1. Introduction

As a globally significant cash crop, cherry cultivation plays a vital role within the agricultural economies of major producing nations, including China, Chile, and Japan [1]. However, the harvesting process faces substantial technical challenges due to dense fruit clustering and frequent occlusion by immature or damaged fruits [2]. Current harvesting operations remain predominantly reliant on manual labor. Industry statistics indicate that labor inputs account for 50–70% of the total production costs in conventional harvesting, rendering the process both inefficient and economically unsustainable [3]. Consequently, the development of automated harvesting solutions is of critical importance. Existing robotic harvesting platforms predominantly utilize vibration-based methods; however, these approaches cause significant fruit damage, lack selective harvesting capability, and yield stemless fruit incompatible with prevailing global consumer preferences for stem-attached cherries [4,5,6,7,8]. Therefore, optimizing manipulator path planning to achieve precise, efficient, and selective picking represents a pressing technological challenge for advancing robotic cherry harvesting systems.

Within the critical research domain of robotic manipulator path planning, numerous algorithms have gained widespread adoption due to their distinct advantages, including genetic algorithms, particle swarm optimization, A* Search, artificial potential fields, ant colony optimization, and the rapidly exploring random tree (RRT) algorithm [9,10,11,12,13,14,15]. Among these, the RRT algorithm, which was pioneered by LaValle [16], has garnered significant attention for its exceptional robustness and strong adaptability within complex environments, leading to its prevalent application in agricultural harvesting in recent years. In agricultural harvesting scenarios, manipulators require precise path planning within dynamic and intricate orchard environments to execute tasks efficiently and safely, a requirement well aligned with the capabilities of the RRT algorithm [17,18,19,20]. Nevertheless, the conventional RRT approach is not without limitations; it suffers from suboptimal node utilization, often resulting in paths of high complexity, coupled with relatively slow convergence rates. These inherent shortcomings have constrained its broader applicability in manipulator path planning [21]. To address these limitations, significant research efforts have been dedicated to exploring various enhancement strategies to improve the algorithm’s performance and applicability.

Wang et al. [22] proposed an enhanced path planning algorithm termed IBPF-RRT*, which accelerates path discovery and improves the resultant path quality through the introduction of a novel artificial potential field strategy incorporating obstacle boundary search mechanisms. Furthermore, the algorithm employs a bidirectional pruning strategy to optimize branch nodes within bidirectional search trees. This optimization is synergistically combined with a bidirectional search strategy, thereby significantly reducing iteration counts and expediting convergence rates. Cao et al. [23] developed an optimized RRT algorithm incorporating the concept of goal gravity bias to accelerate path exploration. Experimental validation demonstrated significantly enhanced planning efficiency for lychee-harvesting robotic manipulators using this improved algorithm. Ye et al. [24] integrated the goal gravity concept with adaptive coefficient adjustment into the Bi-RRT framework, developing the AtBi-RRT algorithm, which demonstrated enhanced efficacy in collision-free path planning for lychee-harvesting robotic manipulators. Chen et al. [25] enhanced exploratory efficiency in configuration spaces by introducing a tertiary node and expanding the tree structure to four configurations, thereby accelerating tree generation. Concurrently, Kang et al. [26] developed a bidirectional RRT-Connect (RRT-C) algorithm incorporating triangular inequality-based optimization, which reduced the path length by 16%, as demonstrated in empirical evaluations.

Wang et al. [27] introduced a four-parallel rapidly exploring random tree (4P-RRT) framework for path planning. This approach remains constrained by uniform global environmental sampling, which inherently limits its capacity to achieve substantial improvements in computational efficiency or algorithmic adaptability. Gammell et al. [28] introduced the Informed RRT algorithm, which is derived by optimizing the sampling process of the RRT algorithm. This approach employs an elliptical sampling strategy instead of global uniform sampling, thereby constraining the sampling range of random points to reduce the sampling space and ultimately attain the globally optimal solution.

Building upon the foundational RRT algorithm, significant research efforts have yielded various enhancements. Zhang et al. [29] introduced a regression mechanism to mitigate excessive global space exploration and implemented an adaptive expansion strategy leveraging free space information to avoid redundant node searches. Amiryan et al. [30] integrated RRT with artificial potential fields, thereby simplifying replanning tasks. Karaman et al. [31] developed the RRT* algorithm, which optimizes path quality by selecting optimal parent nodes within a defined radius around newly extended nodes during the tree expansion phase. Xu et al. [32] refined RRT* path simplification through probabilistic sampling combined with multi-step expansions based on directional similarity. Gammell et al. [28] proposed the Informed-RRT* algorithm, employing heuristic sampling focused on the goal region within an elliptical subset of the configuration space instead of global uniform sampling, significantly accelerating convergence toward asymptotic optimality. Collectively, these RRT variants primarily optimize algorithmic performance by modifying node generation and step expansion strategies, enhancing path generation efficiency while maintaining probabilistic completeness. However, these approaches exhibit two critical limitations [33]. Firstly, their reliance on random sampling across the entire workspace often results in a high proportion of samples falling within obstacle regions, which is particularly detrimental in complex environments. This necessitates extensive collision checking, which is especially burdensome for high-degree-of-freedom manipulators operating in 3D space, where each new node requires collision verification across all joints, consuming substantial computational resources and memory and ultimately diminishing path search efficiency. Secondly, conventional goal-biasing strategies, which directly replace random samples with the goal point at a predefined probability, typically disregard obstacle information. Consequently, in scenarios densely populated with obstacles between the current node and the goal, this biasing strategy often fails to provide effective directional guidance, compromising pathfinding efficacy.

It is important to contextualize these path planning algorithms within the broader “sense-plan-act” paradigm of a complete robotic harvesting system. These algorithms represent the “plan” stage, but they do not operate in a vacuum. Their successful application in real-world scenarios is critically dependent on an effective “sense” stage, which is typically accomplished using machine vision. In a practical application, cameras and sensors first capture the orchard environment. Then, image processing or deep learning models are employed to identify and determine the 3D coordinates of target fruits, branches, leaves, and other potential obstacles. This reconstructed digital map of the environment is then fed to the path planning algorithm. The algorithm, in turn, computes an optimal, collision-free trajectory for the robotic manipulator to “act” (i.e., to execute the picking task). Therefore, the performance of any path planner is intrinsically linked to the quality and accuracy of the perception data it receives, making machine vision an indispensable component of the overall system.

To address the aforementioned limitations, this study proposes a Sampling Point Optimized RRT (SPO-RRT) algorithm for robotic manipulator path planning. The SPO-RRT algorithm incorporates a sampling point filtering mechanism during the sampling phase, explicitly discarding samples falling within obstacle regions. Furthermore, it enhances the goal-biasing strategy by integrating concepts from artificial potential fields. Specifically, heuristic sampling is probabilistically guided toward the potential field gradient direction, thereby significantly improving the intelligence of goal-oriented exploration. Following path generation, a path pruning step eliminates kinematically redundant nodes. Finally, the performance of the SPO-RRT algorithm is rigorously evaluated through comparative analysis against conventional RRT and Biased-RRT algorithms, employing both simulation studies and physical prototype testing in realistic harvesting scenarios.

The contributions of this paper can be summarized as follows:

• The improved RRT algorithm we propose integrates a precondition-based constrained probability-guided sampling strategy, a goal-oriented dynamic sampling approach, and an obstacle density-adaptive step size adjustment algorithm. This integration significantly enhances both the efficiency and accuracy of the algorithm in the context of cherry-picking path planning;
• We have optimized the sequence planning for multi-target fruit picking by integrating the $\sigma$-TSP algorithm, which is designed for multi-objective fruit continuous picking sequence optimization. By incorporating an obstacle coefficient into the model, we have achieved a notable reduction in the length of the picking path, thereby improving the picking efficiency;
• Extensive experimental and simulation results demonstrate the innovative nature of our proposed algorithm and its potential for practical deployment.

2. Materials and Methods

For the cherry-picking task, where cherries are densely distributed and prone to being obscured by unripe or damaged fruits, a single path planning algorithm often fails to meet practical requirements during the picking operation. Consequently, integrating multiple algorithms becomes a necessary approach. The technical workflow of our path planning methodology is illustrated in Figure 1. Consequently, the specific workflow of our algorithm is as follows. The cherry tree information perception system invokes a camera in conjunction with a target detection algorithm to discern the type of target. If the target is identified as a tree, then the system executes obstacle avoidance procedures. In the case where the target is a fruit, the cherry fruit picking system is activated. If the detected entity is an obstacle-like fruit, then the system circumvents it. However, if the fruit is mature and suitable for picking, then it is incorporated into the planning objectives. Subsequently, the MSI-RRTCHP algorithm is employed to derive an optimal path planning solution.

2.1. Cherry Tree Information Perception System

During the cherry-picking process, the robotic arm needs to circumvent cylindrical obstacles such as the trunks and branches of cherry trees. To achieve a more precise fitting of the cherry tree, an improved multi-sphere fitting method is developed by enlarging the sphere radius to ${\displaystyle \frac{\sqrt{3}}{2}}r$ based on the original multi-sphere fitting approach. This adjustment incorporates a collision detection margin $\delta$, effectively virtually enlarging the obstacles. The detailed procedure is depicted in Figure 2. Here, h denotes the height of the cylinder, r represents both the radius of the cylinder and the radius of the fitting spheres, $\left[·\right]$ signifies the floor function for rounding down to the nearest integer, and m indicates the number of spheres that can fit the cylinder.

The precise acquisition of cherry fruit information and accurate target detection and localization serve as the foundation for harvesting path planning. We employ a Gemini binocular structured-light depth camera to capture fruit images and rely on a cherry tree information perception system to achieve fruit recognition. This recognition system encompasses complex scenarios, including trees (trunks and branches), mature fruits, and obstacles (such as damaged or immature fruits). Through hand–eye calibration techniques, we determine the precise positional coordinates of cherry fruits relative to the base of the robotic arm. Furthermore, we utilize the Density-Based Spatial Clustering of Applications with Noise (DBSCAN) clustering algorithm to effectively segment the original 3D dataset [34], forming multiple clusters of fruits with internal similarities. This approach significantly reduces the computational complexity involved in planning the cherry-picking sequence.

2.2. Single-Objective Fruit Picking Path Planning Based on an Improved RRT Algorithm

The rapidly exploring random tree (RRT) algorithm falls under the category of unidirectional path-searching methods. During each path-searching process, it initiates from the starting point and progressively explores until it reaches the target point, constructing a feasible path in the process. The pseudocode for the RRT algorithm is presented in

Table 1. RRT pseudocode.

All nodes in the expansion tree Tree are Tree = ($x_1$, $x_2$, …, $x_N$), starting node $x_{start}$

while (maximum iterations not reached and goal not found) {

$x_{rand}$ = randomly sample a point from the state space;

$x_{near}$ = the closest node to $x_{rand}$ in Tree;

$x_{new}$ = the new node obtained by expanding from $x_{near}$ toward $x_{rand}$;

if ($x_{new}$ does not collide with obstacles) {

Add $x_{new}$ to the tree T;

if ($x_{new}$ is close to the goal) {

Return the path from $x_{start}$ to $x_{new}$;

}}}

.

To achieve harvesting path planning in scenarios where cherries are densely distributed and obscured by obstacle fruits, we first enhance the RRT algorithm to plan a harvesting path between two mature and intact fruits. This is accomplished by introducing a precondition-constrained, probability-guided random sampling algorithm, a goal-oriented dynamic sampling algorithm, and an obstacle density-based adaptive step size adjustment algorithm. These modifications address the issues of high sampling randomness, uncertain expansion directions, and unstable step sizes inherent in the original RRT algorithm. Subsequently, we employ the $\sigma$-TSP algorithm to determine the sequential order for continuous cherry harvesting. Finally, redundant nodes along the path are pruned, and the path is smoothed using B-spline curves to generate a continuous harvesting path. Consequently, we obtain the Multi-Strategy Integrated RRT Continuous Harvesting Path Planning (MSIRRTCHP) algorithm, as illustrated in Figure 3.

In contrast to the traditional RRT algorithm’s global random sampling approach, we introduce the Probabilistic Guided Sampling Algorithm Based on Precondition Constraints (PG-RRT). This method mitigates the randomness in tree expansion directions by enhancing the purposefulness of sampling points. The procedure is as follows. A determination is made as to whether obstacles exist between the node $x_{\mathrm{near}}$, which is the closest node to the target point $x_{\mathrm{goal}}$ in the current expanding tree, and the target point $x_{\mathrm{goal}}$ itself. In the absence of obstacles, sampling is directly conducted toward the target point $x_{\mathrm{goal}}$. However, when obstacles are present, the PG-RRT is employed. A sampling threshold $\mathrm{\Delta }$ = 0.5 is set such that if the generated random sampling value falls below this threshold, then sampling is directly performed toward the target point $x_{\mathrm{goal}}$; otherwise, global random sampling is carried out. The formulation of the PG-RRT is as follows:

$$x_{\mathrm{rand}}=\left\{\begin{array}{c}{x}_{\mathrm{goal}}\hfill \\ x_{\mathrm{near}}\to x_{\mathrm{goal}}\in S_{\mathrm{free}},0\le \mathrm{rand}\le \mathrm{\Delta }\hfill \\ x_{\mathrm{rand}},\mathrm{other}\hfill \end{array}\right.$$

(1)

where rand denotes the random number generated during the sampling process and $\mathrm{\Delta }$ represents the sampling threshold, which was determined to be 0.5 in this experimental environment through trial and error. Meanwhile, $x_{\mathrm{rand}}$ signifies a randomly searched point within the space. The configuration space of the robot is denoted as S, with $S_{\mathrm{obs}}$ representing the obstacle region and $S_{\mathrm{free}}=S-S_{\mathrm{obs}}$ defining the free region.

The enhanced sampling process is illustrated in Figure 4, where the green unripe fruits are depicted as obstacles, the red hollow circles represent mature and harvestable cherry fruits, the orange elements signify the nodes of the expanding tree, and D denotes the step size.

During the initial sampling, the straight-line connection between the starting point $x_{\mathrm{start}}$ and the target point $x_{\mathrm{goal}}$ intersects with an obstacle fruit, indicating the presence of an obstruction between these two points. Consequently, the PG-RRT algorithm is employed to determine the sampling point $x_{\mathrm{rand}1}$. In the subsequent sampling iteration, the node $x_{\mathrm{new}1}$ within the expanding tree that is closest to the target point $x_{\mathrm{goal}}$ is selected. Since no obstacles exist between this node and the target point, sampling is directly conducted toward $x_{\mathrm{goal}}$. The position of the sampling point plays a pivotal role in the generation of new nodes. To enhance the goal-directedness of the sampling points, this paper introduces a goal-oriented sampling algorithm (GO-RRT). This algorithm ensures that the expanding tree always progresses in the direction of the target point, thereby addressing the issue of uncertain expansion directions. Additionally, it restricts the growth distance to accommodate the dense distribution of cherry fruits. In the original RRT algorithm, the generation of new nodes relies on a random sampling approach, and the formula for node generation is as follows:

$$x_{\mathrm{new}i}=x_{\mathrm{near}}+D\times {\displaystyle \frac{x_{\mathrm{rand}}-x_{\mathrm{near}}}{∥x_{\mathrm{rand}}-x_{\mathrm{near}}∥}}$$

(2)

The formula for generating new nodes in the GO-RRT algorithm is expressed as follows:

$$x_{\mathrm{new}i}=x_{\mathrm{new}i}+k\times D\times {\displaystyle \frac{x_{\mathrm{rand}}-x_{\mathrm{near}}}{∥x_{\mathrm{rand}}-x_{\mathrm{near}}∥}}$$

(3)

where the step size D governs the distance of node expansion, while k serves as a proportionality parameter that modulates the relative growth toward both the target point and the sampling point. In this experimental setting, k was determined to be 0.8 through trial and error.

During the process of generating sampling nodes, there typically exist three distinct growth scenarios, which are elaborately illustrated in Figure 5, where $\alpha$, $\beta$, and $\gamma$ denote these three growth conditions.

Among the three growth scenarios depicted in Figure 6, only scenario $\beta$ exhibited a growth direction oriented toward the target point. Upon transforming the growth scenarios illustrated in Figure 6 and incorporating the goal-oriented driving force, the resultant growth states are presented in Figure 6, where $x_{\mathrm{new}}$ represents the growth state under normal conditions, whereas $x_{\mathrm{new}}$ denotes the growth state after the addition of the goal-oriented driving force.

Upon examining Figure 6a–c, it can be observed that as the growth direction $x_{\mathrm{near}}\to x_{\mathrm{new}}$ gradually aligns with the target direction, the angle between $x_{\mathrm{near}}\to x_{\mathrm{goal}}$ diminishes, while the growth distance of $x_{\mathrm{near}}\to x_{\mathrm{new}}$ increases. Consequently, based on the boundary condition depicted in Figure 6a, the growth distance was set to the threshold $D\times \sqrt{1-k^2}$ to ensure that each expansion progressed toward the target point. If the growth distance falls below the threshold $D\times \sqrt{1-k^2}$, then this indicates that the expansion is not oriented toward the target point. In such cases, the current growth operation is terminated, and a new expansion cycle is initiated to seek a fresh sampling point $x_{\mathrm{rand}}$. To enhance planning efficiency, particularly in scenarios where cherry fruits exhibit high-density growth, the value of $D\times \sqrt{1-k^2}$ should be appropriately reduced to permit the expanding tree to grow in reverse directions when necessary. A detailed depiction of the node growth process using the GO-RRT algorithm is provided in Figure 7.

In addressing the cherry-picking path planning problem, where cherries are frequently obscured by obstacle fruits, the capability to adaptively adjust the step size according to the density of obstacles can effectively shorten the step length and enhance efficiency. To this end, we propose the adaptive step size adjustment algorithm based on obstacle density (Obs-RRT). The step size is determined by selecting the shortest connecting line between an obstacle fruit in the direction of the target point and the node in the current expanding tree that is closest to the target point. Meanwhile, certain bounds are set for the step size value. The specific operations are as follows:

• When the expanding tree grows closer to an obstacle, the step size is automatically reduced, enabling the model to move closely alongside the obstacle without collision;
• Upon successfully navigating around the obstacle, the step size is promptly increased to expedite progress toward the target location;
• Reasonable upper and lower bounds are established for the step size to accommodate cherry-picking scenarios. This is because, when confronted with obstacles of complex shapes such as concavities or overlaps, an excessively small step size may lead to stagnation, whereas an overly large step size during the process of moving away from obstacles and gradually approaching the target may result in unnecessary oscillations near the target area.

Let $x_{\mathrm{near}}$ denote the node in the expanding tree (Tree) that is closest to the target point $x_{\mathrm{goal}}$. Meanwhile, define an obstacle set Obs comprising m obstacle nodes $Obs=\left(ob{s}_1,ob{s}_2,\dots ,ob{s}_m\right)$ within the space. Due to the incorporation of a goal-oriented driving force during the growth process, it is stipulated that $\theta <{\displaystyle \frac{1}{1+k}}$, and through trial and error, this parameter was determined to be 0.5 in the experimental environment of this study. The formula for updating the step size after each successful expansion is as follows:

$$\begin{array}{cc}\hfill & D=min\left(d\left(x_{\mathrm{near}},ob{s}_i\right)\right)\times \theta i=1,2,\dots ,m\hfill \end{array}$$

(4)

In a space containing multiple obstacle fruits, the current expanding tree consists of six nodes, as illustrated in Figure 8. Within this tree, the node $x_{\mathrm{near}}$ closest to the target point $x_{\mathrm{goal}}$ is identified, and the distances from this node to the obstacle fruits in the direction of the target point are calculated as $d_1$, $d_2$, and $d_3$. Based on this information, the updated value of the step size D after this expansion is determined by $D=\theta \times min\left(d_1,d_2,d_3\right)=\theta \times d_2$.

2.3. Multi-Objective Sequential Fruit Picking Order Planning Based on the $\sigma$-TSP Algorithm

For multi-target fruit picking tasks, picking robots commonly adopt a picking sequence that progresses from the periphery toward the center and from near to far distances. However, this approach fails to fully consider the optimization of the overall picking path [35]. We aim to improve the length of the multi-target picking path to achieve an optimal layout and enhance the continuous operational efficiency of cherry picking. By drawing inspiration from the core principles of the genetic algorithm in solving the Traveling Salesman Problem (TSP) [36,37,38] and taking into account the influence of obstacle fruits $\sigma$, we have implemented a refined and comprehensive planning of the multi-target picking sequence. The TSP requires determining the shortest path that covers all given city coordinates or the distances between them, ultimately returning to the starting city [39]. The mathematical model formulation for the TSP is expressed as follows:

$$min\sum _{i=1}^{n-1}d\left({\mathrm{city}}_i,{\mathrm{city}}_{i+1}\right)+d\left({\mathrm{city}}_n,{\mathrm{city}}_1\right)$$

(5)

During the execution of cherry-picking tasks, the robotic arm always commences from its initial position, denoted as home. After completing the picking tasks for multiple cherry targets, where Goals $=$ {goals₁, goals₂$,\dots$, goals_n}, it returns to the initial position home to conclude the operation. This sequence can be represented as a path order, where Visit $=$ {home, goals₁, goals₂$,\dots$, goals_n, home} for a series of cherry-picking tasks. Consequently, for the multi-target cherry-picking task, the TSP model can be formulated as follows:

$$min\left(d\left({\mathrm{home}}^{\mathrm{goals}}1\right)+\sum _{i=1}^{n-1}d\left({\mathrm{goals}}_i,{\mathrm{goals}}_{i+1}\right)+d\left({\mathrm{goals}}_n,\mathrm{home}\right)\right)$$

(6)

In addressing multi-target path optimization problems, traditional methods often entail a substantial computational burden, leading to excessively lengthy planning processes. To enhance efficiency, this study employs a genetic algorithm (GA) to tackle the TSP. By analogizing cherry-picking points to city nodes in the TSP, this research aims to optimize the traversal path of the robotic arm’s end effector across all target cherries. When dealing with multiple cherry collection targets, the straight-line distance metric is conventionally employed to evaluate the distances between targets. However, during the cherry-picking process, the presence of obstacle fruits often prevents the robotic arm’s end effector from directly reaching the targets in practical operations, resulting in the actual path length exceeding the distance planned based on the straight-line distance metric. Consequently, the traversal sequence derived by the algorithm may not be the most efficient one. To address this issue, we propose an innovative approach that accounts for the influence of obstacle fruits when calculating the straight-line distance metric between targets. This is achieved by introducing an adjustment factor, termed the “straight-line distance metric considering obstacle influence”. The calculation formula is presented as follows:

$$d_{\sigma }(i,j)=\left\{\begin{array}{c}d(i,j),i\to j\in S_{\mathrm{free}}\hfill \\ d(i,j),i\to j\in S_{\mathrm{obs}}\hfill \end{array}\right.$$

(7)

where the variable $\sigma$ represents the influence coefficient of obstacles, where $\sigma$ > 1. The straight-line path refers to the direct route from the starting point $i\to j$. The region $S_{\mathrm{free}}$ is defined as the obstacle-free space, whereas the region $S_{obs}$ is identified as the area containing obstacles. By integrating the $\sigma$-TSP algorithm with an improved RRT algorithm, we achieve the planning of a sequential cherry-picking path. Suppose that there are n cherries to be picked, and the starting point of the robotic arm’s end effector is set as home. After determining the picking sequence using the $\sigma$-TSP algorithm, a complete path for sequentially picking multiple targets can be obtained. The formula for calculating this path is as follows:

$$RRT\left(\mathrm{home},{\mathrm{goals}}_1\right)+\sum _{i=1}^{n-1}RRT\left({\mathrm{goals}}_i,{\mathrm{goals}}_{i+1}\right)+RRT\left({\mathrm{goals}}_n,\mathrm{home})\right.$$

(8)

where RRT(a,b) denotes the application of the optimized RRT algorithm to design a path from position a to position b. In the constructed model, we selected and labeled 16 terminal branch tips, which symbolize the picking centers of the cherries. For these 16 target points, both TSP path planning and $\sigma$-TSP path planning were implemented to determine the visiting sequence. When obstacles existed between target points, an obstacle influence coefficient was introduced. The results of these path planning efforts are illustrated in Figure 9. The formula for calculating the traversal sequence using the TSP algorithm is as follows:

$$\mathrm{home}-7-8-9-12-4-5-2-11-1-6-3-14-16-13-15-10-\mathrm{home}$$

(9)

The traversal sequence determined by the $\sigma$-TSP algorithm is as follows:

$$\mathrm{home}-13-15-16-14-1-6-11-2-5-4-3-10-12-9-8-7-\mathrm{home}$$

(10)

The optimized path planning yielded a distinct traversal sequence compared with the original planning.

2.4. Path Optimization

The path generated by the RRT algorithm consists of a series of discrete nodes connected sequentially. However, these paths often contain numerous unnecessary nodes, resulting in insufficient path smoothness and compromising the stable operation of the robotic arm [40,41,42]. Let the set of path nodes generated by the RRT algorithm be denoted as Tree $=\left(x_1,x_2,\dots ,x_N\right)$. From this set, three nodes are selected: node i, node i+1, and node i+2, where $i\in (1,2,\dots ,N-2)$. Obstacle detection is performed on nodes $x_i$, $x_{i+1}$, and $x_{i+2}$. If no obstacles obstruct the path between these nodes, then node $x_{i+1}$ and its connecting line segments are removed. The simplified path is illustrated by the arrow lines in Figure 10.

We employed a fourth-order cubic B-spline curve to smooth the path at the inflection points. A B-spline curve of the order k is expressed as follows:

$$P\left(u\right)=\sum _{i=0}^n{P}_i{B}_{i,k}\left(u\right)$$

(11)

where $P_i$ represents the control points on the given curve and $B_{i,k}\left(u\right)$ denotes the B-spline basis function of the order k. The B-spline basis function of the order k can be defined through the following recursive relationship:

$$\begin{array}{cc}\hfill & B_{i,1}\left(u\right)=\left\{\begin{array}{c}1,u_i\le u\le u_{i+1}\\ 0,\mathrm{other}\end{array}\right.\hfill \\ \hfill & B_{i,k}\left(u\right)={\displaystyle \frac{u-u_i}{u_{i+k-1}-u_i}}{B}_{i,k-1}\left(u\right)+{\displaystyle \frac{u_{i+k}-u}{u_{i+k}-u_{i+1}}}{B}_{i+1,k-1}\left(u\right)\hfill \end{array}$$

(12)

where $u_i$ denotes the knot sequence. The expression for the fourth-order cubic B-spline curve is derived using Equation (12), and the calculation is

$$\begin{array}{cc}\hfill & Q=\left[\begin{array}{cccc}1\hfill & u\hfill & u^2\hfill & u^3\hfill \end{array}\right]\left[\begin{array}{cccc}1& 4& 1& 0\\ -3& 0& 3& 0\\ 3& -6& 3& 0\\ -1& 3& -3& 1\end{array}\right]\hfill \\ \hfill & P_i\left(u\right)={\displaystyle \frac{1}{6}}Q\left[\begin{array}{c}{P}_{i-1}\\ P_i\\ P_{i+1}\\ P_{i+2}\end{array}\right]\hfill \end{array}$$

(13)

Upon identifying the critical nodes along the path, the application of Equation (13) enables the derivation of a set of spatial points that satisfy the B-spline interpolation conditions. These points delineate an interpolated path, thereby furnishing a smooth trajectory for the continuous motion of the robotic arm.

3. Experiments and Analysis

3.1. Comparison of Different Planning Algorithms

Within the MATLAB (2023b) environment, a single-objective picking simulation experiment was conducted using 10 spheres to simulate obstacle-laden fruits. The comparative results among the RRT algorithm, PG-RRT, GO-RRT, and Obs-RRT are presented in

Table 2. Comparative table of four planning algorithms in a simulated environment.

Method	Sampling Nodes (Units)	Path Length (mm)	Planning Time (s)
RRT	1141	412	31.84
PG-RRT	58	350	0.66
Go-RRT	45	393	2.42
Obs-RRT	422	108	5.80

.

The simulation experimental results demonstrate that, in terms of planning time, the PG-RRT exhibited a significant advantage over the RRT algorithm, achieving a reduction of approximately 97.93%. Regarding the number of sampled nodes, the GO-RRT reduced it by about 96.06% compared with the RRT algorithm. In terms of path length, the Obs-RRT achieved a reduction of approximately 73.78% relative to the RRT algorithm. To a certain extent, these three algorithms address the issues of high sampling randomness, uncertain expansion directions, and unstable step sizes encountered by the RRT algorithm in the cherry single-objective picking simulation with obstacle avoidance. The simulation results of the RRT algorithm and the three improved algorithms are illustrated in Figure 11. As can be visually observed from the figures, the path generated by the RRT algorithm was disordered, with widely distributed sampling nodes, resulting in a long and tortuous path. In contrast, the three improved algorithms, namely PG-RRT, GO-RRT, and Obs-RRT, generated more concise and efficient paths with more reasonably distributed sampling nodes. To a certain extent, these algorithms address the issues of high sampling randomness, uncertain expansion directions, and unstable step sizes inherent in the RRT algorithm. They are better suited for cherry single-target picking scenarios involving obstacle avoidance, thereby providing a superior path planning solution for robotic cherry picking tasks.

To further investigate the robustness of our model, as well as its anti-interference capability and performance in practical environments, we conducted perturbation experiments. As illustrated in Figure 12, multiple perturbation factors were introduced, including the joint angle error, positioning error, repeatability error, vibration amplitude, and vibration frequency. We posit that these perturbations optimally simulated operational conditions in real-world scenarios, particularly in harsh environments, thereby substantially increasing the load on our model. Analysis of the figure reveals that even under significant perturbations, our model maintained robust operational efficiency. This resilience is primarily attributed to the inherent robustness and anti-interference capacity of our algorithm, ensuring effective performance in real-world applications.

3.2. Single-Objective Path Planning Simulation Experiment

The enhanced RRT algorithm, which integrates PG-RRT, GO-RRT, and Obs-RRT, is a tailored path planning algorithm specifically designed for cherry picking. Given that RRT-connect and RRT* algorithms are frequently employed in agricultural picking path planning, a comparative analysis between the enhanced RRT algorithm and these two algorithms was conducted in this study.

To evaluate the performance of the enhanced RRT algorithm, a single-target picking simulation experiment was carried out in the MATLAB environment, utilizing 10 spheres to simulate obstacle-laden fruits. The starting coordinates of the experiment were set at the origin (0,0,0), while the target coordinates were established at (100,100,100).

Table 3. Comparison of harvesting path lengths among three algorithms in a simulation environment.

Planning Algorithm	Trajectory Length of Multi-Target Picking
RRT*	496
RRT-connect	421
Enhanced RRT	297

presents the average data derived from 100 simulation experiments.

In this study, a comparative performance analysis was conducted between the enhanced RRT algorithm and the aforementioned two algorithms. The data from the simulation experiments indicate that the enhanced RRT algorithm achieved reductions of 40.12% and 29.45% in path length compared with the RRT* algorithm and RRT-connect algorithm, respectively. This outcome robustly demonstrates that, in the context of picking a single cherry, the enhanced RRT algorithm is capable of planning a shorter picking trajectory than both the RRT-connect and RRT* algorithms. The simulation results are illustrated in Figure 13. As clearly demonstrated in the three subfigures of Figure 13, the trajectories generated by the RRT and RRT-connect algorithms appeared more convoluted and dispersed, with the green trajectory lines meandering extensively through the space. In contrast, the trajectory produced by the MSI-RRTCHP was more concise and direct, as evidenced by the concentrated blue trajectory line with a shorter path length. This visually validates that the enhanced RRT algorithm outperformed the RRT-connect and RRT* algorithms in planning shorter picking trajectories, aligning with the experimental data conclusions presented earlier.

3.3. Multi-Objective Path Planning Experiment

In this experimental study, a comparative multi-objective traversal analysis was conducted by integrating RRT*, RRT-connect, and $\sigma$-TSP algorithms, alongside the MSI-RRTCHP. For the purpose of these experiments, “mature fruits” were defined as cherries meeting the following criteria: (1) a color within the deep red spectrum (RGB values with R > 150, G < 50, and B < 50), (2) a diameter greater than 20 mm, and (3) no visible surface defects. “Damaged fruits” were defined as any fruit exhibiting signs of splitting, bruising, mold, or significant immaturity (i.e., green or yellow in color). These were treated as static obstacles in the path planning. To ensure the generalizability of our findings and avoid confounding effects from repeated handling, the 120 field experiments were conducted using 12 different batches of cherries, with 10 repetitions per batch. For each of the 120 trials, the 25 mature fruits and 4 damaged fruits were randomly repositioned on the artificial tree branch. This protocol ensured that each trial was independent and that the quality of the fruits was not affected by previous harvesting attempts, thereby providing a robust assessment of the algorithm’s performance in varied scenarios. Given the relatively slow convergence rates of the RRT* and RRT-connect algorithms, a unified threshold of $\mathrm{\Delta }$ = 0.3 was set. The primary obstacles in the cherry tree were modeled using an improved sphere-fitting method. The resulting movement trajectory of the robotic arm’s end effector, as obtained from the simulation, is depicted in Figure 14. The experiment was replicated 120 times under the aforementioned settings, and

Table 4. Comparative analysis of harvesting performance among three algorithms in a simulated environment.

Planning Algorithm	Multi-Target Picking	Planning Time
$\sigma$-TSP-RRT*	2638	11.81
$\sigma$-TSP-RRT-connect	2542	5.41
MSI-RRTCHP	1966	3.43

presents the average data of the trajectories obtained from the multi-objective path planning.

The experimental findings reveal that, in the context of continuous multi-objective picking planning, the MSI-RRTCHP proposed in this paper demonstrated significant advantages. Specifically, in terms of picking path length, the MSI-RRTCHP achieved reductions of approximately 22.66% and 25.47% compared with the $\sigma$-TSP-RRT* and $\sigma$-TSP-RRT-connect algorithms, respectively. Moreover, with regard to planning time, the MSI-RRTCHP outperformed the $\sigma$-TSP-RRT* and $\sigma$-TSP-RRT-connect algorithms by approximately 54.19% and 36.60%, respectively.

In summary, the MSI-RRTCHP proposed in this study not only substantially enhanced planning efficiency but also significantly shortened the path length during the picking process, thereby exhibiting excellent path planning performance.

To further underscore the advantages of our proposed method, we conducted field experiments. The cherry-picking robot, as illustrated in Figure 15, primarily comprises a six-degree-of-freedom picking mechanism, a mobile chassis, a picking manipulator, and a machine vision system. The mobile picking robot navigates between rows of cherry trees, enabling efficient cherry fruit picking. By employing the MSI-RRTCHP, we selected an artificial tree branch on which 25 mature fruits and 4 damaged fruits were randomly positioned at each instance. The picking experiment was replicated 120 times.

Table 5. Harvesting experiment results.

Planning Algorithm	Mature Fruits	Damaged Fruits
MSI-RRTCHP	23	4
	24	4
	22	4
	21	4
	24	4
	...	...
	25	4
	23	4

presents the results of successfully picking mature fruits and avoiding damaged fruits using the proposed algorithm.

During the experimental process, no damage was observed on the tree trunk or branches, and an average of approximately 22.5 mature fruits were successfully picked. The experimental results indicate that, in the continuous picking planning for multi-task objectives, the algorithm achieved a success rate of approximately 90% in path planning. This demonstrates its exceptional path planning capability during cherry picking operations.

To evaluate the consistency of our model’s performance, the 120 experiments were divided into 12 batches. As shown in Figure 16, a box plot analysis of the coefficient of determination (R²) was conducted. The results show that the R² values across all batches were consistently high, with a mean of 0.8150. This indicates a strong and stable goodness of fit of our path planning model under different experimental conditions. Furthermore, a one-way analysis of variance (ANOVA) was performed to test for significant differences between the experimental batches. The resulting F value was 1.50, which is well below the critical threshold of 2.5 (for p < 0.05), as shown in Figure 16. This F value indicates that there were no statistically significant differences among the 12 experimental batches, confirming the high repeatability and robustness of our proposed MSI-RRTCHP.

3.4. Discussion

Our proposed MSI-RRTCHP demonstrated a significant advancement in path planning for agricultural robotics. In contrast to previous studies, which often enhanced a single aspect of the RRT algorithm, our primary contribution lies in the holistic, multi-strategy fusion of probability-guided sampling, goal-oriented dynamic sampling, and the obstacle density-based adaptive step size. This synergistic approach is specifically tailored to the densely cluttered canopies of cherry trees. Furthermore, the introduction of the $\sigma$-TSP algorithm marks a key innovation for multi-target traversal. Unlike standard TSP solvers, which optimize for idealized geometric paths, our method incorporates an obstacle influence coefficient ($\sigma$), effectively bridging the gap between an abstract optimal tour and a practical, collision-free trajectory. This combined optimization framework directly addresses the unique challenges of agricultural harvesting, leading to superior performance in terms of both path length and planning time compared with benchmark algorithms.

Despite the promising results, we acknowledge several limitations that pave the way for future work. The current study relied on a static environmental model. Future research should focus on integrating the algorithm with real-time vision systems to handle dynamic conditions such as fruit movement. Additionally, the obstacle coefficient $\sigma$ was determined empirically. Developing a method for its dynamic calculation based on real-time environmental data could further enhance the algorithm’s adaptability. Finally, validating the algorithm on a wider variety of fruit tree architectures and in diverse field conditions will be crucial for confirming its generalizability and advancing its potential for practical deployment in automated agriculture.

4. Conclusions

In response to the challenges in picking path planning for cherries, which are densely distributed and often obscured by immature or damaged fruits, this study proposed the MSI-RRTCHP through the comprehensive optimization and innovative integration of the multi-objective traversal planning TSP algorithm and the path planning RRT algorithm. By introducing probability-guided sampling based on precondition constraints, goal-oriented kinetic sampling, and an adaptive step size adjustment algorithm based on the obstacle density, the MSI-RRTCHP effectively addresses the issues of strong sampling randomness and unstable expansion directions and step sizes inherent in the RRT algorithm. When calculating the traversal sequence, an obstacle coefficient $\sigma$ is creatively incorporated to enhance the precision of the sequence, thereby significantly reducing the length of the picking path. Additionally, trajectory smoothing is achieved through the application of B-spline curves, which enhances the algorithm’s convergence speed, planning efficiency, and trajectory smoothness. Experimental data revealed that the MSI-RRTCHP achieved a 90% success rate in continuous picking path planning. Compared with the $\sigma$-TSP-RRT* and $\sigma$-TSP-RRT-connect algorithms, it reduced the picking path length by approximately 22.66% and 25.47%, respectively, and decreased the planning time by about 54.19% and 36.60%, respectively. This algorithm not only substantially shortens the picking path length but also significantly reduces the planning time, thereby improving overall planning efficiency. It demonstrates promising application prospects and development potential in cherry picking operations.