**3. Results**

#### *3.1. Comparative Experiment of Path Planning in a Complex Environment*

To verify the speed, stability, and low path cost of the TO-RRT algorithm, the RRT algorithm, the biased-RRT algorithm with a target offset probability of 50%, the TO-RRT algorithm, the RRT-BCR algorithm, and the NC-RRT algorithm are compared in this section using complex environments (i.e., a multi-sphere environment, a multi-rectangle environment, a single-channel environment, and a multi-channel environment).

In the simulation experiment, the initial step size was 2, the maximum number of failed growth times was 100,000, the map size was 50 × 50 × 50, the starting point was (1, 1, 1), and the target point was (49, 49, 49). The blank area in the map represented the obstacle-free area, other colors represented the obstacle area, the blue path represented the random tree, the black path represented the collision-free path from the starting point to the target point, and the red path represented the path optimized by the greedy algorithm.

Figure 9a,e,i,m,q show that, although the RRT algorithm can be used to find a collisionfree path from the initial point to the target point, the whole space was searched, so that the highest amount path nodes were generated. Compared with the RRT algorithm, the biased-RRT algorithm did not search too much invalid space, so there were fewer path nodes. When using the RRT-BCR algorithm and the NC-RRT algorithm, the sizes of the random trees were reduced through a regression mechanism and an adaptive sampling area, respectively. The TO-RRT algorithm was used to greatly reduce the number of nodes in the space, and its complexity was the lowest. Figure 9b,f,j,n,r show that the RRT algorithm still searched the whole space. Although the biased-RRT algorithm generated fewer

nodes than the RRT algorithm, the search tree generated a large number of nodes on the surface of obstacles, which increased the number of iterations. The NC-RRT algorithm made the random tree tend to expand through boundary nodes through the node control mechanism, so it had fewer redundant nodes. It can be seen from Figure 9c,d,g,h,k,l,o,p,s,t that the RRT algorithm and the biased-RRT algorithm could not quickly find the "escape channel". Although the RRT-BCR algorithm limited the expansion of nodes that were prone to collision, it increased the expansion times of other nodes. Due to the regression superposition algorithm and node-first search strategy introduced into the TO-RRT algorithm, the random tree could quickly search the nearby area to find the "escape channel" in the repulsive potential field.

There are certain errors and contingencies in a single experiment. To better reflect a real situation, 10 simulation experiments were carried out in the same environment as described above, shown in Figure 10.

Figure 10 shows that the TO-RRT algorithm maintained strong stability in 10 experiments and did not traverse the whole space due to being blocked by obstacles, while the RRT algorithm and the biased-RRT algorithm both generated a large number of nodes in the space. In addition, the RRT-BCR algorithm had fewer path nodes than the biased-RRT algorithm, and in the NC-RRT algorithm, there was little difference in the path in each search. The comparison of the running times of the three algorithms in different environments is shown in Figure 11. Figure 11 shows that the RRT algorithm had the longest running time and poor running-time stability, especially in a single-channel environment, with the longest running time at 45.6057 s and the shortest running time at 1.2880 s. Compared with the RRT algorithm, the biased-RRT algorithm had a much shorter running time and strong running-time stability, but the search time in a complex environment was longer. The longest running times of the TO-RRT algorithm in the four environments were 0.0225 s, 0.0420 s, 0.0618 s, and 0.0443 s, and the shortest running times were 0.0056 s, 0.0134 s, 0.0101 s, and 0.0115 s. The difference between the longest search time and the shortest search time in a single environment did not exceed 0.06 s, which not only indicated a short search time but also a strong and stable running time. The NC-RRT algorithm performed poorly in a multi-rectangle environment, with a difference of 4.44 times between the longest running time and the shortest running time, while the RRT-BCR algorithm was only 3.82 times.

