Bidirectional RRT*-Based Path Planning for Tight Coordination of Dual Redundant Manipulators

Abstract

There are closed-chain constraints between the left manipulator and the right manipulator in tight coordination of the dual redundant manipulator. The existing planning algorithms suitable for loose coordination cannot be directly applied to tight coordination, as the planned path cannot satisfy the closed-chain constraints. To solve the above problem, a master-slave planning method based on bidirectional RRT* is proposed for dual redundant manipulators. Bidirectional RRT* is adopted to plan the path of the master manipulator. The path of the slave manipulator is calculated by terminal generalized velocity constraints instead of terminal position and posture constraints. Moreover, a local path replanning strategy is proposed to solve the problem that the planned path is actually not feasible due to the discontinuous joint path of the slave manipulator. The joint self-motion in the null space is utilized to keep the terminal position and posture of the slave manipulator unchanged. The proposed method is verified by simulations and experiments and the results show that it can solve the discontinuity problem, increase the success rate, shorten the planning time and satisfy closed-chain constraints. Therefore, the proposed method can be feasibly and effectively applied to the tight coordination of dual redundant manipulators.

1. Introduction

Redundant manipulators, compared with nonredundant manipulators [1,2], can be more flexible to perform a variety of delicate and complex operations due to their increased degrees of freedom [3,4,5]. Hence, they are widely applied to various industrial fields. For many operations such as transportation, assembly, maintenance and processing of complex parts, single redundant manipulators may not be competent due to the limitations of their load-bearing capacity, workspace and application range. Therefore, coordination operations of dual redundant manipulators have attracted extensive attention [6,7,8].

Path planning is one of the key technologies for safe operations of dual redundant manipulators [9,10,11]. Sampling-based algorithms are the most popular algorithms for path planning. Rapidly exploring random tree (RRT) and its derivative algorithms are the mainstream among various sampling-based algorithms [12,13,14,15,16,17]. Yu et al. [18] proposed a spline RRT* for coordinated motion planning of dual-arm space manipulators. RRT* was adopted to plan the desired path and then the quartic spline function was adopted to smooth it. Ying et al. [19] proposed a bidirectional RRT with deep learning for the cooperative assembly of dual manipulators. The historical training data of deep learning were adopted to generate new nodes that were close to the goal node and away from the obstacles. Chen et al. [20] proposed a bidirectional RRT with post-processing for the cooperative assembly of dual manipulators. After the planning of bidirectional RRT was completed, the path was optimized by post-processing. Compared with RRT, bidirectional RRT has a faster convergence speed, and RRT* can plan an optimized path. To shorten the planning time and path length, bidirectional RRT* (B-RRT*) is proposed by combining bidirectional RRT with RRT* [21,22]. Therefore, B-RRT* is adopted in this study for path planning of dual redundant manipulators.

Existing algorithms based on RRT are generally applied to plan the path for loose coordination of dual redundant manipulators [18,19,20,23,24,25]. Compared with loose coordination, the terminal positions and postures of the left manipulator and the right manipulator are mutually restricted by the kinematic closed-chain constraints in tight coordination [26,27]. Therefore, planning algorithms suitable for loose coordination cannot be directly applied to tight coordination. To solve the above problem, a master-slave planning method based on B-RRT* is proposed for tight coordination of dual redundant manipulators in this study.

For general master-slave planning methods, the path of the slave manipulator in the task space is obtained by the terminal position and posture constraints, and then its path in the joint space is calculated by solving the inverse kinematics [28,29]. For redundant manipulators, the inverse kinematics is usually solved by numerical algorithms. Numerical algorithms generally obtain the joint angular velocity of the redundant manipulator through its terminal generalized velocity and then calculate the joint angles accordingly [30,31]. According to the above characteristics, the terminal generalized velocity constraints are directly adopted to obtain the joint angular velocity of the slave manipulator in our study. Therefore, it can avoid the calculation of inverse kinematics and improve the execution efficiency.

When the master-slave planning method based on B-RRT* is applied to dual redundant manipulators, it cannot guarantee the continuity of the path at the connection nodes of two trees in the joint space. The reasons for this problem are described as follows. The path of the master manipulator is planned by the planning algorithm. Hence, it is continuous at the two connection nodes in both the task space and the joint space. Nevertheless, the path of the slave manipulator is calculated by the closed-chain constraints that are just acted on the terminal states. Therefore, it can only ensure that its path is continuous at the two connection nodes in the task space. Since there are infinite sets of inverse kinematics solutions for redundant manipulators, there is a high probability that the two sets of joint angles at the two connection nodes are inconsistent and usually quite different. That is, the path of the slave manipulator may be discontinuous at the two connection nodes in the joint space. As a result, there is an additional motion between the two connection nodes in the actual motion due to the above discontinuity problem. This additional motion is not included in the planned path. Therefore, the dual redundant manipulator may collide with obstacles or fail to satisfy the closed-chain constraints in the actual motion. The planned path is actually not feasible in this case. To solve the discontinuity problem, it is necessary to modify the master-slave planning method based on B-RRT* so that the planned path is continuous in both the task space and the joint space.

The main contributions of this paper are highlighted as follows:

(1)

For tight coordination of dual redundant manipulators, a master-slave planning method based on B-RRT* is proposed. One manipulator is regarded as the master manipulator and the other is regarded as the slave manipulator. The path of the master manipulator is planned by B-RRT* and then the path of the slave manipulator is calculated according to the kinematic closed-chain constraints.

(2)

Considering the characteristics of dual redundant manipulators, the path of the slave manipulator is calculated by the terminal generalized velocity constraints instead of the terminal position and posture constraints. In this way, the calculation of the inverse kinematics solutions can be avoided, and the planning efficiency can be improved.

