**1. Introduction**

Citrus is one of the most economically important crops in the world, and it is also the most cultivated fruit in southwestern China. Currently, citrus fruits are mainly picked manually, which is time-consuming, laborious, and expensive. According to a survey, the labor used in citrus picking operations accounts for 33 ∼ 50% of the whole production process. With the sharp decline in the number of rural employees in China, the development of the citrus industry has been severely restricted. To improve the efficiency of picking and enhance the competitiveness of China's citrus industry, both the research and development of citrus-picking robots have become research hotspots at home and abroad, and the path planning of the picking manipulator is one of the most difficult technologies.

In recent years, a series of path-planning methods have been proposed. The artificial potential field (APF) can be used to prevent the manipulator from colliding with obstacles when approaching the target. However, the APF easily falls into a local minimum, and it easily falls into oscillation in a complex environment [1]. Compared to the APF, the rapidly-exploring random tree (RRT) is more adaptable, faster, and more variable, but it is difficult to find the best path when using this approach [2]. Bidirectional RRT and RRT-connect algorithms are used to generate two random trees at the initial node and the target node, respectively, which improves the search speed compared with the RRT

**Citation:** Liu, C.; Feng, Q.; Tang, Z.; Wang, X.; Geng, J.; Xu, L. Motion Planning of the Citrus-Picking Manipulator Based on the TO-RRT Algorithm. *Agriculture* **2022**, *12*, 581. https://doi.org/10.3390/ agriculture12050581

Academic Editor: Wen-Hao Su

Received: 16 March 2022 Accepted: 19 April 2022 Published: 21 April 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

algorithm, but the path is still not optimal [3,4]. The RRT-star (RRT\*) algorithm is used to make the path gradually converge with the optimum in the search process by reselecting the parent node and rerouting, but its running time is longer than that of the RRT algorithm [5]. Mohammed et al. [6] defined a straight line connecting the initial node and the target node so that the generation probability of the random tree node was normally distributed with the distance from the straight line, preventing excessive searching and avoiding falling into local extreme values. However, the searching ability in a complex environment still needs to be improved. Akgun et al. [7] combined the bidirectional RRT and RRT\* algorithms to optimize the search time. Jeong et al. [8] proposed an RRT\*-Quick method, which caused the nodes to tend to share the same parent node in a circular (or spherical) neighborhood. While the path generated using this method was smoother than the path generated by the RRT\* algorithm, the search time increased slightly. Jeong et al. [9] introduced an informed-RRT algorithm into the RRT\*-quick method to limit the sampling space of the random tree and solve the problem of increased search time caused by expanding the search domain in the process of improving the quality of the solution. When the tree nodes reached the maximum, the RRT\* Fixed Nodes (RRT\*FN) algorithm was used to remove a weak node and add a high-performance node so that the generated tree node was much smaller than the one in the RRT\* algorithm. However, this method had little performance gap with the RRT\* algorithm before the tree nodes reached the maximum number of nodes [10].

The RRT\* algorithm has a strong ability to optimize the path cost, but its search efficiency is low. Cao et al. [11] introduced the target gravity to the RRT algorithm, and the attraction generated by the random node and the attraction generated by the target node were used to jointly guide the generation of new nodes in the random tree. This method improved the search speed of the random tree, but it could not escape the obstacle area quickly when blocked by obstacles. Wang et al. [12] changed the sampling area and assigned node state values so that the random tree could only be expanded through boundary nodes to reduce the generation of invalid nodes, but many redundant nodes were generated near obstacles. Zhang et al. [13] screened new nodes based on a biased-RRT algorithm. If the distance between the new node and the parent node was greater than the distance between the new node and any other nearby node, the new node was discarded. This method can be used to prevent excessive searching of the space and reduce the total number of nodes. Gong et al. [14] made the search direction of the random tree always point to the target node and performed local path planning near the obstacles. Although this method could reduce excessive searching of the space, its escape speed was slow when the random tree was blocked by many large obstacles. Li et al. [15] put forward an adaptive RRT-connect (ARRT-connect), which allowed the random tree to still have good performance in a narrow environment, and path planning could still be completed in a short time. Gao et al. [16] proposed a planning method based on an independent potential field that made the manipulator explore the gradient direction when it was far away from the target and avoided obstacles through the random search. Wang et al. [17] selected tree nodes according to the geometric structure and position of obstacles so that a path with a lower cost could be quickly obtained, but the effect of avoiding obstacles with irregular shapes was poor.

In this paper, based on a citrus tree environment, taking the shortest time as the optimization goal, and taking the Franka manipulator as the experimental platform, the RRT algorithm is improved in multiple dimensions. Its main contributions are as follows:


