Front. Plant Sci. Frontiers in Plant Science Front. Plant Sci. 1664-462X Frontiers Media S.A. 10.3389/fpls.2024.1337638 Plant Science Original Research Improving path planning for mobile robots in complex orchard environments: the continuous bidirectional Quick-RRT* algorithm Ye Lei * Li Jin Li Pu School of Intelligent Engineering, Shaoguan University, Shaoguan, China

Edited by: Yunchao Tang, Dongguan University of Technology, China

Reviewed by: János Botzheim, Eötvös Loránd University, Hungary

Weikuan Jia, Shandong Normal University, China

Jinhai Wang, Foshan University, China

Xing Wang, Commonwealth Scientific and Industrial Research Organisation (CSIRO), Australia

*Correspondence: Lei Ye, leoye1992@sgu.edu.cn

13 05 2024 2024 15 1337638 13 11 2023 22 04 2024 Copyright © 2024 Ye, Li and Li 2024 Ye, Li and Li

This is an open-access article distributed under the terms of the Creative Commons Attribution License (CC BY). The use, distribution or reproduction in other forums is permitted, provided the original author(s) and the copyright owner(s) are credited and that the original publication in this journal is cited, in accordance with accepted academic practice. No use, distribution or reproduction is permitted which does not comply with these terms.

Efficient obstacle-avoidance path planning is critical for orchards with numerous irregular obstacles. This paper presents a continuous bidirectional Quick-RRT* (CBQ-RRT*) algorithm based on the bidirectional RRT (Bi-RRT) and Quick-RRT* algorithms and proposes an expansion cost function that evaluates path smoothness and length to overcome the limitations of the Quick-RRT* algorithm for non-holonomic mobile robot applications. To improve the zigzag between dual trees caused by the dual-tree expansion of the Bi-RRT algorithm, CBQ-RRT* proposes the CreateConnectNode optimization method, which effectively solves the path smoothness problem at the junction of dual trees. Simulations conducted on the ROS platform showed that the CBQ-RRT* outperformed the unidirectional Quick-RRT* in terms of efficiency for various orchard layouts and terrain conditions. Compared to Bi-RRT*, CBQ-RRT* reduced the average path length and maximum heading angle by 8.5% and 21.7%, respectively. In addition, field tests confirmed the superior performance of the CBQ-RRT*, as evidenced by an average maximum path lateral error of 0.334 m, a significant improvement over Bi-RRT* and Quick-RRT*. These improvements demonstrate the effectiveness of the CBQ-RRT* in complex orchard environments.

 path planning mobile robots orchard environments rapidly exploring random tree bidirectional expansion section-in-acceptance Sustainable and Intelligent Phytoprotection Introduction

Mobile robots are garnering increasing attention among researchers as intelligent devices. Owing to their high degree of automation, mobile robots can improve productivity and provide various conveniences. Mobile robots are currently being utilized in various scenarios (Ferguson and Stentz, 2006; Zhao et al., 2014; Ljungqvist et al., 2019; Reid et al., 2020; Strader et al., 2020; Buchanan et al., 2021; Ye et al., 2023). As agricultural automation research has gained widespread attention in recent years, mobile robots have begun to make their way into agricultural settings (Xiong et al., 2017; Blok et al., 2019; Ren et al., 2020). Khajepour et al. (2020) proposed a particle filter algorithm using a laser beam model and Kalman filter algorithm based on line detection for the positioning and navigation of mobile robots in orchards. Qiu and Li (2022) implemented a path-tracking control for orchard mobile robots using a PID algorithm. Yang et al. (2022) used a dataset of orchard road data to train semantic segmentation networks to extract navigation paths and achieve path tracking for orchard robots. However, the studies above focused on orchard robots operating in structured environments with more prominent road features, with less consideration for path planning and obstacle avoidance. In unstructured orchard scenes, it is difficult for mobile robots to make decisions by obvious features or scene markers to realize specific navigation movements. The mobile robot in the orchard needs to perceive the unstructured orchard environment and plan the movement path for the mobile robot in the navigation operation stably and efficiently through the constructed environment map or by obtaining the real-time perception feedback. Therefore, in the field of mobile robot navigation in orchards, path planning is a fundamental and crucial aspect. Path planning refers to the process by which a mobile robot identifies, within the configuration space, an obstacle-free path from its initial configuration to its target configuration. The path must simultaneously satisfy environmental constraints and the kinematic constraints of the mobile robot itself (Siciliano et al., 2008).

In recent years, a substantial amount of research has been dedicated to the path planning conundrum for orchard robots. Jiang et al. (2022) used 3D and 2D LiDAR to map greenhouse orchards and used the Dijkstra algorithm to plan global paths for mobile robots. Tian et al. (2023) proposed an ant colony optimization algorithm to solve multitarget waypoint planning for unmanned aerial vehicles in orchards. Zhang et al. (2023) introduced an improved artificial potential field algorithm for weed-removal robot path planning, improving the safety of robot automation operations. Zhang et al. (2022) conducted LiDAR scans of greenhouse maps and employed the probabilistic roadmap technique to calculate the paths between all target points, consequently establishing comprehensive global paths for navigating multiple operational points. In addition to general-purpose path planning algorithms, researchers have extensively researched path planning algorithms for special orchard target guidance based on the perception of the orchard scene. Liu et al. (2023) proposed a real-time orchard mapping method based on visual odometry-centered position estimation fused with a LiDAR camera and achieved real-time apple global localization using ORB-SLAM3, YOLO-V5, Euclidean clustering filtering, and the local point cloud sliding window method. Xiong et al. (2023) combined the VINS-RGBD SLAM framework with the semantic segmentation algorithm BiSeNetV1 to propose a real-time localization and semantic map reconstruction method for unstructured citrus orchards. Chen et al. (2021) presented a global map-construction method tailored to the nature of orchard-picking tasks using the ORB-SLAM3 algorithm. Chen et al. (2024) proposed a set of vision algorithms for moving target estimation, real-time self-localization, and dynamic harvesting. In addition, a reliable coordination mechanism was established for continuous motion and harvesting behavior.

