Model-based force/position control of cooperative manipulation systems

ABSTRACT This paper presents a centralized force/position controller for a heavy object manipulation in a multi-manipulator cooperative system. System dynamics of cooperative manipulating tasks comes from complex interaction of the object with robot manipulators and the environment. In this paper, focussing on the interaction effects in the system as well as by noting the role of imposed kinematics and force constraints, manipulator coordination and minimizing internal forces a pre-designed impedance behaviour between manipulator end effectors and the object is developed. The stability of the feedback system is then presented through passivity theorem and simulation results are finally provided with three manipulators handling the object supporting the relevance of the theoretical results.


Introduction
The system of multiple robot manipulators cooperatively handling an object is growing interest in applications such as construction, assembly tasks, rescue, forestry, material handling in industrial automation and aerial tasks. In such tasks, multiple manipulators are employed to increase the capacity of manipulated payload, dexterity and ability for heavy tasks. In this paper, we focus on tasks that come with extensive contact with the environment such as surface cleaning, grinding, lathing and deburring.
As a natural effect, benefits of using a team of cooperative manipulators come at the cost of increased complexity of coordination. Nonlinear behaviour of robot manipulator dynamics, interacted with the dynamics of a heavy object, brings more complexity. Main problematic issues are task space force/motion control schemes, cooperative coordination strategy, interaction forces between manipulators themselves and with the object, and interaction of the object with the environment, that should be analysed thoroughly to achieve desired outcomes of cooperative robot tasks.
In this paper, the subject is to design a reliable controlled cooperative system that focusses on achieving desired force/motion tasks defined for tools. With this target, we will use model-based strategies considering the uncertainties and other noises which come in contact in real world.
Research on force/motion control of cooperative manipulator systems could be found in the literature widely. In [1,2], one robot was used for position control and the others were subject to compliant force control which presented a master/slave scheme. Centralized control architecture [3][4][5] considered grasped payload and robots as a closed kinematic chain based on their dynamic models. Schneider and Cannon presented in [6] an impedance behaviour for the object motion in a multiple armed manipulator system with full inertial compensation and featured for direct specification of desired internal object forces. The authors in [7] proposed a motion control algorithm of manipulators based on the impedance control of each arm that is applicable to both the manipulation of an object and parts-mating. Task-independent controller for each robot in event-based coordination scheme was presented in [8] with position/force controller that incorporates the robot dynamics as well as the dynamics of robot joint motor. It shows that the proportional control actually plays no role in the force feedback control law; therefore, it would be very difficult to achieve both small steady state tracking error and quick transient response.
Desired impedance behaviour could be a challenging subject for robotic manipulators. Caccavale et al. in [9] presented a rotational impedance controller for singularity avoidance by choosing unit quaternion for angle/axis representation. In [10], an adaptive force control algorithm for velocity/position controlled robot arm which is in contact with surfaces of unknown linear compliance was provided. Global asymptotic convergence of force trajectory tracking errors to zero is proved when the robot is under exact or asymptotically exact inner loop velocity control. Mechanically compliant grippers when multiple robots are manipulating a payload were addressed in [11]. Utilizing grippers with built-in compliance is one of the most simple and effective solutions to avoid excessive internal forces that arise with the independent robot control. The compliant gripper consists of a rigid work piece fixed at the end of the manipulator and a flexible mechanism that is modelled as a spring system. The inclusion of flexibility can avoid large internal forces, but simultaneously it causes oscillation of the payload. In the mentioned study, one of the grippers was designed to be rigid to determine the motion of the payload. In [12], authors studied on robust position control simultaneously with a contact force regulation in redundant robot arms. A 6-DOF force/torque sensor was used to measure the interaction forces. Desired inertia and damping were used in the force control loop to improve transient performance. Mehrabi et al. in [13,14] decomposed interaction force between manipulators and the object to shaped and locked systems. They assumed that manipulators dynamics are unknown and no prior information is required regarding the object dynamics and its centre of mass.
In our study, instead of using a mechanical mechanism at each robot end effector (EE) to achieve flexibility, we will use a pre-designed compliant controlled behaviour. Actually, a desired compliance for tool in contact with environment is considered and the designed control system will back propagate this behaviour to each individual robot manipulator.