The rest of this article is organized as follows: The basic principles of the RRT algorithm, as well as some improvement methods and the design process of the TO-RRT algorithm, including the adaptive probability threshold, the node-first search strategy, an attractive step size, "step-size dichotomy", and a regression superposition algorithm are introduced in Section 2. In Section 3, the performance of various algorithms in MATLAB, MoveIt!, and the real environment are compared. The main contributions of the article and future work are discussed in Section 4. The full text is summarized in Section 5.

#### **2. Materials and Methods**

#### *2.1. RRT Algorithm*

The RRT algorithm, which is a spatial search algorithm based on random sampling, aims to generate a collision-free random tree connecting the first and the last positions [18,19].

Each time the *Tree* grows, a random node *qrand* is generated in the space. Then, the tree node *qnear* closest to *qrand* is found in the tree, and a new tree node *qnew* is found in the direction of *qnear* → *qrand* with a fixed step *λ* and is connected to *qnear* as *qnearqnew*. If neither *qnew* nor *qnearqnew* collide with obstacles, *qnew* and *qnearqnew* are added to the random tree. After several expansions, if the distance between *qnew* and *qgoal* is less than the given threshold, the *Tree* finds a path connecting *qinit* to *qgoal*, as shown in Algorithm 1.

#### **Algorithm 1.** RRT Algorithm.

1: *Tree* ← *qinit* 2: for *i* = 1 to *n* do 3: *qrand* ← *RandomSample*(*Cf ree*); 4: *qnear* ← *NearestPoint*(*Tree*, *qrand*); 5: *qnew* ← *Extend*(*qnear*, *qrand*, *λ*); 6: if *CollisionFree*(*qnear*, *qnew*) then 7 : *AddNewPo*int(*Tree*, *qnew*); 8: end if 9 : if *Distance*(*qnew*, *qgoal*) < *ρmin* then 10: return *Tree* 11: end if 12: end for

#### *2.2. Some Improvement Methods*

The RRT algorithm can be used to effectively explore high-dimensional space, but the path cost is high, and the algorithm takes a long time to reach completion. The biased-RRT algorithm can be used to effectively solve the shortcomings of the RRT algorithm [20–23], as shown in Algorithm 2.

**Algorithm 2.** Biased-RRT Algorithm.

1: *Tree* ← *qinit* 2: for *i* = 1 to *n* do 3: if *RandomNumber* <= *m* then 4: *qrand* ← *RandomSample*(*Cf ree*); 5: else 6: *qrand* ← *qgoal*; 7: end condition 8: *qnear* ← *NearestPoint*(*Tree*, *qrand*); 9: *qnew* ← *Extend*(*qnear*, *qrand*, *λ*); 10: if *CollisionFree*(*qnear*, *qnew*) then 11: *AddNewPo*int(*Tree*, *qnew*); 12: end if 13: if *Distance*(*qnew*, *qgoal*) < *ρmin* then 14: return *Tree* 15: end if 16: end for

In Algorithm 2, *qrand* is determined by the size between the random number, *RandomNumber*, and the probability threshold *m*. If *RandomNumber* <= *m*, *qrand* takes any point in the space; otherwise, the target node is taken as the sampling point.

The biased-RRT algorithm is used to guide the growth of the random tree, increase the effectiveness of sampling points, and shorten the time of path planning. However, when obstacles obstruct the growth of random trees, the biased-RRT algorithm cannot escape the obstacles quickly. Therefore, some scholars have put forward corresponding solutions, as shown in Table 1.

**Table 1.** Comparison of RRT improvement methods.


Note: NC-RRT, Node Control-RRT; RRT-BCR, Biased-RRT algorithm with boundary expansion mechanism and regression mechanism; MOD-RRT\*, multi-objective RRT\*; PBG-RRT, rapidly exploring random tree based on heuristic probability bias-goal; HSRRT\*, heuristically sampling-based rapidly exploring random tree.

#### *2.3. TO-RRT Algorithm*

#### 2.3.1. Adaptive Probability Threshold

At present, some improved RRT algorithms have been used to add potential field functions to the target node *qgoal*, random nodes *qrand*, and obstacles. The random tree changes its growth direction under the action of a combined potential field, which makes it expand to the target when avoiding obstacles [27–33]. This kind of algorithm improves the search efficiency of the random tree, but each expansion of the random tree requires several vector operations of the potential field force, which occupies a large amount of the system memory. In addition, if the repulsive potential field of the obstacles is considered, the algorithm may fall into a local minimum problem, as is common in APF, resulting in *qnear* being unable to generate a new node *qnew*, as shown in Figure 1.

**Figure 1.** The algorithm falls into a local minimum.

