- Email: [email protected]
Shaping the Force Feedback by Dynamic Scaling in the Teleoperation of Multi-Robot Systems
6th IFAC Workshop on Lagrangian and Hamiltonian Methods or 6th IFAC Workshop on Lagrangian and Hamiltonian Methods or Nonlinear Control on 6th Hamiltonian Methods or 6th IFAC IFAC Workshop Workshop Lagrangian and and Hamiltonian Methods or Nonlinear Control on Lagrangian Available online at www.sciencedirect.com Universidad Técnicaon Federico Santa María Nonlinear Control 6th IFAC Workshop Lagrangian and Hamiltonian Methods or Nonlinear Control Universidad Técnica Federico Santa María Valparaíso, Chile, May 1-4, 2018 Universidad Técnica Federico Santa Nonlinear Control Universidad Técnica Federico Santa María María Valparaíso, Chile, May 1-4, 2018 Valparaíso, May 1-4, Universidad Técnica Federico Santa María Valparaíso, Chile, Chile, May 1-4, 2018 2018 Valparaíso, Chile, May 1-4, 2018
ScienceDirect
IFAC PapersOnLine 51-3 (2018) 143–148
Shaping the Force Feedback by Dynamic Shaping the Force Feedback by Dynamic Shaping the Force Feedback by Dynamic Shaping the Force Feedback by Dynamic Scaling in the Teleoperation of Multi-Robot Shaping the Force Feedback by Dynamic Scaling in Teleoperation of Multi-Robot Scaling of Multi-Robot Scaling in in the the Teleoperation Teleoperation of Multi-Robot Systems Scaling in the Teleoperation of Multi-Robot Systems Systems Systems Systems Lorenzo Sabattini ∗∗∗ Cesare Fantuzzi ∗∗∗ Cristian Secchi ∗∗∗
Lorenzo Sabattini ∗ Cesare Fantuzzi ∗ Cristian Secchi ∗ Lorenzo Lorenzo Sabattini Sabattini ∗ Cesare Cesare Fantuzzi Fantuzzi ∗ Cristian Cristian Secchi Secchi ∗ ∗ Lorenzo Sabattini ∗ Cesare Fantuzzi ∗ Cristian Secchi ∗ ∗ Department of Sciences and Methods for Engineering (DISMI), ∗ ∗ Department of Sciences and Methods for Engineering (DISMI), ∗ Department of Sciences and Methods for (DISMI), University Modena and Reggio Italy Department for Engineering Engineering (DISMI), Universityofof ofSciences Modenaand andMethods Reggio Emilia, Emilia, Italy (e-mail: (e-mail: ∗ University of Modena and Reggio Emilia, Italy (e-mail: Department of Sciences and Methods for Engineering (DISMI), {lorenzo.sabattini, cristian.secchi, cesare.fantuzzi} @ unimore.it). University of cristian.secchi, Modena and Reggio Emilia, Italy {lorenzo.sabattini, cesare.fantuzzi} @ (e-mail: unimore.it). {lorenzo.sabattini, cristian.secchi, cesare.fantuzzi} @ (e-mail: unimore.it). University of cristian.secchi, Modena and Reggio Emilia, Italy {lorenzo.sabattini, cesare.fantuzzi} @ unimore.it). {lorenzo.sabattini, cristian.secchi, cesare.fantuzzi} @ unimore.it). Abstract: Abstract: In In passivity passivity based based bilateral bilateral teleoperation teleoperation of of multi-robot multi-robot systems systems the the coupling coupling among among Abstract: In passivity based bilateral teleoperation of multi-robot systems the coupling among the robots produces unwanted dynamic effects that deteriorates the force fed back to user. Abstract: In passivity based bilateral teleoperation of multi-robot systems the coupling the robots produces unwanted dynamic effects that deteriorates the force fed back to the theamong user. the robots produces unwanted dynamic effects that deteriorates the force fed back to the user. Abstract: In passivity based bilateral teleoperation of multi-robot systems the coupling among In this paper we propose a passivity based dynamic scaling strategy for compensating these the robots produces unwanted dynamic effects that deteriorates the force fed back to the user. In this paper we propose a passivity based dynamic scaling strategy for compensating these In this paper we propose a passivity based dynamic scaling strategy for compensating these the robots produces unwanted dynamic effects that deteriorates the force fed back to the user. disturbing terms. The effectiveness of the proposed methodology is experimentally validated. In this paper we The propose a passivity based dynamic scaling strategy for compensating these disturbing terms. effectiveness of the proposed methodology is experimentally validated. disturbing terms. The effectiveness of the proposed methodology is experimentally validated. In this paper we propose a passivity based dynamic scaling strategy for compensating these disturbing terms. The effectiveness proposed methodology experimentally validated. © 2018, IFAC (International Federation of of the Automatic Control) Hosting byis Ltd. All rights reserved. disturbing terms. The effectiveness of the proposed methodology isElsevier experimentally validated. Keywords: Keywords: Telerobotics, Telerobotics, Networked Networked robotic robotic system system modeling modeling and and control, control, Decentralized Decentralized Keywords: Telerobotics, control. Keywords: Telerobotics, Networked Networked robotic robotic system system modeling modeling and and control, control, Decentralized Decentralized control. control. Keywords: Telerobotics, Networked robotic system modeling and control, Decentralized control. control. 1. INTRODUCTION In passivity based teleoperation architecture, the multi1. INTRODUCTION In passivity based teleoperation architecture, the multi1. In teleoperation architecture, the multirobot systembased is controlled to behave as a passive system 1. INTRODUCTION INTRODUCTION In passivity passivity based teleoperation architecture, the system multirobot system is controlled to behave as a passive robot system is controlled to behave as a passive system 1. INTRODUCTION In passivity based teleoperation architecture, multiand this is often achieved by interconnecting the agents robot system is controlled to behave as a passive system and this is often achieved by interconnecting the agents and this is often achieved by interconnecting the agents robot system is controlled to behave as a passive system by virtual springs and dampers. In this way the controlled andvirtual this issprings often achieved by interconnecting agents by and dampers. In this way thethe controlled Multi-robot systems have proven to be very effective for by and dampers. In the controlled and this issprings often by agents Multi-robot systems have proven to be very effective for multi-robot systemachieved behaves as ainterconnecting physical system whose dyby virtual virtual springs and dampers. In this this way way thethe controlled Multi-robot systems have proven to be very effective for aaMulti-robot large class of application as, e.g., surveillance and data multi-robot system behaves as a physical system whose dyhave proven be very effective for multi-robot large class systems of application as, e.g.,tosurveillance and data system behaves as aa physical system whose dyby virtual springs and dampers. In this way the controlled namics is felt by the user as a force. Because of the (many) multi-robot system behaves as physical system whose dyacollection of application as, and Multi-robot systems have proven tosurveillance be very for namics is felt by the user as a force. Because of the (many) and the problem of e.g., modeling andeffective controlling a large large class class of the application as, e.g., surveillance and data data namics is felt by the user as aa when force. Because of the (many) multi-robot system behaves as a physical system whose dycollection and problem of modeling and controlling couplings among the agents, the user is moving the namics is felt by the user as force. Because of the (many) collection and the problem of modeling and controlling couplings among the agents, when the user is moving the a largesystems class application as, surveillance and data these has been widely addressed by the robotics collection andofhas the problem of e.g., modeling and controlling couplings among the agents, when the user is moving the namics is felt by the user as a force. Because of the (many) these systems been widely addressed by the robotics group of robots s/he feels a disturbing effect which precouplings among s/he the agents, when the user is moving the these systems has been widely addressed by the robotics group of robots feels a disturbing effect which precollection and the problem of modeling and controlling community, see (Doriya et al., 2015) for a recent survey. these systems has been widely addressed by the survey. robotics group of s/he feels disturbing effect which precouplings among theunderstanding agents, the user moving the community, see (Doriya et al., 2015) for a recent vents her/him from what is is exactly going group her/him of robots robots s/he feels aa when disturbing effect whichgoing precommunity, see (Doriya et al., 2015) for a recent survey. these systems has been widely addressed by the robotics vents from understanding what is exactly community, see (Doriya et al., 2015)offor a tasks recentincreases, survey. vents her/him from understanding what is exactly group of goal robots s/he feels a isdisturbing effect whichgoing preon. The of this paper to develop a strategy for Nevertheless, when the complexity the vents her/him from understanding what is exactly going community, see (Doriya et al., 2015)offor a tasks recentincreases, survey. on. The goal of this paper is to develop strategy for Nevertheless, when the complexity the The goal of this paper is to develop strategy for Nevertheless, when complexity of the tasks increases, vents her/him from understanding what is aaaexactly going compensating this undesired dynamic effect. We consider the intervention of the human is mandatory. Using tele- on. on. The goal of this paper is to develop strategy for Nevertheless, when the complexity of the tasks increases, compensating this undesired dynamic effect. We consider the intervention of the complexity human is mandatory. telethis undesired effect. consider the of is mandatory. Using teleon. The of paper isdynamic toteleoperates develop a We strategy for Nevertheless, when of high-level the tasksUsing increases, the case in the operator directly one operation, it possible to cognitive compensating thisthis undesired dynamic effect. We consider the intervention intervention of the the human human is the mandatory. Using tele- compensating the case goal in which which the operator teleoperates directly one operation, it is is possible to inject inject the high-level cognitive the case in which the operator teleoperates directly one operation, it is possible to inject the high-level cognitive compensating this undesired dynamic effect. We consider the intervention of the human is mandatory. Using teleof the robots, as in (Franchi et al., 2012c). Exploiting abilities of the human into the control of a robotic system the case in which the operator teleoperates directly one operation, it ishuman possible to the inject the high-level cognitive of the robots, as in (Franchi et al., 2012c).directly Exploiting abilities of the into control of a robotic system robots, as (Franchi et al., abilities of the into control of robotic system the case in recently which the operatorin one operation, it possible to the inject theuser high-level cognitive results proposed (Sabattini et Exploiting al., 2017, (Secchi et al., 2007). Since a single cannot directly of the the robots, as in in (Franchi etteleoperates al., 2012c). 2012c).et Exploiting abilities et of al., theishuman human into the control of aacannot robotic system of the results recently proposed in (Sabattini al., 2017, (Secchi 2007). Since a single user directly the results recently proposed in (Sabattini et al., 2017, (Secchi et al., 2007). Since a single user cannot directly of the robots, as in (Franchi et al., 2012c). Exploiting abilities of the human into the control of a robotic system 2018), we propose a passivity preserving strategy for tunteleoperate each robot of a multi-robot system, it is necthe results recentlya passivity proposed preserving in (Sabattini et al., (Secchi et al., 2007). a single usersystem, cannotitdirectly we propose strategy for2017, tunteleoperate each robotSince of a multi-robot is nec- 2018), 2018), we propose aa passivity preserving strategy for tunteleoperate each robot of system, it is the results recently proposed in (Sabattini al., (Secchi al., 2007). acontrol single user cannotwhere the coupling among the in to essary to to the shared paradigm, the 2018), we propose passivity preserving strategy for2017, tunteleoperate each of aa multi-robot multi-robot system, itdirectly is necnecing the coupling among the robots robots in order order toetcompensate compensate essary toetresort resort torobot the Since shared control paradigm, where the ing the coupling robots in to essary resort the control paradigm, where the 2018), propose a passivity preserving strategy for tunteleoperate each robot of with a multi-robot system,for it dealing is necdisturbing effectsamong due tothe the dynamics of thecompensate controlled fleet ofto robots isto endowed some autonomy ing thewe coupling among thethe robots in order order to compensate essary to resort to the shared shared control paradigm, where the ing disturbing effects due to dynamics of the controlled fleet of robots is endowed with some autonomy for dealing effects due to dynamics the controlled fleet robots endowed with some for dealing ing the coupling among robots in order to essary to resort to the (e.g. shared control paradigm, where the disturbing multi-robot system while preserving theof coupling among with low levelis tasks maintenance of a cooperative disturbing effects due tothethe the dynamics ofcoupling thecompensate controlled fleet of oflow robots istasks endowed with some autonomy autonomy for dealing multi-robot system while preserving the among with level (e.g. maintenance of a cooperative multi-robot system while preserving the coupling among with low level tasks (e.g. maintenance of a cooperative disturbing effects due to the dynamics of the controlled fleet of robots is endowed with some autonomy for dealing the robots. behavior, collision avoidance) and the user teleoperates multi-robot among with low level tasks (e.g. maintenance of a teleoperates cooperative the robots. system while preserving the coupling behavior, collision avoidance) and the user behavior, collision avoidance) and teleoperates multi-robot with low as level tasks (e.g. maintenance of ateleoperating cooperative the the fleet aa whole. Several strategies for the robots. robots. system while preserving the coupling among behavior, collision avoidance) and the the user user teleoperates the fleet as whole. Several strategies for teleoperating the fleet aa system whole. Several strategies for teleoperating the robots. behavior, avoidance) and the user teleoperates Notation athe multi-robot have been recently proposed in the 1.1 fleet as ascollision whole. Several strategies for teleoperating 1.1 Notation a multi-robot system been recently proposed in the 1.1 Notation aliterature. multi-robot system have recently proposed in the the fleet asIna (Palafox whole. have Several strategies teleoperating and been Spong, 2009;for Rodr´ ıguez-Seda 1.1 Notation a multi-robot system have been recently proposed in the literature. In (Palafox and Spong, 2009; Rodr´ ıguez-Seda 1.1 NotationIm ∈ Rm×m literature. In and Spong, 2009; Rodr´ ıguez-Seda a multi-robot system have recently in the The indicate the identity matrix et al., strategy for teleoperating the m×m will literature. In aa(Palafox (Palafox and Spong, 2009; proposed Rodr´ ıguez-Seda Rm×m matrix The symbol symbol Im et al., 2010) 2010) strategy for been teleoperating the multi-robot multi-robot m×m will indicate the identity m ∈ m×n m×mthe ∈ R will indicate the matrix The symbol I et al., 2010) a strategy for teleoperating the multi-robot literature. In (Palafox and Spong, 2009; Rodr´ ıguez-Seda of dimension m, and symbol O ∈ R will system as an overall rigid body, constraining the robots on m m×n m,n identity m×n Thedimension symbol Imm, ∈R will indicate the identity matrix et al., 2010) a strategy for teleoperating thethe multi-robot of and the symbol O ∈ R will m,n system as an overall rigid body, constraining robots on m×mthe symbol Om,n ∈ Rm×n m×n of dimension m, and will system as an overall rigid body, constraining robots on The symbol I ∈ R will indicate the identity matrix al., 2010) a strategy teleoperating thethe multi-robot indicate the null matrix of dimension m × n. For ease of aet specific formation, hasfor been proposed. In (Cheung and m,n m of dimension m, and the symbol O ∈ R system as an overall rigid body, constraining the robots on indicate the null matrix of dimension m,n m × n. For easewill of a specific formation, has been proposed. In (Cheung and m×n indicate the null matrix of dimension m × n. For ease of aChung, specific formation, has been proposed. In (Cheung of dimension m, and the symbol O ∈ R will system as an overall rigid body,controller constraining the robotsand on notation, we will omit the dimension of the matrices when 2009) an impedance for teleoperating m,n indicate the null matrix of dimension m × n. For ease of a specific formation, has been proposed. In (Cheung and notation, we will omit the dimension of the matrices when Chung, 2009) an impedance controller for teleoperating ρ×σ notation, we will omit the dimension of the matrices when Chung, 2009) an impedance controller for teleoperating indicate the null matrix of dimension m × n. For ease ofa specific formation, has been proposed. In (Cheung and they appear clearly from the context. Let Ω ∈ R be aaChung, group of robots in a leader-follower modality has been ρ×σ we clearly will omit thethe dimension theΩmatrices when an impedance controllermodality for teleoperating they appear from context.ofLet ∈ Rρ×σ be a group 2009) of robots in a leader-follower has been notation, ρ×σ ρ×σ they appear clearly from the context. Let Ω ∈ R be apresented. group of robots in a leader-follower modality has been notation, we will omit the dimension of the matrices when Chung, 2009) an impedance controller for teleoperating generic matrix. Then, we define Ω [i, j] ∈ R as the element Passivity based control has been successfully appear clearly from the context. Rρ×σ be a a a group of Passivity robots in based a leader-follower modality has been they generic matrix. Then, we define Ω [i, j] Let ∈ RΩas∈the element presented. control has been successfully generic matrix. Then, we Ω ∈ presented. control been from the context. R element be a a group ofinPassivity robots in based a leader-follower modality has since been they (i, of Ω. exploited single-master/single-slave teleoperation generic Then, we define define Ω [i, [i, j] j] Let ∈R RΩas as∈the the element presented. Passivity based control has hasteleoperation been successfully successfully (i, j) j) appear of matrix. Ω. clearly exploited in single-master/single-slave since (i, Ω. exploited in single-master/single-slave since Then, we define Ω [i, j] ∈ R as the element presented. based control hasteleoperation been it allows to guarantee a stable behavior in thesuccessfully interaction (i, j) j) of of matrix. Ω. exploited inPassivity single-master/single-slave teleoperation since generic it allows to guarantee a stable behavior in the interaction it allows to guarantee a stable behavior in the interaction (i, j) of Ω. exploited in single-master/single-slave teleoperation since 2. with any, possibly unknown, passive environment (Secchi it allows guarantee a stablepassive behavior in the interaction 2. PROBLEM PROBLEM STATEMENT STATEMENT with any,topossibly unknown, environment (Secchi 2. with any, possibly unknown, passive environment (Secchi it allows to guarantee a stable behavior in the interaction et al., 2007). Passivity based teleoperation has been also 2. PROBLEM PROBLEM STATEMENT STATEMENT with any, possibly unknown, passive environment (Secchi et al., 2007). Passivity based teleoperation has been also et 2007). based teleoperation has also 2. PROBLEM STATEMENT with possibly unknown, passive environment (Secchi exploited for Passivity teleoperating multi-robot systems. In (Lee et al., al.,any, 2007). Passivity basedmulti-robot teleoperation has been been also We We consider consider the the teleoperation teleoperation of of aa multi-robot multi-robot system: system: exploited for teleoperating systems. In (Lee exploited for teleoperating multi-robot In (Lee We consider the teleoperation of aa the multi-robot system: al., Passivity based teleoperation been alsoa at et al., 2007). 2013) the robots are controlled systems. to has behave as the master side, the user moves master exploited for teleoperating multi-robot systems. In (Lee We consider the teleoperation of multi-robot system:a at the master side, the user moves the master robot, robot, a et al., 2013) the robots are controlled to behave as a et al., 2013) the robots are controlled to behave as a at the master side, the user moves the master robot, a exploited for teleoperating multi-robot systems. In (Lee We consider the teleoperation of a multi-robot system: passive deformable object that can be teleoperated by the haptic device by means of which s/he can teleoperate the et al., 2013) the robots to behavebyasthea at the device masterby side, the ofuser moves robot, a passive deformable object are that controlled can be teleoperated haptic means which s/hethe canmaster teleoperate the passive deformable object that can be teleoperated by the haptic device by means of which s/he can teleoperate the et al., 2013) the robots are controlled to behave as a at the master side, the user moves the master robot, a user, in (Franchi et al., 2012c,a) energy tanks (Ferraguti robot and receive an informative force feedback from passive deformable object that canenergy be teleoperated by the haptic device by means of which s/he teleoperate robot and receive an informative forcecan feedback from the user, in (Franchi et al., 2012c,a) tanks (Ferraguti the user, (Franchi et al., 2012c,a) energy tanks (Ferraguti and receive an informative force feedback from the passive deformable object that can be teleoperated bywith the robot haptic device by means of which s/he can teleoperate et al.,in 2015) have been exploited for passively dealing slave side. The slave side consists of the multi-robot system user, in (Franchi et al., 2012c,a) energy tanks (Ferraguti robot and receive an informative force feedback from the slave side. The slave side consists of the multi-robot system et al.,in 2015) have been for passively dealing with et have exploited for passively dealing with slave side. The slave consists of the multi-robot system user, (Franchi et al.,exploited 2012c,a) energy tanks (Ferraguti robot and receive anside informative feedback from the variable topology during the and in the are virtually coupled their et al., al., 2015) 2015) have been been exploited for passively dealing with where slave The slave side consists offorce thewith multi-robot system variable topology during the teleoperation teleoperation and in (Secchi (Secchi whereside. the robots robots are virtually coupled with their neighbors neighbors variable topology during the teleoperation and in (Secchi where the robots are virtually coupled with their neighbors al., 2015) have been exploited for passively dealing with slave side. The slave side consists of the multi-robot system et a passivity based strategy for conducting the through control actions defined to achieve some desired variable topology duringbased the teleoperation and in (Secchi thecontrol robots actions are virtually coupled with their neighbors et al., 2015) a passivity strategy for conducting the where through defined to achieve some desired et passivity based strategy for the through actions defined to some desired variable topology during the teleoperation and in (Secchi where thecontrol robots are virtually coupled with their neighbors robots over aaaperiodic trajectory has been proposed. behavior. communication delay between the et al., al., 2015) 2015) passivity based strategy for conducting conducting the cooperative through control actions defined to achieve achieve some desired cooperative behavior. communication delay between the robots over a periodic trajectory has been proposed. robots over has proposed. cooperative behavior. communication delay between the et al., 2015) passivitytrajectory based strategy for conducting control actions defined to achieve desired robots over aaaperiodic periodic trajectory has been been proposed. the through cooperative behavior. communication delay some between the robots over a periodic trajectory has been proposed. cooperative behavior. communication delay between the 2405-8963 © 2018 2018, IFAC IFAC (International Federation of Automatic Control) Copyright 143 Hosting by Elsevier Ltd. All rights reserved. Copyright 2018 IFAC 143 Control. Peer review© responsibility of International Federation of Automatic Copyright © under 2018 IFAC IFAC 143 Copyright © 2018 143 10.1016/j.ifacol.2018.06.041 Copyright © 2018 IFAC 143
IFAC LHMNC 2018 144 Valparaíso, Chile, May 1-4, 2018
Lorenzo Sabattini et al. / IFAC PapersOnLine 51-3 (2018) 143–148
master and the slave side is negligible. We consider a passive teleoperation system. Thus, the coupling among the robots is designed in order to achieve a passive behavior of the overall group. Furthermore, master and slave sides are joined by means of a passive coupling by means of which the user can directly teleoperate one of the robots and through which a force feedback can be transmitted to the operator. Using the formulation proposed in (Sabattini et al., 2017), we will develop a passivity preserving strategy for dynamical tuning the inter-robot coupling forces in order to compensate the effects of the couplings on the force fed back to the operator and to allow the user to teleoperate the robot it is connected to as if it were a simple floating mass, characterized by desired values of inertia and damping. 3. SLAVE SIDE: THE MULTI-ROBOT SYSTEM The slave side consists of a group of N robots moving in a m-dimensional environment, whose dynamics are modeled as follows: mi x ¨i = wi i = 1, . . . , N (1) where xi ∈ Rm is the ith robot’s position, mi > 0 is the ith robot’s mass, and wi ∈ Rm collects control inputs and all the external forces each robot is subject to. Each robot is controlled, by means of a local control strategy, in such a way that the overall multi-robot system achieves some common cooperative objective. We consider the case where the robots can exchange information among each other if their distance is smaller than the communication radius R > 0. The neighborhood Ni (t) of the ith robot is then the set of robots that can communicate with robot i at time t, that is (2) Ni (t) = {j �= i such that �xi,j (t)� ≤ R} where xi,j = xi − xj . Hence, consider the control input wi in (1) to be defined as the composition of different terms, namely: N wi = Fi,j − bi x˙ i + Fie + Fis (3) j=1,j =i
3.1 Control input terms Coupling control terms The cooperative behavior is achieved by means of the term Fi,j ∈ Rm , which represents the coupling between the ith robot and its neighbors. In particular, in this paper we assume the coupling term Fi,j to be defined as the composition of an elastic interconnection and an inter-robot damping, namely: N N Fi,j = − ∇Vi,j − βi,j (x˙ i − x˙ j ) (4) j=1,j =i
j=1,j =i
The elastic interconnection in (4) is represented by the N ∇Vi,j , where Vi,j = V (xi,j ) is some approterm − j=1,j =i
priately defined artificial potential field (Sabattini et al., 2011; Leonard and Fiorelli, 2001; Franchi et al., 2012b; Falconi et al., 2015; Reif and Wang, 1999). As in (Leonard and Fiorelli, 2001), we assume the artificial potential field V (xi,j ) to be non-negative and to have a global minimum 144
at the desired distance �xi,j � = δd > 0, and to generate a repulsive force that avoids inter-robot distances to go below the safety value δs , such that 0 < δs < δd . Inter-robot damping is represented in (4) by the term N − βi,j (x˙ i − x˙ j ). In this term, the parameter βi,j is j=1,j =i
defined as follows: βi,j =
β¯ ≥ 0 if j ∈ Ni (t) 0 otherwise
(5)
Together with the coupling artificial potential field, the design of βi,j leads to defining the overall desired behavior of the multi-robot system. Notice that, because of the shape of the considered potential function and of the damping parameter (5), (4) can be implemented in a decentralized way since, if j ∈ / Ni (t), then Fi,j = 0. Individual robot control terms The remaining terms that compose the input wi in (3) are computed independently for each robots, and do not explicitly depend on the neighbors. In particular, the term −bi x˙ i ∈ Rm , with bi > 0, represents a local damping, that describes the viscous friction each robot is subject to, and any possible additional damping added by the control law. The terms Fis , Fie ∈ Rm represent the teleoperation and the force due to the interaction with the environment respectively. Fis = 0m if the corresponding robot is not directly teleoperated. Controlled multi-robot system model Considering now the dynamics of the ith robot given in (1), the generic input defined in (3) with the coupling term specified in (4), the following dynamic model is achieved: mi x ¨i + bi x˙ i +
N
∇Vi,j +
j=1,j =i
N
βi,j (x˙ i − x˙ j ) = Fie + Fis (6)
j=1,j =i
3.2 Passivity of the slave side In order to prove the passivity of the multi-robot system at the slave side, it is instrumental to rewrite its model in port-Hamiltonian form. Let then pi = mi x˙ i be the ith robot’s momentum. The dynamics (6) of the multi-robot system at the slave side can then be rewritten as follows: N N pj pi pi p˙ i + bi + ∇Vi,j + βi,j − = Fie + Fis mi mi mj j=1,j =i
j=1,j =i
(7) We now introduce the total energy of the system H, defined as follows: N N N Ki (pi ) + Vi,j ≥ 0 (8) H= i=1
i=1 i=j,j =i
where pTi pi 2mi is the kinetic energy associated to robot i. Define now the following vectors: Ki (pi ) =
IFAC LHMNC 2018 Valparaíso, Chile, May 1-4, 2018
Lorenzo Sabattini et al. / IFAC PapersOnLine 51-3 (2018) 143–148
�T � F e = F1e T . . . FNe T ∈ RmN � �T (9) F s = F1s T . . . FNs T ∈ RmN � � T T p = p1 . . . pN T ∈ RmN ¯ = N (N −1)/2, and define the following Moreover, let N vectors: � �T �T � ¯ χ = χ 1 T . . . χN T = x T . . . xN −1,N T ∈ RmN � �T � 1,2 �T ¯ β = β 1 T . . . βN T = β1,2 T . . . βN −1,N T ∈ RmN (10)
Furthermore, define the following velocity vector � �T v = x˙ T1 . . . x˙ TN ∈ RmN and define the following matrix �T � G = ImN OTmN¯ ,mN
(11) (12)
¯
Define then IG ∈ RN ×N as the incidence matrix (Godsil and Royle, 2001) of the complete graph among the robots: subsequently, the weighted Laplacian matrix can be computed as ¯ IGT L β = IG B (13) ¯ defined as with the weight matrix B ¯ = diag (β) B (14) Defining (15) B = (Lβ + diag (b1 , . . . , bN )) ⊗ Im and (16) I = IG ⊗ I m it is possible to rewrite the dynamics (6) of the multi-robot system in port-Hamiltonian form as follows: � � �� ∂H � � �� O I B O ∂p p˙ − + G(F e + F s ) = O O ∂H χ˙ −I T O ∂χ ∂H ∂p v = GT ∂H ∂χ (17) Consider now the total energy of the system H, introduced in (8). For ease of notation, we define now Vk = V (χk ). Hence, it is possible to rewrite (8) as: H=
N � i=1
Ki (pi ) +
¯ N �
Vk ≥ 0
(18)
k=1
As shown in (Franchi et al., 2012b, Proposition 1), (17) is passive with respect to the pair (F e + F s , v). We will consider the case in which only the ith robot will be teleoperated and, therefore, all the components of F s but the Fis one will be zero. 4. TELEOPERATION SYSTEM The teleoperation scheme considered in this paper consists in having the human operator able to impose a force Fis on the ith robot at the slave side, through the master robotic device. We consider the case where the operator can freely choose the robot to be directly teleoperated, and we will hereafter focus, without loss of generality, on the ith robot. 145
145
The user teleoperates the multi-robot system by directly teleoperating only the ith robot whose motion, because of the coupling among the agents, will cause the motion of the overall fleet. The master haptic device can be modeled as a fully actuated Euler-Lagrange system, namely as: � � ¨ m + C Xm , X˙ m X˙ m = Fm + Fh M (Xm ) X (19)
where Xm ∈ Rm is the position of the end effector 1 , X˙ m ∈ Rm is its velocity, M (Xm ) > 0 is the inertia matrix, C(Xm , X˙ m )X˙ m encodes the Coriolis and centrifugal terms. Fm , Fh ∈ Rm are the coupling wrench with the slave side and the wrench applied to the robot by the human operator, respectively. As is well known, see e.g. (Secchi et al., 2007), (19) is passive with respect to the pair (Fm + Fh , X˙ m ).
Finally, we exploit the following elastic coupling for interconnecting master and slave sides: F = −η (XM − xi ) m (20) s Fi = η (XM − xi )
where η > 0 is a tuning gain, that can be freely defined based on the application.
Both master and slave side are passive systems and, furthermore, also the elastic interconnection (20) is passive (Secchi et al., 2007). Since the interconnection of passive systems is still a passive system (Secchi et al., 2007), then the overall teleoperation system is passive. 5. VARIABLE DYNAMIC BEHAVIOR OF THE TELEOPERATED MULTI-ROBOT SYSTEM Considering the dynamics of the multi-robot system at the slave side, given in (6), and considering the coupling with the robotic system at the master side defined in Section 4, the force feedback provided to the human operator is given by: Fm = −Fis = = −(mi x ¨i + bi x˙ i +
N �
∇Vi,j +
j=1,j�=i
N �
βi,j (x˙ i − x˙ j ) − Fie )
j=1,j�=i
(21) Specifically, the force feedback is defined based on the control actions that are implemented on the directly teleoperated robot. Hence, it depends on how robots are coupled among each other. We will now introduce a methodology to scale such couplings, in such a way that the ith robot can be teleoperated as if it were a floating mass md subject to a damping coefficient βd . Namely, we would like the teleoperated multi-robot system to provide a force feedback given by the following desired dynamic model: Fid = md x ¨i + βd x˙ i (22) In this way the operator can feel the motion of the teleoperated robot without disturbing effects due to the coupling. 1 For ease of notation, in this paper we consider that the dimension of xi and Xm are the same, namely xi , Xm ∈ Rm . This condition can however be relaxed introducing opportune projection matrices.
IFAC LHMNC 2018 146 Valparaíso, Chile, May 1-4, 2018
Lorenzo Sabattini et al. / IFAC PapersOnLine 51-3 (2018) 143–148
The motion of the ith robot will keep on causing the motion of the overall fleet because of its interconnection with its neighbors. This can be obtained exploiting the coupling variation methodology introduced in (Sabattini et al., 2017). In particular, we introduce scaling factors, opportunely computed by the ith robot to scale the elastic and damping forces that couple it with neighboring robots. In order to scale the elastic coupling forces, define AG = diag(α1 , . . . , αN¯ ) as a diagonal matrix containing scaling factors, one per each pair of robots. To enable the ith robot to scale the forces that couple it with its neighbors, the elements of AG can be defined as follows: � α(t) > 0 if IG [i, k] �= 0 and �χk � ≤ R αk = (23) 1 otherwise Along the same lines, scaling factors can be introduced for modulating the inter-agent damping, defining matrix CG = diag (c1 , . . . , cN¯ ) as a diagonal matrix containing a scaling factor per each pair of robots, defined as follows: � γ(t) > 0 if IG [i, k] �= 0 and �χk � ≤ R (24) ck = 1 otherwise In this manner, the force feedback (21) is modified as follows: Fis = mi x ¨i + bi x˙ i + α(t)
N �
∇Vi,j + γ(t)
j=1,j�=i
N �
βi,j (x˙ i − x˙ j ) − Fie
j=1,j�=i
(25) It is worth noting that the elastic coupling term between any two robots is only a function of their relative positions. Hence, it is always possible to write it as follows: ∇Vi,j = κi,j (xi , xj ) (xi − xj ) (26) In order to keep the notation simple, in the following we will omit the dependency of κi,j on xi , xj . The force feedback in (25) can then be rewritten as follows: Fis = mi x ¨i + bi x˙ i + α(t)
N �
κi,j (xi − xj ) + γ(t)
j=1,j�=i
N �
βi,j (x˙ i − x˙ j ) − Fie
j=1,j�=i
(27) Tuning parameters can then be selected to implement the desired dynamic behavior (22). However, it is worth noting that such parameters can not be chosen in a completely arbitrary manner. In fact, their introduction can lead to breaking the passivity property. This fact can be shown starting from the port-Hamiltonian model of the multi-robot system, given in (17) for the unscaled coupling forces. Scaled coupling forces are exchanged in the multi-robot system according to the following matrices, respectively: (IG AG (t)) ⊗ Im (28) and B(t) = (L¯β + diag (b1 , . . . , bN )) ⊗ Im (29) where � � ¯ IGT L¯β = IG CG B (30)
Exploiting the properties of the Kronecker product, it is possible to obtain: (IG AG (t)) ⊗ Im = (IG AG (t)) ⊗ (Im Im ) = = (IG ⊗ Im ) (AG (t) ⊗ Im ) = IA(t) (31) where A(t) = AG (t) ⊗ Im . 146
Introducing then the scaling matrices into the dynamics of the multi-robot system (17), we obtain the following: � � �� ∂H � � �� p˙ O IA(t) B(t) O ∂p − + G(F e + F s ) χ˙ = O O ∂H −I T O ∂χ ∂H ∂p v = GT ∂H ∂χ
(32) Comparing (32) with (17), it is possible to note that the introduction of the scaling matrices leads to modifying the structure of the dynamic model of the multi-robot system. These changes lead to the fact that passivity can no longer be trivially guaranteed, as discussed in Section 3.2. In particular, while B plays the same role as B (being both matrices positive definite), the introduction of matrix A(t) leads to breaking the skew-symmetric structure of the interconnection matrix. However, exploiting the results of (Sabattini et al., 2017), passivity can still be guaranteed. To this aim, (32) can be rewritten as follows:
∂H � � �� � � �� O I B(t) O ∂p p˙ e s − = ∂H + G(F + F ) χ˙ O O −I T O A(t) ∂χ ∂H T ∂p v = G ∂H ∂χ
(33) Passivity can then be guaranteed utilizing the results of (Sabattini et al., 2017, Proposition 2), considering the following modified energy function: Hs =
N � i=1
Ki (pi ) +
¯ N �
α k Vk ≥ 0
(34)
k=1
The sketch of the proof is given in the following. Proposition 1. Consider the dynamics of the multi-robot system described in port-Hamiltonian form in (33), and consider the total energy of the system given in (34). Then, if ∃αm , αM ∈ R, 0 < αm < αM , such that αm ≤ α(t) ≤ αM for any time t ≥ 0, then the system is passive with respect to the pair (F e + F s , v). Proof. The time derivative of the energy function (34) can be computed as follows: ∂Hs ∂ T Hs ∂Hs B + (35) H˙ s = (F e + F s )T v − ∂p ∂p ∂t Since matrix B is positive definite, (35) can be integrated as follows: � t � t ∂Hs dτ (36) (F e + F s )T vdτ + Hs (t) − Hs (0) ≤ 0 0 ∂τ Following the same steps of (Sabattini et al., 2017, Proposition 2), since both the kinetic energy Ki (·) and the potentials Vk (·) are positive, passivity can then be proven since the following holds: � t N � (F e + F s )T vdτ ≥ − Ki (0) (37) 0
i=1
✷
IFAC LHMNC 2018 Valparaíso, Chile, May 1-4, 2018
Lorenzo Sabattini et al. / IFAC PapersOnLine 51-3 (2018) 143–148
147
Force feedback
20 10 0 -10 -20
Fig. 2. Force feedback, constant coupling parameters (high) Fig. 1. The experimental setup
In order to achieve the desired dynamic behavior (22), we introduce then the following quadratic optimization problem: minimize f (αi , γi ) subject to αm ≤ αi ≤ αM (38) γi > 0 where αM > αm > 0 are the upper- and lower-bounds for αi (t), respectively, defined according to Proposition 1, and the cost function is defined as follows: 2 (39) f (α, γ) = Fis − Fid
The optimization problem (38) can be solved locally by the ith robot, when teleoperation is being performed. Since coupling forces are defined in a bi-directional manner, the result of the optimization problem needs then to be broadcast to all the ith robot’s neighbors: in this way, forces can be opportunely scaled, as desired. It is worth noting that the result of the optimization problem depends on the current position of the robots: it is then necessary to periodically recompute it (every T > 0 seconds, to be defined based on the application), based on current relative positions. Furthermore, the acceleration measurements are required; removing such a need is currently under investigation. 6. EXPERIMENTS The experimental validation of the proposed teleoperation architecture was performed on a setup, shown in Fig. 1, in which the master robot was represented by a Geomagic Touch haptic device, while the slave side was composed of a group of e-puck robots, which are small-size differential drive wheeled mobile robots. The e-puck robots were constrained to move in a 2 × 1.5 m arena, equipped with an overhead camera that provided localization of the robots. Up to eight robots were used in the experiments. The teleoperation and control architecture was implemented in ROS (Quigley et al., 2009), emulating a decentralized implementation: each robot was exploiting only locally available information, and only one robot exchanged information with the master robotic device. Feedback linerarization (Oriolo et al., 2002) is exploited to model the mobile robots as point masses moving in a bi-dimensional environment. In order to have the same dimensionality in the workspaces of the slave and master sides, we considered the motion of the haptic device along two dimensions only. 147
20 10 0 -10
Fig. 3. Force feedback, constant coupling parameters (low) 20
Force feedback
It is worth noting that Proposition 1 defines constraints on the choice of the scaling parameters that, if satisfied, guarantee passivity preservation.
Force feedback
30
10 0 -10 -20
Fig. 4. Force feedback, variable coupling parameters In these experiments we are not considering the presence of environmental obstacles: as a consequence, in (6), we set the external force to zero, that is Fie = 0, ∀i. Hereafter we report the results of a representative run of the experiment, in which 6 robots were utilized. The dynamic model (6) was defined, for each robot i, using the following parameters: mi = 0.1, bi = 1, β¯ = 1, δs = 1 cm, δd = 20 cm. The desired dynamic model was defined according to (22) with md = 0.1, βd = 1. The experiment was performed in three conditions: with constant coupling parameters, high (α(t) = γ(t) = 30) and low (α(t) = γ(t) = 0.1), in order to verify how the multirobot system feels when applying simple scalings, and with variable parameters computed according to the procedure described in Section 5. The behavior of the teleoperation system can be seen in the accompanying video 2 . Force feedback is depicted in Figs. 2, 3 and 4. It can be seen that both in Fig. 2 and in Fig. 3 there are low frequency oscillations due to the coupling among the robots. In Fig. 4 low level oscillations are much more limited due to the variable coupling. In Figs. 5, 6 and 7 the value of (39) is represented for the three conditions. In case of constant scaling what the user feels is quite distant from (22) because of the effect of the multi-robot system dynamics on the force feedback. When the variable scaling is applied, it is evident that what the user feels is very close to (22). 7. CONCLUSIONS In this paper we have proposed a passivity preserving dynamic scaling strategy for improving the teleoperation of a multi-robot system. Experiments have shown the 2
https://www.youtube.com/watch?v=3vqlrFq6h g
IFAC LHMNC 2018 148 Valparaíso, Chile, May 1-4, 2018
Lorenzo Sabattini et al. / IFAC PapersOnLine 51-3 (2018) 143–148
Cost function
10 7
10 6
10 5
Fig. 5. Cost function, constant coupling parameters (high)
Cost function
10 5
10 0
10 -5
Fig. 6. Cost function, constant coupling parameters (low)
Cost function
10 5
10 0
10 -5
Fig. 7. Cost function, variable coupling parameters effectiveness of the proposed strategy for compensating the spurious effects due to the coupling of the robots. Future work aims at extending the proposed strategy for including in the desired dynamic a term informing about the interaction of the fleet with the environment: results along these lines were recently proposed in (Sabattini et al., 2018). REFERENCES Cheung, Y. and Chung, J.S. (2009). Cooperative control of a multi-arm system using semi-autonomous telemanipulation and adaptive impedance. In 2009 International Conference on Advanced Robotics, 1–7. Doriya, R., Mishra, S., and Gupta, S. (2015). A brief survey and analysis of multi-robot communication and coordination. In International Conference on Computing, Communication Automation, 1014–1021. Falconi, R., Sabattini, L., Secchi, C., Fantuzzi, C., and Melchiorri, C. (2015). Edge-weighted consensus based formation control strategy with collision avoidance. Robotica, 33(02), 332–347. Ferraguti, F., Preda, N., Manurung, A., Bonf, M., Lambercy, O., Gassert, R., Muradore, R., Fiorini, P., and Secchi, C. (2015). An energy tank-based interactive control architecture for autonomous and teleoperated robotic surgery. IEEE Transactions on Robotics, 31(5), 1073–1088. doi:10.1109/TRO.2015.2455791. Franchi, A., Secchi, C., Ryll, M., Bulthoff, H.H., and Giordano, P.R. (2012a). Shared control : Balancing autonomy and human assistance with a group of quadrotor uavs. IEEE Robotics Automation Magazine, 19(3), 57– 68. doi:10.1109/MRA.2012.2205625. Franchi, A., Secchi, C., Son, H.I., Bulthoff, H.H., and Robuffo Giordano, P. (2012b). Bilateral teleoperation of groups of mobile robots with time-varying topology. IEEE Transactions on Robotics, 28(5), 1019–1033. doi: 10.1109/TRO.2012.2196304. 148
Franchi, A., Secchi, C., Son, H.I., Bulthoff, H., and Robuffo Giordano, P. (2012c). Bilateral teleoperation of groups of mobile robots with time-varying topology. IEEE Transactions on Robotics, 28(5), 1019 –1033. Godsil, C. and Royle, G. (2001). Algebraic Graph Theory. Springer. Lee, D., Franchi, A., Son, H.I., Ha, C., Blthoff, H.H., and Giordano, P.R. (2013). Semiautonomous haptic teleoperation control architecture of multiple unmanned aerial vehicles. IEEE/ASME Transactions on Mechatronics, 18(4), 1334–1345. Leonard, N.E. and Fiorelli, E. (2001). Virtual leaders, artificial potentials and coordinated control of groups. In Proceedings of the 40th IEEE Conference on Decision and Control (CDC), volume 3, 2968–2973. IEEE. Oriolo, G., De Luca, A., and Vendittelli, M. (2002). Wmr control via dynamic feedback linearization: design, implementation, and experimental validation. IEEE Transactions on control systems technology, 10(6), 835– 852. Palafox, O. and Spong, M. (2009). Bilateral teleoperation of a formation of nonholonomic mobile robots under constant time delay. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2821–2826. Quigley, M., Conley, K., Gerkey, B., Faust, J., Foote, T., Leibs, J., Wheeler, R., and Ng, A.Y. (2009). Ros: an open-source robot operating system. In ICRA workshop on open source software, volume 3, 5. Kobe. Reif, J.H. and Wang, H. (1999). Social potential fields: A distributed behavioral control for autonomous robots. Robotics and Autonomous Systems, 27(3), 171–194. Rodr´ıguez-Seda, E.J., Troy, J.J., Erignac, C.A., Murray, P., Stipanovi´c, D.M., and Spong, M.W. (2010). Bilateral teleoperation of multiple mobile agents: Coordinated motion and collision avoidance. IEEE Transactions on Conttrol Systems Technology, 18(4), 984–992. Sabattini, L., Secchi, C., Capelli, B., and Fantuzzi, C. (2018). Passivity preserving force scaling for enhanced teleoperation of multi-robot systems. IEEE Robotics and Automation Letters. Sabattini, L., Secchi, C., and Fantuzzi, C. (2011). Arbitrarily shaped formations of mobile robots: artificial potential fields and coordinate transformation. Autonomous Robots (Springer), 30(4), 385–397. doi:10.1007/s10514011-9225-4. Sabattini, L., Secchi, C., and Fantuzzi, C. (2017). Achieving the desired dynamic behavior in multi-robot systems interacting with the environment. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Singapore. Secchi, C., Sabattini, L., and Fantuzzi, C. (2015). Conducting multi-robot systems: Gestures for the passive teleoperation of multiple slaves. In 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2803–2808. doi: 10.1109/IROS.2015.7353762. Secchi, C., Stramigioli, S., and Fantuzzi, C. (2007). Control of interactive robotic interfaces: A port-Hamiltonian approach, volume 29. Springer Science & Business Media.
ScienceDirect
IFAC PapersOnLine 51-3 (2018) 143–148
Shaping the Force Feedback by Dynamic Shaping the Force Feedback by Dynamic Shaping the Force Feedback by Dynamic Shaping the Force Feedback by Dynamic Scaling in the Teleoperation of Multi-Robot Shaping the Force Feedback by Dynamic Scaling in Teleoperation of Multi-Robot Scaling of Multi-Robot Scaling in in the the Teleoperation Teleoperation of Multi-Robot Systems Scaling in the Teleoperation of Multi-Robot Systems Systems Systems Systems Lorenzo Sabattini ∗∗∗ Cesare Fantuzzi ∗∗∗ Cristian Secchi ∗∗∗
Lorenzo Sabattini ∗ Cesare Fantuzzi ∗ Cristian Secchi ∗ Lorenzo Lorenzo Sabattini Sabattini ∗ Cesare Cesare Fantuzzi Fantuzzi ∗ Cristian Cristian Secchi Secchi ∗ ∗ Lorenzo Sabattini ∗ Cesare Fantuzzi ∗ Cristian Secchi ∗ ∗ Department of Sciences and Methods for Engineering (DISMI), ∗ ∗ Department of Sciences and Methods for Engineering (DISMI), ∗ Department of Sciences and Methods for (DISMI), University Modena and Reggio Italy Department for Engineering Engineering (DISMI), Universityofof ofSciences Modenaand andMethods Reggio Emilia, Emilia, Italy (e-mail: (e-mail: ∗ University of Modena and Reggio Emilia, Italy (e-mail: Department of Sciences and Methods for Engineering (DISMI), {lorenzo.sabattini, cristian.secchi, cesare.fantuzzi} @ unimore.it). University of cristian.secchi, Modena and Reggio Emilia, Italy {lorenzo.sabattini, cesare.fantuzzi} @ (e-mail: unimore.it). {lorenzo.sabattini, cristian.secchi, cesare.fantuzzi} @ (e-mail: unimore.it). University of cristian.secchi, Modena and Reggio Emilia, Italy {lorenzo.sabattini, cesare.fantuzzi} @ unimore.it). {lorenzo.sabattini, cristian.secchi, cesare.fantuzzi} @ unimore.it). Abstract: Abstract: In In passivity passivity based based bilateral bilateral teleoperation teleoperation of of multi-robot multi-robot systems systems the the coupling coupling among among Abstract: In passivity based bilateral teleoperation of multi-robot systems the coupling among the robots produces unwanted dynamic effects that deteriorates the force fed back to user. Abstract: In passivity based bilateral teleoperation of multi-robot systems the coupling the robots produces unwanted dynamic effects that deteriorates the force fed back to the theamong user. the robots produces unwanted dynamic effects that deteriorates the force fed back to the user. Abstract: In passivity based bilateral teleoperation of multi-robot systems the coupling among In this paper we propose a passivity based dynamic scaling strategy for compensating these the robots produces unwanted dynamic effects that deteriorates the force fed back to the user. In this paper we propose a passivity based dynamic scaling strategy for compensating these In this paper we propose a passivity based dynamic scaling strategy for compensating these the robots produces unwanted dynamic effects that deteriorates the force fed back to the user. disturbing terms. The effectiveness of the proposed methodology is experimentally validated. In this paper we The propose a passivity based dynamic scaling strategy for compensating these disturbing terms. effectiveness of the proposed methodology is experimentally validated. disturbing terms. The effectiveness of the proposed methodology is experimentally validated. In this paper we propose a passivity based dynamic scaling strategy for compensating these disturbing terms. The effectiveness proposed methodology experimentally validated. © 2018, IFAC (International Federation of of the Automatic Control) Hosting byis Ltd. All rights reserved. disturbing terms. The effectiveness of the proposed methodology isElsevier experimentally validated. Keywords: Keywords: Telerobotics, Telerobotics, Networked Networked robotic robotic system system modeling modeling and and control, control, Decentralized Decentralized Keywords: Telerobotics, control. Keywords: Telerobotics, Networked Networked robotic robotic system system modeling modeling and and control, control, Decentralized Decentralized control. control. Keywords: Telerobotics, Networked robotic system modeling and control, Decentralized control. control. 1. INTRODUCTION In passivity based teleoperation architecture, the multi1. INTRODUCTION In passivity based teleoperation architecture, the multi1. In teleoperation architecture, the multirobot systembased is controlled to behave as a passive system 1. INTRODUCTION INTRODUCTION In passivity passivity based teleoperation architecture, the system multirobot system is controlled to behave as a passive robot system is controlled to behave as a passive system 1. INTRODUCTION In passivity based teleoperation architecture, multiand this is often achieved by interconnecting the agents robot system is controlled to behave as a passive system and this is often achieved by interconnecting the agents and this is often achieved by interconnecting the agents robot system is controlled to behave as a passive system by virtual springs and dampers. In this way the controlled andvirtual this issprings often achieved by interconnecting agents by and dampers. In this way thethe controlled Multi-robot systems have proven to be very effective for by and dampers. In the controlled and this issprings often by agents Multi-robot systems have proven to be very effective for multi-robot systemachieved behaves as ainterconnecting physical system whose dyby virtual virtual springs and dampers. In this this way way thethe controlled Multi-robot systems have proven to be very effective for aaMulti-robot large class of application as, e.g., surveillance and data multi-robot system behaves as a physical system whose dyhave proven be very effective for multi-robot large class systems of application as, e.g.,tosurveillance and data system behaves as aa physical system whose dyby virtual springs and dampers. In this way the controlled namics is felt by the user as a force. Because of the (many) multi-robot system behaves as physical system whose dyacollection of application as, and Multi-robot systems have proven tosurveillance be very for namics is felt by the user as a force. Because of the (many) and the problem of e.g., modeling andeffective controlling a large large class class of the application as, e.g., surveillance and data data namics is felt by the user as aa when force. Because of the (many) multi-robot system behaves as a physical system whose dycollection and problem of modeling and controlling couplings among the agents, the user is moving the namics is felt by the user as force. Because of the (many) collection and the problem of modeling and controlling couplings among the agents, when the user is moving the a largesystems class application as, surveillance and data these has been widely addressed by the robotics collection andofhas the problem of e.g., modeling and controlling couplings among the agents, when the user is moving the namics is felt by the user as a force. Because of the (many) these systems been widely addressed by the robotics group of robots s/he feels a disturbing effect which precouplings among s/he the agents, when the user is moving the these systems has been widely addressed by the robotics group of robots feels a disturbing effect which precollection and the problem of modeling and controlling community, see (Doriya et al., 2015) for a recent survey. these systems has been widely addressed by the survey. robotics group of s/he feels disturbing effect which precouplings among theunderstanding agents, the user moving the community, see (Doriya et al., 2015) for a recent vents her/him from what is is exactly going group her/him of robots robots s/he feels aa when disturbing effect whichgoing precommunity, see (Doriya et al., 2015) for a recent survey. these systems has been widely addressed by the robotics vents from understanding what is exactly community, see (Doriya et al., 2015)offor a tasks recentincreases, survey. vents her/him from understanding what is exactly group of goal robots s/he feels a isdisturbing effect whichgoing preon. The of this paper to develop a strategy for Nevertheless, when the complexity the vents her/him from understanding what is exactly going community, see (Doriya et al., 2015)offor a tasks recentincreases, survey. on. The goal of this paper is to develop strategy for Nevertheless, when the complexity the The goal of this paper is to develop strategy for Nevertheless, when complexity of the tasks increases, vents her/him from understanding what is aaaexactly going compensating this undesired dynamic effect. We consider the intervention of the human is mandatory. Using tele- on. on. The goal of this paper is to develop strategy for Nevertheless, when the complexity of the tasks increases, compensating this undesired dynamic effect. We consider the intervention of the complexity human is mandatory. telethis undesired effect. consider the of is mandatory. Using teleon. The of paper isdynamic toteleoperates develop a We strategy for Nevertheless, when of high-level the tasksUsing increases, the case in the operator directly one operation, it possible to cognitive compensating thisthis undesired dynamic effect. We consider the intervention intervention of the the human human is the mandatory. Using tele- compensating the case goal in which which the operator teleoperates directly one operation, it is is possible to inject inject the high-level cognitive the case in which the operator teleoperates directly one operation, it is possible to inject the high-level cognitive compensating this undesired dynamic effect. We consider the intervention of the human is mandatory. Using teleof the robots, as in (Franchi et al., 2012c). Exploiting abilities of the human into the control of a robotic system the case in which the operator teleoperates directly one operation, it ishuman possible to the inject the high-level cognitive of the robots, as in (Franchi et al., 2012c).directly Exploiting abilities of the into control of a robotic system robots, as (Franchi et al., abilities of the into control of robotic system the case in recently which the operatorin one operation, it possible to the inject theuser high-level cognitive results proposed (Sabattini et Exploiting al., 2017, (Secchi et al., 2007). Since a single cannot directly of the the robots, as in in (Franchi etteleoperates al., 2012c). 2012c).et Exploiting abilities et of al., theishuman human into the control of aacannot robotic system of the results recently proposed in (Sabattini al., 2017, (Secchi 2007). Since a single user directly the results recently proposed in (Sabattini et al., 2017, (Secchi et al., 2007). Since a single user cannot directly of the robots, as in (Franchi et al., 2012c). Exploiting abilities of the human into the control of a robotic system 2018), we propose a passivity preserving strategy for tunteleoperate each robot of a multi-robot system, it is necthe results recentlya passivity proposed preserving in (Sabattini et al., (Secchi et al., 2007). a single usersystem, cannotitdirectly we propose strategy for2017, tunteleoperate each robotSince of a multi-robot is nec- 2018), 2018), we propose aa passivity preserving strategy for tunteleoperate each robot of system, it is the results recently proposed in (Sabattini al., (Secchi al., 2007). acontrol single user cannotwhere the coupling among the in to essary to to the shared paradigm, the 2018), we propose passivity preserving strategy for2017, tunteleoperate each of aa multi-robot multi-robot system, itdirectly is necnecing the coupling among the robots robots in order order toetcompensate compensate essary toetresort resort torobot the Since shared control paradigm, where the ing the coupling robots in to essary resort the control paradigm, where the 2018), propose a passivity preserving strategy for tunteleoperate each robot of with a multi-robot system,for it dealing is necdisturbing effectsamong due tothe the dynamics of thecompensate controlled fleet ofto robots isto endowed some autonomy ing thewe coupling among thethe robots in order order to compensate essary to resort to the shared shared control paradigm, where the ing disturbing effects due to dynamics of the controlled fleet of robots is endowed with some autonomy for dealing effects due to dynamics the controlled fleet robots endowed with some for dealing ing the coupling among robots in order to essary to resort to the (e.g. shared control paradigm, where the disturbing multi-robot system while preserving theof coupling among with low levelis tasks maintenance of a cooperative disturbing effects due tothethe the dynamics ofcoupling thecompensate controlled fleet of oflow robots istasks endowed with some autonomy autonomy for dealing multi-robot system while preserving the among with level (e.g. maintenance of a cooperative multi-robot system while preserving the coupling among with low level tasks (e.g. maintenance of a cooperative disturbing effects due to the dynamics of the controlled fleet of robots is endowed with some autonomy for dealing the robots. behavior, collision avoidance) and the user teleoperates multi-robot among with low level tasks (e.g. maintenance of a teleoperates cooperative the robots. system while preserving the coupling behavior, collision avoidance) and the user behavior, collision avoidance) and teleoperates multi-robot with low as level tasks (e.g. maintenance of ateleoperating cooperative the the fleet aa whole. Several strategies for the robots. robots. system while preserving the coupling among behavior, collision avoidance) and the the user user teleoperates the fleet as whole. Several strategies for teleoperating the fleet aa system whole. Several strategies for teleoperating the robots. behavior, avoidance) and the user teleoperates Notation athe multi-robot have been recently proposed in the 1.1 fleet as ascollision whole. Several strategies for teleoperating 1.1 Notation a multi-robot system been recently proposed in the 1.1 Notation aliterature. multi-robot system have recently proposed in the the fleet asIna (Palafox whole. have Several strategies teleoperating and been Spong, 2009;for Rodr´ ıguez-Seda 1.1 Notation a multi-robot system have been recently proposed in the literature. In (Palafox and Spong, 2009; Rodr´ ıguez-Seda 1.1 NotationIm ∈ Rm×m literature. In and Spong, 2009; Rodr´ ıguez-Seda a multi-robot system have recently in the The indicate the identity matrix et al., strategy for teleoperating the m×m will literature. In aa(Palafox (Palafox and Spong, 2009; proposed Rodr´ ıguez-Seda Rm×m matrix The symbol symbol Im et al., 2010) 2010) strategy for been teleoperating the multi-robot multi-robot m×m will indicate the identity m ∈ m×n m×mthe ∈ R will indicate the matrix The symbol I et al., 2010) a strategy for teleoperating the multi-robot literature. In (Palafox and Spong, 2009; Rodr´ ıguez-Seda of dimension m, and symbol O ∈ R will system as an overall rigid body, constraining the robots on m m×n m,n identity m×n Thedimension symbol Imm, ∈R will indicate the identity matrix et al., 2010) a strategy for teleoperating thethe multi-robot of and the symbol O ∈ R will m,n system as an overall rigid body, constraining robots on m×mthe symbol Om,n ∈ Rm×n m×n of dimension m, and will system as an overall rigid body, constraining robots on The symbol I ∈ R will indicate the identity matrix al., 2010) a strategy teleoperating thethe multi-robot indicate the null matrix of dimension m × n. For ease of aet specific formation, hasfor been proposed. In (Cheung and m,n m of dimension m, and the symbol O ∈ R system as an overall rigid body, constraining the robots on indicate the null matrix of dimension m,n m × n. For easewill of a specific formation, has been proposed. In (Cheung and m×n indicate the null matrix of dimension m × n. For ease of aChung, specific formation, has been proposed. In (Cheung of dimension m, and the symbol O ∈ R will system as an overall rigid body,controller constraining the robotsand on notation, we will omit the dimension of the matrices when 2009) an impedance for teleoperating m,n indicate the null matrix of dimension m × n. For ease of a specific formation, has been proposed. In (Cheung and notation, we will omit the dimension of the matrices when Chung, 2009) an impedance controller for teleoperating ρ×σ notation, we will omit the dimension of the matrices when Chung, 2009) an impedance controller for teleoperating indicate the null matrix of dimension m × n. For ease ofa specific formation, has been proposed. In (Cheung and they appear clearly from the context. Let Ω ∈ R be aaChung, group of robots in a leader-follower modality has been ρ×σ we clearly will omit thethe dimension theΩmatrices when an impedance controllermodality for teleoperating they appear from context.ofLet ∈ Rρ×σ be a group 2009) of robots in a leader-follower has been notation, ρ×σ ρ×σ they appear clearly from the context. Let Ω ∈ R be apresented. group of robots in a leader-follower modality has been notation, we will omit the dimension of the matrices when Chung, 2009) an impedance controller for teleoperating generic matrix. Then, we define Ω [i, j] ∈ R as the element Passivity based control has been successfully appear clearly from the context. Rρ×σ be a a a group of Passivity robots in based a leader-follower modality has been they generic matrix. Then, we define Ω [i, j] Let ∈ RΩas∈the element presented. control has been successfully generic matrix. Then, we Ω ∈ presented. control been from the context. R element be a a group ofinPassivity robots in based a leader-follower modality has since been they (i, of Ω. exploited single-master/single-slave teleoperation generic Then, we define define Ω [i, [i, j] j] Let ∈R RΩas as∈the the element presented. Passivity based control has hasteleoperation been successfully successfully (i, j) j) appear of matrix. Ω. clearly exploited in single-master/single-slave since (i, Ω. exploited in single-master/single-slave since Then, we define Ω [i, j] ∈ R as the element presented. based control hasteleoperation been it allows to guarantee a stable behavior in thesuccessfully interaction (i, j) j) of of matrix. Ω. exploited inPassivity single-master/single-slave teleoperation since generic it allows to guarantee a stable behavior in the interaction it allows to guarantee a stable behavior in the interaction (i, j) of Ω. exploited in single-master/single-slave teleoperation since 2. with any, possibly unknown, passive environment (Secchi it allows guarantee a stablepassive behavior in the interaction 2. PROBLEM PROBLEM STATEMENT STATEMENT with any,topossibly unknown, environment (Secchi 2. with any, possibly unknown, passive environment (Secchi it allows to guarantee a stable behavior in the interaction et al., 2007). Passivity based teleoperation has been also 2. PROBLEM PROBLEM STATEMENT STATEMENT with any, possibly unknown, passive environment (Secchi et al., 2007). Passivity based teleoperation has been also et 2007). based teleoperation has also 2. PROBLEM STATEMENT with possibly unknown, passive environment (Secchi exploited for Passivity teleoperating multi-robot systems. In (Lee et al., al.,any, 2007). Passivity basedmulti-robot teleoperation has been been also We We consider consider the the teleoperation teleoperation of of aa multi-robot multi-robot system: system: exploited for teleoperating systems. In (Lee exploited for teleoperating multi-robot In (Lee We consider the teleoperation of aa the multi-robot system: al., Passivity based teleoperation been alsoa at et al., 2007). 2013) the robots are controlled systems. to has behave as the master side, the user moves master exploited for teleoperating multi-robot systems. In (Lee We consider the teleoperation of multi-robot system:a at the master side, the user moves the master robot, robot, a et al., 2013) the robots are controlled to behave as a et al., 2013) the robots are controlled to behave as a at the master side, the user moves the master robot, a exploited for teleoperating multi-robot systems. In (Lee We consider the teleoperation of a multi-robot system: passive deformable object that can be teleoperated by the haptic device by means of which s/he can teleoperate the et al., 2013) the robots to behavebyasthea at the device masterby side, the ofuser moves robot, a passive deformable object are that controlled can be teleoperated haptic means which s/hethe canmaster teleoperate the passive deformable object that can be teleoperated by the haptic device by means of which s/he can teleoperate the et al., 2013) the robots are controlled to behave as a at the master side, the user moves the master robot, a user, in (Franchi et al., 2012c,a) energy tanks (Ferraguti robot and receive an informative force feedback from passive deformable object that canenergy be teleoperated by the haptic device by means of which s/he teleoperate robot and receive an informative forcecan feedback from the user, in (Franchi et al., 2012c,a) tanks (Ferraguti the user, (Franchi et al., 2012c,a) energy tanks (Ferraguti and receive an informative force feedback from the passive deformable object that can be teleoperated bywith the robot haptic device by means of which s/he can teleoperate et al.,in 2015) have been exploited for passively dealing slave side. The slave side consists of the multi-robot system user, in (Franchi et al., 2012c,a) energy tanks (Ferraguti robot and receive an informative force feedback from the slave side. The slave side consists of the multi-robot system et al.,in 2015) have been for passively dealing with et have exploited for passively dealing with slave side. The slave consists of the multi-robot system user, (Franchi et al.,exploited 2012c,a) energy tanks (Ferraguti robot and receive anside informative feedback from the variable topology during the and in the are virtually coupled their et al., al., 2015) 2015) have been been exploited for passively dealing with where slave The slave side consists offorce thewith multi-robot system variable topology during the teleoperation teleoperation and in (Secchi (Secchi whereside. the robots robots are virtually coupled with their neighbors neighbors variable topology during the teleoperation and in (Secchi where the robots are virtually coupled with their neighbors al., 2015) have been exploited for passively dealing with slave side. The slave side consists of the multi-robot system et a passivity based strategy for conducting the through control actions defined to achieve some desired variable topology duringbased the teleoperation and in (Secchi thecontrol robots actions are virtually coupled with their neighbors et al., 2015) a passivity strategy for conducting the where through defined to achieve some desired et passivity based strategy for the through actions defined to some desired variable topology during the teleoperation and in (Secchi where thecontrol robots are virtually coupled with their neighbors robots over aaaperiodic trajectory has been proposed. behavior. communication delay between the et al., al., 2015) 2015) passivity based strategy for conducting conducting the cooperative through control actions defined to achieve achieve some desired cooperative behavior. communication delay between the robots over a periodic trajectory has been proposed. robots over has proposed. cooperative behavior. communication delay between the et al., 2015) passivitytrajectory based strategy for conducting control actions defined to achieve desired robots over aaaperiodic periodic trajectory has been been proposed. the through cooperative behavior. communication delay some between the robots over a periodic trajectory has been proposed. cooperative behavior. communication delay between the 2405-8963 © 2018 2018, IFAC IFAC (International Federation of Automatic Control) Copyright 143 Hosting by Elsevier Ltd. All rights reserved. Copyright 2018 IFAC 143 Control. Peer review© responsibility of International Federation of Automatic Copyright © under 2018 IFAC IFAC 143 Copyright © 2018 143 10.1016/j.ifacol.2018.06.041 Copyright © 2018 IFAC 143
IFAC LHMNC 2018 144 Valparaíso, Chile, May 1-4, 2018
Lorenzo Sabattini et al. / IFAC PapersOnLine 51-3 (2018) 143–148
master and the slave side is negligible. We consider a passive teleoperation system. Thus, the coupling among the robots is designed in order to achieve a passive behavior of the overall group. Furthermore, master and slave sides are joined by means of a passive coupling by means of which the user can directly teleoperate one of the robots and through which a force feedback can be transmitted to the operator. Using the formulation proposed in (Sabattini et al., 2017), we will develop a passivity preserving strategy for dynamical tuning the inter-robot coupling forces in order to compensate the effects of the couplings on the force fed back to the operator and to allow the user to teleoperate the robot it is connected to as if it were a simple floating mass, characterized by desired values of inertia and damping. 3. SLAVE SIDE: THE MULTI-ROBOT SYSTEM The slave side consists of a group of N robots moving in a m-dimensional environment, whose dynamics are modeled as follows: mi x ¨i = wi i = 1, . . . , N (1) where xi ∈ Rm is the ith robot’s position, mi > 0 is the ith robot’s mass, and wi ∈ Rm collects control inputs and all the external forces each robot is subject to. Each robot is controlled, by means of a local control strategy, in such a way that the overall multi-robot system achieves some common cooperative objective. We consider the case where the robots can exchange information among each other if their distance is smaller than the communication radius R > 0. The neighborhood Ni (t) of the ith robot is then the set of robots that can communicate with robot i at time t, that is (2) Ni (t) = {j �= i such that �xi,j (t)� ≤ R} where xi,j = xi − xj . Hence, consider the control input wi in (1) to be defined as the composition of different terms, namely: N wi = Fi,j − bi x˙ i + Fie + Fis (3) j=1,j =i
3.1 Control input terms Coupling control terms The cooperative behavior is achieved by means of the term Fi,j ∈ Rm , which represents the coupling between the ith robot and its neighbors. In particular, in this paper we assume the coupling term Fi,j to be defined as the composition of an elastic interconnection and an inter-robot damping, namely: N N Fi,j = − ∇Vi,j − βi,j (x˙ i − x˙ j ) (4) j=1,j =i
j=1,j =i
The elastic interconnection in (4) is represented by the N ∇Vi,j , where Vi,j = V (xi,j ) is some approterm − j=1,j =i
priately defined artificial potential field (Sabattini et al., 2011; Leonard and Fiorelli, 2001; Franchi et al., 2012b; Falconi et al., 2015; Reif and Wang, 1999). As in (Leonard and Fiorelli, 2001), we assume the artificial potential field V (xi,j ) to be non-negative and to have a global minimum 144
at the desired distance �xi,j � = δd > 0, and to generate a repulsive force that avoids inter-robot distances to go below the safety value δs , such that 0 < δs < δd . Inter-robot damping is represented in (4) by the term N − βi,j (x˙ i − x˙ j ). In this term, the parameter βi,j is j=1,j =i
defined as follows: βi,j =
β¯ ≥ 0 if j ∈ Ni (t) 0 otherwise
(5)
Together with the coupling artificial potential field, the design of βi,j leads to defining the overall desired behavior of the multi-robot system. Notice that, because of the shape of the considered potential function and of the damping parameter (5), (4) can be implemented in a decentralized way since, if j ∈ / Ni (t), then Fi,j = 0. Individual robot control terms The remaining terms that compose the input wi in (3) are computed independently for each robots, and do not explicitly depend on the neighbors. In particular, the term −bi x˙ i ∈ Rm , with bi > 0, represents a local damping, that describes the viscous friction each robot is subject to, and any possible additional damping added by the control law. The terms Fis , Fie ∈ Rm represent the teleoperation and the force due to the interaction with the environment respectively. Fis = 0m if the corresponding robot is not directly teleoperated. Controlled multi-robot system model Considering now the dynamics of the ith robot given in (1), the generic input defined in (3) with the coupling term specified in (4), the following dynamic model is achieved: mi x ¨i + bi x˙ i +
N
∇Vi,j +
j=1,j =i
N
βi,j (x˙ i − x˙ j ) = Fie + Fis (6)
j=1,j =i
3.2 Passivity of the slave side In order to prove the passivity of the multi-robot system at the slave side, it is instrumental to rewrite its model in port-Hamiltonian form. Let then pi = mi x˙ i be the ith robot’s momentum. The dynamics (6) of the multi-robot system at the slave side can then be rewritten as follows: N N pj pi pi p˙ i + bi + ∇Vi,j + βi,j − = Fie + Fis mi mi mj j=1,j =i
j=1,j =i
(7) We now introduce the total energy of the system H, defined as follows: N N N Ki (pi ) + Vi,j ≥ 0 (8) H= i=1
i=1 i=j,j =i
where pTi pi 2mi is the kinetic energy associated to robot i. Define now the following vectors: Ki (pi ) =
IFAC LHMNC 2018 Valparaíso, Chile, May 1-4, 2018
Lorenzo Sabattini et al. / IFAC PapersOnLine 51-3 (2018) 143–148
�T � F e = F1e T . . . FNe T ∈ RmN � �T (9) F s = F1s T . . . FNs T ∈ RmN � � T T p = p1 . . . pN T ∈ RmN ¯ = N (N −1)/2, and define the following Moreover, let N vectors: � �T �T � ¯ χ = χ 1 T . . . χN T = x T . . . xN −1,N T ∈ RmN � �T � 1,2 �T ¯ β = β 1 T . . . βN T = β1,2 T . . . βN −1,N T ∈ RmN (10)
Furthermore, define the following velocity vector � �T v = x˙ T1 . . . x˙ TN ∈ RmN and define the following matrix �T � G = ImN OTmN¯ ,mN
(11) (12)
¯
Define then IG ∈ RN ×N as the incidence matrix (Godsil and Royle, 2001) of the complete graph among the robots: subsequently, the weighted Laplacian matrix can be computed as ¯ IGT L β = IG B (13) ¯ defined as with the weight matrix B ¯ = diag (β) B (14) Defining (15) B = (Lβ + diag (b1 , . . . , bN )) ⊗ Im and (16) I = IG ⊗ I m it is possible to rewrite the dynamics (6) of the multi-robot system in port-Hamiltonian form as follows: � � �� ∂H � � �� O I B O ∂p p˙ − + G(F e + F s ) = O O ∂H χ˙ −I T O ∂χ ∂H ∂p v = GT ∂H ∂χ (17) Consider now the total energy of the system H, introduced in (8). For ease of notation, we define now Vk = V (χk ). Hence, it is possible to rewrite (8) as: H=
N � i=1
Ki (pi ) +
¯ N �
Vk ≥ 0
(18)
k=1
As shown in (Franchi et al., 2012b, Proposition 1), (17) is passive with respect to the pair (F e + F s , v). We will consider the case in which only the ith robot will be teleoperated and, therefore, all the components of F s but the Fis one will be zero. 4. TELEOPERATION SYSTEM The teleoperation scheme considered in this paper consists in having the human operator able to impose a force Fis on the ith robot at the slave side, through the master robotic device. We consider the case where the operator can freely choose the robot to be directly teleoperated, and we will hereafter focus, without loss of generality, on the ith robot. 145
145
The user teleoperates the multi-robot system by directly teleoperating only the ith robot whose motion, because of the coupling among the agents, will cause the motion of the overall fleet. The master haptic device can be modeled as a fully actuated Euler-Lagrange system, namely as: � � ¨ m + C Xm , X˙ m X˙ m = Fm + Fh M (Xm ) X (19)
where Xm ∈ Rm is the position of the end effector 1 , X˙ m ∈ Rm is its velocity, M (Xm ) > 0 is the inertia matrix, C(Xm , X˙ m )X˙ m encodes the Coriolis and centrifugal terms. Fm , Fh ∈ Rm are the coupling wrench with the slave side and the wrench applied to the robot by the human operator, respectively. As is well known, see e.g. (Secchi et al., 2007), (19) is passive with respect to the pair (Fm + Fh , X˙ m ).
Finally, we exploit the following elastic coupling for interconnecting master and slave sides: F = −η (XM − xi ) m (20) s Fi = η (XM − xi )
where η > 0 is a tuning gain, that can be freely defined based on the application.
Both master and slave side are passive systems and, furthermore, also the elastic interconnection (20) is passive (Secchi et al., 2007). Since the interconnection of passive systems is still a passive system (Secchi et al., 2007), then the overall teleoperation system is passive. 5. VARIABLE DYNAMIC BEHAVIOR OF THE TELEOPERATED MULTI-ROBOT SYSTEM Considering the dynamics of the multi-robot system at the slave side, given in (6), and considering the coupling with the robotic system at the master side defined in Section 4, the force feedback provided to the human operator is given by: Fm = −Fis = = −(mi x ¨i + bi x˙ i +
N �
∇Vi,j +
j=1,j�=i
N �
βi,j (x˙ i − x˙ j ) − Fie )
j=1,j�=i
(21) Specifically, the force feedback is defined based on the control actions that are implemented on the directly teleoperated robot. Hence, it depends on how robots are coupled among each other. We will now introduce a methodology to scale such couplings, in such a way that the ith robot can be teleoperated as if it were a floating mass md subject to a damping coefficient βd . Namely, we would like the teleoperated multi-robot system to provide a force feedback given by the following desired dynamic model: Fid = md x ¨i + βd x˙ i (22) In this way the operator can feel the motion of the teleoperated robot without disturbing effects due to the coupling. 1 For ease of notation, in this paper we consider that the dimension of xi and Xm are the same, namely xi , Xm ∈ Rm . This condition can however be relaxed introducing opportune projection matrices.
IFAC LHMNC 2018 146 Valparaíso, Chile, May 1-4, 2018
Lorenzo Sabattini et al. / IFAC PapersOnLine 51-3 (2018) 143–148
The motion of the ith robot will keep on causing the motion of the overall fleet because of its interconnection with its neighbors. This can be obtained exploiting the coupling variation methodology introduced in (Sabattini et al., 2017). In particular, we introduce scaling factors, opportunely computed by the ith robot to scale the elastic and damping forces that couple it with neighboring robots. In order to scale the elastic coupling forces, define AG = diag(α1 , . . . , αN¯ ) as a diagonal matrix containing scaling factors, one per each pair of robots. To enable the ith robot to scale the forces that couple it with its neighbors, the elements of AG can be defined as follows: � α(t) > 0 if IG [i, k] �= 0 and �χk � ≤ R αk = (23) 1 otherwise Along the same lines, scaling factors can be introduced for modulating the inter-agent damping, defining matrix CG = diag (c1 , . . . , cN¯ ) as a diagonal matrix containing a scaling factor per each pair of robots, defined as follows: � γ(t) > 0 if IG [i, k] �= 0 and �χk � ≤ R (24) ck = 1 otherwise In this manner, the force feedback (21) is modified as follows: Fis = mi x ¨i + bi x˙ i + α(t)
N �
∇Vi,j + γ(t)
j=1,j�=i
N �
βi,j (x˙ i − x˙ j ) − Fie
j=1,j�=i
(25) It is worth noting that the elastic coupling term between any two robots is only a function of their relative positions. Hence, it is always possible to write it as follows: ∇Vi,j = κi,j (xi , xj ) (xi − xj ) (26) In order to keep the notation simple, in the following we will omit the dependency of κi,j on xi , xj . The force feedback in (25) can then be rewritten as follows: Fis = mi x ¨i + bi x˙ i + α(t)
N �
κi,j (xi − xj ) + γ(t)
j=1,j�=i
N �
βi,j (x˙ i − x˙ j ) − Fie
j=1,j�=i
(27) Tuning parameters can then be selected to implement the desired dynamic behavior (22). However, it is worth noting that such parameters can not be chosen in a completely arbitrary manner. In fact, their introduction can lead to breaking the passivity property. This fact can be shown starting from the port-Hamiltonian model of the multi-robot system, given in (17) for the unscaled coupling forces. Scaled coupling forces are exchanged in the multi-robot system according to the following matrices, respectively: (IG AG (t)) ⊗ Im (28) and B(t) = (L¯β + diag (b1 , . . . , bN )) ⊗ Im (29) where � � ¯ IGT L¯β = IG CG B (30)
Exploiting the properties of the Kronecker product, it is possible to obtain: (IG AG (t)) ⊗ Im = (IG AG (t)) ⊗ (Im Im ) = = (IG ⊗ Im ) (AG (t) ⊗ Im ) = IA(t) (31) where A(t) = AG (t) ⊗ Im . 146
Introducing then the scaling matrices into the dynamics of the multi-robot system (17), we obtain the following: � � �� ∂H � � �� p˙ O IA(t) B(t) O ∂p − + G(F e + F s ) χ˙ = O O ∂H −I T O ∂χ ∂H ∂p v = GT ∂H ∂χ
(32) Comparing (32) with (17), it is possible to note that the introduction of the scaling matrices leads to modifying the structure of the dynamic model of the multi-robot system. These changes lead to the fact that passivity can no longer be trivially guaranteed, as discussed in Section 3.2. In particular, while B plays the same role as B (being both matrices positive definite), the introduction of matrix A(t) leads to breaking the skew-symmetric structure of the interconnection matrix. However, exploiting the results of (Sabattini et al., 2017), passivity can still be guaranteed. To this aim, (32) can be rewritten as follows:
∂H � � �� � � �� O I B(t) O ∂p p˙ e s − = ∂H + G(F + F ) χ˙ O O −I T O A(t) ∂χ ∂H T ∂p v = G ∂H ∂χ
(33) Passivity can then be guaranteed utilizing the results of (Sabattini et al., 2017, Proposition 2), considering the following modified energy function: Hs =
N � i=1
Ki (pi ) +
¯ N �
α k Vk ≥ 0
(34)
k=1
The sketch of the proof is given in the following. Proposition 1. Consider the dynamics of the multi-robot system described in port-Hamiltonian form in (33), and consider the total energy of the system given in (34). Then, if ∃αm , αM ∈ R, 0 < αm < αM , such that αm ≤ α(t) ≤ αM for any time t ≥ 0, then the system is passive with respect to the pair (F e + F s , v). Proof. The time derivative of the energy function (34) can be computed as follows: ∂Hs ∂ T Hs ∂Hs B + (35) H˙ s = (F e + F s )T v − ∂p ∂p ∂t Since matrix B is positive definite, (35) can be integrated as follows: � t � t ∂Hs dτ (36) (F e + F s )T vdτ + Hs (t) − Hs (0) ≤ 0 0 ∂τ Following the same steps of (Sabattini et al., 2017, Proposition 2), since both the kinetic energy Ki (·) and the potentials Vk (·) are positive, passivity can then be proven since the following holds: � t N � (F e + F s )T vdτ ≥ − Ki (0) (37) 0
i=1
✷
IFAC LHMNC 2018 Valparaíso, Chile, May 1-4, 2018
Lorenzo Sabattini et al. / IFAC PapersOnLine 51-3 (2018) 143–148
147
Force feedback
20 10 0 -10 -20
Fig. 2. Force feedback, constant coupling parameters (high) Fig. 1. The experimental setup
In order to achieve the desired dynamic behavior (22), we introduce then the following quadratic optimization problem: minimize f (αi , γi ) subject to αm ≤ αi ≤ αM (38) γi > 0 where αM > αm > 0 are the upper- and lower-bounds for αi (t), respectively, defined according to Proposition 1, and the cost function is defined as follows: 2 (39) f (α, γ) = Fis − Fid
The optimization problem (38) can be solved locally by the ith robot, when teleoperation is being performed. Since coupling forces are defined in a bi-directional manner, the result of the optimization problem needs then to be broadcast to all the ith robot’s neighbors: in this way, forces can be opportunely scaled, as desired. It is worth noting that the result of the optimization problem depends on the current position of the robots: it is then necessary to periodically recompute it (every T > 0 seconds, to be defined based on the application), based on current relative positions. Furthermore, the acceleration measurements are required; removing such a need is currently under investigation. 6. EXPERIMENTS The experimental validation of the proposed teleoperation architecture was performed on a setup, shown in Fig. 1, in which the master robot was represented by a Geomagic Touch haptic device, while the slave side was composed of a group of e-puck robots, which are small-size differential drive wheeled mobile robots. The e-puck robots were constrained to move in a 2 × 1.5 m arena, equipped with an overhead camera that provided localization of the robots. Up to eight robots were used in the experiments. The teleoperation and control architecture was implemented in ROS (Quigley et al., 2009), emulating a decentralized implementation: each robot was exploiting only locally available information, and only one robot exchanged information with the master robotic device. Feedback linerarization (Oriolo et al., 2002) is exploited to model the mobile robots as point masses moving in a bi-dimensional environment. In order to have the same dimensionality in the workspaces of the slave and master sides, we considered the motion of the haptic device along two dimensions only. 147
20 10 0 -10
Fig. 3. Force feedback, constant coupling parameters (low) 20
Force feedback
It is worth noting that Proposition 1 defines constraints on the choice of the scaling parameters that, if satisfied, guarantee passivity preservation.
Force feedback
30
10 0 -10 -20
Fig. 4. Force feedback, variable coupling parameters In these experiments we are not considering the presence of environmental obstacles: as a consequence, in (6), we set the external force to zero, that is Fie = 0, ∀i. Hereafter we report the results of a representative run of the experiment, in which 6 robots were utilized. The dynamic model (6) was defined, for each robot i, using the following parameters: mi = 0.1, bi = 1, β¯ = 1, δs = 1 cm, δd = 20 cm. The desired dynamic model was defined according to (22) with md = 0.1, βd = 1. The experiment was performed in three conditions: with constant coupling parameters, high (α(t) = γ(t) = 30) and low (α(t) = γ(t) = 0.1), in order to verify how the multirobot system feels when applying simple scalings, and with variable parameters computed according to the procedure described in Section 5. The behavior of the teleoperation system can be seen in the accompanying video 2 . Force feedback is depicted in Figs. 2, 3 and 4. It can be seen that both in Fig. 2 and in Fig. 3 there are low frequency oscillations due to the coupling among the robots. In Fig. 4 low level oscillations are much more limited due to the variable coupling. In Figs. 5, 6 and 7 the value of (39) is represented for the three conditions. In case of constant scaling what the user feels is quite distant from (22) because of the effect of the multi-robot system dynamics on the force feedback. When the variable scaling is applied, it is evident that what the user feels is very close to (22). 7. CONCLUSIONS In this paper we have proposed a passivity preserving dynamic scaling strategy for improving the teleoperation of a multi-robot system. Experiments have shown the 2
https://www.youtube.com/watch?v=3vqlrFq6h g
IFAC LHMNC 2018 148 Valparaíso, Chile, May 1-4, 2018
Lorenzo Sabattini et al. / IFAC PapersOnLine 51-3 (2018) 143–148
Cost function
10 7
10 6
10 5
Fig. 5. Cost function, constant coupling parameters (high)
Cost function
10 5
10 0
10 -5
Fig. 6. Cost function, constant coupling parameters (low)
Cost function
10 5
10 0
10 -5
Fig. 7. Cost function, variable coupling parameters effectiveness of the proposed strategy for compensating the spurious effects due to the coupling of the robots. Future work aims at extending the proposed strategy for including in the desired dynamic a term informing about the interaction of the fleet with the environment: results along these lines were recently proposed in (Sabattini et al., 2018). REFERENCES Cheung, Y. and Chung, J.S. (2009). Cooperative control of a multi-arm system using semi-autonomous telemanipulation and adaptive impedance. In 2009 International Conference on Advanced Robotics, 1–7. Doriya, R., Mishra, S., and Gupta, S. (2015). A brief survey and analysis of multi-robot communication and coordination. In International Conference on Computing, Communication Automation, 1014–1021. Falconi, R., Sabattini, L., Secchi, C., Fantuzzi, C., and Melchiorri, C. (2015). Edge-weighted consensus based formation control strategy with collision avoidance. Robotica, 33(02), 332–347. Ferraguti, F., Preda, N., Manurung, A., Bonf, M., Lambercy, O., Gassert, R., Muradore, R., Fiorini, P., and Secchi, C. (2015). An energy tank-based interactive control architecture for autonomous and teleoperated robotic surgery. IEEE Transactions on Robotics, 31(5), 1073–1088. doi:10.1109/TRO.2015.2455791. Franchi, A., Secchi, C., Ryll, M., Bulthoff, H.H., and Giordano, P.R. (2012a). Shared control : Balancing autonomy and human assistance with a group of quadrotor uavs. IEEE Robotics Automation Magazine, 19(3), 57– 68. doi:10.1109/MRA.2012.2205625. Franchi, A., Secchi, C., Son, H.I., Bulthoff, H.H., and Robuffo Giordano, P. (2012b). Bilateral teleoperation of groups of mobile robots with time-varying topology. IEEE Transactions on Robotics, 28(5), 1019–1033. doi: 10.1109/TRO.2012.2196304. 148
Franchi, A., Secchi, C., Son, H.I., Bulthoff, H., and Robuffo Giordano, P. (2012c). Bilateral teleoperation of groups of mobile robots with time-varying topology. IEEE Transactions on Robotics, 28(5), 1019 –1033. Godsil, C. and Royle, G. (2001). Algebraic Graph Theory. Springer. Lee, D., Franchi, A., Son, H.I., Ha, C., Blthoff, H.H., and Giordano, P.R. (2013). Semiautonomous haptic teleoperation control architecture of multiple unmanned aerial vehicles. IEEE/ASME Transactions on Mechatronics, 18(4), 1334–1345. Leonard, N.E. and Fiorelli, E. (2001). Virtual leaders, artificial potentials and coordinated control of groups. In Proceedings of the 40th IEEE Conference on Decision and Control (CDC), volume 3, 2968–2973. IEEE. Oriolo, G., De Luca, A., and Vendittelli, M. (2002). Wmr control via dynamic feedback linearization: design, implementation, and experimental validation. IEEE Transactions on control systems technology, 10(6), 835– 852. Palafox, O. and Spong, M. (2009). Bilateral teleoperation of a formation of nonholonomic mobile robots under constant time delay. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2821–2826. Quigley, M., Conley, K., Gerkey, B., Faust, J., Foote, T., Leibs, J., Wheeler, R., and Ng, A.Y. (2009). Ros: an open-source robot operating system. In ICRA workshop on open source software, volume 3, 5. Kobe. Reif, J.H. and Wang, H. (1999). Social potential fields: A distributed behavioral control for autonomous robots. Robotics and Autonomous Systems, 27(3), 171–194. Rodr´ıguez-Seda, E.J., Troy, J.J., Erignac, C.A., Murray, P., Stipanovi´c, D.M., and Spong, M.W. (2010). Bilateral teleoperation of multiple mobile agents: Coordinated motion and collision avoidance. IEEE Transactions on Conttrol Systems Technology, 18(4), 984–992. Sabattini, L., Secchi, C., Capelli, B., and Fantuzzi, C. (2018). Passivity preserving force scaling for enhanced teleoperation of multi-robot systems. IEEE Robotics and Automation Letters. Sabattini, L., Secchi, C., and Fantuzzi, C. (2011). Arbitrarily shaped formations of mobile robots: artificial potential fields and coordinate transformation. Autonomous Robots (Springer), 30(4), 385–397. doi:10.1007/s10514011-9225-4. Sabattini, L., Secchi, C., and Fantuzzi, C. (2017). Achieving the desired dynamic behavior in multi-robot systems interacting with the environment. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Singapore. Secchi, C., Sabattini, L., and Fantuzzi, C. (2015). Conducting multi-robot systems: Gestures for the passive teleoperation of multiple slaves. In 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2803–2808. doi: 10.1109/IROS.2015.7353762. Secchi, C., Stramigioli, S., and Fantuzzi, C. (2007). Control of interactive robotic interfaces: A port-Hamiltonian approach, volume 29. Springer Science & Business Media.