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

G I S TBearing-Based Formation Control of A Group of Agents with Leader-First Follower Structure The 2017 Asian Control Conference ASCC 2017 Workshop, Gold Coast, Australia Advances in

G I S T

Bearing-Based Formation Control of A Group of Agents

with Leader-First Follower Structure

The 2017 Asian Control Conference (ASCC 2017) Workshop, Gold Coast, Australia

Advances in distributed control and formation control systems

School of Mechanical Engineering, Gwangju Institute of Science and Technology (GIST), South Korea

Minh Hoang Trinh

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)

2

[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.

1 Introduction

2 Background

3 Bearing-Based Henneberg Construction

4 Bearing-Only Control of Leader-First

Follower Formations

5 Regulating the Target Formation

6 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

4

leaderfollowers

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

Initial formation Desired formation

An agent

Neighbor

agent

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

6

• Consider an 𝑛-agent system in the 𝑑 −dimensional space

• The graph 𝐺 = (𝑉, 𝐸) describes interactions in the system

The Bearing Vector

Figure 6: The bearing vector contains

the directional information

• Assume that all agents’ local coordinates are

Note that 𝐠𝑖𝑗 is a unit vector: 𝐠𝑖𝑗 = 1

• The orthogonal projection matrix:

𝐏𝐠𝑖𝑗 = 𝐈𝑑 − 𝐠𝑖𝑗𝐠𝑖𝑗T

• 𝐏𝐠𝑖𝑗 = 𝐏𝐠T𝑖𝑗 = 𝐏𝐠2𝑖𝑗 ≥ 0

• Eigenvalues: {0,1, … , 1}

• Nullspace: 𝑁(𝐏𝐠𝑖𝑗)=𝑠𝑝𝑎𝑛(𝐠𝑖𝑗)

Figure 7: All agents’ coordinates are aligned Each

agent senses some bearing vectors to its neighbors.

Bearing Rigidity Theory

8

• 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 𝐇, ഥ

• 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

[R4]

(H H Id)

Bearing-Only Formation Control

• The target formation shape is specified by a set of desired bearing

vectors .

• The set of desired bearing vectors is feasible, i.e., there exists a

configuration satisfying all the bearing vectors in 𝐵.

* ( , )

{ }ij i j E

* R dn

p

Problem: From an initial formation , design control law using only

bearing measurements such that the formation converges to a desired

formation shape satisfying all desired bearing vectors in 𝐵

(0)

p

Literature Review

10

• 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 bearing measurements” Proc of the 51st IEEE Conference on Decision and Control, USA, 2012.

Literature Review (Cont.)

• Bearing rigidity theory (or parallel rigidity theory)

• Bearing rigidity in - Eren et al (2003), Franchi et al (2012),

Zhao & Zelazo (2015)

• Bearing rigidity in Zelazo et al (2015),

-Schiano et al (2016), - Michieletto et al (2016)

d R

2 1 (or (2))

3 2 (or (3))

[R7] D Zelazo, P R Giordano, A Franchi “Bearing-only formation control using an SE(2) rigidity theory.” Proc of the

IEEE 54th Conference on Decision and Control (CDC), 2015.

[R8] F Schiano et al “A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs.”

IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2016.

[R9] G Michieletto, A Cenedese, and A Franchi “Bearing rigidity theory in SE (3).” Proc of the IEEE 55th

Conference on Decision and Control (CDC), 2016.

Literature Review (Cont.)

12

• Bearing-only formation control

• Undirected formation:

• Subtended bearing angle-based: Basiri et al (2010), Zhao et al (2014),…

• Bearing vector-based: Schoof et al (2014), Zhao & Zelazo (2015)

• Directed formation:

• Leader-first follower graph: Eren (2012), Trinh et al (2016)

• With communications in SE(2): Zelazo et al (2015)

• Without communication: open problem

[R4] S Zhao, D Zelazo “Bearing rigidity and almost global bearing-only formation stabilization.” IEEE Transactions on

Automatic Control 61.5 (2016): 1255-1268.

[R7] D Zelazo, P R Giordano, A Franchi “Bearing-only formation control using an SE(2) rigidity theory.” IEEE 54th

Conference on Decision and Control (CDC), 2015, pp 6121-6126.

[R10] M Basiri, A N Bishop, P Jensfelt, “Distributed control of triangular formations with angle-only constraints.” Systems

& Control Letters 2010; 59(2):147–154.

[R11] S Zhao, F Lin, K Peng, B M Chen, T H Lee, “Distributed control of angle-constrained cyclic formations using bearing-only measurements.” Systems & Control Letters 2014; 63:12–24.

[R12] E Schoof, A Chapman, M Mesbahi “Bearing-Compass Formation Control: A Human-Swarm Interaction Perspective.” American Control Conference (ACC), 2014, pp 3881-3886.

[R13] T Eren "Formation shape control based on bearing rigidity." International Journal of Control 85.9 (2012): 1361-1379 [R14] M H Trinh, K.-K Oh, K Jeong, H.-S Ahn “Bearing-Only Control of Leader First Follower Formations.” Proc of

14 th IFAC Symposium on Large-Scale Complex Systems: Theory and Applications, 49(4), 7-12.

1 Introduction

2 Background

3 Bearing-Based Henneberg Construction

4 Bearing-Only Control of Leader-First

Follower Formations

5 Regulating the Target Formation

6 Simulations and Conclusion

• In a LFF graph of 𝑛 vertices: one leader, one first follower, and 𝑛 − 2

ordinary followers; 2𝑛 − 3 edges

• Bearing-based Henneberg construction: A procedure to generate LFF graphs specified by some bearing vectors

• A graph built up from a bearing-based Henneberg construction is

directed, the realization is in 𝑅𝑑

Leader-First Follower (LFF) Graphs

14

Figure 9: A LFF graph of eight vertices: vertex 1 (the leader) has no neighbor, vertex 2 (the first

follower) has one neighbor, and each vertex 𝑖 (𝑖 = 3, … , 8) has two neighbors

[R13] T Eren "Formation shape control based on bearing rigidity." International Journal of

Control 85.9 (2012): 1361-1379.

Bearing-Based Henneberg Construction

5 6

• Vertex addition: add a new vertex

together with two directed edges to two

existing vertices , in the graph

• Starting with two vertices ,

and a directed edge

doc v doc v doc v

• Edge splitting: consider a vertex

having 2 neighbors , Remove an

edge from the graph and add a

new vertex together with three

doc v doc v doc v

doc v doc v doc v

Figure 10: An example of LFF graph built up from a

Henneberg construction In each step, the added vertex and edges are in yellow and red, respectively Vertex addition is used in step 2, 4, and 5, while edge splitting is used in step 3 and 6 [R3].

• Choosing the desired bearing vectors:

• Each bearing vector contains 𝑑 − 1 pieces of independent information (PII)

𝐠𝑖𝑗 = [g𝑖𝑗1, … , g𝑖𝑗𝑑]𝑇∈ 𝑅𝑑, 𝐠𝑖𝑗 = 1

• Agent 2: 𝐠21∗ - a direction in 𝑅𝑑, we can choose d-1 (PII)

• Agent 3: 𝐠31∗ , 𝐠32∗ and 𝐠21∗ are coplanar, i.e., there exist 𝑑21, 𝑑31, 𝑑32 s.t

𝑑21𝐠21∗ − 𝑑31𝐠31∗ + 𝑑32𝐠32∗ = 𝟎 Also, 𝐠31∗ ≠ 𝐠32∗

Choose 𝐠31∗ - 𝑑 − 1 (PII), 𝐠32∗ - only 1 (PII)

Thus, there are 𝑑 (PII) in choosing 𝐠31∗ and 𝐠32∗

• For 𝑖 ≥ 3, there are 𝑑 (PII) in choosing 𝐠𝑖𝑗∗ , 𝐠𝑖𝑘∗

• For 𝑛 agents, we choose 2𝑛 − 3 bearing vectors,

there are (𝑑 − 1) + (𝑛 − 2)𝑑 = 𝑛𝑑 − 𝑑 − 1 (PII)

• From bearing rigidity theory:

The LFF graph is Infinitesimally Bearing Rigid

Constraints on Desired Bearing Vectors

16

Figure 11: Two desired bearings vectors of each agent

𝑖 (𝑖 > 2) need to be coplanar and noncollinear.

Given the leader position and the distance , and the set

of desired bearing vectors ,

• Position of agent 2:

• Position of agent 𝑖 (𝑖 ≥ 3):

Uniqueness of The Target Formation

* 1

* ( , )

Figure 12: If the leader position and the distance between the leader and the first follower

are fixed, the positions of all followers can be uniquely calculated.

1 Introduction

2 Background

3 Bearing-Based Henneberg Construction

4 Bearing-Only Control of Leader-First

Follower Formations

5 Regulating the Target Formation

6 Simulations and Conclusion

18

The Proposed Bearing-Only Control Law

* (3)

ij i

Agent 2 (The first-follower):

Lemma Under the control law (2),

(i)

(ii) There are two equilibria:

almost globally exponentially stableunstable

a

b

d d

Figure 13: Agent 2 asymptotically

reaches 𝐩2𝑎∗ from almost all initial positions 𝐩2(0) ≠ 𝐩2𝑏∗ .

Distributed Formation Control of MASs: Bearing-based Approaches and

Applications

Stability Analysis (Cont.)

Lemma The system (4.1) has a unique equilibrium point

The system (4.2) has a unique equilibrium point

Figure 14: Illustration of two equilibria

of the cascade system (2)-(3)

Proposition The desired equilibrium of the

cascade system (1)-(4) is almost globally exponentially stable

Stability Analysis (Cont.)

• The n-agent system can be expressed in the form of a cascade system

(4) ( , , )

Theorem Under the proposed control laws, the system (22.1) has two

equilibria The equilibrium satisfying all desired

bearing constraints in B is almost globally asymptotically stable The

equilibrium is unstable All trajectories starting

with asymptotically converge to

Global Stabilization of LFF Formations

• Since agent 2 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:

• 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 𝑡 → ∞)