Considering the complexity and uncertainty of the above algorithm in different environments, an attractive potential field and a repulsive potential field for the target node and obstacle, respectively, based on the biased-RRT algorithm were established. Therefore, the probability threshold changed according to the different types of potential fields.

Similar to the APF algorithm, the range of the attractive potential field was the whole operating space, while the range of the repulsive potential field was limited to a certain distance around the obstacle. In the range of the repulsive potential field, if the random tree tended to search for the target node *qgoal*, the random tree had a strong ability to grow biased. At this time, if the obstacle blocked *qgoal*, multiple failed growth near obstacles could occur for the random tree, so it tended to search randomly within the repulsive potential field. When the random tree left the range of the repulsive potential field, it continued to tend to search for *qgoal*, as shown in Algorithm 3.


The growths of the random tree under both the control of the constant probability threshold and the adaptive probability threshold are shown in Figure 2a,b, respectively. Figure 2a shows that, if the random tree maintained a constant probability threshold during the search process, the obstacle did not affect the goal of random tree expansion. If the adaptive probability threshold was adopted, the random tree chose a better growth direction according to the location tendency of the new node. It was learned through many experiments that the probability threshold outside the scope of the repulsive potential field was 0.3, and the probability threshold inside the range of the repulsive field was 0.7.

#### 2.3.2. Node-First Search Strategy

According to the biased-RRT algorithm, when *RandomNumber* > *m*, *qrand* takes the coordinate value of *qgoal* and then selects the *qnear* closest to *qrand* in the random tree as the parent node of *qnew*. If the random tree only expands to the target in each search round without considering the random search, then the new node in this search round will become the parent node of the new node in the next search round, and the random tree is a straight line segment connecting *qinit* and *qgoal*.

**Figure 2.** Threshold comparison. (**a**) Constant probability threshold; (**b**) Adaptive probability threshold.

As in Section 2.3.1, when the end node of the random tree expands to the range of the repulsive potential field, the random tree tends to select any node in the space as *qrand* in the next search selection, so the probability of random expansion of the end node is small. If the next round of search satisfies *RandomNumber* > *threshold*, since the end node of the random tree is closest to *qgoal*, *qgoal* will be expanded, causing the newly generated path to collide with the obstacle. To summarize, when *RandomNumber* > *threshold*, the end node collides with the obstacle; when *RandomNumber* <= *threshold*, any node in the tree will be selected for expansion, which is no different from the traditional RRT algorithm. This phenomenon is called "falling into a trap", as shown in Figure 3.

**Figure 3.** The random tree falls into a trap in the repulsive potential field. (**a**) The random tree entering obstacle potential field; (**b**) The random tree begins to expand randomly; (**c**) The random trees collide with obstacles; (**d**) The random tree is expanded several times.

For this reason, a node-first search strategy was proposed in this paper, as shown in Algorithm 4. When the *qnew* of the random tree grew into the range of the repulsive potential field of obstacles, a virtual spherical surface with a radius *r* and center *qnew* was generated. If *RandomNumber* <= *threshold* was satisfied in the next round of search, point *qrand* on the virtual spherical surface was preferentially selected, and *qnew* was used as the parent node of the next round of search to generate a new node *qnew*2. If *qnew*<sup>2</sup> and the line segment *qnewqnew*<sup>2</sup> did not collide with obstacles, the path and *qnew*<sup>2</sup> were kept. A new search round continued until the end node of the random tree was separated from the obstacles, as shown in Figure 4.

**Algorithm 4.** Node-First Search Algorithm.

1: if *RandomNumber* <= *threshold*(*qnew*, *obstacle*) then 2: if *Distance*(*qnew*, *obstacle*) < *Rrpf* then 3: *qrand* ← *sphere*(*qnew*,*rvirtual*); 4: else 5: *qrand* ← *RandomSample*(*Cf ree*); 6: end if 7: else 8: *qrand* ← *qgoal*; 9: end if 10: return *qrand*

**Figure 4.** Schematic diagram of the node-first search strategy.

2.3.3. Attractive Step Size and Step-Size Dichotomy

From the above description, the node-first search strategy was used to prevent the random tree "falling into a trap" within the range of the repulsive potential field. Since the random tree has a certain probability of random search outside the range of the repulsive potential field of obstacles, more iterations will be generated. An appropriate step size can effectively reduce the iterations of the random tree. In the case that the length of the path is determined, a small step size will cause more collision detections and distance calculations, and a large step size will often make the random tree collide with obstacles. Therefore, the step size should be expanded as much as possible on the premise of reducing the number of collisions [34,35].

