Decentralized force and motion control of multiple cooperative manipulators

Decentralized force and motion control of a heavy object with lathing tools, in a cooperative multiple manipulator system is studied in this paper. Interaction of the object with robot manipulators and the environment and interaction of manipulators with each other that produce internal forces in the object are complexities in the system dynamics of cooperative manipulation tasks. In this paper, impedance behaviour between manipulator end-effectors (EE) and the object is developed to achieve springy links that lead to impedance effect between the object and the environment. Then, by focusing on the interaction effects in the system as well as the role of imposed kinematics and force constraints, each manipulator will have its own controller for coordination tasks, environment force regulation and internal forces minimization, without force/torque sensors in the EE. The relevance of the theoretical findings is illustrated using simulation results involving three robots manipulating a common object.


Introduction
A cooperative robotic system is a proper choice to accomplish complicated and tough tasks in various fields such as automation, machining, construction, etc. Cooperative systems employ multiple robot manipulators in order to increase their ability in power and agility. Multiple manipulators which are doing a task cooperatively are more helpful although more difficult to control. Such difficulty emerges due to several factors such as nonlinear dynamics of manipulators, complex and sometimes unknown interactions between manipulators and the object, interactions between manipulators and so on.
Recently, many researches have focused on cooperative robotics for several objectives based on position and/or force control strategies [1]. Many researchers design the control algorithm of robots based on the overall dynamic model of interconnected robots which can be called centralized control architectures [2][3][4][5][6]. On the other hand, in a decentralized approach each robot manipulator will be controlled individually without need of any information about the other robots. Decentralized control algorithms are much easier to implement; however, sometimes this is more difficult to design and demonstrate their overall stability. Several approaches have been chosen in this area such as robust [7][8][9], adaptive [10][11][12][13] and [14] and fuzzy control [14,15]. Mukaiyama et al. implemented the theory of two-degrees-of-freedom controller on a cooperative robotic system in a decentralized control approach [7]. In this approach, one robot role as the leader and the others use position-based force controllers in order to hold and move an object. For Euler-Lagrange mechanical systems, Tang et al. proposed a decentralized control algorithm with a uniform ultimate stability guarantee [8]. They studied the performance of the algorithm on a robotic system. An adaptive variable structure controller with decentralized approach is suggested by Hsu and Fu for cooperative robot manipulators without the need of prior knowledge of robots [10]. Eliminating force sensors, Kawasaki et al. presented an adaptive method for cooperative robot arms [11]. In their method, the controller uses the position and velocity only, while its stability is demonstrated by the Lyapunov-like Lemma. An observer-based adaptive sliding mode control was the strategy employed by Yang et al. for cooperative manipulators [12]. In this research, a disturbance observer is used for low-passed uncertainties, while the fast changes in system characteristics are compensated by means of the adaptive sliding term. Another disturbance observer-aided adaptive control for cooperative robots is presented by Li et al, where the control system suffers from dynamic uncertainties and external disturbances [13]. The adaptive controller is designed for position and force error convergence while the effect of uncertainties which cannot be parameterized is compensated by the disturbance observer.
Using the fuzzy control with a combination of adaptive parameter estimation and disturbance observation, Li et al. designed a decentralized control algorithm for two robot manipulators which are moving an object cooperatively [15]. They modelled the contact force by the impedance approach and demonstrated the effectiveness of the controller when the object interacts with the environment. Another fuzzy adaptive approach for cooperative robotic systems is proposed by Zhou et al. for a robot finger [14]. Having several joints, the robot finger is considered as a large-scale system and the desired input signals were estimated using an adaptive fuzzy procedure. In Ref. [9], Pettiti et al. propose a decentralized control strategy for cooperative mobile robots. They designed two order of controllers for the goals of load twist tracking, parameter estimation and making the control law robust. Erhart et al. in [16] offered a closed-form solution for interaction manipulating wrenches that is based on Gauss' principle of the least constraint in an optimization problem, which states that the acceleration of a constrained system is affected regarding the acceleration of an equivalent unconstrained system in a way that in the least-squares sense, the acceleration difference be minimal.
A centralized force/position control in task space is presented by us for cooperative manipulator systems [17]. The contribution of this paper is designing a decentralized control using a physically consistent and compact model of interaction dynamics by applying Gauss' principle of the least constraint [18] to the common object and the apparent end-effector (EE) dynamics. This model comes from kinematic constraints enforced on multi-robot manipulators by grasping rigidly a common object. We use impedance control behaviour for each individual manipulator without addressing this control design, and focusing on modelling of manipulator's interaction wrenches and internal force/torques. As a result, task space decentralized control method is provided in the cooperative system without needing force/torque sensors in each individual robot's EE.