In large and complex orchard environments, efficiently determining the navigation paths for mobile robots is of concern (Ye et al., 2023). The rapid-exploration random tree (RRT) algorithm is a framework for path planning algorithms widely used by mobile robots. This algorithm iteratively generates feasible paths by incorporating points obtained from random sampling of the state space, providing probabilistic completeness (LaValle et al., 2001). The efficiency of RRT algorithms for path planning has been highlighted in recent studies. Huan et al. (2023) employed the RRT*-connect algorithm to plan the navigation path of a microrobot in a plant–vein environment. Combined with the characteristics of the Dijkstra algorithm for path optimization, Wencheng et al. (2020) improved the RRT algorithm to solve the path planning problem of orchard spraying robots. Castro et al. (2023) proposed an online adaptive path planning solution fusing RRT and deep reinforcement learning algorithms. Despite these advantages, the RRT has inherent limitations due to its random sampling nature, including (1) reliance on a global uniform random sampling strategy, which results in slower convergence due to the lack of guided search (Li et al., 2020); (2) path generation from random sampling and iteration, resulting in non-optimality (Liu et al., 2023); and (3) unsuitability of RRT-generated paths for scenarios involving mobile robot tracking motion because it overlooks kinematic constraints (Webb and Van Den Berg, 2013).

Researchers have refined the basic RRT algorithm to accommodate various application environments and overcome limitations. Kuffner and LaValle (2000) proposed Bi-RRT, a bidirectional search tree that initiates two parallel trees from the start and target states to expedite algorithm convergence to improve the convergence speed of the RRT algorithm. Ye et al. (2021) enhanced the Bi-RRT by incorporating a gravitational algorithm with an adaptive step and gravitational coefficient adjustment into the AtBi-RRT algorithm, which they applied to a picking robot, achieving improved search efficiency and the ability to navigate complex obstacle regions. Tahir et al. (2018) proposed a potentially guided intelligent bidirectional RRT* algorithm that significantly improves convergence speed and memory utilization. To advance the optimization of paths generated through iterative random sampling in the RRT algorithm, Karaman and Frazzoli (2011) introduced the RRT* algorithm that incorporates key processes such as ChooseParent and Rewire and proved its asymptotic optimality. As the number of samples increased, the algorithm gradually approached the optimal solution, with the path acquisition cost converging to the optimum. However, the convergence is slow, especially in complex environments. The informed RRT* algorithm was introduced to enhance convergence efficiency (Gammell et al., 2014). This algorithm restricts the sampling to an elliptical region that narrows as the path length decreases after finding a feasible path. This strategy preserves the probabilistic completeness and asymptotic optimality of RRT*, speeds up convergence, and improves solution quality. Furthermore, Nasir et al. (2013) developed RRT*-Smart to further speed up convergence. The algorithm starts from the sampled nodes and seeks direct, unobstructed connections to ancestral nodes, reducing path costs. Similarly, Jeong et al. (2019) proposed Quick-RRT*, which extends the set of parent nodes beyond the immediate neighbors to include their ancestors. This approach also influences the rewiring process, favoring the alignment of new nodes with nearby parent nodes, improving initial solutions, and achieving faster optimal convergence compared to RRT. Li et al. (2020) combined a potentially guided method and Quick-RRT* to propose the PQ-RRT* algorithm, which achieved faster convergence and better initial path quality than the Quick-RRT*. Moreover, researchers have proposed two approaches to apply RRT algorithms to kinematically constrained navigation scenarios. The first is the consideration of kinematic constraints during the RRT path search process. Webb and Van Den Berg (2013) introduced kinodynamic-RRT*, an asymptotically optimal path planning solution for robots with linear dynamics. Moon and Chung (2014) proposed a dual-tree RRT algorithm for path planning for mobile robots with differential wheels. Wang et al. (2021) proposed the KB-RRT* algorithm, incorporating kinematic constraints to prevent unnecessary node expansion and speed up the feasible path identification for differential-wheel mobile robots. The second approach involves post-processing to mitigate increasing computational complexity during sampling. Methods such as spline fitting maintain the planning speed while producing smooth paths. Elbanhawi et al. (2015) proposed a cubic B-spline smoothing algorithm that ensured curvature continuity while satisfying a robot’s incompleteness constraint. Other smoothing methods, such as Bezier curves, have also been widely used to smooth the initial RRT paths, making them suitable for different motion planning scenarios (Liu et al., 2019; Li and Yang, 2020; Gan et al., 2021).

The conventional directional extension algorithm research pays relatively little attention to the smooth connection of paths at the conjunction between two trees; in the RRT sampling algorithm, processing the dynamics constraint during each iteration will occupy a significant amount of computational resources and increase the planning time, especially in complex environments; in path planning research combining RRT algorithm with spline fitting, relatively few studies consider the corner constraints of path points, resulting in large deviations between the actual trajectory and planned path, which can easily trigger accidental collisions in complex environments. Therefore, applying the existing path planning methods for the path planning problem of orchard mobile robots in complex environments is relatively difficult. Based on the limitations of the existing studies, our primary contributions are presented below.

(1) The path length and maximum heading angle of the path are simultaneously adopted as the path cost according to the mobile robot. Hence, the algorithm is optimized for path smoothness and improved in terms of path executability during the extension process.

(2) The path planning speed is improved by adding a directional extension method to the Quick-RRT* algorithm.

(3) To improve the local path smoothness of the bidirectional extension algorithm in conjunction with the two trees, the CreateConnectNode optimization algorithm is proposed to generate the optimal connection point directly in the conjunction region of the two trees.

The remainder of this paper is organized as follows: Section 2 discusses the specific implementation of the proposed continuous bidirectional Quick-RRT* (CBQ-RRT*) algorithm. Simulation comparisons and field experiments for B-RRT*, Quick-RRT*, and CBQ-RRT* are described in Section 3, and the results are compared and analyzed. Finally, Section 4 presents the conclusions and discusses the study.

 Methods

