An Informed-Bi-Quick RRT* Algorithm Based on Offline Sampling: Motion Planning Considering Multiple Constraints for a Dual-Arm Cooperative System

Abstract

Aiming to address problems such as low sampling success rate and long computation time in the motion planning of a dual-arm cooperative system with multiple constraints, this paper proposes an Informed-Bi-Quick RRT* algorithm based on offline sampling. First, in the process of pre-sampling, the new algorithm relaxes the approximation of constrained manifolds by introducing the idea of incremental construction, and it incorporates the stochastic gradient descent method to replace global random sampling with local random sampling, which enriches the data set and shortens the offline sampling time of the data set. Second, the new algorithm improves the original Quick-RRT* algorithm by combining the two-tree idea and the multi-target bias expansion strategy, and it improves the adaptability of the algorithm to different obstacle environments. In addition, the loosely constrained motion and tightly constrained motion in the two-arm cooperative system are analyzed, and the adaptive planning of the two-arm trajectory in different motions is described in detail. In this paper, the two-arm cooperative model constructed with UR5 and UR10 robot arms is studied, and the ability of the proposed algorithm to deal with multiple constraints is verified by simulating assembly and handling tasks. The experimental results show that compared with other methods, the proposed algorithm has obvious advantages in path quality and planning efficiency.

1. Introduction

The basis of the coordinated motion realization system is to perform reasonable motion planning according to the operation scene [1,2]. With the continuous expansion of the application scenarios of the two-armed cooperative system, increasing numbers of task constraints are faced, such as end-effector constraints, closed-loop constraints, and geometric constraints [3]. Planning complex cooperative systems is an important and specific problem when many constraints need to be considered in special cases. At present, the mainstream motion-planning methods [4,5] that deal with a single constraint are relatively common and effective, but many algorithms have certain limitations in the face of dealing with multiple constraints [6,7,8,9]. For example, methods based on Inverse Kinematics (IK) [10] are traditional methods for dealing with end-effector constraints of redundant robotic arms that can directly evaluate constraints and sample target poses. In the face of multiple end-effector constraints, the IK-based response control method [11,12] relaxes the hard geometric constraints to soft ones based on cost through nonlinear optimization. Under operator supervision, this optimization is feasible. Optimizer-based programming methods [13,14] are effective for everyday operational tasks, adding kinematic constraints to the optimizer to generate a trajectory that follows a given path. Long et al. [15] formed a constrained operable polyhedron that help generate trajectories in optimization-based motion planners. However, these two types of algorithms share the same problem of being easily trapped in local minima and having difficulty dealing with complex paths with multiple constraints [16,17].

Sampling-based planning methods [18,19,20,21,22] are preferred for solving obstacle constraints in high-dimensional configuration spaces due to their adaptability. However, when faced with multiple task constraints, sampling planning can present challenges. The main difficulty arises from the requirement that the configuration satisfies the constraint function, which defines the manifold in the configuration space. Sampling-based motionplanning algorithms often struggle to uniformly sample the configuration space and obtain constrained configurations.

The planning methods based on multi-constraint sampling include projection, continuous, and offline sampling [23]. The projection-based method finds the configuration that satisfies the constrained function by solving the system of constrained equations. The projection operator then obtains the configuration and maps it to the implicit manifold. This method was first used by Yakey et al. [24] for the closed-loop control of a parallel manipulator. The authors apply the projection idea to general end-effector constraints and optimize the projection process using stochastic gradient descent and Jacobian gradient descent. For instance, Berenson et al. [25] combine the Jacobian matrix projection method with C-space sampling to move the sample onto the constrained manifold and solve the motion-planning problem in high-dimensional space with end-effector attitude and joint torque constraints. Recently implemented planners can solve complex combinations of constraints using projections with general constraints. For example, humanoid path planner (HPP) [26,27] systems combine explicit and implicit manifold constraints into more efficient projections. Projection enforces strict adherence to the constraints, but it can result in a lack of available information, and the iterative projection process consumes significant computational resources, leading to unacceptably long computation times.

The continuation-based method generates the tangential space of an implicit manifold from a known target configuration. By applying the projection to a configuration sampled within the tangential space, an efficient local motion can be generated. The tangent spaces are grouped together to create piecewise linear approximations or continuations of the manifold, resulting in a new configuration close to the target manifold [28]. The study utilized early manipulators constrained by general end-effectors [29] and redundant manipulators constrained by curve tracking [30]. To compute the solution of a constrained manifold, some methods [31,32] combined with numerical continuation techniques define the atlas as a piecewise linear approximation of a constrained manifold in the tangent space. This is achieved by gradually building an atlas interleaved with the space, allowing planners to explore or reuse the results of previous runs online. However, linear approximation, or continuation, also has clear disadvantages. In addition to the expensive calculation of the matrix, when the manifold becomes highly curved, the tangential motion quickly deviates from the surface of the manifold, and the tangential space will not be reasonable.

In general, the above two algorithms can only be designed for or applied to relatively simple problems or specific robot arm structures, and it is difficult to provide general solutions for general motion-planning problems.

Therefore, this paper proposes an optimized off-line sampling-based motion-planning method for multi-constrained dual-arm cooperative systems. The offline sampling-based approach [33,34,35] is a configuration that precomputes a set of constraints and then uses this set for sampling and local planning. At its core, it is possible to construct an approximation of a constrained manifold offline and then use this approximation to plan directly on the constrained manifold rather than in the entire configuration space. The advantage is that constrained manifolds can be quickly sampled online using data structures computed offline, rather than using relatively slow dedicated sampling algorithms to sample the configuration space. To address the issues of low sampling success rate and the slow generation speed of offline data sets, this paper proposes the idea of relaxing constraints during the sample set generation process. The paper establishes constraint tolerances and task errors, and controllably relaxes the range of constrained manifold approximation. This approach increases the number of possible pre-sampling configurations, resulting in an improved success rate of subsequent sampling. Secondly, local random sampling is used instead of pure random sampling. Additionally, a random gradient descent method is employed to prioritize the presence of a matching new configuration within a certain range of collected target configurations. This helps to shorten the construction time of the sample data set. In addition, this paper enhances the original Quick-RRT* algorithm by combining the two-tree concept and multi-objective biasing expansion strategy. This optimization improves the initial path and accelerates the convergence speed while ensuring asymptotic optimization. Additionally, it enhances the algorithm’s adaptability to different obstacle environments. Finally, this paper analyzes the difference in task constraints in loosely constrained motion and tightly constrained motion (taking assembly and handling as examples, respectively) under collaborative tasks, and describes the influence of different task constraints on the motion planning of both arms in detail. In addition, under master–slave control, the optimized planning method based on advance sampling is used to realize the assembly and handling of dual robot arms.

