Real-Time Trajectory Generation Methods for Cooperating Mobile Manipulators Subject to State and Control Constraints

This paper deals with the real-time trajectory generation problem for two cooperating mobile robots moving the common rigid object. The holonomic constraints resulting from a closed kinematic chain and the dynamics of such a system are considered. Two methods of generation sub-optimal trajectories allowing for mechanical and control limitations and collision avoidance conditions are proposed. The first solution is based on a leader-follower approach, in the second one the robotic system is treated as a whole mechanism. In both cases, the trajectories are generated in order to avoid singularities and they are scaled to satisfy control constraints. Advantages and disadvantages of both presented approaches are discussed. A computer example involving two mobile manipulators consisting of nonholonomic platform (2,0) class and planar 3-DOF holonomic manipulator is presented.


Introduction
There is a class of tasks that cannot be executed by a single robot. They are associated with the movement of heavy objects that a single robot cannot lift due to physical limitations of its actuators. Another example of these tasks is the movement of the large objects which must be supported at several points. The necessity of performing such tasks leads to rise the interest in the trajectory generation for multiple robots performing the common task. Combining the mobility of the platform with dexterity of the manipulator, the mobile manipulators seem to be especially attractive in this application. The platform significantly increases the operating range of the robot, and therefore such a system may be used to transport objects over long distances. Moreover, after reaching the desired location, dexterity of the manipulator allows to the precise placement of the object in the desired position.
Solving the trajectory generation problem for multiple mobile manipulators is more complicated than for a single Iwona  robot. At first, limitations arising from the physical abilities of the cooperating robots have to be taken into account. Additionally, the use of the mobile robots with nonholonomic platforms requires consideration of pure rolling and non-slip conditions. Moreover, the robots moving the common object create the closed kinematic chain which imposes an additional constraint that has to be allowed for.

Related Works
In the literature there are several approaches to the motion control for mobile manipulators. Some solutions employ the calculus of variations. This approach has been presented by Desai and Kumar in [7,8] to formulate a general approach in order to obtain optimal trajectories for nonholonomic cooperating mobile robots. Solutions based on a pseudoinverse of the Jacobian matrix have been proposed in [11,12] in order to generate real-time collision-free trajectories. Seraji in [24] has used the augmented Jacobian matrix and has introduced an on-line approach for motion control on the kinematic level. The method discussed in this paper employs the augmented Jacobian matrix, however it is applicable to cooperative robot tasks, generated trajectories take account of the dynamics of the system, control limitations and state constraints.
In the recent years an increasing research interest in the multi-robot systems has been noticed. A large number of studies concern problems of motion control for multiple robots performing a common task in the same workspace. Some works consider control design problems for groups of vehicles which have to achieve a specified location [29] or move in a given formation [17]. Such solutions are often limited to use only the mobile platforms, thus the robots do not have manipulation abilities. Moreover, the method of this type cannot be directly adopted to solving the cooperative transportation tasks by multiple mobile robots. In the case of such tasks, two general methods are used: pushing and grasping. In the first approach the object is pushed by a group of the robots [6,9,30], however, in this case the motion of the object cannot be controlled precisely.
In the second approach robots are equipped with graspers and they hold the object during transportation, and therefore the more precise manipulation is possible. The solution of such tasks is more difficult due to the necessity to satisfy constraints resulting from the closed kinematic chain established by cooperative robots and the object held by them. The methods of motion control for multiple cooperative robots have been first developed for multiple manipulators. These studies are further intensively developed especially in the case of coordinated manipulation tasks, such as bolt and nut mating [32] and transportation tasks [10,14]. Stationary manipulators have a limited workspace, thus they cannot be used to carry long objects over long distances. Due to the mobility of the platform, mobile robots are more suitable for this type of tasks. The ideas used in the case of cooperative manipulators may be employed for motion control of mobile robots but the control task is more complicated due to limitations resulting from a platform movement.
Research carried out in the field of multiple robots motion control has been aimed at solving the problem using centralized and decentralized approaches. In the centralized methods, motion control applies to all cooperating robots simultaneously. The methods of this type are based on full knowledge of the environment and they concentrate on searching an optimal solution. Desai and Kumar in [7,8] have presented the method based on calculus of variations for optimal control for mobile manipulators cooperating in the workspace including obstacles. In [26] Tanner et al. have discussed globally asymptotically stable centralized feedback controller for mobile manipulators moving a deformable object in the environment including obstacles.
While the centralized methods usually lead to algorithms with high computational complexity, the decentralized approaches are computationally more efficient due to decomposition of the motion control task. In [15,18] the authors have considered a leader-follower motion control algorithm for multiple mobile manipulators moving a common object. In this approach, each robot has had a caster-like dynamics in 3D space and an object has been manipulated without using the geometric relations among the robots. Luh and Zheng in [19] have proposed a leader-follower approach in which they derived the relation between the joint positions and velocities of the leader and the follower as a set of holonomic equality constraints. Other leader-follower approaches have been presented in [25,27]. Sugar and Kumar in [25] have discussed the motion control method for a small team of mobile manipulators that cooperatively transport a large, flexible object in a two dimensional environment with obstacles. In [27] Trebi-Ollennu et al. have proposed an decentralized algorithm for two Mars rovers carrying a long object over an uneven, natural terrain. Abou-Samah et al. in [2] have considered the leader-follower approach for cooperative payload transport based on the individual mobile manipulator control schemes. In [13] a decentralized hybrid position/force controller has been presented for the control of multiple tightly coupled manipulators handling a common object. Adaptive control law developed from the virtual decomposition approach has been designed by Brahmi et al. in [3].