Object task specification
Consider a heavy machine (the object) with a tool used for peeling off the surface of a metal block. In this case, any desired specification could be decomposed into position (location and orientation) control and wrench (force and torque) control tasks. When the object touches the surface, as depicted in Figure 1, 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 centre-of-mass (COM) frame {O}.
We will use leading upper index to show vectors in certain coordination, such as O x which means vector expression in frame {O}. For convenience of notation, 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 presented in (1) could be decomposed, with respect to surface, into normal By considering x t as position of tool frame, 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 (3), target acceleration of the origin of {T} expressed in frame {C}, Cẍt t , is calculated by solving (2) and (3), 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.

Cooperative manipulator system
Assuming heavy peel-off machinery, one solution is using several manipulators to handle it. In this situation, several manipulators manipulate the object. Manipulators share wrenches that object needs to perform the task. We assume that each manipulator rigidly grasps the object in its contact point and there is no relative movement between manipulators EE and the object. Manipulator bases are fixed with respect to the world frame {W}. Each manipulator EE has its own individual coordinate frame {X i }.

Object dynamics
The object is assumed to be rigid and its equations of motion could be derived by applying Lagrangian mechanics. The object dynamics with respect to its COM could be derived as wherein are the object's mass and inertia, respectively, and C o (x o ,ẋ o ) incorporate the gyroscopic effects, and h g is the gravity force vector. h o is the effective wrench acting on the COM of the object andh o is disturbance which comes from external forces and dynamic uncertainties, and h e is external wrench in accordance to interaction with the environment where J e is the Jacobian matrix.

Manipulators dynamics
Every manipulator as a nonlinear system has its own complexity. Here we concentrate on a cooperative system where the interaction effects between manipulators and the object takes an important role. To reduce complexities, each manipulator is locally controlled in task space such that the desired compliance behaviour is achieved. This could be done for example by applying local feedback linearization technique [15,16]. For the ith manipulator, the compliant behaviour in task space may be described 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 inaccuracies in the 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, assumed to be block diago-

Constraints
As discussed before, the object is a rigid body and the manipulators are assumed to be rigidly grasping the object ( Figure 2). Each manipulator EE has its own individual coordinate frame {X i }.
T , due to constraints on relative displacement of EE about the object, we have O x i = const. and kinematic constraints could be shown as wherein W R O (q o ) denotes rotation matrix from frame {O} to frame {W}. 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. The × means cross product. As shown above, angular velocity of the object and end effectors are equal, meaning ω o = ω i and also angular acceleration of them show another constraintω o =ω i .

Cooperative dynamics
Recall that each manipulator individually is commanded by compliance control law. Then, the wrenches applied by each manipulator affect the object and make the object to move and affect the environment. Generally, the total wrench seen by COM of the object could be expressed by a grasp matrix G [17] Replacing h o in motion equation of the object (5) with (11) and (7) and with some mathematical manipulations (which are presented in Appendix), we have wherein In Equations (14)- (21), p oi × is replaced by the cross product matrix S pi . It is well known that S pi is a skewsymmetric matrix.

Target motion
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 setpoints in motion control.

System-level controller
Load distribution for dynamic coordination is a wellknown problem (Figure 3). Main contribution is due to redundancy, which has no unique solution for load distribution. Also avoiding or minimizing internal wrenches add more complexity in such problems. The main approach to allocate the individual manipulator load is the use of pseudo inverse of the grasp matrix. This solution guarantees no internal wrenches in order to rescue the object from squeezing or stretching. We have wherein G † denotes Moore-Penrose inverse of G.
Using kinematic constraints of (10) with the object accelerationẍ t o in hand, desired motion of the end effectors as kinematic coordination could be determined by

Centralized feedback
The proposed control system is depicted in Figure 4.
Here we assume that a wrench (force/torque) sensor at the object's tool frame gives us the wrench effects with the environment h e . The controller is assumed to work in world frame {W} and is able to calculate real-time position of the object. As shown previously, Hybrid Impedance Control (HIC) block [18] 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 by (see appendix) After generating desired signal in constraint frame, its calculation in the world frame {W} comes from

Stability analysis
We will apply passivity theorem for the assessment of stability of the system. First, we will show strict output passivity property of the cooperative system combined with the resource allocation block. After that, asymptotic stability of the cooperative system will be assessed, and at the end, stability of closed loop system will be analysed. As mentioned earlier, robot manipulators are assumed to be locally controlled such that their behaviour could be represented by Equation (7).