The structure of this thesis is as follows: The second section introduces a review of work related to constrained motion planning; the third section describes, in detail, how to optimize the motion-planning method based on offline sampling; the fourth section analyzes the task constraints present in collaborative systems, and the fifth section presents various experimental results and discusses our approach and other previous work. The sixth section contains the conclusion.

2. Method

2.1. Problem Description

Since the constraints of the end pose of the manipulator and the obstacles in the environment are treated as separate entities during the motion-planning process, it is possible to acquire some prior knowledge about the constraint manifold formed by the constrained end pose before the motion planning.

In this paper, we opt to create an offline sampling set beforehand, which includes numerous configurations of pose constraints, to provide an approximate description of the constraint manifold. The standard sampling-based motion-planning algorithm explores the high-dimensional configuration space through random sampling and collision detection to locate a collision-free path. In the unconstrained situation, a manipulator with six degrees of freedom, the sampling space for the entire configuration space, can be defined as $Q$. However, in the scene with final pose constraints, the configuration that satisfies both the pose constraints and the obstacle avoidance constraints is usually distributed over a lower-dimensional manifold.

The initial configuration space $Q$ is reduced to a two-dimensional surface, referred to as the constraint manifold. This surface has a volume almost equal to zero within the three-dimensional space. The function that evaluates whether q satisfies the constraint in the configuration space $Q$ can be defined as $F\left(q\right)$ (when $F\left(q\right)=0$, q satisfies the constraint). The constraint manifold defined by the constraint function $F\left(q\right)$ can be defined as X:

$$X=\left\{q\in Q|F\left(q\right)=0\right\}.$$

(1)

The problem of planning motion for a manipulator while taking into account the end pose constraint is defined as follows:

The starting configuration is denoted as $q_{\mathrm{start} }$, whereas the target configuration is referred to as $q_{\mathrm{goal} }$. Both $q_{\mathrm{start} }$ and $q_{\mathrm{goal} }$ are on the constraint manifold X. A path is determined on the constraint manifold X that leads from $q_{\mathrm{start} }$ to $q_{\mathrm{goal} }$, while ensuring that the manipulator remains collision-free as it traverses this path through the environment.

2.2. Constraint Tolerance and Task Error

The main concept of this approach is to construct an offline sampling dataset that contains a significant number of configurations satisfying the constraints. This sampling dataset is then used to provide an approximate description of the constraint manifold. The key to judging whether the data set satisfies the constraint is how to correctly measure the concept of approximation. In this paper, this is measured by constraining the tolerance ϵ with the task error $\mathsf{\Delta }{\mathrm{x}}_{\mathrm{err}}$.

The constraint tolerance denotes the acceptable range for linear approximation. If the curvature of the constraint manifold is finite, the random expansion tree’s step length is relatively brief, and the constraint manifold can be approximated through piecewise linear approximation. However, it is crucial to ensure that the linear approximation falls within the acceptable range when transitioning from the previous configuration which meets the constraint to the subsequent configuration meeting the constraint in the constraint manifold. The range is defined as the tolerance ε, and the constraint manifold X changes accordingly:

$$X=\left\{q\in Q|F\left(q\right)<ϵ\right\}.$$

(2)

The task error refers to the distance between the obtained configuration $q_s$ and the defined constraint manifold. Typically, the manipulator’s end-effector’s degree of freedom is determined by rotation and translation. The transformation of the coordinate system $F^B$ relative to the coordinate system $F^A$ can be expressed by the homogeneous matrix $T_B^A$:

$$T_B^A=\left[\begin{array}{cc}{R}_B^A& t_B^A\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}{n}_x& 0_x& a_x& p_x\\ n_y& 0_y& a_y& p_y\\ n_z& 0_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right].$$

(3)

The six-dimensional vector $d_e^0$ of the end pose information of the manipulator can be expressed as:

$$d_e^0={[\mathrm{O} \mathrm{R}]=[\mathrm{x}, \mathrm{y}, \mathrm{z}, \mathrm{R}, \mathrm{P}, \mathrm{Y}]}^{\mathrm{T}}.$$

(4)

Therefore, the end constraint of the manipulator $c$ in the task space can be expressed as:

$$c=\left[\begin{array}{cc}{x}_{min}& x_{max}\\ y_{min}& y_{max}\\ z_{min}& z_{max}\\ R_{min}& R_{max}\\ P_{min}& P_{max}\\ Y_{min}& Y_{max}\end{array}\right],$$

(5)

Then the task errors $\mathsf{\Delta }{\mathrm{x}}_{\mathrm{err}}$ between the configuration and the constraint manifold can be defined as:

$$\mathsf{\Delta }{x}_{\mathrm{err}\left(i\right)}=\left\{\begin{array}{cc}{d}_e^0\left(i\right)-c_{i_{max}}& \mathrm{if} d_e^0\left(i\right)>c_{i_{max}}\\ d_e^0\left(i\right)-c_{i_{min}}& \mathrm{if} d_e^0\left(i\right)<c_{i_{min}}\\ 0& \mathrm{otherwise} \end{array}\right..$$

(6)

2.3. Construction of Offline Sampling Set

In this paper, an offline sampling set containing a large number of pose constraint configurations is constructed in advance to approximately describe the constraint manifold. The fundamental concept behind forming the sampling set is to randomly create the initial configuration $q_{\mathrm{rand}}$ in the joint space and assess whether or not it is a potential target configuration $q_s$. By using the idea of continuous sampling and approximation of the Monte Carlo method, $q_{\mathit{rand}}$ can be randomly generated as much as possible until the whole configuration space is obtained, which ensures the probability completeness. When judging that the random mechanism type is not in the constraint manifold, in order to accelerate the random sampling rate in advance, this paper incorporates the idea of the stochastic gradient descent method to select a new configuration within a certain range near $q_{\mathit{rand}}$ and re-judge it.

The flowchart shown in Figure 1 can be broken down into five steps, as follows:

Step 1: A random mechanism type $q_{\mathit{rand}}$ is generated in the joint space and the task error $\mathsf{\Delta }{x}_{\mathrm{err}\left(i\right)}$ of $q_{\mathit{rand}}$ is calculated according to the task error formula.

Step 2: To determine whether the $q_{\mathit{rand}}$ is on the constrained manifold, if $\mathsf{\Delta }{x}_{\mathrm{err}\left(i\right)}$ < $ϵ$, then the $q_{\mathit{rand}}$ is identified as the configuration $q_s$ that satisfies the target constraint.

Step 3: Conversely, if the $q_{\mathit{rand}}$ is not on the constrained manifold, then new configurations $q_s^{\prime }$ are randomly selected within a specified range of the $q_{\mathit{rand}}$, and their task errors are calculated to determine whether the task errors of the $q_s^{\prime }$ are less than those of $q_{\mathit{rand}}$. Then the $q_{\mathit{rand}}$ is replaced by $q_s^{\prime }$.

Step 4: The iterative process is repeated until the $q_s^{\prime }$ task error $\mathsf{\Delta }{x}_{\mathrm{err}\left(i\right)}$ meets the constraint tolerance $\mathsf{ϵ}$. In this case, the $q_s^{\prime }$ is identified as the configuration $q_s$ that satisfies the target constraint.

Step 5: If the task error $\mathsf{\Delta }{x}_{\mathrm{err}\left(i\right)}$ of the obtained configurations $q_s^{\prime }$ is still greater than the constraint tolerance $\mathsf{ϵ}$ within the set number of iterations, it will fail and return to step 1.

2.4. Informed Bidirectional Quick-RRT* Algorithm

2.4.1. Multi-Objective Bias Strategy

In the Bi-Quick-RRT* algorithm, the sampling points generated by double trees are random and single, and lack the ability to jump out of local traps when dealing with complex obstacles; there is also the lack of search efficiency when dealing with simple obstacles, which means there is the lack of adaptability to obstacles of different complexity. Because the multi-objective bias strategy is added to solve this problem, the core idea of the strategy is to extend the sampling process of random tree to three biased samples. Each biased sample has different effects.

• Tree 1 and tree 2 randomly pick points in the whole map. The purpose is to enhance the ability to jump out of the obstacle area faster when facing more complex obstacles, reduce the number of invalid samples, and improve the sampling efficiency.
• Tree 1 takes the starting point of tree 2 as the biased target for sampling points, and tree 2 takes the starting point of tree 1 as the biased target for sampling points. The purpose is to ensure the overall direction of random sampling and ensure the efficiency of the algorithm.
• Tree 1 uses the previous sampling point of tree 2 as a biased target for sampling points, and tree 2 uses the previous sampling point of tree 1 as a biased target for sampling points. The purpose is to speed up the connection of double trees and shorten the path process when facing simple and uncomplicated obstacles.

Depending on the complexity of the obstacles in the environment, different purposes can be achieved by changing the probability of the occurrence of each mining point. The pseudo-code for the multi-target bias strategy is provided in Algorithm 1.

Algorithm 1: InformedSampleFree Function.

$1:For i=1 \mathrm{to} \mathrm{K} do$

$2: \hspace{1em}\mathrm{P}=\mathrm{Rand}\left(0,1\right)$

$3: \hspace{1em}if 0<\mathrm{P}\le 0.5$

$4: \hspace{1em} \hspace{1em}{q}_{\mathrm{rand}1}=\mathrm{SampleFree}(q_{\mathrm{rand}1});q_{\mathrm{rand}2}=\mathrm{SampleFree}(q_{\mathrm{rand}2});$

$6: \hspace{1em}else if 0.5<\mathrm{P}\le 0.75$

$7: \hspace{1em} \hspace{1em} q_{\mathrm{rand}1}=q_{\mathrm{goal}};q_{\mathrm{rand}2}=q_{\mathrm{start}};$

$8: \hspace{1em}else 0.5<\mathrm{P}\le 0.75$

$9: \hspace{1em} \hspace{1em} q_{\mathrm{rand}1}=q_{\mathrm{rand}2};q_{\mathrm{rand}2}=q_{\mathrm{rand}1};$

$10: \hspace{1em}end if$

$11:end for$

$12:return q_{\mathrm{rand}1},q_{\mathrm{rand}2}$

2.4.2. Informed Bidirectional Quick-RRT* algorithm

To enhance the RRT exploration efficiency, the algorithm reduces the time spent solving the effective path. Informed-Bi-Quick-RRT* adds two optimization processes to the RRT algorithm: selecting the optimal parent node and pruning, and using the triangle inequality theorem to optimize the random tree structure [36]. This enlarges the traceability range of the two optimization processes and enhances the initial path to a certain extent while ensuring asymptotic optimization [37].

On this basis, the new algorithm incorporates the idea of double-tree expansion, starts randomly sampling from the starting point and the end point at the same time to find possible paths, and finally connects the two trees to determine a complete feasible path, which greatly improves the convergence speed [38].

However, since the randomly generated tree nodes are randomly distributed in the map, the generated random points are too scattered. In this paper, the sampling process of the random tree is extended to three-stage bias sampling in combination with the multi-objective bias expansion strategy mentioned above, and the algorithm is improved. The optimized algorithm can adjust the probability of three-stage sampling according to the complexity of the actual environment, and the algorithm can better deal with simple or complex obstacle environments.

The pseudo-code for the Informed-Bi-Quick-RRT* algorithm is provided in Algorithm 2.

Algorithm 2: Informed-Bi-Quick-RRT*.

$1: {\mathrm{T}}_1\leftarrow {\mathrm{T}}_{\mathrm{init}} (q_{\mathrm{start} }{), \mathrm{T}}_2\leftarrow {\mathrm{T}}_{\mathrm{init}} \left(q_{\mathrm{goal} }\right)$

$2: for i=1 to K do$

$3: q_{\mathrm{rand}1},q_{\mathrm{rand}2}\leftarrow \mathrm{InformedSampleFree}(i);$

$4: q_{\mathrm{nearest}1}\leftarrow \mathrm{Nearest}(q_{\mathrm{rand}1}{, \mathrm{T}}_1);q_{\mathrm{nearest}2}\leftarrow \mathrm{Nearest}(q_{\mathrm{rand}2}{, \mathrm{T}}_2);$

$5: q_{\mathrm{new}1}\leftarrow \mathrm{Steer}(q_{\mathrm{nearest}1}, q_{\mathrm{rand}1}, \delta );q_{\mathrm{new}}\leftarrow \mathrm{Steer}(q_{\mathrm{nearest}2}, q_{\mathrm{rand}2}, \delta );$

$6: if \mathrm{CollisionDetection}(q_{\mathrm{nearest}1,2}, q_{\mathrm{new}1,2}, \mathrm{map})$

$7: \hspace{1em}{\mathrm{Q}}_{\mathrm{near}1}\leftarrow {\mathrm{Near}(\mathrm{T}}_1, q_{\mathrm{new}1}, r_{\mathrm{near}1}); {\mathrm{Q}}_{\mathrm{near}2}\leftarrow {\mathrm{Near}(\mathrm{T}}_2, q_{\mathrm{new}2}, r_{\mathrm{near}2});$

$8: \hspace{1em} {\mathrm{Q}}_{\mathrm{P}1-\mathrm{near}}\leftarrow {\mathrm{Ancestry}(\mathrm{T}}_1, {\mathrm{Q}}_{\mathrm{near}1}, d_{\mathrm{near}1}{); \mathrm{Q}}_{\mathrm{P}2-\mathrm{near}}\leftarrow {\mathrm{Ancestry}(\mathrm{T}}_2, {\mathrm{Q}}_{\mathrm{near}2}, d_{\mathrm{near}2});$

$9: \hspace{1em}{q}_{\mathrm{parent}1}\leftarrow {\mathrm{ChooseParent}(\mathrm{Q}}_{\mathrm{near}1}\cup {\mathrm{Q}}_{\mathrm{P}1-\mathrm{near}}, q_{\mathrm{new}1});$ $\hspace{1em}\hspace{1em}{q}_{\mathrm{parent}2}\leftarrow {\mathrm{ChooseParent}(\mathrm{Q}}_{\mathrm{near}2}\cup {\mathrm{Q}}_{\mathrm{P}2-\mathrm{near}}, q_{\mathrm{new}2});$

$10: \hspace{1em}{\mathrm{T}}_1\leftarrow {\mathrm{Link}(q}_{\mathrm{parent}}, q_{\mathrm{new}}{); \mathrm{T}}_2\leftarrow {\mathrm{Link}(q}_{\mathrm{parent}2}, q_{\mathrm{new}2});$

$11: \hspace{1em}{\mathrm{Q}}_{\mathrm{P}1-\mathrm{near}}\leftarrow {\mathrm{Ancestry}(\mathrm{T}}_1, q_{\mathrm{new}1}, d_{\mathrm{new}1}{); \mathrm{Q}}_{\mathrm{P}2-\mathrm{near}}\leftarrow {\mathrm{Ancestry}(\mathrm{T}}_2, q_{\mathrm{new}2}, d_{\mathrm{new}2});$

$12: \hspace{1em}{\mathrm{T}}_1\leftarrow {\mathrm{Rewire}(\mathrm{Q}}_{\mathrm{near}1}, q_{\mathrm{new}1}\cup {\mathrm{Q}}_{\mathrm{P}1-\mathrm{near}}{); \mathrm{T}}_2\leftarrow {\mathrm{Rewire}(\mathrm{Q}}_{\mathrm{near}2}, q_{\mathrm{new}2}\cup {\mathrm{Q}}_{\mathrm{P}2-\mathrm{near}});$

$13: \hspace{1em}{\mathrm{T}}_1\leftarrow {\mathrm{Connnect}(\mathrm{T}}_2, q_{\mathrm{new}1}{); \mathrm{T}}_2\leftarrow {\mathrm{Connnect}(\mathrm{T}}_1, q_{\mathrm{new}2});$

$14: \hspace{1em}\mathrm{if} {\mathrm{IsConnected}(\mathrm{T}}_1{, \mathrm{T}}_2)$

$15: \hspace{1em}\hspace{1em}\hspace{1em}{\mathrm{Path}=\mathrm{FillPath}(\mathrm{T}}_1{,\mathrm{T}}_2);$

$16: \hspace{1em}end if$

$17: \mathrm{Swap}({\mathrm{T}}_1{, \mathrm{T}}_2);$

$18: end if$

$19: end for$

$20: return Path$

The Informed-Bi-Quick-RRT* runs as follows:

1.

The InformedSampleFree($i$) function is used to generate random sampling points for both trees. Algorithm 2 presents the pseudocode for the InformedSampleFree($i$) function. In the exploration process, the two trees are consistent in steps 2 through 4, so the related steps use the same expression.

2.

Two new nodes are used: $q_{\mathit{nearest}}$ and $q_{\mathit{new}}$. The Nearest ($q_{\mathit{rand}}$, $T_1$) function should be used to produce $q_{\mathit{nearest}}$, and the Steer ($q_{\mathit{nearest}}$, $q_{\mathit{rand}}$, δ) function should be used to create $q_{\mathit{new}}$.

3.

Under the premise that there is no collision between $q_{\mathit{nearest}}$ and $q_{\mathit{new}}$, the path from $q_{\mathrm{start} }$ to $q_{\mathrm{goal} }$ is optimized. The Near ($T_1$, $q_{\mathit{new}}$, $r_{\mathit{near}}$) and Ancestry ($T_1$, $Q_{\mathit{near}}$, $d_{\mathit{near}}$) functions are used to identify the potential parent node $Q_{\mathit{\text{P-near}}}$ of $q_{\mathit{new}}$. Then, the ChooseParent (‘$Q_{\mathit{near}}$ ∪ $Q_{\mathit{\text{P-near}}}$, $q_{\mathit{new}}$) function is used to select the parent node $q_{\mathit{parent}}$ in the potential parent node, so that the path distance from $q_{\mathrm{start} }$ to $q_{\mathit{new}}$ is the smallest; finally, the Link ($q_{\mathit{parent}}$, $q_{\mathit{new}}$) function is used to form $T_1$.

4.

The path from $q_{\mathrm{start} }$ to the midpoint of $Q_{\mathit{near}}$ is optimized. The Ancestry ($T_1$, $q_{\mathit{new}}$, $d_{\mathit{new}}$) function is used to find the parent node $Q_{\mathit{\text{P-near}}}$ of $q_{\mathit{new}}$; the $q_{\mathit{new}}$ and $q_{\mathit{parent}}$ are used as the potential parent nodes of the midpoint of $Q_{\mathit{\text{P-near}}}$, and the Rewire ($Q_{\mathit{near}}$, $q_{\mathit{new}}$ ∪ $Q_{\mathit{\text{P-near}}}$) function is used to find the parent node at the potential parent node, so that the $T_1$ path distance is minimized.

5.

The Connect ($T_2$, $q_{\mathit{new}1}$) function is used to connect $T_2$ and $q_{\mathit{new}1}$. Firstly, the nodes closest to $q_{\mathit{new}1}$ in $T_2$: $q_{\mathit{newest}1}^{\prime }$ are determined, and then $q_{\mathit{newest}}^{\prime }$ continuously advances δ with $q_{\mathit{new}1}$ as the goal until they encounter obstacles or link to $q_{\mathit{new}1}$.

6.

The FillPath ($T_1$, $T_2$) function is used to stitch paths. In the case of a link between $T_2$ and $q_{\mathit{new}1}$, $T_1$ traces the parent node from $q_{\mathit{new}1}$ to $q_{\mathit{start}1}$, thus forming the final $T_1$. $T_1$ begins to trace the parent node to $q_{\mathrm{goal} }$ from the node connected to $q_{\mathit{new}1}$, thus forming the final $T_2$.

7.

The Swap ($T_1$, $T_2$) function is used to exchange the contents of two trees, so that the number of nodes of the two trees remains balanced after several iterations.

3. Different Task Constraints in Dual-Arm Cooperative Systems

In the trajectory planning of heterogeneous two-arm collaboration, in order to ensure the relative consistency of the end-effector postures in collaboration so that the heterogeneous two-arm collaboration system is able to meet higher task requirements, this paper investigates the relative motion of the end-postures in different types of collaborative motions, and realizes the adaptive planning from the trajectory of the robot arm through the establishment of different kinematic constraint relations. According to the different motion constraint relationships between the arms, the two-arm cooperative motion can be broadly categorized into loose-constraint cooperative motion and tight-constraint cooperative motion.

3.1. Collaborative Movement with Loose Constraints

There are relatively strict constraints in the coordinated motion of the two arms under loose constraints. The two arms and the manipulated object will form a semi-closed chain system with a certain constraint relationship but not completely closed. In the typical pine cooperative assembly task [39], the dual-arm end-effector is required to perform relative motion on the same horizontal line with a fixed attitude, as shown in Figure 2.

Suppose that the pose matrix of any point on the end-effector trajectory of the master robot is $Q$,; then,${ }^A{\mathit{U}}_Q$ is the pose transformation matrix from the base coordinate of the master robot to the point on the end-effector trajectory. Therefore, the pose matrix of the end-effector trajectory point at any time can be obtained as:

$$\mathit{Q}\left(t\right)={ }^A\mathit{Q}\left(t\right).$$

(7)

Let ${ }^A{\mathit{U}}_B$ be the pose transformation matrix from the base coordinate of the manipulator to the base coordinate of the main manipulator, and ${ }^A\mathit{S}$ be the pose matrix of the trajectory point of the end-effector of the manipulator under the base coordinate of the main manipulator. ${ }^B\mathit{S}$ is the pose matrix of the trajectory point of the end-effector of the slave manipulator under the base coordinate of the slave manipulator. Then the pose matrix of the trajectory point of the end-effector of the slave manipulator under the base coordinate of the main manipulator can be obtained as follows:

$${ }^A\mathit{S}={ }^A{\mathit{U}}_B\cdot { }^B\mathit{S}.$$

(8)

According to the constraint between the trajectory points at the end of the master–slave manipulator, the pose matrix of the trajectory points at the end of the slave manipulator under the base coordinate of the master manipulator is solved:

$${ }^A\mathit{S}={\left({ }^A\mathit{Q}\right)}^{-1}\cdot { }^s{\mathit{U}}_Q.$$

(9)

Among them: ${ }^s{\mathit{U}}_Q$ is the pose matrix of the trajectory from the end point of the robot actuator under the base coordinate of the main manipulator.

By using Formulas (8) and (9), we can obtain:

$${ }^B\mathit{S}={ }^B{\mathit{U}}_A\cdot {\left({ }^A\mathit{Q}\right)}^{-1}\cdot { }^S{\mathit{U}}_Q,$$

(10)

Therefore, the pose matrix of the end point of the slave manipulator under the base coordinate of the slave manipulator can be obtained as follows:

$${ }^B\mathit{S}\left(t\right)={ }^B{\mathit{U}}_A\cdot {\left({ }^A\mathit{Q}\left(t\right)\right)}^{-1}\cdot { }^s{\mathit{U}}_Q.$$

(11)

From Equation (11), it can be seen that the end point pose of the slave manipulator can be solved under the condition of obtaining the base coordinate pose relationship matrix of the master–slave manipulator and the relative motion pose constraint of the end point and the end point pose of the master manipulator, so as to realize the adaptive planning of the slave manipulator trajectory according to the trajectory of the master manipulator.

3.2. Collaborative Movement with Tightly Constraints

The dual-arm cooperative motion under tight constraints has the strictest constraints on the motion process, which means that the dual arms and the operated object form a closed chain system with complete constraints [39]. In a typical tight cooperative task handling task [40], there is no relative motion between the dual-arm end-effector and the object to be carried, and the relative pose of the dual-arm end-effector is required to remain unchanged, as shown in Figure 3.

The key point is to ensure that when the end-effector of the main arm moves from the ${\mathit{Q}}_1$ point to the ${\mathit{Q}}_1$ point, the pose relationship between the ${\mathit{Q}}_1$ point and the ${\mathit{S}}_1$ point is equal to that between the ${\mathit{Q}}_2$ point and the ${\mathit{S}}_2$ point in the process of moving the end-effector of the manipulator from ${\mathit{S}}_1$ to ${\mathit{S}}_2$. The specific kinematics derivation is as follows:

When the pose transformation matrix from the robot base coordinate to the master robot base coordinate is set as ${ }^A{\mathit{U}}_B$, the pose relationship from the ${\mathit{Q}}_1$ point to the ${\mathit{S}}_1$ point is ${ }^{Q_1}{\mathit{U}}_{S_1}$, and the pose relationship from ${\mathit{Q}}_2$ to ${\mathit{S}}_2$ is ${ }^{Q_2}{\mathit{U}}_{S_2}$.

Since the relative pose relationship between the main manipulator and the end-effector of the slave manipulator remains unchanged at all times, the following relationship is satisfied:

$${ }^{Q_1}{\mathit{U}}_{S_1}={ }^{Q_2}{\mathit{U}}_{S_2}.$$

(12)

Then:

$${ }^A{\mathit{Q}}_1={ }^{Q_1}{\mathit{U}}_{S_1}\cdot { }^A{\mathit{S}}_1,$$

(13)

$${ }^A{\mathit{Q}}_2={ }^{Q_2}{\mathit{U}}_{S_2}\cdot { }^A{\mathit{S}}_2.$$

(14)

Let ${ }^A{\mathit{Q}}_1$ and ${ }^A{\mathit{Q}}_2$ be the pose matrix of the ${\mathit{Q}}_1$ and ${\mathit{Q}}_2$ points in the master arm coordinate system, and ${ }^B{\mathit{S}}_1$ and ${ }^B{\mathit{S}}_2$ be the pose matrix of the ${\mathit{S}}_1$ and ${\mathit{S}}_2$ points in the slave arm coordinate system. Then the coordinates of the ${\mathit{S}}_1$ and ${\mathit{S}}_2$ points under the base coordinates of the main manipulator are:

$${ }^A{\mathit{S}}_1={ }^A{\mathit{U}}_B\ast {\hspace{0.17em}}^B{\mathit{S}}_1,$$

(15)

$${ }^A{\mathit{S}}_2={ }^A{\mathit{U}}_B\ast {\hspace{0.17em}}^B{\mathit{S}}_2.$$

(16)

Since ${ }^A{\mathit{Q}}_1$, ${ }^{Q_1}{\mathit{U}}_{S_1}$, ${ }^B{\mathit{S}}_1$, and ${ }^A{\mathit{U}}_B$ constitute a complete closed chain of motion rings, they are obtained using Formulas (12) and (13):

$$\begin{array}{ll}{ }^{Q_1}{\mathit{U}}_{S_1}& ={ }^A{\mathit{Q}}_1\cdot {\left({ }^A{\mathit{S}}_1\right)}^{-1}\\ & ={ }^A{\mathit{Q}}_1\cdot {\left({ }^A{\mathit{U}}_B\cdot { }^B{\mathit{S}}_1\right)}^{-1}\\ & ={ }^A{\mathit{Q}}_1\cdot {\left({ }^B{\mathit{S}}_1\right)}^{-1}\cdot { }^B{\mathit{U}}_A\end{array},$$

(17)

$${ }^{Q_2}{\mathit{U}}_{S_2}={ }^A{\mathit{Q}}_2\cdot {\left({ }^B{\mathit{S}}_2\right)}^{-1}\cdot { }^B{\mathit{U}}_A.$$

(18)

From Formulas (14), (17) and (18), the following can be obtained:

$$\begin{array}{ll}{ }^A{\mathit{Q}}_2& ={ }^A{\mathit{Q}}_1\cdot {\left({ }^B{\mathit{S}}_1\right)}^{-1}\cdot { }^B{\mathit{U}}_A\cdot { }^A{\mathit{U}}_B\cdot { }^B{\mathit{S}}_2\\ & ={ }^A{\mathit{Q}}_1\cdot {\left({ }^B{\mathit{S}}_1\right)}^{-1}\cdot { }^B{\mathit{S}}_2.\end{array}$$

(19)

The position of the slave manipulator in the base coordinate is:

$${ }^B{\mathit{S}}_2={ }^B{\mathit{S}}_1\cdot \left({ }^A{\mathit{Q}}_1\right){ }^{-1}\cdot { }^A{\mathit{Q}}_2.$$

(20)

By solving the inverse solution of ${ }^B{\mathit{S}}_2$, the angle value of each joint of the slave manipulator can be obtained. The slave manipulator can achieve the specified pose and position of the end point, so as to realize the adaptive planning of the slave manipulator trajectory according to the trajectory of the main manipulator.

4. Experiment

This research focuses on the dual-arm cooperation system composed of UR5 and UR10. MATLAB 2022b was used as the simulation software to complete the simulation of the algorithm and the system model construction.

The Informed-Bi-Quick-RRT* algorithm proposed in this paper was simulated with the RRT*, Quick-RRT*, and Bi-Quick-RRT* algorithms in different two-dimensional environments. It has been demonstrated that the new algorithm exhibits fast convergence speed, a short search path, and adaptability to various obstacle environments.

The simulation of both loose and close collaboration tasks in a 3D environment demonstrates the effect of the pre-sampling planning algorithm on the horizontal constraints of different end-effectors, as well as the rationality of adaptive planning for dual-arm trajectories.

Finally, the simulation environment for close cooperative motion includes the addition and simulation of obstacles. The simulation results demonstrate the ability of the Informed-Bi-Quick-RRT* algorithm, based on advanced sampling, to handle the horizontal and obstacle avoidance constraints of the end-effector. The results also prove the adaptability of the new algorithm to multi-constraint environments.

4.1. Algorithm Comparison

To demonstrate the adaptability of the new algorithm to complex obstacle environments, the experiment compared its performance in two environments: one simple and one complex. To reduce the impact of random sampling, the experiment recorded and summarized the average results of each algorithm after 20 runs, including path length, running time, and number of sampling nodes.

4.1.1. Simple Environment

The results of algorithm comparison in a simple environment are shown in Figure 4, The simple environment shows a 1400 mm × 1400 mm environment containing four matrices and three circular obstacles. The start point is located in the upper right corner and the end point is located in the lower left corner. The red line represents the last viable path generated, while the green and blue lines represent the branches generated by the two trees, respectively.

Table 1. Simple environment algorithm comparison table.

Algorithm	Average Path Length (mm)	Average Running Time (s)	Average Iterations
RRT*	245.90	11.09	877
Quick-RRT*	244.74	3.97	624
Bi-Quick-RRT*	240.35	2.85	605
Informed-Bi-Quick-RRT*	229.25	0.17	480

provides the specific statistics in a simple environment. As can be seen in Figure 4a), the RRT* algorithm performs a large number of random samples in the environment and has a large number of redundant calculations, resulting in a very pronounced inflection point in the final trajectory. Figure 4b) shows the effect of the Quick-RRT* algorithm on the optimization of node links. Compared to the RRT* algorithm, the number of path nodes is reduced by 28.8%, the generation of prominent inflection points is reduced, and the final trajectory is clearer and smoother. In addition, the computational cost is greatly reduced and the overall execution time is correspondingly reduced by 64%. The Bi-Quick-RRT* algorithm in Figure 4c) generates two trees simultaneously on the basis of the above, further improving the search efficiency of the algorithm and reducing the running time by 28.2%. Figure 4d) shows the planning effect of the Informed-Bi-Quick-RRT* algorithm. In a simple environment, according to the multi-objective bias strategy, the probability of the third segment is increased to speed up the connection speed and shorten the path length. It is not difficult to find in Table 1 that the new algorithm has fewer redundant nodes and branches, and shorter path length. Compared with the basic algorithm, the search efficiency of the new algorithm is improved by 94.3%, which meets the actual working requirements of the robot arm and proves the superiority of the new algorithm.

