Canopy-Adaptive TAD-IRRT* Algorithm for 3D Path Planning of 6-DOF Apple-Harvesting Robots in Dense Orchards

Abstract

This study proposes a canopy-adaptive TAD-IRRT* (target-biased sampling, artificial potential field, and dynamic step-size informed rapidly-exploring random tree star) algorithm to solve the collision-free 3D path-planning problem for a 6-DOF apple-harvesting robotic arm. To improve computational speed and search directionality, the method integrates target-biased sampling and a distance-regulated artificial potential field (APF) into the Informed-RRT* framework. Furthermore, an obstacle-distance-based dynamic step-size mechanism is introduced to optimize spatial exploration. The generated routes undergo greedy path pruning and cubic B-spline smoothing to ensure kinematic executability. The simulation results in complicated ROS-based scenarios demonstrate that the TAD-IRRT* algorithm achieves a 100% planning success rate, reducing the average computational time and joint-space path length by approximately 60.1% and 15.6%, respectively, compared to the standard Informed-RRT*. Kinematic analysis via Fourier curve fitting ($R^2=0.9849$) confirms continuous angular velocity and acceleration without high-frequency chattering. Physical prototype experiments in the dense-obstacle scenarios show that the proposed method increases the path execution success rate by 36.7% and reduces the average execution time by 41% compared to the standard Informed-RRT* algorithm. The proposed approach effectively balances high-quality path generation with low computational overhead, providing a reliable and safe solution that significantly reduces mechanical wear.

1. Introduction

As one of the world’s major fruits, apples are widely cultivated globally [1]. With the continuous advancement of technology, harvesting robots have gradually become the optimal choice for mechanized apple harvesting in the agricultural sector [2,3,4]. Among these, the six-degree-of-freedom (6-DOF) robotic arm serves as the core mechanism for executing harvesting, obstacle avoidance, and posture adjustment. Its motion efficiency and operational safety highly depend on high-quality three-dimensional (3D) spatial path-planning algorithms. However, due to the dense branches and leaves, the irregular distribution of obstacles, and the complicated distribution of target points within orchards, the path planning of the robotic arm faces significant challenges such as high-dimensional nonlinearity, strong constraints, and real-time processing requirements [2]. Therefore, generating collision-free, safe, and smooth executable trajectories for robotic arms in complicated scenarios has become a crucial scientific problem in the research of intelligent harvesting robots.

At present, trajectory-planning strategies for robotic arms primarily fall into three categories: traditional approaches [5], graph-based search techniques [6], and sampling-based methods [7]. Among them, the artificial potential field (APF) method, proposed by Khatib, is representative of traditional methods [8]. This method is computationally efficient, but it is highly susceptible to falling into local minima or experiencing target unreachability when obstacles exist near the target point [9]. To address such issues, numerous scholars have conducted corresponding improvement studies. For instance, Zhuang et al. [10] proposed an obstacle avoidance path-planning method integrating APF and the A* algorithm, which improved obstacle avoidance efficiency and path smoothness by incorporating the IRRT (Improved Rapidly Exploring Random Tree) algorithm for path replanning and optimizing the potential field function. Wang et al. [11] proposed the DAPF-RRT path-planning method, combining dynamic step sizes with APF, which effectively resolved the randomness problem of node expansion and improved planning accuracy by using attractive and repulsive functions to constrain the node growth direction.

To overcome the inherent tendency of conventional techniques to become trapped in local optima, researchers have increasingly adopted globally optimal graph-based search strategies, including the A*, Dijkstra, and D* algorithms. These frameworks are highly valued for their adaptable and predictable trajectory generation [12]. For instance, Gu et al. [13] engineered an upgraded Dijkstra approach for mobile robot navigation, leveraging ultrasonic sensing to enable real-time path updates. Similarly, Ji et al. [14] implemented a modified Dijkstra strategy to establish preliminary routes for apple-picking robots, which were subsequently refined through an advanced ant colony optimization. Furthermore, a customized hybrid D* Lite framework introduced by Jiang et al. [15] for orchard harvesters notably augmented both the stability of global routes and the success rates of collision avoidance. Nevertheless, as the dimensionality of the map expands or the orchard environment becomes increasingly cluttered, the computational overhead of these graph-search techniques grows exponentially. Consequently, they struggle to satisfy the strict real-time execution constraints required for the online control of robotic manipulators.

In the motion planning of multi-DOF robotic arms in high-dimensional spaces, sampling-based methods are widely adopted [16,17]. Among them, sampling-based path-planning methods represented by Rapidly-exploring Random Trees (RRTs) [18,19] have become a research hotspot due to their simple structural formulation and ability to rapidly generate feasible paths. These methods can obtain feasible solutions relatively quickly in low-dimensional spaces. However, when confronting the high-dimensional complicated configuration space of a robotic arm, the search space grows exponentially, leading to a sharp decline in planning efficiency [20]. Pure random sampling not only struggles to balance search efficiency and path quality, but its high randomness also increases search blindness, often resulting in tortuous paths that deviate from the optimal solution [21,22,23]. Consequently, how to balance search efficiency and optimality while maintaining the probabilistic completeness of RRT has emerged as the focal point of current research.

In recent years, scholars have proposed various improvement strategies addressing the sampling efficiency and path optimization problems of the RRT algorithm, primarily focusing on two aspects: heuristic spatial restriction and goal-oriented expansion. In terms of spatial restriction, Gammell et al. [24] proposed the Informed-RRT* algorithm, which significantly accelerates convergence speed by constructing an admissible ellipsoidal sampling region after finding an initial path. The EP-RRT* proposed by Ding et al. [25] further improves search efficiency by utilizing extended region heuristic sampling. Regarding goal-oriented expansion, numerous studies have introduced multi-tree growth and adaptive mechanisms. Li et al. [26] and Chen et al. [27], respectively, proposed improved RRT-Connect algorithms based on the simultaneous expansion of multiple random trees, significantly reducing the number of iterations by incorporating a goal-biasing strategy. The BPFPS-RRT algorithm proposed by Liu et al. [28] introduced a high-probability goal bias coupled with a dynamic step-size exploration strategy, demonstrating excellent performance in search time.

Nevertheless, although the aforementioned optimization algorithms have achieved significant success in conventional industrial or structured scenarios, substantial limitations remain when they are directly applied to orchard harvesting environments. The unstructured environment of an orchard is characterized by a highly uneven distribution of obstacles (such as branches, leaves, and fruits) and severely restricted local spaces. Existing ellipsoidal sampling domains or global goal-biasing strategies are highly prone to generating numerous invalid collision detections when encountering dense overlapping branches due to the lack of fine guidance for local obstacle avoidance [29]. Concurrently, fixed or simplistic step-size adjustment mechanisms struggle to switch flexibly between open rows and crowded canopies, causing the algorithm to easily stall during expansion within narrow passages [30].

To overcome these limitations, we introduce the Canopy-Adaptive TAD-IRRT* (Target-biased, APF-guided, Dynamic step-size Informed-RRT*) algorithm, a multi-faceted 3D trajectory-planning framework tailored for 6-DOF apple-harvesting manipulators. Building upon the foundational Informed-RRT* architecture, our approach fuses a target-oriented sampling heuristic with a refined artificial potential field (APF) to effectively guide the growth of the random tree while bypassing local minima. Furthermore, the integration of an obstacle-aware dynamic step-size controller optimizes the trade-off between rapid exploration in open spaces and precise maneuvering in cluttered zones. To yield executable motions, the raw routes undergo greedy pruning followed by cubic B-spline smoothing, ultimately delivering seamless trajectories fully compliant with the robot’s physical constraints.