In developing path planning algorithms for the autonomous navigation of mobile robots in orchards, the primary considerations are the executability and efficiency of the paths planned by the algorithm and the computational efficiency of the algorithm. This study improves the Quick-RRT* algorithm and introduces a CBQ-RRT* algorithm to meet these requirements. The pseudocode for the CBQ-RRT* algorithm is presented in Algorithm 1 . Initially, an extended cost function containing the kinematic constraints of the mobile robot is established, which is applied to lines 4 and 11 in Algorithm 1 to constrain the extension direction of the nodes. The optimal parent nodes are further selected for Xnew and Xnew2 in lines 7 and 14 using this cost function. Then, a bidirectional augmentation method is introduced to accelerate path planning. Finally, an optimization strategy is proposed for connecting dual trees to avoid abrupt connections that violate the kinematic constraints (line 19). Further details of the implementation are presented in the following section.

CBQ-RRT*.

1 Va ← {Xstart }; Vb ← { Xgoal }; Ea ← ∅; Eb ← ∅; Ta ← (Va , Ea ); Tb ← (Vb , Eb ); 2 for i ← 0 to N do 3  Xrand ← Sample(); 4  Xnearest ← NearestVertex(Xrand , Ta ); 5  Xnew ← Extend(Xnearest , Xrand ); 6  σa ← Steer(Xnearest , Xnew ); 7  Xparent_Ta ← ChooseParent(Xnew , Depth); 8  if Xparent_Ta ≠ ∅ then 9   Ta ← Rewire(Ta , Xnew , Xancestries_Ta ) 10  end if 11  Xnearest_Tb ← NearestVertex (Xnew , Tb ); 12  Xnew2 ← Extend(Xnearest_Tb , XTb_Extend ); 13  σb ← Steer(Xnearest_Tb , Xnew2 ); 14  Xparent_Tb ← ChooseParent(Xnew2 , Depth); 15  if Xparent ≠ Ø then 16   Tb ←Rewire(Tb , Xnew2 , X ancestries_Tb ) 17  end if 18  if distance(Xnew2 ,Xnew ) ≤ GoalRadiu; 19   Xcreate_conn ← CreateConnectNode(Ta , Tb , Xnew ); 20  end if 21  end for
Cost function of smooth expansion

In the application of mobile robots in orchards, the efficacy of the RRT and its extended variants in path planning has been suboptimal. The heading angles of the generated path nodes have large variations in the path planning phase because of the random nature of the algorithm, which challenges the kinematic constraints of the mobile robot. This leads to significant lateral deviation in the navigation of the robot, contributing to navigation failure. This section proposes a novel path cost function based on the analysis of the kinematic constraints of a mobile robot that mitigates this problem.

Assuming a uniform terrain, the spatial position of a tracked mobile robot within an orchard environment can be defined as Xi = (xi , yi , θi ), as illustrated in Figure 1 . Here, xi and yi represent the coordinates in the global xw –yw coordinate system, while θi signifies the robot’s orientation angle. The computation of θi is done using Equation (1).

Schematic diagram of the mobile robot configuration.

 θ i = arctan ( y i − y i − 1 x i − x i − 1 )

Additionally, by neglecting any track deformation during the motion of the tracked mobile robots, we can approximate their kinematics as that of a differentially driven mobile robot. According to the kinematic model formula, the kinematic constraints can be articulated using Equation (2).