4.1.2. Complex Environment

Figure 5 depicts a more complex environment, which includes two rectangular and four circular obstacles. This change in the position of the obstacles results in a more winding possible driving path.

Table 2. Algorithm comparison in complex environment.

Algorithm	Average Path Length (mm)	Average Running Time (s)	Average Iterations
RRT*	234.157	37.34	986
Quick-RRT*	225.80	10.31	733
Bi-Quick-RRT*	222.47	8.70	715
Informed-Bi-Quick-RRT*	214.41	5.52	590

provides specific statistics in a complex environment. When analyzing Figure 5a–c, it becomes apparent that in a more complex environment, the Quick-RRT* and Bi-Quick-RRT* algorithms have significant advantages over RRT*. Specifically, the number of path nodes is reduced by 25.6% and 27.4%, respectively, and the time is shortened by 72.3% and 76.7%, respectively.

In the face of a complex environment, the new algorithm improves the probability of occurrence of the first paragraph according to the multi-objective bias strategy to improve the ability of the random tree to jump out of the obstacle area and reduce the generation of redundant nodes, as shown in Figure 5d). According to Table 2, the running time of the new algorithm is further reduced by 36.5%, the number of path nodes is further reduced by 17.4%, and the comprehensive search efficiency is increased by 85.2%. This shows that the algorithm has a strong ability to deal with complex obstacles.