According to the APF algorithm, the attractive force of *qgoal* acts on the whole operating space and is proportional to the distance between the end joints of the manipulator, which is beneficial to control the growth step of the RRT. If obstacles are not considered, the random tree should increase the step size when it is far away from *qgoal* to quickly expand to *qgoal*. When the random tree is closer to *qgoal*, if it continues to maintain a large step size, a large number of redundant nodes will be generated at *qgoal*, as shown in Figure 5a. Therefore, the random tree should gradually approach *qgoal* with small step sizes, as shown in Figure 5b.

For this reason, an attractive step size was proposed, which was defined as:

$$\text{AttSttepsize} = k \times Distance(q\_{near}, q\_{goal}) \tag{1}$$

where *attStepsize* represents the attractive step size, and *k* is the attractive parameter.

**Figure 5.** Fixed step size and attractive step size. (**a**) Fixed step size; (**b**) Attractive step size.

If obstacles are considered, the step size of the random tree in the random search is fixed, and an attractive step size is taken when growing toward *qgoal*. This method ensures that the random tree grows toward *qgoal* as soon as possible outside the range of the repulsive potential field and avoids collisions with obstacles due to excessive steps within the range of the repulsive potential field.

The parameters of the potential field function of the manipulator are different in different operating spaces. For example, when the attractive parameter *k* is too large, *attStepsize* will increase accordingly. If *Distance*(*qnear*, *obstacle*) < *attStepsize*, *qnew* will collide with obstacles. In addition, the end nodes tend to grow toward *qgoal* outside the range of the repulsive potential field. Therefore, the random tree still has a high probability of colliding with obstacles in the next round of search.

For this reason, a "step-size dichotomy" was introduced to solve the problem of excessive step size. When *qnear* grew toward *qgoal* and there were obstacles between them, the distance *dnob* between *qnear* and the obstacles was calculated. If *dnob* <= *attStepsize*, the *attStepsize* was shortened to the original value of 2−1, and the sizes of *attStepsize* and *dnob* were compared again until *dnob* > *attStepsize*; see Algorithm 5.


2.3.4. Regression Superposition Algorithm

From Section 2.3.3, if the random tree grows within the range of the obstacle repulsive potential field, a large number of redundant nodes will be generated on the surface of the obstacle due to the high probability of the random search, as shown in Figure 6a. As a result, a regression superposition algorithm is proposed in this section to adaptively select extended nodes and change the step size of the random search, as shown in Figure 6b.

**Figure 6.** Differences before and after improvement. (**a**) The random trees generated a large number of nodes on the surface of obstacles; (**b**) The random tree had fewer nodes on the obstacle surface.

The regression superposition algorithm consists of a regression algorithm [36] and a step-size superposition algorithm. In the regression algorithm, if the distance between *qnew* and *qnear* was larger than the distance between *qnew* and any node *qi* in the random tree except *qnear*, it was considered to meet the regression conditions:

$$\begin{cases} \begin{array}{l}Distance(q\_{near}, q\_{new}) > Distance(q\_{near}, q\_i) \\ \end{array} \end{cases} \begin{array}{l} Distance(q\_{near}, q\_i) \\ Time(q\_i) \end{array} \tag{2}$$

If Formula (2) was satisfied, *qnew* was regarded as a regression node. The regression node would not become the tree node of the random tree, but it was removed until a new node that did not meet the regression condition was found, as shown in Figure 7a.

**Figure 7.** Regression superposition algorithm. (**a**) Regression algorithm; (**b**) Regression superposition algorithm; (**c**) Step-size superposition algorithm. Note: *λ*, the initial step size; *ε*, the step size of superposition; *ρ*, the distance between *qnew* and any node.

To further reduce the number of tree nodes, the step-size superposition algorithm was incorporated based on a regression algorithm. When the random tree was searched randomly, the initial step size was set to *λ*, and the step size was increased by *ε* after each round of the random search until the extended branches of the random tree collided with obstacles. Then, the search step size of the next round was returned to the initial step size *λ*, and the step size was superimposed again until the random tree searched toward *qgoal*, as shown in Figure 7c.

The random tree used an attractive step size when searching toward *qgoal* to reduce the generation of redundant nodes. During the random search of the random tree, the regression superposition algorithm was used to enhance the ability of the random tree to search the unknown space, as shown in Figure 7b.

The TO-RRT algorithm was used to dynamically adjust the growth direction of the random tree by the probability threshold controlled by the potential field and to define two different growth methods according to the different growth directions. Therefore, the random tree could quickly grow to the target outside the range of the repulsive potential field and quickly determine the escape path within the range of the repulsive potential field. The algorithm flow chart is shown in Figure 8.

**Figure 8.** Flowchart of the TO-RRT algorithm. Note: A, yes; N, no.