(3)

To solve the discontinuity problem in the joint path of the slave manipulator at the connection nodes when the master-slave planning method based on B-RRT* is applied to dual redundant manipulators, a local path replanning strategy is designed on the basis of the joint self-motion in the null space. On the premise that the terminal position and posture of the slave manipulator remain unchanged, the local path of the slave manipulator between the two connection nodes is replanned to guarantee its continuity and satisfy the closed-chain constraints.

The subsequent contents of this paper are organized as follows. In Section 2, the closed-chain constraints of dual redundant manipulators are described. The master-slave planning method based on B-RRT* for dual redundant manipulators is introduced. The local path replanning strategy is designed. In Section 3, comparative simulations and experiments are carried out, and their results are analyzed. In Section 4, innovations, extended applications and limitations of our study are discussed. In Section 5, conclusions are given.

2. Materials and Methods

2.1. Closed-Chain Constraints for Dual Redundant Manipulators

The general situation of tight coordination operation of dual redundant manipulators is that the left manipulator and the right manipulator cooperate with each other to grip a workpiece and then move it along a desired path. In this situation, a closed-chain structure is formed between the left manipulator, the right manipulator and the workpiece. There are strict constraints between them, which are called the closed-chain constraints. The transportation of a workpiece is taken as an example to describe the closed-chain constraints in the tight coordination operation of dual redundant manipulators, as shown in Figure 1. The left manipulator is taken as the benchmark in the following description.

2.1.1. Terminal Position and Posture Constraints

It can be seen from the purple dotted line box in Figure 1 that there is a closed-chain relationship between the terminal position of the left manipulator and the terminal position of the right manipulator. Therefore, the closed-chain constraint between them can be obtained according to the addition rule of vectors [32,33], as described below:

$$d_0+R_w^{l0}{p}_{l0}^{l7}\left(q^l\right)+R_w^{l0}{R}_{l0}^{l7}\left(q^l\right)d_7=p_w^{r7}\left(q^r\right)$$

(1)

where $d_0$ is the position vector of the base coordinate frame of the left manipulator in the world coordinate frame, $R_w^{l0}$ is the posture transformation matrix of the base coordinate frame of the left manipulator with respect to the world coordinate frame, $p_{l0}^{l7}\left(q^l\right)$ is the position vector of the terminal coordinate frame of the left manipulator in its base coordinate frame, $R_{l0}^{l7}\left(q^l\right)$ is the posture transformation matrix of the terminal coordinate frame of the left manipulator with respect to its base coordinate frame, $d_7$ is the distance vector between the terminal coordinate frames of the left manipulator and the right manipulator in the terminal coordinate frame of the left manipulator and $p_w^{r7}\left(q^r\right)$ is the position vector of the terminal coordinate frame of the right manipulator in the world coordinate frame.

According to the transformation relation between the coordinate frames at the four vertices of the purple dotted line box in Figure 1, the constraint between the terminal posture of the left manipulator and the terminal posture of the right manipulator can be obtained [32,33], as described below:

$$R_w^{r0}{R}_{r0}^{r7}\left(q^r\right)=R_w^{l0}{R}_{l0}^{l7}\left(q^l\right)R_{l7}^{r7}$$

(2)

where $R_w^{r0}$ is the posture transformation matrix of the base coordinate frame of the right manipulator with respect to the world coordinate frame, $R_{r0}^{r7}\left(q^r\right)$ is the posture transformation matrix of the terminal coordinate frame of the right manipulator with respect to its base coordinate frame and $R_{l7}^{r7}$ is the posture transformation matrix of the terminal coordinate frame of the right manipulator with respect to the terminal coordinate frame of the left manipulator.

2.1.2. Terminal Linear and Angular Velocity Constraints

The constraint between the terminal linear velocity of the left manipulator and the terminal linear velocity of the right manipulator can be obtained through the derivation of Equation (1) [32,33], as described below:

$$\left[R_w^{l0}{J}_l\left(q^l\right)+L\left(q^l\right)\right]{\dot{q}}^l=R_w^{r0}{J}_l\left(q^r\right){\dot{q}}^r$$

(3)

where $L\left(q^l\right)=\frac{\partial \left[R_w^{l7}\left(q^l\right)d_7\right]}{\partial q^l}$, $J_l\left(q^l\right)$ is the first three rows of the Jacobian matrix of the left manipulator, $J_l\left(q^r\right)$ is the first three rows of the Jacobian matrix of the right manipulator, ${\dot{q}}^l$ is the joint angular velocity of the left manipulator and ${\dot{q}}^r$ is the joint angular velocity of the right manipulator.

For the transportation of a workpiece, the posture transformation matrix $R_{l7}^{r7}$ is a constant matrix. Therefore, the angular velocity of the terminal coordinate frame of the right manipulator with respect to the terminal coordinate frame of the left manipulator is 0. That is, the terminal angular velocity of the right manipulator is equal to the terminal angular velocity of the left manipulator. The constraint between the terminal angular velocity of the left manipulator and the terminal angular velocity of the right manipulator is described below [32,33]:

$$R_w^{l0}{\omega }_l=R_w^{r0}{\omega }_r$$

(4)

where ${\omega }_l=J_a\left(q^l\right){\dot{q}}^l$, ${\omega }_r=J_a\left(q^r\right){\dot{q}}^r$, ${\omega }_l$ is the terminal angular velocity of the left manipulator in its base coordinate frame, ${\omega }_r$ is the terminal angular velocity of the right manipulator in its base coordinate frame, $J_a\left(q^l\right)$ is the last three rows of the Jacobian matrix of the left manipulator and $J_a\left(q^r\right)$ is the last three rows of the Jacobian matrix of the right manipulator.