Table 2 shows the average values of each index of the 3 algorithms over 10 experiments (biased-RRT represents the biased-RRT algorithm with a target offset probability of 50%). In the multi-sphere environment, the TO-RRT algorithm had a running time that was 99.74% less than the RRT algorithm, which was mainly because the number of collision detections and the number of failed node growths of the former were reduced by 99.39% and 97.17%, respectively, compared with the latter. In addition, compared with the RRT algorithm, the number of path nodes in the TO-RRT algorithm was reduced by 82.92%, which shortened the length of its search path by 18.99%. When the random tree encountered a large area of obstacles, the TO-RRT algorithm was used to reflect the advantages in the search time more than the RRT algorithm. For example, the number of tree nodes and the number of failed growths of nodes of the RRT algorithm in the multi-rectangle environment reached 17,358.3 and 3144.8, respectively, resulting in a running time of 7.8822 s, while the running time of the TO-RRT algorithm was only 0.0213 s. In addition, the RRT-BCR algorithm performed better than the NC-RRT algorithm in a multi-rectangle environment, and its running time was shortened by 29.14% compared with the NC-RRT algorithm because the RRT-BCR algorithm removed nodes that collided many times when facing obstacles with large occlusion areas. The biased-RRT algorithm produced too much failure growth when encountering obstacles with large areas. For example, in a multi-channel environment, the node failure growth rate of the biased-RRT algorithm was 62.54%, while the RRT algorithm and TO-RRT algorithm had node failure growth rates of only 36.40% and 15.82%, respectively. Therefore, the biased-RRT algorithm was not ideal in a complex environment. Since the NC-RRT algorithm always took the area between the configuration point and the target as the sampling radius and tended to use boundary nodes for expansion, it could not

produce valid nodes when the obstacle was between the configuration point and the target. For example, in multi-channel and multi-rectangle environments, the collision detection times of the NC-RRT algorithm were 21,487 times and 55,077 times. In summary, compared with the other algorithms, the TO-RRT algorithm had significant advantages in searching speed and the number of nodes in the random tree.

**Figure 9.** The performances in different environments of: the RRT algorithm (**a**–**d**); the biased-RRT algorithm with a target offset probability of 50% (**e**–**h**); the TO-RRT algorithm (**i**–**l**); the RRT-BCR algorithm (**m**–**p**); and the NC-RRT algorithm (**q**–**t**).

**Figure 10.** Ten experiments each of: the RRT algorithm (**a**–**d**); the biased-RRT algorithm with a target offset probability of 50% (**e**–**h**); the TO-RRT algorithm (**i**–**l**); the RRT-BCR algorithm (**m**–**p**); and the NC-RRT algorithm (**q**–**t**).

**Figure 11.** The running times of the RRT algorithm, the biased-RRT algorithm with a target offset probability of 50%, the TO-RRT algorithm, the RRT-BCR algorithm, and the NC-RRT algorithm. (**a**) Multi-sphere environment; (**b**) Multi-rectangle environment; (**c**) Single-channel environment; (**d**) Multi-channel environment.




**Table 2.** *Cont*.

Note: RRT, rapidly-exploring random tree; Biased-RRT, rapidly-exploring random tree with target Bias; TO-RRT, time-optimal rapidly-exploring random tree; RRT-BCR, Biased-RRT with boundary expansion mechanism and regression mechanism; NC-RRT, Node Control-RRT.

#### *3.2. Obstacle Avoidance Test Based on the Robotics Toolbox*

To verify the feasibility of the TO-RRT algorithm on the manipulator, Robotics Toolbox 10.2 in MATLAB was used to model the Franka manipulator. Franka is a 7-DOF robot with high precision and fast response. Its payload is 3 kg, and the maximum contact area is 855 mm. The Franka manipulator can realize two-way communication between itself and the workstation through the Franka Control Interface (FCI) and an Ethernet connection. Therefore, complete real-time control can be achieved with a sampling frequency of 1 kHz. In terms of picking performance, Franka's pose repeatability is within 0.1 mm. Even at the highest speed of 2 m/s, the path deviation can be ignored, which provides good working conditions for fruit picking. The physical object of the Franka manipulator and its D-H parameters are shown in Figure 12a and Table 3, respectively.

**Figure 12.** Materials and results of simulation experiments based on using Robotics Toolbox. (**a**) The physical object of the Franka manipulator; (**b**) Trunk model; (**c**) The Franka manipulator avoids obstacles.


**Table 3.** D-H parameters.

To simplify the trunk and improve the operation speed of the TO-RRT algorithm, the trunk was regarded as a combination of spheres [29], as shown in Figure 12b and Table 4. To judge whether the manipulator collided with obstacles, the shortest distance *dcollision* from the center of the sphere to the origin of the coordinate system of adjacent links of the manipulator was used. The three-dimensional coordinates of each joint of the manipulator were obtained through a forward kinematics solution, and if the manipulator did not collide with the tree trunk, the following conditions must be met:

$$d\_{collision} > R + r \tag{3}$$

**Table 4.** Obstacle parameters.


In the formula, *R* = 5 cm is the radius of the obstacle ball, and *r* = 3 cm is the radius of the cylinder.

Figure 12c shows the Franka manipulator using the TO-RRT algorithm to plan its path, and the minimum-snap trajectory optimization algorithm was used to smooth the trajectory of the manipulator [37,38]. Figure 13 shows the shortest distance.