Cooperative manipulator system
Assume several manipulators manipulating a heavy peel-off machine to handle it for moving on a surface with exertion of certain forces. In Figure 1, a model of three manipulators which are holding a heavy machining tool is shown. To perform the object task, manipulators share wrenches that the object needs.
We assume that there is no relative movement between manipulators EE and the object due to rigidly grasping of the object in its contact points with each manipulator. Each manipulator EE has its own individual coordinate frame {X i } and manipulator bases are fixed with respect to world frame {W}. In Figure 2, a schematic of kinematic constraints and robot wrenches acting on the tool are depicted.

Object dynamics
Rigidity of the object lets us to derive its equations of motion by applying Lagrangian mechanics. The object dynamics regarding its centre of mass (COM) could be derived as  the gravity force vector. h o is the effective wrench acting in the COM of the object, andh o is assumed to be disturbance which comes from dynamic uncertainties and external forces, h e is the external wrench in accordance to interaction with the environment and J e is the Jacobian matrix.

Compliant behaviour
Physical contact between manipulators and the object in a cooperative system, including constraints, makes interaction effects between them. These interactions could be decomposed to motion inducing and constraint violating wrenches (external and internal, respectively). Rigid coupling in contact points with independent control strategies could easily lead to excessive internal forces resulting from small kinematic and control errors, with undesirable consequences such as reduced life span of components, damaged parts and task failure. One of the simplest solutions to add flexibility in multi-robot systems is utilizing contact points with mechanical built-in compliance such as spring-damper parts [19]. Based on the concept of impedance control as an alternative, a strategic compliant behaviour control method is proposed early in [5] and [6]. In this method, each manipulator EE behaves like a mass-damper-spring in its contact point with the object.

Manipulators dynamics
Every manipulator has its own complexity as a nonlinear system. Moreover, here we are working on a cooperative system where the interaction effects between manipulators and the object are more complicated. By achieving compliance behaviour of each manipulator, complexities will be reduced. This could be done by locally controlled techniques in task space like feedback linearization [20,21].
The desired compliant behaviour of a distinct ith manipulator may be described in task space by where x i denotes the position of EE of robot i, and h i the wrench of EE acting on the environment. The superscript d denotes desired values.h i comes from uncertainties in local feedback linearization and other disturbances. The M i , D i and K i ∈ R 6×6 are compliance parameters representing respectively the inertia, damping and stiffness, supposed to be block diago-

Constraints
As mentioned before, the object assumed to be a rigid body and the manipulators are supposed to be rigidly grasping the object and each manipulator EE has its own individual coordinate frame {X i }.

Representing every EE in object frame {O} means
Kinematic constraints could be shown as For representation of orientation, we use unit quaternion q = η T T ∈ Spin(3), where η ∈ R is the real part and ∈ R 3 is the vector part of it. The × represents cross product. As shown above, angular velocity of the object and end-effectors are equal, lead to ω o = ω i and also angular acceleration of them show another constraintω o =ω i .

Cooperative dynamics
Each manipulator will be individually commanded by compliance control law. After that, the wrenches applied by each manipulator affect the object and make the object to move and affect the environment. According to object, the total wrench seen by COM of the object could be expressed by where matrix G is called grasp matrix [22]. Note that the minus sign in (7) comes from Newton's third law by interacting the individual end-effectors with the object. By replacing h o in motion equation of the object (1) with (3) and (7) and after some mathematical manipulations (which are presented in Appendix), we have Wherein In Equations (10)- (17), S pi is the cross product matrix replacing of p oi ×, and it is well-known that S pi is a skew-symmetric matrix.

Constraint dynamic model
By rearranging (3) and (9), one has Wherein Combination of (20) with (21) yields On the other hand, acceleration constraints of (6) could be rewritten in compact form as For given (22) and (25), solution for h is given explicitly by [23] It is important to emphasize that the calculated EE wrenches of manipulators, h 1 to h N , could be found in the vector h in (29) and therefore there is no need to install wrist mounted force/torque sensors in each manipulator EE. The contribution of this modelling technique is that instead of measuring interaction wrenches at the EEs via real force/torque sensors, it gives a closed-form expression for computing them. Block diagram of the constrained dynamics complex system is shown in Figure 3.