The constraint between the terminal generalized velocity of the left manipulator and the terminal generalized velocity of the right manipulator can be obtained by combining constraint (3) with constraint (4) [32], as described below:

$${\dot{q}}^r=J^{+}\left(q^r\right){\left(R_w^0\right)}^{-1}{J}_n\left(q^l\right){\dot{q}}^l$$

(5)

where $R_w^0=\left[\begin{array}{cc}{R}_w^{r0}& O\\ O& R_w^{r0}\end{array}\right]$, $J_n\left(q^l\right)=\left[\begin{array}{c}{R}_w^{l0}{J}_l\left(q^l\right)+L\left(q^l\right)\\ R_w^{l0}{J}_a\left(q^l\right)\end{array}\right]$, $J^{+}\left(q^r\right)$ is the pseudo inverse of the Jacobian matrix of the right manipulator.

2.2. Master-Slave Planning Based on B-RRT* for Dual Redundant Manipulators

For dual redundant manipulators, B-RRT* establishes a tree T₁ at the initial node q_init and another tree T₂ at the goal node q_goal, respectively. The root node of the tree T₁ is q_init and the root node of the tree T₂ is q_goal. Then, two trees are extended successively until they are connected to each other, as shown in Algorithm 1.

Algorithm 1. Bidirectional RRT*

Input: the initial node qinit and the goal node qgoal

Output: the desired path σ

1. T1.init(qinit); T2.init(qgoal);

2. For i = 1 To N

3.  (T1, flag1, qe1, qe2) ← Extend(T1, T2);

4.  If  flag1 = true

5.   Break;

6.  End If

7.  (T2, flag2, qe1, qe2) ← Extend(T2, T1);

8.  If  flag2 = true

9.   Break;

10.  End If

11. End For

12. If  flag1 = true or flag2 = true

13.  σ ← ExtractPath (T1, T2, qinit, qgoal, qe1, qe2);

14. Else

15.  σ ← Ø;

16. End If

17. Return σ;

Assuming that both the left manipulator and the right manipulator have n degrees of freedom, the dual redundant manipulator can be regarded as a whole, which is equivalent to a single manipulator with 2n degrees of freedom. In this case, the nodes of two trees can be expressed as below:

$$q=\left[\begin{array}{cc}{q}^l& q^r\end{array}\right]$$

(6)

where $q^l=\left[\begin{array}{ccccc}{q}_1^l& \cdots & q_i^l& \cdots & q_n^l\end{array}\right]$, $q^r=\left[\begin{array}{ccccc}{q}_1^r& \cdots & q_i^r& \cdots & q_n^r\end{array}\right]$, $q$ is the node of one tree of the dual redundant manipulator, $q^l$ is the set of n joint angles of the left manipulator, $q^r$ is the set of n joint angles of the right manipulator, $q_i^l$ is the ith joint angle of the left manipulator and $q_i^r$ is the ith joint angle of the right manipulator.

The extension processes of two trees are identical. Hence, the extension process of the tree T₁ is taken as an example to elaborate the details, as shown in Algorithm 2.

Algorithm 2 (T1, flag, qe1, qe2) ← Extend(T1, T2)

flag ← false; qrand ← Sample(qlimit); qnear1 ← Near(qrand, T1); qnew1 ← Steer(qrand, qnear1); feasibility ← DetectCollision(qnew1); If  feasibility = true  T1 ← Insert(qnew1, qnear1);  T1 ← ReselectParentNode(qnew1, qnear1, T1);  T1 ← Rewire(qnew1, T1);  (flag, qe1, qe2) ← JudgeConnectivity(qnew1, T2);  While feasibility = true And flag = false   qnear2 ← Near(qnew1, T2);   qnew2 ← Steer(qnew1, qnear2);   feasibility ← DetectCollision(qnew2);   If feasibility = true    T2 ← Insert(qnew2, qnear2);    T2 ← ReselectParentNode(qnew2, qnear2, T2);    T2 ← Rewire(qnew2, T2);    (flag, qe1, qe2) ← JudgeConnectivity(qnew2, T1);   End If  End While End If

Firstly, a sampling point q_rand can be obtained by random sampling within the motion range of the dual redundant manipulator. Secondly, the node q_near1 that is nearest to the sampling point q_rand is searched in the tree T₁. Thirdly, a new node q_new1, whose parent node is q_near1, can be obtained by expanding, as shown in Algorithm 3. Fourthly, the collision between the dual redundant manipulator and obstacles at the new node q_new1 is detected. If a collision occurs, the new node q_new1 will be abandoned and then the tree T₂ is switched to extend. Conversely, if no collision occurs, the new node q_new1 will be added to the tree T₁. Fifthly, the parent node of the new node q_new1 is reselected in its neighborhood. Its new parent node is the node with the lowest path cost. The path cost of a node is the sum of the path length from the root node q_init to this node and the path length from this node to the new node q_new1. Sixthly, all nodes in the neighborhood of the new node q_new1 are rewired. The new node q_new1 is taken as the parent node of these nodes and then their new path costs are calculated. If the new path costs are less than the old path costs, the parent nodes of these nodes are changed to the new node q_new1. On the contrary, the parent nodes of these nodes remain unchanged. Seventhly, the connectivity of the two trees is checked, as shown in Algorithm 4. If the distance between the new node q_new1 and some node of the tree T₂ is less than the connection threshold, the two trees have been connected to each other and then the extensions of two trees are stopped. If the distance between the new node q_new1 and any node of the tree T₂ is greater than the connection threshold, the tree T₂ is extended towards the new node q_new1 until there is a collision or the two trees are connected to each other. In the case that there is a collision, the tree T₂ is switched to perform the above steps. Finally, in the case that the extensions of two trees are completed, if the two trees are connected to each other, the desired path can be extracted from the two trees and it will be the output of the algorithm. Otherwise, the empty set is directly returned as the output of the algorithm.

