INTRODUCTION {#sec1-1}
============

Cooperative mobile manipulators have received extensive attention in recent years due to their wide range of applications including transporting long and heavy materials.\[[@ref1]--[@ref16]\] While cooperative mobile manipulators can offer advantages over a single mobile manipulator in terms of their capability to carry out difficult tasks, motion planning of these systems is complicated by the need to maintain a closed-chain structure, the so-called closure constraint. A cooperative mobile manipulator system, in addition to closed-chain constraints, is subject to kinodynamic and nonholonomic constraints in the form of differential equations,\[[@ref17]\] along with collision constraints, which make motion planning even more difficult. However, these constraints are necessary to find a reliable and efficient solution, often called a trajectory.

Many previous studies have investigated the trajectory generation for cooperative mobile manipulators, under various conditions.\[[@ref1]--[@ref16]\] Two approaches for a solution to this problem can be distinguished,\[[@ref18][@ref19]\] namely: The decoupled and direct approaches. The decoupled approach, involves first searching for a path in the configuration space and then finding a time-optimal time scaling for the path, subject to the actuator limits.\[[@ref18]\] This has the desirable benefit of decomposing the complexity of motion planning problems in two steps, as mentioned earlier. However, the path from the first stage might not be transformable into an executable trajectory and the cost associated with the final trajectory could be expensive. The main reason for these problems is that the approach ignores the differential constraints of the action model in the first stage.\[[@ref19]\] To overcome these deficiencies, a direct approach has been developed, in which, differential constraints are considered in the planning process. Most of the literatures available on the direct approach, compute exact trajectories using the optimal control theory and nonlinear optimization methods for some specific low-dimensional problems.\[[@ref1]--[@ref11]\] The main drawback of these methods is that the number of variables and the complexity of the formulation rapidly increase for problems with high degrees of freedom. To this end, in recent years, probabilistic complete methods have been extensively studied within the direct framework.\[[@ref13]--[@ref16]\]

In contrast to other available methods that construct a global representation of the configuration space, these methods discretize their representation of free configuration space to handle high-dimensional configuration spaces. Due to the effectiveness of these methods in cooperative manipulation systems,\[[@ref12][@ref20][@ref21]\] some recent efforts have been directed to generalize them for cooperative mobile manipulators.\[[@ref13]--[@ref16]\] For instance, in\[[@ref13]\] a single query method based on the Rapidly-exploring Random Tree algorithm (RRT) has been presented. The method constructs a randomized tree between the start and goal configurations and searches for an optimal path during the construction phase. A disadvantage of this method is that the generated tree is only valid for certain start and goal configurations; hence, for a new query, another tree will have to be constructed, which restricts the method to a limited range of applications.\[[@ref22]\] Therefore,\[[@ref14]\] proposes a multiple query two-stage method, in which first a graph, namely, the 'Roadmap' is constructed, based on the Probabilistic Roadmap Method (PRM) by neglecting the presence of obstacles and assuming a fixed location for the bases of mobile manipulators. Then, it populates the environment with copies of the kinematic roadmap in random locations and connects the collision-free configurations of the same closure type to build the final roadmap. The method, because of its higher speed in the query phase and ability to change its goal configuration in an online manner, is more executable in real-time practical situations. However, in this method, the probability of satisfying loop closure equations in a randomly sampled configuration is near zero and this fact lowers the performance of the algorithm. To solve this problem in\[[@ref15][@ref16]\] a simple and general geometric-guided sampling algorithm called Random Loop Generator (RLG) has been proposed, which notably increases the probability of obtaining real solutions when solving the loop closure equations. The main deficiency of the method presented for randomized motion planning of cooperative mobile manipulators is that they ignore differential constraints in the motion planning process.\[[@ref23]\] However, these constraints will be considered to calculate a reliable and efficient solution.

Therefore, in this study we propose a novel two stage scheme that considers the trajectory planning problem of two mobile manipulators for cooperative transportation of a rigid body. The environment is supposed to be a 3-D space that includes static obstacles. The method finds an optimal trajectory in which such constraints as nonholonomic and closed-chain constraints, along with the joint and acceleration limits can be easily be dealt with in the presence of static obstacles. The method utilizes the advantages of direct and decoupled approaches along with the power of probabilistic methods in handling high dimensional configuration spaces to have a reliable and fast trajectory planning algorithm.

The article is organized as follows: Section 2 introduces the model of the cooperative system. In section 3, we design the details of our new method for trajectory planning of the cooperative mobile manipulators under differential constraints in the presence of static obstacles. Finally, we present simulation results in section 4, to show the effectiveness of the algorithm, and some concluding remarks in section 5.

MODEL OF COOPERATIVE SYSTEM {#sec1-2}
===========================

[Figure 1](#F1){ref-type="fig"} shows our selected model, including a cooperative system of two-wheeled mobile manipulators transporting a common payload. Each mobile manipulator module consists of a wheeled mobile robot with a 5-DOF mounted revolute-joint manipulator. Two rotational joints have been considered at the bottoms and the tips of both the manipulators, with the aim of making the system applicable to more practical situations. In the sequel, it is assumed that the motion of vehicles is restricted to the horizontal plane and both the end effectors catch the object tightly.

![Cooperative transportation by a dual mobile manipulator system](JMSS-1-24-g001){#F1}

The problem can now be stated as follows: Given a group of two nonholonomic mobile manipulators grasping a rigid body, we should find a trajectory to steer the system in a cooperative manner between two configurations, in an environment with static obstacles such that the acceleration of each variable in the configuration space remains within certain bounds.

It should be noted that the combination of different types of constraints (including holonomic, nonholonomic, and dynamic constraints) in such a system makes the motion planning problem complicated and it requires careful evaluation to realize the payload manipulation task efficiently. Therefore, in the remaining section we use the above-mentioned model to generate the constraint equations of the system.

Closed-chain Constraints {#sec2-1}
------------------------

When a collection of links is arranged so that it forms a loop, the configuration space becomes much more complicated because the joint angles must be chosen in a way that the loops remain closed. This leads to constraints in which some links must maintain specified positions relative to each other. To derive these constraints, we consider the cooperative system in more detail, as shown in [Figure 2](#F2){ref-type="fig"}. Nomenclatures in this figure are defined as follows:

*l~i~* (*i* = 1,...,6): Length of *i^th^* link in the closed kinematic chain

θ~*i*~ (*i* = 1,...,6): The *i^th^* joint angle rotating in the vertical plane in the closed kinematic chain

θ~*pi*~ (*i* = 1,...,4): The *i^th^* joint angle rotating in the horizontal plane in the closed kinematic chain

*l~ajb~*: Length of object between two end effectors

*l*~*p*~*i*1~~, *l*~*p*~*i*2~~ (*i* = 1,...,4): Length of links attached to the *i^th^* joint rotating in the horizontal plane

![Cooperative system with its attached coordinate system](JMSS-1-24-g002){#F2}

From this figure and the assumptions mentioned before, one can conclude that the mobile manipulator system is subject to holonomic constraints, expressed as:

![](JMSS-1-24-g003.jpg)

where θ~*b*~1~~ and θ~*b*~2~~ represent the orientation of the base structures (detailed derivation of this equation can be found in Appendix A).

However, in order to lower the computational cost of our method, we have designed the desired trajectory in a manner that both the mobile bases have a fixed position and orientation relative to each other. To this end, we define the following conditions:

![](JMSS-1-24-g004.jpg)

where is *g~b~* a constant positive value. We also make the mounted manipulators to cooperate in a single plane. By using (2), this leads to the following desired trajectories:

θ~*p*~1~~ =θ~*p*~2~~ =θ~*p*~3~~ =θ~*p*~4~~ =0      (3)

Therefore, by substituting (2) and (3) in (1) and after further manipulation one gets:

θ~1~ + θ~2~ + θ~3~ = θ~4~ + θ~5~ + θ~6~

*l*~3~*s*~123~ + *l*~2~*s*~12~ + *l*~1~*s*~1~ -- *l*~*obj*~*c*~456~ = *l*~4~*s*~456~ + *l*~5~*s*~56~ + *l*~6~*s*~6~      (4)

*l*~3~*c*~123~ + *l*~2~*c*~12~ + *l*~1~*c*~1~ + *l*~*obj*~*s*~456~ = *l*~4~*c*~456~ + *l*~5~*c*~56~ + *l*~6~*c*~6~ + *g*~*b*~

in which, we define the following parameters:

*c*~*ijk*~ = cos(θ~*i*~ + θ~*j*~ + θ~*k*~), *c*~*ij*~ = cos(θ~*i*~ + θ~*j*~), *c*~*i*~ = cos(θ~*i*~)

*s*~*ijk*~ = sin(θ~*i*~ + θ~*j*~ + θ~*k*~), *s*~*ij*~ = sin(θ~*i*~ + θ~*j*~), *s*~*i*~ = sin(θ~*i*~)

*l*~3~ = *l*~*p*~21~~ + *l*~*p*~22~~, *l*~4~ = *l*~*p*~31~~ + *l*~*p*~32~~      (5)

Differential Constraints {#sec2-2}
------------------------

Differential constraints exist in a model of every nonholonomic motion planning problem and restrict admissible velocities and accelerations. Here, we consider an upper limit on the acceleration of all joint space variables. In addition, the mobile platform used in this article is a kind of car-like mobile robot as shown in [Figure 3](#F3){ref-type="fig"}; therefore, we suppose that the wheels are rolling without skidding and slipping.

![Base structure subsystem of the cooperative system](JMSS-1-24-g005){#F3}

To derive the equations due to these constraints, we need to compute the velocity of each wheel in its attached coordinate system \[[Figure 3](#F3){ref-type="fig"}\].

In this regard, we can state that:\[[@ref17][@ref24]\]

![](JMSS-1-24-g006.jpg)

where *x~b~i~~* and *y~b~i~~* denote the position and θ*~b~i~~* represents the orientation of the *i^th^* base structure (*i* = 1,2) and *l^ωj^* is the position vector from the origin of {*v~i~*} to the contact point of the wheel with the ground \[[Figure 2](#F2){ref-type="fig"}\]. Hence, the velocity of each wheel can be written as:

![](JMSS-1-24-g007.jpg)

and θ~ω~*j*~~ is the steering angle of the *j^th^* wheel (*j* = 1,...,4) and also:

*l*^ω~*j*~^~*y*~ = (-1)^*j*^ *l*~*y*~      (8)

The non-skidding condition implies that the second term of the velocity vector in (7) vanishes. Therefore, for each mobile manipulator module these constraints can be taken into account as:

![](JMSS-1-24-g008.jpg)

Consequently, the steering angle of the wheels is uniquely determined through the following equations:

![](JMSS-1-24-g009.jpg)

Furthermore, by defining *r*^ω~*j*~^ as the radius of the *j^th^* wheel, the non-slipping constraints can be written as:\[[@ref17][@ref24]\]

*ν*~*i*~^ω~*j*~^ = ω~*i*~^ω~*j*~^ × r^ω~*j*~^z^ω~*j*~^      (12)

Now, we can obtain the angular velocities corresponding to the driving torques as follows:

![](JMSS-1-24-g010.jpg)

DESIGN OF THE TRAJECTORY UNDER CLOSED-CHAIN AND DIFFERENTIAL CONSTRAINTS {#sec1-3}
========================================================================

The general methodology of the PRMs is to construct a graph (roadmap) during a preprocessing stage that represents the connectivity of the robot\'s free configuration space and then query the roadmap using an optimal graph-search algorithm (e.g., A\*), in order to find the shortest possible path between start and goal configurations. However, most of the motion planning techniques for closed-chain mechanisms could not directly account for the differential constraints, which could render the planned trajectory infeasible. Here, we have to find a trajectory between intermediate points, generated during the path planning process, considering differential constraints.

Roadmap Construction {#sec2-3}
--------------------

Some efforts have been recently made to apply the PRM method to closed chain systems.\[[@ref12]--[@ref16][@ref20][@ref21]\] The main drawback of these approaches is that too many samples may have to be tested before finding a feasible configuration and too much computing time is spent in solving closure equations leading to imaginary values. To solve this problem, we utilize the PRM approach, with a geometrically guided sampling method called the Random Loop Generator (RLG),\[[@ref15]\] which notably increases the probability of obtaining real solutions when solving the loop closure equations \[Algorithm 1 in [Table 1](#T1){ref-type="table"}\].

###### 

RLG method for guided random sampling in the roadmap construction phase (Algorithm 1)

![](JMSS-1-24-g011)

In addition, in the large workspace of a mobile manipulator system, PRM methods require several hours of computation time to generate a well-connected roadmap.

Thus, we fix the position and orientation of both the manipulator bases first, and construct a roadmap (fixed-base roadmap), which contains *n* different self-collision-free closure configurations. Then we populate the environment in random locations with copies of the kinematic roadmap and connect the configurations of the same closure type.

The basic principle of the RLG method, as stated in algorithm 1, is grounded on separating the configuration variables into two sets: Active (q~a~) and passive (q~p~).

There are some limitations in defining active and passive variables,\[[@ref15]\] for example:

The joint space variables in *q~a~* and *q~p~* correspond to the consecutive joints in the mechanism.The number of passive variables is equal to the object\'s degrees of freedom.

If multiple choices exist for these variables that satisfy the above-mentioned limitations, all of them can be used in the method. In the remaining part of this article, we refer to active and passive variables as follows:

*q~a~* = \[θ~1~, θ~2~, θ~6~\]^*T*^      (14)

*q~p~* = \[θ~3~, θ~4~, θ~5~\]^*T*^

The planner directly acts on the active variables while the passive ones are obtained by solving loop-closure equations. An important part of algorithm 1 is to compute an interval for each of the active variables, increasing the probability of having real solutions for loop closure equations. Therefore, we should find a subset of values for each active variable that makes its workspace reachable by the remaining chain of the system. To this end, we require to illustrate the workspace of the closed chain system, which is usually a very complicated task. Thus, we have to use an approximate approach. In this regard, we simplify the model of our system as shown in [Figure 4](#F4){ref-type="fig"}.

![A simplified model of the fixed base system to be used in RLG algorithm](JMSS-1-24-g012){#F4}

Therefore, we can express the reachable workspace for each active variable as the intersection area illustrated in [Figure 5](#F5){ref-type="fig"}. The external and internal radii, *r~ext~* and *r~int~*, in the figure correspond to the maximum and minimum extensions of the chain respectively and are approximated as follows:

![](JMSS-1-24-g013.jpg)

![Subset of values for an active joint to be reachable by the remaining chain of the system](JMSS-1-24-g014){#F5}

where *L~i~* and *L~max~* are the lengths of the *i^th^* and the longest link in the chain respectively. The proof of this equation can be found in Appendix B. Then, after choosing each active variable in its computed interval, we solve the closed-chain constraint equations for passive variables. Now, the configuration vector of the fixed-base system, is written as:

θ = \[*q~a~^T^*, *q~p~^T^*\]^*T*^      (16)

Moreover, in order to consider the base structure mobility, utilizing algorithm 2 \[[Table 2](#T2){ref-type="table"}\], we populate the entire workspace with randomly selected parts of the initial roadmap. Let us choose the base configuration, *g*~ω*b*~, as a random vector including the position and orientation of the first robot, relative to the world frame:

*g*~ω*b*~ = (*x~b~*, *y~b~*, θ~*b*~)      (17)

###### 

Populating the environment with copies of the fixed-base roadmap (Algorithm 2)

![](JMSS-1-24-g015)

Then, we sample a node, θ, randomly from the initial roadmap and check the combined vector of (*g*~ω*b*~, θ) for collision. If the node is collision-free, we add it to the new roadmap. This routine continues for all neighbors of θ and is repeated for *m* different positions, to cover the entire workspace.

We collected all roadmap nodes with the same closed configuration in a set and used the PRM connection method to connect the nodes in the set, as illustrated in [Figure 6](#F6){ref-type="fig"}. Finally, we searched the graph for the shortest possible path between the start and goal configurations, with an optimal graph search algorithm.

![(a) Initial roadmap without base structure mobility;\
(b) Distributing the initial roadmap to consider the base structure mobility](JMSS-1-24-g016){#F6}

Avoiding Collision with Fixed Obstacles {#sec2-4}
---------------------------------------

Sampling-based planners must perform many collision checks in order to build a roadmap and spend most of their running time performing such checks. Therefore, their collision detection method must be very fast, without missing any collision or incorrectly detecting collisions, even when the workspace has complex geometry.

We used two types of collision checks in our motion planner: Static checks, which were used to test whether a sampled configuration in the roadmap was in the free space, and dynamic checks to test its local paths, which were continuous sets of configurations. In the static methods a common method was to break complex objects (robot link, obstacle, etc.) down, using a bounding volume hierarchy (BVH), which was a hierarchy of BVs (e.g., spheres) that approximated the geometry of the object at successive levels of detail.\[[@ref25]\]

### BVH model of the cooperative system {#sec3-1}

The choice of the type of bounding volume for a given application is a trade-off between the 'tightness of fit' and the speed of operations between two such volumes. Therefore, to build a BVH for the cooperative mobile manipulator system we use bounding spheres, which are very quick to test for collision with each other in conjunction with a more precise, but also more expensive type of bounding volume according to [Figure 7](#F7){ref-type="fig"}.

![A BVH model for the mobile manipulator system](JMSS-1-24-g017){#F7}

### Static collision checking {#sec3-2}

The basic idea behind this method is, to check two objects for collisions, their BVHs are searched from top down, making it possible to quickly discard large subsets of the objects contained in disjointed BVs. In other words, if the BVs at the top level are in collision then their children are also checked for collision. Otherwise, the algorithm will not search any of the children.

### Dynamic collision checking {#sec3-3}

The classical approach to perform dynamic checks is to sample each path at some fixed resolution and statically check each sampled configuration for collision. This approach is approximate and can miss collisions. Therefore, we use a newly presented dynamic checker\[[@ref26]\] that exactly determines whether a path lies in free space, by choosing an adaptive sampling resolution along the local path. This checker automatically decides whether a path segment between two collision-free configurations needs to be bisected further.

Let *A~i~(q)* denote an object *A~i~* from a collection of rigid objects (including each of the mobile manipulators, payload, obstacles, etc.) at configuration *q*. We define *n~ij~*(*q*) to be any non-trivial lower bound on the Euclidian distance between *A~i~*(*q*) and *A~j~*(*q*). Let λ~*i*~(*q~a~,q~b~*) be an upper bound on the lengths of the curves traced by all points in *A~i~* between configurations *q~a~* and *q~b~* along a path segment. A sufficient condition for two objects *A~i~* and *A~j~* not to collide along a path in the configuration space is:\[[@ref26]\]

λ~*i*~ (*q~a~,q~b~*) + λ~*j*~ (*q~a~,q~b~*) \< *n~ij~* (*q~a~*) + *n~ij~* (*q~b~*)      (18)

If this inequality is verified for all pairs of objects *A~i~* and *A~j~*, then the path segment is collision-free, otherwise, it must be bisected. Furthermore, to compute a lower bound on the distance between two objects we work according to the classical BVH collision checker. However, instead of testing if two BVs intersect, we compute the distance between them and find the smallest distance found.

Design of the Trajectory {#sec2-5}
------------------------

A challenging problem during the planning process is that the differential constraints shall be satisfied without missing closed-chain constraints. Here, we present an extension of the RLG algorithm to solve this trajectory planning problem. First, we describe the trajectory of each active variable between the sequential pairs of way points using the third order polynomial equations, which are expressed in a normalized interval of time as follows:

![](JMSS-1-24-g018.jpg)

where, *n~seg~* is the number of path segments, and:

![](JMSS-1-24-g019.jpg)

where, *t~fik~* is the time required to traverse the *k^th^* segment of the path. Now, we use the position and velocity values at the endpoints of the path segment to calculate the coefficients of *q~ik~* as follows:

![](JMSS-1-24-g020.jpg)

In this regard, we use a simple numerical approach to find the velocity vector in the intermediate points of the path: If the slope sign of the straight lines between each point and the previous and next ones change, the velocity is equal to zero, otherwise it is computed as the average of the two slopes. Next, utilizing loop closure equations we extract a closed form to find passive variables from (4):

![](JMSS-1-24-g021.jpg)

where:

![](JMSS-1-24-g022.jpg)

Now, we calculate the minimum time to go from one intermediate point to the next with regard to the maximum allowed accelerations. Toward this end, the following theorem establishes a lower limit for the trajectories, in the form of cubic polynomials such as those in (19).

**Theorem 1.** Consider the cubic polynomial given by the following equation:

*q~ik~* (*s*) = *a*~0*ik*~ + *a*~1*ik*~s + *a*~2*ik*~s^2^ + *a*~3*ik*~s^3^, 0 ≤*s* ≤ 1      (24)

with ![](JMSS-1-24-g023.jpg) Then the lower bound on *t~fik~* that guaranties the acceleration limits of *q~ik~* is:

![](JMSS-1-24-g024.jpg)

**Proof.** As the second derivative of a cubic polynomial is a line, its maximum value occurs in one of the corresponding endpoints:

![](JMSS-1-24-g025.jpg)

Furthermore, in view of the constraint ![](JMSS-1-24-g026.jpg) the following condition shall be satisfied:

![](JMSS-1-24-g027.jpg)

and (25) is concluded.

However, due to the nonlinearity and complexity of the equations for the wheel velocities and the passive variables, solving the acceleration condition for the exact lower bound on *t~fik~* can be prohibitively expensive and furthermore pose numerical problems. Therefore, utilizing equations (11), (13), and (22) we compute a linear approximation of the acceleration and write the following condition:

![](JMSS-1-24-g028.jpg)

where α~*jk*~ is the maximum allowed acceleration and β~*ins*~ \> 1 is an insurance factor that compensates for the approximation error and maintains the actuator acceleration in the safe bound. Obviously, using path-smoothing algorithms and increasing the number of path segments lowers the corresponding deviation from the exact value of the acceleration. Consequently, the total lower bound on the time to move between adjacent points in each section of the trajectory would be the maximum of the lower bounds computed in (25) and (30):

![](JMSS-1-24-g029.jpg)

However, to find these lower bounds, because of the lack of time response information, we utilized an approximation of ![](JMSS-1-24-g030.jpg) in each intermediate point of the path, in place of the actual velocities, and assumed that:

![](JMSS-1-24-g031.jpg)

Hence, from (20) we can write:

![](JMSS-1-24-g032.jpg)

which shows that with this assumption there are discontinuous velocities when we move between adjacent segments of the path. Therefore, we apply the bounds computed in (31) to find an approximated velocity in each intermediate point through the previously mentioned numerical approach. Then, we can write:

![](JMSS-1-24-g033.jpg)

Finally, substituting (34) into (25) and using (30) and (31) we can compute new values for *t~fk~*.

SIMULATION RESULTS {#sec1-4}
==================

Let us consider two same mobile manipulators, as shown in [Figure 2](#F2){ref-type="fig"}. To verify the effectiveness of our method we will conduct simulations based on the following assumptions:

Each mobile manipulator is subjected to the differential constraints mentioned in (9) and (13) and its goal is to cooperate with the other to satisfy the closed-chain constraints in (1)The transporting object is a rigid body that cannot be deformedThe start and goal configurations are set to:![](JMSS-1-24-g034.jpg)The environment is populated with six static obstacles in the form of spheres with specified position and radius, as shown in Figure [8b](#F8){ref-type="fig"} and [c](#F8){ref-type="fig"}Some important parameters used in the planner are chosen as per [Table 3](#T3){ref-type="table"}:

![Object\'s trajectory between the predefined start and goal configurations: (a) without obstacle, (b) with obstacles, (c) with obstacles (top view)](JMSS-1-24-g035){#F8}

###### 

Simulation parameters

![](JMSS-1-24-g036)

[Figure 8](#F8){ref-type="fig"} shows a 3-D visualization for the trajectory of the object with and without obstacles. As shown here, there is a smooth trajectory between the start and goal configurations in both cases.

Furthermore, accelerations of the computed trajectories for joint space variables are shown in [Figure 9](#F9){ref-type="fig"}, and it can be seen that they are bounded within the maximum allowed accelerations (1 rad/s^2^).

![Joint accelerations of the manipulators](JMSS-1-24-g037){#F9}

From (19-21) the trajectory of each active variable between the sequential pairs of way points is described by the third order polynomial equations. Therefore, in [Figure 9](#F9){ref-type="fig"} the accelerations of the active joints are in the form of ramp signals; however, this is not the case for the passive variables. The quality of the designed trajectory is shown in [Figure 10](#F10){ref-type="fig"}. This figure illustrates the deviation of the joint space variables from their final values.

![Errors of the joint space variables: (a) without obstacle, (b) with obstacles](JMSS-1-24-g038){#F10}

It can be seen that the errors converge to zero as time goes on. [Figure 11](#F11){ref-type="fig"} illustrates the time required to reach the goal configuration.

![The distance between current and goal positions of the first end effector: (a) without any obstacle, (b) with static obstacles](JMSS-1-24-g039){#F11}

CONCLUSIONS {#sec1-5}
===========

A novel trajectory planning method has been designed for dual mobile manipulators grasping a common object. Because of its structure, our method can handle high dimensional configuration spaces efficiently.

It also utilizes the advantage of decoupled methods to decompose the complexity of the trajectory planning problem in two steps, which increases the simplicity of the planning process. The main advantage of this method is that such constraints as nonholonomic and closed-chain constraints, along with the joint and acceleration limits, can be easily dealt with in the presence of static obstacles. In comparison with the other approaches discussed in literature, the advantages of the proposed method have been shown in [Table 4](#T4){ref-type="table"}.

###### 

Comparison of recent approaches on motion planning for cooperative mobile manipulators

![](JMSS-1-24-g040)

Furthermore, the results of computer simulations confirmed the effectiveness of the method. A good idea to improve the algorithm proposed in this study is to develop a method to keep the system from colliding with the moving obstacles. Also, investigating the trajectory planning of more than two cooperating mobile manipulators to perform the carrying task seems to be an interesting route to follow.

APPENDIX {#sec1-6}
========

Detail Derivation of Equation (1) {#sec2-6}
---------------------------------

Utilizing the coordinate systems in [Figure 2](#F2){ref-type="fig"} along with the concept of transformation matrices, we can write:

![](JMSS-1-24-g041.jpg)

in which ^*A*^~*B*~*T* is a matrix that transforms the coordinate system {*B*} into {*A*}, and the different coordinate systems in the above equation are defined as follows:

Therefore, from (A.1), after further manipulation, the validity of (1) can be verified.

Proof of Equation (15) {#sec2-7}
----------------------

The reachable workspace of any articulated mechanism can be bounded by two concentric spheres, as shown in [Figure 5](#F5){ref-type="fig"}. The radii of these spheres correspond to the length of the mechanism in the case of minimum and maximum ![](JMSS-1-24-g042.jpg) extensions. This length is defined by the distance between the origins of the base-frame and the end-frame as:

![](JMSS-1-24-g043.jpg)

where *L~i~* (*i* = 1,...,*k*) is the length of *i^th^* link and *a~i~* ∈ \[--1,1\] is a factor that maps these lengths in the direction of the vector, connecting the origins of the base-frame and the end-frame. Thus, by substituting *a~i~* = --1 or *a~i~* = 1, ∀*i* ∈ {1,...,*k*} in (B.1) we can compute a maximum value for *r* as follows:

![](JMSS-1-24-g044.jpg)

Now, suppose that the *j^th^* link of the system has the longest length, *L*~max~; then, from (B.1) one gets:

![](JMSS-1-24-g045.jpg)

Therefore, we can write the following equation to compute a minimum value for *r*:

![](JMSS-1-24-g046.jpg)

and from (B.2) we can conclude that:

![](JMSS-1-24-g047.jpg)

which shows the validity of (15).

BIOGRAPHIES {#sec1-7}
===========

**Hossein Bolandi** received his D.Sc. degree in electrical engineering from George Washington University, Washington, D.C., in 1990.

Since 1990, he has been with the College of Electrical Engineering, Iran University of Science and Technology, Tehran, Iran, where he is an associate professor. His research interests are in attitude determination and control subsystems of satellites, robotics, and adaptive control.

Dr. Bolandi is the author of 25 journal articles and 105 articles in the international and Iranian conference proceedings.

**Amir Farhad Ehyaei** received the B.Sc. degree in control engineering from the Sharif University of Technology, Tehran, Iran, in 2001. He received the M.Sc. degree in control engineering in 2003, from the Iran University of Science and Technology, Tehran, Iran, where he is currently a Ph.D. candidate. His main fields of interest include navigation and control of multi-robot systems.

Since 2004, he has worked with several companies in the field of Instrumentation and Process control including Power Engineering Consultants of the Ministry of Energy (MOSHANIR) and Iran International Engineering Company (IRITEC).
