FBi-RRT: a path planning algorithm for manipulators with heuristic node expansion

Abstract

This paper is concerned with the problem of collision-free path planning for manipulators in multi-obstacle scenarios. Aiming at overcoming the deficiencies of existing algorithms in excessive time consumption and poor expansion quality, a path planning algorithm named Fast Bi-directional Rapidly-exploring Random Tree (FBi-RRT) with novel heuristic node expansion is proposed, which includes a selective-expansion strategy and a vertical-exploration strategy. The selective-expansion strategy is designed to guide the selection of the nearest-neighbor node to avoid the repeated expansion failure, thereby shortening the overall planning time. Also, the vertical-exploration strategy is developed to regulate the expansion direction of the collision nodes to escape from the obstacle space with less blindness, thus improving the expansion quality and further reducing time cost. Compared with previous planning algorithms, FBi-RRT can generate a feasible path for manipulators in a drastically shorter time. To validate the effectiveness of the proposed heuristic node expansion, FBi-RRT is conducted on a 6-DOF manipulator and tested in five scenarios. The experimental results demonstrate that FBi-RRT outperforms the existing methods in time consumption and expansion quality.

Nomenclature

$\boldsymbol{\Theta }_{\textit{start}}$

Initial joint configuration of the manipulator. Start node of the random tree

$\boldsymbol{{X}}_{goal}$

Terminal pose of the manipulator’s end-effector

$\boldsymbol{\Theta }_{goal}$

Terminal joint configuration of the manipulator. Goal node of the random tree

$\boldsymbol{\Theta }_{\textit{limit}}$

Joint angle limits, that is, minimum value and maximum value of joint angle in manipulator motion

$\boldsymbol{\Theta }_{\textit{space}}$

Sampling space of the random tree

$\boldsymbol{\Theta }_{\textit{sample}}$

Sampling point selected in sampling space

$\boldsymbol{\Theta }_{\textit{nearest}}$

Nearest-neighbor node is the node nearest to the sampling point on the random tree

$\boldsymbol{\Theta }_{new}$

New node expanded from the nearest-neighbor node

$\boldsymbol{\Theta }_{dir}$

Expansion direction of the new node

$\boldsymbol{{E}}_{tree}$

Random tree is used to store all feasible nodes

$\boldsymbol{\Theta }_{i}$

The $i$ th node on the random tree corresponds to a joint configuration of the manipulator

$\theta _{step}$

Expansion step size used in expanding a new node

$\theta _{step\_ \textit{collision}}$

Interpolation step size used in collision detection

$\mathbf{\Pi }_{path}$

Feasible path generated by path planning algorithm

1. Introduction

Over the past few decades, the manipulators have received increasing attention, due to their extensive applicability in civil and military fields, for example, medical sampling [1–3], on-orbit servicing [4–6], agricultural picking [7, 8], nuclear facility maintenance [9, 10], etc. In the path planning problem of manipulators, given the initial and terminal configurations, the manipulator is required to generate a feasible path (i.e., a sequence of configurations), following which the manipulator can reach the desired configuration with the consideration of avoiding self-collision and collision with environmental obstacles, also considering the object potentially attached to the end-effector [11]. As the computational complexity and time consumption increase exponentially with the dimension of configuration space and scenario complexity, planning feasible paths becomes rather challenging for the operation of high-dimensional manipulators in multi-obstacle scenarios. To tackle this problem, the rapidly exploring random tree (RRT) and many variants extended from RRT (denoted by RRT+ in this paper) have been proposed in the robotics community. Numerous RRT+ algorithms have been verified to be effective in the applications of manipulators [12–15].

In the scheme of RRT+, the sampling point is probabilistically set to coincide with the goal node for the sake of speeding up the growth of the random tree toward the goal node. By minimizing the cost determined by the Euclidean distance, the nearest-neighbor node to the goal node is selected from the existing nodes in the random tree [16, 17]. Then, a new node will be generated and the collision detection will be performed. The expansion procedures will be iteratively executed until a new collision-free node is acquired. However, it turns out that the second effort to expand a new node from a failed node selected as the goal node’s nearest-neighbor node also results in a collision. So, the random tree will repeatedly fail to generate a new node toward the goal node. Thus, additional time will be required, which seriously limits the capacity of manipulators in real-world applications that give an extreme emphasis on time consumption. Therefore, it is significant to develop a new strategy to guide the selection of the nearest-neighbor node and address the problems of repeated expansion failure, which have not been considered yet.