Algorithm 3 qnew ← Steer(qtarget, qnear)

($q_{target}^l$, $q_{target}^r$) ← Decompose(qtarget); ($q_{near}^l$, $q_{near}^r$) ← Decompose(qnear); $q_{new}^l$ ← Expand($q_{target}^l$, $q_{near}^l$); $\dot{\mathrm{q}}$l ← ($q_{new}^l$ − $q_{near}^l$)/t; $\dot{\mathrm{q}}$r ← Constraint($\dot{\mathrm{q}}$l, $q_{near}^l$, $q_{near}^r$); $q_{new}^r$← $q_{near}^r$ + t∙$\dot{\mathrm{q}}$r; qnew ← Merge($q_{new}^l$, $q_{new}^r$);

Algorithm 4 (flag, qe1, qe2) ← JudgeConnectivity(qnew, T)

qe1 ← qnew; Fori = 1 To k  qi ← ExtractNode(T, i);  If  Distance(qnew, qi) < threshold   flag ← true;   qe2 ← qi;   Break;  Else   flag ← false;  End If End For

In the expansion phase, for single redundant manipulators, a new node q_new can be obtained directly by extending a step distance δ forward in the direction from the node q_near to the sampling point q_rand. However, for dual redundant manipulators, considering the closed-chain constraints (1)–(5) between the left manipulator and the right manipulator in tight coordination, a new node q_new cannot be obtained by the expansion mode of single redundant manipulators. Therefore, the master-slave planning method is adopted to extend the trees of dual redundant manipulators. It is assumed that the left manipulator is the master manipulator and the right manipulator is the slave manipulator. A new set of joint angles $q_{new}^l$ of the left manipulator is generated by the expansion mode of single redundant manipulators. Then, the corresponding set of joint angles $q_{new}^r$ of the right manipulator can be calculated according to the closed-chain constraints. Lastly, a new node q_new of the dual redundant manipulator can be obtained by merging the two sets of joint angles $q_{new}^l$ and $q_{new}^r$.

The closed-chain constraints (1)–(5) of the dual redundant manipulator are the constraints between the terminal states of the left manipulator and the right manipulator. Therefore, the general execution process of the master-slave planning method is described as follows. After obtaining the set of joint angles of the left manipulator, the terminal position and posture of the left manipulator are calculated through forward kinematics. Then, the terminal position and posture of the right manipulator can be calculated through the closed-chain constraints (1) and (2). Finally, the set of joint angles of the right manipulator is solved by inverse kinematics [28,29]. The numbers of degrees of freedom of the left manipulator and the right manipulator are both greater than 6. It results in the calculation of the analytic solution of inverse kinematics being very tedious. Hence, the inverse kinematics is usually solved by numerical algorithms. Numerical algorithms are generally based on the relationship (7) between the terminal generalized velocity of the redundant manipulator and its joint angular velocity. The joint angular velocity of the redundant manipulator is calculated by the numerical algorithm, and then its corresponding set of joint angles can be calculated accordingly [30,31].

$$\dot{q}=J^{+}\left(q\right)V+\left[I_{n\times n}-J^{+}\left(q\right)J\left(q\right)\right]\epsilon$$

(7)

where $V$ is the terminal generalized velocity vector of the redundant manipulator, $J^{+}\left(q\right)$ is the pseudo inverse of the Jacobian matrix and $\epsilon$ is a vector in the null space.

The closed-chain constraints (3)–(5) of the dual redundant manipulator are essentially the constraints between the terminal generalized velocities of the left manipulator and the right manipulator, but they have been transformed into the constraints between the joint angular velocities of the left manipulator and the right manipulator through the Jacobian matrix. To simplify the calculation of the set of joint angles of the right manipulator, the joint angular velocity of the left manipulator is calculated after obtaining the set of joint angles of the left manipulator. Then, the joint angular velocity of the right manipulator can be calculated through the closed-chain constraints (3)–(5), and the set of joint angles of the right manipulator can be calculated accordingly, as shown in Algorithm 3. This method can avoid the calculation of the inverse kinematics solution and can effectively improve the planning efficiency and shorten the planning time. The specific steps are described as follows:

(I)

In the direction from the node $q_{near}^l$of the left manipulator to its temporary target point $q_{\mathit{target}}^l$, the new node $q_{new}^l$ can be obtained by extending a step distance δ forward, as described below:

$$q_{new}^l=q_{near}^l+\delta$$

(8)

where $q_{new}^l$ is the new node of the left manipulator and $q_{near}^l$ is the node closest to the temporary target point $q_{target}^l$ of the left manipulator.

(II)

The motion time t between two adjacent nodes of the dual redundant manipulator is given. Assuming that the left manipulator moves at a uniform speed between the two adjacent nodes in the joint space, the joint angular velocity $\dot{q}$^l of the left manipulator can be calculated by Equation (9).

$${\dot{q}}^l=\frac{q_{new}^l-q_{near}^l}{t}$$

(9)

where ${\dot{q}}^l$ is the joint angular velocity of the left manipulator and $t$ is the motion time between two adjacent nodes of the dual redundant manipulator.

(III)

According to the closed-chain constraint (5), the joint angular velocity $\dot{q}$^r of the right manipulator is calculated.