[ x i + 1 y i + 1 θ i + 1 ] = [ x i y i θ i ] + [ ∫ 0 Δ t v c cos ( θ i + ω t ) d t ∫ 0 Δ t v c sin ( θ i + ω t ) d t ∫ 0 Δ t ω d t ] = { [ x i y i θ i ] + [ − R sin θ i + R sin ( θ i + ω Δ t ) R cos θ i − R cos ( θ i + ω Δ t ) ω Δ t ] ω ≠ 0 [ x i y i θ i ] + [ v c Δ t cos θ i v c Δ t sin θ i 0 ] w = 0

where vc and ω are the linear and angular velocities of the robot, respectively; R is the turning radius of the robot, and R = v c ω .

From Equation (2), the transformation between the robot’s current configurations depends on the kinematic parameter constraints. When the planned path includes points with significant changes in yaw angles, it can result in unreachable points along the path. Consequently, significant lateral deviation errors occur during the robot’s trajectory tracking process, as shown in Figure 2 , where X 2 = (x 2 , y 2 , θ 2) is the path point of the planned path and X 2’ = (x 2’, y 2’, θ 2’) is the path point reached by the mobile robot under the kinematic constraints.

Schematic of mobile robot trajectory deviation.

Thus, significant changes in the heading angle at the waypoint result in considerable lateral error in the path-following trajectory of the mobile robot during navigation. This section introduces a novel path cost function, denoted as C(xi , xi+1 ), designed to regulate the expansion direction of the nodes, as shown in Equations (3) and (4).

C ( X i , X x + 1 ) = D ( x i , x x + 1 ) + A ( X i , X x + 1 ) { D ( X i , X i + 1 ) = ( X x _ i − X x _ i + 1 ) 2 + ( X y _ i − X y _ i + 1 ) 2 A ( X i , x i + 1 ) = { 0 , D ( X i , X i + 1 ) + D ( X i , X i ( p a r e n t ) ) < D ( X i ( p a r e n t ) , X i + 1 ) + ∞ , o t h e r s

where Xi ( parent ) denotes the parent node of Xi . The path cost function C(Xi , Xi +1) comprises a distance cost function D(Xi , xi +1) and an angle penalty function A(Xi , Xi +1). The computation of D(Xi , Xi +1) involves using the Euclidean distance formula, whereas A(Xi , Xi +1) is calculated indirectly using the cosine formula. An angle cost penalty is introduced when the heading angle between path nodes exceeds π/2.

The CBQ-RRT* algorithm uses the cost function in lines 4—6 and 11—13 of Algorithm 1 to select the initial parents of Xrand and Xnew in the two extended trees, ensuring that the paths between them and their respective parents satisfy the kinematic constraints. Thus, the CBQ-RRT* algorithm restricts the expansion direction of the nodes, ensuring compliance with the kinematic constraints of the mobile robot. Figure 3 illustrates the node expansion process. Additionally, the CBQ-RRT* algorithm incorporates the cost function into the ChooseParent and Rewire functions in lines 7–10 and 14–17 of Algorithm 1 , which further maintains the path to satisfy the kinematic constraints during the path optimization process of the CBQ-RRT* algorithm. The pseudocode for implementing the ChooseParent and Rewire functions is shown in Algorithm 2 and Algorithm 3 .

CBQ-RRT* algorithm node expansion process.

ChooseParent(Xnew , Depth).

1   Xnear ← NearVertices(Xnew , T, R); 2  for Xnear ∈ X near do 3  Xancestry ← Xnear ;4   for i = 0 to Depth do 5    if Xancestry ∉ Xancestries ; 6      Xancestries ← Xancestry ∪ Xancestries ; 7    end if 8    Xancestry ←parent(Xancestry ); 9   end for 10  end for 11  Xparent_new ←findCost_min( Xancestry , Xnew ) 12  Return Xparent_new

Rewire( Xnew , Xnear , Xancestries).

1  for all Xfrom ∈Xnew ∪ Xancestries do 2   for all Xnear ∈ Xnear 3    if Cost(Xfrom ) + Cost(Xfrom , Xnear ) ≤ Cost ( X near ) & CollisonFree then; 4     Xparent_near ← Xfrom ; 5    end if 6  end for
Dual-tree smooth connected method

Adding the Quick-RRT* algorithm to the bidirectional expansion method in Algorithm 1 improves the planning efficiency, but in the expansion direction; Ta expands in the Xrand direction and Tb expands in the Xnew direction generated by Ta . Moreover, ChooseParent and Rewire optimization during the dual-tree expansion is performed independently, making it difficult to obtain the optimal connection point by random sampling in a complex environment. For example, in Figure 4 , if the Quick-RRT* algorithm is added directly to the bidirectional expansion method, the iteration is stopped after the dual-tree connection. In addition, there is a higher probability of path points with large changes in the heading angle in the path connection, and a large redundant section of the path is added, as shown in Figure 4A . To enhance this situation, once a connection path is obtained between the two trees (line 18 in Algorithm 1 ), the CBQ-RRT* algorithm proposes the CreateConnectNode algorithm to generate a more optimal dual-tree connection point Xcreat_conn (line 19 in Algorithm 1 ). The pseudocode implementation is displayed in Algorithm 4 , and an example of the algorithm implementation is displayed in Figure 4B .

CreateConnectNode algorithm implementation procedure. (A) Originally planned path; (B) optimized planned path.

CreateConnectNode(Ta , Tb , Xnew ).

1  Xup ← Xconn 2  Xbottom ← Xparent_Ta 3  while distance(Xup , Xbottom ) > dichotomy do 4  Xtransition ← (Xup + Xbottom )/2; 5   if CollisionFree(Xtransition , Xparent_x_new ) 6    Xup ← Xtransition; 7   else 8    Xbottom ← Xtransition ; 9   end if 10  end while 11  Xbottom ← Xparent_Tb ; 12  while distance(Xup , Xbottom ) > dichotomy do 13   Xcreat_conn ← (Xup + Xbottom )/2; 14   if CollisionFree(Xcreat_conn , Xparent_x_new ) 15    Xup ← Xcreat_conn ; 16   else 17    Xbottom ← Xcreat_conn ; 18   end if 19  end while 20  return Xcreat_conn

The CreateConnectNode algorithm operates as follows:

(1) When the two expansion trees Ta and Tb are connected, the respective node sets X ancestry_Ta and X ancestry_Tb belonging to node Xconn are identified. The parent nodes Xparent_Ta and Xparent_Tb of Xconn are also located within this set.

(2) Initially, the transition point Xtransition on the line linking Xparent_Ta and Xconn is ascertained. At the iteration onset, Xparent_Ta and Xconn are characterized as the lower and upper endpoints Xbottom and Xtop , respectively. The midpoint between Xbottom and Xtop is defined as Xmid . This is followed by connecting Xparent_Tb and Xmid . In the absence of a collision, Xmid is reassigned as Xtop ; otherwise, Xmid is redefined as Xbottom . The iteration continues, and according to the dichotomy principle, the distance between Xtop and Xbottom is progressively reduced until it falls below a pre-established dichotomy parameter. Using this approximation, Xtransition on the line connecting Xparent_Ta and Xconn is obtained. Applying the same principle, Xtransition and Xparent_Tb are designated as Xup and Xbottom , respectively, with the optimal connection point Xcreate_conn discovered on the line connecting Xtransition and Xparent_Tb .

(3) Eventually, Xcreate_conn serves as the link connecting the two expansion trees Ta and Tb, with Xparent_Ta and Xparent_Tb assigned as parent nodes, respectively, thereby combining the two trees and revealing the final path, as depicted in Figure 5B . The CreateConnectNode optimization algorithm produces an improved connection point, Xcreate_conn , over the original. This resolves the smoothing issue encountered when connecting two expansion trees, thereby reducing both the trajectory’s extensive angular change and its length.

Scenarios of simulation experiment. (A) Scenario 1; (B) scenario 2; (C) scenario 3.

 Simulations and experimentations Path planning simulations Simulation setting

To evaluate the performance of the proposed algorithm, three representative orchard scenarios ( Figure 6 ) were used to construct the corresponding simulation maps ( Figure 5 ). The white and black areas indicate the feasible and obstacle areas, respectively; the green and red points indicate the starting and target points of the scenario path plan, respectively. The maps have dimensions of 1,152×648 pixels and a resolution of 0.025 m/pixel. The starting and target configurations for all three scenarios are presented in Table 1 . The simulation experiments were conducted on a computer powered by an Intel Core i5 processor with 16 GB of RAM utilizing the ROS-Melodic platform. For a comparative analysis, the proposed algorithm was pitted against the Bi-RRT* and Quick-RRT* algorithms, run with the depth parameter set to three during the simulation.

Simulated orchard scene. (A) Standardized orchard planting; (B) orchard with irregular roads; (C) orchard with irregular planting.

Parameters of the starting and target points of the simulation environment.

Scenarios Starting configuration Target configuration
1 [0 m, 0 m, 0°] [0 m,-14 m, 180°]
2 [0 m, 0 m, 0°] [3 m,27 m, 90°]
3 [0 m, 0 m, 0°] [26.5 m,-1.6 m, 0°]

In the practical application of the orchard mobile robot navigation path planning, we focused on the executability, execution efficiency of the planned paths, and computational efficiency of the algorithm. Therefore, we systematically recorded the key parameters: the length and maximum steering angle of the algorithmically planned path, and the time taken by the algorithm for path formulation in the simulation. Path length serves as a central metric for evaluating the execution efficiency of an algorithmically planned path. A shorter path length directly corresponds to reduced time to execute the path, particularly for a given linear velocity of the mobile robot. Simultaneously, the maximum steering angle acts as an indicator of path smoothness. The conclusions drawn from the previous kinematic analysis indicate that a smaller maximum steering angle in the path correlates with a smaller lateral error generated by the mobile robot during path execution. The execution time of an algorithm is a direct indicator of its computational efficiency, which is related to its real-time responsiveness to dynamic scenarios.

In this section, each algorithm was executed 100 times in the three scenarios. The evaluation aimed to assess the efficiency of path planning, the feasibility of mobile robots in orchards, and overall algorithmic efficiency.

 Analysis of simulation results

Examples of the simulation results are shown in Figures 7 – 9 . The thin red lines indicate the randomly constructed trees, whereas the thick green lines indicate the final planned paths.

Example of simulation results for scenario 1. (A) Bi-RRT*; (B) Quick-RRT*; (C) CBQ-RRT*.

Example of simulation results for scenario 2. (A) Bi-RRT*; (B) Quick-RRT*; (C) CBQ-RRT*.

Example of simulation results for scenario 3. (A) Bi-RRT*; (B) Quick-RRT*; (C) CBQ-RRT*.

The path planning results highlight a notable reduction in the number of red lines for the Bi-RRT and CBQ-RRT* algorithms compared to the Quick-RRT* algorithm. This reduction implies that the Bi-RRT* and CBQ-RRT* algorithms can formulate planned paths with an expansion of a relatively modest number of nodes, indicating superior efficiency in path planning. Additionally, an examination of the final planned path, represented by the thick green line, reveals distinctive characteristics. The path of the Bi-RRT* algorithm at the dual-tree node, as indicated by the rectangular box, displays a noticeable curvature. In contrast, the path planned by the CBQ-RRT* algorithm demonstrates remarkable smoothness, with a shorter length than that planned by the Bi-RRT* algorithm. This observation underscores the superior quality of the paths generated by the CBQ-RRT* algorithm compared to the Bi-RRT* algorithm. The simulation examples provided initially demonstrate that the CBQ-RRT* algorithm can achieve higher-quality path planning while maintaining superior algorithmic efficiency. More detailed simulation results and statistics are presented in Figure 10 and Tables 2 – 4 .

Statistics of simulation results. (A) The path length statistics results; (B) the maximum path angle statistics results; (C) the planning time statistics results.

Path length statistics of simulation results.

     ScenariosAlgorithm Scenario 1 Scenario 2 Scenario 3
CBQ-RRT* 80.84 m 39.93 m 43.65 m
Quick-RRT* 81.06 m(+0.2%) 40.33 m(+1.0%) 43.92 m(+0.6%)
Bi-RRT* 85.33 m(+5.3%) 44.71 m(+10.9%) 46.55 m(+6.0%)

Maximum path angle of simulation results.

     ScenariosAlgorithm Scenario 1 Scenario 2 Scenario 3
CBQ-RRT* 60.89° 53.93° 52.89°
Quick-RRT* 61.74°(+1.4%) 54.78°(+1.6%) 58.26°(+1.0%)
Bi-RRT* 74.33°(+41.44%) 60.46°(+20.4%) 71.61°(+22.9%)

Planning time statistics of simulation results.

     ScenariosAlgorithm Scenario 1 Scenario 2 Scenario 3
CBQ-RRT* 2.96 s 2.14 s 1.33 s
Quick-RRT* 41.97 s 8.86 s 2.24 s
Bi-RRT* 2.15 s 1.47 s 1.49 s

First, Figure 10A and Table 2 demonstrate that the CBQ-RRT* algorithm consistently outperforms the Bi-RRT* and Quick-RRT* algorithms in generating more efficient paths across the three distinct environmental scenarios. During iteration, the CBQ-RRT* and Quick-RRT* algorithms enhance the potential parent node set in the ChooseParent and Rewire phases, finding shorter paths. Additionally, the CBQ-RRT* algorithm employs CreateConnectNode optimization, significantly improving path quality and yielding shorter paths than the Quick-RRT* algorithm, as shown in Table 2 .

Second, unlike Bi-RRT* and Quick-RRT* algorithms, the CBQ-RRT* algorithm integrates a cost function that accounts for cornering constraints, significantly reducing sudden changes in path curvature. The CBQ-RRT* algorithm uses a smoothing connection technique in the bidirectional tree connection process to produce smoother paths and is further supported by the maximum path-turning angle data in Figure 10B and Table 3 . Based on the constraints imposed by the robot kinematics, the smaller the maximum turning angle, the smaller the lateral deviation error of the mobile robot when executing the path.

Finally, regarding algorithmic efficiency, Figure 10C and Table 4 show that Bi-RRT* and CBQ-RRT* algorithms have shorter path initialization times than the Quick-RRT* algorithm, particularly in Scenario 1, as characterized by the smaller feasible region and a more complex environment. This emphasizes the effectiveness of bidirectional RRT algorithms, including CBQ-RRT*, in meeting the efficiency needs of path planning for mobile robots in orchard settings.

The results confirm that the CBQ-RRT* algorithm is superior to Bi-RRT* and Quick-RRT* algorithms in terms of overall performance and demonstrates its ability to efficiently plan shorter collision-free paths that align better with the kinematic constraints of mobile robots in a shorter time.

 Planned path tracking experiment Experiment setting

The algorithmic performance of CBQ-RRT* was further evaluated in an orchard context using real-world experiments. The physical orchard mobile robot used is shown in Figure 11 , and its motion parameters are listed in Table 5 . The experimental environment consisted of an orchard with complex obstacles, as shown in Figure 12A . The mobile robot, equipped for picking, incorporated a three-dimensional LiDAR “Robosensen_16” for environmental sensing and a nine-axis IMU module to monitor its movement. From the resulting sensor data, an offline map is constructed using the cartographer algorithm proposed by Hess et al. (2016) to facilitate path planning. This map, shown in Figure 12B , consists of 712×659 pixels at a resolution of 0.05 m/pixel.

Self-developed mobile robot.

The motion parameters of the mobile robot.

Parameter Value
Minimum turning radius Rmin 0.6 m
Maximum curvature kmax 1.6 m-1
Maximum angular velocity ωmax 20°/s
Maximum speed vc_max 2 m/s

Experimental environment. (A) Experimental field scenario; (B) grid map of the experimental scenario.

This map was utilized to plan paths using the Bi-RRT*, Quick-RRT*, and CBQ-RRT* algorithms, each originating at [6 m, 2 m, 0°] and terminating at [22.73 m, 1 m, 90°], over five trials per algorithm. The experiments assessed the executability of each path planning algorithm by comparing the lateral deviation error between the planned and actual robot trajectory. The results of these on-field experiments are represented in Figures 13 , 14 ; Supplementary Figure 1 statistics regarding the maximum path deviation are summarized in Table 6 .

Planned paths result. (A) Bi-RRT*; (B) Quick-RRT*; (C) CBQ-RRT*.

The lateral error records. (A) Bi-RRT*; (B) Quick-RRT*; (C) CBQ-RRT*.

Maximum lateral error statistics of the path.

Sequence Bi-RRT* Quik-RRT* CBQ-RRT*
1 0.77 m 0.60 m 0.32 m
2 0.52 m 0.40 m 0.36 m
3 0.92 m 0.37 m 0.42 m
4 1.05 m 0.46 m 0.31 m
5 0.72 m 0.45 m 0.26 m
Analysis of experiment results

Figure 13 shows the results of the paths planned by the Bi-RRT*, Quick-RRT*, and CBQ-RRT* algorithms in a real environment map. These results are consistent with those obtained in the simulation experiments, with the CBQ-RRT* algorithm demonstrating the ability to generate planned paths of relatively high quality using fewer extended nodes.

The real-time trajectory of the robot was monitored during path execution using the localization module of the Cartographer algorithm. Supplementary Figure 1 shows the paths planned by the three algorithms, represented by red line segments, along with the actual trajectories of the mobile robot during navigation, represented by blue curves. The planned and actual trajectories are length normalized to quantify the executability of the path during the navigation process. We select n observation points from them in equal proportions and calculate the lateral error of the mobile robot during the path-following process as Equation (5).

L a t e r a l _ e r r ( i ) = [ x ( i ) p − x ( i ) r ] 2 + [ y ( i ) p − y ( i ) r ] 2

where x ( i ) p , y ( i ) p and x ( i ) r , y ( i ) r are the coordinates of the ith observation point in the planned path and actual trajectory, respectively. Figure 14 shows the lateral error between the actual and planned trajectories when the mobile robot executes the paths planned by these algorithms. Supplementary Figure 2 illustrates the movement of a mobile robot when executing a planned path. The mobile robot exhibits comparatively smaller lateral errors in Figure 14 ; Supplementary Figures 1, 2 when executing the paths planned by the CBQ-RRT* algorithm, suggesting superior executability of the paths planned by the CBQ-RRT* algorithm.

Table 6 presents the maximum path lateral error statistics obtained from these experiments. CBQ-RRT* recorded an average maximum path lateral error of 0.334 m across five trials. In contrast, Quick-RRT* and Bi-RRT* recorded lateral errors of 0.455 and 0.796 m, respectively. The field experiments validated the superior performance of the CBQ-RRT* algorithm in challenging orchards.

 Conclusions

When operating autonomously, orchard mobile robots, such as those used for weeding, spraying, and picking, must easily traverse through the orchard. Existing research on the navigation scheme of orchard mobile robots focuses on path extraction and fixed path following in a structured orchard. However, this type of research cannot be applied to unstructured orchard scenes where the road characteristics are fuzzy, and the robot needs to autonomously plan the navigation path according to environmental perception. Therefore, an efficient path planning algorithm is key to achieving improved automation for mobile orchard robots in unstructured orchard environments.

In this study, based on the Quick-RRT* algorithm, a bidirectional expansion method was introduced, and a computationally lightweight path cost function was proposed to maintain the smoothness of the paths between nodes and the efficiency of node expansion during random tree expansion. Moreover, the algorithm performs smoothness optimization during dual-tree link processing, which locally improves the kinematics of the paths at the dual-tree link. We prove the effectiveness of the CBQ-RRT* algorithm for the path planning problem of working mobile robots in an orchard scene by setting three types of orchard simulation maps: a standardized planting scene, an irregular road scene, and an irregular planting scene. The statistical simulation results of the experiment show that, compared with the Bi-RRT* algorithm, the CBQ-RRT* algorithm improves the length and smoothness of the planned paths under the premise of using a similar path planning time, and the planning efficiency is significantly improved compared with the Quick-RRT* algorithm. Path-following experiments were conducted in a real field environment to prove further the effectiveness of the CBQ-RRT* algorithm in practical work-robot applications. The experimental results show that the mobile robot can follow the path planned by the CBQ-RRT* algorithm more accurately. The experimental results show that the CBQ-RRT* algorithm can effectively plan a safe and feasible point-to-point navigation path for a mobile robot in a complex orchard environment and the mobile robot can follow the path planned by the CBQ-RRT* algorithm more accurately. Autonomous point-to-point navigation is integral in orchard robotics, particularly spraying, weeding, and inspection robots. These robots also share structural similarities with the tracked chassis used in our experiment, leading us to believe that CBQ-RRT* can be directly applied to enhance operational efficiency.

 Data availability statement

The original contributions presented in the study are included in the article/ Supplementary Material . Further inquiries can be directed to the corresponding author.

 Author contributions

LY: Conceptualization, Methodology, Software, Writing – original draft. JL: Resources, Writing – review & editing. PL: Data curation, Writing – review & editing.

 Funding

The author(s) declare financial support was received for the research, authorship, and/or publication of this article. This research was supported by the the National Natural Science Foundation of China (52105268), Shaoguan Science and Technology Project (210725144530830 and 230403118030785), and the Key Research Project of Shaoguan University (SZ2023KJ14 and SZ2022KJ13).

 Conflict of interest

The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.

 Publisher’s note

All claims expressed in this article are solely those of the authors and do not necessarily represent those of their affiliated organizations, or those of the publisher, the editors and the reviewers. Any product that may be evaluated in this article, or claim that may be made by its manufacturer, is not guaranteed or endorsed by the publisher.

 Supplementary material

The Supplementary Material for this article can be found online at: https://www.frontiersin.org/articles/10.3389/fpls.2024.1337638/full#supplementary-material

References Blok P. M. van Boheemen K. van Evert F. K. Ijsselmuiden J. Kim G.-H. (2019). Robot navigation in orchards with localization based on Particle filter and Kalman filter. Comput. Electron. Agric. 157, 261–269. doi: 10.1016/j.compag.2018.12.046 Buchanan R. Wellhausen L. Bjelonic M. Bandyopadhyay T. Kottege N. Hutter M. (2021). Perceptive whole-body planning for multilegged robots in confined spaces. J. Field Robot. 38, 68–84. doi: 10.1002/rob.21974 Castro G. G. D. Berger G. S. Cantieri A. Teixeira M. Lima J. Pereira A. I. . (2023). Adaptive path planning for fusing rapidly exploring random trees and deep reinforcement learning in an agriculture dynamic environment UAVs. Agriculture 13, 354. doi: 10.3390/agriculture13020354 Chen M. Chen Z. Luo L. Tang Y. Cheng J. Wei H. . (2024). Dynamic visual servo control methods for continuous operation of a fruit harvesting robot working throughout an orchard. Comput. Electron. Agric. 219, 108774. doi: 10.1016/j.compag.2024.108774 Chen M. Tang Y. Zou X. Huang Z. Zhou H. Chen S. (2021). 3D global mapping of large-scale unstructured orchard integrating eye-in-hand stereo vision and SLAM. Comput. Electron. Agric. 187, 106237. doi: 10.1016/j.compag.2021.106237 Elbanhawi M. Simic M. Jazar R. N. (2015). Continuous path smoothing for car-like robots using B-spline curves. J. Intell. Robot. Syst. 80, 23–56. doi: 10.1007/s10846-014-0172-0 Ferguson D. Stentz A. (2006). Using interpolation to improve path planning: The Field D* algorithm. J. Field Robot. 23, 79–101. doi: 10.1002/rob.20109 Gammell J. D. Srinivasa S. S. Barfoot T. D. (2014). Informed RRT: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic. 2014 IEEE/RSJ Int. Conf. Intell. Robots Syst. 2997–3004. doi: 10.1109/IROS.2014.6942976 Gan Y. Zhang B. Ke C. Zhu X. He W. Ihara T. (2021). Research on robot motion planning based on RRT algorithm with nonholonomic constraints. Neural Process. Lett. 53, 3011–3029. doi: 10.1007/s11063-021-10536-4 Hess W. Kohler D. Rapp H. Andor D. (2016). “Real-time loop closure in 2D LIDAR SLAM,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, Sweden, pp. 1271–1278. doi: 10.1109/ICRA.2016.7487258 Huan Z. Wang J. Zhu L. Zhong Z. Ma W. Chen Z. (2023). Navigation and closed-loop control of magnetic microrobot in plant vein mimic environment. Front. Plant Sci. 14. doi: 10.3389/fpls.2023.1133944 Jeong I.-B. Lee S.-J. Kim J.-H. (2019). Quick-RRT*: Triangular inequality-based implementation of RRT* with improved initial solution and convergence rate. Expert Syst. Appl. 123, 82–90. doi: 10.1016/j.eswa.2019.01.032 Jiang S. Wang S. Yi Z. Zhang M. Lv X. (2022). Autonomous navigation system of greenhouse mobile robot based on 3D Lidar and 2D Lidar SLAM. Front. Plant Sci. 13. doi: 10.3389/fpls.2022.815218 Karaman S. Frazzoli E. (2011). Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res. 30, 846–894. doi: 10.1177/0278364911406761 Khajepour A. Sheikhmohammady M. Nikbakhsh E. (2020). Field path planning using capacitated arc routing problem. Comput. Electron. Agric. 173, 105401. doi: 10.1016/j.compag.2020.105401 Kuffner J. J. LaValle S. M. (2000). “RRT-connect: An efficient approach to single-query path planning,” in Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), San Francisco, CA, USA. 2, 995–1001. doi: 10.1109/ROBOT.2000.844730 LaValle S. M. Kuffner J. J. Donald B. R. (2001). Rapidly-exploring random trees: Progress and prospects. Algorithmic Comput. robotics: New Dir. 5, 293–308. Li J. Yang C. (2020). “AUV path planning based on improved RRT and Bezier curve optimization,” in 2020 IEEE International Conference on Mechatronics and Automation (ICMA), Beijing, China, pp. 1359–1364. doi: 10.1109/ICMA49215.2020.9233842 Li Y. Wei W. Gao Y. Wang D. Fan Z. (2020). PQ-RRT*: An improved path planning algorithm for mobile robots. Expert Syst. Appl. 152, 113425. doi: 10.1016/j.eswa.2020.113425 Liu H. Zhang X. Wen J. Wang R. Chen X. (2019). Goal-biased bidirectional RRT based on curve-smoothing. IFAC-PapersOnLine 52, 255–260. doi: 10.1016/j.ifacol.2019.12.417 Liu T. Kang H. Chen C. (2023). ORB-Livox: A real-time dynamic system for fruit detection and localization. Comput. Electron. Agric. 209, 107834. doi: 10.1016/j.compag.2023.107834 Ljungqvist O. Evestedt N. Axehill D. Cirillo M. Pettersson. H. (2019). A path planning and path-following control framework for a general 2-trailer with a car-like tractor. J. Field Robot. 36, 1345–1377. doi: 10.1002/rob.21908 Moon C.-B. Chung W. (2014). Kinodynamic planner dual-tree RRT (DT-RRT) for two-wheeled mobile robots using the rapidly exploring random tree. IEEE Trans. Ind. Electron. 62, 1080–1090. doi: 10.1109/TIE.2014.2345351 Nasir J. Islam F. Ayaz Y. (2013). Adaptive rapidly-exploring-random-tree-star (RRT*)-smart: algorithm characteristics and behavior analysis in complex environments. Asia Pac. J. Inf. Tech. Multimed. 2, 39–51. doi: 10.17576/apjitm Qiu Q. Li X. (2022). “LiDAR point-cloud odometer based mobile robot routine tracking in orchards,” in 2022 12th International Conference on CYBER Technology in Automation, Control, and Intelligent Systems (CYBER), Baishan, China, pp. 735–739. doi: 10.1109/CYBER55403.2022.9907082 Reid W. Fitch R. Göktoğan A. H. Sukkarieh S. (2020). Sampling-based hierarchical motion planning for a reconfigurable wheel-on-leg planetary analogue exploration rover. J. Field Robot 37, 786–811. doi: 10.1002/rob.21894 Ren G. Lin T. Ying Y. Chowdhary G. Ting K. C. (2020). Agricultural robotics research applicable to poultry production: A review. Comput. Electron. Agric. 169, 105216. doi: 10.1016/j.compag.2020.105216 Siciliano B. Khatib O. Kröger T. (2008). Springer handbook of robotics Vol. 200 (Berlin: Springer). Strader J. Otsu K. Agha-mohammadi A. A. (2020). Perception-aware autonomous mast motion planning for planetary exploration rovers. J. Field Robot 37, 812–829. doi: 10.1002/rob.21925 Tahir Z. Qureshi A. H. Ayaz Y. Nawaz R. (2018). Potentially guided bidirectionalized RRT* for fast optimal path planning in cluttered environments. Robot. Auton. Syst. 108, 13–27. doi: 10.1016/j.robot.2018.06.013 Tian H. Mo Z. Ma C. Xiao J. Jia R. Lan Y. . (2023). Design and validation of a multi-objective waypoint planning algorithm for UAV spraying in orchards based on improved ant colony algorithm. Front. Plant Sci. 14. doi: 10.3389/fpls.2023.1101828 Wang J. Li B. Meng M. Q.-H. (2021). Kinematic Constrained Bi-directional RRT with Efficient Branch Pruning for robot path planning. Expert Syst. Appl. 170, 114541. doi: 10.1016/j.eswa.2020.114541 Webb D. J. Van Den Berg J. (2013). “Kinodynamic RRT*: Asymptotically optimal motion planning for robots with linear dynamics,” in 2013 IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, pp. 5054–5061. doi: 10.1109/ICRA.2013.6631299 Wencheng W. Gege Z. Xinlin C. Weixian W. (2020). Research on path planning of orchard spraying robot based on improved RRT algorithm. Proc. 2020 2nd Int. Confe. Big Data Artif. Intell. 311–316. doi: 10.1145/3436286.3436412 Xiong Y. Ge Y. Liang Y. Blackmore S. (2017). Development of a prototype robot and fast path-planning algorithm for static laser weeding. Comput. Electron. Agric. 142, 494–503. doi: 10.1016/j.compag.2017.11.023 Xiong J. Liang J. Zhuang Y. Hong D. Zheng Z. Liao S. . (2023). Real-time localization and 3D semantic map reconstruction for unstructured citrus orchards. Comput. Electron. Agric. 213, 108217. doi: 10.1016/j.compag.2023.108217 Yang Z. Ouyang L. Zhang Z. Duan J. Yu J. Wang H. (2022). Visual navigation path extraction of orchard hard pavement based on scanning method and neural network. Comput. Electron. Agric. 197, 106964. doi: 10.1016/j.compag.2022.106964 Ye L. Duan J. Yang Z. Zou X. Chen M. Zhang S. (2021). Collision-free motion planning for the litchi-picking robot. Comput. Electron. Agric. 185, 106151. doi: 10.1016/j.compag.2021.106151 Ye L. Wu F. Zou X. Li J. (2023). Path planning for mobile robots in unstructured orchard environments: An improved kinematically constrained bi-directional RRT approach. Comput. Electron. Agric. 215, 108453. doi: 10.1016/j.compag.2023.108453 Zhang W. Zeng Y. Wang S. Wang T. Li H. Fei K. . (2023). Research on the local path planning of an orchard mowing robot based on an elliptic repulsion scope boundary constraint potential field method. Front. Plant Sci. 14. doi: 10.3389/fpls.2023.1184352 Zhang X. Guo Y. Yang J. Li D. Wang Y. Zhao R. (2022). Many-objective evolutionary algorithm based agricultural mobile robot route planning. Comput. Electron. Agric. 200, 107274. doi: 10.1016/j.compag.2022.107274 Zhao Y. M. Lin Y. Xi F. Guo. S. (2014). Calibration-based iterative learning control for path tracking of industrial robots. IEEE Trans. Ind. Electron. 62, 2921–2929. doi: 10.1109/TIE.2014.2364800