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

Real-Time Trajectory Generation Methods for Cooperating Mobile Manipulators Subject to State and... 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. Keywords Multiple mobile robots · Cooperative manipulation · Trajectory generation 1 Introduction robot. At first, limitations arising from the physical abilities of the cooperating robots have to be taken into account. There is a class of tasks that cannot be executed by a single Additionally, the use of the mobile robots with nonholo- robot. They are associated with the movement of heavy nomic platforms requires consideration of pure rolling and non-slip conditions. Moreover, the robots moving the com- objects that a single robot cannot lift due to physical limita- tions of its actuators. Another example of these tasks is the mon object create the closed kinematic chain which imposes movement of the large objects which must be supported at an additional constraint that has to be allowed for. several points. The necessity of performing such tasks leads to rise the interest in the trajectory generation for multiple 1.1 Related Works robots performing the common task. Combining the mobil- In the literature there are several approaches to the motion ity of the platform with dexterity of the manipulator, the control for mobile manipulators. Some solutions employ the mobile manipulators seem to be especially attractive in this calculus of variations. This approach has been presented by application. The platform significantly increases the oper- Desai and Kumar in [7, 8] to formulate a general approach ating range of the robot, and therefore such a system may in order to obtain optimal trajectories for nonholonomic be used to transport objects over long distances. Moreover, cooperating mobile robots. Solutions based on a pseudo- after reaching the desired location, dexterity of the manip- inverse of the Jacobian matrix have been proposed in [11, ulator allows to the precise placement of the object in the 12] in order to generate real-time collision-free trajectories. desired position. Seraji in [24] has used the augmented Jacobian matrix and Solving the trajectory generation problem for multiple has introduced an on-line approach for motion control on mobile manipulators is more complicated than for a single the kinematic level. The method discussed in this paper employs the augmented Jacobian matrix, however it is appli- cable to cooperative robot tasks, generated trajectories take Iwona Pajak account of the dynamics of the system, control limitations i.pajak@iizp.uz.zgora.pl and state constraints. In the recent years an increasing research interest in the Faculty of Mechanical Engineering, University of Zielona Gora, 65-246 Zielona Gora, Poland multi-robot systems has been noticed. A large number of J Intell Robot Syst studies concern problems of motion control for multiple using the geometric relations among the robots. Luh and robots performing a common task in the same workspace. Zheng in [19] have proposed a leader-follower approach in Some works consider control design problems for groups which they derived the relation between the joint positions of vehicles which have to achieve a specified location [29] and velocities of the leader and the follower as a set of holo- or move in a given formation [17]. Such solutions are often nomic equality constraints. Other leader-follower approaches limited to use only the mobile platforms, thus the robots do have been presented in [25, 27]. Sugar and Kumar in [25] not have manipulation abilities. Moreover, the method of this have discussed the motion control method for a small team type cannot be directly adopted to solving the cooperative of mobile manipulators that cooperatively transport a large, transportation tasks by multiple mobile robots. In the case flexible object in a two dimensional environment with of such tasks, two general methods are used: pushing and obstacles. In [27] Trebi-Ollennu et al. have proposed an grasping. In the first approach the object is pushed by a decentralized algorithm for two Mars rovers carrying a long group of the robots [6, 9, 30], however, in this case the object over an uneven, natural terrain. Abou-Samah et al. in motion of the object cannot be controlled precisely. [2] have considered the leader-follower approach for coop- In the second approach robots are equipped with erative payload transport based on the individual mobile graspers and they hold the object during transportation, and manipulator control schemes. In [13] a decentralized hybrid therefore the more precise manipulation is possible. The position/force controller has been presented for the control solution of such tasks is more difficult due to the neces- of multiple tightly coupled manipulators handling a com- sity to satisfy constraints resulting from the closed kine- mon object. Adaptive control law developed from the virtual matic chain established by cooperative robots and the object decomposition approach has been designed by Brahmi et al. held by them. The methods of motion control for multiple in [3]. cooperative robots have been first developed for multiple manipulators. These studies are further intensively devel- 1.2 Summary of the Proposed Solution oped especially in the case of coordinated manipulation tasks, such as bolt and nut mating [32] and transportation In this paper, two trajectory generation methods for mobile tasks [10, 14]. Stationary manipulators have a limited work- manipulators moving the common rigid object are pre- space, thus they cannot be used to carry long objects over sented. The proposed solutions are the development of the long distances. Due to the mobility of the platform, mobile method discussed in [21] and shows that an approach for robots are more suitable for this type of tasks. The ideas used a single mobile manipulator can be easily adopted to the in the case of cooperative manipulators may be employed cooperative task of two mobile robots. In the first method, the trajectory generation problem is solved by using the for motion control of mobile robots but the control task is more complicated due to limitations resulting from a plat- leader-follower conception. In this approach, the leader form movement. determines the motion and the follower assists the leader Research carried out in the field of multiple robots supporting the object. In the presented work, the leader per- motion control has been aimed at solving the problem using forms the point-to-point task moving its end-effector to the centralized and decentralized approaches. In the centralized final location resulting from the desired pose of the object. methods, motion control applies to all cooperating robots The follower tracks the trajectory imposed by the leader simultaneously. The methods of this type are based on motion ensuring the fulfillment of the constraints which full knowledge of the environment and they concentrate on results from the closed kinematic chain formed by coop- searching an optimal solution. Desai and Kumar in [7, 8] erating robots. This approach is the development of the have presented the method based on calculus of variations idea presented in [23]. In the second method, the trajec- for optimal control for mobile manipulators cooperating in tory generation problem is solved in a centralized way. In the workspace including obstacles. In [26] Tanner et al. have this case, the both robots are treated as the one system con- discussed globally asymptotically stable centralized feedback strained by conditions resulting from the closed kinematic controller for mobile manipulators moving a deformable chain and they perform the point-to-point task moving the object in the environment including obstacles. object to its final location. The solution scheme for both While the centralized methods usually lead to algorithms proposed approaches is similar, however, each of them has with high computational complexity, the decentralized some advantages and disadvantages that can be crucial in approaches are computationally more efficient due to decom- choosing a particular approach for a particular application. position of the motion control task. In [15, 18] the authors These issues will be detailed discussed in Section 6. have considered a leader-follower motion control algorithm In both presented methods, the trajectories of the robots for multiple mobile manipulators moving a common object. are generated to minimize the performance indexes to avoid In this approach, each robot has had a caster-like dynam- manipulators singularities. In the first case, maximization of ics in 3D space and an object has been manipulated without the manipulability measures of the leader and the follower J Intell Robot Syst is carried out. In the second one, the total manipulability manipulators consisting of a nonholonomic (2,0) class plat- of two cooperative robots is maximized. Additionally, the form and a 3-DOF planar holonomic manipulator moving constraints causing by mechanical and control limitations as the rigid beam in 2D space. 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 optimiza- 2 Model of the System tion problem with holonomic and nonholonomic equality constrains, and inequality constraints resulting from col- In the cooperative task considered in this paper two mobile lision avoidance and mechanical limitations. Finally, the manipulators are moving the common rigid object, firmly resulting trajectories are scaled to fulfill the constraints connected to their end-effectors, to the specified location in imposed on the controls. As it is shown further on, in the the workspace. Manipulators and an object form a closed leader-follower approach, controls of the system depend on kinematic chain, therefore knowledge of the kinematics and the motion imposed by the leader, thus satisfying control dynamics of such a system is necessary to formulate the task constraints is obtained by trajectory scaling of the leader as a consequence of a cooperation. only. In the centralized approach, controls of the system depend on the trajectory of the whole system, and there- 2.1 Kinematics fore in this case, the trajectory of both cooperating robots is scaled to fulfill control constraints. In order to describe the kinematics of the closed kinematic To the best of the author knowledge, no research has con- chain consisted of two mobile robots and an object carried sidered the cooperative task formulated in the above manner. by them, the location of the object in the workspace must be Some of the existing research addresses the problem using specified. For this purpose, as it is shown in Fig. 1, a local only kinematic equations of the mobile manipulators [19, coordinate frame {O} attached to the object at any given 24, 26] and the dynamics of the robots is not considered at point is introduced. The pose of this local frame in a global all. The studies considering the dynamics of the cooperative reference frame {B} specifies the location of the object is systems do not allow to take into account control constraints described by the m-elemental vector p: [2, 3, 7, 8]. In contrast, the presented solution takes into con- def T T p = x ,ϕ , (1) siderations dynamics of the cooperative system, moreover, it is distinguished by the method of determining the con- where x and ϕ are the vectors describing position and tinuous controls which fulfill the assumed constraints. The orientation of the local frame attached to the object in a results of the proposed approach are trajectories parameter- global reference frame. ized by gain coefficients, which can be chosen in such a way In this paper, it is assumed that each of cooperating robots to fulfill the imposed control constraints. The proposed tra- is composed of a nonholonomic platform and a holonomic jectory generation algorithm does not need the knowledge manipulator with kinematic pairs of the 5th class. They are of the system dynamics, the dynamic model is necessary described by the vectors of generalized coordinates: to determine the values of gain coefficients only. Other def T T research considering the dynamics of the robots produces q = q q ,i = 1, 2, (2) p,i r,i controls parameterized by gain coefficients of the controller where q is the n -dimensional vector of the generalized i i directly. In such cases, it is very difficult (or impossible) to coordinates of the i-th mobile manipulator, q is the p - p,i i find coefficients which fulfill control limits. dimensional vector of the coordinates of the i-th nonholo- The paper is organized as follows. Section 2 presents kine- nomic platform, q is the r -dimensional vector of joints r,i i matic and dynamic model of cooperating robots. Section 3 coordinates of the i-th holonomic manipulator, n = p + r . i i i 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 con- trol constraints. Advantages and disadvantages of presented methods are discussed in Section 6. The proposed methods are demonstrated numerically in Section 7, for two mobile Fig. 1 Virtual stick conception J Intell Robot Syst In order to describe the geometry of the grasp the where M (q) is the (n × n ) positive definite inertia i i i approach of a virtual stick is adopted [28]. In this case the matrix, F (q , q ˙ ) is the n -dimensional vector representing i i i i virtual stick is a constant vector which determines the origin Coriolis, centrifugal, viscous, Coulomb friction and gravity of the object frame in the manipulator end-effector frame. forces, M and F contain both robot and object dynamic i i In Fig. 1 two manipulators holding a common object are properties, A (q) = A 0 , 0 is the (h × r ) zero matrix, i i i i presented. The vectors vs and vs represent virtual sticks λ is the h -dimensional vector of the Lagrange mul- 1 2 i i of the first and second robot. They connect the origins of tipliers corresponding to constraints (6)and B is the frames {G }, {G } attached to the end-effectors of both n × n − h full rank matrix (by definition) describing ( ( )) 1 2 i i i robots with the origin of the object frame {O}. Additionally, which state variables of the mobile manipulator are directly at the end points of vs and vs the virtual end-effectors driven by the actuators. 1 2 T T with the same orientation as the object frame are introduced. Introducing the combined vectors q = q ,q , q ˙ = 1 2 The idea mentioned above is used in this work to describe T T q ˙ , q ˙ and the matrix J (q) = [J , −J ], the kinematic 1 2 1 2 geometry of the closed kinematic chain formed by two constraints (5) can be expressed in a compact form as: cooperating robots. J q ˙ = 0, (8) Using the virtual stick approach, the location of the object can be expressed with respect to generalized coordinates of therefore the overall dynamic equations for the cooperating the mobile manipulator as: robots taking into account both platform (6) and closed kinematic chain (8) constraints, can be written as: p = f (q ),i = 1, 2, (3) i i T T n m Mq ¨ + F + A λ + J λ = Bu.(9) i p c where f : → denotes the m-dimensional mapping, T T T which describes the position and orientation of the virtual T T T T T T where q ¨ = q ¨ , q ¨ , F = F ,F , λ = λ ,λ , 1 2 1 2 1 2 end-effector of the i-th manipulator in the workspace. T T u = u ,u , A, M, B are the block diagonal matrices 1 2 Since there are no relative motions between the virtual end- defined as follows: M = blockdiag (M ,M ), A = blockdiag 1 2 effectors of two cooperating robots (assuming the object is (A ,A ), B = blockdiag (B ,B ), λ is the m-dimensional 1 2 1 2 c rigid), the following geometric and kinematic constraints vector of the Lagrange multipliers corresponding to the should be fulfilled: constraints (8). q = p = f q , (4) f ( ) ( ) 1 1 2 2 A (q) is the full rank matrix, thus there exists the full rank J (q ) q ˙ = v = J (q ) q ˙.(5) 1 1 1 2 2 2 (n + n )×(n + n − h − h ) matrix N (q), orthogonal 1 2 1 2 1 2 p to A (q), which satisfies the relation: where J = ∂f (q ) /∂q is the (m × n ) Jacobian matrix of i i i i the i-th manipulator and v denotes the common velocity of AN = 0. (10) the virtual end-effectors of the robots in the workspace. Using the above dependency and multiplying the dynamic Moreover, due to use of mobile robots, it is necessary to equation (9)by N , the vector of Lagrange multipliers λ take into account holonomic and nonholonomic constraints can be eliminated and the model takes a new form as: which limit the platform motion capabilities. The con- T T T T T N Mq ¨ + N F + N J λ = N Bu. (11) straints of this type, for the i-th platform, can be described c p p p p in the Pfaffian form as: Assuming that the matrix JN is the full rank (rank JN = m), the full rank (n + n − h − h ) × A q q ˙ = 0,i = 1, 2, (6) p 1 2 1 2 i p,i p,i (n + n − h − h − m) matrix N (q), orthogonal to 1 2 1 2 c where A (q ) is the (h × p ) Pfaffian full rank matrix and i p,i i i JN , can be determined in such a way that the following h is the number of independent holonomic and nonholo- relation is fulfilled: nomic constraints. JN N = 0. (12) p c 2.2 Dynamics Finally, multiplying dependency (11)by N , the dynamic model of the system is reduced to the form given below: The cooperation causes that manipulators and object form a T T T T T T N N Mq ¨ + N N F = N N Bu. (13) c p c p c p closed kinematic chain, and consequently reduces the degree of freedom of the system. Thus, owing to the redundant Cooperative robots moving the common rigid object form number of the actuators, this system can be considered as a an over-actuated dynamic system, thus there is no unique rigid over-actuated multi-body system. After disconnection solution of the Eq. 13 for u. Assuming the full rank of matrix T T of this multi-body system at the origin of the object frame N N B , the inverse dynamic model of the system may be c p [16], the dynamic equations for i-th robot are given as: determined as: M (q ) q ¨ + F (q , q ˙ ) + A (q ) λ = B u , (7) u = Mq ¨ + F, (14) i i i i i i i i i i i J Intell Robot Syst + T T + T T T T where M = B N N M, F = B N N F , B = N N B Additionally, each control should be within the limits c p c p c p and B is the Moore-Penrose pseudoinverse of matrix B . defined by the physical capabilities of the actuator, then the following limitations for controls u and u for the first and 1 2 the second robot, should be satisfied: 3 Problem Formulation u ≤ u ≤ u ,j = 1,...,n − h , min,1,j 1,j max,1,j 1 1 1 1 1 1 This paper considers the cooperative task of two mobile mani- u ≤ u ≤ u ,j = 1,...,n − h , (19) min,2,j 2,j max,2,j 2 2 2 2 2 2 pulators moving the common rigid object to the specified location in the workspace. Using the vector p describing where u is the j -th control (torque or force) of the first 1,j 1 the location of the object (1), this task can be formulated as robot, u is the j -th control (torque or force) of the sec- 2,j 2 displacement of the object from the given initial location p ond robot, and u , u , u , u are min,1,j max,1,j min,2,j max,2,j 1 1 2 2 to the final location p . the lower and upper limits on u and u , respectively. 1,j 2,j 1 2 Additionally, the motion of the mobile robots have to To solve the cooperative task satisfying kinematic con- take into account mechanical limits, boundary constraints straints (4)–(6) and limitations resulting from the construc- resulting from the task and collision-free conditions. At tion of the robots and the nature of the task (15)–(19), the the initial moment of motion the manipulators are (by control system shown schematically in Fig. 2 is proposed. assumption) in the collision-free configuration for which the As can be seen, the problem will be solved by a real-time virtual end-effectors are in the initial location p with zero trajectory generator working in the closed loop. The input velocity: signal of the generator is the error vector E calculated based on the desired location of the object and its current p = f (q (0)) , q ˙ (0) = 0,i = 1, 2. (15) 0 i i i location obtained from a forward kinematic model (FKM) At the final moment of motion the both virtual end-effec- given by dependency (3). The other components of vector E tors have to reach the final location p with zero velocity: result from holonomic and nonholonomic platform con- straints (PC), described by Eq. 6, and additional constrains p = f (q (T )) , q ˙ (T ) = 0,i = 1, 2, (16) f i i i (AC) introduced by dependencies (4), (5), (17), (18). The controls are determined from the dynamic model of the sys- where T stands for an unknown time of performing the task. tem (model) given by dependency (14). The main advantage Conditions resulting from the mechanical limits and of the proposed method is that, in contrast to other solutions, constraints connected with the potential collisions between it generates trajectories ensuring fulfillment of both the state cooperating robots or between robots and object carried by and control constraints. them can be written as a set of inequalities: In the next sections two trajectory generators are considered. The first one is based on the leader-follower conception ψ q t ≥ 0,k = 1, ...,l ,i = 1, 2, (17) ( ( )) k,i i i [19]. In this approach, the leader determines the motion and the follower assists the leader supporting the object. In where ψ is the scalar function which involves the fulfill- k,i the second one, the centralized approach is presented and ment of the constraints imposed by the i-th robot mecha- the cooperating robots are taken into account together and nical limits and collision avoidance conditions, l stands for treated as the one system. The solutions are based on the the total number of constraints. previous author’s work on the trajectory generation for a sin- In practice, the configuration of mobile manipulator gle mobile manipulator [21] and use of the penalty function joints should be far away from singular configurations. The approach and a redundancy resolution at the acceleration distance from singularities can be defined based on various level. The advantages and disadvantages of both solutions manipulability measures. The next sections will discuss are also discussed. 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: def I = 1/ σ + w(· ) , (18) ( ) where σ is the small positive coefficient and w (· ) denotes the manipulability measure (explicit forms are given in the following sections). Fig. 2 The idea of control system for cooperating robots J Intell Robot Syst 4 Leader-Follower Approach 4.2 Trajectory Generation for the Leader In this solution cooperating mobile manipulators are The main task of the leader is to move its virtual end- considered as two individual systems called the leader and effector to the final location p in such a way as not to the follower. The main task of the first mobile manipulator violate constraints (6). Additionally, the trajectory of the (leader) is to move its virtual end-effector from the initial robot has to satisfy inequality conditions (17) and reach the location p to the final location p . The second robot minimum value of the performance index (18) in each time 0 f (follower) has to follow the leader, tracking the trajectory instant. To solve this problem, an approximate implementa- determined by its virtual end-effector. During this motion tion of inequality constraints (17) is accepted in this work. both robots should allow for constraints described by The approach is to use penalty functions which cause the dependencies (4)–(19) and minimize performance indexes inequality constraints to be satisfied, but the performance (18). index (18) to be somewhat increased. In this case the Section 4.1 presents the manipulability measure for the instantaneous performance index (18) can be expressed as: single robot. In Sections 4.2 and 4.3 the methods of trajec- def tory generation for the leader and the follower satisfying I (q ) = I + κ ψ , (23) 1 1 1 i,1 i,1 constraints (4)–(17) are considered. Trajectory scaling tech- i=0 nique leading to fulfillment of control constraints (19)is where κ is the penalty function which associates a penalty i,1 presented in Section 4.4. with a violation of a leader constraint, see Numerical example section for an exemplary form of this function. 4.1 Manipulability Measure In order to find a trajectory of the leader minimizing the performance index (23), first let us consider the problem of In order to avoid singular configurations the manipulabil- finding an optimal robot configuration q (T ) at the final ity of mobile manipulators moving the common object has location p : to be taken into account. In the solution presented in this section, the manipulability measure is considered for each f (q ) − p = 0, 1 1 f subsystem independently, and it is based on the manip- A q ˙ = 0, (24) 1 1 ulability ellipsoid introduced by Yoshikawa [31]. In this min I (q ) . 1 1 approach the relationship between unit norm generalized velocities of the robot and its virtual end-effector velocity Necessary condition to obtain the minimum of the per- in the workspace is determined. The unit norm velocities in formance index I (q ) is that the differential of I at q (T ) 1 1 1 1 the configuration space fulfill the following dependency: equals zero: q ˙ q ˙ = 1,i = 1, 2, (20) I (q ) ,δq = 0 (25) q 1 1 Using the Moore-Penrose pseudoinverse of Jacobian matrix where ·  means the dot product of the vectors, I (q) = J , velocities in the workspace can be determined from Eq. 5 ∂I /∂q and δq is the variation of q . 1 1 1 1 as q ˙ = J v, therefore (20) can be rewritten as: i Moreover, at the configuration q (T ) equality con- straints given by the first and second dependency from 1 =˙ q q ˙ Eq. 24 have to be fulfilled. After differentiating the first of + + = (J v) J v i i them, they can be expressed in a compact form as: T + T + = v (J ) J v i i q δq = 0, (26) J ( ) 1 1 1 T T T −1 T T T −1 = v (J (J J ) ) J (J J ) v i i i i i i T T T T −1 T T T −1 where J (q ) = J A is the ((m + h ) × n ) non- 1 1 1 1 = v ((J J ) ) J J (J J ) v 1 1 i i i i i i singular matrix (nonsingularity is forced by minimization T T −1 T = v ((J J ) ) v procedure). T T T −1 = v ((J J ) ) v i Since J is the full rank matrix, it is possible to rewritten T T −1 = v (J J ) v. (21) the above dependency as: Thus, a unit sphere from the configuration space is mapped T T J J δq δq = 0, (27) R,1 F,1 R,1 F,1 into an ellipsoid in the workspace. According to Yoshikawa, the manipulability of the robot can be measured as propor- where J is the square matrix constructed from (m + h ) R,1 1 tional to the volume of this ellipsoid as: linear independent columns of J , J is the matrix 1 F,1 obtained by excluding J from J , δq is the (m + h ) R,1 1 R,1 1 w (q ) = det (J J ), i = 1, 2. (22) i i dimensional vector corresponding to the reduced matrix i J Intell Robot Syst J , obtained by choosing elements from δq related to Hence, the second order differential equation with respect R,1 1 columns of the matrix J and δq is the vector obtained to q is needed. Equation 32 is a system of homogeneous R,1 F,1 1 by excluding δq from δq . differential equations with constant coefficients. In order to R,1 1 Multiplying the left side of Eq. 27, the related vector solve it and find the trajectory of the mobile manipulator δq can be expressed in terms of free vector δq as: 2n − h consistent dependencies should be given. These R,1 F,1 1 1 dependencies are obtained from E for t = 0takinginto −1 1 δq =−J J δq . (28) R,1 F,1 F,1 R,1 account conditions (15), especially zero initial velocity. Then, taking into account dependency (28), necessary As it has been shown in [21], the proposed form of condition (25) may be rewritten as: differential equation (32) ensures that its solution is asymp- totically stable which implies fulfillment of the conditions −1 T I − (J J ) I ,δq = 0, (29) F,1 F,1 R,1 F,1 R,1 (16) for the leader. What is more, if corresponding elements I I where I is the (m + h ) dimensional vector correspond- R,1 1 of the main diagonals of matrices and satisfy V,1 L,1 ingtothevector δq , obtained by choosing elements from R,1 I I condition λ > 2 λ , the solution is also a strictly V,1 L,1 I related to components of the vector δq and I is the q R,1 F,1 monotonic function, thus for the initial nonsingular config- vector obtained by excluding I from I . R,1 q uration q (0) fulfilling mechanical and collision avoidance The consequence of the free vector δq in Eq. 29 is the F,1 constraints (17) leader motion is free of singularities and following equation (called transversality condition): fulfills constraints (17) during the movement to the final −1 T I − (J J ) I = 0. (30) F,1 F,1 R,1 location p . R,1 f In order to obtain trajectory of the leader, Eq. 32 can be Equation 30 introduces (n − m − h ) dependencies 1 1 rewritten in the expanded form: which in combination with the equality conditions from Eq. 24 allow to obtain an optimal configuration for a given I I I I I I ˙ ˙ E q ¨ + E q ˙ + E + E 1 1 final location. Then, in order to find a trajectory of the q,1 q,1 V,1 1 L,1 1 = 0, (33) II II II II leader, mapping E q , q ˙ , which may be interpreted as a ( ) E q ¨ + E q ˙ + E 1 1 1 1 1 q, ˙ 1 q,1 L,1 1 measure of the error between a current configuration q (t ) and unknown final configuration q (T ), is introduced as I I I I where E = ∂E /∂q , E = dE /dt, q,1 1 q,1 q,1 follows: II II II II E = ∂E /∂q ˙ , E = ∂E /∂q . 1 1 ⎡ ⎤ q, ˙ 1 1 q,1 1 f (q ) − p 1 1 f Then, after simple transformations, Eq. 33 can be written def −1 ⎣ ⎦ E = I − (J J ) I . (31) 1 F,1 F,1 R,1 as: R,1 A q ˙ 1 1 I I I I I ˙ ˙ E q ˙ + E + E q,1 V,1 1 L,1 1 The m-first components of E are responsible for reach- E q ¨ =− , (34) 1 1 II II II E q ˙ + E ing the given final location p ,the h -last items ensure 1 f 1 q,1 L,1 1 fulfillment of constraints (6). The remaining n − m − h 1 1 dependencies are derived above from the necessary condi- I II T T where E = (E ) (E ) . q,1 q, ˙ 1 tion for the minimum of the performance index (23), thus Finally, the trajectory of the leader is given by: they are responsible for the fulfillment of constraints (17)as well as minimizing performance index (18). I I I I I ˙ ˙ E q ˙ + E + E Applying the error measure (31), the dependency speci- 1 −1 q,1 V,1 1 L,1 1 q ¨ =−E . (35) 1 II II II fying the trajectory of the leader to the final location p is f E q ˙ + E q,1 L,1 1 postulated as the following system of differential equations: 4.3 Trajectory Generation for the Follower I I I I I ¨ ˙ E + E + E 1 V,1 1 L,1 1 = 0, (32) II II II E + E 1 L,1 1 The task of the second robot is to follow the trajectory of the leader virtual end-effector d (t ). This trajectory can be where E is constructed from the first two components of II I I determined from the forward kinematic model of the leader mapping E , E is the third component of E , , 1 1 1 V,1 L,1 (4), as follows: are the ((n − h ) × (n − h )) diagonal matrices with 1 1 1 1 positive coefficients ensuring the stability of the equilibrium II d (t ) = f (q (t )) . (36) of the first equation and is the (h × h ) diagonal 1 1 1 1 L,1 matrix with positive coefficients ensuring the stability of the equilibrium of the second equation. Just as the leader the follower, during its motion, should The proposed form of dependency (32) results from the also keep inequality constraints (17) and avoid singular con- necessity to determine trajectory at the acceleration level. figurations. In this case, the same approach as for the leader J Intell Robot Syst is employed and the extended performance index I (q ) is Finally, the trajectory of the follower takes the following 2 2 defined in the same way as I (q ), as follows: form: 1 1 I I I I I ˙ ¨ ˙ E q ˙ − d + E + E 2 −1 q,2 V,2 2 L,2 2 q ¨ =−E (42) def 2 II II II I (q ) = I + κ ψ . (37) E q ˙ + E 2 2 2 i,2 i,2 q,2 L,2 2 i=0 where d = d 0 , 0 is the (n − m − h ) elemental 2 2 where κ is the penalty function which associates a penalty i,2 I II I II vector, E , E , E , E , E are defined in similar way q,2 q, ˙ 2 q,2 q,2 with a violation of the follower constraint, see Numerical as for the trajectory of the leader (33). example section for an exemplary form of this function. The trajectory q (t ) is the result of solving a system of In the case of finding trajectory for the follower, the prob- differential equations whose form ensures that its solution is lem of determining an optimal robot configuration q (t ) asymptotically stable for positive definite diagonal matrices at the given time instant t should be considered: I I II , and . This property combined with the V,2 L,2 L,2 fulfillment of the condition (16) for the leader implies that f (q ) − d (t ) = 0, 2 2 the follower reaches the final location p with zero velocity, A q ˙ = 0, (38) 2 2 thus the condition (16) is also fulfilled for the follower. I I min I (q ) . 2 2 Additionally, for appropriately chosen matrices , q V,2 L,2 the solution is also a strictly monotonic function, thus for the Similarly as in previous subsection, using necessary con- initial nonsingular configuration q (0) fulfilling constraints dition of performance index (37), transversality condition (4), (5)and(17), the follower motion is free of singularities for the follower can be derived as: and fulfills constraints (4), (5), (17) during its movement to the final location p . −1 I − (J J ) I = 0, (39) F,2 F,2 R,2 R,2 4.4 Control Constraints where I , I , J , J are defined in similar way as R,2 F,2 R,2 F,2 I , I , J , J . R,1 F,1 R,1 F,1 To satisfy control constraints (19) the development of the The counterpart of the error measure E (31)for the methods presented in [22] for stationary cooperating robots follower is the mapping E (q , q ˙ ) defined as follows: 2 2 2 andin[21] for single mobile manipulator is proposed. ⎡ ⎤ f (q ) − d (t ) Denoting: 2 2 def −1 ⎣ ⎦ E = I − (J J ) I (40) 2 F,2 F,2 R,2 T R,2 I T I T = diag( ) ,diag( ) A q ˙ V,1 L,1 2 2 and introducing the following substitutions: In this case the m-first components of the above I I dependency result from the grasp constraint of the two diag(E ), diag(E ) −1 1 1 A =−E , cooperating robots (4) and describe an error between a current follower virtual end-effector location and trajectory d (t ). The next n − m − h dependencies are obtained from E q ˙ 2 2 1 −1 q,1 b =−E , II II II the necessary condition for the minimum of the performance E q ˙ + E q,1 L,1 1 index (37) and they ensure the fulfillment of constraints trajectory of the leader (35) can be written as linear with (17) as well as minimizing performance index (18)for the respect to : follower. The h -last items are responsible for fulfillment of constraints (6) of the second robot. q ¨ = A + b . (43) 1 1 1 1 To find the trajectory of the follower the same approach (m) (m) (m) I I I ˙ ¨ Denoting by E , E and E the vectors as in the previous subsection is used. For this purpose, the 1 1 1 I I I ˙ ¨ built by the m-first coordinates of the E , E and E , differential equation for mapping E , defined in accordance 1 1 1 respectively and taking into account (36), it is easy to show to Eq. 32, is introduced: from Eq. 31 that: I I I I I ¨ ˙ E + E + E (m) 2 V,2 2 L,2 2 = 0, (41) d = E + p , II II II f E + E 2 L,2 2 (m) ˙ I (44) d = E , (m) I II ¨ I where E , E are constructed from the mapping E in sim- 2 d = E . 2 2 I I ilar way as for the trajectory of the leader (32), , , V,2 L,2 The m-first dependencies from Eq. 32 can be rewritten as: II are the diagonal matrices with positive coefficients L,2 ¨ I (m) ˙ I (m) ensuring the stability of the equilibrium of the Eq. 41. d + ( ) d + ( ) (d − p ) = 0, (45) V,1 L,1 J Intell Robot Syst (m) (m) I I is considered in Section 5.1. The centralized trajectory where , are the (m × m) submatrices V,1 L,1 generation method is presented in Section 5.2. Section 5.3 I I of , , respectively. V,1 L,1 shows application of the trajectory scaling technique to Taking into account that d = d 0 , dependency (45) fulfillment of the control constraints (19). can be written in the expanded form as: T 5.1 Manipulability Measure I I T ¨ ˙ d + d + d − p 0 = 0. (46) V,1 L,1 f This section considers the total manipulability measure of Introducing the matrix A : the system consisting of two mobile robots moving the common rigid object. Based on the manipulability ellipsoid A =− diag(d), diag d − p 0 , method for two cooperative robots [5], since there are no relative motions between the virtual end-effectors of the it is easy to show that the trajectory of the leader virtual robots, kinematic constraints (5) are written in a matrix form end-effector is linear with respect to : as: d = A . (47) 0 1 ϑ = Jq, ˙ (51) Finally, inserting (47) into the trajectory of the follower (42) and introducing substitutions: T T where ϑ = v ,v is the combined vector of the end- effectors velocities and J = blockdiag (J ,J ) is the −1 0 1 2 A = E , 0 block-diagonal matrix combined from the Jacobian matrices of both robots. I I I I I ˙ ˙ E q ˙ + E + E q,2 V,2 2 L,2 2 −1 Taking into account (51), the unit norm velocities in the b =−E , 2 II II II E q ˙ + E configuration space: q,2 L,2 2 the trajectory of the follower (42) can be also written as q ˙ q ˙ = 1, (52) linear with respect to : are transformed into an ellipsoid in the workspace as q ¨ = A + b . (48) 2 2 1 2 follows: Substituting (43)and (48) into the dynamic model (14), 1 =˙ q q ˙ controls of cooperating robots can be determined as: + T + = (J ϑ) J ϑ ˇ ˇ u = M + F, (49) T + T + T T = ϑ (J ) J ϑ T T T T ˇ ˇ where M = M A A and F = M b b + F. 1 2 1 2 T T T −1 T T T −1 = ϑ (J (JJ ) ) J (JJ ) ϑ As it is seen, controls of both cooperating robots are T T −1 T T T −1 linear with respect to gain coefficients of the leader .In = ϑ ((JJ ) ) JJ (JJ ) ϑ T T −1 T order to find controls fulfilling constraints resulting from = ϑ ((JJ ) ) ϑ limitations of mobile robots actuators, dependency (49)is T T T −1 = ϑ ((JJ ) ) ϑ substituted into conditions (19) and the following system of T T −1 = ϑ (JJ ) ϑ. (53) inequalities is obtained: ˇ ˇ The dependency (53) can be easily transformed to show u ≤ M + F ≤ u . (50) min 1 max relation with the Jacobian matrices of both robots: The dependency (50) introduces (n − h ) + (n − h ) 1 1 2 2 −1 inequalities, thus choice of the leader such that (n − h ) ≥ T T 1 1 1 = ϑ JJ ϑ (n − h ) results in dim( ) = 2 (n − h ), hence it is 2 2 1 1 1 −1 possible to determine ensuring fulfillment of constraints J 0 J 0 v 1 1 T T = v v (19). An alternative approach based on scaling of the 0 J 0 J v trajectory using certain heuristics can be found in [20]. −1 J J 0 v T T = v v −1 0 J J 5 Centralized Approach T T −1 T −1 = v (J J ) + (J J ) v, (54) 1 2 1 2 In this solution cooperating mobile manipulators are therefore the manipulability of cooperating robots can be considered as one centralized system, and therefore both measured as: robots are treated equally and none of them performs the T T −1 −1 −1 w(q) = det (((J J ) + (J J ) ) ). (55) 1 2 role of the leader. The manipulability measure of this system 1 2 J Intell Robot Syst 5.2 Trajectory Generation Then, taking into account the dependency (61), necessary condition (58) may be written as follows: In order to find, in a centralized way, the trajectory of both −1 T I − (J J ) I ,δq = 0, (62) F F R F robots fulfilling inequality constraints (17) and avoiding where I is the (2m + h + h ) dimensional vector corre- singular configurations, the extended performance index R 1 2 sponding to the vector δq , obtained by choosing elements I (q) is defined in the similar way as for individual robots: R from I related to components of the vector δq and q R l l 1 2 def I is the vector obtained by excluding I from I . F R q I (q) = I + κ ψ + κ ψ . (56) i,1 i,1 i,2 i,2 The consequence of the free vector δq in Eq. 62 is the i=0 i=0 following equation: where κ , κ are the penalty functions which associates i,1 i,2 −1 a penalty with a violation of the first and the second robot I − (J J ) I = 0. (63) F F R constraint, see Numerical example section for an exemplary The total error measure E (q, q ˙ ) of whole robot system is form of this function. determined analogously to error measures introduced earlier In the case of two cooperative robots considered as in Section 4: the one system, the problem of finding an optimal robots ⎡ ⎤ f q − p ( ) 1 1 f configuration q (T ) at the final location p should be ⎢ ⎥ f (q ) − f (q ) 1 1 2 2 considered: ⎢ ⎥ def −1 ⎢ T ⎥ E = I − (J J ) I . (64) F F R ⎢ R ⎥ f (q ) − p = 0, 1 1 f ⎣ ⎦ A q ˙ 1 1 f (q ) − f (q ) = 0, 1 1 2 2 A q ˙ 2 2 A q ˙ = 0, 1 1 In the similar way as in Section 4, using the error measure A q ˙ = 0, 2 2 (64), the dependency specifying the trajectory of the whole system of cooperating robots is proposed as a following min I (q) . (57) system of differential equations: Similarly as in previous section, transversality condition I I I I I ¨ ˙ E + E + E V L for the cooperating robots can be derived from the necessary = 0, (65) II II II E + E condition for minimum of the performance index I (q): I (q) ,δq= 0, (58) where E is constructed from the first three components of II mapping E, E is formed from the remaining components where I (q) = ∂I /∂q and δq is the variation of q. I I of E, , are the (n + n − h − h ) square diagonal 1 2 1 2 V L At the configuration q (T ) equality constraints from matrices with positive coefficients ensuring the stability Eq. 57 have to be fulfilled. After differentiating the first two II of the equilibrium of the first equation and is the constraints, they can be expressed in a compact form as: h + h square diagonal matrix with positive coefficients ( ) 1 2 J (q) δq = 0, (59) ensuring the stability of the equilibrium of the second T T T equation. where J = [J 0] , [J − J ] ,blockdiag(A ,A ) 1 1 2 1 2 Similarly as in previous section, the trajectory of coop- is the nonsingular matrix (nonsingularity is forced by erating robots can be determined by simple transformation minimization procedure). from Eq. 65 as: Since J is the full rank matrix, it is possible to written the I I I I I above dependency as: ˙ ˙ E q ˙ + E + E −1 V L q ¨ =−E (66) II II II E q ˙ + E T T q L [J J ] δq δq = 0, (60) R F R F I T II T I I I where J is the square matrix constructed from ˙ R where E = (E ) (E ) , E = ∂E /∂q, E = q q ˙ q q (2m + h + h ) linear independent columns of J, J is 1 2 F I II II II II dE /dt, E = ∂E /∂q ˙, E = ∂E /∂q. q q q ˙ the matrix obtained by excluding J from J, δq is the R R As in the leader-follower approach, suitable choice of the (2m + h + h ) dimensional vector corresponding to the 1 2 I I II gain coefficients , and leads to solution which V L L reduced matrix J , obtained by choosing elements from δq is asymptotically stable and strictly monotone. These prop- related to columns of the matrix J ,and δq is the vector R F erties imply that the robots reach the final location p with obtained by excluding δq from δq. zero velocity and their motion is free of singularities and ful- Multiplying the left side of Eq. 60, the related vector δq fills constraints (4), (5), (17) during the movement if initial can be expressed in terms of free vector δq as: configurations are nonsingular and satisfy constraints (4), −1 δq =−J J δq . (61) (5)and(17). R F F R J Intell Robot Syst 5.3 Control Constraints Fig. 2, the input signal of the trajectory generator is the error measure determined based on the current state of the system. To satisfy control constraints (19), trajectory (66) is scaled The output signals q, q ˙, q ¨ are obtained by solving the ini- in similar was as in leader-follower approach. Denoting: tial value problem given by dependencies (32)and(41)for the leader-follower approach or Eq. 65 for the centralized I T I T = diag( ) ,diag( ) approach. In each case, initial conditions are determined V L by using a suitable error measure for t = 0 allowing for and introducing the following substitutions: conditions (15). I I The solution scheme for both proposed approaches is diag(E ), diag(E ) −1 A =−E , 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 E q ˙ −1 q b =−E , II II II paragraphs. E q ˙ + E q L trajectory of the robots (66) can be written as linear with 6.1 Computational Burden respect to : Regardless of the used approach, finding the trajectory of q ¨ = A + b. (67) cooperating robots requires solving the initial value problem Similarly as in previous section, substituting (67)into givenbysystemof (n + n ) secondary differential equa- 1 2 dynamic model (14), controls of cooperating robots can be tions. The differences in a computational effort are related to determined as: the cost of determining the components of these equations. Let us compare the computational effort needed to deter- ˆ ˆ u = M + F, (68) mine the error measures in the case of the leader-follower ˆ ˆ where M = MA and F = Mb + F. (31), (40) and centralized approach (64). All components As it is seen, in contrast to leader-follower approach, in from E and E are included in the error measure E,how- 1 2 this case controls are linearly dependent on gain coefficients ever, the leader-follower approach requires computing two of both cooperating robots. In order to find controls ful- separate transversality conditions - one for the leader (30), filling constraints resulting from limitations of mobile robots and one for the follower (39) and in the centralized approach actuators, dependency (68) is substituted into conditions the single transversality condition (63) for both cooperating (19) and the following system of inequalities is obtained: robots is determined. In the first approach, robots are con- sidered as two individual systems, thus J , J are matrices 1 2 ˆ ˆ u ≤ M + F ≤ u . (69) min max of dimensions ((m + h ) × n ) and ((m + h ) × n ) and 1 1 2 2 vectors I , I have n and n elements, respectively. In q q 1 2 The dependency (69) introduces (n − h ) + (n − h ) 1 1 2 2 1 2 the second method, robots are treated as one centralized inequalities whereas dim( ) = 2 (n − h + n − h ), 1 1 2 2 system, in this case J is ((2m + h + h ) × (n + n )) thus it is possible to determine ensuring fulfillment of 1 2 1 2 matrix and I is (n + n ) elemental vector. The greater constraints (19). q 1 2 number of elements in the matrix J, in the case of the cen- tralized approach, leads to more operations needed to deter- mine the transversality condition. In particular, in the leader- 6 Discussion follower approach it is necessary to invert two matrices with dimensions m + h × m + h and m + h × m + h ( ) ( ) ( ) ( ) It seems that the both presented approaches are computa- 1 1 2 2 and in the centralized approach one (2m + h + h ) × 1 2 tionally effective and allow to find solutions satisfying all (2m + h + h ) matrix has to be inverted. the imposed constraints which is confirmed by the results of 1 2 The higher calculation costs associated with the deter- the simulations presented in Numerical example section. How- mination of the error measure E result in higher costs of ever, each of them has some advantages and disadvantages. I I I ˙ ˙ computing E , E and E . Moreover, in the centralized The methods described in this paper are based on the sim- q q approach, derivatives are calculated with respect to com- ilar approach in which trajectories of the robots are derived T T T bined vector q =[q ,q ] . It causes dimensions of matri- from differential equations using suitable defined error 1 2 I II ces E , E , and in a consequence E is ((n + n )× (n + 1 2 1 measures. The both proposed trajectory generators take state q q ˙ dependent constraints into account, maximize the manip- n )) matrix and E is ((2m + h + h ) × (n + n )) matrix. 2 1 2 1 2 ulability measure, and using suitable scaling techniques In the case of the leader-follower approach, derivatives are determine trajectories satisfying control constrains. Accord- calculated with respect to q and q , thus dimensions of 1 2 ing to the idea of the proposed control system shown in matrices E , E are equal to (n ×n ) and (n ×n ), whereas 1 2 1 1 2 2 J Intell Robot Syst I I ˙ ˙ dimensions of E , E are equal to ((m + h ) × n ) and proposed approaches can be employed directly in generat- 1 1 q,1 q,2 ((m + h ) × n ), respectively. Determining the trajectories ing the trajectory for multiple cooperating robots. In the 2 2 of robots requires inverting the matrices E and E and cal- case of the leader-follower approach, the robot with the 1 2 I I ˙ ˙ culation of products E q ˙ , E q ˙ in the leader-follower number of the degree of freedom not less than the num- 1 2 q,1 q,2 approach and the inversion of matrix E and calculation of ber of degrees of freedom of other robots is chosen as a product E q ˙ in the centralized approach. The larger matri- leader, the other robots become followers. The trajectory of ces size in the second case results in higher computational the leader is derived from dependency (35) and trajectories burden and it can lead to larger computation time. of the followers according to dependency (42). In the case of the centralized approach, the error measure (64)has to 6.2 Secondary Cost Functions be modified. Taking another robot into account requires the addition of two components, one related to the existence of According to the above analysis, the centralized approach a closed kinematic chain and the other resulting from the requires more computational burden but it allows to use constraints imposed on the robot platform motion. Simi- secondary cost functions taking into account properties larly, equality constraints in Eq. 57 should be completed in of the whole system such as a measure describing total order to derive the transversality condition for the whole manipulability of two cooperative robots. In the case of the system. Consideration about the computational burden and leader-follower approach, each of the robots is considered secondary cost functions for the case of two cooperating as an individual subsystem, and therefore secondary robots also applies the case of multiple robot system. performance indexes can be functions of state variables of The above analysis presents the general idea of the appli- the single robot only. Such approach allows to maximize cation of the method in the case of trajectory generation for the manipulability measure of each individual mechanism, multiple robots. However, these issues are out of the scope however does not provide optimization of the manipulability of this paper, they require a further elaboration and will be of the whole system, which is confirmed by the simulation the subject of further research. 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 7 Numerical Example of the whole mechanism is possible. Summarizing, where a sufficient computing power is available and it is important In the numerical example two mobile manipulators, shown to optimize secondary cost functions describing properties in Fig. 3, each composed of the nonholonomic differentially driven platform (2,0) class and planar 3-DOF holonomic of the whole system, the application of the centralized approach should be considered. manipulator are considered. The task of the robots is to move the uniform, rigid beam to the final location in the 6.3 Trajectory Generation for Multiple Robots workspace. Two cases of performing above task are consi- dered. In the first one control constraints are not taken The scope of the work is limited to the case of generating into account. In the second case, the methods described in the trajectory for two robots, however it seems that both Sections 4.4 and 5.3 are used to satisfy control constraints Fig. 3 Schematic diagram of two cooperative robots J Intell Robot Syst (19). In each example, two simulations for: leader-follower to limitations resulting from rolling and sliding constrains and centralized approach are carried out. of each platform wheel. For differentially driven platforms discussed in this work, each of fixed wheels of the i-th 7.1 System Parameters and Constraints platform introduces one rolling constraint: ˙ ˙ x ˙ c +˙ y s − a θ = r φ , c,i θ,i c,i θ,i i i i 1,i The system of two cooperating mobile manipulators taken ˙ ˙ x ˙ c +˙ y s + a θ = r φ , c,i θ,i c,i θ,i i i i 2,i into account in simulations is shown in Fig. 3.The i-th robot is described by the vector of generalized coordinates: where r is the radius of driving wheels and a is half- i i distance between the wheels. q = x ,y ,θ ,φ ,φ ,q ,q ,q , i c,i c,i i 1,i 2,i 1,i 2,i 3,i The wheels of each platform are parallel, thus the non- sliding constraints are equivalent and they can be written where [x ,y ] is the platform center location, θ is the c,i c,i i as: ,φ are the angles of driving platform orientation, φ 1,i 2,i wheels, q , q , q are the configuration angles of the 1,i 2,i 3,i −˙ x s +˙ y c = 0. c,i θ,i s,i θ,i manipulator joints. The mobile manipulators work in X Y plane of the B B Taking above wheel constraints into account, the velocity base coordinate system {B}. The coordinate system {P } is of the platform x ˙ , y ˙ ,θ can be determined from: c,i c,i i attached to the i-th mobile platform at the midpoint of the ⎡ ⎤ r r i i ⎡ ⎤ line segment connecting the two driving-wheels. The i-th c c θ,i θ,i 2 2 x ˙ c,i ⎢ ⎥ ˙ holonomic manipulator is connected to the i-th platform at r r 1,i i i ⎣ ⎦ ⎢ ⎥ y ˙ = s s . θ,i θ,i c,i 2 2 T ⎣ ⎦ the point [l , 0, 0] {P } system. The frame {O} is attached φ 2,i 0,i i θ r r i i i to the selected point of the object moved by the cooperating 2a 2a i i robots. The object frame {O} is rotated by ϕ around O,i Finally, using the above dependency, platform constraints Z-axis of the coordinate frame {G } attached to the can be expressed in the Pfaffian form as: grasper of i-th manipulator and translated by the vector ⎡ ⎤ [x ,y , 0] . The line section connecting the origins of ⎡ ⎤ O,i O,i x ˙ c,i r r i i 100 − c − c θ,i θ,i 2 2 ⎢ ⎥ the {G } and {O} systems is a virtual stick of the i-th i y ˙ c,i ⎢ ⎥ ⎢ ⎥ r r i i ⎢ ⎥ ⎢ ⎥ robot introduced in Section 2. The transformation matrices ˙ 010 − s − s θ = 0. θ,i θ,i i 2 2 ⎢ ⎥ ⎣ ⎦ ⎣ ˙ ⎦ describing kinematics of the mobile manipulators are given r r φ i i 1,i 001 − 2a 2a i i as follows: ˙ 2,i ⎡ ⎤ c −s 0 x θ,i θ,i c,i It is assumed that both robots have same kinematic and ⎢ ⎥ s c 0 y B θ,i θ,i c,i ⎢ ⎥ dynamic parameters, they are given as (all physical values T = , i ⎣ ⎦ 00 1 0 are expressed in the SI system): 00 0 1 l = l = 0.3,l = l = 0.5,a = 0.2,r = 0.08. 0,i 3,i 1,i 2,i i i ⎡ ⎤ c −s 0 l c + l 3,i 3,i k,i k,i 0,i k=1 The masses of the mobile manipulator elements amount to: ⎢ ⎥ P ⎢ s c 0 l s ⎥ i 3,i 3,i k,i k,i k=1 T = , ⎢ ⎥ m =20,m =0.2,m =2.5,m =2,m =1, ⎣ ⎦ p,i w,i 1,i 2,i 3,i 0 010 0 001 where m is the total mass of the platform, m is the p,i w,i mass of the platform wheel and m , m , m are the ⎡ ⎤ 1,i 2,i 3,i c −s 0 x ϕ,i ϕ,i O,i masses of the manipulator links. ⎢ ⎥ s c 0 y G ϕ,i ϕ,i O,i ⎢ ⎥ The task of the mobile manipulators is to move the T = , ⎣ ⎦ 00 1 0 uniform 2 kg beam 1.69 meter long from the initial location: 00 0 1 p =[1.15, 0.5,π/2] where l , l , l are the lengths of the manipulator links, 1,i 2,i 3,i c = cos(θ ), c = cos( q ), s = sin(θ ), θ,i i k,i j,i θ,i i j =1 to the final one: s = sin( q ). k,i j,i j =1 p =[4.0, 2.0, −π/2] . During the task accomplishment, the mobile robots are subject to limitations causing by the constraints of the The location and orientation of the object frame {O} with platform motion. According to the analysis of kinematic respect to the grasper frames {G } determine parameters: properties of wheeled mobile robots presented by Campion et al. in [4], the motion of the wheeled platform is subject x = 1.69/2,y = 0,ϕ = 0,ϕ = π. 0,i 0,i O,1 O,2 J Intell Robot Syst At the initial moment of motion the robots are in the the given value e and generalized velocities of both robots non-singular, collision-free configurations: are less than the given value e , i.e.: q (0) =[0, −1, 0, 0, 0, 0,π/4,π/4] , f (q )−p <e and (˙ q <e ) and (˙ q <e ) . 1 1 1 f p 1 v 2 v In the all presented simulations, the values for e and e q (0) =[0, 2, 0, 0, 0, 0, −π/4, −π/4] , p v equal to 0.01 have been used. for which the beam is in the location p . To preserve mechanical constraints generalized coordi- 7.2 Trajectory Without Control Constraints nates of holonomic manipulators should not exceed limits: q =−q = [2π/5,π/3,π/3] . In the first case, methods described in Sections 4 and 5 are max,i min,i used to generate trajectory without taking into account con- Scalar functions involving fulfillment of k-th constraint trol constraints. The values of the gain coefficients for the connected to k-th generalized coordinate are taken as: leader-follower approach are taken as: ψ = q − q k,i k,i min,k,i I I II = I, = 2.1 , = I, for k = 1, 2, 3. L,i V,i L,i L,i ψ = q − q k+3,i max,k,i k,i and for the centralized approach they are equal to: Then, the penalty functions used in the performance indexes I I I II = I, = 2.1 , = I, (23), (37)and (56), ensuring fulfillment of the mechanical L V L L constraints are proposed as: where I is the identity matrix of the appropriate size. The final time of task execution T for the leader-follower κ ψ = ρ /ψ , k,i k,i m k,i approach is obtained as 11.31 [s]. The robots motion is where ρ is the positive coefficient determining strength of shown in Fig. 4. The leader is plotted with solid line and penalty for the mechanical constraints. the follower with dashed line. The initial, the final and In order to preserve potential collisions between robots two intermediate phases of the motion are shown. The and the object, the approach based on obstacles enlargement obtained trajectories of holonomic parts of the robots are with a simultaneous discretization of the mobile manipula- showninFig. 5. The horizontal lines represent the assumed tor,shownin[21], has been adopted. In order to apply this mechanical constraints: the dashed lines for the first joint, approach, the carried object is an obstacle for both robots the dotted lines for the second and the third joint. As can and each robot is treated as an obstacle for the second be seen the obtained trajectories do not exceed the assumed one. The object and segments of the mobile manipulators constraints. Finally, the controls of the leader and the (platforms, manipulator links) are approximated by smooth surfaces and collision avoidance conditions are written as: ψ = η x − δ, f or k > 6, k,i j d,i where η is the equation of j-th obstacle surface, x is j d,i the point from the set of points which approximates the i- th 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: ρ ψ − ε for ψ ≤ ε c k,i k,i 1 κ ψ = , k,i k,i 0 otherwise where ρ denotes the constant positive coefficient determin- c 0 ing the strength of penalty and ε is the constant positive coefficient determining the threshold value which activates −1 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 −2 −1 0 1 2 3 4 5 6 p and fulfillment of the boundary conditions (16) with a X [m] desired accuracy. It has been assumed that the algorithms stop when the distance to the desired location p is less than Fig. 4 Robots motion for the leader-follower approach Y [m] B J Intell Robot Syst q q q 1,1 2,1 3,1 u u 1,1 2,1 −1 −5 −10 0 2 4 6 8 10 12 0 2 4 6 8 10 12 t [s] t [s] q q q 1,2 2,2 3,2 −2 −1 −4 u u u 3,1 4,1 5,1 0 2 4 6 8 10 12 −6 0 2 4 6 8 10 12 t [s] t [s] Fig. 5 Trajectories of the holonomic manipulators of the leader (top) Fig. 6 Leader torques for the first case and the follower (bottom) for the first case follower (torques of the platform wheels and manipulator joints) are shown in Figs. 6 and 7, respectively. Let us note that controls are continuous functions of the time and they u u 1,2 2,2 are within ranges: T T −[2.9, 6.9, 0.5, 2.6, 3.6] ≤ u ≤[6.3, 5.4, 2.7, 0.2, 0.1] T T −[3.3, 3.4, 0.7, 0.2, 5.1] ≤ u ≤[19.8, 10.7, 2.5, 1.1, 0.0] . 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 −5 in Fig. 8. The first robot is plotted with solid line and the −10 second robot with dashed line. Similarly as for the leader- 0 2 4 6 8 10 12 follower approach, four phases of the motion are shown. In t [s] 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.Asitis seen, controls are continuous functions of the time and in this case they are within ranges: −2 T T −[3.4, 5.4, 0.7, 2.4, 3.4] ≤ u ≤[6.5, 5.8, 2.8, 0.3, 0.3] −4 T T −[3.4, 3.4, 1.4, 0.1, 4.9] ≤ u ≤[18.0, 11.6, 2.5, 1.3, 0.6] . u u u 3,2 4,2 5,2 −6 Additionally, the comparison of robots motions obtained 0 2 4 6 8 10 12 t [s] in both simulations can be seen at the animation available on-line at [1]. Fig. 7 Follower torques for the first case q , q , q [rad] q , q , q [rad] 1,2 2,2 3,2 1,1 2,1 3,1 u , u [Nm] u , u [Nm] u , u , u [Nm] u , u , u [Nm] 1,2 2,2 1,1 2,1 3,2 4,2 5,2 3,1 4,1 5,1 J Intell Robot Syst 5 20 u u 1,1 2,1 −5 −10 0 2 4 6 8 10 12 t [s] −1 −2 −2 −1 0 1 2 3 4 5 6 X [m] −4 u u u Fig. 8 Robots motion for the centralized approach (solid line - the first 3,1 4,1 5,1 −6 robot, dashed line - the second robot) 0 2 4 6 8 10 12 t [s] Fig. 10 First robot torques for the first case q q q 1,1 2,1 3,1 1 u u 1,2 2,2 −1 −5 0 2 4 6 8 10 12 −10 t [s] 0 2 4 6 8 10 12 t [s] q q q 1,2 2,2 3,2 −2 −1 −4 u u u 3,2 4,2 5,2 0 2 4 6 8 10 12 −6 t [s] 0 2 4 6 8 10 12 t [s] Fig. 9 Trajectories of the holonomic manipulators of the first (top) and the second (bottom) robot for the first case Fig. 11 Second robot torques for the first case q , q , q [rad] q , q , q [rad] Y [m] 1,2 2,2 3,2 1,1 2,1 3,1 B u , u [Nm] u , u [Nm] u , u , u [Nm] u , u , u [Nm] 1,2 2,2 1,1 2,1 3,2 4,2 5,2 3,1 4,1 5,1 J Intell Robot Syst q q q u u 1,1 2,1 3,1 1,2 2,2 −2 −1 −4 −6 0 4 8 12 16 20 24 0 4 8 12 16 20 24 t [s] t [s] 1.5 q q q 1,2 2,2 3,2 0.5 −0.5 −1 −1 u u u 3,2 4,2 5,2 0 4 8 12 16 20 24 −1.5 0 4 8 12 16 20 24 t [s] t [s] Fig. 12 Trajectories of the holonomic manipulators of the leader (top) Fig. 14 Follower torques for the second case and the follower (bottom) for the second case q q q 1,1 2,1 3,1 u u 1,1 2,1 −2 −1 −4 0 4 8 12 16 20 24 −6 t [s] 0 4 8 12 16 20 24 t [s] q q q 1.5 1,2 2,2 3,2 0.5 −0.5 −1 −1 u u u 3,1 4,1 5,1 0 4 8 12 16 20 24 −1.5 t [s] 0 4 8 12 16 20 24 t [s] Fig. 15 Trajectories of the holonomic manipulators of the first (top) Fig. 13 Leader torques for the second case and the second (bottom) robot for the second case u , u , u [Nm] q , q , q [rad] q , q , q [rad] u , u [Nm] 3,1 4,1 5,1 1,2 2,2 3,2 1,1 2,1 3,1 1,1 2,1 u , u , u [Nm] q , q , q [rad] q , q , q [rad] u , u [Nm] 1,2 2,2 3,2 1,1 2,1 3,1 3,2 4,2 5,2 1,2 2,2 J Intell Robot Syst 7.3 Trajectory with Control Constraints 6 u u 1,2 2,2 In the second case, it is assumed that limitations on controls resulting from physical abilities of the actuators are equal to: u =−u = [5.0, 5.0, 1.0, 1.0, 1.0] . 0 max,i min,i −2 As can be seen, controls determined in the previous subsection exceed above limits both for the leader-follower −4 and centralized approach. −6 As it has been shown in Sections 4.4 and 5.3, controls 0 4 8 12 16 20 24 are linearly dependent on or . Therefore, it is possible t [s] to determine the values of gain coefficients in such a 1.5 way that control constraints are satisfied. For simplicity of simulations, and are assumed to be constant and for the leader-follower approach they are equal to: 0.5 I I I = 0.18 I, = 2.1 , L,1 V,1 L,1 and for the centralized approach are taken as: I I I −0.5 = 0.19 I, = 2.1 . L V L −1 The final time T , in both simulations, is increased and u u u 3,2 4,2 5,2 it equals 23.22 [s] for the leader-follower approach and it −1.5 0 4 8 12 16 20 24 equals 23.91 [s] for the centralized approach. The deter- t [s] mined trajectories of the holonomic manipulators and robot Fig. 17 Second robot torques for the second case controls in both simulations are shown in Figs. 12, 13, 14, 15, 16 and 17. The dashed and dotted horizontal lines repre- sent the assumed mechanical and control constraint. As can 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 u u 1,1 2,1 techniques results in a reduction of controls and, as a conse- quence, 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. −2 The advantage of the centralized approach is the ability to take into account the total manipulability of the whole sys- −4 tem, which was not possible in the leader-follower method. −6 In order to compare results of both trajectory generators, the 0 4 8 12 16 20 24 t [s] 1.5 0.355 0.35 0.5 0.345 −0.5 0.34 −1 u u u 3,1 4,1 5,1 0.335 0 4 8 12 16 20 24 −1.5 t [s] 0 4 8 12 16 20 24 t [s] Fig. 18 Total manipulability of the system for the second case (leader- Fig. 16 First robot torques for the second case follower approach) u , u , u [Nm] u , u [Nm] 3,1 4,1 5,1 1,1 2,1 w u , u , u [Nm] u , u [Nm] 3,2 4,2 5,2 1,2 2,2 J Intell Robot Syst 0.355 due to the higher computation efficiency resulting in lower computational burden. The centralized approach is more computationally demanding but it allows to take into 0.35 account more complex performance indexes including state variables of both cooperating robots, thus it lets to optimize 0.345 total manipulability measure of the whole system. However, the presented work is limited to the case of generating the 0.34 trajectory for two robots, the application of the proposed approaches in generating the trajectory for multiple coop- 0.335 erating robots has been discussed and it will be the subject 0 4 8 12 16 20 24 t [s] of the further research. In the proposed approaches, the original boundary value Fig. 19 Total manipulability of the system for the second case problems have been transformed into the initial value (centralized approach) problems, then the proposed solutions are computationally efficient. The effectiveness of the presented methods are confirmed by the results of computer simulations. total manipulability measure (55) has been calculated for both the leader-follower and the centralized approach. Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http:// Changes of these indexes over time are shown in Figs. 18 creativecommons.org/licenses/by/4.0/), which permits unrestricted and 19, respectively. As it is seen in Fig. 18, optimization use, distribution, and reproduction in any medium, provided you give of the manipulability measure of the individual subsystems appropriate credit to the original author(s) and the source, provide a (leader and follower) does not lead to optimal manipulabil- link to the Creative Commons license, and indicate if changes were made. ity of the whole system. Publisher’s Note Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations. 8 Conclusion In the presented paper, the original boundary value problem References has been transformed into the initial value problem - so it seems that the proposed solution is computationally efficient. 1. Results of the simulations. http://staff.uz.zgora.pl/gpajak/rtoolbox/ In this paper two suboptimal methods of trajectory jint17i generation for mobile manipulators moving the common 2. Abou-Samah, M., Tang, C.P., Bhatt, R.M., Krovi, V.: A kinemati- rigid object to a specified location in the workspace have cally compatible framework for cooperative payload transport by non- holonomic mobile manipulators. Auton. Robot. 21(3), 227–242 been proposed. Presented approaches guarantee fulfillment (2006). https://doi.org/10.1007/s10514-005-9717-9 of constraints imposed by the closed kinematic chain as 3. Brahmi, A., Saad, M., Gauthier, G., Zhu, W.H., Ghommam, J.: well as boundary conditions resulting from the task to Adaptive control of multiple mobile manipulators transporting a be performed. Constraints connected with the existence of rigid object. Int. J. Control. Autom. Syst. 15(4), 1779–1789 (2017). https://doi.org/10.1007/s12555-015-0116-x mechanical limitations for manipulator configuration, col- 4. Campion, G., Bastin, G., DAndreaNovel, B.: Structural properties lision avoidance conditions and control constraints have and classification of kinematic and dynamic models of wheeled been considered in both methods. Minimization of instanta- mobile robots. IEEE Trans. Robot. Autom. 12(1), 47–62 (1996). neous performance indexes ensures that during the motion, https://doi.org/10.1109/70.481750 5. Chiacchio, P., Chiaverini, S., Sciavicco, L., Siciliano, B.: On the robots are far away from singular configurations. The pro- manipulability of dual cooperative robots. In: NASA Conference posed methods do not guarantee the global optimality on Space Telerobotics. Pasadena (1989) because they minimize the performance indexes in each 6. Dai, Y., Kim, Y., Wee, S., Lee, D., Lee, S.: Symmetric caging time instant, then the obtained solutions are suboptimal. formation for convex polygonal object transportation by multiple mobile robots based on fuzzy sliding mode control. ISA Trans. 60, The problem has been solved by using penalty functions 321–332 (2016). https://doi.org/10.1016/j.isatra.2015.11.017 and a redundancy resolution at the acceleration level. The 7. Desai, J., Kumar, V.: Nonholonomic motion planning for mul- resulting trajectories have been scaled in a manner to fulfill tiple mobile manipulators. In: Proc. IEEE International Confer- the constraints imposed on controls. The property of asymp- ence on Robotics and Automation (ICRA’97), pp. 3409–3414. Albuquerque (1997) totic stability of the proposed control systems implies fulfill- 8. Desai, J., Kumar, V.: Motion planning for cooperating mobile ment of all the constraints imposed. manipulators. J. Robot. Syst. 16(10), 557–579 (1999) Advantages and disadvantages of both methods have 9. Fink, J., Hsieh, M.A., Kumar, V.: Multi-robot manipulation via been discussed. The leader-follower approach is attractive caging in environments with obstacles. In: Proc. IEEE International w J Intell Robot Syst Conference on Robotics and Automation, vol. 1–9, pp. 1471+. Motion and Control (RoMoCo’04), pp. 121–126. Puszczykowo. New York. https://doi.org/10.1109/ROBOT.2008.4543409 (2008) https://doi.org/10.1109/ROMOCO.2004.240908 (2004) 10. Florin, A., Silvia, A.: Synchronizing robot motions in cooperative 23. Pajak, I.: Trajectory planning of cooperative mobile manipulators tasks. Control Eng. Appl. Inform. 13(1), 43–48 (2011) subject to control constraints. In: Proc. 11th International 11. Galicki, M.: Two-stage constrained control of mobile manipulators. Workshop on Robot Motion and Control (RoMoCo’17), pp. 123– Mech. Mach. Theory 54, 18–40 (2012). https://doi.org/10.1016/ 128 (2017) j.mechmachtheory.2012.04.001 24. Seraji, H.: A unified approach to motion control of mobile manip- 12. Galicki, M.: Real-time constrained trajectory generation of mobile ulators. Int. J. Robot. Res. 17(2), 107–118 (1998). https://doi.org/ manipulators. Robot. Auton. Syst. 78, 49–62 (2016). https://doi.org/ 10.1177/027836499801700201 10.1016/j.robot.2016.01.008 25. Sugar, T., Kumar, V.: Control of cooperating mobile manipulators. 13. Gueaieb, W., Al-Sharhan, S., Bolic, M.: Robust computationally IEEE Trans. Robot. Autom. 18(1), 94–103 (2002). https://doi.org/ efficient control of cooperative closed-chain manipulators with 10.1109/70.988979 uncertain dynamics. Automatica 43(5), 842–851 (2007). https:// 26. Tanner, H., Loizou, S., Kyriakopoulos, K.: Nonholonomic naviga- doi.org/10.1016/j.automatica.2006.10.025 tion and control of cooperating mobile manipulators. IEEE Trans. 14. Hacioglu, Y., Arslan, Y.Z., Yagiz, N.: MIMO fuzzy sliding mode Robot. Autom. 19(1), 53–64 (2003). https://doi.org/10.1109/TRA. controlled dual arm robot in load transportation. J. Franklin 2002.807549 I. 348(8), 1886–1902 (2011). https://doi.org/10.1016/j.jfranklin. 27. Trebi-Ollennu, A., Das Nayar, H., Aghazarian, H., Ganino, A., 2011.05.009 Pirjanian, P., Kennedy, B., Huntsberger, T., Schenker, P.: Mars 15. Hirata, Y., Kume, Y., Sawada, T., Wang, Z., Kosuge, K.: Handling rover pair cooperatively transporting a long payload. In: Proc. of an object by multiple mobile manipulators in coordination based on IEEE International Conference on Robotics and Automation, vol. caster-like dynamics. In: Proc. IEEE International Conference on I-IV, pp. 3136–3141 (2002) Robotics and Automation, vol. 1–5, pp. 807–812. https://doi.org/ 28. Uchiyama, M., Dauchez, P.: A symmetric hybrid position/force 10.1109/ROBOT.2004.1307248 (2004) control scheme for the coordination of two robots. In: Proc. IEEE 16. Jain, A.: Robot and multibody dynamics: analysis and algorithms. International Conference on Robotics and Automation, pp 350– In: Robot and Multibody Dynamics: Analysis and Algorithms, 356. Philadelphia (1988) pp. 3–482. Springer, New York (2011). https://doi.org/10.1007/ 29. Wang, Y., Wang, D., Zhu, S.: A new navigation function 978-1-4419-7267-5 based decentralized control of multi-vehicle systems in unknown 17. Khan, M.U., Li, S., Wang, Q., Shao, Z.: Formation control and environments. J. Intell. Robot. Syst. 87(2), 363–377 (2017). tracking for co-operative robots with non-holonomic constraints. https://doi.org/10.1007/s10846-016-0450-0 J. Intell. Robot. Syst. 82(1), 163–174 (2016). https://doi.org/10.1007/ 30. Yamada, S., Saito, J.: Adaptive action selection without explicit s10846-015-0287-y communication for multirobot box-pushing. IEEE Trans. Syst. 18. Kume, Y., Hirata, Y., Wang, Z., Kosuge, K.: Decentralized control Man Cybern. C 31(3), 398–404 (2001). https://doi.org/10.1109/ of multiple mobile manipulators handling a single object in 5326.971668 coordination. In: 2002 IEEE/RSJ International conference on 31. Yoshikawa, T.: Manipulability of robotic mechanisms. Int. J. intelligent robots and systems, vol. 1–3, Proceedings, pp. 2758– Robot. Res. 4(2), 3–9 (1985). https://doi.org/10.1177/02783649 2763 (2002) 19. Luh, J., Zheng, Y.: Constrained relations between two coordinated 32. Zhang, F., Hua, L., Fu, Y., Guo, B.: Dynamic simulation and industrial robots for motion control. Int. J. Robot. Res. 6(3), 60–70 analysis for bolt and nut mating of dual arm robot. In: 2012 IEEE (1987) International Conference on Robotics and Biomimetics (ROBIO 20. Pajak, G.: Trajectory planning for mobile manipulators subject 2012) (2012) to control constraints. In: Proc. 11th International Workshop on Robot Motion and Control (RoMoCo’17), pp. 117–122 (2017) 21. Pajak, G., Pajak, I.: Point-to-point collision-free trajectory Iwona Pajak received PhD degree in Automatics and Robotics from planning for mobile manipulators. J. Intell. Robot. Syst. 85(3-4, Poznan University of Technology, Poland in 2000. Now she works at SI), 523–538 (2017). https://doi.org/10.1007/s10846-016-0390-8 22. Pajak, G., Pajak, I., Galicki, M.: Trajectory planning of multiple University of Zielona Gora. Her current research interests include real manipulators. In: Proc. 4th International Workshop on Robot time motion planning of mobile manipulators and robots. http://www.deepdyve.com/assets/images/DeepDyve-Logo-lg.png Journal of Intelligent & Robotic Systems Springer Journals

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