To summarize, in comparison with traditional planning algorithms, the Informed-Bi-Quick-RRT* algorithm has clear advantages in terms of convergence speed, path length, and quality, and performs well in various obstacle environments. In straightforward environments, this technique can significantly accelerate path generation. In more complicated environments, it can assist branches in avoiding obstacles more effectively.

4.2. Three-Dimensional Simulation Considering Horizontal Constraints

The UR5 and UR10 manipulators used in the laboratory are both six-degree-of-freedom serial manipulators. They are widely used in mechanical operations because of their programming simplicity, high flexibility, and high safety. The actual working environment of the dual-arm cooperative system is shown in Figure 6. A is the UR5 manipulator, and B is the UR10 manipulator.

According to the Denavit–Hartenberg (DH) parameters shown in

Table 3. UR5 D-H Table.

$\mathbf{Joint}/\mathit{i}$	${\mathit{\alpha }}_{\mathit{i}}$/(°)	${\mathit{a}}_{\mathit{i}}$$/\mathbf{m}\mathbf{m}$	${\mathit{d}}_{\mathit{i}}$$/\mathbf{m}\mathbf{m}$	${\mathit{\theta }}_{\mathit{i}}$/(°)
1	90	0	89.2	${\theta }_1$
2	0	−425	0	${\theta }_2$
3	0	−392	0	${\theta }_3$
4	−90	0	109.3	${\theta }_4$
5	−90	0	94.75	${\theta }_5$
6	0	0	82.5	${\theta }_6$