Moreover, in the existing studies, the new node expanding from the nearest-neighbor node toward the sampling point will be eliminated if the new node runs into an obstruction. Then, a new node will be regenerated toward a new sampling point in the sampling space, and collision detection will be performed. The aforementioned procedures will be iteratively executed until a new collision-free node is successfully acquired or the iteration limit is reached [18–20]. However, due to the randomness of RRT+, the new sampling point may be located anywhere in the sampling space, resulting in repeated and ineffective exploration. Therefore, the new nodes expanded from the node near obstacles toward the new sampling point still have a high probability of collision, which causes excessive time consumption and restricts the application of RRT+ in highly cluttered obstacle environments [21, 22]. Thus, the issue of designing a strategy to regulate the expansion direction of the collision nodes to escape from the obstacle space is worthwhile for further investigation.

Motivated by the above considerations, in this paper, a memory pool is built up to prevent the same node from being selected twice as the nearest-neighbor node extended toward the goal node. Moreover, the expansion direction of the collision node is steered to the vertical direction of the collision direction to expand a new node, so that the newly expanding node can be as far away from obstacles as possible. On this basis, a novel pathplanning algorithm named Fast Bi-directional Rapidly-exploring Random Tree (FBi-RRT) is proposed to determine collision-free paths in a drastically shorter amount of time for manipulators in multi-obstacle scenarios. The contributions of this article are as follows:

(1) A selective-expansion strategy is designed to guide the selection of the nearest-neighbor node and avoid the repeated expansion failure. This considerably reduces the planning time and increases the success rate of node expansion.

(2) A vertical-exploration strategy is developed to regulate the expansion direction of the collision nodes to escape from the obstacle space with less blindness, thereby improving the expansion quality and shortening the planning time.

(3) The overall framework of the FBi-RRT algorithm is implemented to generate collision-free paths for manipulator from the initial configuration to the desired end-effector pose with the joint angle limit.

(4) Five experimental scenarios with different numbers of obstacles are designed to demonstrate the performance of the proposed algorithm in different complexity scenarios.

The remainder of this paper is organized as follows. Section 2 explains the heuristics for node expansion. Section 3 presents the FBi-RRT algorithm. Section 4 describes manipulator model and the experimental setup. Section 5 discusses the experimental results. Section 6 concludes this paper with some suggestions for further research.

2. Heuristics for node expansion

In this section, the heuristics for node expansion are presented to improve the expansion quality and accelerate the planning process. A selective-expansion strategy is designed to deal with the problems of repeated expansion failure, thereby shortening the overall planning time. Also, a vertical-exploration strategy is developed to regulate the expansion direction of the collision nodes to escape from the obstacle space with less blindness, thus further reducing time cost.

2.1. Selective-expansion strategy

The selection process of the nearest-neighbor node based on the conventional goal-biased expansion strategy in RRT + is shown in Eqs. (1)–(5). The sampling point $\boldsymbol{\Theta }_{\textit{sample}}$ is probabilistically set to coincide with the goal node $\boldsymbol{\Theta }_{goal}$ , for the sake of speeding up the growth of the random tree toward the goal node $\boldsymbol{\Theta }_{goal}$ . By minimizing the cost determined by the Euclidean distance, the RRT + algorithm selects the nearest-neighbor node $\boldsymbol{\Theta }_{\textit{nearest}}$ to the sampling point $\boldsymbol{\Theta }_{\textit{sample}}$ from the existing nodes in the random tree. Then, a new node $\boldsymbol{\Theta }_{new}$ will be generated from $\boldsymbol{\Theta }_{\textit{nearest}}$ toward $\boldsymbol{\Theta }_{\textit{sample}}$ , and the collision detection will be performed. The expansion procedures will be iteratively executed until a new collision-free node is successfully acquired or the iteration limit is reached. However, for each node selected as the goal node’s nearest-neighbor node, if it collides with environmental obstacles in its first expansion toward the goal node, according to Eqs. (2)–(5), the second expansion will lead to the same generated node and also result in a collision. Therefore, the random tree will repeatedly fail to generate a new node toward the goal node.