(IV)

Similarly, assuming that the right manipulator moves at a uniform speed between the two adjacent nodes in the joint space, the new node $q_{new}^r$ of the right manipulator can be calculated by Equation (10).

$$q_{new}^r=q_{near}^r+t·{\dot{q}}^r$$

(10)

where $q_{new}^r$ is the new node of the right manipulator, $q_{near}^r$ is the node closest to the temporary target point $q_{target}^r$ of the right manipulator and ${\dot{q}}^r$ is the joint angular velocity of the right manipulator.

(V)

The new node $q_{new}^l$ of the left manipulator and the new node $q_{new}^r$ of the right manipulator are merged to obtain the new node q_new = [$q_{new}^l$ $q_{new}^r$] of the dual redundant manipulator.

To ensure that the new node q_new can satisfy the closed-chain constraints (1) and (2), the step distance δ and the motion time t should not be too large. That is, the motion between the two adjacent nodes should be a differential motion.

2.3. Local Path Replanning

The connection condition of two trees is that the distance between some node q_1i of the tree T₁ and some node q_2j of the tree T₂ is less than the given threshold, as shown in Algorithm 4. In the case that B-RRT* is applied to dual redundant manipulators, its node q contains both the set of joint angles q^l of the left manipulator and the set of joint angles q^r of the right manipulator. In the actual planning, when the distance between two sets of joint angles of the left manipulator (or the right manipulator) in different trees is less than the given threshold, the distance between two sets of joint angles of the right manipulator (or the left manipulator) in different trees may be not less than the given threshold in a high probability. That is, it is difficult to meet the connection condition of two trees. Therefore, it is necessary to modify the connection condition of two trees for solving the above problem.

Since it is difficult for the left manipulator and the right manipulator to meet the connection condition at the same time, it can first ensure that the parts corresponding to the left manipulator in two nodes of different trees are connected with each other, as shown in Algorithm 5. Then, the local path between the parts corresponding to the right manipulator in two nodes of different trees is replanned. That is, while keeping the left manipulator stationary, the right manipulator moves from the set of joint angles corresponding to the connection node of the tree T₁ (or T₂) to the set of joint angles corresponding to the connection node of the tree T₂ (or T₁). Moreover, to satisfy the closed-chain constraints (1) and (2), the terminal position and posture of the right manipulator must always remain unchanged during the movement.

Algorithm 5 (flag, qe1, qe2) ← NewJudgeConnectivity(qnew, T)

qe1 ← qnew; ($q_{new}^l$, $q_{new}^r$) ← Decompose(qnew); Fori = 1 To k  qi ← ExtractNode(T, i);  ($q_i^l$, $q_i^r$) ← Decompose(qi);  If  Distance($q_{new}^l$, $q_i^l$) < threshold   flag ← true;   qe2 ← qi;   Break;  Else   flag ← false;  End For End For

To ensure that the terminal position and posture of the right manipulator remains unchanged during the local path replanning, it can be realized through the joint self-motion in the null space [34,35] on the basis of the characteristics of redundant manipulators. That is, the joint angular velocity of the right manipulator is calculated by setting the terminal generalized velocity V in Equation (7) to zero, as described below:

$${\dot{q}}^r=\left(I_{n\times n}-J^{+}\left(q^r\right)J\left(q^r\right)\right){\epsilon }^r$$

(11)

where ${\epsilon }^r$ is a vector in the null space of the right manipulator.

To ensure that the right manipulator can start from the starting point $q_{localstart}^r$ of the local path and move towards the end point $q_{localgoal}^r$ of the local path, the value of vector ε^r can be calculated by Equation (12).

$${\epsilon }^r=\frac{\left(q_{localgoal}^r-q_k^r\right)}{\left(m-k+1\right)t}$$

(12)

where $m=\frac{\left(q_{localgoal}^r-q_{localstart}^r\right)}{\delta }$, $k=1\sim m$, $q_{localstart}^r$ is the starting point of the local path of the right manipulator, $q_{localgoal}^r$ is the end point of the local path of the right manipulator, $m$ is the upper limit of the number of discrete points in the local path of the right manipulator and $k$ is the serial number of the current discrete point in the local path of the right manipulator.

The process of local path replanning is shown in Algorithm 6. In addition, the extension process of bidirectional RRT* with local path replanning (B-RRT*-LPR) is shown in Algorithm 7.

Algorithm 6 (T, success, qe1) ← Localpath(qe1, qe2, T)