and

Table 4. UR10 D-H Table.

$\mathbf{Joint}/\mathit{i}$	${\mathit{\alpha }}_{\mathit{i}}$/(°)	${\mathit{a}}_{\mathit{i}}$$/\mathbf{m}\mathbf{m}$	${\mathit{d}}_{\mathit{i}}/\mathbf{m}\mathbf{m}$	${\mathit{\theta }}_{\mathit{i}}$/(°)
1	90	0	128	${\theta }_1$
2	0	−612.7	0	${\theta }_2$
3	0	−571.6	0	${\theta }_3$
4	90	0	163.9	${\theta }_4$
5	−90	0	115.7	${\theta }_5$
6	0	0	92.2	${\theta }_6$

UR5, UR10 manipulator in the initial position: ${\theta }_1$ = 0°, ${\theta }_2$ = 90°, ${\theta }_3$ = −90°, ${\theta }_4$ = −90°, ${\theta }_5$ = 0°, ${\theta }_6$ = 90°.

, the UR5 and UR10 manipulator models are defined respectively, as shown in Figure 7.

According to the transformation principle of the coordinate system and the establishment rule of the D–H coordinate system, the forward transformation matrix between adjacent joint coordinate systems can be derived, that is, the D–H transformation matrix, as shown in Formula (21).