Free
20 pages

Loading next page...
 
/lp/springer_journal/real-time-trajectory-generation-methods-for-cooperating-mobile-9VDdsXXTS1
Publisher
Springer Journals
Copyright
Copyright © 2018 by The Author(s)
Subject
Engineering; Control, Robotics, Mechatronics; Electrical Engineering; Artificial Intelligence (incl. Robotics); Mechanical Engineering
ISSN
0921-0296
eISSN
1573-0409
D.O.I.
10.1007/s10846-018-0878-5
Publisher site
See Article on Publisher Site

Abstract

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. Keywords Multiple mobile robots · Cooperative manipulation · Trajectory generation 1 Introduction robot. At first, limitations arising from the physical abilities of the cooperating robots have to be taken into account. There is a class of tasks that cannot be executed by a single Additionally, the use of the mobile robots with nonholo- robot. They are associated with the movement of heavy nomic platforms requires consideration of pure rolling and non-slip conditions. Moreover, the robots moving the com- objects that a single robot cannot lift due to physical limita- tions of its actuators. Another example of these tasks is the mon object create the closed kinematic chain which imposes movement of the large objects which must be supported at an additional constraint that has to be allowed for. several points. The necessity of performing such tasks leads to rise the interest in the trajectory generation for multiple 1.1 Related Works robots performing the common task. Combining the mobil- In the literature there are several approaches to the motion ity of the platform with dexterity of the manipulator, the control for mobile manipulators. Some solutions employ the mobile manipulators seem to be especially attractive in this calculus of variations. This approach has been presented by application. The platform significantly increases the oper- Desai and Kumar in [7, 8] to formulate a general approach ating range of the robot, and therefore such a system may in order to obtain optimal trajectories for nonholonomic be used to transport objects over long distances. Moreover, cooperating mobile robots. Solutions based on a pseudo- after reaching the desired location, dexterity of the manip- inverse of the Jacobian matrix have been proposed in [11, ulator allows to the precise placement of the object in the 12] in order to generate real-time collision-free trajectories. desired position. Seraji in [24] has used the augmented Jacobian matrix and Solving the trajectory generation problem for multiple has introduced an on-line approach for motion control on mobile manipulators is more complicated than for a single the kinematic level. The method discussed in this paper employs the augmented Jacobian matrix, however it is appli- cable to cooperative robot tasks, generated trajectories take Iwona Pajak account of the dynamics of the system, control limitations i.pajak@iizp.uz.zgora.pl and state constraints. In the recent years an increasing research interest in the Faculty of Mechanical Engineering, University of Zielona Gora, 65-246 Zielona Gora, Poland multi-robot systems has been noticed. A large number of J Intell Robot Syst studies concern problems of motion control for multiple using the geometric relations among the robots. Luh and robots performing a common task in the same workspace. Zheng in [19] have proposed a leader-follower approach in Some works consider control design problems for groups which they derived the relation between the joint positions of vehicles which have to achieve a specified location [29] and velocities of the leader and the follower as a set of holo- or move in a given formation [17]. Such solutions are often nomic equality constraints. Other leader-follower approaches limited to use only the mobile platforms, thus the robots do have been presented in [25, 27]. Sugar and Kumar in [25] not have manipulation abilities. Moreover, the method of this have discussed the motion control method for a small team type cannot be directly adopted to solving the cooperative of mobile manipulators that cooperatively transport a large, transportation tasks by multiple mobile robots. In the case flexible object in a two dimensional environment with of such tasks, two general methods are used: pushing and obstacles. In [27] Trebi-Ollennu et al. have proposed an grasping. In the first approach the object is pushed by a decentralized algorithm for two Mars rovers carrying a long group of the robots [6, 9, 30], however, in this case the object over an uneven, natural terrain. Abou-Samah et al. in motion of the object cannot be controlled precisely. [2] have considered the leader-follower approach for coop- In the second approach robots are equipped with erative payload transport based on the individual mobile graspers and they hold the object during transportation, and manipulator control schemes. In [13] a decentralized hybrid therefore the more precise manipulation is possible. The position/force controller has been presented for the control solution of such tasks is more difficult due to the neces- of multiple tightly coupled manipulators handling a com- sity to satisfy constraints resulting from the closed kine- mon object. Adaptive control law developed from the virtual matic chain established by cooperative robots and the object decomposition approach has been designed by Brahmi et al. held by them. The methods of motion control for multiple in [3]. cooperative robots have been first developed for multiple manipulators. These studies are further intensively devel- 1.2 Summary of the Proposed Solution oped especially in the case of coordinated manipulation tasks, such as bolt and nut mating [32] and transportation In this paper, two trajectory generation methods for mobile tasks [10, 14]. Stationary manipulators have a limited work- manipulators moving the common rigid object are pre- space, thus they cannot be used to carry long objects over sented. The proposed solutions are the development of the long distances. Due to the mobility of the platform, mobile method discussed in [21] and shows that an approach for robots are more suitable for this type of tasks. The ideas used a single mobile manipulator can be easily adopted to the in the case of cooperative manipulators may be employed cooperative task of two mobile robots. In the first method, the trajectory generation problem is solved by using the for motion control of mobile robots but the control task is more complicated due to limitations resulting from a plat- leader-follower conception. In this approach, the leader form movement. determines the motion and the follower assists the leader Research carried out in the field of multiple robots supporting the object. In the presented work, the leader per- motion control has been aimed at solving the problem using forms the point-to-point task moving its end-effector to the centralized and decentralized approaches. In the centralized final location resulting from the desired pose of the object. methods, motion control applies to all cooperating robots The follower tracks the trajectory imposed by the leader simultaneously. The methods of this type are based on motion ensuring the fulfillment of the constraints which full knowledge of the environment and they concentrate on results from the closed kinematic chain formed by coop- searching an optimal solution. Desai and Kumar in [7, 8] erating robots. This approach is the development of the have presented the method based on calculus of variations idea presented in [23]. In the second method, the trajec- for optimal control for mobile manipulators cooperating in tory generation problem is solved in a centralized way. In the workspace including obstacles. In [26] Tanner et al. have this case, the both robots are treated as the one system con- discussed globally asymptotically stable centralized feedback strained by conditions resulting from the closed kinematic controller for mobile manipulators moving a deformable chain and they perform the point-to-point task moving the object in the environment including obstacles. object to its final location. The solution scheme for both While the centralized methods usually lead to algorithms proposed approaches is similar, however, each of them has with high computational complexity, the decentralized some advantages and disadvantages that can be crucial in approaches are computationally more efficient due to decom- choosing a particular approach for a particular application. position of the motion control task. In [15, 18] the authors These issues will be detailed discussed in Section 6. have considered a leader-follower motion control algorithm In both presented methods, the trajectories of the robots for multiple mobile manipulators moving a common object. are generated to minimize the performance indexes to avoid In this approach, each robot has had a caster-like dynam- manipulators singularities. In the first case, maximization of ics in 3D space and an object has been manipulated without the manipulability measures of the leader and the follower J Intell Robot Syst is carried out. In the second one, the total manipulability manipulators consisting of a nonholonomic (2,0) class plat- of two cooperative robots is maximized. Additionally, the form and a 3-DOF planar holonomic manipulator moving constraints causing by mechanical and control limitations as the rigid beam in 2D space. 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 optimiza- 2 Model of the System tion problem with holonomic and nonholonomic equality constrains, and inequality constraints resulting from col- In the cooperative task considered in this paper two mobile lision avoidance and mechanical limitations. Finally, the manipulators are moving the common rigid object, firmly resulting trajectories are scaled to fulfill the constraints connected to their end-effectors, to the specified location in imposed on the controls. As it is shown further on, in the the workspace. Manipulators and an object form a closed leader-follower approach, controls of the system depend on kinematic chain, therefore knowledge of the kinematics and the motion imposed by the leader, thus satisfying control dynamics of such a system is necessary to formulate the task constraints is obtained by trajectory scaling of the leader as a consequence of a cooperation. only. In the centralized approach, controls of the system depend on the trajectory of the whole system, and there- 2.1 Kinematics fore in this case, the trajectory of both cooperating robots is scaled to fulfill control constraints. In order to describe the kinematics of the closed kinematic To the best of the author knowledge, no research has con- chain consisted of two mobile robots and an object carried sidered the cooperative task formulated in the above manner. by them, the location of the object in the workspace must be Some of the existing research addresses the problem using specified. For this purpose, as it is shown in Fig. 1, a local only kinematic equations of the mobile manipulators [19, coordinate frame {O} attached to the object at any given 24, 26] and the dynamics of the robots is not considered at point is introduced. The pose of this local frame in a global all. The studies considering the dynamics of the cooperative reference frame {B} specifies the location of the object is systems do not allow to take into account control constraints described by the m-elemental vector p: [2, 3, 7, 8]. In contrast, the presented solution takes into con- def T T p = x ,ϕ , (1) siderations dynamics of the cooperative system, moreover, it is distinguished by the method of determining the con- where x and ϕ are the vectors describing position and tinuous controls which fulfill the assumed constraints. The orientation of the local frame attached to the object in a results of the proposed approach are trajectories parameter- global reference frame. ized by gain coefficients, which can be chosen in such a way In this paper, it is assumed that each of cooperating robots to fulfill the imposed control constraints. The proposed tra- is composed of a nonholonomic platform and a holonomic jectory generation algorithm does not need the knowledge manipulator with kinematic pairs of the 5th class. They are of the system dynamics, the dynamic model is necessary described by the vectors of generalized coordinates: to determine the values of gain coefficients only. Other def T T research considering the dynamics of the robots produces q = q q ,i = 1, 2, (2) p,i r,i controls parameterized by gain coefficients of the controller where q is the n -dimensional vector of the generalized i i directly. In such cases, it is very difficult (or impossible) to coordinates of the i-th mobile manipulator, q is the p - p,i i find coefficients which fulfill control limits. dimensional vector of the coordinates of the i-th nonholo- The paper is organized as follows. Section 2 presents kine- nomic platform, q is the r -dimensional vector of joints r,i i matic and dynamic model of cooperating robots. Section 3 coordinates of the i-th holonomic manipulator, n = p + r . i i i 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 con- trol constraints. Advantages and disadvantages of presented methods are discussed in Section 6. The proposed methods are demonstrated numerically in Section 7, for two mobile Fig. 1 Virtual stick conception J Intell Robot Syst In order to describe the geometry of the grasp the where M (q) is the (n × n ) positive definite inertia i i i approach of a virtual stick is adopted [28]. In this case the matrix, F (q , q ˙ ) is the n -dimensional vector representing i i i i virtual stick is a constant vector which determines the origin Coriolis, centrifugal, viscous, Coulomb friction and gravity of the object frame in the manipulator end-effector frame. forces, M and F contain both robot and object dynamic i i In Fig. 1 two manipulators holding a common object are properties, A (q) = A 0 , 0 is the (h × r ) zero matrix, i i i i presented. The vectors vs and vs represent virtual sticks λ is the h -dimensional vector of the Lagrange mul- 1 2 i i of the first and second robot. They connect the origins of tipliers corresponding to constraints (6)and B is the frames {G }, {G } attached to the end-effectors of both n × n − h full rank matrix (by definition) describing ( ( )) 1 2 i i i robots with the origin of the object frame {O}. Additionally, which state variables of the mobile manipulator are directly at the end points of vs and vs the virtual end-effectors driven by the actuators. 1 2 T T with the same orientation as the object frame are introduced. Introducing the combined vectors q = q ,q , q ˙ = 1 2 The idea mentioned above is used in this work to describe T T q ˙ , q ˙ and the matrix J (q) = [J , −J ], the kinematic 1 2 1 2 geometry of the closed kinematic chain formed by two constraints (5) can be expressed in a compact form as: cooperating robots. J q ˙ = 0, (8) Using the virtual stick approach, the location of the object can be expressed with respect to generalized coordinates of therefore the overall dynamic equations for the cooperating the mobile manipulator as: robots taking into account both platform (6) and closed kinematic chain (8) constraints, can be written as: p = f (q ),i = 1, 2, (3) i i T T n m Mq ¨ + F + A λ + J λ = Bu.(9) i p c where f : → denotes the m-dimensional mapping, T T T which describes the position and orientation of the virtual T T T T T T where q ¨ = q ¨ , q ¨ , F = F ,F , λ = λ ,λ , 1 2 1 2 1 2 end-effector of the i-th manipulator in the workspace. T T u = u ,u , A, M, B are the block diagonal matrices 1 2 Since there are no relative motions between the virtual end- defined as follows: M = blockdiag (M ,M ), A = blockdiag 1 2 effectors of two cooperating robots (assuming the object is (A ,A ), B = blockdiag (B ,B ), λ is the m-dimensional 1 2 1 2 c rigid), the following geometric and kinematic constraints vector of the Lagrange multipliers corresponding to the should be fulfilled: constraints (8). q = p = f q , (4) f ( ) ( ) 1 1 2 2 A (q) is the full rank matrix, thus there exists the full rank J (q ) q ˙ = v = J (q ) q ˙.(5) 1 1 1 2 2 2 (n + n )×(n + n − h − h ) matrix N (q), orthogonal 1 2 1 2 1 2 p to A (q), which satisfies the relation: where J = ∂f (q ) /∂q is the (m × n ) Jacobian matrix of i i i i the i-th manipulator and v denotes the common velocity of AN = 0. (10) the virtual end-effectors of the robots in the workspace. Using the above dependency and multiplying the dynamic Moreover, due to use of mobile robots, it is necessary to equation (9)by N , the vector of Lagrange multipliers λ take into account holonomic and nonholonomic constraints can be eliminated and the model takes a new form as: which limit the platform motion capabilities. The con- T T T T T N Mq ¨ + N F + N J λ = N Bu. (11) straints of this type, for the i-th platform, can be described c p p p p in the Pfaffian form as: Assuming that the matrix JN is the full rank (rank JN = m), the full rank (n + n − h − h ) × A q q ˙ = 0,i = 1, 2, (6) p 1 2 1 2 i p,i p,i (n + n − h − h − m) matrix N (q), orthogonal to 1 2 1 2 c where A (q ) is the (h × p ) Pfaffian full rank matrix and i p,i i i JN , can be determined in such a way that the following h is the number of independent holonomic and nonholo- relation is fulfilled: nomic constraints. JN N = 0. (12) p c 2.2 Dynamics Finally, multiplying dependency (11)by N , the dynamic model of the system is reduced to the form given below: The cooperation causes that manipulators and object form a T T T T T T N N Mq ¨ + N N F = N N Bu. (13) c p c p c p closed kinematic chain, and consequently reduces the degree of freedom of the system. Thus, owing to the redundant Cooperative robots moving the common rigid object form number of the actuators, this system can be considered as a an over-actuated dynamic system, thus there is no unique rigid over-actuated multi-body system. After disconnection solution of the Eq. 13 for u. Assuming the full rank of matrix T T of this multi-body system at the origin of the object frame N N B , the inverse dynamic model of the system may be c p [16], the dynamic equations for i-th robot are given as: determined as: M (q ) q ¨ + F (q , q ˙ ) + A (q ) λ = B u , (7) u = Mq ¨ + F, (14) i i i i i i i i i i i J Intell Robot Syst + T T + T T T T where M = B N N M, F = B N N F , B = N N B Additionally, each control should be within the limits c p c p c p and B is the Moore-Penrose pseudoinverse of matrix B . defined by the physical capabilities of the actuator, then the following limitations for controls u and u for the first and 1 2 the second robot, should be satisfied: 3 Problem Formulation u ≤ u ≤ u ,j = 1,...,n − h , min,1,j 1,j max,1,j 1 1 1 1 1 1 This paper considers the cooperative task of two mobile mani- u ≤ u ≤ u ,j = 1,...,n − h , (19) min,2,j 2,j max,2,j 2 2 2 2 2 2 pulators moving the common rigid object to the specified location in the workspace. Using the vector p describing where u is the j -th control (torque or force) of the first 1,j 1 the location of the object (1), this task can be formulated as robot, u is the j -th control (torque or force) of the sec- 2,j 2 displacement of the object from the given initial location p ond robot, and u , u , u , u are min,1,j max,1,j min,2,j max,2,j 1 1 2 2 to the final location p . the lower and upper limits on u and u , respectively. 1,j 2,j 1 2 Additionally, the motion of the mobile robots have to To solve the cooperative task satisfying kinematic con- take into account mechanical limits, boundary constraints straints (4)–(6) and limitations resulting from the construc- resulting from the task and collision-free conditions. At tion of the robots and the nature of the task (15)–(19), the the initial moment of motion the manipulators are (by control system shown schematically in Fig. 2 is proposed. assumption) in the collision-free configuration for which the As can be seen, the problem will be solved by a real-time virtual end-effectors are in the initial location p with zero trajectory generator working in the closed loop. The input velocity: signal of the generator is the error vector E calculated based on the desired location of the object and its current p = f (q (0)) , q ˙ (0) = 0,i = 1, 2. (15) 0 i i i location obtained from a forward kinematic model (FKM) At the final moment of motion the both virtual end-effec- given by dependency (3). The other components of vector E tors have to reach the final location p with zero velocity: result from holonomic and nonholonomic platform con- straints (PC), described by Eq. 6, and additional constrains p = f (q (T )) , q ˙ (T ) = 0,i = 1, 2, (16) f i i i (AC) introduced by dependencies (4), (5), (17), (18). The controls are determined from the dynamic model of the sys- where T stands for an unknown time of performing the task. tem (model) given by dependency (14). The main advantage Conditions resulting from the mechanical limits and of the proposed method is that, in contrast to other solutions, constraints connected with the potential collisions between it generates trajectories ensuring fulfillment of both the state cooperating robots or between robots and object carried by and control constraints. them can be written as a set of inequalities: In the next sections two trajectory generators are considered. The first one is based on the leader-follower conception ψ q t ≥ 0,k = 1, ...,l ,i = 1, 2, (17) ( ( )) k,i i i [19]. In this approach, the leader determines the motion and the follower assists the leader supporting the object. In where ψ is the scalar function which involves the fulfill- k,i the second one, the centralized approach is presented and ment of the constraints imposed by the i-th robot mecha- the cooperating robots are taken into account together and nical limits and collision avoidance conditions, l stands for treated as the one system. The solutions are based on the the total number of constraints. previous author’s work on the trajectory generation for a sin- In practice, the configuration of mobile manipulator gle mobile manipulator [21] and use of the penalty function joints should be far away from singular configurations. The approach and a redundancy resolution at the acceleration distance from singularities can be defined based on various level. The advantages and disadvantages of both solutions manipulability measures. The next sections will discuss are also discussed. 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: def I = 1/ σ + w(· ) , (18) ( ) where σ is the small positive coefficient and w (· ) denotes the manipulability measure (explicit forms are given in the following sections). Fig. 2 The idea of control system for cooperating robots J Intell Robot Syst 4 Leader-Follower Approach 4.2 Trajectory Generation for the Leader In this solution cooperating mobile manipulators are The main task of the leader is to move its virtual end- considered as two individual systems called the leader and effector to the final location p in such a way as not to the follower. The main task of the first mobile manipulator violate constraints (6). Additionally, the trajectory of the (leader) is to move its virtual end-effector from the initial robot has to satisfy inequality conditions (17) and reach the location p to the final location p . The second robot minimum value of the performance index (18) in each time 0 f (follower) has to follow the leader, tracking the trajectory instant. To solve this problem, an approximate implementa- determined by its virtual end-effector. During this motion tion of inequality constraints (17) is accepted in this work. both robots should allow for constraints described by The approach is to use penalty functions which cause the dependencies (4)–(19) and minimize performance indexes inequality constraints to be satisfied, but the performance (18). index (18) to be somewhat increased. In this case the Section 4.1 presents the manipulability measure for the instantaneous performance index (18) can be expressed as: single robot. In Sections 4.2 and 4.3 the methods of trajec- def tory generation for the leader and the follower satisfying I (q ) = I + κ ψ , (23) 1 1 1 i,1 i,1 constraints (4)–(17) are considered. Trajectory scaling tech- i=0 nique leading to fulfillment of control constraints (19)is where κ is the penalty function which associates a penalty i,1 presented in Section 4.4. with a violation of a leader constraint, see Numerical example section for an exemplary form of this function. 4.1 Manipulability Measure In order to find a trajectory of the leader minimizing the performance index (23), first let us consider the problem of In order to avoid singular configurations the manipulabil- finding an optimal robot configuration q (T ) at the final ity of mobile manipulators moving the common object has location p : to be taken into account. In the solution presented in this section, the manipulability measure is considered for each f (q ) − p = 0, 1 1 f subsystem independently, and it is based on the manip- A q ˙ = 0, (24) 1 1 ulability ellipsoid introduced by Yoshikawa [31]. In this min I (q ) . 1 1 approach the relationship between unit norm generalized velocities of the robot and its virtual end-effector velocity Necessary condition to obtain the minimum of the per- in the workspace is determined. The unit norm velocities in formance index I (q ) is that the differential of I at q (T ) 1 1 1 1 the configuration space fulfill the following dependency: equals zero: q ˙ q ˙ = 1,i = 1, 2, (20) I (q ) ,δq = 0 (25) q 1 1 Using the Moore-Penrose pseudoinverse of Jacobian matrix where ·  means the dot product of the vectors, I (q) = J , velocities in the workspace can be determined from Eq. 5 ∂I /∂q and δq is the variation of q . 1 1 1 1 as q ˙ = J v, therefore (20) can be rewritten as: i Moreover, at the configuration q (T ) equality con- straints given by the first and second dependency from 1 =˙ q q ˙ Eq. 24 have to be fulfilled. After differentiating the first of + + = (J v) J v i i them, they can be expressed in a compact form as: T + T + = v (J ) J v i i q δq = 0, (26) J ( ) 1 1 1 T T T −1 T T T −1 = v (J (J J ) ) J (J J ) v i i i i i i T T T T −1 T T T −1 where J (q ) = J A is the ((m + h ) × n ) non- 1 1 1 1 = v ((J J ) ) J J (J J ) v 1 1 i i i i i i singular matrix (nonsingularity is forced by minimization T T −1 T = v ((J J ) ) v procedure). T T T −1 = v ((J J ) ) v i Since J is the full rank matrix, it is possible to rewritten T T −1 = v (J J ) v. (21) the above dependency as: Thus, a unit sphere from the configuration space is mapped T T J J δq δq = 0, (27) R,1 F,1 R,1 F,1 into an ellipsoid in the workspace. According to Yoshikawa, the manipulability of the robot can be measured as propor- where J is the square matrix constructed from (m + h ) R,1 1 tional to the volume of this ellipsoid as: linear independent columns of J , J is the matrix 1 F,1 obtained by excluding J from J , δq is the (m + h ) R,1 1 R,1 1 w (q ) = det (J J ), i = 1, 2. (22) i i dimensional vector corresponding to the reduced matrix i J Intell Robot Syst J , obtained by choosing elements from δq related to Hence, the second order differential equation with respect R,1 1 columns of the matrix J and δq is the vector obtained to q is needed. Equation 32 is a system of homogeneous R,1 F,1 1 by excluding δq from δq . differential equations with constant coefficients. In order to R,1 1 Multiplying the left side of Eq. 27, the related vector solve it and find the trajectory of the mobile manipulator δq can be expressed in terms of free vector δq as: 2n − h consistent dependencies should be given. These R,1 F,1 1 1 dependencies are obtained from E for t = 0takinginto −1 1 δq =−J J δq . (28) R,1 F,1 F,1 R,1 account conditions (15), especially zero initial velocity. Then, taking into account dependency (28), necessary As it has been shown in [21], the proposed form of condition (25) may be rewritten as: differential equation (32) ensures that its solution is asymp- totically stable which implies fulfillment of the conditions −1 T I − (J J ) I ,δq = 0, (29) F,1 F,1 R,1 F,1 R,1 (16) for the leader. What is more, if corresponding elements I I where I is the (m + h ) dimensional vector correspond- R,1 1 of the main diagonals of matrices and satisfy V,1 L,1 ingtothevector δq , obtained by choosing elements from R,1 I I condition λ > 2 λ , the solution is also a strictly V,1 L,1 I related to components of the vector δq and I is the q R,1 F,1 monotonic function, thus for the initial nonsingular config- vector obtained by excluding I from I . R,1 q uration q (0) fulfilling mechanical and collision avoidance The consequence of the free vector δq in Eq. 29 is the F,1 constraints (17) leader motion is free of singularities and following equation (called transversality condition): fulfills constraints (17) during the movement to the final −1 T I − (J J ) I = 0. (30) F,1 F,1 R,1 location p . R,1 f In order to obtain trajectory of the leader, Eq. 32 can be Equation 30 introduces (n − m − h ) dependencies 1 1 rewritten in the expanded form: which in combination with the equality conditions from Eq. 24 allow to obtain an optimal configuration for a given I I I I I I ˙ ˙ E q ¨ + E q ˙ + E + E 1 1 final location. Then, in order to find a trajectory of the q,1 q,1 V,1 1 L,1 1 = 0, (33) II II II II leader, mapping E q , q ˙ , which may be interpreted as a ( ) E q ¨ + E q ˙ + E 1 1 1 1 1 q, ˙ 1 q,1 L,1 1 measure of the error between a current configuration q (t ) and unknown final configuration q (T ), is introduced as I I I I where E = ∂E /∂q , E = dE /dt, q,1 1 q,1 q,1 follows: II II II II E = ∂E /∂q ˙ , E = ∂E /∂q . 1 1 ⎡ ⎤ q, ˙ 1 1 q,1 1 f (q ) − p 1 1 f Then, after simple transformations, Eq. 33 can be written def −1 ⎣ ⎦ E = I − (J J ) I . (31) 1 F,1 F,1 R,1 as: R,1 A q ˙ 1 1 I I I I I ˙ ˙ E q ˙ + E + E q,1 V,1 1 L,1 1 The m-first components of E are responsible for reach- E q ¨ =− , (34) 1 1 II II II E q ˙ + E ing the given final location p ,the h -last items ensure 1 f 1 q,1 L,1 1 fulfillment of constraints (6). The remaining n − m − h 1 1 dependencies are derived above from the necessary condi- I II T T where E = (E ) (E ) . q,1 q, ˙ 1 tion for the minimum of the performance index (23), thus Finally, the trajectory of the leader is given by: they are responsible for the fulfillment of constraints (17)as well as minimizing performance index (18). I I I I I ˙ ˙ E q ˙ + E + E Applying the error measure (31), the dependency speci- 1 −1 q,1 V,1 1 L,1 1 q ¨ =−E . (35) 1 II II II fying the trajectory of the leader to the final location p is f E q ˙ + E q,1 L,1 1 postulated as the following system of differential equations: 4.3 Trajectory Generation for the Follower I I I I I ¨ ˙ E + E + E 1 V,1 1 L,1 1 = 0, (32) II II II E + E 1 L,1 1 The task of the second robot is to follow the trajectory of the leader virtual end-effector d (t ). This trajectory can be where E is constructed from the first two components of II I I determined from the forward kinematic model of the leader mapping E , E is the third component of E , , 1 1 1 V,1 L,1 (4), as follows: are the ((n − h ) × (n − h )) diagonal matrices with 1 1 1 1 positive coefficients ensuring the stability of the equilibrium II d (t ) = f (q (t )) . (36) of the first equation and is the (h × h ) diagonal 1 1 1 1 L,1 matrix with positive coefficients ensuring the stability of the equilibrium of the second equation. Just as the leader the follower, during its motion, should The proposed form of dependency (32) results from the also keep inequality constraints (17) and avoid singular con- necessity to determine trajectory at the acceleration level. figurations. In this case, the same approach as for the leader J Intell Robot Syst is employed and the extended performance index I (q ) is Finally, the trajectory of the follower takes the following 2 2 defined in the same way as I (q ), as follows: form: 1 1 I I I I I ˙ ¨ ˙ E q ˙ − d + E + E 2 −1 q,2 V,2 2 L,2 2 q ¨ =−E (42) def 2 II II II I (q ) = I + κ ψ . (37) E q ˙ + E 2 2 2 i,2 i,2 q,2 L,2 2 i=0 where d = d 0 , 0 is the (n − m − h ) elemental 2 2 where κ is the penalty function which associates a penalty i,2 I II I II vector, E , E , E , E , E are defined in similar way q,2 q, ˙ 2 q,2 q,2 with a violation of the follower constraint, see Numerical as for the trajectory of the leader (33). example section for an exemplary form of this function. The trajectory q (t ) is the result of solving a system of In the case of finding trajectory for the follower, the prob- differential equations whose form ensures that its solution is lem of determining an optimal robot configuration q (t ) asymptotically stable for positive definite diagonal matrices at the given time instant t should be considered: I I II , and . This property combined with the V,2 L,2 L,2 fulfillment of the condition (16) for the leader implies that f (q ) − d (t ) = 0, 2 2 the follower reaches the final location p with zero velocity, A q ˙ = 0, (38) 2 2 thus the condition (16) is also fulfilled for the follower. I I min I (q ) . 2 2 Additionally, for appropriately chosen matrices , q V,2 L,2 the solution is also a strictly monotonic function, thus for the Similarly as in previous subsection, using necessary con- initial nonsingular configuration q (0) fulfilling constraints dition of performance index (37), transversality condition (4), (5)and(17), the follower motion is free of singularities for the follower can be derived as: and fulfills constraints (4), (5), (17) during its movement to the final location p . −1 I − (J J ) I = 0, (39) F,2 F,2 R,2 R,2 4.4 Control Constraints where I , I , J , J are defined in similar way as R,2 F,2 R,2 F,2 I , I , J , J . R,1 F,1 R,1 F,1 To satisfy control constraints (19) the development of the The counterpart of the error measure E (31)for the methods presented in [22] for stationary cooperating robots follower is the mapping E (q , q ˙ ) defined as follows: 2 2 2 andin[21] for single mobile manipulator is proposed. ⎡ ⎤ f (q ) − d (t ) Denoting: 2 2 def −1 ⎣ ⎦ E = I − (J J ) I (40) 2 F,2 F,2 R,2 T R,2 I T I T = diag( ) ,diag( ) A q ˙ V,1 L,1 2 2 and introducing the following substitutions: In this case the m-first components of the above I I dependency result from the grasp constraint of the two diag(E ), diag(E ) −1 1 1 A =−E , cooperating robots (4) and describe an error between a current follower virtual end-effector location and trajectory d (t ). The next n − m − h dependencies are obtained from E q ˙ 2 2 1 −1 q,1 b =−E , II II II the necessary condition for the minimum of the performance E q ˙ + E q,1 L,1 1 index (37) and they ensure the fulfillment of constraints trajectory of the leader (35) can be written as linear with (17) as well as minimizing performance index (18)for the respect to : follower. The h -last items are responsible for fulfillment of constraints (6) of the second robot. q ¨ = A + b . (43) 1 1 1 1 To find the trajectory of the follower the same approach (m) (m) (m) I I I ˙ ¨ Denoting by E , E and E the vectors as in the previous subsection is used. For this purpose, the 1 1 1 I I I ˙ ¨ built by the m-first coordinates of the E , E and E , differential equation for mapping E , defined in accordance 1 1 1 respectively and taking into account (36), it is easy to show to Eq. 32, is introduced: from Eq. 31 that: I I I I I ¨ ˙ E + E + E (m) 2 V,2 2 L,2 2 = 0, (41) d = E + p , II II II f E + E 2 L,2 2 (m) ˙ I (44) d = E , (m) I II ¨ I where E , E are constructed from the mapping E in sim- 2 d = E . 2 2 I I ilar way as for the trajectory of the leader (32), , , V,2 L,2 The m-first dependencies from Eq. 32 can be rewritten as: II are the diagonal matrices with positive coefficients L,2 ¨ I (m) ˙ I (m) ensuring the stability of the equilibrium of the Eq. 41. d + ( ) d + ( ) (d − p ) = 0, (45) V,1 L,1 J Intell Robot Syst (m) (m) I I is considered in Section 5.1. The centralized trajectory where , are the (m × m) submatrices V,1 L,1 generation method is presented in Section 5.2. Section 5.3 I I of , , respectively. V,1 L,1 shows application of the trajectory scaling technique to Taking into account that d = d 0 , dependency (45) fulfillment of the control constraints (19). can be written in the expanded form as: T 5.1 Manipulability Measure I I T ¨ ˙ d + d + d − p 0 = 0. (46) V,1 L,1 f This section considers the total manipulability measure of Introducing the matrix A : the system consisting of two mobile robots moving the common rigid object. Based on the manipulability ellipsoid A =− diag(d), diag d − p 0 , method for two cooperative robots [5], since there are no relative motions between the virtual end-effectors of the it is easy to show that the trajectory of the leader virtual robots, kinematic constraints (5) are written in a matrix form end-effector is linear with respect to : as: d = A . (47) 0 1 ϑ = Jq, ˙ (51) Finally, inserting (47) into the trajectory of the follower (42) and introducing substitutions: T T where ϑ = v ,v is the combined vector of the end- effectors velocities and J = blockdiag (J ,J ) is the −1 0 1 2 A = E , 0 block-diagonal matrix combined from the Jacobian matrices of both robots. I I I I I ˙ ˙ E q ˙ + E + E q,2 V,2 2 L,2 2 −1 Taking into account (51), the unit norm velocities in the b =−E , 2 II II II E q ˙ + E configuration space: q,2 L,2 2 the trajectory of the follower (42) can be also written as q ˙ q ˙ = 1, (52) linear with respect to : are transformed into an ellipsoid in the workspace as q ¨ = A + b . (48) 2 2 1 2 follows: Substituting (43)and (48) into the dynamic model (14), 1 =˙ q q ˙ controls of cooperating robots can be determined as: + T + = (J ϑ) J ϑ ˇ ˇ u = M + F, (49) T + T + T T = ϑ (J ) J ϑ T T T T ˇ ˇ where M = M A A and F = M b b + F. 1 2 1 2 T T T −1 T T T −1 = ϑ (J (JJ ) ) J (JJ ) ϑ As it is seen, controls of both cooperating robots are T T −1 T T T −1 linear with respect to gain coefficients of the leader .In = ϑ ((JJ ) ) JJ (JJ ) ϑ T T −1 T order to find controls fulfilling constraints resulting from = ϑ ((JJ ) ) ϑ limitations of mobile robots actuators, dependency (49)is T T T −1 = ϑ ((JJ ) ) ϑ substituted into conditions (19) and the following system of T T −1 = ϑ (JJ ) ϑ. (53) inequalities is obtained: ˇ ˇ The dependency (53) can be easily transformed to show u ≤ M + F ≤ u . (50) min 1 max relation with the Jacobian matrices of both robots: The dependency (50) introduces (n − h ) + (n − h ) 1 1 2 2 −1 inequalities, thus choice of the leader such that (n − h ) ≥ T T 1 1 1 = ϑ JJ ϑ (n − h ) results in dim( ) = 2 (n − h ), hence it is 2 2 1 1 1 −1 possible to determine ensuring fulfillment of constraints J 0 J 0 v 1 1 T T = v v (19). An alternative approach based on scaling of the 0 J 0 J v trajectory using certain heuristics can be found in [20]. −1 J J 0 v T T = v v −1 0 J J 5 Centralized Approach T T −1 T −1 = v (J J ) + (J J ) v, (54) 1 2 1 2 In this solution cooperating mobile manipulators are therefore the manipulability of cooperating robots can be considered as one centralized system, and therefore both measured as: robots are treated equally and none of them performs the T T −1 −1 −1 w(q) = det (((J J ) + (J J ) ) ). (55) 1 2 role of the leader. The manipulability measure of this system 1 2 J Intell Robot Syst 5.2 Trajectory Generation Then, taking into account the dependency (61), necessary condition (58) may be written as follows: In order to find, in a centralized way, the trajectory of both −1 T I − (J J ) I ,δq = 0, (62) F F R F robots fulfilling inequality constraints (17) and avoiding where I is the (2m + h + h ) dimensional vector corre- singular configurations, the extended performance index R 1 2 sponding to the vector δq , obtained by choosing elements I (q) is defined in the similar way as for individual robots: R from I related to components of the vector δq and q R l l 1 2 def I is the vector obtained by excluding I from I . F R q I (q) = I + κ ψ + κ ψ . (56) i,1 i,1 i,2 i,2 The consequence of the free vector δq in Eq. 62 is the i=0 i=0 following equation: where κ , κ are the penalty functions which associates i,1 i,2 −1 a penalty with a violation of the first and the second robot I − (J J ) I = 0. (63) F F R constraint, see Numerical example section for an exemplary The total error measure E (q, q ˙ ) of whole robot system is form of this function. determined analogously to error measures introduced earlier In the case of two cooperative robots considered as in Section 4: the one system, the problem of finding an optimal robots ⎡ ⎤ f q − p ( ) 1 1 f configuration q (T ) at the final location p should be ⎢ ⎥ f (q ) − f (q ) 1 1 2 2 considered: ⎢ ⎥ def −1 ⎢ T ⎥ E = I − (J J ) I . (64) F F R ⎢ R ⎥ f (q ) − p = 0, 1 1 f ⎣ ⎦ A q ˙ 1 1 f (q ) − f (q ) = 0, 1 1 2 2 A q ˙ 2 2 A q ˙ = 0, 1 1 In the similar way as in Section 4, using the error measure A q ˙ = 0, 2 2 (64), the dependency specifying the trajectory of the whole system of cooperating robots is proposed as a following min I (q) . (57) system of differential equations: Similarly as in previous section, transversality condition I I I I I ¨ ˙ E + E + E V L for the cooperating robots can be derived from the necessary = 0, (65) II II II E + E condition for minimum of the performance index I (q): I (q) ,δq= 0, (58) where E is constructed from the first three components of II mapping E, E is formed from the remaining components where I (q) = ∂I /∂q and δq is the variation of q. I I of E, , are the (n + n − h − h ) square diagonal 1 2 1 2 V L At the configuration q (T ) equality constraints from matrices with positive coefficients ensuring the stability Eq. 57 have to be fulfilled. After differentiating the first two II of the equilibrium of the first equation and is the constraints, they can be expressed in a compact form as: h + h square diagonal matrix with positive coefficients ( ) 1 2 J (q) δq = 0, (59) ensuring the stability of the equilibrium of the second T T T equation. where J = [J 0] , [J − J ] ,blockdiag(A ,A ) 1 1 2 1 2 Similarly as in previous section, the trajectory of coop- is the nonsingular matrix (nonsingularity is forced by erating robots can be determined by simple transformation minimization procedure). from Eq. 65 as: Since J is the full rank matrix, it is possible to written the I I I I I above dependency as: ˙ ˙ E q ˙ + E + E −1 V L q ¨ =−E (66) II II II E q ˙ + E T T q L [J J ] δq δq = 0, (60) R F R F I T II T I I I where J is the square matrix constructed from ˙ R where E = (E ) (E ) , E = ∂E /∂q, E = q q ˙ q q (2m + h + h ) linear independent columns of J, J is 1 2 F I II II II II dE /dt, E = ∂E /∂q ˙, E = ∂E /∂q. q q q ˙ the matrix obtained by excluding J from J, δq is the R R As in the leader-follower approach, suitable choice of the (2m + h + h ) dimensional vector corresponding to the 1 2 I I II gain coefficients , and leads to solution which V L L reduced matrix J , obtained by choosing elements from δq is asymptotically stable and strictly monotone. These prop- related to columns of the matrix J ,and δq is the vector R F erties imply that the robots reach the final location p with obtained by excluding δq from δq. zero velocity and their motion is free of singularities and ful- Multiplying the left side of Eq. 60, the related vector δq fills constraints (4), (5), (17) during the movement if initial can be expressed in terms of free vector δq as: configurations are nonsingular and satisfy constraints (4), −1 δq =−J J δq . (61) (5)and(17). R F F R J Intell Robot Syst 5.3 Control Constraints Fig. 2, the input signal of the trajectory generator is the error measure determined based on the current state of the system. To satisfy control constraints (19), trajectory (66) is scaled The output signals q, q ˙, q ¨ are obtained by solving the ini- in similar was as in leader-follower approach. Denoting: tial value problem given by dependencies (32)and(41)for the leader-follower approach or Eq. 65 for the centralized I T I T = diag( ) ,diag( ) approach. In each case, initial conditions are determined V L by using a suitable error measure for t = 0 allowing for and introducing the following substitutions: conditions (15). I I The solution scheme for both proposed approaches is diag(E ), diag(E ) −1 A =−E , 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 E q ˙ −1 q b =−E , II II II paragraphs. E q ˙ + E q L trajectory of the robots (66) can be written as linear with 6.1 Computational Burden respect to : Regardless of the used approach, finding the trajectory of q ¨ = A + b. (67) cooperating robots requires solving the initial value problem Similarly as in previous section, substituting (67)into givenbysystemof (n + n ) secondary differential equa- 1 2 dynamic model (14), controls of cooperating robots can be tions. The differences in a computational effort are related to determined as: the cost of determining the components of these equations. Let us compare the computational effort needed to deter- ˆ ˆ u = M + F, (68) mine the error measures in the case of the leader-follower ˆ ˆ where M = MA and F = Mb + F. (31), (40) and centralized approach (64). All components As it is seen, in contrast to leader-follower approach, in from E and E are included in the error measure E,how- 1 2 this case controls are linearly dependent on gain coefficients ever, the leader-follower approach requires computing two of both cooperating robots. In order to find controls ful- separate transversality conditions - one for the leader (30), filling constraints resulting from limitations of mobile robots and one for the follower (39) and in the centralized approach actuators, dependency (68) is substituted into conditions the single transversality condition (63) for both cooperating (19) and the following system of inequalities is obtained: robots is determined. In the first approach, robots are con- sidered as two individual systems, thus J , J are matrices 1 2 ˆ ˆ u ≤ M + F ≤ u . (69) min max of dimensions ((m + h ) × n ) and ((m + h ) × n ) and 1 1 2 2 vectors I , I have n and n elements, respectively. In q q 1 2 The dependency (69) introduces (n − h ) + (n − h ) 1 1 2 2 1 2 the second method, robots are treated as one centralized inequalities whereas dim( ) = 2 (n − h + n − h ), 1 1 2 2 system, in this case J is ((2m + h + h ) × (n + n )) thus it is possible to determine ensuring fulfillment of 1 2 1 2 matrix and I is (n + n ) elemental vector. The greater constraints (19). q 1 2 number of elements in the matrix J, in the case of the cen- tralized approach, leads to more operations needed to deter- mine the transversality condition. In particular, in the leader- 6 Discussion follower approach it is necessary to invert two matrices with dimensions m + h × m + h and m + h × m + h ( ) ( ) ( ) ( ) It seems that the both presented approaches are computa- 1 1 2 2 and in the centralized approach one (2m + h + h ) × 1 2 tionally effective and allow to find solutions satisfying all (2m + h + h ) matrix has to be inverted. the imposed constraints which is confirmed by the results of 1 2 The higher calculation costs associated with the deter- the simulations presented in Numerical example section. How- mination of the error measure E result in higher costs of ever, each of them has some advantages and disadvantages. I I I ˙ ˙ computing E , E and E . Moreover, in the centralized The methods described in this paper are based on the sim- q q approach, derivatives are calculated with respect to com- ilar approach in which trajectories of the robots are derived T T T bined vector q =[q ,q ] . It causes dimensions of matri- from differential equations using suitable defined error 1 2 I II ces E , E , and in a consequence E is ((n + n )× (n + 1 2 1 measures. The both proposed trajectory generators take state q q ˙ dependent constraints into account, maximize the manip- n )) matrix and E is ((2m + h + h ) × (n + n )) matrix. 2 1 2 1 2 ulability measure, and using suitable scaling techniques In the case of the leader-follower approach, derivatives are determine trajectories satisfying control constrains. Accord- calculated with respect to q and q , thus dimensions of 1 2 ing to the idea of the proposed control system shown in matrices E , E are equal to (n ×n ) and (n ×n ), whereas 1 2 1 1 2 2 J Intell Robot Syst I I ˙ ˙ dimensions of E , E are equal to ((m + h ) × n ) and proposed approaches can be employed directly in generat- 1 1 q,1 q,2 ((m + h ) × n ), respectively. Determining the trajectories ing the trajectory for multiple cooperating robots. In the 2 2 of robots requires inverting the matrices E and E and cal- case of the leader-follower approach, the robot with the 1 2 I I ˙ ˙ culation of products E q ˙ , E q ˙ in the leader-follower number of the degree of freedom not less than the num- 1 2 q,1 q,2 approach and the inversion of matrix E and calculation of ber of degrees of freedom of other robots is chosen as a product E q ˙ in the centralized approach. The larger matri- leader, the other robots become followers. The trajectory of ces size in the second case results in higher computational the leader is derived from dependency (35) and trajectories burden and it can lead to larger computation time. of the followers according to dependency (42). In the case of the centralized approach, the error measure (64)has to 6.2 Secondary Cost Functions be modified. Taking another robot into account requires the addition of two components, one related to the existence of According to the above analysis, the centralized approach a closed kinematic chain and the other resulting from the requires more computational burden but it allows to use constraints imposed on the robot platform motion. Simi- secondary cost functions taking into account properties larly, equality constraints in Eq. 57 should be completed in of the whole system such as a measure describing total order to derive the transversality condition for the whole manipulability of two cooperative robots. In the case of the system. Consideration about the computational burden and leader-follower approach, each of the robots is considered secondary cost functions for the case of two cooperating as an individual subsystem, and therefore secondary robots also applies the case of multiple robot system. performance indexes can be functions of state variables of The above analysis presents the general idea of the appli- the single robot only. Such approach allows to maximize cation of the method in the case of trajectory generation for the manipulability measure of each individual mechanism, multiple robots. However, these issues are out of the scope however does not provide optimization of the manipulability of this paper, they require a further elaboration and will be of the whole system, which is confirmed by the simulation the subject of further research. 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 7 Numerical Example of the whole mechanism is possible. Summarizing, where a sufficient computing power is available and it is important In the numerical example two mobile manipulators, shown to optimize secondary cost functions describing properties in Fig. 3, each composed of the nonholonomic differentially driven platform (2,0) class and planar 3-DOF holonomic of the whole system, the application of the centralized approach should be considered. manipulator are considered. The task of the robots is to move the uniform, rigid beam to the final location in the 6.3 Trajectory Generation for Multiple Robots workspace. Two cases of performing above task are consi- dered. In the first one control constraints are not taken The scope of the work is limited to the case of generating into account. In the second case, the methods described in the trajectory for two robots, however it seems that both Sections 4.4 and 5.3 are used to satisfy control constraints Fig. 3 Schematic diagram of two cooperative robots J Intell Robot Syst (19). In each example, two simulations for: leader-follower to limitations resulting from rolling and sliding constrains and centralized approach are carried out. of each platform wheel. For differentially driven platforms discussed in this work, each of fixed wheels of the i-th 7.1 System Parameters and Constraints platform introduces one rolling constraint: ˙ ˙ x ˙ c +˙ y s − a θ = r φ , c,i θ,i c,i θ,i i i i 1,i The system of two cooperating mobile manipulators taken ˙ ˙ x ˙ c +˙ y s + a θ = r φ , c,i θ,i c,i θ,i i i i 2,i into account in simulations is shown in Fig. 3.The i-th robot is described by the vector of generalized coordinates: where r is the radius of driving wheels and a is half- i i distance between the wheels. q = x ,y ,θ ,φ ,φ ,q ,q ,q , i c,i c,i i 1,i 2,i 1,i 2,i 3,i The wheels of each platform are parallel, thus the non- sliding constraints are equivalent and they can be written where [x ,y ] is the platform center location, θ is the c,i c,i i as: ,φ are the angles of driving platform orientation, φ 1,i 2,i wheels, q , q , q are the configuration angles of the 1,i 2,i 3,i −˙ x s +˙ y c = 0. c,i θ,i s,i θ,i manipulator joints. The mobile manipulators work in X Y plane of the B B Taking above wheel constraints into account, the velocity base coordinate system {B}. The coordinate system {P } is of the platform x ˙ , y ˙ ,θ can be determined from: c,i c,i i attached to the i-th mobile platform at the midpoint of the ⎡ ⎤ r r i i ⎡ ⎤ line segment connecting the two driving-wheels. The i-th c c θ,i θ,i 2 2 x ˙ c,i ⎢ ⎥ ˙ holonomic manipulator is connected to the i-th platform at r r 1,i i i ⎣ ⎦ ⎢ ⎥ y ˙ = s s . θ,i θ,i c,i 2 2 T ⎣ ⎦ the point [l , 0, 0] {P } system. The frame {O} is attached φ 2,i 0,i i θ r r i i i to the selected point of the object moved by the cooperating 2a 2a i i robots. The object frame {O} is rotated by ϕ around O,i Finally, using the above dependency, platform constraints Z-axis of the coordinate frame {G } attached to the can be expressed in the Pfaffian form as: grasper of i-th manipulator and translated by the vector ⎡ ⎤ [x ,y , 0] . The line section connecting the origins of ⎡ ⎤ O,i O,i x ˙ c,i r r i i 100 − c − c θ,i θ,i 2 2 ⎢ ⎥ the {G } and {O} systems is a virtual stick of the i-th i y ˙ c,i ⎢ ⎥ ⎢ ⎥ r r i i ⎢ ⎥ ⎢ ⎥ robot introduced in Section 2. The transformation matrices ˙ 010 − s − s θ = 0. θ,i θ,i i 2 2 ⎢ ⎥ ⎣ ⎦ ⎣ ˙ ⎦ describing kinematics of the mobile manipulators are given r r φ i i 1,i 001 − 2a 2a i i as follows: ˙ 2,i ⎡ ⎤ c −s 0 x θ,i θ,i c,i It is assumed that both robots have same kinematic and ⎢ ⎥ s c 0 y B θ,i θ,i c,i ⎢ ⎥ dynamic parameters, they are given as (all physical values T = , i ⎣ ⎦ 00 1 0 are expressed in the SI system): 00 0 1 l = l = 0.3,l = l = 0.5,a = 0.2,r = 0.08. 0,i 3,i 1,i 2,i i i ⎡ ⎤ c −s 0 l c + l 3,i 3,i k,i k,i 0,i k=1 The masses of the mobile manipulator elements amount to: ⎢ ⎥ P ⎢ s c 0 l s ⎥ i 3,i 3,i k,i k,i k=1 T = , ⎢ ⎥ m =20,m =0.2,m =2.5,m =2,m =1, ⎣ ⎦ p,i w,i 1,i 2,i 3,i 0 010 0 001 where m is the total mass of the platform, m is the p,i w,i mass of the platform wheel and m , m , m are the ⎡ ⎤ 1,i 2,i 3,i c −s 0 x ϕ,i ϕ,i O,i masses of the manipulator links. ⎢ ⎥ s c 0 y G ϕ,i ϕ,i O,i ⎢ ⎥ The task of the mobile manipulators is to move the T = , ⎣ ⎦ 00 1 0 uniform 2 kg beam 1.69 meter long from the initial location: 00 0 1 p =[1.15, 0.5,π/2] where l , l , l are the lengths of the manipulator links, 1,i 2,i 3,i c = cos(θ ), c = cos( q ), s = sin(θ ), θ,i i k,i j,i θ,i i j =1 to the final one: s = sin( q ). k,i j,i j =1 p =[4.0, 2.0, −π/2] . During the task accomplishment, the mobile robots are subject to limitations causing by the constraints of the The location and orientation of the object frame {O} with platform motion. According to the analysis of kinematic respect to the grasper frames {G } determine parameters: properties of wheeled mobile robots presented by Campion et al. in [4], the motion of the wheeled platform is subject x = 1.69/2,y = 0,ϕ = 0,ϕ = π. 0,i 0,i O,1 O,2 J Intell Robot Syst At the initial moment of motion the robots are in the the given value e and generalized velocities of both robots non-singular, collision-free configurations: are less than the given value e , i.e.: q (0) =[0, −1, 0, 0, 0, 0,π/4,π/4] , f (q )−p <e and (˙ q <e ) and (˙ q <e ) . 1 1 1 f p 1 v 2 v In the all presented simulations, the values for e and e q (0) =[0, 2, 0, 0, 0, 0, −π/4, −π/4] , p v equal to 0.01 have been used. for which the beam is in the location p . To preserve mechanical constraints generalized coordi- 7.2 Trajectory Without Control Constraints nates of holonomic manipulators should not exceed limits: q =−q = [2π/5,π/3,π/3] . In the first case, methods described in Sections 4 and 5 are max,i min,i used to generate trajectory without taking into account con- Scalar functions involving fulfillment of k-th constraint trol constraints. The values of the gain coefficients for the connected to k-th generalized coordinate are taken as: leader-follower approach are taken as: ψ = q − q k,i k,i min,k,i I I II = I, = 2.1 , = I, for k = 1, 2, 3. L,i V,i L,i L,i ψ = q − q k+3,i max,k,i k,i and for the centralized approach they are equal to: Then, the penalty functions used in the performance indexes I I I II = I, = 2.1 , = I, (23), (37)and (56), ensuring fulfillment of the mechanical L V L L constraints are proposed as: where I is the identity matrix of the appropriate size. The final time of task execution T for the leader-follower κ ψ = ρ /ψ , k,i k,i m k,i approach is obtained as 11.31 [s]. The robots motion is where ρ is the positive coefficient determining strength of shown in Fig. 4. The leader is plotted with solid line and penalty for the mechanical constraints. the follower with dashed line. The initial, the final and In order to preserve potential collisions between robots two intermediate phases of the motion are shown. The and the object, the approach based on obstacles enlargement obtained trajectories of holonomic parts of the robots are with a simultaneous discretization of the mobile manipula- showninFig. 5. The horizontal lines represent the assumed tor,shownin[21], has been adopted. In order to apply this mechanical constraints: the dashed lines for the first joint, approach, the carried object is an obstacle for both robots the dotted lines for the second and the third joint. As can and each robot is treated as an obstacle for the second be seen the obtained trajectories do not exceed the assumed one. The object and segments of the mobile manipulators constraints. Finally, the controls of the leader and the (platforms, manipulator links) are approximated by smooth surfaces and collision avoidance conditions are written as: ψ = η x − δ, f or k > 6, k,i j d,i where η is the equation of j-th obstacle surface, x is j d,i the point from the set of points which approximates the i- th 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: ρ ψ − ε for ψ ≤ ε c k,i k,i 1 κ ψ = , k,i k,i 0 otherwise where ρ denotes the constant positive coefficient determin- c 0 ing the strength of penalty and ε is the constant positive coefficient determining the threshold value which activates −1 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 −2 −1 0 1 2 3 4 5 6 p and fulfillment of the boundary conditions (16) with a X [m] desired accuracy. It has been assumed that the algorithms stop when the distance to the desired location p is less than Fig. 4 Robots motion for the leader-follower approach Y [m] B J Intell Robot Syst q q q 1,1 2,1 3,1 u u 1,1 2,1 −1 −5 −10 0 2 4 6 8 10 12 0 2 4 6 8 10 12 t [s] t [s] q q q 1,2 2,2 3,2 −2 −1 −4 u u u 3,1 4,1 5,1 0 2 4 6 8 10 12 −6 0 2 4 6 8 10 12 t [s] t [s] Fig. 5 Trajectories of the holonomic manipulators of the leader (top) Fig. 6 Leader torques for the first case and the follower (bottom) for the first case follower (torques of the platform wheels and manipulator joints) are shown in Figs. 6 and 7, respectively. Let us note that controls are continuous functions of the time and they u u 1,2 2,2 are within ranges: T T −[2.9, 6.9, 0.5, 2.6, 3.6] ≤ u ≤[6.3, 5.4, 2.7, 0.2, 0.1] T T −[3.3, 3.4, 0.7, 0.2, 5.1] ≤ u ≤[19.8, 10.7, 2.5, 1.1, 0.0] . 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 −5 in Fig. 8. The first robot is plotted with solid line and the −10 second robot with dashed line. Similarly as for the leader- 0 2 4 6 8 10 12 follower approach, four phases of the motion are shown. In t [s] 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.Asitis seen, controls are continuous functions of the time and in this case they are within ranges: −2 T T −[3.4, 5.4, 0.7, 2.4, 3.4] ≤ u ≤[6.5, 5.8, 2.8, 0.3, 0.3] −4 T T −[3.4, 3.4, 1.4, 0.1, 4.9] ≤ u ≤[18.0, 11.6, 2.5, 1.3, 0.6] . u u u 3,2 4,2 5,2 −6 Additionally, the comparison of robots motions obtained 0 2 4 6 8 10 12 t [s] in both simulations can be seen at the animation available on-line at [1]. Fig. 7 Follower torques for the first case q , q , q [rad] q , q , q [rad] 1,2 2,2 3,2 1,1 2,1 3,1 u , u [Nm] u , u [Nm] u , u , u [Nm] u , u , u [Nm] 1,2 2,2 1,1 2,1 3,2 4,2 5,2 3,1 4,1 5,1 J Intell Robot Syst 5 20 u u 1,1 2,1 −5 −10 0 2 4 6 8 10 12 t [s] −1 −2 −2 −1 0 1 2 3 4 5 6 X [m] −4 u u u Fig. 8 Robots motion for the centralized approach (solid line - the first 3,1 4,1 5,1 −6 robot, dashed line - the second robot) 0 2 4 6 8 10 12 t [s] Fig. 10 First robot torques for the first case q q q 1,1 2,1 3,1 1 u u 1,2 2,2 −1 −5 0 2 4 6 8 10 12 −10 t [s] 0 2 4 6 8 10 12 t [s] q q q 1,2 2,2 3,2 −2 −1 −4 u u u 3,2 4,2 5,2 0 2 4 6 8 10 12 −6 t [s] 0 2 4 6 8 10 12 t [s] Fig. 9 Trajectories of the holonomic manipulators of the first (top) and the second (bottom) robot for the first case Fig. 11 Second robot torques for the first case q , q , q [rad] q , q , q [rad] Y [m] 1,2 2,2 3,2 1,1 2,1 3,1 B u , u [Nm] u , u [Nm] u , u , u [Nm] u , u , u [Nm] 1,2 2,2 1,1 2,1 3,2 4,2 5,2 3,1 4,1 5,1 J Intell Robot Syst q q q u u 1,1 2,1 3,1 1,2 2,2 −2 −1 −4 −6 0 4 8 12 16 20 24 0 4 8 12 16 20 24 t [s] t [s] 1.5 q q q 1,2 2,2 3,2 0.5 −0.5 −1 −1 u u u 3,2 4,2 5,2 0 4 8 12 16 20 24 −1.5 0 4 8 12 16 20 24 t [s] t [s] Fig. 12 Trajectories of the holonomic manipulators of the leader (top) Fig. 14 Follower torques for the second case and the follower (bottom) for the second case q q q 1,1 2,1 3,1 u u 1,1 2,1 −2 −1 −4 0 4 8 12 16 20 24 −6 t [s] 0 4 8 12 16 20 24 t [s] q q q 1.5 1,2 2,2 3,2 0.5 −0.5 −1 −1 u u u 3,1 4,1 5,1 0 4 8 12 16 20 24 −1.5 t [s] 0 4 8 12 16 20 24 t [s] Fig. 15 Trajectories of the holonomic manipulators of the first (top) Fig. 13 Leader torques for the second case and the second (bottom) robot for the second case u , u , u [Nm] q , q , q [rad] q , q , q [rad] u , u [Nm] 3,1 4,1 5,1 1,2 2,2 3,2 1,1 2,1 3,1 1,1 2,1 u , u , u [Nm] q , q , q [rad] q , q , q [rad] u , u [Nm] 1,2 2,2 3,2 1,1 2,1 3,1 3,2 4,2 5,2 1,2 2,2 J Intell Robot Syst 7.3 Trajectory with Control Constraints 6 u u 1,2 2,2 In the second case, it is assumed that limitations on controls resulting from physical abilities of the actuators are equal to: u =−u = [5.0, 5.0, 1.0, 1.0, 1.0] . 0 max,i min,i −2 As can be seen, controls determined in the previous subsection exceed above limits both for the leader-follower −4 and centralized approach. −6 As it has been shown in Sections 4.4 and 5.3, controls 0 4 8 12 16 20 24 are linearly dependent on or . Therefore, it is possible t [s] to determine the values of gain coefficients in such a 1.5 way that control constraints are satisfied. For simplicity of simulations, and are assumed to be constant and for the leader-follower approach they are equal to: 0.5 I I I = 0.18 I, = 2.1 , L,1 V,1 L,1 and for the centralized approach are taken as: I I I −0.5 = 0.19 I, = 2.1 . L V L −1 The final time T , in both simulations, is increased and u u u 3,2 4,2 5,2 it equals 23.22 [s] for the leader-follower approach and it −1.5 0 4 8 12 16 20 24 equals 23.91 [s] for the centralized approach. The deter- t [s] mined trajectories of the holonomic manipulators and robot Fig. 17 Second robot torques for the second case controls in both simulations are shown in Figs. 12, 13, 14, 15, 16 and 17. The dashed and dotted horizontal lines repre- sent the assumed mechanical and control constraint. As can 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 u u 1,1 2,1 techniques results in a reduction of controls and, as a conse- quence, 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. −2 The advantage of the centralized approach is the ability to take into account the total manipulability of the whole sys- −4 tem, which was not possible in the leader-follower method. −6 In order to compare results of both trajectory generators, the 0 4 8 12 16 20 24 t [s] 1.5 0.355 0.35 0.5 0.345 −0.5 0.34 −1 u u u 3,1 4,1 5,1 0.335 0 4 8 12 16 20 24 −1.5 t [s] 0 4 8 12 16 20 24 t [s] Fig. 18 Total manipulability of the system for the second case (leader- Fig. 16 First robot torques for the second case follower approach) u , u , u [Nm] u , u [Nm] 3,1 4,1 5,1 1,1 2,1 w u , u , u [Nm] u , u [Nm] 3,2 4,2 5,2 1,2 2,2 J Intell Robot Syst 0.355 due to the higher computation efficiency resulting in lower computational burden. The centralized approach is more computationally demanding but it allows to take into 0.35 account more complex performance indexes including state variables of both cooperating robots, thus it lets to optimize 0.345 total manipulability measure of the whole system. However, the presented work is limited to the case of generating the 0.34 trajectory for two robots, the application of the proposed approaches in generating the trajectory for multiple coop- 0.335 erating robots has been discussed and it will be the subject 0 4 8 12 16 20 24 t [s] of the further research. In the proposed approaches, the original boundary value Fig. 19 Total manipulability of the system for the second case problems have been transformed into the initial value (centralized approach) problems, then the proposed solutions are computationally efficient. The effectiveness of the presented methods are confirmed by the results of computer simulations. total manipulability measure (55) has been calculated for both the leader-follower and the centralized approach. Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http:// Changes of these indexes over time are shown in Figs. 18 creativecommons.org/licenses/by/4.0/), which permits unrestricted and 19, respectively. As it is seen in Fig. 18, optimization use, distribution, and reproduction in any medium, provided you give of the manipulability measure of the individual subsystems appropriate credit to the original author(s) and the source, provide a (leader and follower) does not lead to optimal manipulabil- link to the Creative Commons license, and indicate if changes were made. ity of the whole system. Publisher’s Note Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations. 8 Conclusion In the presented paper, the original boundary value problem References has been transformed into the initial value problem - so it seems that the proposed solution is computationally efficient. 1. Results of the simulations. http://staff.uz.zgora.pl/gpajak/rtoolbox/ In this paper two suboptimal methods of trajectory jint17i generation for mobile manipulators moving the common 2. Abou-Samah, M., Tang, C.P., Bhatt, R.M., Krovi, V.: A kinemati- rigid object to a specified location in the workspace have cally compatible framework for cooperative payload transport by non- holonomic mobile manipulators. Auton. Robot. 21(3), 227–242 been proposed. Presented approaches guarantee fulfillment (2006). https://doi.org/10.1007/s10514-005-9717-9 of constraints imposed by the closed kinematic chain as 3. Brahmi, A., Saad, M., Gauthier, G., Zhu, W.H., Ghommam, J.: well as boundary conditions resulting from the task to Adaptive control of multiple mobile manipulators transporting a be performed. Constraints connected with the existence of rigid object. Int. J. Control. Autom. Syst. 15(4), 1779–1789 (2017). https://doi.org/10.1007/s12555-015-0116-x mechanical limitations for manipulator configuration, col- 4. Campion, G., Bastin, G., DAndreaNovel, B.: Structural properties lision avoidance conditions and control constraints have and classification of kinematic and dynamic models of wheeled been considered in both methods. Minimization of instanta- mobile robots. IEEE Trans. Robot. Autom. 12(1), 47–62 (1996). neous performance indexes ensures that during the motion, https://doi.org/10.1109/70.481750 5. Chiacchio, P., Chiaverini, S., Sciavicco, L., Siciliano, B.: On the robots are far away from singular configurations. The pro- manipulability of dual cooperative robots. In: NASA Conference posed methods do not guarantee the global optimality on Space Telerobotics. Pasadena (1989) because they minimize the performance indexes in each 6. Dai, Y., Kim, Y., Wee, S., Lee, D., Lee, S.: Symmetric caging time instant, then the obtained solutions are suboptimal. formation for convex polygonal object transportation by multiple mobile robots based on fuzzy sliding mode control. ISA Trans. 60, The problem has been solved by using penalty functions 321–332 (2016). https://doi.org/10.1016/j.isatra.2015.11.017 and a redundancy resolution at the acceleration level. The 7. Desai, J., Kumar, V.: Nonholonomic motion planning for mul- resulting trajectories have been scaled in a manner to fulfill tiple mobile manipulators. In: Proc. IEEE International Confer- the constraints imposed on controls. The property of asymp- ence on Robotics and Automation (ICRA’97), pp. 3409–3414. Albuquerque (1997) totic stability of the proposed control systems implies fulfill- 8. Desai, J., Kumar, V.: Motion planning for cooperating mobile ment of all the constraints imposed. manipulators. J. Robot. Syst. 16(10), 557–579 (1999) Advantages and disadvantages of both methods have 9. Fink, J., Hsieh, M.A., Kumar, V.: Multi-robot manipulation via been discussed. The leader-follower approach is attractive caging in environments with obstacles. In: Proc. IEEE International w J Intell Robot Syst Conference on Robotics and Automation, vol. 1–9, pp. 1471+. Motion and Control (RoMoCo’04), pp. 121–126. Puszczykowo. New York. https://doi.org/10.1109/ROBOT.2008.4543409 (2008) https://doi.org/10.1109/ROMOCO.2004.240908 (2004) 10. Florin, A., Silvia, A.: Synchronizing robot motions in cooperative 23. Pajak, I.: Trajectory planning of cooperative mobile manipulators tasks. Control Eng. Appl. Inform. 13(1), 43–48 (2011) subject to control constraints. In: Proc. 11th International 11. Galicki, M.: Two-stage constrained control of mobile manipulators. Workshop on Robot Motion and Control (RoMoCo’17), pp. 123– Mech. Mach. Theory 54, 18–40 (2012). https://doi.org/10.1016/ 128 (2017) j.mechmachtheory.2012.04.001 24. Seraji, H.: A unified approach to motion control of mobile manip- 12. Galicki, M.: Real-time constrained trajectory generation of mobile ulators. Int. J. Robot. Res. 17(2), 107–118 (1998). https://doi.org/ manipulators. Robot. Auton. Syst. 78, 49–62 (2016). https://doi.org/ 10.1177/027836499801700201 10.1016/j.robot.2016.01.008 25. Sugar, T., Kumar, V.: Control of cooperating mobile manipulators. 13. Gueaieb, W., Al-Sharhan, S., Bolic, M.: Robust computationally IEEE Trans. Robot. Autom. 18(1), 94–103 (2002). https://doi.org/ efficient control of cooperative closed-chain manipulators with 10.1109/70.988979 uncertain dynamics. Automatica 43(5), 842–851 (2007). https:// 26. Tanner, H., Loizou, S., Kyriakopoulos, K.: Nonholonomic naviga- doi.org/10.1016/j.automatica.2006.10.025 tion and control of cooperating mobile manipulators. IEEE Trans. 14. Hacioglu, Y., Arslan, Y.Z., Yagiz, N.: MIMO fuzzy sliding mode Robot. Autom. 19(1), 53–64 (2003). https://doi.org/10.1109/TRA. controlled dual arm robot in load transportation. J. Franklin 2002.807549 I. 348(8), 1886–1902 (2011). https://doi.org/10.1016/j.jfranklin. 27. Trebi-Ollennu, A., Das Nayar, H., Aghazarian, H., Ganino, A., 2011.05.009 Pirjanian, P., Kennedy, B., Huntsberger, T., Schenker, P.: Mars 15. Hirata, Y., Kume, Y., Sawada, T., Wang, Z., Kosuge, K.: Handling rover pair cooperatively transporting a long payload. In: Proc. of an object by multiple mobile manipulators in coordination based on IEEE International Conference on Robotics and Automation, vol. caster-like dynamics. In: Proc. IEEE International Conference on I-IV, pp. 3136–3141 (2002) Robotics and Automation, vol. 1–5, pp. 807–812. https://doi.org/ 28. Uchiyama, M., Dauchez, P.: A symmetric hybrid position/force 10.1109/ROBOT.2004.1307248 (2004) control scheme for the coordination of two robots. In: Proc. IEEE 16. Jain, A.: Robot and multibody dynamics: analysis and algorithms. International Conference on Robotics and Automation, pp 350– In: Robot and Multibody Dynamics: Analysis and Algorithms, 356. Philadelphia (1988) pp. 3–482. Springer, New York (2011). https://doi.org/10.1007/ 29. Wang, Y., Wang, D., Zhu, S.: A new navigation function 978-1-4419-7267-5 based decentralized control of multi-vehicle systems in unknown 17. Khan, M.U., Li, S., Wang, Q., Shao, Z.: Formation control and environments. J. Intell. Robot. Syst. 87(2), 363–377 (2017). tracking for co-operative robots with non-holonomic constraints. https://doi.org/10.1007/s10846-016-0450-0 J. Intell. Robot. Syst. 82(1), 163–174 (2016). https://doi.org/10.1007/ 30. Yamada, S., Saito, J.: Adaptive action selection without explicit s10846-015-0287-y communication for multirobot box-pushing. IEEE Trans. Syst. 18. Kume, Y., Hirata, Y., Wang, Z., Kosuge, K.: Decentralized control Man Cybern. C 31(3), 398–404 (2001). https://doi.org/10.1109/ of multiple mobile manipulators handling a single object in 5326.971668 coordination. In: 2002 IEEE/RSJ International conference on 31. Yoshikawa, T.: Manipulability of robotic mechanisms. Int. J. intelligent robots and systems, vol. 1–3, Proceedings, pp. 2758– Robot. Res. 4(2), 3–9 (1985). https://doi.org/10.1177/02783649 2763 (2002) 19. Luh, J., Zheng, Y.: Constrained relations between two coordinated 32. Zhang, F., Hua, L., Fu, Y., Guo, B.: Dynamic simulation and industrial robots for motion control. Int. J. Robot. Res. 6(3), 60–70 analysis for bolt and nut mating of dual arm robot. In: 2012 IEEE (1987) International Conference on Robotics and Biomimetics (ROBIO 20. Pajak, G.: Trajectory planning for mobile manipulators subject 2012) (2012) to control constraints. In: Proc. 11th International Workshop on Robot Motion and Control (RoMoCo’17), pp. 117–122 (2017) 21. Pajak, G., Pajak, I.: Point-to-point collision-free trajectory Iwona Pajak received PhD degree in Automatics and Robotics from planning for mobile manipulators. J. Intell. Robot. Syst. 85(3-4, Poznan University of Technology, Poland in 2000. Now she works at SI), 523–538 (2017). https://doi.org/10.1007/s10846-016-0390-8 22. Pajak, G., Pajak, I., Galicki, M.: Trajectory planning of multiple University of Zielona Gora. Her current research interests include real manipulators. In: Proc. 4th International Workshop on Robot time motion planning of mobile manipulators and robots.

Journal

Journal of Intelligent & Robotic SystemsSpringer Journals

Published: Jun 5, 2018

References

You’re reading a free preview. Subscribe to read the entire article.


DeepDyve is your
personal research library

It’s your single place to instantly
discover and read the research
that matters to you.

Enjoy affordable access to
over 18 million articles from more than
15,000 peer-reviewed journals.

All for just $49/month

Explore the DeepDyve Library

Search

Query the DeepDyve database, plus search all of PubMed and Google Scholar seamlessly

Organize

Save any article or search result from DeepDyve, PubMed, and Google Scholar... all in one place.

Access

Get unlimited, online access to over 18 million full-text articles from more than 15,000 scientific journals.

Your journals are on DeepDyve

Read from thousands of the leading scholarly journals from SpringerNature, Elsevier, Wiley-Blackwell, Oxford University Press and more.

All the latest content is available, no embargo periods.

See the journals in your area

DeepDyve

Freelancer

DeepDyve

Pro

Price

FREE

$49/month
$360/year

Save searches from
Google Scholar,
PubMed

Create lists to
organize your research

Export lists, citations

Read DeepDyve articles

Abstract access only

Unlimited access to over
18 million full-text articles

Print

20 pages / month

PDF Discount

20% off