Passivity of cooperative system
Lemma 1: Assume that the target motion is a regulation problem meaningẋ d o = 0 6×1 , and the object is interacting with a passive environment. The system of manipulators and the object (13) is output strictly passive with respect to the inputs u 1 = h t o and u 2 =h x , and the output y =ẋ o with storage function of V in (27).
Proof: According to dynamic equation of the cooperative system (13), total kinetic and potential energy of the system could be calculated by storage function V as By substituting (13) into (28), and using the fact thaṫ Assuming thatẋ d o = 0 6×1 , we havė A dynamical system with input u and output y is said to be output strictly passive [19, p. 236] if there exists a positive-semidefinite storage function V and a positive definite function of the output as y T ρ(y) such that With assumption of object interaction with passive environment, h e consists of contact force and friction force which both of them are in the same direction, 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 .

Asymptotic stability of cooperative system without uncertainties
Theorem 1: Assume the cooperative manipulator system (13) without uncertainties, then it is asymptotically stable about x o = x d o = const.
Proof: A strictly passive dynamical system is asymptotically stable if it is zero state observable [19, Lemma 6.7]. Consider dynamical system of (13) without uncertainties, which meansh x = 0 6×1 . Supposing zero output y =ẋ o = 0 6×1 , follow immediately that So, the dynamic system (13) is zero state observable and with employing strictly passive property of the system, asymptotic stability of the cooperative system implies.

Asymptotic stability of feedback system
Theorem 2: Assume the feedback system depicted in Figure 4 without uncertainties. Then closed loop system is asymptotically stable when C x d t = const.
Proof: Assume that hybrid impedance controller block in Figure 4 has a transfer function with two poles that their real parts are negative. In this case, according to [19,Lemma 6.4], this block is strictly passive. As discussed in [19, Th. 6.3], feedback connection of two dynamical systems is asymptotically stable if one component is strictly passive and the other one is output strictly passive and zero state observable. In addition, with assumption of continuous Cẍd t and C h d t , HIC will produce continuous and boundedẍ t o . The systemlevel controller block uses (22)-(24) to produce system manipulated variablesẍ d i and h d i . As Moore-Penrose inverse of grasp matrix G has always rank 6, and with assumption that manipulator's initial configurations are not in their singular position, it is obvious that these variables will remain bounded. Now, it is straight forward to verify that the feedback system depicted in Figure 4 satisfies stability conditions and the overall system is asymptotically stable.