1 Introduction

2 Background

3 Bearing-Based Henneberg Construction

4 Bearing-Only Control of Leader-First

Follower Formations

5 Regulating the Target Formation

6 Simulations and Conclusion

24

The Dynamical Model in R 3 × 𝑆𝑂(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):

• 𝐑𝑖 ∈ 𝑆𝑂(3): the rotation matrix from 𝑖∑ to 𝑔∑

• Agent 𝑖 can measure in its local coordinate 𝑖∑ :

• the bearing vector 𝐠𝑖𝑗𝑖 = 𝐑𝑇𝑖 𝐠𝑖𝑗, ∀𝑗 ∈ 𝑁𝑖

• the relative orientation 𝐑𝑖𝑗 = 𝐑𝑇𝑖 𝐑𝑗, ∀𝑗 ∈ 𝑁𝑖

• The bearing and orientation sensing graph: a LFF graph

• Strategy: using two control layers

• Orientation alignment layer:

• 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 𝐑2 0 ≠

𝐑1, 𝐩2 0 ≠ 𝐩2𝑏∗ , then 𝐩 → 𝐩𝑎∗ as 𝑡 → ∞

Control of LFF Formations

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

28

3 4

7 8

1

2 3 4

5

6 7

8

3 4

7 8

3 4

7 8

Figure 15: Regulating formation’s orientation and rescaling the formation

1 Introduction

2 Background

3 Bearing-Based Henneberg Construction

4 Bearing-Only Control of Leader-First

Follower Formations

5 Regulating the Target Formation

6 Simulations and Conclusion

7 8

Figure 16: The target formation shape is a

cube in three-dimensional space

Simulation Results

Simulation 1: Achieving the target formation shape

• 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

Thank You!

Q & A.