The remainder of this paper is organized as follows: Section 2 details the materials and methods, including the kinematic modeling, collision detection strategy, and the optimization components of the Canopy-Adaptive TAD-IRRT* algorithm. Section 3 presents the 2D and 3D simulation results, simulation obstacle avoidance path-planning experiment, joint space continuity analysis and physical obstacle avoidance verification experiments. Section 4 discusses the algorithm’s performance advantages and outlines directions for future research. Finally, Section 5 concludes this study.

2. Materials and Methods

2.1. Kinematic Model of the Robotic Arm

Figure 1 depicts the physical prototype of the apple-harvesting robot utilized in this study. The integrated system consists of a mobile chassis, a robotic arm, a force-controlled gripper, a vision sensor (ZED2i stereo camera (Stereolabs, San Diego, CA, USA)), a communication router, a human–machine interface, and a control cabinet. Serving as the primary execution mechanism for harvesting tasks, the manipulator features six rotational degrees of freedom (6-DOF).

To mathematically describe the spatial pose of the end-effector relative to the base frame and to establish a theoretical foundation for subsequent 3D obstacle-avoidance path planning, the standard Denavit–Hartenberg (D-H) convention was employed to formulate the kinematics model. The nominal D-H parameters of the robotic arm are summarized in

Table 1. D-H parameters of the six-DOF apple-harvesting robotic arm.

Joint No (i)	${\mathit{a}}_{\mathit{i}}$ (m)	${\mathit{\alpha }}_{\mathit{i}}$ (Rad)	${\mathit{d}}_{\mathit{i}}$ (m)	${\mathit{\theta }}_{\mathit{i}}$ (Rad)
1	0	$\pi /2$	0.08916	${\theta }_1$
2	−0.42500	0	0	${\theta }_2$
3	−0.39225	0	0	${\theta }_3$
4	0	$\pi /2$	0.10915	${\theta }_4$
5	0	$-\pi /2$	0.09465	${\theta }_5$
6	0	0	0.08230	${\theta }_6$

.

As detailed in Table 1, i denotes the link index; $a_i$ represents the link length, defined as the common normal distance between two adjacent joint axes; ${\alpha }_i$ indicates the link twist angle; $d_i$ represents the joint offset along the joint axis; and ${\theta }_i$ is the joint angle variable.

2.2. Collision Detection of Robotic Arm

Collision detection serves as a fundamental safety constraint in the path-planning process of apple-harvesting robotic arms, ensuring that the manipulator avoids spatial interference with orchard elements (e.g., trunks, branches, and fruits) and prevents self-collision during movement. This process provides robust safety boundaries for the planning algorithm, guaranteeing that the generated motion trajectories are strictly collision-free [31]. To achieve an optimal balance between real-time computational efficiency and the precision required for unstructured orchard environments, this study adopts a geometric bounding volume strategy to model both the robotic structure and environmental obstacles [32]. To accurately represent the irregular and curved nature of actual tree branches in dense orchards, a piecewise cylindrical approximation strategy is employed in this model. Specifically, a continuous curved branch is discretized into a series of short, interconnected straight cylindrical segments. Furthermore, the radius of these bounding cylinders is deliberately inflated with a predefined safety margin. This margin ensures that any local curvatures, minor bends, and irregular protrusions of natural branches are strictly enveloped within the cylindrical volumes. Consequently, this approach transforms the complicated curved obstacle detection into mathematically efficient multi-cylinder distance calculations, thereby ensuring the absolute collision-free execution of the manipulator.As illustrated in Figure 2, discrete objects such as target apples are abstracted as sphere models, while the manipulator links and elongated branches are simplified into capsules (sphero-cylinders). This modeling approach maintains essential spatial constraints while significantly streamlining the mathematical evaluation of distance-based collision criteria.

Based on these geometric abstractions, the detection task is transformed into evaluating the proximity between capsules and spheres or between pairs of capsules. Following the methodology introduced in literature [33], physical interferences are evaluated by transforming physical boundaries into a set of distance-based nonlinear inequalities. For a manipulator link modeled as a capsule with axis $A_1{A}_2$ and radius $r_A$, and a spherical obstacle with center P and radius $r_S$, the relative spatial relationship is defined by the projection parameter t of the sphere center onto the link axis:

$$t={\displaystyle \frac{(P-A_1)·(A_2-A_1)}{\parallel A_2-A_1{\parallel }^2}}$$

(1)

The absolute shortest spatial distance, $d_{min}$, between the obstacle center P and the link axis segment $A_1{A}_2$ is subsequently determined by the following piecewise relationship:

$$d_{min}=\left\{\begin{array}{cc}\parallel P-A_1\parallel ,\hfill & t<0\hfill \\ {\displaystyle \frac{\parallel (P-A_1)\times (A_2-A_1)\parallel }{\parallel A_2-A_1\parallel }},\hfill & t\in [0,1]\hfill \\ \parallel P-A_2\parallel ,\hfill & t>1\hfill \end{array}\right.$$

(2)

Figure 3 explicitly illustrates the shortest spatial distance $d_{min}$ and the projection parameter t.

A collision state is registered if the calculated minimum distance falls below the combined safety threshold of their respective radii:

$$d_{min}\le r_A+r_S$$

(3)

Furthermore, the assessment of potential collisions between the manipulator and capsule-shaped branches depends on the relative spatial orientation of their corresponding axes. This is categorized into two geometric scenarios:

(1) When the axes are parallel: The coordinate system is rotated to project both axes orthogonally onto a common perpendicular plane. The interference threshold is then determined by the planar projection distance ($u_1$) and the axial spacing offset ($u_2$):

$$D_{min}=\sqrt{u_1^2+u_2^2}\le r_A+r_C$$

(4)

(2) When the axes are non-parallel (skew or intersecting): An auxiliary reference plane is constructed such that it contains the branch axis $C_1{C}_2$ while remaining parallel to the manipulator axis $A_1{A}_2$. The collision criterion is established as follows:

$$D_{min}=\sqrt{u_3^2+u_4^2}\le r_A+r_C$$

(5)

where $u_3$ represents the normal distance from the link axis $A_1{A}_2$ to the auxiliary plane, and $u_4$ denotes the minimum planar separation between the branch axis $C_1{C}_2$ and the projection of the manipulator axis on said plane.

2.3. Standard Informed-RRT* Algorithm

The standard RRT* algorithm typically performs uniform sampling across the entire free space, which often leads to the generation of numerous redundant branches and high computational costs when searching for a globally optimal path. To mitigate this inefficiency and accelerate convergence, Gammell et al. [24] proposed the Informed-RRT* algorithm. This framework initially utilizes standard RRT* exploration to identify a feasible guiding path. Once an initial solution is found, the algorithm constructs a heuristic elliptical sampling region (or hyperellipsoid in higher dimensions) defined by the start point, the goal point, and the current path length. This strategic restriction effectively concentrates the search within a promising subset of the state space, thereby significantly accelerating the convergence toward the optimal path.

The mathematical formulation of this elliptical boundary is expressed as in Equation (6):

$${\displaystyle \frac{x^2}{a^2}}+{\displaystyle \frac{y^2}{b^2}}=1$$

(6)

where a and b represent the lengths of the semi-major and semi-minor axes, respectively. The foci of the ellipse are located at $(\pm c,0)$, satisfying the geometric relationship $a^2=b^2+c^2$. In the context of robotic path planning, the semi-major axis a is defined as half of the current optimal path length ($c_{best}/2$), while the focal distance c corresponds to half of the theoretical minimum distance between the start and goal positions ($c_{min}/2$). Consequently, the semi-minor axis is computed as $b={\displaystyle \frac{\sqrt{c_{best}^2-c_{min}^2}}{2}}$. The conceptual geometric modeling of this elliptical region is illustrated in Figure 4.

During each subsequent iteration, if a superior path with a shorter length is identified, the algorithm updates the current optimal path length $c_{best}$ and reconfigures the sampling ellipse. As $c_{best}$ decreases, the major axis of the ellipse contracts, causing the sampling space to shrink accordingly. This dynamic adaptation enables the random tree to converge more rapidly toward the theoretical optimal solution, thereby significantly enhancing planning efficiency.

2.4. Optimization Methods for Informed-RRT*

2.4.1. Target-Biased Sampling Strategy

In apple-harvesting operations, the harvesting targets are typically distributed as discrete fruits within the canopy, with clear spatial locations but surrounded by complicated obstacles. Although the traditional Informed-RRT* algorithm narrows the search scope through elliptical constraints, it may still generate a large number of invalid expansions in non-target areas when lacking clear directional guidance. By introducing a target-biased strategy, this study aims to guide the search process toward higher-quality solution spaces, reduce the search space and execution time, and accelerate the convergence to the optimal solution. Furthermore, this strategy assists the algorithm in escaping local dead ends and avoiding the problem of becoming trapped in local optima, thereby enhancing the robustness and reliability of the path-planning process.

First, a target bias probability p ($0\le p\le 1$) is established. During each sampling iteration, a uniformly distributed random variable $r\in [0,1]$ is generated. The sampling mode is determined by comparing r and p: if $r<p$, the algorithm executes target-directed sampling, directly setting the target point $x_{goal}$ as the current sampling point $x_{sample}$ to force the random tree to extend toward the endpoint. If $r\ge p$, the algorithm executes heuristic candidate sampling, generating multiple random candidate points within the current sampling regoin and selecting the one closest to the target point as $x_{sample}$.

The mathematical expressions for generating the sampling points are presented in Equations (7) and (8), and the underlying principle of the target bias strategy and tree expansion is illustrated in Figure 5.

$$x_{sample}=\left\{\begin{array}{cc}{x}_{goal},\hfill & r<p\hfill \\ x_{random\_sample},\hfill & r\ge p\hfill \end{array}\right.$$

(7)

$$x_{random\_sample}=\arg \underset{x\in \{x_{sample1},x_{sample2},\dots ,x_{samplen}\}}{\min }\parallel x-x_{goal}\parallel$$

(8)

Figure 5 intuitively demonstrates the algorithm’s decision-making process for generating the sampling point $x_{sample}$ and the subsequent tree node expansion. Rather than relying solely on uniform, blind random sampling, the algorithm combines probabilistic guidance with heuristic selection. The right side of Figure 5 illustrates the heuristic candidate sampling process when $r\ge p$: the algorithm generates multiple candidate nodes in the operational space (e.g., $x_{sample1},x_{sample2},x_{sample3}$) and calculates their Euclidean distances to the target point $x_{goal}$, selecting the node with the shortest distance as the final target for the current sampling iteration. The left side of Figure 5 illustrates the tree expansion process, where the random tree extends from the nearest node $x_{near}$ toward the chosen sampling direction to generate a new node $x_{new}$. This mechanism preserves the algorithm’s exploration capability in unknown spaces while simultaneously enhancing the directionality of growth toward the target through the optimal selection of candidate points.

2.4.2. Guided Sampling Strategy Based on Improved Artificial Potential Field

In highly constrained 3D orchard canopy environments, the random tree of the standard Informed-RRT* algorithm tends to blindly expand into infeasible regions, significantly reducing planning efficiency. To mitigate this issue within the Informed-RRT* framework, this study integrates an improved artificial potential field (APF) as a local guidance mechanism. However, traditional APF models often encounter the “Goal Non-Reachability with Obstacle Near” (GNRON) problem. In dense orchards, target fruits are typically surrounded closely by branches; as the end-effector approaches the target sampling point, the local repulsive force often becomes overwhelmingly dominant, preventing the robotic arm from converging to the final picking posture.

To fundamentally resolve this limitation and ensure high-precision kinematic convergence of the end-effector, this study introduces a nonlinear distance regulation factor, $k_g$, based on an exponential function, into the repulsive field model. Its mathematical definition is expressed in Equation (9):

$$k_g=1-e^{-\alpha \rho (x_{\mathrm{near}},x_{\mathrm{sample}})}$$

(9)

where $\alpha$ is a positive tuning parameter ($\alpha >0$), and $\rho (x_{\mathrm{near}},x_{\mathrm{sample}})$ represents the Euclidean distance between the current search tree node $x_{\mathrm{near}}$ and the sampling point $x_{\mathrm{sample}}$. When the robotic arm operates at a location far from the target (i.e., $\rho$ is large), the exponential term approaches 0, ensuring $k_g\approx 1$. This allows the APF to exert the complete repulsive force for safe obstacle avoidance. Conversely, as the end-effector converges toward the sampling point ($\rho \to 0$), the exponential term approaches 1, forcing the regulation factor $k_g$ to decrease to zero smoothly and nonlinearly.

By employing this enhanced APF approach, the overall potential field $U_{total}$ governing the manipulator is formulated by superimposing the localized attraction $U_{att}$ and the cumulative repulsion $U_{rep,i}$ generated by $N_{obs}$ surrounding obstacles:

$$U_{total}=U_{att}+{\displaystyle \sum _{i=1}^{N_{obs}}}{U}_{rep,i}$$

(10)

Consequently, the net spatial force $F_{total}$ guiding the robotic arm’s configuration is computed via Equation (11):

$$F_{total}=F_{att}+{\displaystyle \sum _{i=1}^{N_{obs}}}{F}_{rep,i}$$

(11)

The structure of the distance-regulated repulsive field is defined in Equation (12):

$$U_{rep}=\left\{\begin{array}{cc}{\displaystyle \frac{1}{2}}{k}_{rep}{k}_g{\left({\displaystyle \frac{1}{\rho (x_{near},x_{obs})}}-{\displaystyle \frac{1}{{\rho }_0}}\right)}^2,\hfill & 0\le \rho (x_{near},x_{obs})\le {\rho }_0\hfill \\ 0,\hfill & \rho (x_{near},x_{obs})>{\rho }_0\hfill \end{array}\right.$$

(12)

where $k_{rep}$ signifies the repulsion gain coefficient, $\rho (x_{near},x_{obs})$ represents the spatial clearance to the collision source, and ${\rho }_0$ establishes the maximum effective boundary of the repulsive force. As shown in Equation (12), after integrating this factor into the traditional repulsive potential field, the resultant repulsive force naturally drops to zero at the target position. This nonlinear mathematical architecture not only continuously eliminates repulsive interference near the target, but also effectively prevents the abrupt force jumps and joint trajectory oscillations that might be induced by simple linear adjustments. Consequently, it theoretically guarantees the force equilibrium and robust execution of the 6-DOF robotic arm within the target region.

By computing the negative gradient of $U_{rep}$, the resultant repulsive vector $F_{rep}$ is derived. As delineated in Equations (13)–(15), this vector splits into two distinct components: a primary outward push ($F_{rep1}$) steering the arm away from physical barriers, and a supplementary inward pull ($F_{rep2}$) dragging the node toward the intended sampling coordinate:  

$$F_{rep}=-\nabla U_{rep}=\left\{\begin{array}{cc}{F}_{rep1}+F_{rep2},\hfill & 0\le \rho (x_{near},x_{obs})\le {\rho }_0\hfill \\ 0,\hfill & \rho (x_{near},x_{obs})>{\rho }_0\hfill \end{array}\right.$$

(13)

$$F_{rep1}=k_{rep}{k}_g\left({\displaystyle \frac{1}{\rho (x_{near},x_{obs})}}-{\displaystyle \frac{1}{{\rho }_0}}\right){\displaystyle \frac{1}{{\rho }^2(x_{near},x_{obs})}}\nabla \rho (x_{near},x_{obs})$$

(14)

$$F_{rep2}=-{\displaystyle \frac{1}{2}}{k}_{rep}{\left({\displaystyle \frac{1}{\rho (x_{near},x_{obs})}}-{\displaystyle \frac{1}{{\rho }_0}}\right)}^2\nabla k_g$$

(15)

Geometrically, the direction of $F_{rep1}$ aligns along the vector pointing from $x_{obs}$ to $x_{near}$, whereas the negative gradient logic within $F_{rep2}$ ensures its vector persistently points from $x_{near}$ straight to $x_{sample}$.

The formulations for the attractive potential function $U_{att}$ and the resultant attractive force $F_{att}$ (derived via its negative gradient) are detailed as follows:

$$U_{att}={\displaystyle \frac{1}{2}}{k}_{att}{\rho }^2(x_{near},x_{sample})$$

(16)

$$F_{att}=-\nabla U_{att}=k_{att}(x_{sample}-x_{near})$$

(17)

where $k_{att}$ denotes the attractive gain coefficient. The conceptual geometric modeling of this improved APF-guided sampling process is illustrated in Figure 6. By introducing $k_g$, which encapsulates the distance information relative to the sampling point, the factor approaches zero as the tree node continuously converges toward $x_{sample}$. This mechanism guarantees that both the attractive and repulsive forces decay to zero synchronously. Consequently, it effectively overcomes the inherent GNRON deficiency of the traditional APF method and maximizes the avoidance of local minima traps by synergizing with the probabilistic exploration characteristics of the Informed-RRT* algorithm.

2.4.3. Dynamic Expansion Step Size Mechanism

In orchard environments, the distribution of obstacles (such as branches and leaves) exhibits strong non-uniformity, characterized by high density in local regions and relative openness in inter-row spaces or the canopy periphery. This dramatic variation in spatial scale makes it difficult for the fixed expansion step size in the traditional Informed-RRT* algorithm to balance the search requirements of different regions. Specifically, an overly small fixed step size drastically retards the growth of the search tree in open areas, thereby severely degrading the global exploration efficiency of the algorithm. Conversely, an excessively large step size renders the algorithm highly prone to collisions with obstacles within dense canopies, generating a multitude of invalid samples and significantly compromising the system’s traversal performance in narrow spaces.

To resolve this spatial adaptability conflict caused by the fixed step size, inspired by the step-size calculation method proposed in literature [34], this paper introduces a dynamic step-size strategy based on obstacle–distance perception. This strategy establishes a non-linear mapping relationship, and its calculation is formulated as in Equation (18):

$${\lambda }_d=(\zeta e^{A·D_{obs}}-0.8)·\lambda$$

(18)

where the default initial expansion length is denoted by $\lambda$; ${\lambda }_d$ represents the dynamically adjusted expansion step size; $\zeta$ is the regulation coefficient controlling the baseline step amplitude; A is the positive coefficient adjusting the decay rate of the exponential function; and $D_{obs}$ denotes the shortest spatial distance between the nearest node of the current search tree and the surrounding obstacles. By dynamically adjusting the step size, this strategy not only assists the random tree in escaping obstacle-dense regions but also enhances the overall smoothness of the path.

Based on this exponential mapping model, when a node is situated in an open area far from obstacles (i.e., $D_{obs}$ is large), the exponential term tends to amplify, and the algorithm adaptively uses a larger step size ${\lambda }_d$ for expansion, thereby maximizing search efficiency and accelerating the convergence rate. Conversely, when the node approaches dense branches (i.e., $D_{obs}$ converges toward a minimum value), the exponential term decays rapidly, forcing the expansion step size ${\lambda }_d$ to shrink continuously and smoothly. Through this dynamic step-size regulation mechanism of “fine-tuning near obstacles and accelerating in open spaces,” it not only compels the manipulator to execute high-resolution, safe collision-avoidance movements in hazardous areas, substantially improving the escape rate of the random tree in dense obstacle regions, but also effectively suppresses trajectory oscillations induced by fixed large step sizes, inherently enhancing the overall smoothness and execution safety of the path.

To prevent this localized expansion stagnation, we engineered a bounded, piecewise adaptive mechanism. This upgraded strategy introduces a perception threshold $D_s$ alongside a hard operational lower bound ${\lambda }_{min}$, comprehensively formulated in Equation (19):

$$stepsize=\left\{\begin{array}{cc}\lambda ,\hfill & D_{obs}>D_s\hfill \\ {\lambda }_d,\hfill & D_{obs}\le D_s\mathrm{and}{\lambda }_d\ge {\lambda }_{min}\hfill \\ {\lambda }_{min},\hfill & D_{obs}\le D_s\mathrm{and}{\lambda }_d<{\lambda }_{min}\hfill \end{array}\right.$$

(19)

The geometric interpretation of this bounded expansion strategy is depicted in Figure 7. When the spatial clearance $D_{obs}$ exceeds the safety threshold $D_s$, the environment is deemed sufficiently open, prompting the algorithm to execute maximum-stride explorations using the default $\lambda$. Conversely, once the arm breaches the obstacle perception zone ($D_{obs}\le D_s$), the dynamic scaling factor ${\lambda }_d$ is activated, proportionally dampening the step size to delicately maneuver through narrow branch gaps. Crucially, if this decayed value drops below the functional limit ${\lambda }_{min}$, the algorithm overrides the continuous decay and enforces ${\lambda }_{min}$ as the absolute minimum stride. This safeguard ensures continuous, albeit cautious, mobility even in the most severely constricted environments.

Ultimately, synthesizing the heuristic target-biasing, the enhanced APF directional guidance, and the bounded dynamic step size yields the final node generation protocol. For the current expanding node $x_{near}$, the exact spatial coordinate of the newly generated node $x_{new}$ is calculated via Equation (20):

$$x_{new}=x_{near}+stepsize·n$$

(20)

where n represents the normalized unit vector dictating the optimal growth trajectory. This vector is directly extracted from the localized APF resultant force $F_{total}$, mathematically defined in Equation (21):

$$n={\displaystyle \frac{F_{total}(x_{near},x_{sample})}{\parallel F_{total}(x_{near},x_{sample})\parallel }}$$

(21)

2.4.4. Path Post-Processing and Trajectory Optimization Strategy

Raw spatial trajectories synthesized by the aforementioned sampling phase typically consist of disjointed linear segments, inducing undesirable sharp corners and unnecessary joint wear for the robotic arm. To refine these geometric zigzags into an efficient, kinematically executable trajectory, we propose a two-stage post-processing framework: a greedy line-of-sight (LOS) pruning mechanism followed by cubic B-spline relaxation.

The core logic of the redundant node elimination is driven by a forward-scanning spatial inspection, structured as follows:

(1) Initialize the pruned trajectory array, $Pat{h}_{new}$, with the spatial starting point $x_{init}$, designating it as the active anchor point $x_{cur}$:

$$Pat{h}_{new}=\left\{x_{init}\right\}$$

(22)

$$x_{cur}=x_{init}$$

(23)

(2) From the active anchor $x_{cur}$, sequentially project a virtual LOS vector to subsequent waypoints in the unoptimized sequence $Pat{h}_{old}$. If the linear interpolation between $x_{cur}$ and a candidate node is strictly collision-free, the inspection continues to advance to the next consecutive node.

(3) Once spatial interference is detected between $x_{cur}$ and a specific distant node $x_{stop}$, the waypoint immediately preceding the collision point (denoted as $x_{last}$) is verified as the furthest reachable safe node. This node is then appended to the optimized array and becomes the new anchor:

$$Pat{h}_{new}\leftarrow Pat{h}_{new}\cup \left\{x_{last}\right\}$$

(24)

$$x_{cur}=x_{last}$$

(25)

(4) This forward-scanning loop persists until a clear LOS is established between the current anchor and the global destination $x_{goal}$. The final step appends $x_{goal}$ to finalize the simplified route:

$$Pat{h}_{new}=\{x_{init},\dots ,x_{goal}\}$$

(26)

The geometric mechanics of this greedy pruning are visualized in Figure 8. The original trajectory (black sequence) is evaluated via virtual LOS rays (red dashed lines). Initiating at $x_{init}$, direct connections to nodes $x_1$ and $x_2$ are safe, but the ray to $x_4$ intersects obstacle 1. Consequently, the boundary node $x_3$ is retained as a permanent waypoint. Resuming from $x_3$, the rays safely bypass $x_4$ and $x_5$, but a collision occurs at $x_7$ due to obstacle 2. Hence, $x_6$ is registered. Finally, an unobstructed path connects $x_6$ directly to $x_{goal}$. The newly established minimal-node trajectory (red solid line) reduces to:

$$Pat{h}_{new}=\{x_{init},x_3,x_6,x_{goal}\}$$

(27)

While greedy pruning effectively removes macroscopic detours, the residual trajectory retains abrupt angular transitions at the remaining waypoints (e.g., $x_3$ and $x_6$). To guarantee $C^2$ continuity for the 6-DOF manipulator’s joints, a cubic B-spline function is mapped over the pruned vertices. The parametric curve $C\left(u\right)$ is expressed mathematically as follows:

$$C\left(u\right)={\displaystyle \sum _{i=0}^n}{N}_{i,3}\left(u\right)·P_i,u\in [0,1]$$

(28)

where $P_i$ denotes the set of spatial control points corresponding to the elements in $Pat{h}_{new}$, and $N_{i,3}\left(u\right)$ represents the third-degree B-spline basis function regulating the curve’s local support. Figure 9 showcases the resulting smooth curvature.

Synthesizing all the aforementioned optimization modules, the holistic architectural flowchart of the proposed TAD-IRRT* algorithm pipeline is summarized in Figure 10.

This flowchart illustrates the proposed target-biased, APF-enhanced, and dynamic step-size integrated Informed-RRT* (TAD-IRRT*) path-planning algorithm tailored for an apple-harvesting robotic manipulator. Initialized with the start configuration $x_{\mathrm{start}}$, target destination $x_{\mathrm{goal}}$, and environmental obstacle models, the algorithm performs the spatial path search by iteratively expanding a random tree. Specifically, a target-biased heuristic sampling strategy is employed to determine optimal sample points, while an improved artificial potential field (APF) method calculates the guiding force vector. Concurrently, a dynamic step-size mechanism regulated by the obstacle distance is introduced to generate the new candidate node $x_{\mathrm{new}}$. Once verified through collision detection, valid nodes undergo the standard rewiring process, and the informed-ellipse sampling domain is adaptively updated to enhance path optimization efficiency. Upon discovering an initial feasible path, a greedy pruning criterion is executed to eliminate redundant nodes, followed by cubic B-spline smoothing to generate a continuous, executable, and high-quality trajectory that drives the manipulator to accomplish the apple-harvesting task. If the maximum iteration limit is reached without identifying a valid path, planning terminates as a failure.

3. Results

3.1. Simulation Results of the TAD-IRRT* Algorithm

To verify the effectiveness of the TAD-IRRT* algorithm, 2D and 3D simulation environments with dimensions of 500 × 500 cm and 500 × 500 × 500 cm were constructed in MATLAB 2023a, respectively. The simulation results, which compare the path-searching performance of the RRT, RRT*, standard Informed-RRT*, and TAD-IRRT* algorithms in both 2D and 3D spaces, are illustrated in Figure 11 and Figure 12. In the 2D configuration (Figure 11), the starting and target points are defined at coordinates (1, 490) and (490, 1), with black rectangles and circles representing obstacles. Similarly, the 3D environment (Figure 12) initializes the starting point at (1, 1, 1) and the target at (490, 490, 490), where spheres denote obstacles. In these figures, the blue solid lines indicate the final trajectories planned by the respective algorithms.

As illustrated in Figure 11 and Figure 12, in both 2D and 3D environments, the RRT algorithm exhibits distinct characteristics of global random expansion; the extensive and unstructured gray search trees spread throughout the free space, leading to numerous invalid detours in the generated paths. The RRT* algorithm improves the overall path quality to a certain extent, but the dense distribution of gray branches indicates that it still generates a large number of redundant, invalid sampling branches in the vicinity of obstacles. The standard Informed-RRT* algorithm effectively constrains the diffusion range of the sampling tree through an elliptical boundary, which reduces the overall path length; however, due to the lack of directional guidance during its internal expansion, unnecessary local path deviations still occur in regions with unevenly distributed obstacles. In contrast, under the guidance of the target bias and the improved artificial potential field (APF) method, the gray search branches of the TAD-IRRT* algorithm are highly compressed and concentrated within the effective obstacle-avoidance channels, significantly eliminating invalid expansions. Simultaneously, the blue solid line of TAD-IRRT* algorithm in the figures intuitively demonstrate that, after path post-processing, this algorithm generates smoother and more direct collision-free trajectories in both 2D and 3D environments. The number of path turning points is greatly reduced, which better satisfies the actual kinematic and dynamic requirements of the 6-DOF apple-harvesting robotic arm in complicated orchard environments.

To mitigate the influence of inherent algorithmic randomness on the generated paths, every method was independently tested 30 times, with an upper limit of 1500 iterations per run. The mean success rate is calculated as the frequency of successfully navigating from the initial state to the target destination across these 30 attempts. Detailed quantitative outcomes are summarized in

Table 2. Quantitative performance comparison of the evaluated algorithms across 2D and 3D simulation environments.

Simulation Environment	Algorithm	Average Path-Planning Time (s)	Average Path Length (m)	Success Rate (%)
2D space	RRT [22]	0.04	8.46	100
	RRT* [23]	0.51	7.47	100
	Informed-RRT* [24]	0.54	7.46	100
	TAD-IRRT*	0.46	7.08	100
3D space	RRT [22]	0.03	10.74	100
	RRT* [23]	0.42	9.16	100
	Informed-RRT* [24]	0.43	9.25	100
	TAD-IRR*	0.28	8.51	100

Note: The robot is abstracted as a spatial point mass. Therefore, the average path length is evaluated as the pure Cartesian Euclidean distance (m).

.

As shown in Table 2, in the 2D environment, the average path length of the TAD-IRRT* algorithm is 7.08 m, which is approximately 16.3%, 5.2%, and 5.1% shorter than that of RRT, RRT*, and standard Informed-RRT*, respectively. In the 3D environment, its average path length is 8.51 m, representing reductions of approximately 20.8%, 7.1%, and 8.0% compared to the aforementioned three algorithms, respectively. Although the overall path lengths for all algorithms increase as the spatial dimensionality expands from 2D to 3D, the TAD-IRRT* algorithm maintains the shortest path length in both environments, indicating its strong adaptability to spatial dimensionality changes.

Regarding planning time, similar trends are observed in both 2D and 3D environments: RRT requires less planning time but yields limited path quality due to the lack of heuristic guidance; RRT* experiences a surge in sampling nodes and significantly prolonged computation time owing to its global blind search and rewiring mechanism; the standard Informed-RRT* requires relatively more planning time because of the calculation of the elliptical sampling domain. In comparison, the average planning times of the TAD-IRRT* in 2D and 3D environments are 0.46 s and 0.28 s, respectively, both of which are lower than those of the standard Informed-RRT*. It is worth noting that in this experiment, the average planning time of the TAD-IRRT* algorithm in the 3D environment is lower than that in the 2D environment. This phenomenon indicates that when facing obstacle layouts in a 2D plane that easily form local minima, the proposed algorithm can fully utilize the increased degrees of freedom in 3D space. Combined with the improved APF guidance and dynamic step size mechanism, it leverages the height dimension (Z-axis) to bypass obstacles, effectively mitigating invalid sampling and iteration stagnation in constrained, lower-dimensional areas.

Based on the comprehensive 2D and 3D experimental results, it can be concluded that the TAD-IRRT* algorithm demonstrates consistent superiority in key performance metrics, such as path length and computation time, across different spatial dimensions corresponding to orchard operations. Under conditions of rising dimensionality and dense obstacles, the proposed algorithm can still efficiently generate stable, high-quality collision-free paths, providing a reliable foundation for subsequent robotic arm path-planning experiments in complicated orchard harvesting scenarios.

3.2. Simulation Obstacle Avoidance Path-Planning Experiment

To evaluate the obstacle avoidance performance of the algorithm in realistic unstructured orchard environments, a complicated harvesting scenario accurately replicating the spatial constraints of an actual orchard was constructed in the RViz visual simulation platform and MoveIt motion planning framework under ROS (Robot Operating System). The scenario focuses on simulating the main trunk and multi-layered crossing branches of fruit trees, which are characterized by irregular distributions and overlapping occlusions. Such a highly constrained motion space imposes stringent requirements on the computational efficiency and heuristic directional guidance of the planning algorithm. In this simulation, the initial joint angles of the robotic arm were configured as $[-1.36,-1.88,-0.04,0.76,1.31,-1.05]$ radians, and the target joint angles were set to $[0.43,1.88,-0.13,-0.64,-0.49,1.28]$ radians. Figure 13 shows the initial position of the robotic arm in the harvesting scenario.

Under the same initial conditions, this study compared the planning effects of four algorithms: RRT, RRT*, standard Informed-RRT*, and the proposed TAD-IRRT*. To ensure the comparability of the experimental results and meet the real-time requirements of actual agricultural harvesting, a maximum time constraint (5 s) was set for a single planning attempt in the path-planning experiment. When the time limit is reached, if the algorithm fails to generate a feasible path, it is forced to terminate and judged as a failure. The intuitive trajectory performance of the simulation results is shown in Figure 14.

As illustrated in Figure 14, due to its global random sampling characteristic, the standard RRT algorithm generates a highly tortuous and rigid path, characterized by numerous inefficient detours, backtracks, and irregular sharp angles. The RRT* algorithm exhibits noticeable improvement over RRT; its overall trajectory is more logical, and non-target-directed explorations are significantly reduced. However, local segments of the path still display distinct sharp bends, indicating insufficient smoothness. By introducing an elliptical sampling heuristic, the standard Informed-RRT* algorithm successfully restricts the search space, effectively bounding the path between the start and goal configurations to produce a more compact trajectory than RRT*. Nevertheless, when navigating through densely obstructed regions, local detours persist, leading to trajectory redundancy and extraneous folds. In stark contrast, empowered by target biasing, improved artificial potential field (APF) guidance, and a dynamic step-size mechanism, the proposed Canopy-Adaptive TAD-IRRT* algorithm generates the shortest path among the four algorithms. It achieves the lowest path curvature—yielding the highest visual smoothness—and demonstrates the capability to proactively and cleanly circumvent fine branches and foliage, ultimately presenting an optimal geometric morphology.

An evaluation of the four algorithms in challenging scenarios demonstrates that the TAD-IRRT* method substantially outperforms the alternatives regarding both trajectory length and computational speed. To ensure a robust quantitative assessment, each approach underwent 30 distinct motion planning trials within this complicated environment. Here, the success rate denotes the frequency with which a planner successfully navigates from the origin to the destination under a strict 5 s limit across these iterations. The corresponding statistical outcomes are detailed in

Table 3. Quantitative performance evaluation of the tested algorithms in a cluttered-obstacle scenario.

Algorithm	Average Path Planning Time (s)	Average Path Length (Rad)	Success Rate (%)
RRT [22]	1.70	9.86	100.00
RRT* [23]	5.01	6.86	90.00
Informed-RRT* [24]	3.36	6.61	70.00
TAD-IRRT*	1.34	5.58	100.00

Note: The average planning time and average path length are calculated only for the successful trials. And the average path length here is quantified as the cumulative joint-space displacement (rad).

.

As can be seen from Table 3, the average joint space path length of the TAD-IRRT* algorithm is only 5.58 rad, which is about 15.6% lower than that of the standard Informed-RRT* and 43.4% lower than that of the traditional RRT. The significant reduction in the joint space path length directly reflects the reduction in invalid detours and redundant pose switches of the robotic arm in the complicated 3D space, which is mainly attributed to the deep simplification of the trajectory by the greedy pruning strategy and the effective guidance of the search direction by the APF.

In terms of planning time and success rate, the experimental data reveals a highly typical calculation bottleneck phenomenon in complicated restricted spaces: since standard RRT does not require node rewiring optimization, it can quickly find a sub-optimal path through random walks within 5 s, thus achieving a 100% success rate. However, to optimize path quality, RRT* and standard Informed-RRT* frequently perform nearest-neighbor searches and collision detection in the dense fruit tree point cloud/mesh environment, leading to a surge in computational load, with average times reaching 5.01 s and 3.36 s, respectively. This massive computational overhead frequently triggers the 5 s timeout mechanism, causing their planning success rates to drop to 90% and 70%, respectively.

Addressing this pain point, the TAD-IRRT* demonstrates an overwhelming advantage. Its average planning time is only 1.34 s, which is significantly shortened by about 60.1% compared to the standard Informed-RRT*, and the success rate is firmly stabilized at 100%. This fully proves that the dynamic step size mechanism (utilizing larger expansion steps in obstacle-free regions and finer resolution near obstacles) and the potential field guidance strategy introduced in this paper effectively eliminate the massive invalid collision detection calculations caused by blind sampling in traditional algorithms. When facing high-difficulty, high-density orchard obstacle avoidance tasks, the TAD-IRRT* algorithm exhibits extremely strong environmental adaptability and robustness.

3.3. Continuity Analysis of Joint Space Trajectories

Based on the simulation results in Section 3.1 and Section 3.2, the proposed TAD-IRRT* algorithm demonstrates superior performance across all evaluated spatial planning metrics. However, deploying the algorithm in the physical workspace of the manipulator necessitates strict operational safety and stability, particularly during continuous motion execution. To verify that the TAD-IRRT* algorithm can ensure the stable operation of the harvesting manipulator, real-time kinematic monitoring was conducted on the obstacle-avoidance trajectory generated within the complicated scenario from Section 3.2. In the MoveIt and RViz co-simulation environment, the motion state of the manipulator was continuously published to the joint_states topic. A custom ROS node was developed to subscribe to this topic and record the temporal angular displacements of the six joints, yielding the position-time curves plotted in Figure 15a.

To rigorously extract the underlying kinematic characteristics from the recorded data, a Fourier curve-fitting method was employed to model the mathematical relationship between joint displacement and time. Figure 15b illustrates the fitted curves for each joint. The position-time fitting functions for the six joints are shown in Equations (29)–(34).

$$\begin{array}{cc}\hfill po{s}_1\left(t\right)=& -0.1318-0.6668\cos \left(0.3026t\right)-0.5415\sin \left(0.3026t\right)-0.3332\cos \left(0.6052t\right)-0.1574\sin \left(0.6052t\right)\hfill \\ \hfill & -0.2260\cos \left(0.9078t\right)-0.0141\sin \left(0.9078t\right)\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill po{s}_2\left(t\right)=& -0.1010-1.2173\cos \left(0.3957t\right)-1.4320\sin \left(0.3957t\right)-0.4327\cos \left(0.7914t\right)+0.0194\sin \left(0.7914t\right)\hfill \\ \hfill & -0.1209\cos \left(1.1871t\right)+0.1381\sin \left(1.1871t\right)\hfill \end{array}$$

(30)

$$po{s}_3\left(t\right)=0.0130-0.0758\cos \left(0.4637t\right)+0.1235\sin \left(0.4637t\right)$$

(31)

$$po{s}_4\left(t\right)=0.0727+0.6033\cos \left(0.3377t\right)+0.3603\sin \left(0.3377t\right)+0.0781\cos \left(0.6754t\right)-0.0476\sin \left(0.6754t\right)$$

(32)

$$po{s}_5\left(t\right)=0.4491+0.8182\cos \left(0.3147t\right)+0.5446\sin \left(0.3147t\right)$$

(33)

$$po{s}_6\left(t\right)=0.1847-0.9301\cos \left(0.3634t\right)-0.5353\sin \left(0.3634t\right)-0.3331\cos \left(0.7268t\right)+0.0854\sin \left(0.7268t\right)$$

(34)

To quantitatively evaluate the precision of the Fourier fitting, the coefficient of determination ($R^2$) and the Root Mean Square Error (RMSE) were calculated for each joint’s fitted curve, with the results summarized in

Table 4. Evaluation indexes of fitted curves for each joint position over time.

Joint Name	R-Square (${\mathit{R}}^2$)	RMSE
Joint 1	0.9995	0.03161
Joint 2	0.9999	0.02004
Joint 3	0.9199	0.03994
Joint 4	0.9987	0.02429
Joint 5	0.9962	0.05550
Joint 6	0.9957	0.07426
Average	0.9849	0.04094

.

As shown in Table 4, the average $R^2$ value reaches 0.9849, and the RMSE remains at an extremely low level, indicating that the Fourier mathematical model can reconstruct the actual motion trajectory of the manipulator with exceptionally high fidelity. To quantitatively evaluate the trajectory-tracking fidelity, the final Cartesian positioning error of the end-effector was analyzed. Specifically, by substituting the terminal joint angles derived from the fitted functions at $t=12\mathrm{s}$ into the forward kinematics model of the manipulator, the actual coordinates reached by the end-effector were determined as $[342.21,-40.53,-710.22]\mathrm{mm}$. Compared with the ideally planned target coordinates of $[348.95,-39.27,-709.35]\mathrm{mm}$, the final absolute error in the 3D operational space is merely $6.911\mathrm{mm}$. This sub-centimeter level deviation thoroughly demonstrates that the proposed curve-fitting strategy preserves high terminal accuracy while smoothing the joint motions. Furthermore, observing Figure 15a, it is evident that the manipulator initiates movement at approximately $t=2$ s, gradually decelerating to a stop around $t=10$ s. To accurately evaluate the smoothness of this motion, this study isolated the core execution interval (2–10 s). The first and second derivatives of the fitted joint displacement functions were computed to derive the time-varying curves for angular velocity and angular acceleration, respectively, as presented in Figure 16.

An analysis of Figure 16 clearly reveals that both the angular velocity and angular acceleration of the manipulator maintain smooth and continuous trends throughout the entire execution period, without any pronounced abrupt changes or chattering phenomena. This indicates that the manipulator moves at a coordinated and stable pace when executing the planned path. These results verify that the TAD-IRRT* algorithm effectively guarantees the kinematic feasibility and operational stability of the manipulator, which is crucial for reducing mechanical wear and extending the service life of agricultural robotic systems.

3.4. Physical Obstacle Avoidance Verification Experiment

To further verify the practical obstacle avoidance efficiency and operational robustness of the proposed TAD-IRRT* algorithm in complicated unstructured environments, verification experiments were conducted in a real physical space utilizing the apple-harvesting robot prototype. Three operating scenarios with distinct obstacle densities—sparse, medium, and dense—were manually constructed. In these three scenarios, the initial pose of the manipulator and the spatial position of the target fruit remained completely identical, with the sole control variable being the occlusion degree of obstacles (such as branches) in front of the target fruit. This experimental design aims to highly replicate the variable and severely restricted 3D spatial constraints within the orchard canopy.

Since the paths generated by the baseline RRT and RRT* algorithms are highly unsmooth, direct execution on the hardware platform would lead to severe mechanical jitter and hardware wear. Therefore, the real-world experiments in this section only compare the standard Informed-RRT*, which inherently possesses path-smoothing characteristics, with the proposed TAD-IRRT* algorithm. For each density scenario, both algorithms initiated 30 independent planning and execution tests from the same initial pose toward the target fruit to comprehensively evaluate their performance in terms of key metrics, including the path-planning success rate, average real execution time, and average path length. It is worth noting that the low-level servo control system of the 6-DOF harvesting manipulator used in this experiment integrates an overload protection mechanism based on current-loop feedback and a soft collision detection mechanism. In an unstructured orchard environment, if the manipulator makes unexpected contact with branches during motion and generates external resistance exceeding the safety threshold, the system immediately triggers an emergency stop strategy to prevent physical damage. Such cases are explicitly determined as path-planning failures. The specific experimental comparison data are summarized in

Table 5. Comparison of physical experimental results under different obstacle densities.

Task Scenario	Algorithm	Average Real Execution Time (s)	Average Path Length (Rad)	Success Rate (%)
Task 1 (Sparse)	Informed-RRT*	8.45	5.73	100.0
	TAD-IRRT*	7.12	4.86	100.0
Task 2 (Medium)	Informed-RRT*	15.65	6.47	76.7
	TAD-IRRT*	8.55	5.38	93.3
Task 3 (Dense)	Informed-RRT*	18.32	7.69	53.3
	TAD-IRRT*	10.79	5.91	90.0

Note: Average real execution time and average path length are calculated only using test samples that were successfully executed without physical collisions. And the average path length here is quantified as the cumulative joint-space displacement (rad).

.

As shown in Table 5, the physical experimental results indicate that in the dense-obstacle scenario (Task 3), where the spatial restriction is most severe, the standard Informed-RRT* algorithm easily causes physical scratching and triggers emergency stops when traversing narrow gaps between branches and leaves. This is primarily because its generated paths are excessively tortuous and lack fine local potential field guidance, resulting in a path-planning success rate of only 53.3% and a high average real execution time of 18.32 s. In contrast, the TAD-IRRT* algorithm proposed in this study demonstrates a significant competitive advantage in robustness. In the identical dense occlusion scenario, the path-planning success rate of the proposed algorithm is substantially increased to 90.0%. Benefiting from the deep integration of the greedy pruning strategy and the cubic B-spline smoothing technology, TAD-IRRT* effectively eliminates redundant via-points and high-frequency mechanical jitter during physical execution. This feature not only renders the manipulator’s motion more coherent but also significantly shortens the average real execution time in the dense scenario to 10.79 s.

This fully proves that the proposed algorithm not only improves searching and convergence efficiency at the theoretical level but also effectively guarantees the smoothness of the manipulator trajectory at the physical engineering level, thereby greatly enhancing its reliability and safety during practical deployment. To intuitively demonstrate this process, the sequential obstacle avoidance workflow of the TAD-IRRT* algorithm in a dense scenario is illustrated in Figure 17.

4. Discussion

To address the aforementioned shortcomings of existing algorithms in complicated, unstructured orchard environments, this paper proposes the TAD-IRRT* algorithm, a 3D obstacle avoidance path-planning method for a 6-DOF apple-harvesting manipulator that incorporates multi-strategy optimization into the Informed-RRT* framework. In recent years, although cutting-edge algorithms such as heuristic goal biasing or bidirectional tree growth have demonstrated certain potential in narrow space planning [35], they remain prone to expansion stagnation when confronted with the extremely dense and unstructured canopies of orchards. To address this, this paper integrates an improved artificial potential field (APF) deeply within the standard Informed-RRT* framework. This strategy not only endows the random tree with more acute obstacle penetration and directional perception capabilities at the micro-level, but also effectively resolves the persistent issue wherein current mainstream adaptive exploration mechanisms [36] easily fall into local optimal deadlocks among intersecting 3D branches by combining goal biasing with dynamic anti-collision repulsive forces. Meanwhile, whereas most existing variable step-size planning algorithms are often restricted to a single distance threshold or macroscopic environmental complexity [37], the dynamic expansion mechanism based on obstacle distance perception constructed in this study achieves a seamless transition between large-stride efficient exploration in sparse spaces and small-stride precise obstacle avoidance in crowded regions. Finally, addressing the problem that many current efficient sampling algorithms tend to overlook physical feasibility, thereby causing high-frequency chattering during robotic arm execution [38], this paper introduces greedy criterion-based path-pruning and cubic B-spline curve techniques for deep post-processing. This series of optimization strategies successfully transforms the theoretical geometric path into a high-quality, smooth, and continuous trajectory that strictly adheres to the underlying kinematic and dynamic constraints of the 6-DOF apple-harvesting robotic arm.

The experimental results comprehensively demonstrate the superiority of the proposed algorithm in both simulated and physical environments. The simulation results in complicated ROS-based scenarios demonstrate that the proposed algorithm achieves a 100% planning success rate. Compared to the standard Informed-RRT* algorithm, its average computational time and joint-space path length are reduced by approximately 60.1% and 15.6%, respectively. Concurrently, kinematic analysis via Fourier curve fitting ($R^2=0.9849$) confirms that the angular velocity and acceleration of each joint remain smooth and continuous, successfully eliminating high-frequency chattering. Crucially, these theoretical advantages successfully translated to real-world engineering applications. Physical verification experiments revealed that in the dense-obstacle scenarios, the TAD-IRRT* algorithm reduces the average execution time by 41% and increases the path execution success rate by 36.7% compared to the standard Informed-RRT*. This demonstrates that the TAD-IRRT* algorithm not only enhances spatial search efficiency but also ensures trajectory smoothness at the physical control level, thereby greatly improving operational reliability and safety in complicated orchards.

Despite this excellent performance, certain limitations remain regarding the transition to real-world agricultural applications. The current collision detection approach utilizes a rigid geometric bounding volume method (sphere and capsule models), which may overestimate or underestimate collision risks when encountering highly irregular physical boundaries. Future research will focus on exploring flexible collision detection mechanisms integrated with real-time 3D visual perception to adapt to highly irregular branch and leaf boundaries. Furthermore, regarding the physical deployment, this study currently assumes a decoupled ‘stop-and-pick’ operational paradigm. Specifically, the mobile base is responsible for macroscopic navigation between tree rows and remains strictly stationary during the harvesting execution. Consequently, the proposed TAD-IRRT* algorithm focuses exclusively on the high-dimensional, fine-grained obstacle avoidance of the 6-DOF manipulator within the canopy, excluding the motion planning of the mobile base. Although this static-base assumption ensures operational stability and effectively reduces the planning dimensionality, integrating the mobile base and the manipulator into a coupled whole-body motion planning framework remains a promising direction for future research to further enhance continuous harvesting efficiency.

5. Conclusions

To address the collision-free path-planning problem for a 6-DOF apple-harvesting robotic arm in complicated working environments, this study proposes a canopy-adaptive TAD-IRRT* algorithm. Based on the standard Informed-RRT* framework, this method deeply integrates target-biased sampling, improved artificial potential field (APF) guidance, and a dynamic step-size mechanism, further incorporating cubic B-spline techniques for trajectory smoothing. The simulation results in complicated ROS-based scenarios demonstrate that the proposed algorithm achieves a 100% planning success rate. Compared to the standard Informed-RRT* algorithm, its average computational time and joint-space path length are reduced by approximately 60.1% and 15.6%, respectively. Concurrently, kinematic analysis via Fourier curve fitting ($R^2=0.9849$) confirms that the angular velocity and acceleration of each joint remain smooth and continuous, successfully eliminating high-frequency chattering. Furthermore, physical prototype experiments in the dense-obstacle scenarios demonstrate that the proposed method reduces the average execution time by 41% and increases the path execution success rate by 36.7% compared to the standard Informed-RRT*. In conclusion, this study effectively resolves the contradiction between generating high-quality paths and high computational overhead in high-dimensional spaces, providing a reliable solution that significantly reduces mechanical wear.