Summary of the Proposed Solution
In this paper, two trajectory generation methods for mobile manipulators moving the common rigid object are presented. The proposed solutions are the development of the method discussed in [21] and shows that an approach for a single mobile manipulator can be easily adopted to the cooperative task of two mobile robots. In the first method, the trajectory generation problem is solved by using the leader-follower conception. In this approach, the leader determines the motion and the follower assists the leader supporting the object. In the presented work, the leader performs the point-to-point task moving its end-effector to the final location resulting from the desired pose of the object. The follower tracks the trajectory imposed by the leader motion ensuring the fulfillment of the constraints which results from the closed kinematic chain formed by cooperating robots. This approach is the development of the idea presented in [23]. In the second method, the trajectory generation problem is solved in a centralized way. In this case, the both robots are treated as the one system constrained by conditions resulting from the closed kinematic chain and they perform the point-to-point task moving the object to its final location. The solution scheme for both proposed approaches is similar, however, each of them has some advantages and disadvantages that can be crucial in choosing a particular approach for a particular application. These issues will be detailed discussed in Section 6.
In both presented methods, the trajectories of the robots are generated to minimize the performance indexes to avoid manipulators singularities. In the first case, maximization of the manipulability measures of the leader and the follower is carried out. In the second one, the total manipulability of two cooperative robots is maximized. Additionally, the constraints causing by mechanical and control limitations as well as constraints connected with the potential collisions between cooperating robots, are taken into account. In the proposed solutions, the task is transformed into an optimization problem with holonomic and nonholonomic equality constrains, and inequality constraints resulting from collision avoidance and mechanical limitations. Finally, the resulting trajectories are scaled to fulfill the constraints imposed on the controls. As it is shown further on, in the leader-follower approach, controls of the system depend on the motion imposed by the leader, thus satisfying control constraints is obtained by trajectory scaling of the leader only. In the centralized approach, controls of the system depend on the trajectory of the whole system, and therefore in this case, the trajectory of both cooperating robots is scaled to fulfill control constraints.
To the best of the author knowledge, no research has considered the cooperative task formulated in the above manner. Some of the existing research addresses the problem using only kinematic equations of the mobile manipulators [19,24,26] and the dynamics of the robots is not considered at all. The studies considering the dynamics of the cooperative systems do not allow to take into account control constraints [2,3,7,8]. In contrast, the presented solution takes into considerations dynamics of the cooperative system, moreover, it is distinguished by the method of determining the continuous controls which fulfill the assumed constraints. The results of the proposed approach are trajectories parameterized by gain coefficients, which can be chosen in such a way to fulfill the imposed control constraints. The proposed trajectory generation algorithm does not need the knowledge of the system dynamics, the dynamic model is necessary to determine the values of gain coefficients only. Other research considering the dynamics of the robots produces controls parameterized by gain coefficients of the controller directly. In such cases, it is very difficult (or impossible) to find coefficients which fulfill control limits.
The paper is organized as follows. Section 2 presents kinematic and dynamic model of cooperating robots. Section 3 formulates the cooperative task, constraints taken into account and general idea of proposed control system. Two approaches to solve the problem formulated in Section 3 are presented in Sections 4 and 5. The first solution is based on a leader-follower approach, in the second one the robotic system is treated as a whole mechanism. Each of these sections includes derivation of the manipulability measure of the considered system, the proposed form of the trajectory generator and approach used to satisfy control constraints. Advantages and disadvantages of presented methods are discussed in Section 6. The proposed methods are demonstrated numerically in Section 7, for two mobile manipulators consisting of a nonholonomic (2,0) class platform and a 3-DOF planar holonomic manipulator moving the rigid beam in 2D space.

Model of the System
In the cooperative task considered in this paper two mobile manipulators are moving the common rigid object, firmly connected to their end-effectors, to the specified location in the workspace. Manipulators and an object form a closed kinematic chain, therefore knowledge of the kinematics and dynamics of such a system is necessary to formulate the task as a consequence of a cooperation.

Kinematics
In order to describe the kinematics of the closed kinematic chain consisted of two mobile robots and an object carried by them, the location of the object in the workspace must be specified. For this purpose, as it is shown in Fig. 1, a local coordinate frame {O} attached to the object at any given point is introduced. The pose of this local frame in a global reference frame {B} specifies the location of the object is described by the m-elemental vector p: where x and ϕ are the vectors describing position and orientation of the local frame attached to the object in a global reference frame. In this paper, it is assumed that each of cooperating robots is composed of a nonholonomic platform and a holonomic manipulator with kinematic pairs of the 5th class. They are described by the vectors of generalized coordinates: where q i is the n i -dimensional vector of the generalized coordinates of the i-th mobile manipulator, q p,i is the p idimensional vector of the coordinates of the i-th nonholonomic platform, q r,i is the r i -dimensional vector of joints coordinates of the i-th holonomic manipulator, n i = p i +r i . In order to describe the geometry of the grasp the approach of a virtual stick is adopted [28]. In this case the virtual stick is a constant vector which determines the origin of the object frame in the manipulator end-effector frame. In Fig. 1 two manipulators holding a common object are presented. The vectors vs 1 and vs 2 represent virtual sticks of the first and second robot. They connect the origins of frames {G 1 }, {G 2 } attached to the end-effectors of both robots with the origin of the object frame {O}. Additionally, at the end points of vs 1 and vs 2 the virtual end-effectors with the same orientation as the object frame are introduced. The idea mentioned above is used in this work to describe geometry of the closed kinematic chain formed by two cooperating robots.
Using the virtual stick approach, the location of the object can be expressed with respect to generalized coordinates of the mobile manipulator as: where f i : n i → m denotes the m-dimensional mapping, which describes the position and orientation of the virtual end-effector of the i-th manipulator in the workspace.
Since there are no relative motions between the virtual endeffectors of two cooperating robots (assuming the object is rigid), the following geometric and kinematic constraints should be fulfilled: where J i = ∂f i (q i ) /∂q is the (m × n i ) Jacobian matrix of the i-th manipulator and v denotes the common velocity of the virtual end-effectors of the robots in the workspace. Moreover, due to use of mobile robots, it is necessary to take into account holonomic and nonholonomic constraints which limit the platform motion capabilities. The constraints of this type, for the i-th platform, can be described in the Pfaffian form as: where A i (q p,i ) is the (h i × p i ) Pfaffian full rank matrix and h i is the number of independent holonomic and nonholonomic constraints.