success ← false; ($q_{localstart}^l$, $q_{localstart}^r$) ← Decompose(qe1); ($q_{localgoal}^l$, $q_{localgoal}^r$) ← Decompose(qe2); q1 ← qe1; $q_1^r$ ← $q_{localstart}^r$; Fork = 1 To m − 1  $J_k^r$ ← Jacobian($q_k^r$);  ${\epsilon }_k^r$ ← NullSpaceVector($q_k^r$, $q_{localgoal}^r$, k);  ${\dot{q}}_k^r$ ← Velocity($J_k^r$, ${\epsilon }_k^r$);  $q_{k+1}^r$ ← $q_k^r$ + t ${\dot{q}}_k^r$  $q_{k+1}$ ← Merge($q_{localstart}^l$, ${\mathrm{q}}_{k+1}^r$r k;  feasibility ← DetectCollision(qk+1);  If feasibility = true   T ← Insert(qk+1, qk);   qk ← qk+1;   If  Distance($q_{k+1}^r$, $q_{localgoal}^r$) < threshold    qe1 ← qk+1;    success ← true;    Break;   End If  End If End For

The specific steps of local path replanning are described as follows:

(a)

The parts corresponding to the right manipulator in the connection node of the tree T₁ (or T₂) are selected as the starting point $q_{localstart}^r$ of the local path. The parts corresponding to the right manipulator in the connection node of the tree T₂ (or T₁) are selected as the end point $q_{localgoal}^r$ of the local path. The first discrete point $q_1^{\mathrm{r}}$of the local path is $q_{rlocalstart}^r$.

(b)

The Jacobian matrix $J_k^r$ and the vector ${\epsilon }_k^r$ of the current discrete point $q_k^r$ are calculated. Then, the joint angular velocity ${\dot{q}}_k^r$of the right manipulator is calculated according to Equation (11).

(c)

The next discrete point $q_{k+1}^r$ is calculated by Equation (13).

$$q_{k+1}^r=q_k^r+t\cdot {\dot{q}}_k^r$$

(13)

where $q_{k+1}^r$ is the next discrete point of the local path of the right manipulator, $q_k^r$ is the current discrete point of the local path of the right manipulator and ${\dot{q}}_k^r$ is the joint angular velocity of the right manipulator at the current discrete point of the local path.

Algorithm 7 (T1, flag, qe1, qe2) ← NewExtend(T1, T2)

flag ← false; qrand ← Sample(qlimit); qnear1 ← Near(qrand, T1); qnew1 ← Steer(qrand, qnear1); feasibility ← DetectCollision(qnew1); Iffeasibility = true  T1 ← Insert(qnew1, qnear1);  T1 ← ReselectParentNode(qnew1, qnear1, T1);  T1 ← Rewire(qnew1, T1);  (flag, qe1, qe2) ← NewJudgeConnectivity(qnew1, T2);  If flag = true   (T1, flag, qe1) ← Localpath(qe1, qe2, T1);  End If  While feasibility = true And flag = false   qnear2 ← Near(qnew1, T2);   qnew2 ← Steer(qnew1, qnear2);   feasibility ← DetectCollision(qnew2);   If feasibility = true    T2 ← Insert(qnew2, qnear2);    T2 ← ReselectParentNode(qnew2, qnear2, T2);    T2 ← Rewire(qnew2, T2);    (flag, qe2, qe1) ← NewJudgeConnectivity(qnew2, T1);    If flag = true     (T2, flag, qe2) ← Localpath(qe2, qe1, T2);    End If   End If  End While End If

(d)

The set of joint angles $q_{localstart}^l$ of the left manipulator corresponding to the starting point $q_{localstart}^r$ of the local path of the right manipulator is merged with the next discrete point $q_{k+1}^r$ of the local path of the right manipulator to obtain the next discrete point q_k+1 = [ $q_{llocalstart}^l q_{k+1}^r$] of the local path of the dual redundant manipulator.

(e)

The collision detection of the next discrete point q_k+1 of the local path of the dual redundant manipulator is carried out. If the dual redundant manipulator does not collide, step (f) is performed. If the dual redundant manipulator collides, the local path replanning is stopped and the result that the node q_localstart = [q $q_{localstart}^r$] and the node q_localgoal = [$q_{localgoal}^l$ $q_{localgoal}^r$] cannot be connected with each other is returned.

(f)

The next discrete point q_k+1 of the local path of the dual redundant manipulator is added to the tree T₁ (or T₂).

(g)

The distance between the next discrete point $q_{k+1}^r$of the local path of the right manipulator and the end point$q_{rlocalgoal}^r$ of the local path of the right manipulator is judged to determine whether it is less than the given threshold. Alternatively, the serial number k of the current discrete point is judged to determine whether it is greater than the upper limit m of the number of discrete points. If the distance is less than the given threshold or the serial number k is greater than the upper limit m, the local path replanning is stopped and the result that the node q_localstart and the node q_localgoal can be connected with each other is returned. If the distance is not less than the given threshold value and the serial number k is not greater than the upper limit m, the current discrete point of the local path of the right manipulator is replaced by $q_k^r$+1, and then step (b) is performed again.

3. Results

To verify the effectiveness of B-RRT*-LPR for dual redundant manipulators, the Baxter dual redundant manipulator is taken as the experimental object to plan its motion path of tight coordination operations. The experimental platform is built for the situation of collaborative transportation, as shown in Figure 2. There are two obstacles in the environment. Obstacle 1 is a dark yellow paper box suspended in the air, and obstacle 2 is a white storage box placed on the desktop. The workpiece is an industrial aluminum profile whose length, width and height are 0.235 m, 0.04 m and 0.04 m, respectively. A simulation platform, whose size is consistent with the actual size, is built accordingly, as shown in Figure 3. In Figure 3, the yellow cuboids are the obstacles. Then, the simulations are carried out by Matlab 2014a, which runs on a computer with an Intel(R) Core(TM) i7-4710HQ quad-core processor, 2.50 GHz main frequency and 8G memory. Finally, the experiment is carried out according to the simulation results.

3.1. Simulation

Considering the randomness of sampling-based algorithms, B-RRT* and B-RRT*-LPR are adopted to plan 10 groups of paths in the case that the connection threshold is 1°, 5°, 10°, 15°, 20°, 25° and 30°, respectively. The planning results are arranged in ascending order according to the time cost of each group of paths, as shown in Figure 4. The maximum number of iterations of the two algorithms is set to 500. That is, if no feasible path is obtained after 500 iterations, the planning fails.

The maximum time cost, minimum time cost, average time cost and success rate of 10 groups of planning results of the 2 algorithms with different connection thresholds are shown in

Table 1. Planning results of B-RRT*.

	tmax (s)	tmin (s)	tavg (s)	sr
Th: 1 (°)	5294.087	4323.168	4855.917	0%
Th: 5 (°)	5376.889	4379.025	4838.732	0%
Th: 10 (°)	5283.419	4359.348	4885.978	0%
Th: 15 (°)	5197.209	4097.994	4815.204	0%
Th: 20 (°)	5316.107	4438.916	4811.795	20%
Th: 25 (°)	4902.081	1856.069	2972.804	80%
Th: 30 (°)	1414.534	963.751	1106.657	100%

where t_min, t_max and t_avg are the minimum, maximum and average time costs of 10 groups of paths, sr is the success rate of 10 groups of paths and Th is the connection threshold that is used to judge the connectivity of two trees.

and

Table 2. Planning results of B-RRT*-LPR.

	tmax (s)	tmin (s)	tavg (s)	sr
Th: 1 (°)	1355.623	756.654	1066.326	100%
Th: 5 (°)	1264.872	805.944	1006.879	100%
Th: 10 (°)	1279.273	784.542	976.011	100%
Th: 15 (°)	1166.911	682.488	872.420	100%
Th: 20 (°)	1017.361	619.926	795.953	100%
Th: 25 (°)	984.236	601.268	761.544	100%
Th: 30 (°)	965.477	534.450	708.229	100%

.

It can be seen from Figure 4 and Table 1 and Table 2 that the success rate of B-RRT* is 0% in the case that the connection threshold is less than or equal to 15°. With the increase of the connection threshold, the success rate gradually increases until it reaches 100% when the connection threshold is 30°. However, the success rate of B-RRT*-LPR is 100% in the case that the connection threshold is any one of the seven given values. Therefore, the results show that the local path replanning strategy can effectively solve the discontinuity problem in the joint path of the slave manipulator at the connection nodes in the case that B-RRT* is applied to dual redundant manipulators. It can significantly improve the success rate.

The success rate of B-RRT* is 100% in the case that the connection threshold is 30°, but its average time cost is 1106.657 s. On the contrary, with the same connection threshold, B-RRT*-LPR can ensure that the success rate is 100%, and its average time cost is 708.229 s, which is 36.00% lower than the one of B-RRT*. Therefore, B-RRT*-LPR can effectively shorten the planning time.

In the case that B-RRT* fails to plan, its time cost is always more than 4000 s. That is, for B-RRT*, the time cost of 500 iterations is more than 4000 s. Therefore, in the case that the maximum number of iterations is greater than 500, although it is possible to improve the success rate, the time cost will be further increased. That is, if the number of iterations exceeds 500, B-RRT* may successfully plan the desired path, but the time cost must exceed 4000 s. However, B-RRT*-LPR spends no more than 1400 s successfully planning the desired path with any connection threshold. Therefore, instead of increasing the maximum number of iterations to improve the success rate of B-RRT*, it is better to directly adopt B-RRT*-LPR for path planning.

In addition, although B-RRT* can improve the success rate by increasing the connection threshold, the increase of the connection threshold means that the difference between the connection nodes of two trees will also become larger. When the connection threshold is much larger than the step distance, the local path between the two connection nodes essentially contains some other hidden nodes. These hidden nodes have not been detected whether they would collide with obstacles. Hence, they may collide with obstacles. As a result, the planned path may actually be infeasible. For B-RRT*-LPR, it can ensure that the success rate is 100% with any connection threshold. Therefore, a smaller connection threshold can be selected in the actual planning to ensure that B-RRT*-LPR will not have the above problem.

3.2. Experiment

The most representative path in the 10 groups of planning results of B-RRT*-LPR is chosen in the case that the connection threshold is 1°. Its time cost is closest to the average time cost of 10 paths. That is, the fifth path is chosen. The display of this path in the task space is shown in Figure 5. In Figure 5, the green hollow dot represents the node of two trees, the blue thin solid line represents the local path between two nodes and the red thick solid line represents the final path. The yellow cuboid suspended above is obstacle 1, and the yellow cuboid placed below is obstacle 2.

According to the path shown in Figure 5, the motion process of the dual redundant manipulator can be described as follows. At the initial stage of motion, the dual redundant manipulator is far away from obstacles 1 and 2. Hence, it moves directly towards the goal state. When the dual redundant manipulator approaches obstacle 1, it moves downward to avoid collision with obstacle 1. After moving a certain distance, the dual redundant manipulator moves towards the goal state again on the premise that it does not collide with obstacles 1 and 2. After successfully passing through the gap between obstacle 1 and obstacle 2, the dual redundant manipulator moves upward until its height is close to the height of the goal state. Then, the dual redundant manipulator moves towards the goal state until it reaches the goal state.

In the case that the dual redundant manipulator moves along the path shown in Figure 5, its actual motion process is shown in Figure 6.

It can be seen from Figure 6 that the dual redundant manipulator starts from the initial state, as shown in Figure 2a. Then, the dual redundant manipulator moves to the vicinity of obstacle 1 in the direction of the goal state, as shown in Figure 6a. The state of the dual redundant manipulator in Figure 6a corresponds to the connection node of the tree T₁. Afterwards, the dual redundant manipulator moves along the local path planned by the local path replanning strategy until it reaches the state corresponding to the connection node of the tree T₂, as shown in Figure 6b. The terminal positions and postures of both the left manipulator and the right manipulator remain unchanged during the local motion shown in Figure 6a,b. After that, the dual redundant manipulator moves downward for a distance and then passes through the gap between obstacle 1 and obstacle 2 without collision, as shown in Figure 6c,d. Finally, the dual redundant manipulator can reach the goal state successfully, as shown in Figure 2b.

The above results show that the dual redundant manipulator can move according to the desired path and satisfy the closed-chain constraints in the actual motion. The actual path of the dual redundant manipulator is continuous in both the task space and the joint space. Therefore, B-RRT*-LPR can effectively plan a collision-free path for tight coordination operation of the dual redundant manipulator.

4. Discussions

4.1. Innovations

For RRT and its derivative algorithms, they are generally applied to plan the path for loose coordination of dual redundant manipulators, such as assembly [36,37] and grasp [38,39]. In these cases, there are three most common planning strategies, as described as follows: (1) RRT or its derivative algorithm is adopted to plan the paths of the left manipulator and the right manipulator in parallel. After obtaining the paths of the two manipulators, collision detection between the two paths is performed [23,24]. (2) RRT or its derivative algorithm is adopted to plan the path of one manipulator first, and then the path of another manipulator. During the planning of the second manipulator, the first manipulator is treated as a dynamic obstacle [19]. (3) The dual redundant manipulator is treated as a single redundant manipulator with 2n degrees of freedom. Then, the path of this single redundant manipulator is planned by RRT or its derivative algorithm. Collision detection between the left manipulator and the right manipulator can be transformed into self-collision detection of the single redundant manipulator [20].

For tight coordination of dual redundant manipulators, there are kinematic closed-chain constraints between the left manipulator and the right manipulator. The terminal positions and postures of the left manipulator and the right manipulator are mutually restricted. However, these kinematic closed-chain constraints are not considered for the above three planning strategies. Therefore, the above three planning strategies are not applicable to tight coordination of dual redundant manipulators. To solve this problem, a master-slave planning method based on B-RRT* is proposed in our study. The path of the master manipulator is directly planned by B-RRT*. Then, the path of the slave manipulator is calculated by the closed-chain constraints. The experimental and simulation results show that the closed-chain constraints can always be satisfied during the motion of the dual redundant manipulator. Therefore, the master-slave planning method based on B-RRT* can be effectively applied to tight coordination of dual redundant manipulators.

Due to the redundant characteristics of dual redundant manipulators, the master-slave planning method based on B-RRT* will cause a discontinuity problem in the joint path of the slave manipulator at the connection nodes. This problem will result in the actual motion path being infeasible. To solve this problem, a local path replanning strategy is designed in our study. The joints of the slave manipulator move only in the null space for satisfying the closed-chain constraints. The experimental and simulation results show that the dual redundant manipulator can move continuously in both the task space and the joint space. Therefore, the local path replanning strategy can effectively solve the discontinuity problem.

Moreover, since the closed-chain constraints act on the terminal state of the dual redundant manipulator, the calculation of the path of the slave manipulator is generally to calculate its terminal position and posture first, and then calculate its joint angles through inverse kinematics [28,29]. For redundant manipulators, the inverse kinematics is usually solved by numerical algorithms [30,31]. However, numerical algorithms generally have the disadvantages of large amounts of calculation and high time cost [40]. To avoid the calculation of inverse kinematics, the terminal generalized velocity constraints are directly adopted to obtain the joint angular velocity of the slave manipulator in our study. Then, the joint angles of the slave manipulator can be calculated accordingly. Therefore, it can effectively improve the execution efficiency.

4.2. Extended Applications

In our study, the master-slave planning method on B-RRT* is proposed to plan the path for tight coordination of dual redundant manipulators. However, for the path planning of the master manipulator, B-RRT* is not the only choice; other algorithms, such as other derivative algorithms [41,42] of RRT and PRM [43,44], or an artificial potential field method [45,46], can also be chosen.

The local path replanning strategy can also be applied to realize the obstacle avoidance [47,48] of redundant manipulators. In the case that the end-effector does not collide with obstacles, but some joints collide with obstacles, the local path replanning strategy can be adopted to make these joints move away from or around obstacles on the premise that the end-effector remains stationary. In this way, it is not necessary to replan the entire path, but only to replan the local path of joints.

4.3. Limitations

Both the master-slave planning method based on B-RRT* and the local path replanning strategy take advantage of the unique kinematic characteristics of redundant manipulators in the design process. Therefore, the proposed methods are not applicable to dual nonredundant manipulators, but only to dual redundant manipulators.

5. Conclusions

To effectively plan a safe path for tight coordination operations of dual redundant manipulators, a bidirectional RRT* satisfying closed-chain constraints is proposed. B-RRT* is adopted to plan the path of the master manipulator, and the path of the slave manipulator is then obtained based on the closed-chain constraints. When the connectivity between the two nodes of different trees is checked, only the parts corresponding to the master manipulator are checked. The connectivity between the parts corresponding to the slave manipulator is guaranteed by the local path replanning strategy. The simulation results show that, compared with B-RRT*, the proposed method can effectively improve the success rate when the connection threshold is small and can shorten the planning time when the connection threshold is the same. The experimental results show that the dual redundant manipulator can effectively avoid obstacles and satisfy the closed-chain constraints during the motion. Therefore, the proposed method can effectively plan a collision-free path for tight coordination operations of dual redundant manipulators.

Our current study is the path planning for tight coordination of dual redundant manipulators in the static environment. That is, obstacles are stationary. The positions and postures of obstacles are known. However, if obstacles are in motion, their positions and postures will change in real time and are unknown. Hence, to apply the proposed method to the dynamic environment, it is necessary to add some external equipment to obtain the real-time positions and postures of obstacles. In the future, we will use visual sensors to obtain the information of obstacles in real time and then extract the positions and postures of obstacles through image processing technologies. On the basis of the above, the proposed method can be adopted to plan the path for tight coordination of dual redundant manipulators in the dynamic environment.