bg bearing based formation control of a group of agents with leader first follower structure 9579

Bearing-Based Formation Control of A Group of Agents with Leader-First Follower Structure Minh Hoang Trinh School of Mechanical Engineering, Gwangju Institute of Science and Technology (GIST), South Korea G I T S The 2017 Asian Control Conference (ASCC 2017) Workshop, Gold Coast, Australia Advances in distributed control and formation control systems The material presented in this talk is from a joint work [R0] with: • Assistant Prof Shiyu Zhao (Sheffield University) • Dr Zhiyong Sun (Australian National University) • Assistant Prof Daniel Zelazo (Technion- Israel Institute of Technology) • Prof Brian D O Anderson (Australian National University, Data61-CSIRO, and Hangzhou Dianzi University) • Prof Hyo-Sung Ahn (Gwangju Institute of Science and Technology) [R0] M.H Trinh, S Zhao, Z Sun, D Zelazo, B D O Anderson, and H.-S Ahn, “Bearing-based Formation Control of A Group of Agents with Leader-First Follower Structure” submitted to IEEE Transactions on Automatic Control, 2016 Contents Introduction Background Bearing-Based Henneberg Construction Bearing-Only Control of Leader-First Follower Formations Regulating the Target Formation Conclusion Formation-Type Behaviors • WHY? defense, save energy, forage • HOW? • Each individual sees, hears, smells, a few nearby individuals • Each individual acts similar in a distributed manner • Leader-following behaviors leader followers Figure 1: Hierarchical structure in bird formation flight [R1] B D O Anderson, C Yu, and J M Hendrickx "Rigid graph control architectures for autonomous formations." IEEE Control Systems Magazine (2008), pp 48-63 Formation Control Problem An agent Neighbor agent Initial formation Desired formation Figure 2: The group has to achieve a desired formation in a decentralized/distributed manner • • Each agent senses/communicates some geometric variables of the formation Controls its position to reduce the error on the desire variable [R1] B D O Anderson, C Yu, B Fidan, and J M Hendrickx "Rigid graph control architectures for autonomous formations." IEEE Control Systems Magazine (2008), pp 48-63 Preliminaries • Consider an 𝑛-agent system in the 𝑑 −dimensional space • The graph 𝐺 = (𝑉, 𝐸) describes interactions in the system • 𝑉 = 𝑣1 , … , 𝑣𝑛 : a vertex ↔ an agent • 𝐸 = {(𝑣𝑖 , 𝑣𝑗 )| 𝑖, 𝑗 𝜖 𝑉, 𝑖 ≠ 𝑗}: an edge ↔ agent 𝑖 senses and control the bearing vector with regard to agent 𝑗 • 𝑁𝑖 = 𝑣𝑗 ∈ 𝑉 (𝑣𝑖 , 𝑣𝑗 ) ∈ 𝐸 : the neighbor set of agent 𝑖 • 𝐺(𝐩): a framework, 𝐩 = 𝐩1T, … , 𝐩nT T ϵ R𝑑𝑛: a configuration Figure 5: Example of a four-agent system The Bearing Vector • Assume that all agents’ local coordinates are aligned • The displacement between 𝑖 and 𝑗: 𝐳𝑖𝑗 ≜ 𝐩𝑗 − 𝐩𝑖 • The bearing vector from agent 𝑖 to agent 𝑗 [R3]: 𝐠 𝑖𝑗 ≜ 𝐩𝑗 − 𝐩𝑖 𝐩𝑗 − 𝐩𝑖 = 𝐳𝑖𝑗 𝐳𝑖𝑗 Note that 𝐠 𝑖𝑗 is a unit vector: 𝐠 𝑖𝑗 = Figure 6: The bearing vector contains the directional information • The orthogonal projection matrix: 𝐏𝐠𝑖𝑗 = 𝐈𝑑 − 𝐠 𝑖𝑗 𝐠 T𝑖𝑗 • 𝐏𝐠𝑖𝑗 = 𝐏𝐠T𝑖𝑗 = 𝐏𝐠2𝑖𝑗 ≥ • Eigenvalues: {0,1, … , 1} • Nullspace: 𝑁(𝐏𝐠𝑖𝑗 )=𝑠𝑝𝑎𝑛(𝐠 𝑖𝑗 ) Figure 7: All agents’ coordinates are aligned Each agent senses some bearing vectors to its neighbors [R3] S G Loizou, and V Kumar "Biologically inspired bearing-only navigation and tracking." In Proc of 46th IEEE Conference on Decision and Control, pp 1386-1391, 2007 Bearing Rigidity Theory [R4] • Let 𝐠1 , … , 𝐠 𝑚 be a set of bearing vectors in G(p) • Define the bearing function: FB(p)= [𝐠1𝑇, … , 𝐠𝑚𝑇] 𝑇 ∈ 𝑅𝑑𝑚 • The bearing rigidity matrix: 𝐑 p ≜ δ𝐅B 𝐩 δ𝐩 = diag 𝐏𝐠 k 𝐳k ഥ , (H H Id ) 𝐇 • Infinitesimally bearing rigid (IBR): 𝑁(R(p)) = 𝑑𝑛– 𝑑– • An IBR framework can be uniquely determined up to a translational and a scaling factor Figure 8: Example of an IBR framework in two-dimensional space (bearing vectors are colored red) [R4] S Zhao, D Zelazo “Bearing rigidity and almost global bearing-only formation stabilization”, IEEE Transactions on Automatic Control 61 (5), 2016, pp 1255-1268 Bearing-Only Formation Control • The target formation shape is specified by a set of desired bearing vectors B {gij* }(i, j ) E • The set of desired bearing vectors is feasible, i.e., there exists a configuration p* Rdn satisfying all the bearing vectors in 𝐵 Problem: From an initial formation p(0), design control law using only bearing measurements such that the formation converges to a desired formation shape satisfying all desired bearing vectors in 𝐵 Literature Review • Bearing rigidity theory (or parallel rigidity theory) • Bearing rigidity in 𝑅𝑑 - Eren et al (2003), Franchi et al (2012), Zhao & Zelazo (2015) [R4] S Zhao, D Zelazo “Bearing rigidity and almost global bearing-only formation stabilization.” IEEE Transactions on Automatic Control 61.5 (2016): 1255-1268 [R5] T Eren, W, Whiteley, A S Morse, P N Belhumeur, B D O Anderson, “Sensor and network topologies of formations with direction, bearing and angle information between agents ” Proc of the 42nd IEEE Conference on Decision and Control, USA, 2003 [R6] A Franchi, P R Giordano, “Decentralized control of parallel rigid formations with direction constraints and 10 bearing measurements” Proc of the 51st IEEE Conference on Decision and Control, USA, 2012 The Proposed Bearing-Only Control Law Agent (The leader): p1 u1 (1) Agent (The first-follower): p2 u2 pi ui * Pg g 21 (2) 21 Agent i (Follower): Pg gij* j Ni Projection matrix: Pgij Pg ij (3) ij gij gTij Id PgT ij Pg2 , N (Pg ) ij ij span{gij } [R4] S Zhao, D Zelazo “Bearing rigidity and almost global bearing-only formation stabilization.” IEEE Transactions on Automatic Control 61.5 (2016): 1255-1268 19 Stability Analysis Agent (The first-follower): p2 u2 * Pg g 21 21 (2) Lemma Under the control law (2), (i) d21(t ) d21(0), t (ii) There are two equilibria: p*2a p1* d21g *21 : almost globally exponentially stable p*2b p1* d21g*21 : unstable Figure 13: Agent asymptotically reaches 𝐩∗2𝑎 from almost all initial positions 𝐩2 (0) ≠ 𝐩∗2𝑏 Distributed Formation Control of MASs: Bearing-based Approaches and Applications 20 Stability Analysis (Cont.) Agent (Follower): p3 u (p3 , p2 ) Pg g *31 Pg g *32 31 (4) 32 Consider p2 as an input to the unforced systems: p3 u (p3 , p2*a ) Pg g *31 31 Pg g *32 (4.1) p3 u (p3 , p2*b ) Pg g *31 Pg g *32 (4.2) 32 31 32 Figure 14: Illustration of two equilibria of the cascade system (2)-(3) Lemma The system (4.1) has a unique equilibrium point p*3a (Pg Pg ) 1(Pg p1* Pg p*2a ) The system (4.2) has a Pg ) 1(Pg p1* Pg p*2b ) unique equilibrium point p*3b (Pg * 31 * 32 * 31 * 32 * 31 * 32 * 31 * 32 Proposition The desired equilibrium p2 p2*a , p3 p*3a of the cascade system (1)-(4) is almost globally exponentially stable 21 Stability Analysis (Cont.) • The n-agent system can be expressed in the form of a cascade system p1 p2 u (p2 ) p3 u (p3 , p2 ) p (4) pi ui (pi 1, , p2 ) pn un (pn , , p2 ) Theorem Under the proposed control laws, the system (22.1) has two * *T *T *T T equilibria The equilibrium pa [p1 , p2a , , pna ] satisfying all desired bearing constraints in B is almost globally asymptotically stable The * *T *T *T T equilibrium pb [p1 , p2b , , pnb ] is unstable All trajectories starting with p2 (0) p*2b asymptotically converge to pa* 22 Global Stabilization of LFF Formations • Since agent may start from the undesired equilibrium, the control laws (1),(2),(3) cannot globally stabilize the LFF formation to the target formation shape • The modified control law: * Pg g 21 p2 k n 21 k g 21 * g 21 Pg 21 * sgn(Pg g 21 ) 21 : control gain, sgn: the signum function sgn(x) [n1(t ), T d , nd (t )] , ni2 (t ) c n [sgn(x ), (5) , sgn(xd )]T i • Due to the adjustment term, 𝐩∗2𝑏 is not an equilibrium of (5) • The control law (5) is a bearing-only global stabilization control law • The n-agent system under the control laws (1), (5), (3) globally asymptotically converges to the target formation (𝐩2 → 𝐩∗𝑎 , as 𝑡 → ∞) 23 Contents Introduction Background Bearing-Based Henneberg Construction Bearing-Only Control of Leader-First Follower Formations Regulating the Target Formation Simulations and Conclusion 24 The Dynamical Model in R3 ì (3) ã Consider a group of agents in 𝑅3 × 𝑆𝑂(3) Denote ∑ as the global reference frame • Let agent 𝑖 maintain a local reference frame 𝑖∑ • 𝐩𝑖 , 𝐮𝑖 , 𝐰𝑖 : the position, linear velocity, and angular velocity of agent i expressed in the global frame • 𝐩𝑖𝑖 , 𝐮𝑖𝑖 , 𝐰𝑖𝑖 : the position, linear velocity, and angular velocity of agent i expressed in the global frame • The agent’s model (position and orientation dynamics): 𝐩ሶ 𝑖 = 𝐑 𝑖 𝐮𝑖𝑖 (6.1) 𝐑ሶ 𝑖 = 𝐑 𝑖 𝐒𝑖 (6.2) 𝑔 • 𝐑 𝑖 ∈ 𝑆𝑂(3): the rotation matrix from 𝑖∑ to ∑ • 𝐒𝑖 = −𝑤𝑧𝑖 𝑤𝑧𝑖 −𝑤𝑦𝑖 𝑤𝑥𝑖 𝑤𝑦𝑖 −𝑤𝑥𝑖 , where 𝐰𝑖𝑖 = 𝑤𝑥𝑖 𝑤𝑦𝑖 𝑤𝑧𝑖 𝑇 25 Assumptions • Agent 𝑖 can measure in its local coordinate 𝑖 ∑ : • the bearing vector 𝐠 𝑖𝑖𝑗 = 𝐑𝑇𝑖 𝐠 𝑖𝑗 , ∀𝑗 ∈ 𝑁𝑖 • the relative orientation 𝐑 𝑖𝑗 = 𝐑𝑇𝑖 𝐑𝑗 , ∀𝑗 ∈ 𝑁𝑖 • The bearing and orientation sensing graph: a LFF graph g j 𝐠 𝑖𝑖𝑗 𝑗 𝐠 𝑗𝑖 𝑗 𝐠 𝑖𝑖𝑗 ≠ −𝐠 𝑗𝑖 i Figure 14: Illustration of the sensing between two agents 𝑖 and 𝑗 26 Control of LFF Formations • Strategy: using two control layers • Orientation alignment layer: 𝐑ሶ 𝑖 = 𝐑 𝑖 𝐒𝑖 = − ෍ 𝑗∈𝑁𝑖 𝐑 𝑖 𝐑𝑗𝑇 𝐑 𝑖 − 𝐑𝑇𝑖 𝐑𝑗 (7.1) • Formation control layer: 𝐩ሶ 𝑖 = − ෍ 𝐏𝐠𝑖 𝐈3 + 𝐑𝑇𝑖 𝐑𝑗 𝐠 ∗𝑖𝑗 𝑗∈𝑁𝑖 𝑖𝑗 (7.2) • The orientation alignment dynamics acts as an input to the formation control dynamics • If the symmetric part of 𝐑𝑇1 𝐑 𝑖 (0) are positive definite for all i, 𝐑 𝑖 → 𝐑1 , 𝑖 = 2, … , 𝑛 as 𝑡 → ∞ • From notions of almost global Input-to-State Stability, If 𝐑 ≠ 𝐑1 , 𝐩2 ≠ 𝐩∗2𝑏 , then 𝐩 → 𝐩∗𝑎 as 𝑡 → ∞ 27 Regulating The Target Formation • Consider a formation in its desired formation shape • Controlling the formation’s orientation: • Control the leader’s orientation • The followers track the leader’s orientation under (7.1) • Rescaling the formation: • Control a distance 𝑑12 between the leader and the first follower 6 8 5 7 3 Figure 15: Regulating formation’s orientation and rescaling the formation 28 Contents Introduction Background Bearing-Based Henneberg Construction Bearing-Only Control of Leader-First Follower Formations Regulating the Target Formation Simulations and Conclusion 29 Simulation Figure 15: The LFF graph of agents used in simulations Figure 16: The target formation shape is a cube in three-dimensional space 30 Simulation Results Simulation 1: Achieving the target formation shape Simulation 2: Rotating formation Simulation 3: Rescaling formation 31 Conclusion • Summary • Bearing-based Henneberg construction • LFF formations have cascade structure, which ease the control design • Almost global/global stabilization of LFF formations with/without a common global reference frame • Strategies to regulate the target formation • Further studies • Bearing-only control of directed formation is still an open problem • Formations on directed graphs • Implementation of bearing-based LFF formation in quadcopters 32 Q & A Thank You! 33