Dynamics
The cooperation causes that manipulators and object form a closed kinematic chain, and consequently reduces the degree of freedom of the system. Thus, owing to the redundant number of the actuators, this system can be considered as a rigid over-actuated multi-body system. After disconnection of this multi-body system at the origin of the object frame [16], the dynamic equations for i-th robot are given as: where M i (q) is the (n i × n i ) positive definite inertia matrix, F i (q i ,q i ) is the n i -dimensional vector representing Coriolis, centrifugal, viscous, Coulomb friction and gravity forces, M i and F i contain both robot and object dynamic properties, A i (q) = A i 0 , 0 is the (h i × r i ) zero matrix, λ i is the h i -dimensional vector of the Lagrange multipliers corresponding to constraints (6) and B i is the (n i × (n i − h i )) full rank matrix (by definition) describing which state variables of the mobile manipulator are directly driven by the actuators. Introducing the combined vectors q = q T 1 , q T 2 T ,q = q T 1 ,q T 2 T and the matrix J (q) = [J 1 , −J 2 ], the kinematic constraints (5) can be expressed in a compact form as: therefore the overall dynamic equations for the cooperating robots taking into account both platform (6) and closed kinematic chain (8) constraints, can be written as: vector of the Lagrange multipliers corresponding to the constraints (8).
A (q) is the full rank matrix, thus there exists the full rank (n 1 + n 2 )×(n 1 + n 2 − h 1 − h 2 ) matrix N p (q), orthogonal to A (q), which satisfies the relation: Using the above dependency and multiplying the dynamic equation (9) by N T p , the vector of Lagrange multipliers λ p can be eliminated and the model takes a new form as: Assuming that the matrix J N p is the full rank (rank J N p = m), the full rank (n 1 + n 2 can be determined in such a way that the following relation is fulfilled: Finally, multiplying dependency (11) by N T c , the dynamic model of the system is reduced to the form given below: Cooperative robots moving the common rigid object form an over-actuated dynamic system, thus there is no unique solution of the Eq. 13 for u. Assuming the full rank of matrix N T c N T p B , the inverse dynamic model of the system may be determined as: where

Problem Formulation
This paper considers the cooperative task of two mobile manipulators moving the common rigid object to the specified location in the workspace. Using the vector p describing the location of the object (1), this task can be formulated as displacement of the object from the given initial location p 0 to the final location p f .
Additionally, the motion of the mobile robots have to take into account mechanical limits, boundary constraints resulting from the task and collision-free conditions. At the initial moment of motion the manipulators are (by assumption) in the collision-free configuration for which the virtual end-effectors are in the initial location p 0 with zero velocity: At the final moment of motion the both virtual end-effectors have to reach the final location p f with zero velocity: where T stands for an unknown time of performing the task. Conditions resulting from the mechanical limits and constraints connected with the potential collisions between cooperating robots or between robots and object carried by them can be written as a set of inequalities: where ψ k,i is the scalar function which involves the fulfillment of the constraints imposed by the i-th robot mechanical limits and collision avoidance conditions, l i stands for the total number of constraints. In practice, the configuration of mobile manipulator joints should be far away from singular configurations. The distance from singularities can be defined based on various manipulability measures. The next sections will discuss manipulability indexes adopted in this paper. Regardless of the form of the manipulability measure, to maximize the distance from the robot singular configurations, in this paper minimization of certain instantaneous performance index is used. This index may be in the general form written as: where σ is the small positive coefficient and w (· ) denotes the manipulability measure (explicit forms are given in the following sections).
Additionally, each control should be within the limits defined by the physical capabilities of the actuator, then the following limitations for controls u 1 and u 2 for the first and the second robot, should be satisfied: where u 1,j 1 is the j 1 -th control (torque or force) of the first robot, u 2,j 2 is the j 2 -th control (torque or force) of the second robot, and u min,1,j 1 , u max,1,j 1 , u min,2,j 2 , u max,2,j 2 are the lower and upper limits on u 1,j 1 and u 2,j 2 , respectively.
To solve the cooperative task satisfying kinematic constraints (4)-(6) and limitations resulting from the construction of the robots and the nature of the task (15)-(19), the control system shown schematically in Fig. 2 is proposed. As can be seen, the problem will be solved by a real-time trajectory generator working in the closed loop. The input signal of the generator is the error vector E calculated based on the desired location of the object and its current location obtained from a forward kinematic model (FKM) given by dependency (3). The other components of vector E result from holonomic and nonholonomic platform constraints (PC), described by Eq. 6, and additional constrains (AC) introduced by dependencies (4), (5), (17), (18). The controls are determined from the dynamic model of the system (model) given by dependency (14). The main advantage of the proposed method is that, in contrast to other solutions, it generates trajectories ensuring fulfillment of both the state and control constraints.
In the next sections two trajectory generators are considered. The first one is based on the leader-follower conception [19]. In this approach, the leader determines the motion and the follower assists the leader supporting the object. In the second one, the centralized approach is presented and the cooperating robots are taken into account together and treated as the one system. The solutions are based on the previous author's work on the trajectory generation for a single mobile manipulator [21] and use of the penalty function approach and a redundancy resolution at the acceleration level. The advantages and disadvantages of both solutions are also discussed.

Leader-Follower Approach
In this solution cooperating mobile manipulators are considered as two individual systems called the leader and the follower. The main task of the first mobile manipulator (leader) is to move its virtual end-effector from the initial location p 0 to the final location p f . The second robot (follower) has to follow the leader, tracking the trajectory determined by its virtual end-effector. During this motion both robots should allow for constraints described by dependencies (4)- (19) and minimize performance indexes (18). Section 4.1 presents the manipulability measure for the single robot. In Sections 4.2 and 4.3 the methods of trajectory generation for the leader and the follower satisfying constraints (4)-(17) are considered. Trajectory scaling technique leading to fulfillment of control constraints (19) is presented in Section 4.4.

Manipulability Measure
In order to avoid singular configurations the manipulability of mobile manipulators moving the common object has to be taken into account. In the solution presented in this section, the manipulability measure is considered for each subsystem independently, and it is based on the manipulability ellipsoid introduced by Yoshikawa [31]. In this approach the relationship between unit norm generalized velocities of the robot and its virtual end-effector velocity in the workspace is determined. The unit norm velocities in the configuration space fulfill the following dependency: Using the Moore-Penrose pseudoinverse of Jacobian matrix J i , velocities in the workspace can be determined from Eq. 5 (20) can be rewritten as: Thus, a unit sphere from the configuration space is mapped into an ellipsoid in the workspace. According to Yoshikawa, the manipulability of the robot can be measured as proportional to the volume of this ellipsoid as:

Trajectory Generation for the Leader
The main task of the leader is to move its virtual endeffector to the final location p f in such a way as not to violate constraints (6). Additionally, the trajectory of the robot has to satisfy inequality conditions (17) and reach the minimum value of the performance index (18) in each time instant. To solve this problem, an approximate implementation of inequality constraints (17) is accepted in this work. The approach is to use penalty functions which cause the inequality constraints to be satisfied, but the performance index (18) to be somewhat increased. In this case the instantaneous performance index (18) can be expressed as: where κ i,1 is the penalty function which associates a penalty with a violation of a leader constraint, see Numerical example section for an exemplary form of this function.
In order to find a trajectory of the leader minimizing the performance index (23), first let us consider the problem of finding an optimal robot configuration q 1 (T ) at the final location p f : Necessary condition to obtain the minimum of the performance index I 1 (q 1 ) is that the differential of I 1 at q 1 (T ) equals zero: J R,1 , obtained by choosing elements from δq 1 related to columns of the matrix J R,1 and δq F,1 is the vector obtained by excluding δq R,1 from δq 1 .
Multiplying the left side of Eq. 27, the related vector δq R,1 can be expressed in terms of free vector δq F,1 as: Then, taking into account dependency (28), necessary condition (25) may be rewritten as: where I R,1 is the (m + h 1 ) dimensional vector corresponding to the vector δq R,1 , obtained by choosing elements from I q 1 related to components of the vector δq R,1 and I F,1 is the vector obtained by excluding I R,1 from I q 1 . The consequence of the free vector δq F,1 in Eq. 29 is the following equation (called transversality condition): Equation 30 introduces (n 1 − m − h 1 ) dependencies which in combination with the equality conditions from Eq. 24 allow to obtain an optimal configuration for a given final location. Then, in order to find a trajectory of the leader, mapping E 1 (q 1 ,q 1 ), which may be interpreted as a measure of the error between a current configuration q 1 (t) and unknown final configuration q 1 (T ), is introduced as follows: The m-first components of E 1 are responsible for reaching the given final location p f , the h 1 -last items ensure fulfillment of constraints (6). The remaining n 1 − m − h 1 dependencies are derived above from the necessary condition for the minimum of the performance index (23), thus they are responsible for the fulfillment of constraints (17) as well as minimizing performance index (18).
Applying the error measure (31), the dependency specifying the trajectory of the leader to the final location p f is postulated as the following system of differential equations: where E I 1 is constructed from the first two components of mapping E 1 , The proposed form of dependency (32) results from the necessity to determine trajectory at the acceleration level.
Hence, the second order differential equation with respect to q 1 is needed. Equation 32 is a system of homogeneous differential equations with constant coefficients. In order to solve it and find the trajectory of the mobile manipulator 2n 1 − h 1 consistent dependencies should be given. These dependencies are obtained from E 1 for t = 0 taking into account conditions (15), especially zero initial velocity.
As it has been shown in [21], the proposed form of differential equation (32) ensures that its solution is asymptotically stable which implies fulfillment of the conditions (16) for the leader. What is more, if corresponding elements of the main diagonals of matrices I V ,1 and I L,1 satisfy condition λ I V ,1 > 2 λ I L,1 , the solution is also a strictly monotonic function, thus for the initial nonsingular configuration q 1 (0) fulfilling mechanical and collision avoidance constraints (17) leader motion is free of singularities and fulfills constraints (17) during the movement to the final location p f .
In order to obtain trajectory of the leader, Eq. 32 can be rewritten in the expanded form: where E 1 = (E I q,1 ) T (E I İ q,1 ) T T . Finally, the trajectory of the leader is given by:

Trajectory Generation for the Follower
The task of the second robot is to follow the trajectory of the leader virtual end-effector d (t). This trajectory can be determined from the forward kinematic model of the leader (4), as follows: Just as the leader the follower, during its motion, should also keep inequality constraints (17) and avoid singular configurations. In this case, the same approach as for the leader is employed and the extended performance index I 2 (q 2 ) is defined in the same way as I 1 (q 1 ), as follows: where κ i,2 is the penalty function which associates a penalty with a violation of the follower constraint, see Numerical example section for an exemplary form of this function.
In the case of finding trajectory for the follower, the problem of determining an optimal robot configuration q 2 (t) at the given time instant t should be considered: Similarly as in previous subsection, using necessary condition of performance index (37), transversality condition for the follower can be derived as: where I R,2 , I F,2 , J R,2 , J F,2 are defined in similar way as The counterpart of the error measure E 1 (31) for the follower is the mapping E 2 (q 2 ,q 2 ) defined as follows: In this case the m-first components of the above dependency result from the grasp constraint of the two cooperating robots (4) and describe an error between a current follower virtual end-effector location and trajectory d (t). The next n 2 − m − h 2 dependencies are obtained from the necessary condition for the minimum of the performance index (37) and they ensure the fulfillment of constraints (17) as well as minimizing performance index (18) for the follower. The h 2 -last items are responsible for fulfillment of constraints (6) of the second robot.
To find the trajectory of the follower the same approach as in the previous subsection is used. For this purpose, the differential equation for mapping E 2 , defined in accordance to Eq. 32, is introduced: where E I 2 , E I I 2 are constructed from the mapping E 2 in similar way as for the trajectory of the leader (32) Finally, the trajectory of the follower takes the following form: vector, E 2 , E I q,2 , E I İ q,2 ,Ė I q,2 , E I I q,2 are defined in similar way as for the trajectory of the leader (33).
The trajectory q 2 (t) is the result of solving a system of differential equations whose form ensures that its solution is asymptotically stable for positive definite diagonal matrices the solution is also a strictly monotonic function, thus for the initial nonsingular configuration q 2 (0) fulfilling constraints (4), (5) and (17), the follower motion is free of singularities and fulfills constraints (4), (5), (17) during its movement to the final location p f .

Control Constraints
To satisfy control constraints (19) the development of the methods presented in [22] for stationary cooperating robots and in [21] for single mobile manipulator is proposed. Denoting: and introducing the following substitutions: Denoting by E I 1 (m) , Ė I 1 (m) and Ë I 1 (m) the vectors built by the m-first coordinates of the E I 1 ,Ė I 1 andË I 1 , respectively and taking into account (36), it is easy to show from Eq. 31 that: The m-first dependencies from Eq. 32 can be rewritten as: can be written in the expanded form as: Introducing the matrix A 0 : it is easy to show that the trajectory of the leader virtual end-effector is linear with respect to 1 : Finally, inserting (47) into the trajectory of the follower (42) and introducing substitutions: the trajectory of the follower (42) can be also written as linear with respect to 1 : Substituting (43) and (48) into the dynamic model (14), controls of cooperating robots can be determined as: As it is seen, controls of both cooperating robots are linear with respect to gain coefficients of the leader 1 . In order to find controls fulfilling constraints resulting from limitations of mobile robots actuators, dependency (49) is substituted into conditions (19) and the following system of inequalities is obtained: The dependency (50) introduces (n 1 − h 1 ) + (n 2 − h 2 ) inequalities, thus choice of the leader such that (n 1 − h 1 ) ≥ (n 2 − h 2 ) results in dim( 1 ) = 2 (n 1 − h 1 ), hence it is possible to determine 1 ensuring fulfillment of constraints (19). An alternative approach based on scaling of the trajectory using certain heuristics can be found in [20].

Centralized Approach
In this solution cooperating mobile manipulators are considered as one centralized system, and therefore both robots are treated equally and none of them performs the role of the leader. The manipulability measure of this system is considered in Section 5.1. The centralized trajectory generation method is presented in Section 5.2. Section 5.3 shows application of the trajectory scaling technique to fulfillment of the control constraints (19).

Manipulability Measure
This section considers the total manipulability measure of the system consisting of two mobile robots moving the common rigid object. Based on the manipulability ellipsoid method for two cooperative robots [5], since there are no relative motions between the virtual end-effectors of the robots, kinematic constraints (5) are written in a matrix form as: where ϑ = v T , v T T is the combined vector of the endeffectors velocities and J = blockdiag (J 1 , J 2 ) is the block-diagonal matrix combined from the Jacobian matrices of both robots. Taking into account (51), the unit norm velocities in the configuration space: are transformed into an ellipsoid in the workspace as follows: The dependency (53) can be easily transformed to show relation with the Jacobian matrices of both robots: therefore the manipulability of cooperating robots can be measured as:

Trajectory Generation
In order to find, in a centralized way, the trajectory of both robots fulfilling inequality constraints (17) and avoiding singular configurations, the extended performance index I (q) is defined in the similar way as for individual robots: where κ i,1 , κ i,2 are the penalty functions which associates a penalty with a violation of the first and the second robot constraint, see Numerical example section for an exemplary form of this function.
In the case of two cooperative robots considered as the one system, the problem of finding an optimal robots configuration q (T ) at the final location p f should be considered: Similarly as in previous section, transversality condition for the cooperating robots can be derived from the necessary condition for minimum of the performance index I (q): where I q (q) = ∂I/∂q and δq is the variation of q.
At the configuration q (T ) equality constraints from Eq. 57 have to be fulfilled. After differentiating the first two constraints, they can be expressed in a compact form as: where J = [J 1 0] T , [J 1 − J 2 ] T , blockdiag(A 1 , A 2 ) T T is the nonsingular matrix (nonsingularity is forced by minimization procedure).
Since J is the full rank matrix, it is possible to written the above dependency as: where J R is the square matrix constructed from (2m + h 1 + h 2 ) linear independent columns of J, J F is the matrix obtained by excluding J R from J, δq R is the (2m + h 1 + h 2 ) dimensional vector corresponding to the reduced matrix J R , obtained by choosing elements from δq related to columns of the matrix J R , and δq F is the vector obtained by excluding δq R from δq.
Multiplying the left side of Eq. 60, the related vector δq R can be expressed in terms of free vector δq F as: Then, taking into account the dependency (61), necessary condition (58) may be written as follows: where I R is the (2m + h 1 + h 2 ) dimensional vector corresponding to the vector δq R , obtained by choosing elements from I q related to components of the vector δq R and I F is the vector obtained by excluding I R from I q .
The consequence of the free vector δq F in Eq. 62 is the following equation: The total error measure E (q,q) of whole robot system is determined analogously to error measures introduced earlier in Section 4: In the similar way as in Section 4, using the error measure (64), the dependency specifying the trajectory of the whole system of cooperating robots is proposed as a following system of differential equations: where E I is constructed from the first three components of mapping E, E I I is formed from the remaining components of E, I V , I L are the (n 1 + n 2 − h 1 − h 2 ) square diagonal matrices with positive coefficients ensuring the stability of the equilibrium of the first equation and I I L is the (h 1 + h 2 ) square diagonal matrix with positive coefficients ensuring the stability of the equilibrium of the second equation.
Similarly as in previous section, the trajectory of cooperating robots can be determined by simple transformation from Eq. 65 as: where E = (E I q ) T (E I İ q ) T T , E I q = ∂E I /∂q,Ė I q = dE I q /dt, E I İ q = ∂E I I /∂q, E I I q = ∂E I I /∂q. As in the leader-follower approach, suitable choice of the gain coefficients I V , I L and I I L leads to solution which is asymptotically stable and strictly monotone. These properties imply that the robots reach the final location p f with zero velocity and their motion is free of singularities and fulfills constraints (4), (5), (17) during the movement if initial configurations are nonsingular and satisfy constraints (4), (5) and (17).

Control Constraints
To satisfy control constraints (19), trajectory (66) is scaled in similar was as in leader-follower approach. Denoting: and introducing the following substitutions: Similarly as in previous section, substituting (67) into dynamic model (14), controls of cooperating robots can be determined as: As it is seen, in contrast to leader-follower approach, in this case controls are linearly dependent on gain coefficients of both cooperating robots. In order to find controls fulfilling constraints resulting from limitations of mobile robots actuators, dependency (68) is substituted into conditions (19) and the following system of inequalities is obtained: The dependency (69) introduces (n 1 − h 1 ) + (n 2 − h 2 ) inequalities whereas dim( ) = 2 (n 1 − h 1 + n 2 − h 2 ), thus it is possible to determine ensuring fulfillment of constraints (19).

Discussion
It seems that the both presented approaches are computationally effective and allow to find solutions satisfying all the imposed constraints which is confirmed by the results of the simulations presented in Numerical example section. However, each of them has some advantages and disadvantages.
The methods described in this paper are based on the similar approach in which trajectories of the robots are derived from differential equations using suitable defined error measures. The both proposed trajectory generators take state dependent constraints into account, maximize the manipulability measure, and using suitable scaling techniques determine trajectories satisfying control constrains. According to the idea of the proposed control system shown in Fig. 2, the input signal of the trajectory generator is the error measure determined based on the current state of the system. The output signals q,q,q are obtained by solving the initial value problem given by dependencies (32) and (41) for the leader-follower approach or Eq. 65 for the centralized approach. In each case, initial conditions are determined by using a suitable error measure for t = 0 allowing for conditions (15).
The solution scheme for both proposed approaches is similar, however, there are important differences that can be crucial for choosing a particular approach for a particular application. These issues will be discussed in the following paragraphs.

Computational Burden
Regardless of the used approach, finding the trajectory of cooperating robots requires solving the initial value problem given by system of (n 1 + n 2 ) secondary differential equations. The differences in a computational effort are related to the cost of determining the components of these equations.
Let us compare the computational effort needed to determine the error measures in the case of the leader-follower (31), (40) and centralized approach (64). All components from E 1 and E 2 are included in the error measure E, however, the leader-follower approach requires computing two separate transversality conditions -one for the leader (30), and one for the follower (39) and in the centralized approach the single transversality condition (63) for both cooperating robots is determined. In the first approach, robots are considered as two individual systems, thus J 1 , J 2 are matrices of dimensions ((m + h 1 ) × n 1 ) and ((m + h 2 ) × n 2 ) and vectors I q 1 , I q 2 have n 1 and n 2 elements, respectively. In the second method, robots are treated as one centralized system, in this case J is ((2m + h 1 + h 2 ) × (n 1 + n 2 )) matrix and I q is (n 1 + n 2 ) elemental vector. The greater number of elements in the matrix J, in the case of the centralized approach, leads to more operations needed to determine the transversality condition. In particular, in the leaderfollower approach it is necessary to invert two matrices with dimensions (m + h 1 ) × (m + h 1 ) and (m + h 2 ) × (m + h 2 ) and in the centralized approach one (2m + h 1 + h 2 ) × (2m + h 1 + h 2 ) matrix has to be inverted.
The higher calculation costs associated with the determination of the error measure E result in higher costs of computing E I q ,Ė I q andĖ I . Moreover, in the centralized approach, derivatives are calculated with respect to combined vector q = [q T 1 , q T 2 ] T . It causes dimensions of matrices E I q , E I İ q , and in a consequence E is ((n 1 + n 2 )× (n 1 + n 2 )) matrix andĖ I q is ((2m + h 1 + h 2 ) × (n 1 + n 2 )) matrix. In the case of the leader-follower approach, derivatives are calculated with respect to q 1 and q 2 , thus dimensions of matrices E 1 , E 2 are equal to (n 1 ×n 1 ) and (n 2 ×n 2 ), whereas dimensions ofĖ I q,1 ,Ė I q,2 are equal to ((m + h 1 ) × n 1 ) and ((m + h 2 ) × n 2 ), respectively. Determining the trajectories of robots requires inverting the matrices E 1 and E 2 and calculation of productsĖ I q,1q 1 ,Ė I q,2q 2 in the leader-follower approach and the inversion of matrix E and calculation of productĖ I qq in the centralized approach. The larger matrices size in the second case results in higher computational burden and it can lead to larger computation time.

Secondary Cost Functions
According to the above analysis, the centralized approach requires more computational burden but it allows to use secondary cost functions taking into account properties of the whole system such as a measure describing total manipulability of two cooperative robots. In the case of the leader-follower approach, each of the robots is considered as an individual subsystem, and therefore secondary performance indexes can be functions of state variables of the single robot only. Such approach allows to maximize the manipulability measure of each individual mechanism, however does not provide optimization of the manipulability of the whole system, which is confirmed by the simulation results presented in Section 7. In the centralized approach, the performance index can be a function of state variables of the whole system, hence optimization of the manipulability of the whole mechanism is possible. Summarizing, where a sufficient computing power is available and it is important to optimize secondary cost functions describing properties of the whole system, the application of the centralized approach should be considered.

Trajectory Generation for Multiple Robots
The scope of the work is limited to the case of generating the trajectory for two robots, however it seems that both proposed approaches can be employed directly in generating the trajectory for multiple cooperating robots. In the case of the leader-follower approach, the robot with the number of the degree of freedom not less than the number of degrees of freedom of other robots is chosen as a leader, the other robots become followers. The trajectory of the leader is derived from dependency (35) and trajectories of the followers according to dependency (42). In the case of the centralized approach, the error measure (64) has to be modified. Taking another robot into account requires the addition of two components, one related to the existence of a closed kinematic chain and the other resulting from the constraints imposed on the robot platform motion. Similarly, equality constraints in Eq. 57 should be completed in order to derive the transversality condition for the whole system. Consideration about the computational burden and secondary cost functions for the case of two cooperating robots also applies the case of multiple robot system.
The above analysis presents the general idea of the application of the method in the case of trajectory generation for multiple robots. However, these issues are out of the scope of this paper, they require a further elaboration and will be the subject of further research.

Numerical Example
In the numerical example two mobile manipulators, shown in Fig. 3, each composed of the nonholonomic differentially driven platform (2,0) class and planar 3-DOF holonomic manipulator are considered. The task of the robots is to move the uniform, rigid beam to the final location in the workspace. Two cases of performing above task are considered. In the first one control constraints are not taken into account. In the second case, the methods described in Sections 4.4 and 5.3 are used to satisfy control constraints  (19). In each example, two simulations for: leader-follower and centralized approach are carried out.

System Parameters and Constraints
The system of two cooperating mobile manipulators taken into account in simulations is shown in Fig. 3. The i-th robot is described by the vector of generalized coordinates: where [x c,i , y c,i ] is the platform center location, θ i is the platform orientation, φ 1,i , φ 2,i are the angles of driving wheels, q 1,i , q 2,i , q 3,i are the configuration angles of the manipulator joints. The mobile manipulators work in X B Y B plane of the base coordinate system {B}.
where l 1,i , l 2,i , l 3,i are the lengths of the manipulator links, c θ,i = cos(θ i ), c k,i = cos( k j =1 q j,i ), s θ,i = sin(θ i ), s k,i = sin( k j =1 q j,i ). During the task accomplishment, the mobile robots are subject to limitations causing by the constraints of the platform motion. According to the analysis of kinematic properties of wheeled mobile robots presented by Campion et al. in [4], the motion of the wheeled platform is subject to limitations resulting from rolling and sliding constrains of each platform wheel. For differentially driven platforms discussed in this work, each of fixed wheels of the i-th platform introduces one rolling constraint: where r i is the radius of driving wheels and a i is halfdistance between the wheels.
The wheels of each platform are parallel, thus the nonsliding constraints are equivalent and they can be written as: −ẋ c,i s θ,i +ẏ s,i c θ,i = 0.
Taking above wheel constraints into account, the velocity of the platform ẋ c,i ,ẏ c,i , θ i T can be determined from: Finally, using the above dependency, platform constraints can be expressed in the Pfaffian form as: It is assumed that both robots have same kinematic and dynamic parameters, they are given as (all physical values are expressed in the SI system): At the initial moment of motion the robots are in the non-singular, collision-free configurations: q 1 (0) = [0, −1, 0, 0, 0, 0, π/4, π/4] T , q 2 (0) = [0, 2, 0, 0, 0, 0, −π/4, −π/4] T , for which the beam is in the location p 0 .
To preserve mechanical constraints generalized coordinates of holonomic manipulators should not exceed limits: Scalar functions involving fulfillment of k-th constraint connected to k-th generalized coordinate are taken as: Then, the penalty functions used in the performance indexes (23), (37) and (56), ensuring fulfillment of the mechanical constraints are proposed as: where ρ m is the positive coefficient determining strength of penalty for the mechanical constraints.
In order to preserve potential collisions between robots and the object, the approach based on obstacles enlargement with a simultaneous discretization of the mobile manipulator, shown in [21], has been adopted. In order to apply this approach, the carried object is an obstacle for both robots and each robot is treated as an obstacle for the second one. The object and segments of the mobile manipulators (platforms, manipulator links) are approximated by smooth surfaces and collision avoidance conditions are written as: where η j is the equation of j -th obstacle surface, x d,i is the point from the set of points which approximates the ith mobile manipulator and δ denotes a small positive scalar, safety margin.
In order to take into account above constraints, in the indexes (23), (37) and (56), the following penalty functions acting in the neighborhood of the obstacles are used: where ρ c denotes the constant positive coefficient determining the strength of penalty and ε is the constant positive coefficient determining the threshold value which activates the k-th constraint. Due to the asymptotic stability of the equilibria of Eqs. 35, 42 and 66, it is possible to reach the final location p f and fulfillment of the boundary conditions (16) with a desired accuracy. It has been assumed that the algorithms stop when the distance to the desired location p f is less than the given value e p and generalized velocities of both robots are less than the given value e v , i.e.: f 1 (q 1 )−p f <e p and ( q 1 <e v ) and ( q 2 <e v ) .
In the all presented simulations, the values for e p and e v equal to 0.01 have been used.

Trajectory Without Control Constraints
In the first case, methods described in Sections 4 and 5 are used to generate trajectory without taking into account control constraints. The values of the gain coefficients for the leader-follower approach are taken as:  In the second simulation the centralized approach is used to perform the same task. In this case, the final time of task execution T is equal to 11.62 [s]. The robots motion is shown in Fig. 8. The first robot is plotted with solid line and the second robot with dashed line. Similarly as for the leaderfollower approach, four phases of the motion are shown. In Fig. 9 the obtained trajectories of holonomic parts of the first and the second robot are shown (the horizontal dashed and dotted lines represent the mechanical constraints). As in the leader-follower approach, the obtained trajectories satisfy the assumed constraints. The controls of the first and the second robot are shown in Figs. 10 and 11. As it is seen, controls are continuous functions of the time and in this case they are within ranges:

Trajectory with Control Constraints
In the second case, it is assumed that limitations on controls resulting from physical abilities of the actuators are equal to: As can be seen, controls determined in the previous subsection exceed above limits both for the leader-follower and centralized approach.
As it has been shown in Sections 4.4 and 5.3, controls are linearly dependent on 1 or . Therefore, it is possible to determine the values of gain coefficients in such a way that control constraints are satisfied. For simplicity of simulations, 1 and are assumed to be constant and for the leader-follower approach they are equal to: be seen, the proposed methods are effective and all imposed constraints are satisfied, moreover, obtained controls are continuous functions of the time. Application of the scaling techniques results in a reduction of controls and, as a consequence, the longer execution time, but it has no significant impact on the way the task is accomplished, therefore the way of task execution in both simulations is similar to these presented in Figs. 4 and 8.
The advantage of the centralized approach is the ability to take into account the total manipulability of the whole system, which was not possible in the leader-follower method. In order to compare results of both trajectory generators, the  Fig. 19 Total manipulability of the system for the second case (centralized approach) total manipulability measure (55) has been calculated for both the leader-follower and the centralized approach. Changes of these indexes over time are shown in Figs. 18 and 19, respectively. As it is seen in Fig. 18, optimization of the manipulability measure of the individual subsystems (leader and follower) does not lead to optimal manipulability of the whole system.

Conclusion
In the presented paper, the original boundary value problem has been transformed into the initial value problem -so it seems that the proposed solution is computationally efficient.
In this paper two suboptimal methods of trajectory generation for mobile manipulators moving the common rigid object to a specified location in the workspace have been proposed. Presented approaches guarantee fulfillment of constraints imposed by the closed kinematic chain as well as boundary conditions resulting from the task to be performed. Constraints connected with the existence of mechanical limitations for manipulator configuration, collision avoidance conditions and control constraints have been considered in both methods. Minimization of instantaneous performance indexes ensures that during the motion, robots are far away from singular configurations. The proposed methods do not guarantee the global optimality because they minimize the performance indexes in each time instant, then the obtained solutions are suboptimal.
The problem has been solved by using penalty functions and a redundancy resolution at the acceleration level. The resulting trajectories have been scaled in a manner to fulfill the constraints imposed on controls. The property of asymptotic stability of the proposed control systems implies fulfillment of all the constraints imposed.
Advantages and disadvantages of both methods have been discussed. The leader-follower approach is attractive due to the higher computation efficiency resulting in lower computational burden. The centralized approach is more computationally demanding but it allows to take into account more complex performance indexes including state variables of both cooperating robots, thus it lets to optimize total manipulability measure of the whole system. However, the presented work is limited to the case of generating the trajectory for two robots, the application of the proposed approaches in generating the trajectory for multiple cooperating robots has been discussed and it will be the subject of the further research.
In the proposed approaches, the original boundary value problems have been transformed into the initial value problems, then the proposed solutions are computationally efficient. The effectiveness of the presented methods are confirmed by the results of computer simulations.
Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http:// creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.
Publisher's Note Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.