(1) \begin{align} \boldsymbol{\Theta }_{\textit{sample}}=\left\{\begin{array}{l} \boldsymbol{\Theta }_{goal},\,\,\,\,\mathbf{rand}\left(1,1\right)\lt p_{goal}\\[5pt] \boldsymbol{\Theta }_{rand},\,\,\,\,\mathbf{rand}\left(1,1\right)\geq p_{goal} \end{array} \right. \end{align}

(2) \begin{equation} \boldsymbol{\Theta }_{\textit{nearest}}=\textbf{argmin}\left(\sum _{i=1}^{n}\omega _{i}\left| \boldsymbol{\Theta }_{k}\left(i\right)-\boldsymbol{\Theta }_{\textit{sample}}\left(i\right)\right| \right),\,\,\boldsymbol{\Theta }_{k}\in \boldsymbol{{E}}_{tree} \end{equation}

(3) \begin{align} \theta _{step}=\left\{\begin{array}{l} \theta _{step\_ max},\,\theta _{step\_ max}\lt \left\| \boldsymbol{\Theta }_{sn}\right\| \\[5pt] \left\| \boldsymbol{\Theta }_{sn}\right\|,\,\,\theta _{step\_ min}\leq \left\| \boldsymbol{\Theta }_{sn}\right\| \leq \theta _{step\_ max}\\[5pt] 0,\qquad \left\| \boldsymbol{\Theta }_{sn}\right\| \lt \theta _{step\_ min} \end{array} \right.\end{align}

(4) \begin{equation} \boldsymbol{\Theta }_{dir}=\boldsymbol{\Theta }_{sn}/||\boldsymbol{\Theta }_{sn}|| \end{equation}

(5) \begin{equation} \boldsymbol{\Theta }_{new}=\boldsymbol{\Theta }_{\textit{nearest}}+\theta _{step}\boldsymbol{\Theta }_{dir} \end{equation}

where $\boldsymbol{\Theta }_{sn}=\boldsymbol{\Theta }_{\textit{sample}}-\boldsymbol{\Theta }_{\textit{nearest}}$ , $\textbf{rand}(a,b)$ stands for an $a\times b$ matrix consisting of random scalars drawn from the uniform distribution in the interval $(0,1)$ , and $\textbf{argmin}(f(x))$ stands for the value of the argument $x$ for which the given expression $f(x)$ attains its minimum value. $n$ represents the number of joints of the manipulator. $\omega _{i}=1-0.4i/n(i=1,2,\ldots,n)$ are the weight coefficients of joint angle changes. The closer the joint $i$ is to the manipulator base, the greater the weight coefficient $\omega _{i}$ .

To avoid the problems of repeated expansion failure, the selective-expansion strategy with memory pool $\boldsymbol{{E}}_{\textit{memory}}$ is developed to guide the selection of the nearest-neighbor node. The memory pool $\boldsymbol{{E}}_{\textit{memory}}$ is a set designed to store the nodes that have not expanded toward the goal node. $\boldsymbol{{E}}_{\textit{memory}}$ will dynamically adjust its element composition with the expansion of the random tree. In the beginning, the memory pool only contains the start nodes of the random tree. Every new node of the random tree will be added to the memory pool. Once a node is selected as the nearest-neighbor node to the goal node from the memory pool $\boldsymbol{{E}}_{\textit{memory}}$ , it will be removed from $\boldsymbol{{E}}_{\textit{memory}}$ and loses the chance to expand toward the goal node again.

(6) \begin{equation} \boldsymbol{{E}}_{\textit{memory}}=\left\{\boldsymbol{\Theta }_{\textit{start}}\right\} \end{equation}

(7) \begin{equation} \boldsymbol{{E}}_{\textit{memory}}\overset{\quad +\quad }{\longleftarrow }\boldsymbol{\Theta }_{new} \end{equation}

(8) \begin{equation} \boldsymbol{\Theta }_{\textit{nearest}}=\textbf{argmin}\left(\sum _{i=1}^{n}\omega _{i}\left| \boldsymbol{\Theta }_{k}\left(i\right)-\boldsymbol{\Theta }_{\textit{sample}}\left(i\right)\right| \right),\,\,\boldsymbol{\Theta }_{k}\in \boldsymbol{{E}}_{\textit{memory}} \end{equation}

(9) \begin{equation} \boldsymbol{{E}}_{\textit{memory}}\overset{\quad -\quad }{\longleftarrow }\boldsymbol{\Theta }_{\textit{nearest}} \end{equation}

The improved selection process of the nearest-neighbor node based on the selective-expansion strategy is shown in Eqs. (6)–(9) and Fig. 1. By using the proposed strategy, the node expansion will not be trapped in local optimum. Thereby, the success rate of the random tree reaching the goal node is increased, and the feasible path can be generated in less time.

Figure 1. Process of expanding new node to terminal node.

2.2. Vertical-exploration strategy

To reduce search blindness and avoid unnecessary sampling nodes, this subsection develops the vertical-exploration strategy to improve the expansion quality. In the existing literature, the newly expanded node will be discarded if it collides with the environmental obstacles, and the expansion procedures will be iteratively executed until a new collision-free node is successfully acquired or the iteration limit is reached. However, due to the randomness of RRT+, the new sampling point may be located anywhere in the sampling space or even overlap fully with the original sampling point, resulting in repeated and ineffective exploration. Therefore, the new nodes expanding from the node near obstacles toward the new sampling point still have a high probability of collision, as shown in Fig. 2, which causes excessive time consumption and restricts the application of RRT+ in highly cluttered obstacle environments.

Figure 2. New nodes randomly expanded near obstacles have a high probability of collision.

Guided by the vertical-exploration strategy, the new node will be generated in the vertical direction of the current expansion direction of the collision node. Using the orthogonality between two vertical vectors, the expansion direction of the node is updated by

(10) \begin{equation} \boldsymbol{\Theta }_{ver}=\textbf{rand}\left(1,n\right) \end{equation}

(11) \begin{equation} i_{\max}=\textbf{argmax}\left(||\boldsymbol{\Theta }_{dir}\left(1,i\right)||\right), i\in \left\{1,2,\ldots,n\right\} \end{equation}

(12) \begin{equation} \boldsymbol{\Theta }_{ver}\left(1,i_{\max}\right)\leftarrow -\frac{\boldsymbol{\Theta }_{dir}\boldsymbol{\Theta }_{ver}-\boldsymbol{\Theta }_{dir}\left(1,i_{\max}\right)\boldsymbol{\Theta }_{ver}\left(1,i_{\max}\right)}{\boldsymbol{\Theta }_{dir}\left(1,i_{\max}\right)} \end{equation}

(13) \begin{equation} \boldsymbol{\Theta }_{ver\_ unit}=\frac{\boldsymbol{\Theta }_{ver}}{||\boldsymbol{\Theta }_{ver}||} \end{equation}

(14) \begin{equation} \boldsymbol{\Theta }_{new}=\boldsymbol{\Theta }_{\textit{nearest}}+\theta _{step}\boldsymbol{\Theta }_{ver\_ unit} \end{equation}

where $\boldsymbol{\Theta }_{dir}$ is the current expansion direction of the node, $n_{\max}$ is the sequence number corresponding to the largest value in the array $\boldsymbol{\Theta }_{dir}$ , and $\boldsymbol{\Theta }_{ver}$ is the direction perpendicular to $\boldsymbol{\Theta }_{dir}$ .

Benefiting from the vertical-exploration strategy, the unawareness of the collision direction information and the blindness of the new node expansion can be avoided. Hence, the expansion quality is significantly improved and the exploration efficiency for a feasible path is speeded up.

3. The FBi-RRT algorithm

In this section, the FBi-RRT is established to generate a collision-free path for manipulators from the initial configuration $\boldsymbol{\Theta }_{\textit{start}}$ to the desired terminal pose $\boldsymbol{{X}}_{goal}$ with the joint angle limit $\boldsymbol{\Theta }_{\textit{limit}}$ . The overall framework of FBi-RRT is summarized as Algorithm 1.

Algorithm 1. $\textbf{FBi-RRT}({\Theta} _{\textit{start}},X_{goal},{\Theta} _{\textit{limit}})$

Note that in most manipulation tasks, the terminal condition is an end-effector pose $\boldsymbol{{X}}_{goal}$ given in the workspace, while the path is planned in the configuration space. Therefore, the first step is to use the Inverse kinematics of the manipulator to transform the terminal pose $\boldsymbol{{X}}_{goal}$ into the set of feasible configurations $[\boldsymbol{\Theta }_{goal1},\boldsymbol{\Theta }_{goal2},\ldots,\boldsymbol{\Theta }_{\textit{goal}\mathrm{m}}]$ , in which the optimal goal configuration $\boldsymbol{\Theta }_{goal}$ with the lowest path cost is selected through the well-known L ² norm, that is, the Euclidean distance from the initial configuration to the feasible configurations. Then, random trees $\boldsymbol{{E}}_{tree1}$ and $\boldsymbol{{E}}_{tree2}$ are initialized at the start node $\boldsymbol{\Theta }_{\textit{start}}$ and goal node $\boldsymbol{\Theta }_{goal}$ , respectively. To improve the sufficiency and flexibility of random sampling, the sampling space for each tree is set according to joint angle limitation $\boldsymbol{\Theta }_{\textit{limit}}$ and its initial node ( $\boldsymbol{\Theta }_{\textit{start}}$ or $\boldsymbol{\Theta }_{goal}$ ). Two random trees expand new nodes by turn. Every time a new node is successfully expanded, the algorithm will check whether there exists a feasible connection between them because these two trees need to be connected to find a feasible path from the start node to the goal node. The algorithm will continue until a path is found or the iteration limit is reached. The feasible path is extracted from the random tree by tracing back from the goal node $\boldsymbol{\Theta }_{goal}$ to the start node $\boldsymbol{\Theta }_{\textit{start}}$ .

The specific extension process of the random tree is shown in Algorithm 2. Firstly, the flag variables $f_{succ}$ and $f_{link}$ are initialized to mark whether a new node is successfully expanded and whether two trees are successfully connected, respectively. To avoid repeated failures in generating a new node, the conventional goal-biased expansion strategy and the proposed selective-expansion strategy are used to obtain a sampling point $\boldsymbol{\Theta }_{\textit{sample}}$ and the nearest-neighbor node $\boldsymbol{\Theta }_{\textit{nearest}}$ . The vertical-exploration strategy proposed in Section 2 is applied to generate a new node $\boldsymbol{\Theta }_{new}$ . Then, the nearest-neighbor node $\boldsymbol{\Theta }_{\textit{nearest}2}$ is determined by the least Euclidean distance between the generated node $\boldsymbol{\Theta }_{new}$ and the nodes in the tree $\boldsymbol{{E}}_{tree2}$ . A set of interpolating points is sampled along the edge from node $\boldsymbol{\Theta }_{\textit{nearest}2}$ to node $\boldsymbol{\Theta }_{new}$ with step size $\theta _{step\_ \textit{collision}}$ for collision detection. If all the interpolating points are collision-free, a feasible connection between two random trees can be obtained and the tree extension will be stopped. Finally, a set of interpolating nodes from $\boldsymbol{\Theta }_{\textit{nearest}2}$ to $\boldsymbol{\Theta }_{new}$ with step size $\theta _{step}$ are added to $\boldsymbol{{E}}_{tree1}$ , as shown in Fig. 3.

Figure 3. Expansion process of connecting two random trees.

Algorithm 2. $\textbf{TreeExtension}({\Theta} _{goal},{\Theta} _{\textit{space}\_ 1},E_{tree1},E_{tree2})$

The selection process of the sampling point $\boldsymbol{\Theta }_{\textit{sample}}$ and the nearest-neighbor node $\boldsymbol{\Theta }_{\textit{nearest}}$ based on the proposed selective-expansion strategy is shown in Algorithm 3. Firstly, the value of the random scalar $p$ is sampled from the uniform distribution in the interval (0,1). If $p$ is greater than $p_{goal}$ , the sampling point $\boldsymbol{\Theta }_{\textit{sample}}$ is randomly sampled within the configuration space. The nearest-neighbor node $\boldsymbol{\Theta }_{\textit{nearest}}$ to the sampled point $\boldsymbol{\Theta }_{\textit{sample}}$ is selected from the existing nodes of the random tree $\boldsymbol{{E}}_{tree1}$ by the minimum Euclidean distance. If $p$ is less than $p_{goal}$ , $\boldsymbol{\Theta }_{\textit{sample}}$ is set to coincide with the goal node $\boldsymbol{\Theta }_{goal}$ . The nearest-neighbor node $\boldsymbol{\Theta }_{\textit{nearest}}$ to the goal node $\boldsymbol{\Theta }_{goal}$ is selected from the memory pool $\boldsymbol{{E}}_{\textit{memory}}$ . Then, the nearest-neighbor node $\boldsymbol{\Theta }_{\textit{nearest}}$ will be removed from $\boldsymbol{{E}}_{\textit{memory}}$ and loses the chance to expand toward the goal node again. As a result, the node expansion will not be trapped into local optimum, such that the success rate is increased.

Algorithm 3. $\textbf{NodeS}\textbf{election}({\Theta} _{goal},{\Theta} _{\textit{space}},E_{tree})$

Moreover, the expansion process of the new node based on the vertical-exploration strategy proposed in Section 2 is shown in Algorithm 4. Firstly, a node $\boldsymbol{\Theta }_{new}$ is generated from the nearest-neighbor node $\boldsymbol{\Theta }_{\textit{nearest}}$ toward the sampling point $\boldsymbol{\Theta }_{\textit{sample}}$ with a step size $\theta _{step}$ . Note that the step size $\theta _{step}$ is dynamically adjusted with the distance between $\boldsymbol{\Theta }_{\textit{nearest}}$ and $\boldsymbol{\Theta }_{\textit{sample}}$ according to Eq. (4). Then, the function CollisionDetection( ) checks whether the connection between the nearest-neighbor node $\boldsymbol{\Theta }_{\textit{nearest}}$ and the node $\boldsymbol{\Theta }_{new}$ collides with obstacles. If the connection fails to satisfy the collision-free constraint, to escape from the obstacle space with less blindness, the node expansion direction $\boldsymbol{\Theta }_{dir}$ will be updated to the vertical direction of the expansion direction of the collision node $\boldsymbol{\Theta }_{new}$ . The node $\boldsymbol{\Theta }_{new}$ will be regenerated along the direction $\boldsymbol{\Theta }_{dir}$ and the connection between $\boldsymbol{\Theta }_{\textit{nearest}}$ and $\boldsymbol{\Theta }_{new}$ will be collision detected. The expansion procedures will be iteratively executed until a new collision-free node is successfully acquired or the iteration limit is reached.

Algorithm 4. $\textbf{NodeExpansion}({\Theta} _{\textit{sample}},{\Theta} _{\textit{nearest}},\theta _{step})$

4. Path planning experiment of manipulator

The model of the manipulator and the coordinate frame of each joint-link are presented to establish the kinematic model. Also, the collision detection method is introduced to check whether the manipulator collides with obstacles in a certain configuration. Besides, a series of comparison experiments are set to demonstrate the effectiveness and superiority of the proposed algorithm FBi-RRT.

4.1. Model of the manipulator

A representative 6-DOF robotic manipulator composed of 6 rotating joint-links and an end-effector is employed in this paper. Figure 4(a) shows the three-dimensional model of the manipulator. Joint-link six is coupled to the end-effector. The mechanical kinematic schematic of the manipulator and the coordinate frame of each joint-link are illustrated in Fig. 4(c). The kinematic model of the manipulator used for path planning is established based on the coordinate structure shown in Fig. 4 and the D-H parameters listed in Table I. A joint configuration of the manipulator can be converted to a workspace pose of the end-effector using the kinematic model. In addition, collision detection is a crucial step in path planning that can significantly impact the efficiency of the overall algorithm, because every time a new node is generated, a large number of collision detections are required to determine whether the node is collided with obstacles [23]. Moreover, it is inefficient and unnecessary to provide accurate mathematical descriptions of the manipulator with irregular shapes. Thus, the joint-links of the manipulator are simplified to multiple cuboids for the purposes of planning efficiency. As shown in Fig. 4(c), each joint-link is simplified and modeled as a cuboid. Each cuboid’s coordinates are from the forward kinematics of the manipulator.

Figure 4. Model of the 6-DOF robotic manipulator.

Table I. D-H parameters of the manipulator.

4.2. Experimental setup

To demonstrate the effectiveness and superiority of the proposed algorithm FBi-RRT, a series of comparisons against the existing algorithm Bi-RRT is conducted under an actual manipulation task, as shown in Fig. 5(a) and (b). A mobile manipulator, a cup, two desks, and six file boxes are arranged in the task scenario. The manipulator is required to plan a feasible path, following which the manipulator can transfer the cup rigidly grasped by the end-effector reaching the desired pose with the consideration of avoiding collision with obstacles. Note that all obstacles are reasonably modeled as cuboids in the simulation experiment, as shown in Fig. 5(c) and (d). In order to verify the effectiveness of FBi-RRT in distinct scenarios, five groups of different obstacle settings are designed in the experiment, including challenging scenarios with multiple obstacles. As shown in column 2 of Fig. 6, Scenario V has the most obstacles, leading to more collisions for random tree expansion. The pose and size parameters of the cuboid obstacles in the scenario are given in Table II.

Figure 5. Real-world scenario (a)–(b) and corresponding simulation scenario (c)–(d).

Figure 6. Comparison of random trees and feasible paths generated by Bi-RRT and FBi-RRT in five distinct scenarios. The marker points depict the position of the manipulator’s end-effector in the workspace, which corresponds to the nodes in the configuration space. Invalid nodes that collide are represented by red dots, while nodes that don’t collide are represented by blue dots. The parent-child relationship between two nodes is visualized by blue line. The feasible path is visualized by green line.

Table II. Pose and size of the obstacles in simulation scenario.

The parameters of the algorithm FBi-RRT are set as follows: $p_{goal}=0.5$ , $n_{\textit{steer}}=5$ , $\theta _{step\_ \textit{collision}}=0.1\,\mathrm{rad}$ , $\theta _{step\_ max}=0.4\,\mathrm{rad}$ , $\theta _{step\_ min}=0.1\,\mathrm{rad}$ , $\boldsymbol{\Theta }_{\textit{start}}=[\!-\!0.21,-2.78,-1.22,-2.28,2.93,1.57]\;\mathrm{rad}$ , and $\boldsymbol{{X}}_{goal}=[\!-\!0.46,0.23,0.06,0,-1.57,0]$ . The planning is considered to be failed if a feasible path is not found within the iteration limit. The FBi-RRT and Bi-RRT adopt the same parameter settings for a fair comparison. All experiments were performed in MATLAB on Windows 10 environment, using an Intel Core i7-8700K processor with a 3.7 GHz main frequency and 24.0 GB memory.

Due to the randomness of the probabilistic sampling-based algorithms, each time the generated node and determined path might be different. In order to obtain a credible experimental conclusion, each group of experiments was carried out 100 times in each scenario, where the mean value and standard deviation are compared in Table III and Fig. 7. The resulting paths in Scenario I to V are also illustrated in Fig. 6. Compared with Bi-RRT, the performance improvements of the proposed algorithm FBi-RRT are further discussed as follows in terms of expansion quality, time consumption, and memory cost.

Figure 7. Comparison of the path planning performances of Bi-RRT and FBi-RRT algorithm in a diverse set of five scenarios. The mean value and the standard deviation are represented by the bar and the error bar, respectively.

Table III. Bi-directional Rapidly-exploring Random Tree and Fast Bi-directional Rapidly-exploring Random Tree path planning performance index data in five distinct scenarios.

5. Experimental result discussions

Time Consumption: The time consumption is evaluated by the time consumed by the algorithm to obtain a collide-free path for the planning problem, which is the critical difference between whether a given setup is feasible or not in real-time systems. It is evident from Fig. 7(a) that the time consumption of path planning for manipulators is significantly increased in challenging environments with more obstacles. Fortunately, as expected, the proposed FBi-RRT algorithm always obtains a valid path in less than 0.3 s, significantly faster than the Bi-RRT algorithm in all scenarios. As shown in column 3 of Table III, in the simple environment (Scenario I), the time consumption by Bi-RRT and FBi-RRT are both small (≤0.2 s), but in the highly clustered environment (Scenario V), the time consumption by FBi-RRT (0.335 s) is significantly saved compared with Bi-RRT (1.254 s). In addition, as shown in column 4 of Table III, the standard deviation of the time consumption of FBi-RRT is always smaller than Bi-RRT. That is to say, in 100 repeated experiments, the fluctuation of experimental results of FBi-RRT is less than that of Bi-RRT, which indicates FBi-RRT has lower search uncertainty and more stable time consumption.

Expansion Quality: The expansion quality is evaluated by the ratio of the number of nodes on the feasible path to the total number of sampling nodes in the tree. Suppose that the tree $\boldsymbol{{E}}_{tree}$ contains $M$ nodes in total, and the feasible path $\mathbf{\Pi }_{path}$ is composed of $m$ nodes. Then, the expansion quality is defined as $m/M$ . Note that, the expansion quality indicates the utilization of nodes sampled in the exploration process. The distribution of nodes depicted in Fig. 6 intuitively shows the strong randomness of the Bi-RRT, where many nodes are generated in the areas outside the feasible path, leading to ineffective exploration. By contrast, benefiting from the vertical-exploration strategy, the FBi-RRT can get around obstacles correctly and quickly, thus the feasible path can be found by only a few nodes. Moreover, it can be seen from Fig. 7(b) and column 5 of Table III that the proposed FBi-RRT has higher expansion quality compared to the Bi-RRT in all scenarios. Especially in the highly clustered environment (Scenario V), the expansion quality of the FBi-RRT algorithm (0.299) is significantly improved (about 2 times) compared with the Bi-RRT (0.105), which means that the FBi-RRT algorithm achieves a better balance between the exploration and the exploitation, thus a feasible path can be generated with the least number of sampling iterations.

Memory Cost: The memory cost is indicated by the number of nodes in the random tree. Recording nodes occupy the largest memory consumption in path planning, and there are great differences in different algorithms. This index is critical to the robotic system with limited computing power, such as on-orbit service and deep-space exploration. As shown in Fig. 7(c), the memory cost of FBi-RRT is much smaller than that of Bi-RRT in various scenarios, and the superior is more significant in complex scenarios. In observation of the result from column 7 of Table III, the memory cost of the FBi-RRT algorithm (31.94) is effectively reduced compared with the Bi-RRT algorithm (93.63) in Scenario V. The FBi-RRT greatly relaxes the constraints on hardware computing power, so it has advantages on low-power platforms designed for embedded applications, such as the lunar surface sampling manipulator.

In summary, the improvement of FBi-RRT in generating a feasible path for manipulators is mainly due to two strategies: (1) The selective-expansion strategy is designed to guide the selection of the nearest-neighbor node to avoid the problems of repeated expansion failure, thereby shortening the overall planning time. (2) The vertical-exploration strategy is developed to regulate the expansion direction of the collision nodes to escape from the obstacle space with less blindness, thus improving the expansion quality and further reducing time consumption.

6. Conclusions and future work

This paper presents a new sampling-based algorithm named FBi-RRT to plan collision-free paths for manipulators in multi-obstacle scenarios. Compared with existing methods, FBi-RRT is mainly improved in two new strategies. The selective-expansion strategy increases the success probability of node expansion by avoiding the problems of local optimum, thereby shortening the overall planning time of the conventional methods that select nearest-neighbor nodes purely in the whole random tree. Meanwhile, the vertical-exploration strategy is proposed to improve the expansion quality by regulating the expansion direction of the collision nodes to escape from the obstacle space with less blindness. The performance of the proposed strategy is validated through experiments on a 6-DOF manipulator and tested in 5 scenarios. The experimental results indicate that the path planning process based on the FBi-RRT has higher expansion quality, shorter time consumption, and less memory consumption overall than the Bi-RRT.

To perform task-constrained motion, where the manipulator comes in contact with constrained objects, such as transporting an open liquid-filled container always keep it upright to prevent spillage, one future research direction will be to include constraints, for example, end-effector pose constraints, dynamics constraints, and actuator force-torque limits into the path planning framework. In addition, another research direction will be to extend the FBi-RRT to real-time systems by considering the information changes in dynamic environments. Moreover, one more worthy research direction will be to gather more knowledge about environmental obstacles in order to better guide the planning process.