Stability analysis
We will apply passivity theorem for assessment of stability of the cooperative system. First, we will show strict output passivity property of the cooperative system combined with the resource allocation block. After that, cooperative system's asymptotic stability will be assessed, and at the end, stability of closed loop system will be analysed. Proof: According to the cooperative system's dynamic equation (9), total kinetic and potential energy of the system could be calculated by storage function V as

Passivity of cooperative system
Time derivation of (31) yieldṡ By substituting (9) into (32), and using the fact thaṫ A dynamical system with input u and output y is output strictly passive [24, p. 236] if there is a positive definite function of the output as y T ρ(y) and a positivesemidefinite storage function V such that y T u ≥V + y T ρ(y); y T ρ(y) > 0, ∀y = 0 (35) With assumption of object interaction with passive environment, h e consists of contact force and friction force which are in the same direction and in contradiction withẋ o , meaning thatẋ T o J T e h e ≤ 0 6×1 . As a resulṫ Therefore, the output strictly passivity between inputs u 1 = h t o and u 2 =h x and the output velocity y =ẋ o is satisfied with storage function of V and ρ(ẋ o ) = Dẋ o  (9) is zero state observable and with employing strictly passive property of the system, asymptotic stability of the cooperative system implies.

Object task specification
As an example, consider a heavy machine (the object) with a tool used for peeling off the surface of a sheet. In this case, any desired specification could be decomposed into motion (location and orientation) control and, wrench (force and torque) control tasks.
When the object touches the surface, as depicted in Figure 4, desired behaviour of tool frame {T}, could be presented in {C} frame considered in constraint surface which is fixed with respect to the world frame {W}. The tool frame {T} is fixed with respect to object COM frame {O}.
We will use leading upper index to show vectors in certain coordination, such as O x which means vector expression in the frame {O}. For notation convenience, we omit the indication of vectors expressed in world (inertial) frame {W}.
Hereafter x = p T q T T denotes the position (location and orientation) and h = f T τ T T denotes the wrench (force and torque). Interaction wrench of the environment with the tools of the object, h e , as shown in (1) could be decomposed, with respect to surface, into the normal wrench h en and the tangent wrench h em h e = h en + h em (37) By considering x t as position of tool frame, the desired equations of motion for impedance controlled direction and wrench control direction are, respectively, where x d t and h d t denote the desired values for position and wrench.
It should be so defined that desired position and wrench axes are orthogonal to each other.
According to (39), target acceleration of the origin of {T} expressed in the frame {C}, Cẍt t , is calculated by solving (38) and (39), presented in a compact form of where M t , D t and K t are diagonal matrices and diagonal elements of them represent the desired mass, damping and stiffness in each direction, respectively.

System-level controller
To achieve desired motion of the object, it is needed to specify target motion and allocate specific motion for each manipulator. In cooperative manipulation tasks, kinematic and dynamic coordination strategies are commonly used. The kinematic coordination is concerned to determine the desired wrench and the dynamic coordination is used for computation of desired set points in motion control. Load distribution for dynamic coordination is a well-known problem ( Figure 5). Main contribution is due to redundancy, which has no unique solution for load distribution. Also, avoiding internal wrenches or reaching the desired internal wrenches are side effects in such problems. The main approach for allocating the load to the manipulators is the use of pseudo inverse of the grasp matrix [25]. This solution eliminates the internal wrenches and prevent the object from squeezing or stretching. We have wherein G † denotes Moore-Penrose inverse of G.
Using kinematic constraints of (6) with the object accelerationẍ t o in hand, desired motion of the endeffectors as kinematic coordination could be determined by

Decentralized control
Based on the constraint dynamic model shown above, a decentralized control system is presented here. Each manipulator will have its own processor and will perform its role in Cartesian task specification for motion and force control of the object. Because of rigidity of the connections, each manipulator is able to calculate its own position individually by its position sensor. Figure 6 shows the block diagram of the control scheme. The main inputs of Cẍd t and C h d t for desired object behaviour come from high level trajectory planning. Applying cooperative system model and no necessity to wrench (force/torque) sensor in the manipulator end-effectors, we have h e as a feedback for desired signal generator. Each manipulator will use same resource allocator algorithm and generates desired motion and wrench for all manipulators but uses just its own parameters to apply on real plant. For the manipulator i, interaction wrench h e will be applied in (40) which givë x t oi , and using it in (43), (41) and (42) gives usẍ d ji , h d ji for j = 1toN. All will be used as input to system model and x d ii , h d ii will be applied to real plant.
Manipulators will use just their own data and there is no need to communicate between them. So, appreciate to system model, it is a fully decentralized control system.
As shown previously, Hybrid Impedance Control block [26], as desired signal generator, will generate target motion acceleration of tools frame Cẍt t in the constraint frame {C}. As this block works in the constraint frame, all of its inputs should be converted from other frames to this frame bÿ After generating desired signal in constraint frame, its calculation in the world frame {W} comes from