$$A_i^{i-1}=\left[\begin{array}{cc}\begin{array}{cc} c{\theta }_n& -s{\theta }_n\ast c{\alpha }_n\\ s{\theta }_n& c{\theta }_n\ast c{\alpha }_n\end{array}& \begin{array}{cc}s{\theta }_n\ast s{\alpha }_n& {\alpha }_n\ast c{\theta }_n\\ -c{\theta }_n\ast s{\alpha }_n& {\alpha }_n\ast s{\theta }_n\end{array}\\ \begin{array}{cc} 0& s{\alpha }_n\\ 0& 0\end{array}& \begin{array}{cc}c{\alpha }_n& { d}_n\\ 0& 1\end{array}\end{array}\right] i=1,2,\dots ,6 .$$

(21)

The transformation matrix that is homogeneous between the UR series manipulator base and the end-effector in the Cartesian coordinate system is as follows:

$$T_6^0=A_1^0{A}_2^1{A}_3^2{A}_4^3{A}_5^4{A}_6^5=\left[\begin{array}{cc}\begin{array}{cc}{n}_x& o_x\\ n_y& o_y\end{array}& \begin{array}{cc}{a}_x& p_x\\ a_y& p_y\end{array}\\ \begin{array}{cc}{n}_z& o_z\\ 0& 0\end{array}& \begin{array}{cc}{a}_z& p_z\\ 0& 1\end{array}\end{array}\right].$$