Simulation results
Although our analytical stability proof was for regulation problem without uncertainties, numerical studies for a tracking problem with some disturbances have been performed. 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 prespecified trajectory. Simulation was done using MAT-LAB in separated blocks that simulate object, manipulators, controller and environment effects.
The overall mass of the object assumed to be m o = 10I 3 kg and its 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.2I 3 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.
Robots grasping location expressed in the object frame In Equations (33)-(35), all values are in metre(s) and degrees. It may seem that the position values are too large but it should be considered that in this case study the machining tool is a disk with 20 cm diameter and because of the large machining forces, the tool is attached to a large holder block (as shown in Figure 1) and the cooperative robots are going to grasp the holder block. Therefore, the values in Equations (33)-(35) represent the dimensions of the machining tool holder.
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 equals to 0.2. Also, another white noise with power of 1 was added to the object modelling.
Actually the objective of cooperation system in this paper is a peeling off procedure. In this procedure, the interaction between object (which is the peeling off tool here) and the environment is not like a contact between two objects. Peeling off can be considered as a branch of machining processes (e.g. milling) and in order to model the interaction between tool and environment we should consider the forces which are exerted to the tool during the machining procedure. In Figure 5, a schematic for a peeling off process is shown.
The amount of force and torque which is exerted to the machining tool depending on a number of factors such as material specifications, cutting elements, tool rotation speed, tool moving speed and depth of cutting. Among the mentioned factors, moving speed and cutting depth are going to be controlled by robot. The other factors could be assumed constant. The function of machining force is quiet complex [20]. Therefore, for simulation, without loss of generality we assume the exerted force and torque to the tool as linear functions of moving speed and cutting depth as follows: where in (36) f e and τ e are exerted force and torque to the tool respectively, A f and A τ are force and torque coefficients which are calculated experimentally. V tool is the linear velocity of the cutting tool in the machining surface and d m is the depth of cut. The scenario for simulation is based on a peeling off procedure which is going to be done by means of the cooperative robot system. For example, suppose that we  are going to peel off a layer with 1 cm thickness from the surface of a large rectangular metal block. Figure 6 depicts a schematic of the mentioned scenario.
As shown in Figure 6, the goal surface is in the X-Z plane. Depth of cut should be 1 cm in the Y direction. Cutting tool is a surface milling one with the radius of 10 cm and it is going to move in the shown path in Figure 6. However, the control strategy for movement in the X-Y plane is a force controlled one and in the Y direction a position regulation method will be implemented. The cause of such strategy is that generally we don't have detailed information of the block properties. For we don't know the material characteristics, homogeneity, geometrical uncertainties on the surface and . . . . Therefore, if we use a position control method for tool movement in the X-Z plane, in some positions very large force may be applied to the tool which can be destructive. One solution to this phenomenon could be to reduce the desired velocity and to move conservatively in the X-Z plane which makes the machining procedure too time-consuming. Therefore, the optimum movement strategy is force controlling the end effector in the X-Z plane. As a result, the tool will move as fast as possible without any risk. So the machining scenario will be as follows: − Initially the tool will move in the -X direction until it reaches the border of the block (which is 2 m here). Simultaneously in the Y direction the tool should be regulated to 1 cm depth in the block surface. − After reaching the right-hand border, the tool will be force controlled in the -Z direction with a proper force reference (based on the tool strength, robotic system power and . . . ) until it moves 20 cm in the Z direction. − After that the direction of the force control changes to -X (in other words the reference force signal for the Z direction becomes zero and for the X direction changes to the prescribed value) until the tool reaches the left border of the block surface.
− Above sequence will be repeated until the whole surface is peeled off.
Overall simulation time was 200 s and simulation results are presented in Figures 7-14.
As mentioned earlier, in the Y direction tool should be regulated to a desired value which applies intended depth of cut. Figure 7 depicts the position regulation in the Y direction and it can be observed that after 1.5 s the tool has reached to the desired position in the Y direction without overshoot. It was mentioned that the        overall simulation time is 200 s but in Figure 7 the position diagram is zoomed for better observation. Figure 8 shows the position tracking error in the Y direction.
The resulting external forces applied by the environment to the tools frame are plotted in Figures 9-12. As described earlier, the desired force will be changed consecutively based on the position of the tool. The desired and actual forces in X are plotted in Figure 9 and the tracking error is depicted in Figure 10. The same for the Z direction can be found in Figures 11 and 12. It can be observed that when the tool is going to move in the X direction, desired force in the Z direction is set to zero and vice versa. Moving in the X-Z plane based on the shown desired force will result in a movement in the mentioned directions which are presented in Figures 13  and 14.
In Figure 14, movements in the X and Z directions are plotted relative to each other to show the tool (centre point of the tool) path in the X-Z plane during the operation. As can be seen, the speed of the tool in the X-Z plane will be regulated automatically based on the desired force and material characteristics.

Conclusion
In this paper, we have presented a model-based centralized robust controller for force/position control of multiple robotic manipulators that cooperatively carrying a heavy load. A hybrid impedance control block was served to produce force and position signals in the constraint coordinate. We then proposed a systemlevel controller block that allocates desired motion and forces to each manipulator end effectors in such a way that minimizes internal forces. The overall closed loop system ensures asymptotic convergence of the tools position and forces to their desired values.

Disclosure statement
No potential conflict of interest was reported by the authors.

A. Derivation of Equation (13)
Let's rearrange Equation (7) as follows: Expanding (38) will result in Using Equations (8), (9) and (10) and replacing p oi × with the cross product matrix S pi , (39) can be transformed to then Now regarding Equation (11), let's multiply both sides of (41) by the non-singular matrix I 3 0 3 S pi I 3 Doing the matrix products in (42), the following equation will be achieved: Finally according to Equation (11), h o can be written as follows: Equation (44) can be represented as (13) with terms described by (14)-(21).

B. Coordination equations
Consider three coordination system depicted in Figure 15. Assuming position vector A x ab = A p ab A q b T shows origin of {B} in {A} where vector A p ab ∈ R 3 is the location part, and vector (3) is the unit quaternion representation of orientation part of the position, where A η b ∈ R is the real part and A b ∈ R 3 is the vector part of it. Given B x bc and by using A R B ( A q b )