Simulation results
The proposed control schemes have been applied to a system composed of three identical manipulators that cooperatively manipulate a heavy rigid object with machining mission of a wall in a pre-specified trajectory with some disturbances. Simulation was done using MATLAB in separated blocks that simulate object, manipulators, controllers and environment contact effects. The mass of the object is assumed to be m o = 10 kg and its overall moment of inertia according to its COM is J o = 8I 3 kgm 2 . Manipulators are simulated to work in an impedance control scheme. The impedance control parameters for all three manipulators are m i = 1.2 kg, d i = 12I 3 Ns/m and k i = 2.3I 3 N/m for the translational behaviour, and J i = 1.3I 3 kgm 2 , δ i = 10I 3 Nmrad/s and κ i = 5I 3 Nm/rad for the rotational behaviour.
To carry out more realistic simulations, we considered an uncertainty in sensor simulation data by adding a band-limited white noise to its output with noise power of 0.1. Also, another white noise with power of 0.3 was added to the object modelling. Desired motion for the object is to move towards the wall such that aligned with the wall and begin lathing it with desired force of 60N normal to the wall, and to move tangent to it in a zig-zag form simultaneously. Imagine a spectator located in the constraint wall coordination {W}, designed tools frame wrench and motion are Figure 9. Position and orientation trajectories of first manipulators.
Referenced to the desired equations of motion for impedance and wrench controlled directions, described in (38) and (39), the control parameters are chosen as m t = 1.0I 3 kg, d t = 32I 3 Ns/m and k t = 23I 3 N/m for the translational behaviour, and also J t = 1.0I 3 kgm 2 , δ t = 25I 3 Nmrad/s and κ t = 15I 3 Nm/rad for rotational behaviour to have smooth movements. An external forces simulation block simulates spring contact force in y-direction with stiffness coefficient K s = 500N/m and friction force in x-and z-directions with friction force coefficient μ k = 0.05.
To reach desired contact force, tools will move towards the y-direction. Then, as an easy algorithm, the "hybrid impedance control" block waits for almost stable force in the y-direction and after that, generates moving patterns in the x-and z-directions {C}. In Figure 7, external forces applied by the environment to the tools frame are depicted.
After an acceptable over-shoot within 2 s from the beginning, it shows stable approximate −60N force in the y-direction. The tools frame motions are plotted in Figure 8. Due to force control in y-direction, it shows movement in that direction until reaches desired force. From t = 2s, zig-zag motion begins that makes the tool frame to scan the surface of the wall while machining it. Changes in the orientation of tools frame shows acceptable smooth movement from initial values to desired values.
Although an impact between tools and the wall occurred, almost smooth force interactions between them are shown as desired. In addition, position and orientation graphs show the smoothness as designed.
Motion of three manipulators are depicted in Figures 9-11. Trajectories of position and orientation of manipulators show how they incorporate together to produce motion of the object in its desired pattern.

Conclusions
In this paper, we have presented a model based decentralized controller for force and motion control of multiple robotic manipulators that are cooperatively carrying a heavy load with a desired compliance behaviour for tool's end-effector in contact with the environment. In a decentralized controller design approach, the hybrid impedance controller was served to produce force and motion signals in constrained coordination based on desired values of tools frame. We then proposed a system-level controller block for each robot that uses a model of interaction wrenches and allocates the desired motion and wrenches to manipulator's end-effector in such a way that back propagate compliance behaviour of the tools frame to each individual robot manipulator and also minimizes overall internal forces. Also, the proposed modelling made it possible to use a single sensor installed in the tools end-effector and eliminate force\torque sensors from each individual manipulators. The simulation results of the overall closed loop system with three cooperative robots, confirm the significance of the proposed controller.

Disclosure statement
No potential conflict of interest was reported by the author(s).

Mohammad Bagher Menhaj
http://orcid.org/0000-0002-9470-5532 Using Equations (4), (5) and (6) and replacing p oi × with the cross product matrix S pi , (52) can be transformed to Finally, Now regarding Equation (7), and multiplying both sides of (54) by the nonsingular matrix I 3 0 3 S pi I 3 we obtain Doing the matrix products in (55), the following equation will be achieved Finally, according to Equation (7), h o can be written as: Noting that Equation (57) can be represented by (9) in terms defined by (10) to (17).