(22)

We take the two-arm collaborative model built in the laboratory environment as the research object, with UR5 as the main robot arm and UR10 as the slave robot arm, and perform the assembly task and the handling task, respectively, through the master–slave control. In both tasks, the horizontal constraint of the end-effector and the dynamic constraint between the two arms are considered. We use a series of postures taken at fixed time intervals to show the movement of the task, and a joint position graph to show the changes of all joints.

4.2.1. Loose Collaborative Motion: Assembly Task

The objective of the assembly task is to hold objects horizontally and assemble them on a level plane. Throughout the assembly process, it is necessary for the end-effector of the two arms to maintain a horizontal attitude constantly, whilst the joint motion is smooth.

The simulated diagram of the dual-arm collaborative system’s pose is depicted in Figure 8. Specifically, the poses at 0 s, 2 s, 4 s, 6 s, 8 s, and 10 s were selected for observation. The steady convergence of the arms during the assembly process, while the end-effector maintains a Y-axis level, can be observed. This conforms to the attitude relationship required during the said assembly process.

Further, Figure 9 and Figure 10 display the joint position variations of the two arms. From the green dot on the diagram, it is evident that the end-effector remains at a constant position of 150 mm whilst maintaining the same horizontal line and smooth joint changes.

4.2.2. Tightly Collaborative Motion: Handling Task

The goal of the handling task is to carry objects with two arms. During the motion, the end-effector of the two arms must maintain the same level of posture at all times; then, the joint motion is smooth.

The position of the dual-arm cooperative system during the handling task is depicted in Figure 11, showcasing six different moments: 0 s, 2 s, 4 s, 6 s, 8 s, and 10 s. As demonstrated, the arms move synchronously throughout the handling process, and the end-effector stays levelled along the Y-axis, thus adhering to the required attitude relationship.

Moreover, Figure 12 and Figure 13 present the handling task variations in the joint position of the main–slave manipulator. From the end-effector joint represented by the green component in the figure, it can be observed that the end-effector remains at a fixed position of 175 mm whilst maintaining the same horizontal alignment and smooth joint movement.

In summary, the above two motion processes realized by the offline sampling-based motion-planning method preliminarily demonstrate the effectiveness of this method in dealing with end-effector constraints and horizontal direction constraints.

4.3. Three-Dimensional Simulation Considering Multiple Constraints

In Section 4.2, we simulate the two tasks of assembly and handling, focusing on the directional constraints. The experimental results show that the directional constraints of the end-effector have been well handled. Based on this, we now need to consider additional obstacle avoidance constraints for the handling task. To test the motion-planning effect of the Informed Bi-Quick RRT* algorithm, we set up a 3000 mm × 4000 mm × 4500 mm map and placed five dark blue spherical obstacles with different radii on the 3D map. We set the right side as the start point of the moving task and the left side as the end point of the moving task. Throughout the handling process, we needed the two-armed end-effectors to maintain a horizontal position and be able to avoid obstacles and collisions.

As shown in Figure 14, the two-arm collaborative system obtains a collision-free and smooth path after running the simulation. In the actual handling process, the end-effector of both arms can complete the handling task by following this collision-free path.

Figure 14 shows the complete process of moving both arms from the predetermined start point to the target end point, confirming that the algorithm is efficient and feasible as there is no collision with surrounding obstacles and the end-effector direction obeys horizontal holding constraints throughout the movement.

5. Conclusions

This paper presents an Informed Bi-Quick RRT* algorithm for rational motion planning of two-arm cooperative systems under multiple constraints. The algorithm is based on offline sampling and rich data sets, and the offline sampling time is reduced by incremental construction and random gradient descent. However, the traditional Quick-RRT* algorithm is enhanced by integrating a two-tree concept and a multi-objective bias expansion strategy. In this paper, simulation experiments are conducted in two obstacle environments with different complexity levels. The results show that compared with other traditional algorithms, the computation time of the proposed algorithm is reduced by about 70% and the number of path nodes is reduced by about 40%, which proves the superiority of the new algorithm. In addition, the directional constraints in the two-arm cooperative system are analyzed in detail, and the adaptive planning effect of the two-arm trajectory is demonstrated through the three-dimensional simulation of the assembly and transportation tasks, and it is verified that the loosely constrained cooperative motion algorithm and the tightly constrained cooperative motion algorithm can accurately describe the nonlinear kinematic constraints between the two arms. Finally, in the handling task, a three-dimensional motion simulation of the two-arm cooperative system is performed considering obstacle avoidance and directional constraints. The system runs stably and avoids possible collisions while maintaining the level of the end-effector, demonstrating the feasibility and effectiveness of the proposed algorithm.

In summary, the motion-planning technology based on offline sampling can shorten the subsequent planning time by sampling the basic constrained manifolds in advance and generating sample databases that satisfy the constraints. However, although the offline sampling process is optimized in this paper, a large number of offline computations are inevitably required to satisfy more constraints. Second, although the offline database can be continuously supplemented and improved, it is not as flexible as the real-time planning method. In order to adapt to a changing external environment, the method based on offline sampling needs to introduce some online planning elements to cope with the changing obstacle configuration and possible fault edges in the offline calculation roadmap.