5th IFAC Workshop on Lagrangian and Hamiltonian Methods 5th IFAC Workshop on 5th IFACLinear Workshop on Lagrangian Lagrangian and and Hamiltonian Hamiltonian Methods Methods for Non Control 5th IFACLinear Workshop on Lagrangian and Hamiltonian Methods for Non Control Available online at www.sciencedirect.com July 4-7, 2015. Lyon, France for Non Linear Control for Non Control July 4-7,Linear 2015. Lyon, France July July 4-7, 4-7, 2015. 2015. Lyon, Lyon, France France
ScienceDirect
IFAC-PapersOnLine 48-13 (2015) 063–068
Formation control of nonholonomic wheeled Formation control of nonholonomic wheeled Formation control of nonholonomic wheeled Formation control of nonholonomic wheeled robots in the presence of matched input robots in the presence of matched input robots in the presence of matched input robots in the disturbances presence of matched input disturbances disturbances disturbances Ewoud Vos ∗,∗∗ Matin Jafarian ∗∗∗ Claudio De Persis ∗∗∗ ∗,∗∗ ∗,∗∗ Ewoud Vos Jafarian De Persis ∗∗∗∗ ∗,∗∗ Matin ∗ Claudio ∗ Ewoud Vos Jafarian De ∗,∗∗ Matin ∗ Claudio Jacquelien M.A. Scherpen Arjan J. van der Schaft ∗∗ ∗ Ewoud Vos Matin Jafarian Claudio De Persis Persis ∗ Jacquelien M.A. Scherpen ∗ Arjan J. van der Schaft ∗∗ ∗∗ ∗ Arjan J. van der Schaft ∗∗ Jacquelien M.A. Scherpen Jacquelien M.A. Scherpen Arjan J. van der Schaft ∗ Engineering and Technology institute Groningen(ENTEG), ∗ ∗ Engineering and Technology institute Groningen(ENTEG), ∗ ∗ Engineering and institute Groningen(ENTEG), University of Groningen, Nijenborgh 4, 9747AG, The Netherlands Engineering and Technology Technology institute Groningen(ENTEG), University of Groningen, Nijenborgh 4, 9747AG, The Netherlands University of Groningen, Nijenborgh 4, 9747AG, (e-mail: {m.jafarian,e.vos,c.de.persis,j.m.a.scherpen}@rug.nl). University of Groningen, Nijenborgh 4, 9747AG, The The Netherlands Netherlands (e-mail: {m.jafarian,e.vos,c.de.persis,j.m.a.scherpen}@rug.nl). {m.jafarian,e.vos,c.de.persis,j.m.a.scherpen}@rug.nl). ∗∗ (e-mail: Johann Bernoulli Institute for Mathematics and Computer Science ∗∗ (e-mail: {m.jafarian,e.vos,c.de.persis,j.m.a.scherpen}@rug.nl). ∗∗ Johann Bernoulli Institute for Mathematics and Computer Science ∗∗ ∗∗(JBI), Johann Bernoulli Institute for Computer Science University Groningen, Nijenborgh 9,and 9747AG Groningen, Johann Bernoulliof Institute for Mathematics Mathematics and Computer Science (JBI), University of Groningen, Nijenborgh 9, 9747AG Groningen, (JBI), University of Groningen, Nijenborgh 9, 9747AG Groningen, The Netherlands (e-mail:
[email protected]). (JBI), The University of Groningen, Nijenborgh 9, 9747AG Groningen, Netherlands (e-mail:
[email protected]). The The Netherlands Netherlands (e-mail: (e-mail:
[email protected]).
[email protected]).
Abstract: This paper presents a new approach for formation keeping control of a network Abstract: This This paper presents presents aa new new approach for for formation keeping keeping control of of aa network network Abstract: of nonholonomic wheeled robots awithin the port-Hamiltonian in theofpresence of Abstract: This paper paper presents new approach approach for formation formation framework keeping control control a network of nonholonomic nonholonomic wheeled robots within within the port-Hamiltonian port-Hamiltonian framework in the the presence presence of of wheeled robots the framework in matched input disturbances. The within formation keeping controller framework drives the network towards of a of nonholonomic wheeled robots the port-Hamiltonian in the presence of matched input input disturbances. disturbances. The The formation formation keeping keeping controller controller drives drives the the network network towards towards aa matched desired formation by assigningThe virtual couplings between the robots, while an internal-modelmatched input disturbances. formation keeping controller drives the network towards a desired formation formation by by assigning assigning virtual virtual couplings couplings between the the robots, while while an an internal-modelinternal-modeldesired based controller designed to locally the disturbance for while each of robots. desired formationis assigning virtualcompensate couplings between between the robots, robots, anthe internal-modelbased controller controller is by designed to locally locally compensate the disturbance disturbance for each each of of the robots. based is designed to compensate the for the robots. based is designed to locally compensate the disturbance for each of All therights robots. © 2015,controller IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. reserved. Keywords: Disturbance rejection, formation control, nonholonomic systems, port-Hamiltonian Keywords: Disturbance rejection, formation control, nonholonomic systems, port-Hamiltonian Keywords: Disturbance control, nonholonomic systems, port-Hamiltonian systems, internal model rejection, control formation Keywords: Disturbance systems, internal internal model rejection, control formation control, nonholonomic systems, port-Hamiltonian systems, model control systems, internal model control 1. INTRODUCTION type of robot is challenging, since it does not satisfy Brock1. INTRODUCTION type of robot is challenging, since it does not satisfy Brock1. type of robot since it satisfy Brockett’s necessary condition for stabilization 1. INTRODUCTION INTRODUCTION type robot is is challenging, challenging, since it does does not not using satisfysmooth Brockett’s of necessary condition for for stabilization using smooth ett’s necessary condition stabilization using smooth Coordination of a network of autonomous robots, which feedback (Brockett (1983)). In this regard, both disconett’s necessary condition forInstabilization using smooth Coordination of a network of autonomous robots, which feedback (Brockett (1983)). this regard, both discon(Brockett In both disconCoordination aa network of robots, which allows for the of execution of complex tasks, has attracted a feedback tinuous control laws(1983)). and time-varying control laws have Coordination network of autonomous autonomous robots, which (Brockett (1983)). In this this regard, regard, both disconallows for for the the of execution of complex complex tasks, has has attracted a feedback tinuous control laws and time-varying control laws have allows execution of tasks, attracted a tinuous control laws and time-varying control laws have great deal of attention in recent years. Formation control is been studied to stabilize a time-varying single robot (e.g. Astolfi (1999) allows for the execution of complex tasks, has attracted a tinuous control laws and control laws have great deal of attention in recent years. Formation control is been studied to stabilize a single robot (e.g. Astolfi (1999) great deal of attention in recent years. Formation control is been studied to stabilize a single robot (e.g. Astolfi (1999) a coordination control problem which aims at controlling and Samson (1993)). The multiple robots case, which is great deal of attention in recent years. Formation control is been studied to stabilize a single robot (e.g. Astolfi (1999) a coordination coordination control problem which aims at controlling and Samson (1993)). The multiple robots case, which is a control problem which aims at controlling and Samson (1993)). The multiple robots case, which is the geometrical shape (and orientation) of the network. naturally more challenging, has also been studied (see e.g. athe coordination problem which aims at network. controlling naturally and Samson (1993)). The multiple robots case, which is geometricalcontrol shape (and (and orientation) of the the more challenging, has also been studied (see e.g. the geometrical shape orientation) of network. naturally more challenging, has also been studied (see e.g. Among differentshape approaches in formation keeping con- naturally Ren and Beard (2007), Do and and Sadowska the geometrical (and orientation) of the network. more challenging, has Pan also (2007), been studied (see e.g. Among different approaches in formation keeping conRen and Beard (2007), Do and Pan (2007), and Sadowska Among different in keeping conRen Beard and Pan (2007), Sadowska trol presented in approaches the literature, Arcak (2007) presented al.and (2012)). To copeDo smooth Among different in formation formation keeping con- et Ren and Beard (2007), (2007), Dowith and the Pan restriction (2007), and andof Sadowska trol presented presented in approaches the literature, literature, Arcak (2007) (2007) presented et al. (2012)). To cope with the restriction of smooth trol in the Arcak presented et al. (2012)). To cope with the restriction of an innovative approach using passivity-based techniques controllers to stabilize both the position and orientation trol presented approach in the literature, Arcak (2007) techniques presented et al. (2012)). To copeboth withthethe restriction of smooth smooth an innovative innovative using passivity-based passivity-based controllers to stabilize position and orientation an approach using techniques controllers to stabilize both the position and orientation which was later extended by passivity-based Bai et al. (2011). In line of the robots, and Beard (2007) considered formation an innovative approach using techniques controllers to Ren stabilize both the position and orientation which was later extended by Bai et al. (2011). In line of the robots, Ren and Beard (2007) considered formation which was later by (2011). the Ren Beard (2007) considered with the passivity-based theal. port-Hamiltonian keeping control of and the front of the robots. formation The front which was later extended extendedapproach, by Bai Bai et et (2011). In In line line of of the robots, robots, Ren and Beardend (2007) considered formation with the the passivity-based approach, theal. port-Hamiltonian keeping control of the front end of the robots. The front passivity-based with approach, the port-Hamiltonian keeping control of the front end of the robots. framework, which is an energy-based modeling framework, end of a wheeled robot lies at a distance L = 0 The alongfront the with the passivity-based approach, the port-Hamiltonian keeping control of the front end of the robots. The front framework, which is an energy-based modeling framework, end of a wheeled robot lies at a distance L = 0 along the framework, which is an energy-based modeling framework, end of a wheeled robot lies at a distance L = 0 along describes a large class of (nonlinear) systems including pasline that is normal to the wheel axle. This assumption framework, whichclass is anofenergy-based modeling framework, a wheeled robot a distance L = assumption 0 along the the describes aa large large (nonlinear) systems systems including pas- end line of that is normal normal to lies the at wheel axle. This This describes class of (nonlinear) including pasline that is to the wheel axle. assumption sive mechanical systems (see van der Schaft problem, however, is of high practical describes a large and classelectrical of (nonlinear) systems including pas- simplifies line that isthe normal to the wheel it axle. This assumption sive mechanical and electrical systems (see van der Schaft simplifies the problem, however, it is of high practical der sive and (see the problem, however, it high practical and mechanical Jeltsema (2014)). Ortegasystems et al. (2002) showed that simplifies interests due numerous applications sive and electrical electrical (see van van der Schaft Schaft the to problem, however, it is is of of(e.g. highcontrolling practical and mechanical Jeltsema (2014)). (2014)). Ortegasystems et al. al. (2002) (2002) showed that simplifies interests due to numerous applications (e.g. controlling and Jeltsema Ortega et showed that interests due to numerous applications (e.g. controlling the use of energy based-models provides a clear physical the gripper position located at the front end of a wheeled and Jeltsema (2014)). Ortega et al. (2002) showed that interests due to numerous applications (e.g. controlling the use use of of energy energy based-models based-models provides provides aa clear physical the gripper position located at the front end of a wheeled the clear physical the gripper position located at the front end of a wheeled interpretation for engineering problems. Recently, van der robot). Vos et al. (2014) considered a similar approach the use of energy based-models providesRecently, a clear physical the gripper locatedconsidered at the front end of aapproach wheeled interpretation for engineering engineering problems. van der robot). Vos position et al. al. (2014) (2014) a similar similar interpretation for problems. Recently, van der robot). Vos et considered a approach Schaft and Maschke (2013) introduced the concept van of portin the port-Hamiltonian framework using passivity-based interpretation for engineering problems. Recently, der robot). Vos et al. (2014)framework consideredusing a similar approach Schaft and Maschke (2013) introduced the concept of portin the port-Hamiltonian passivity-based (2013) introduced the concept Schaft and of the framework Hamiltonian of systems on graphs, which for in design techniques. Schaft and Maschke Maschke (2013) introduced theprovides concepttools of portportthe port-Hamiltonian port-Hamiltonian framework using using passivity-based passivity-based Hamiltonian of systems systems on graphs, graphs, which provides tools for in design techniques. Hamiltonian of on which provides tools for design techniques. the analysis of (complex) networks. Preliminary results Another practical challenge in the field of formation keepHamiltonian of systems on graphs, which provides tools for design techniques. the analysis analysis of of (complex) networks. networks. Preliminary Preliminary results for for Another practical challenge in the field of formation keepthe practical field formation keepformation controlnetworks. in the port-Hamiltonian frameing is to reach andchallenge maintain in thethe desired the analysiskeeping of (complex) (complex) Preliminary results results for Another Another practical challenge in the field of offormation formationdespite keepformation keeping control in in the the port-Hamiltonian port-Hamiltonian frameing is to reach and maintain the desired formation despite formation keeping control frameing is to reach and maintain the desired formation despite work were keeping presented in Vosinetthe al.port-Hamiltonian (2012, 2014). input disturbances. In this the regard, different approaches formation control frame- ing is to reach and maintain desired formation despite work were presented in Vos et al. (2012, 2014). input disturbances. In this regard, different approaches work were in Vos 2014). disturbances. this different approaches One of thepresented important in formation control input have been pursued inIn to deal with disturbances work were in components Vos et et al. al. (2012, (2012, 2014). disturbances. Inliterature this regard, regard, different approaches One of of thepresented important components in formation formation control input have been pursued in literature to deal with disturbances One the important components in control have been pursued in literature to deal with disturbances problems are the dynamics of the agents. In particular, for some classes of dynamical systems. De Persis and One of the important components in formation control have been pursued in literature to deal with disturbances problems are are the the dynamics dynamics of of the the agents. agents. In In particular, particular, for for some some classes classes of of dynamical dynamical systems. systems. De De Persis Persis and and problems there has been a strong interest in formation control of Jayawardhana (2014) discussed the role of the interproblems dynamics of theinagents. In particular, some classes of dynamical systems. De Persis and there has has are beenthe a strong strong interest formation control of of for Jayawardhana (2014) discussed the role of the interthere been a interest in formation control Jayawardhana (2014) discussed the role of the internonholonomic robots. Such are subject nal model principle the passivity property deal there has beenwheeled a strong interest in robots formation control to of Jayawardhana (2014)and discussed the role of theto internonholonomic wheeled robots. Such robots are subject to nal model principle and the passivity property to deal nonholonomic robots. are to principle the passivity property to a nonholonomicwheeled constraint due Such to therobots constrained motion withmodel input disturbances coordination relative-degreenonholonomic robots. are subject subject to nal nal model principle and andin the passivity of property to deal deal nonholonomicwheeled constraint due Such to the therobots constrained motion with input disturbances in coordination of relative-degreeaaa constraint due to constrained motion with input disturbances in coordination of relative-degreeof nonholonomic its wheels. Stabilizing the position and heading of this one and -two incrementally passive systems. B¨ u rger and nonholonomic constraint due to the constrained motion with input disturbances in coordination of relative-degreeof its its wheels. wheels. Stabilizing Stabilizing the the position position and and heading heading of this one and and -two -two incrementally incrementally passive passive systems. systems. B¨ B¨ urger rger and and of of this one u De Persis (2015) studied the problem of output agree of its wheels. Stabilizing position and heading of this one and -two incrementally passive systems. B¨ urger and work was supported the by the Netherlands Organization for De Persis (2015) studied the problem of output agree This De Persis (2015) studied the problem of output agreeThis work was supported by the Netherlands Organization for mentPersis in networks of nonlinear dynamical under De (2015) studied the problem of systems output agreeScientific Research NWO and the Dutch Technology Foundation work was supported by the Netherlands Organization for This ment in networks of nonlinear dynamical systems under This work was supported by the Netherlands Organization for Scientific Research NWO and the Dutch Technology Foundation ment in of dynamical under time-varying disturbances. Jafarian and Desystems Persis (2015) STW under the auspices respectively the Technology QUICK (Quantized InScientific Research NWO and the Foundation ment in networks networks of nonlinear nonlinear dynamical systems under Scientific Research NWOof the Dutch Dutch Foundation time-varying disturbances. Jafarian and De Persis (2015) STW under the auspices ofand respectively the Technology QUICK (Quantized Intime-varying disturbances. Jafarian and De Persis (2015) studied disturbance rejection (both internal-model-based formation and Control for Formation Keeping) and ROSE (EnergySTW under the auspices of respectively the QUICK (Quantized Intime-varying disturbances. Jafarian and De Persis (2015) STW under the auspices of respectively the QUICK (Quantized Instudied disturbance rejection (both internal-model-based formation and for Keeping) and ROSE (Energystudied rejection (both formation and Control Control for Formation Formation Keeping) ROSEnetworks) (Energyefficient and control of mobile ROboticand SEnsor approach and observer-based in formation control studied disturbance disturbance rejectiondesigns) (both internal-model-based internal-model-based formationdesign and Control for Formation Keeping) ROSEnetworks) (Energyefficient design and control of mobile ROboticand SEnsor approach and observer-based designs) in formation control project. efficient design and control of mobile RObotic SEnsor networks) approach and observer-based designs) in efficient design and control of mobile RObotic SEnsor networks) approach and observer-based designs) in formation formation control control project. project. project. project.
63 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 ©© 2015, IFAC (International Federation of Automatic Control) Copyright 2015 IFAC 63 Copyright © 2015 IFAC 63 Peer review under responsibility of International Federation of Automatic Copyright © 2015 IFAC 63 Control. Copyright © 2015 IFAC 10.1016/j.ifacol.2015.10.215
IFAC LHMNC 2015 64 July 4-7, 2015. Lyon, France
Ewoud Vos et al. / IFAC-PapersOnLine 48-13 (2015) 063–068
labeling of the nodes can be done in an arbitrary manner, and it does not have any effect on the final results. Label one end of each edge in E with a positive sign and the other end with a negative sign. The incidence matrix B associated to G(V, E) describes which nodes are coupled by an edge, and is defined as +1 if node i is at the positive side of edge , bi = −1 if node i is at the negative side of edge , 0 otherwise. Moreover, we define the relative position zk between agent i and j as follows r − rj if node i is the positive end of edge k, zk = i rj − ri if node j is the positive end of edge k,
of strictly passive systems with coarse exchanged data. In the port-Hamiltonian framework, disturbance rejection has been studied for a single agent rather than a network. Gentili and van der Schaft (2003) and Gentili et al. (2007) analyzed the input disturbance suppression for one dynamical system using an adaptive internal-model-based controller. In this paper harmonic disturbances are considered. Among practical examples of harmonic disturbances are acoustic disturbances and vibrations in rotating equipment Pigg (2011). Main contribution. This work analyzes formation control of a network of nonholonomic wheeled robots to achieve a prescribed desired formation despite of matched input disturbances. The results are stated within the portHamiltonian framework and build upon previous works of Jafarian and De Persis (2015) and Vos et al. (2014). We study formation control of nonholonomic wheeled robots in the presence of harmonic input disturbances using design tools of passivity-based (Arcak (2007)) and internal-model-based regulation (Isidori et al. (2003)) approaches. To the authors best knowledge, the combination of problems that we consider, that is disturbance rejection for a network of nonholonomic agents within the port-Hamiltonian framework, has not been considered before. Comparing with literature on disturbance rejection using internal-model-based approach within the portHamiltonian framework (e.g. Gentili and van der Schaft (2003) and Gentili et al. (2007)), our contribution is to consider a network of agents rather than a single robot as well as considering robots with a nonholonomic constraint. An extended version of this paper including constant disturbances has been submitted by Jafarian et al. (2015). The outline of the paper is as follows. Section 1.1 recalls some preliminaries from graph theory and portHamiltonian systems. Section 2 continues with the formal problem statement. Section 3 presents the controller design to reach the desired formation in the presence of matched input disturbances. Section 4 presents the stability and convergence analysis of the closed-loop system. Simulation results illustrating the effectiveness of the approach are given in Section 5. Section 6 concludes the paper.
where ri ∈ R2 is the position of agent i expressed in an inertial reference frame. By definition of B, we can represent the relative position variable z, with z T T [z1T . . . zM ] , z ∈ R2M , as z = (B T ⊗ I2 ) r, which implies that z ∈ R(B T ⊗ I2 ).
Now, we provide preliminaries on passivity and the portHamiltonian framework. Consider the system ξ˙ = f (ξ) + g(ξ)u, (1) with state ξ ∈ Rn , input u ∈ Rm , output y ∈ Rm . System (1) is said to be (strictly) output passive from the input u to the output y if there is a continuously differentiable storage function S : Rn → R+ which is positive definite, radially unbounded, and satisfies ∂S [f (ξ) + g(ξ)u] ≤ −αy T y + y T u. (2) ∂ξ The dynamics of the robots and the controllers are modeled within the port-Hamiltonian framework. Define the state x ∈ Rn , input u ∈ Rm , and output y ∈ Rm . The product of the input and output uT (t)y(t) is the power supplied to the system. The general form of a portHamiltonian system is given by x˙ = [J(x) − R(x)] ∇x H(x) + g(x)u (3) y = g(x)T ∇x H(x)
Notation Given a matrix C of real numbers, we denote by R(C) and N (C) respectively the range and the null space. The symbols 1, 0 denote vectors or matrices of all 1 and 0 respectively, where the dimension is sometimes explicitly given (e.g. 1N denotes the N -dimensional column vector of all ones). Ip is the p×p identity matrix. Given two matrices A, B, the symbol A ⊗ B denotes the Kronecker product. A = block.diag {A1 , . . . , AN } denotes a diagonal matrix where Ai is its i-th diagonal element. Finally, ∇x H(x) denotes the column vector of partial derivatives of scalar function H with respect to x ∈ Rn .
where J(x) = −J(x)T , R(x) = RT (x) ≥ 0. The Hamiltonian H(x) equals the total energy stored in the system, and its time derivative is given by H˙ ≤ uT (t)y(t). Hence the increase in the stored energy is always equal or smaller than the power supplied through the power–port (u, y). Therefore (if H is bounded from below) (3) is a strictly passive (lossless) system when R(x) > 0 (R(x) = 0). See van der Schaft and Jeltsema (2014) for a concise overview of the port-Hamiltonian framework. 2. PROBLEM STATEMENT
1.1 Preliminaries
2.1 Wheeled robot dynamics
We consider a network of N nonholonomic robots evolving in R2 . The way in which the robots exchange information is modeled by a connected undirected graph G(V, E), where the node-set V corresponds to N robots and the edge-set E ⊂ V × V corresponds to M virtual couplings. In order to use tools from graph theory, we assign a positive/negative label to each of the nodes connected by an edge. The
Consider wheeled robot i with heading φi and let (xA,i , yA,i ) and (xC,i , yC,i ) respectively denote the center of the wheel axle and a point at the front end of the robot (see Fig. 1). The model presented here is based on the model by Vos et al. (2014), where the center of mass is assumed to be at a distance from the center of the wheel axle. In this paper, the center of mass is assumed to be at 64
IFAC LHMNC 2015 July 4-7, 2015. Lyon, France
Ewoud Vos et al. / IFAC-PapersOnLine 48-13 (2015) 063–068
yC,i
d AC
yA,i
,i
65
T T T q = q1T , . . . , qN , p = pT1 , . . . , pTN , T T T u = uT1 , . . . , uTN , y = y1T , . . . , yN , S(q) = block.diag {S1 (q1 ), . . . , SN (qN )} , G(q) = block.diag {G1 (q1 ), . . . , GN (qN )} , r Dr = block.diag {D1r , . . . , DN }, N 1 T r r −1 and H r (p) = p, with M r = i=1 Hi (pi ) = 2 p (M ) r r block.diag {M1 , . . . , MN }.
φi
2.2 Control goals xA,i
In this section we formally define the control goals for the controller design presented in Section 3. We consider the control law ui as the sum of two control laws ufi and udi . Hence, we can rewrite (4) as follows 0 Si (qi ) ∇qi Hir q˙i = ∇pi Hir p˙ i −SiT (qi ) −Dir 0 (6) + (ufi + udi + di ) Gi (qi )
xC,i
Fig. 1. Wheeled robot i the center of the wheel axle. Furthermore, let dAC,i denote the distance between these points. Each robot is modeled as a rigid body, subject to a nonholonomic constraint. For robot i define the position T T qi = (xA,i , yA,i , φi ) and momentum pi = (pf,i , hi ) . Here pf,i and hi refer to respectively the forward and angular momentum. The input vector ui = (ui,x , ui,y )T are forces (along the x and y direction) acting on point C, while the output vector yi = (yi,x , yi,y )T are the corresponding velocities. Based on the dynamical model of such a robot by Vos et al. (2014), we formulate the dynamics of robot i affected by matched input disturbance di as 0 Si (qi ) ∇qi Hir q˙i = ∇pi Hir p˙ i −SiT (qi ) −Dir 0 (4) + (ui + di ), Gi (qi )
yi = GTi (q)∇pi Hir . Control law ufi achieves the desired formation, based on the results by Vos et al. (2014), while control law udi is based on the results by Jafarian and De Persis (2015) to reject the matched input disturbance di in (6). As was mentioned in Section 1.1, the way in which the robots exchange information is modeled by a connected undirected graph. The formation control goal is to make the relative position zk between robots i and j converge to a desired relative position zk∗ if there exists an edge k in between nodes i and j in the graph. Note that zk is the relative position between robots i and j with respect to their front end (point C in Fig. 1). Hence, assuming that agent i is labeled by the positive sign in the graph G, we can write zx,k = xC,i − xC,j , (7) zy,k = yC,i − yC,j . Defining the error variables z˜k = zk − zk∗ and d˜i = di − udi , we formally state the control goals as lim d˜i = 0 for i = 1 . . . , N, (8)
yi = GTi (qi )∇pi Hir , where matrices Si (qi ) and Gi (qi ) are given by cos φi 0 Si (qi ) = sin φi 0 , 0 1 cos φi sin φi Gi (qi ) = . −dAC,i sin φi dAC,i cos φi
Input disturbance di is generated by an exosystem (see (12)). Dissipation matrix Dir = diag (df,i , dφ,i ) models the friction, with forward friction coefficient df,i and angular friction coefficient dφ,i . The Hamiltonian Hir (pi ) equals the kinetic energy of the robot and is given by 1 1 2 1 p + h2 , Hir (pi ) = pTi (Mir )−1 pi = 2 2mi f,i 2ICM,i i with mass matrix Mir = diag (mi , ICM,i ), where mi and ICM,i denote respectively the mass and moment of inertia. Writing the dynamics of a network of N robots in compact form gives 0 S(q) ∇q H r q˙ = ∇p H r p˙ −S T (q) −Dr 0 (5) + (u + d), G(q) T
t→∞
lim z˜k = 0 for k = 1, . . . , M.
t→∞
(9)
In the next section, we design the control laws ufi and udi in (6). 3. CONTROL DESIGN In Section 3.1, we design the control law ufi , based on the passivity-based approach by Arcak (2007), to achieve the desired formation. The control law udi is designed in Section 3.2 to reject matched input disturbance di . 3.1 Formation control The interpretation of the formation control law is as follows. We consider N (strictly output) passive wheeled robots of the form (5), which correspond to the nodes of the graph. The M edges of the graph correspond to virtual
r
y = G (q)∇p H , where 65
IFAC LHMNC 2015 66 July 4-7, 2015. Lyon, France
Ewoud Vos et al. / IFAC-PapersOnLine 48-13 (2015) 063–068
couplings, which consist of virtual springs and dampers in parallel. In other words, those robots that are exchanging information are interconnected by a virtual spring and damper. If all of the springs of the network reach their minimum potential energy, the desired formation is reached. The state of virtual coupling k is the relative error position z˜x,k ∈ R along x and relative error position z˜y,k ∈ R along y. The velocity inputs are denoted by vx,k , vy,k ∈ R, while the force outputs are denoted by λx,k , λy,k ∈ R. T T Define z˜k = (˜ zx,k , z˜y,k ) , vk = (vx,k , vy,k ) , and λk = T (λx,k , λy,k ) , then the dynamics for virtual coupling k are given by z˜˙k = vk (10) λk = ∇z˜k Hkz˜ + Dkv vk , with dissipation matrix Dkv = diag (dx,k , dy,k ) > 0, where dx,k , dy,k denote the friction coefficients along the x and y directions respectively. The Hamiltonian Hkz˜k equals the potential energy stored in the virtual spring which is given by Hkz˜k = 12 z˜kT Kk z˜k , with spring constant matrix Kk = diag (κx,k , κy,k ) > 0. Writing the dynamics of M virtual spring-damper systems in compact form gives z˜˙ = v (11) λ = ∇z˜H z˜ + Dv v, M with Hamiltonian H z˜ = k=1 Hkz˜k = 12 z˜T K z˜, where z˜ = T T T T T T z˜1 , . . . , z˜M , v = v1T , . . . , vM , λ = λT1 , . . . , λTM , v Dv = block.diag {D1v , . . . , DM }, K = block.diag{K1 , . . . , KM }. Now, let B denote the incidence matrix associated to the undirected connected graph G(V, E), then the control T T T is (Vos et al. (2014)) law uf = uf1 , . . . , ufN
di
Exosystem Internal model based controller
− +
d˜i
dˇi u ˇi
Fig. 2. The exosystem (12) together with the disturbance rejecting controller (13) form the lossless subsystem (14) with port variables u ˇi , d˜i . with internal model controller state θi ∈ R4 , input u ˇ i ∈ R2 , 4×2 2 d ˇ output di ∈ R , and input matrix Gi ∈ R . When u ˇi = 0 and the system is appropriately initialized, the latter system is able to generate any wid solution to (12). Define the error variables d˜i = dˇi − di and θ˜i = θi − wid . T Set Gdi = Γdi and assume that Φdi is a skew-symmetric matrix, then T ˙ θ˜i = Φdi θ˜i + Γdi u ˇi (14) d˜ ˜ di = Γi θi . In compact form we write T ˙ θ˜ = Φd θ˜ + Γd u ˇ (15) d˜ ˜ d = Γ θ, T T T T , u ˇ = u ˇ1 , . . . , u ˇTN , d˜ = with θ˜ = θ˜1T , . . . , θ˜N T d˜T1 , . . . , d˜TN , Φd = block.diag Φd1 , . . . , ΦdN , Γd = block.diag Γd1 , . . . , ΓdN . Note that the exosystem (12) itself is not a passive system, since it has no input. However, interconnecting exosystem (12) with internal model controller (13), the resulting system (14) is lossless with respect to the port variables u ˇi , d˜i (see Fig. 2). Furthermore (15) is easily represented in the port-Hamiltonian ˜ = 1 θ˜T θ, ˜ framework by defining the Hamiltonian H d (θ) 2 such that (15) can be rewritten as T ˙ θ˜ = Φd ∇θ˜H d + Γd u ˇ, (16) d d d˜ = Γ ∇θ˜H .
uf = −(B ⊗ I2 )λ. Note that this control law with coupling dynamics (10) is distributed. We now continue with the disturbance rejection controller design. 3.2 Input disturbance rejection: An internal-model-based approach
This section considers that each robot (6) is affected by a class of disturbances generated by an autonomous system that we refer to as the exosystem. Given two matrices Φdi ∈ R4×4 and Γdi ∈ R2×4 , whose properties will be made precise later on, the exosystem of robot i obeys the following dynamics w˙ id = Φdi wid , (12) di = Γdi wid , i = 1, 2, . . . , N,
Note that in (16) the port-Hamiltonian structure is pre T served, since Φd is skew-symmetric. Let dˇ = dˇT1 , . . . , dˇTN , T T T then control law ud = ud1 , . . . , udN follows as ˇ ud = −d.
with exosystem state wid ∈ R4 and output di ∈ R2 . Inspired by the theory of output regulation (see e.g. Isidori et al. (2003)), an internal-model-based controller is adopted to counteract the effect of the disturbance (see Fig. 2). Let θi , u ˇi , dˇi denote the state, input and output of the internal-model-based controller respectively. Then the internal model dynamics are given by ˇi θ˙i = Φdi θi + Gdi u (13) i = 1, 2, . . . , N, dˇi = Γdi θi ,
3.3 Closed-loop dynamics In the previous sections, we designed two passive control systems: one for reaching the desired formation and one for counteracting matched input disturbances. By interconnecting the systems appropriately, the closed-loop system preserves passivity properties and the port-Hamiltonian structure. To obtain the closed-loop dynamics, define the interconnection structure as 66
IFAC LHMNC 2015 July 4-7, 2015. Lyon, France
Ewoud Vos et al. / IFAC-PapersOnLine 48-13 (2015) 063–068
f d ˇ u = u + u = −(B ⊗ I2 )λ − d, T (17) v = (B ⊗ I2 )y, u ˇ = y, with u, y the port-variables for the robots (5), v, λ the port-variables for the virtual couplings (11), and u ˇ, dˇ the port-variables for the internal model controller (13). Define T ξ = q T , pT , z˜T , θ˜T . The overall closed-loop dynamics
67
5. SIMULATION RESULTS We run simulations for a network of N = 5 wheeled robots, where the robots exchange information according to the following incidence matrix −1 0 0 0 +1 −1 0 0 B = 0 +1 −1 0 . 0 0 +1 −1 0 0 0 +1
follows from (5), (11), (16), (17) and are given by ξ˙ = (J(ξ) − R(ξ))∇ξ H,
(18) where the matrix J(ξ) − R(ξ) is given by 0 S(q) 0 0 ˇ ˇ −S T (q) −D(q) −G(q) −G(q)Γd , T ˇ 0 G (q) 0 0 dT T d 0 Γ G (q) 0 Φ r v ˇT ˇ ˇ ˇ G (q). with G(q) = G(q)(B ⊗I2 ) and D(q) = D + G(q)D Finally, the closed-loop Hamiltonian is given by ˜ = H r (p) + H z˜(˜ ˜ H(q, p, z˜, θ) z ) + H d (θ).
The desired formation is a line formation defined by interrobot distance vectors zk∗ = [1 0]T for k = 1, . . . , 4. Note that the number of edges of the graph is four. The initial position of the robots is set to xA (0) = T T (0, 0.1, 0.2, 0.3, 0.4, 0.5) , yA (0) = (0, 0, 0, 0, 0) , φ(0) = T (1.5124, 4.2482, 1.8162, 4.2211, 4.3677) . In this example the robot parameters are based on the epuck wheeled robot developed by Mondada et al. (2009) and are taken as mi = 0.167 kg, df,i = 2 kg/s, dφ,i = 0.2 kg m2 /s, dAC,i = 0.06 m, ICM,i = 9.69 · 10−5 kg m2 for i = 1, . . . , 5. Here we consider harmonic disturbances (Corollary 2), where we set 0 i , Γdi = I2 ⊗ ( 0 2 ) , Φdi = I2 ⊗ −i 0 T T wid (0) = (1, 1, 1, 1) , θid (0) = (0, 0, 0, 0) ,
We continue with the analysis of (18) in the next section. 4. ANALYSIS In this section, we present the results of the stability and convergence analysis of the closed loop system (18). The proofs are omitted to adhere to the workshop paper length requirements and are available upon request. Proposition 1. The closed-loop system (18) asymptotically converges to the largest invariant set where p = 0 provided that Dv > 0 and there is at least one robot i satisfying Dir > 0.
for i = 1, . . . , 5. The virtual coupling parameters are chosen as κx,k = κy,k = 2, dx,k = dy,k = 1 for k = 1, . . . , 4. First, we set the robot parameters such that all robots are lossless except for robot 1, which is strictly output passive (i.e., df,1 = 2, dφ,1 = 0.2 and df,i = dφ,i = 0 for i = 2, . . . , 5). For readability reasons, we only present the plots along the x direction here, the plots along y show a similar trend. Fig. 3 shows the time evolution of the relative distance z˜x , velocity vx , and internal-model-based controller state θ˜x in the presence of harmonic disturbances. As shown, all three variables converge to zero in accordance with Corollary 2 (see Fig. 3). Finally, we simulate a network where all 5 robots are strictly passive (i.e., df,i = 2, dφ,i = 0.2 for i = 1, . . . , 5) and we set the virtual damping coefficients to zero, dx,k = dy,k = 0 for k = 1, . . . , 4 (see Fig. 4). Here, the robots are subject to the same harmonic disturbances as in Fig. 3. Similar to Fig. 3, in Fig. 4 all variables converge to zero (see Remark 1), thereby achieving objectives (8)-(9).
Proposition 1 guarantees stability of the system and by substituting p = 0 into (18) its convergence to the dynamics given by q˙ = 0, ˜ 0 = −(B ⊗ I2 )K z˜ − Γd θ, (19) z˜˙ = 0, ˙ ˜ θ˜ = Φd θ. However, it guarantees neither reaching the desired formation (i.e., z˜ = 0) nor rejecting the input disturbances (i.e., θ˜ = 0). To verify more, we now consider a special type of the disturbance generated by the exosystem (12), namely harmonic disturbances. Corollary 2. (Harmonic disturbances). Assume that the exosystems’ matrices (Γdi , Φdi ) are of the form 0 ωi2 0 ωi1 d , , (20) Φi = block.diag −ωi1 0 −ωi2 0 with ωi = 0 for = 1, 2, and Γdi = block.diag Γdi1 , Γdi2 ,
6. CONCLUSIONS This paper considered formation keeping control with matched input disturbance rejection for a network of wheeled robots. The model of the network and the control design were presented within the port-Hamiltonian framework. We used tools from passivity and internal-modelbased approaches to design the controllers. The tasks of reaching the desired formation and rejecting the input disturbances were successfully achieved for a class of (strictly) passive robots affected by harmonic disturbances. Future avenues of the research include extending the results to counteract constant disturbances and to track a desired reference velocity.
with Γdi = 0, for all = 1, 2, and the pair (Γdi , Φdi ) is observable. Then the closed loop system (18) converges to the set where p = 0, θ˜ = 0, and z˜ = 0. Remark 1. The results of Proposition 1 and Corollary 2 hold if all robots are strictly output passive and Dv ≥ 0. In the next section, we present simulation results to illustrate the effectiveness of the approach. 67
IFAC LHMNC 2015 68 July 4-7, 2015. Lyon, France
Ewoud Vos et al. / IFAC-PapersOnLine 48-13 (2015) 063–068
systems. IEEE Transactions on Control of Network Systems, 1(3), 272–282. Do, K. and Pan, J. (2007). Nonlinear formation control of unicycle-type mobile robots. Robotics and Autonomous Systems, 55(3), 191–204. Gentili, L., Paoli, A., and Bonivento, C. (2007). Input disturbance suppression for port-Hamiltonian systems: an internal model approach. In Advances in Control Theory and Applications, 85–98. Springer. Gentili, L. and van der Schaft, A. (2003). Regulation and input disturbance suppression for port-controlled Hamiltonian systems. In IFAC workshop on Lagrangian and Hamiltonian methods for nonlinear control. Seville, Spain. Isidori, A., Marconi, L., and Serrani, A. (2003). Robust autonomous guidance: an internal model approach. Springer. Jafarian, M. and De Persis, C. (2015). Formation control using binary information. Automatica, 53, 125–135. Jafarian, M., Vos, E., De Persis, C., Scherpen, J., and van der Schaft, A. (2015). Disturbance rejection in formation keeping control of nonholonomic wheeled robots. Submitted. Mondada, F., Bonani, M., Raemy, X., Pugh, J., Cianci, C., Klaptocz, A., Magnenat, S., Zufferey, J., Floreano, D., and Martinoli, A. (2009). The e-puck, a robot designed for education in engineering. In Conference on autonomous robot systems and competitions, volume 1, 59–65. Ortega, R., van der Schaft, A., Mareels, I., and Maschke, B. (2002). Putting energy back in control. IEEE Control Systems Magazine, 21(2), 18–33. Pigg, S. (2011). Adaptive algorithms for the rejection of sinusoidal disturbances acting on unkown plants. Ph.D. thesis, University of Utah. Ren, W. and Beard, R. (2007). Distributed consensus in multi-vehicle cooperative control: theory and applications. Springer. Sadowska, A., Kostic, D., van de Wouw, N., Huijberts, H., and Nijmeijer, H. (2012). Distributed formation control of unicycle robots. In Robotics and Automation (ICRA), 2012 IEEE International Conference on, 1564– 1569. IEEE. Samson, C. (1993). Time-varying feedback stabilization of car-like wheeled mobile robots. The International journal of robotics research, 12(1), 55–64. van der Schaft, A. and Jeltsema, D. (2014). PortHamiltonian systems theory: An introductory overview. Foundations and Trends in Systems and Control, 1(2-3), 173–378. van der Schaft, A. and Maschke, B. (2013). PortHamiltonian systems on graphs. SIAM Journal on Control and Optimization, 51(2), 906–937. Vos, E., Scherpen, J., and van der Schaft, A. (2012). PortHamiltonian approach to deployment. In International Symposium on Mathematical Theory of Networks and Systems. Melbourne, Australia. Vos, E., Scherpen, J., van der Schaft, A., and Postma, A. (2014). Formation control of wheeled robots in the port-Hamiltonian framework. In World Congress of the International Federation of Automatic Control, 6662– 6667. Cape Town, South Africa.
z˜x [m]
2 0 −2
0
20
40
60
80
0
20
40
60
80
0
20
40 tim e t[s]
60
80
vx [m/s]
5 0 −5
θ˜x [N]
2 0 −2
Fig. 3. Time evolution of relative distance z˜x , velocity vx , and internal model controller state θ˜x (one strictly output passive robot).
z˜x [m]
2 0 −2
0
20
40
60
80
0
20
40
60
80
0
20
40 tim e t[s]
60
80
vx [m/s]
2 0 −2
θ˜x [N]
2 0 −2
Fig. 4. Time evolution of relative distance z˜x , velocity vx , and internal model controller state θ˜x (all robots strictly output passive). REFERENCES Arcak, M. (2007). Passivity as a design tool for group coordination. IEEE Transactions on Automatic Control, 52(8), 1380–1390. Astolfi, A. (1999). Exponential stabilization of a wheeled mobile robot via discontinuous control. Journal of dynamic systems, measurement, and control, 121(1), 121–126. Bai, H., Arcak, M., and Wen, J. (2011). Cooperative control design: A systematic, passivity-based approach, volume 89. Springer. Brockett, R. (1983). Asymptotic stability and feedback stabilization. Birkh¨ auser, Boston. B¨ urger, M. and De Persis, C. (2015). Dynamic coupling design for nonlinear output agreement and time-varying flow control. Automatica, 51, 210–222. De Persis, C. and Jayawardhana, B. (2014). On the internal model principle in the coordination of nonlinear 68
