FACULDADE DE ENGENHARIA DA UNIVERSIDADE DO PORTO
COORDINATED MULTI-ROBOT
FORMATION CONTROL
Tiago Pe ei a do Nascimen o
Doc o al P og am in Elec ical and Compu e s Enginee ing
Supe iso : An ónio Paulo G. M. Mo ei a (PhD.)
Co-Supe iso : And é Gus a o Scola i Conceição (PhD.)
Oc obe , 2012
COORDINATED MULTI-ROBOT FORMATION
CONTROL
Tiago Pe ei a do Nascimen o
Doc o al P og am in Elec ical and Compu e s Enginee ing
Resumo
Um sis ema mul i- obôs é o mado quando um g upo de obôs in e age com o ambien e
como um único sis ema. Es e sis ema pode ambém se o mado pa a ealiza a e as
ou o a di íceis ou a é impossí eis de se em ealizadas po um único obô.
Es e abalho ap esen a os esul ados da pesquisa des a ese de dou o ado obje i ando
o con ole de o mação de um g upo de obôs mó eis no p oblema de as eamen o a i o
de al o. Todos os obôs ocam in o mações sob e sua posição (localização) no mundo, a
posição e elocidade do al o o mando um sis ema o almen e dis ibuído. Dessa o ma,
o p incipal obje i o des e abalho é p ocu a e as ea um al o minimizando a ince eza
o al sob e sua obse ação, des iando de colegas e obs áculos, obedecendo as ca ac e ís-
icas impos as à o mação.
Um con olado p edi i o não linea oi p opos o pa a soluciona o p oblema de as-
eamen o a i o de al o a a és da o mação de um sis ema mul i- obôs. O con olado de
o mação c iado como a p incipal con ibuição des a ese é um con olado de o mação
p edi i o não-linea (NMPFC). Es e abalho ambém ap esen a ou as con ibuições em
con ole de o mação no que ange o des io de obs áculos, a ausência empo á ia do
al o e a o mação de um g upo he e ogêneo ( obôs não-holonômicos e holonômicos).
Es as con ibuições melho a am a o mação o nando-a e sá il, conside ando ince ezas
dinâmicas, bem como a busca e pe seguição de um al o inicialmen e ausen e. Simulações
e expe imen os com obôs eais o am ealizados, ap esen ados e discu idos.
Es e abalho é pa e do p oje o FCT: PTDC/EEA-CRO/100692/2008 - Pe cep ion-
D i en Coo dina ed Mul i-Robo Mo ion Con ol
i

ii
Abs ac
A mul i- obo sys em is o med when a g oup o obo s in e ac wi h he en i onmen as
a single sys em. This sys em can also be in o ma ion in o de o accomplish asks a he
di icul o impossible o achie e wi h a single obo .
This wo k p esen s esul s o a doc o al hesis esea ch aiming he o ma ion con ol
o a mobile obo g oup in he ac i e a ge acking p oblem. All obo s exchange in-
o ma ion abou hei posi ion (localiza ion) in he wo ld ame, and he a ge posi ion
and eloci y o ming a dis ibu ed sys em. The e o e, he main objec i e o his wo k
is o sea ch and ack a a ge minimizing he o al amoun o unce ain y in he a ge ’s
obse a ion, a oiding ma es and obs acles, and obeying he cha ac e is ics imposed o he
o ma ion.
A nonlinea model p edic i e con olle was p oposed in o de o deal wi h he ac i e
a ge acking p oblem h ough he o ma ion o a mul i- obo sys em. The o ma ion
con olle c ea ed as he majo con ibu ion o his hesis is a nonlinea model p edic-
i e o ma ion con olle (NMPFC). This wo k also p esen s some con ibu ions in he
o ma ion con ol conce ning he obs acle a oidance, he empo a y absence o he a -
ge and he o ma ion con ol o a he e ogeneous g oup (holonomic and nonholonomic
obo s). These con ibu ions imp o ed he o ma ion making i e sa ile wi h dynamic
unce ain ies as well as he sea ch and ack o an ini ially absen a ge . Simula ions and
expe imen s wi h eal obo s we e made, p esen ed and discussed.
Finally, his wo k is inse ed in he FCT p ojec PTDC/EEA-CRO/100692/2008 -
Pe cep ion-D i en Coo dina ed Mul i-Robo Mo ion Con ol.
iii
i
Suppo
Wi h he suppo h ough he PhD Schola ship SFRH/BD/70698/2010 and
xii CONTENTS
4.4 The Op imiza ion Algo i hm . . . . . . . . . . . . . . . . . . . . . . . . 58
4.5 Weigh Tuning Me hodology . . . . . . . . . . . . . . . . . . . . . . . . 60
4.5.1 Ini ial Analysis o he Te ms and Weigh s o he Cos Func ion . . 61
4.5.2 TheTuning............................. 62
4.5.3 The In luence o he Numbe o Te ms . . . . . . . . . . . . . . . 63
4.6 Resul s.................................... 66
4.6.1 Simula ions............................. 67
4.6.2 Resul s o he Expe imen s wi h Real Robo s . . . . . . . . . . . 76
4.7 Conclusion ................................. 83
5 Obs acle A oidance in Fo ma ion Con ol 85
5.1 In oduc ion................................. 85
5.2 P oblemFo mula ion............................ 87
5.3 Po en ial Field App oach in NMPFC . . . . . . . . . . . . . . . . . . . . 87
5.3.1 Ma e A oidance Func ion . . . . . . . . . . . . . . . . . . . . . 88
5.3.2 Obs acle A oidance Func ion . . . . . . . . . . . . . . . . . . . 90
5.4 The Modi ied A* Pa h Planning Algo i hm . . . . . . . . . . . . . . . . . 92
5.4.1 TheModi ica ions ......................... 94
5.5 Resul s.................................... 95
5.5.1 Simula ions............................. 95
5.5.2 Resul s o he Expe imen s wi h Real Robo s . . . . . . . . . . . 101
5.6 Conclusion .................................105
6 In elligen S a e Changing Applied o Mul i-Robo Sys ems 107
6.1 In oduc ion.................................107
6.2 TheS a eMachine .............................111
6.3 The Takagi-Sugeno Type Fuzzy Au oma on . . . . . . . . . . . . . . . . 113
6.4 P oblemFo mula ion............................115
6.4.1 Membe ship Func ions . . . . . . . . . . . . . . . . . . . . . . . 117
6.4.2 The P oblem Example . . . . . . . . . . . . . . . . . . . . . . . 117
6.5 Resul s....................................119
6.5.1 Simula ions.............................119
6.5.2 Resul s o he Expe imen s wi h Real Robo s . . . . . . . . . . . 123
6.6 Conclusion .................................125
7 Conclusions and Fu u e Wo k 127
7.1 Conclusion .................................127
7.2 Fu u eWo k.................................129
7.2.1 Changeable Leade Robo . . . . . . . . . . . . . . . . . . . . . 129
7.2.2 Model Conside a ions . . . . . . . . . . . . . . . . . . . . . . . 129
7.2.3 Pa ame e sTuning .........................129
7.2.4 He e ogenei y Conside a ions . . . . . . . . . . . . . . . . . . . 130
7.2.5 Ta ge Obse a ion.........................130
7.3 FinalConside a ions ............................130
A The Co a iance Model 131
A.1 T ans o ma ion in he Canonical Fo m . . . . . . . . . . . . . . . . . . . 131

CONTENTS xiii
Re e ences 135
xi CONTENTS
Lis o Figu es
1.1 Examples o Mobile Robo s . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Examples o Leade Following Fo ma ions . . . . . . . . . . . . . . . . . 2
2.1 Flocks o Mig a o y Bi ds in a V Fo ma ion . . . . . . . . . . . . . . . . 8
2.2 Vi ual S uc u e Fo ma ion. . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3 Leade -Following Fo ma ion wi hou Ta ge . . . . . . . . . . . . . . . . . 13
2.4 Leade -Following Fo ma ion wi h Ta ge . . . . . . . . . . . . . . . . . . 13
2.5 Basic P inciple o he MPC [89] . . . . . . . . . . . . . . . . . . . . . . 18
2.6 Gene icMPCS uc u e............................ 18
3.1 Omni-di ec ional Wheel . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.2 The 5DPO Mobile Socce Robo s . . . . . . . . . . . . . . . . . . . . . 27
3.3 TheMini-AGVRobo ............................ 27
3.4 The Gene al A chi ec u e . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.5 TheHALSo wa e ............................. 29
3.6 CoachApplica ion ............................. 30
3.7 TheDecSo wa e.............................. 30
3.8 The SimTwo Main Windows . . . . . . . . . . . . . . . . . . . . . . . . 31
3.9 SimTwo Con igu a ion Window . . . . . . . . . . . . . . . . . . . . . . 32
3.10SimTwoCodeEdi o ............................ 32
3.11SimTwoPlo Window............................ 32
3.12SimTwoXMLEdi o ............................ 33
3.13 Geome ic Rep esen a ion o he Robo ’s Body . . . . . . . . . . . . . . 36
3.14 T ajec o y o Model Valida ion . . . . . . . . . . . . . . . . . . . . . . 38
3.15 Tes wi h 1m/sand θ=0◦......................... 38
3.16 Tes wi h 1m/sand θ=Va iable ...................... 39
4.1 S uc u e o he Nonlinea Model P edic i e Fo ma ion Con olle Ap-
plied o obo 1 ............................... 44
4.2 Con olle Diag am............................. 45
4.3 The 5dpo obo g aph ep esen a ion. . . . . . . . . . . . . . . . . . . . . 47
4.4 Modelo Obse a ion............................ 51
4.5 Fo ma ion Following a Leade . . . . . . . . . . . . . . . . . . . . . . . 58
4.6 TuningTes Se up.............................. 63
4.7 Tes bed esul s: using he ini ial weigh s . . . . . . . . . . . . . . . . . . 65
4.8 Simula ionSe up .............................. 67
4.9 Simula ion 1: Homogeneous Fo ma ion Con e gence - Plo XY . . . . . 67
x
x i LIST OF FIGURES
4.10 Simula ion 1: Homogeneous Fo ma ion Con e gence - Dis ance be ween
Robo and De e minan o Σ⊥
Me ged(k).................... 68
4.11 Simula ion 1: He e ogeneous Fo ma ion Con e gence - Plo XY . . . . . 68
4.12 Simula ion 1: He e ogeneous Fo ma ion Con e gence - Dis ance be ween
Robo and Ball and he De e minan o Σ⊥
Me ged .............. 69
4.13 Simula ion 2: Homogeneous Fo ma ion Con e gence - Plo XY . . . . . 70
4.14 Simula ion 2: Homogeneous Fo ma ion Con e gence - Dis ance be ween
Robo and De e minan o Σ⊥
Me ged(k).................... 70
4.15 Simula ion 2: He e ogeneous Fo ma ion Con e gence - Plo XY . . . . . 71
4.16 Simula ion 2: He e ogeneous Fo ma ion Con e gence - Dis ance be ween
Robo and Ball and he De e minan o Σ⊥
Me ged .............. 71
4.17 Simula ion 3: Homogeneous Leade Following - Plo XY and Dis ance
be ween he Leade Robo and he Followe s . . . . . . . . . . . . . . . . 72
4.18 Simula ion 3: Homogeneous Leade Following - In e nal P oduc Be-
ween Leade and Followe s and E o Angle o he Followe s O ien a ion
Facing heLeade .............................. 72
4.19 Simula ion 3: He e ogeneous Leade Following - Plo XY and he Dis-
ance be ween he Leade Robo . . . . . . . . . . . . . . . . . . . . . . 73
4.20 Simula ion 3: He e ogeneous Leade Following - In e nal P oduc Be-
ween Leade and Followe s and Angle Be ween he Leade and he Fol-
lowe s.................................... 73
4.21 Simula ion 4: Homogeneous Keeping Fo ma ion - Plo XY . . . . . . . . 74
4.22 Simula ion 4: Homogeneous Keeping Fo ma ion - Dis ance be ween Robo
and Ball and De e minan o Σ⊥
Me ged(k).................. 74
4.23 Simula ion 4: Homogeneous Keeping Fo ma ion - In e nal P oduc Be-
ween he Robo s and he Ball and E o Angle o he Robo s O ien a ion
Facing heTa ge .............................. 75
4.24 Simula ion 4: He e ogeneous Keeping Fo ma ion - Plo XY . . . . . . . . 75
4.25 Simula ion 4: He e ogeneous Keeping Fo ma ion - Dis ance be ween Robo
and De e minan o Σ⊥
Me ged ......................... 76
4.26 Simula ion 4: He e ogeneous Keeping Fo ma ion - In e nal P oduc Be-
ween he Robo s and he Ball and Angle Be ween he Robo s and he
Ball’sVeloci yVec o ............................ 76
4.27Expe imen Se up.............................. 77
4.28 Real Expe imen 1: Fo ma ion Con e gence . . . . . . . . . . . . . . . . 78
4.29 Real Expe imen 1: Fo ma ion Con e gence - Dis ance be ween he obo
and he ball and De e minan o Σ⊥
Me ged(k)................. 79
4.30 Real Expe imen 2: Fo ma ion Con e gence . . . . . . . . . . . . . . . . 79
4.31 Real Expe imen 2: Fo ma ion Con e gence - Dis ance be ween he obo
and he ball and De e minan o Σ⊥
Me ged(k)................. 80
4.32 Real Expe imen 3: Leade Following . . . . . . . . . . . . . . . . . . . 80
4.33 Real Expe imen 3: Leade Following - Plo XY and Dis ance be ween
he Leade Robo and he Followe s . . . . . . . . . . . . . . . . . . . . 80
4.34 Real Expe imen 3: Leade Following - In e nal P oduc Be ween Leade
and Followe s and E o Angle o he Followe s O ien a ion Facing he
Leade .................................... 81
4.35 Real Expe imen 4: Keeping Fo ma ion . . . . . . . . . . . . . . . . . . 82
LIST OF FIGURES x ii
4.36 Real Expe imen 4: Keeping Fo ma ion - Plo XY and De e minan o
Σ⊥
Me ged(k).................................. 82
4.37 Real Expe imen 4: Keeping Fo ma ion - In e nal P oduc Be ween he
Robo s and he Ball and E o Angle o he Robo s O ien a ion Facing he
Ta ge .................................... 82
5.1 Beha io o unc ion in 5.5 ......................... 89
5.2 Local Minima P oblem Case . . . . . . . . . . . . . . . . . . . . . . . . 90
5.3 Swi chingapp oach............................. 91
5.4 Map Cell Decomposi ion [110] . . . . . . . . . . . . . . . . . . . . . . . 94
5.5 Obs acle’s o al adius [110] . . . . . . . . . . . . . . . . . . . . . . . . 94
5.6 Simula ion 1: Homogeneous Fo ma ion - Plo XY . . . . . . . . . . . . . 95
5.7 Simula ion 1: Homogeneous Fo ma ion - Dis ance be ween Robo and
Leade and Angle Be ween he Leade and he Followe s . . . . . . . . . 96
5.8 Simula ion 1: He e ogeneous Fo ma ion - Plo XY . . . . . . . . . . . . 96
5.9 Simula ion 1: He e ogeneous Fo ma ion - Dis ance be ween Robo and
Leade and Angle Be ween he Leade and he Followe s . . . . . . . . . 97
5.10 Simula ion 2: Homogeneous Fo ma ion - Plo XY . . . . . . . . . . . . . 97
5.11 Simula ion 2: Homogeneous Fo ma ion - Dis ance be ween Robo and
De e minan o Σ⊥
Me ged ........................... 98
5.12 Simula ion 2: He e ogeneous Fo ma ion - Plo XY . . . . . . . . . . . . 98
5.13 Simula ion 2: He e ogeneous Fo ma ion - Dis ance be ween Robo and
Ball and he De e minan o Σ⊥
Me ged .................... 99
5.14 Simula ion 3 - En i onmen and Plo XY . . . . . . . . . . . . . . . . . . 100
5.15 Simula ion 3 - Dis ance be ween Robo and Ball and De e minan o Σ⊥
Me ged100
5.16 Real Expe imen 1 - En i onmen and Plo XY . . . . . . . . . . . . . . 101
5.17 Real Expe imen 1 - Dis ance be ween Robo and he Ball and De e mi-
nan o Σ⊥
Me ged ...............................102
5.18 Real Expe imen 2 - En i onmen and Plo XY . . . . . . . . . . . . . . 102
5.19 Real Expe imen 2 - Dis ance be ween Robo and De e minan o Σ⊥
Me ged 103
5.20 Real Expe imen 3 - Plo XY . . . . . . . . . . . . . . . . . . . . . . . . 104
5.21 Real Expe imen 3 - Dis ance be ween Robo and De e minan o Σ⊥
Me ged 104
5.22RealExpe imen 3 .............................104
6.1 Robo S a eMachine ............................112
6.2 Membe ship Func ions and Su ace . . . . . . . . . . . . . . . . . . . . 117
6.3 The P oblem Simula ion: Fo ma ion Beha io wi hou he TS-TFA ap-
p oach....................................118
6.4 The Solu ion Simula ion: Fo ma ion Beha io wi h he TS-TFA app oach 118
6.5 Simula ion 1: Dis ance Robo -Ball and Plo XY o he Robo s Pa h . . . . 120
6.6 Simula ion1.................................121
6.7 Simula ion 2: Dis ance Robo -Ball and Plo XY o he Robo s Pa h . . . . 122
6.8 Simula ion 2: O ien a ion o he obo s . . . . . . . . . . . . . . . . . . . 122
6.9 Simula ion 3: Dis ance Robo -Ball and Plo XY o he Robo s Pa h . . . . 123
6.10 Real Expe imen : Dis ance Robo -Ball and Plo XY o he Robo s Pa h . . 123
6.11 Real Expe imen : Coach View . . . . . . . . . . . . . . . . . . . . . . . 124
A.1 Modelo Obse a ion............................131

x iii LIST OF FIGURES
Lis o Tables
3.1 5dpo inpu pa ame e s in SimTwo . . . . . . . . . . . . . . . . . . . . . 35
3.2 Mo o pa ame e s in SimTwo . . . . . . . . . . . . . . . . . . . . . . . . 36
3.3 Pa ial Momen s o Ine ia . . . . . . . . . . . . . . . . . . . . . . . . . 37
3.4 Mean Squa e E o o Model Valida ion T ajec o ies . . . . . . . . . . . 39
4.1 Weigh s o he Cos Func ion wi h Only Fou Te ms . . . . . . . . . . . 64
4.2 Weigh s o he Cos Func ion wi h Fi e Te ms . . . . . . . . . . . . . . 64
4.3 Final Weigh s o he Cos Func ion: Holonomic Robo s . . . . . . . . . 65
4.4 Final Weigh s o he Cos Func ion: Nonholonomic Robo s . . . . . . . 66
6.1 Membe ship Func ions-S a e T ansi ions. The alues NV, VR and VM
in he Quali y (q) membe ship unc ion means, espec i ely, No Visible,
Visible Reasonably and Visible Much, and HT, TR and ST in he T us
( ) membe ship unc ion mean High T us ,T us Reasonably and Small
T us , espec i ely. .............................113
xix
xx LIST OF TABLES
Abb e ia ions and Symbols
AGV Au oma ed Guided Vehicles
APF A i icial Po en ial Fields
AUV Au onomous Unde wa e Vehicles
BP Back-P opaga ion
CAD Compu e Aided Design
CC Con inuous Cu en
DC Di ec Cu en
FEUP Facul y o Enginee ing om Po o Uni e si y
HAL Ha dwa e Abs ac ion Laye
LP Linea P oblem
MPC Model P edic i e Con ol
NGN No malize Gaussian Ne wo k
NLP Non Linea P oblem
NM Numbe o Ma es
NMPC Nonlinea Model P edic i e Con ol
NMPFC Nonlinea Model P edic i e Fo ma ion Con ol
NO Numbe o Obs acles
ODE Open Dynamics Engine
PI P opo ional and In eg a i e
PIC P og ammable In e ace Con olle
PTS Pe iodic Team Synch oniza ion
RHPC Receding Ho izon P edic i e Con ol
RPROP Resilien PROPaga ion algo i hm
RTDB Real Time Da a Based
TS Takagi-Sugeno
TS-TFA Takagi-Sugeno Type Fuzzy Au oma on
UDP Use Da ag am P o ocol
XML Ex ensible Ma kup Language
xxi
6In oduc ion

Chap e 2
S a e o he A
This chap e summa izes he app oaches mos used in o ma ion con ol o mul i- obo
sys ems and ac i e a ge acking p oblem.
2.1 In oduc ion
A o ma ion is usually de ined as he spa ial a angemen o a g oup o agen s, whe e
he ela i e pose o i s elemen s is s eady e en i he o ma ion is mo ing. The concep
o o ma ion is no he p oduc o human ingenui y. Fo ma ion comes om obse able
beha io s common in na u e. Fo example, i is cus oma y o glimpse locks o mig a o y
bi ds lying in a V o ma ion (also called he Fou -Finge Fo ma ion), which allows hem
o maximize he dis ance a eled and minimize a igue by educing ai ic ion which is
possible due o hem lying in he ai unnel le by he bi d a eling di ec ly in on o
hem (see Fig. 2.1). This is ue o all bi ds excep he one ha occupies he on pose.
Howe e , he speci ic bi d ha is gi en such ask changes cyclically in o de o dis ibu e
a igue among membe s o he lock. Column o ma ions a e also qui e common. Fo
example, in he cases o a bi d ha leads a column made by hei o sp ing allowing he
mo he o ensu e a sa e pa h as well as ha ing he o sp ing main aining eye con ac wi h
he bi d di ec ly in on in o de o a oid ge ing los ( e y common in ducks).
In he human sphe e, o ma ions a e used in di e en si ua ions anging om mili a y
ope a ions o eam spo s. A o ma ion is used in o de o gain a ac ical ad an age in
he mili a y con ex . Team spo s also make ex ensi e use o o ma ions. Fo example,
oo ball is a spo whe e he highly ac ical o ma ions ake on a ole. In ac , he e a e
o e 30 o hese a angemen s a ying om T o I o highly complex a angemen s. In
socce , aining desc ibes he numbe o playe s assigned o each ac ical pose in he ield
7
8S a e o he A
Figu e 2.1: Flocks o Mig a o y Bi ds in a V Fo ma ion
(gene ally de ense, mid ield and a ack). Fo ma ions a e less igid in his case, being i an
alloca ion o he ac ion a ea o each playe [20].
In obo ics, he e a e wo ypes o o ma ion con ol app oaches: cen alized and de-
cen alized. This chap e gi es he ad ances in he decen alized mul i- obo mo ion co-
o dina ion echniques o e he las decade [37], [38]. The h ee majo decen alized ap-
p oaches o o ma ion con ol s udied a e: Vi ual S uc u e, Beha io -Based and Leade -
Following. This las one being one o he mos s udied in mul i- obo o ma ion [39], [40],
[41], [42] and [43]. A good e iew o he h ee majo app oaches in o ma ion con ol can
be seen in [44].
2.2 App oaches in he Fo ma ion Con ol P oblem
This sec ion will discuss he la es de elopmen s, in all h ee app oaches, as well as sum-
ma ize he con olle s used in he chosen app oach o his wo k.
2.2.1 Vi ual S uc u e App oach
Rigid- o ma ion app oaches ha e been eme ging o almos a decade [45], [46], [47]. The
i ual s uc u e app oach, was i s p esen ed by [48]. In he Vi ual S uc u e app oach
he obo ’s o ma ion no longe consis s o leade s no ollowe s, i.e. no hie a chy ex-
is s in he o ma ion (see Fig. 2.2). Con ol me hods a e de eloped o o ce a g oup o
obo s o beha e in a igid o ma ion. In he i ual s uc u e app oach, he con olle is
de i ed om h ee s eps. Fi s , he desi ed dynamics o he i ual s uc u e is de ined.
Second, he desi ed mo ion o he i ual s uc u e is ansla ed in o desi ed mo ions o
each agen . Finally, indi idual acking con olle s o each agen a e de i ed om agen
acking. La e on, many a ian s o his app oach ha e eme ged in ecen yea s. In [49]
he au ho s conside wo coope a i e con ol p oblems o nonholonomic mobile agen s.
In he i s p oblem, he design o coope a i e con ol laws is discussed so ha a g oup o
2.2 App oaches in he Fo ma ion Con ol P oblem 9
nonholonomic mobile agen s coope a i ely con e ges o some s a iona y poin unde a -
ious communica ion scena ios. Dynamic con ol laws o each agen a e p oposed wi h
he aid o σ-p ocesses and esul s om g aph heo y. In he second p oblem, he au ho s
discuss he design o coope a i e con ol laws such ha a g oup o mobile agen s con-
e ges o and acks a a ge poin which mo es along a desi ed ajec o y unde a ious
communica ion scena ios.
Figu e 2.2: Vi ual S uc u e Fo ma ion.
The au ho s in [50] p esen a synch oniza ion app oach o ajec o y acking o mul-
iple mobile obo s while main aining ime- a ying o ma ions in a igid s uc u e. The
main idea is o con ol each obo o ack i s desi ed ajec o y while synch onizing i s
mo ion wi h hose o o he obo s o keep ela i e kinema ics ela ionships, as equi ed by
he o ma ion. Fi s , hey pose he o ma ion-con ol p oblem as a synch oniza ion con ol
p oblem and iden i y he synch oniza ion con ol goal acco ding o he o ma ion equi e-
men . Second, he au ho s de elop a synch onous con olle o each obo ’s ansla ion
o gua an ee ha bo h posi ion and synch oniza ion e o s app oach ze o asymp o ically.
The o a ional con olle is also designed o ensu e ha he obo is always o ien ed o-
wa d i s desi ed posi ion. Bo h ansla ional and o a ional con ols a e suppo ed by a
cen alized high-le el planne o ask moni o ing and obo global localiza ion.
Ano he a ia ion o he Vi ual S uc u e app oach is he Clus e Rep esen a ion. The
au ho s in [51] used a clus e space s a e ep esen a ion o mobile mul i- obo sys ems
as a means o enabling enhanced con ol o mobile mul i- obo sys ems. A concep ual
amewo k was p oposed o he selec ion o app op ia e clus e space s a e a iables o a
n- obo sys em, he de elopmen o o mal kinema ics ha associa e he clus e space s a e
a iables wi h obo -speci ic a iables, and he implemen a ion o a clus e space con ol
sys em a chi ec u e. The clus e space app oach was hen demons a ed by examples o
wo- and h ee- obo clus e s consis ing o di e en ial d i e obo s ope a ing in a plane.
The con ol issue in a i ual s uc u e is hen app oached by [52] we e hey p esen a
10 S a e o he A
mul i-laye scheme ha con ols a o ma ion o nmobile obo s, which includes a s a egy
o obs acle a oidance. The con olle adop ed is able o guide he obo s o compose he
desi ed o ma ion and o ack a desi ed ajec o y, a oiding obs acles du ing he na iga-
ion. Two planning laye s a e esponsible o : o ganizing he na iga ion o he indi idual
obo s owa ds he desi ed o ma ion, minimizing ene gy consump ion, and changing he
obo s e e ence signals in o de o a oid collisions. S abili y analysis pe o med on he
closed-loop sys em shows ha he o ma ion e o s a e ul ima ely bounded.
Following he pape in 2007, [53] p oposed a consensus acking algo i hm explic-
i ly accoun ing o bounded con ol e o . This algo i hm was hen analyzed unde a
di ec ed ixed in e ac ion opology. Fu he mo e, con e gence analysis o a consensus
acking algo i hm was p o ided when he ime- a ying consensus e e ence s a e was
a ailable o a dynamically changing subg oup o he eam unde di ec ed swi ching in e -
ehicle in e ac ion opologies. Expe imen al esul s o a o ma ion con ol applica ion
we e demons a ed on a mul i- obo pla o m o alida e one o he p oposed consensus
acking algo i hms.
Ano he app oach was done by [54]. He e, an adap i e o ma ion con ol me hod was
p oposed o mul iple unce ain nonholonomic mobile obo s a he ac ua o dynamics
le el. All pa ame e s o he obo kinema ics and dynamics, and ac ua o dynamics a e
unknown. The i ual s uc u e wi h pa h pa ame e s and he dynamic su ace design
me hodology we e combined o design an adap i e o ma ion con ol scheme simple han
he p e ious backs epping-based con ol sys em. Using he Lyapuno s abili y heo em,
he au ho s p esen ed he adap a ion laws o uning all unknown pa ame e s o mul iple
mobile obo s ega dless o pa h pa ame e s in he e e ence ajec o ies.
One o he la es de elopmen s in he i ual s uc u e app oach was p esen ed by [55].
He e, he au ho s combined he i ual s uc u e and pa h ollowing app oaches o de i e
he o ma ion a chi ec u e. A o ma ion con olle was p oposed by he au ho s o he
kinema ic model o wo-deg ee-o - eedom unicycle- ype mobile obo s. The app oach
was hen ex ended o conside he o ma ion con olle by aking in o accoun he physical
dimensions and dynamics o he obo s. The con olle he e was designed in such a way
ha he pa h de i a i e was le as a ee inpu o synch onize he obo ’s mo ion.
The i ual s uc u e can also be used in many ields besides g ound ehicles such as
UAV applica ions [56], o manipula o s [12]. Ne e heless, he i ual s uc u e app oach
has many issues ega ding compu a ional cos s. As i can be seen, only ew wo ks ha
ha e used his app oach conside obs acle a oidance ([3] and [52]). E en hough, he
obs acles a e s a ic and hese wo ks only ha e simula ions esul s. When making expe i-
men s, mos o he wo ks do no conside obs acle a oidance and he a e age eloci y is
ela i ely low gi ing ime o a cen alized con olle o p ocess he in o ma ion as i can
be seen in [51], [50], [9], [53] and [55].
2.2 App oaches in he Fo ma ion Con ol P oblem 11
2.2.2 Beha io -Based App oach
Beha io -based app oach and po en ial ield app oach a e o en combined in he appli-
ca ion o o ma ion con ol. In beha io -based app oach [7], [57], [8], each obo has a
basic mo o schema. Each schema gene a es a ec o ep esen ing he desi ed beha io
esponse o senso y inpu . Possible mo o schema include collision a oidance, obs acle
a oidance, goal seeking, and o ma ion keeping. The con ol ac ion o each obo is a
ec o weigh ed a e age o he con ol o each mo o schema beha io . Ano he good
mix u e o Beha io -based app oach wi h igid g aph heo y, can be seen in [58].
In [59], he au ho s desc ibe he con ol o obo eams in he amewo k o Hilbe
spaces. The ocus o he pape was he in insic p ope ies o obo con ol a chi ec u es,
namely he condi ions unde which a gene ic mission can be success ully execu ed. The
p oposed pa adigm was de eloped in wo le els: (i) single obo con ol suppo ed on a
mono onic and non-expansi e p ojec ion map de ined o e some beha io al space such as
he obo con igu a ion space o he eloci y space, and (ii) eam con ol suppo ed on a
supe ised scheme o e a se o neighbo ing ela ions among he eamma es, accoun ing
o hei ela i e mo ion. Each obo moni o s i s own neighbo ing ela ions o ele an
changes and adap s i s mo ion o he objec i es o he eam using a ini e s a e au oma on
supe iso .
In he same yea , [60] p oposed a eedback con ol s a egy ha achie es con e gence
o a mul i-agen sys em o a desi ed o ma ion con igu a ion while a oiding collisions.
The collision a oidance objec i e was handled by a decen alized na iga ion unc ion
ha anishes when he desi ed o ma ion ends o be ealized. He e, i was shown ha
unde ce ain assump ions, o ma ion in easibili y o ces he agen s’ eloci y ec o s o a
common alue a s eady s a e. This p o ides a connec ion be ween o ma ion in easibili y
and locking beha io o he mul i-agen sys em.
Ano he amous applica ion o he beha io -based app oach can be seen in obo soc-
ce compe i ions. He e, [61] shows ha mul i- obo coo dina ion is one c ucial aspec
in obo ic socce . The way each eam coo dina es i s indi idual obo s in o coope a i e
global ac ions de ine he ounda ion o i s s a egy. In hei wo k he au ho s explain
how he obo s om he CAMBADA eam explo e hei beha io o win a ma ch. He e,
each obo is an independen agen . I coo dina es i s ac ions wi h i s eamma es h ough
communica ion and in o ma ion exchange. The esul ing beha io o he indi idual obo
should be in eg a ed in o he global eam s a egy, hus esul ing in coope a i e ac ions by
all he obo s. New oles we e c ea ed o add o he eam s a egy and some o he p e-
ious exis ing oles we e imp o ed. Some o he exis ing beha io s we e also imp o ed
o be e i he desi ed goals. Each ole and beha io is desc ibed as well as he changes
made.
A p oblem called he ini ial o ma ion p oblem, wi hin he mul i- obo ask alloca ion

12 S a e o he A
domain is add essed in [62]. This p oblem consis s in deciding which obo should go o
each o he posi ions o he o ma ion in o de o minimize an objec i e. Two di e en
dis ibu ed algo i hms ha sol e his p oblem a e explained in [62]. The second algo i hm
p esen s a no el app oach ha uses cos means o model he cos dis ibu ion and imp o es
he pe o mance o he ask alloca ion algo i hm. Also, he au ho s p esen an app oach
ha in eg a es dis ibu ed ask alloca ion algo i hms wi h a beha io -based a chi ec u e o
con ol o ma ions o obo eams.
Many decen alized a chi ec u es use beha io -based app oach in o ma ion con ol.
In [63], a decen alized o ma ion con ol was p oposed which enables collision ee co-
o dina ion and na iga ion o agen s. The au ho s p esen ed a simple me hod o de ine
he o ma ion o mul i-agen s and indi idual iden i ies (IDs) o agen s. Two decen al-
ized coo dina ion and na iga ion echniques we e p oposed o he o ma ion o o e s.
Agen s decide hei own beha io s onboa d depending upon he mo ion ini ia i e o he
mas e agen o he o ma ion. In hese app oaches, any agen can es ima e he beha io
o o he agen s in he o ma ion. This educes he dependency o indi idual agen on o he
agen s while aking decisions. These app oaches also educe he communica ion bu den
on he o ma ion whe e only he mas e agen b oadcas s i s mo ion s a us pe sampled
ime. The main idea o his pape was o de elop an adequa e compu a ional model, un-
de which agen s in he o ma ion pe o med wi h he goal o coo dina ing among each
o he .
Recen ly, a beha io -based app oach was implemen ed in [64]. The au ho s we e in-
spi ed by a socie y o animals, which hey s udied, along wi h he coali ion o ma ion o
obo s o de ec ing in usions using game heo y. The au ho s conside coali ion o ma-
ion in a g oup o h ee obo s ha de ec and cap u e in usions in a closed cu e loop. In
hei analy ical model, indi iduals seek alliances i hey " hink" ha hei de ec egions
a e oo sho o gain an in usion cap u ing p obabili y la ge han hei own. The au ho s
also assumed ha coali ion seeking has an in es men cos and ha he o ma ion o a
coali ion de e mines he ou comes o pa i ies, wi h he de ec leng h o a coali ion simply
being he sum o hose o sepa a e coali ion membe s.
Finally, many applica ions beside g ound ehicle applica ions also use he beha io
app oach. Ke iczky e . al. [65] desc ibe he applica ion o a no el me hodology o high-
le el con ol and coo dina ion o au onomous ehicle eams and i s demons a ion on
high- ideli y models o he o ganic ai ehicle de eloped a Honeywell Labo a o ies. The
scheme employs decen alized eceding ho izon con olle s ha eside on each ehicle
o achie e coo dina ion among eam membe s. An app op ia e g aph s uc u e desc ibes
he unde lying communica ion opology be ween he ehicles. On each UAV, in o ma ion
abou neighbo s is used o p edic hei beha io and plan con lic - ee ajec o ies ha
main ain coo dina ion and achie e eam objec i es.
2.2 App oaches in he Fo ma ion Con ol P oblem 13
This app oach is he mos e sa ile o dynamic en i onmen s. Ne e heless, he di -
icul y o model he g oup dynamics causes his app oach o be oo speci ic. Each appli-
ca ion en i onmen equi es a di e en beha io and, he e o e, a di e en model o he
sys em’s dynamics and con olle . Ano he issue add essed o his app oach is he s abili y
o he g oup, which is ha d o gua an ee.
2.2.3 Leade -Following App oach
Finally, he leade ollowing app oach is one o he mos s udied [1], [14], [16], [15], [17],
[5], [18], [19], [11] and [66]. I is based on he exis ence o a leade ( eal o i ual) ha
ollows he p ecise desi ed ajec o y while he o he obo s o he o ma ion jus ollow
i , main aining a p ese dis ance and ela i e posi ion. The leade obo can be a eal obo
(see Fig. 2.3) o i can be a i ual leade such as a a ge (see Fig. 2.4).
Figu e 2.3: Leade -Following Fo ma ion wi hou Ta ge .
Figu e 2.4: Leade -Following Fo ma ion wi h Ta ge .
This app oach was chosen as being he mos adap able o he ac i e a ge acking
p oblem u he discussed in his chap e . He e, many con olle s can be used o main ain
he usually igid o ma ion. In [67], he au ho s in es iga e he con ol and localiza ion
o a he e ogeneous g oup o mobile obo s. The e o e, he nonholonomic leade obo
14 S a e o he A
guides a g oup o nonholonomic ollowe s when he ollowe obo s a e di e en om
he leade obo . The g oup conside ed had se e al inexpensi e senso -limi ed and com-
pu a ionally limi ed obo s, which ollow a leade obo in a desi ed o ma ion o e long
dis ances. Complex sensing and compu a ion we e pe o med by he leade , while he
ollowe s pe o med simple ope a ions unde he leade ’s guidance. This a chi ec u e
allowed ollowe s o be simple, inexpensi e, and o ha e minimal senso s. Theo e ical
and s a is ical analysis o a acking-based localiza ion me hod was p o ided. A simple
ollow- he-leade con ol me hod is also p esen ed, including a me hod o changing ol-
lowe ’s con igu a ion. Ne e heless, in he p oposed a icle, he a e age eloci y o he
g oup is no g ea e han 0,15m/s.
The au ho s in [68] in oduce and discuss he coope a i e leade ollowing ask o
mul i- obo eams. They desc ibe he design and implemen a ion o a dis ibu ed ech-
nique o coo dina e eam le el and obo le el beha io s o his ask, as well as a mul i-
h eaded amewo k o he implemen a ion o a mul i- obo sys em wi h he e ogeneous
sensing capabili ies. This app oach enables obo s o emain in o ma ion as hey deal
wi h o he obs acles ha may appea wi hin he o ma ion. The au ho s show some o he
esul s o he eam implemen a ions in indoo and ou doo en i onmen s.
In [69], a good implemen a ion o a linea con olle can be seen. In hei p oposed
pape , hey s udy he p oblem o modeling and con olling leade - ollowe o ma ion o
mobile obo s. Fi s , a kinema ics model o leade - ollowe obo o ma ion was o mu-
la ed based on he ela i e mo ion s a es be ween he obo s and he local mo ion o he
ollowe obo . Using his model, he ela i e cen ipe al and Co iolis accele a ions be-
ween obo s we e compu ed di ec ly by measu ing he ela i e and local mo ion senso s,
and u ilized o linea ize he nonlinea sys em equa ions. A o ma ion con olle , consis -
ing o a eedback linea iza ion pa and a sliding mode compensa o , was designed o
s abilize he o e all sys em including he in e nal dynamics. The con ol gains we e de-
e mined by sol ing a obus ness inequali y and assumed o sa is y a coope a i e p o ocol
ha gua an ees he s abili y o he ze o dynamics o he o ma ion sys em. The p oposed
con olle gene a ed he commanded accele a ion o he ollowe obo and made he
o ma ion con ol sys em obus o he e ec o unmeasu ed accele a ion o he leade
obo . Fu he mo e, a obus adap i e con olle was de eloped o deal wi h pa ame ic
unce ain y in he sys em.
Many o he examples o linea con olle s applied o he leade - ollowing app oach
eme ged in he las decade such as [70], [71], [72] and [73]. Ne e heless, in many cases
his canno be gene alized o some imes a e no e en easible [74], [75]. The p oblem
wi h linea con olle s o o ma ion con ol p oblems is he cons ains o he o ma ion
g oup. I he ajec o y is dynamically gene a ed o he e a e dynamic unce ain ies, his
ype o con olle does no gene a e as good esul s as nonlinea con olle s.
2.2 App oaches in he Fo ma ion Con ol P oblem 15
A new posi ion eedback based o ma ion con ol me hod o he e ogeneous mul i-
obo eams is p esen ed and e alua ed in he pape om [76]. The o ma ion beha io s
a e in eg a ed wi h dynamic e e ence objec based collabo a i e na iga ion and e icien
obs acle a oidance o main ain and change o ma ion eal- ime. The me hod om [76]
is compu a ionally e icien and easy o coo dina e in he e ogeneous sys ems. The ime
o o malize and swi ch speci ied o ma ion pa e ns can be con olled by adjus ing he
posi ion eedback pa ame e . Sa is ac o y expe imen al esul s a e ob ained in simula ion
and eal he e ogeneous mul i- obo sys ems which consis o au onomous ehicles and
legged obo s.
Linea con olle s conside ing dynamic cons ains ha e also been conside ed in leade -
ollowing o ma ion con ol [77]. In [78], he au ho s de eloped a dis ibu ed acking
con ol scheme wi h dis ibu ed es ima o s o a leade - ollowe mul i-agen sys em wi h
noise measu emen and di ec ed in e connec ion opology. I was supposed ha each ol-
lowe could only measu e he ela i e posi ions o i s neighbo s in a noisy en i onmen ,
including he ela i e posi ion o he second-o de ac i e leade . A neighbo -based ack-
ing p o ocol oge he wi h dis ibu ed es ima o s was designed based on a no el eloci y
decomposi ion echnique. I was shown ha he closed loop acking con ol sys em is
s ochas ically s able in mean squa e and he es ima ion e o s con e ge o ze o in mean
squa e as well.
A inal app oach in he leade - ollowe a ea is he Model P edic i e Con olle (MPC).
I has been he a ge o s udies in mul i- obo mo ion con ol o almos a decade [1]. The
MPC is p o en o be mo e e ec i e han o he ypes o linea con olle s specially in low
dynamic sys ems and mul i- a iable sys ems. Amongs o he easons o use he MPC,
he e a e he ollowing:
•As he u u e is known ( e e ence beha io ) i is possible o p edic u u e sys em
beha io s;
•The gene ic s uc u e o he MPC o e s he possibili y o con olling di e en ypes
o obo s;
•The obo ’s dynamics can be conside ed mo e o less p ecise depending on he ap-
plied sys em;
•The nonlinea i ies can be included in he p edic ion model;
•The p edic ion model can be swi ched while main aining he same con ol s uc u e;
•The MPC can be implemen ed in a dis ibu ed ashion;
•In a single op imiza ion p oblem, ene gy cos s, obs acle a oidance and o he issues
can be conside ed;
22 S a e o he A
2.4 Ac i e Ta ge T acking P oblem
Op imally acking a mo ing a ge unde mo ion and p ocess cons ain s is necessa y in
a numbe o applica ions such as su eillance, en i onmen al moni o ing, de ense appli-
ca ions and so on. In mos s udies abou a ge acking, he senso s in ol ed a e s a ic
and he emphasis is on he op imal p ocessing o he a ailable in o ma ion. In con as
o using s a ic senso s, he deploymen o mobile senso s (o obo s) o acking o e s
signi ican ad an ages. Fo example, a la ge a ea can be co e ed wi hou he need o
inc ease he numbe o nodes in he sensing ne wo k. The idea o op imally choosing he
mobile senso s’ loca ions in o de o maximize in o ma ion gain (also known as adap i e
sensing o ac i e pe cep ion) has been applied o he p oblems o coope a i e localiza-
ion [100], simul aneous localiza ion and mapping [101], pa ame e es ima ion [102], and
op imal senso selec ion [103].
Yang e al. [24] p esen an ac i e sensing s a egy using dis ance-only measu emen s,
whe e bo h he ace and he de e minan o he a ge posi ion es ima es o he co a i-
ance a e conside ed as he objec i e unc ions. The au ho s p opose a con ol law, wi h
cons an s ep size, based on he g adien o he cos unc ion wi h espec o each senso ’s
coo dina es.
In [21], Ma ínez and Bullo add ess he p oblem o op imal senso placemen and mo-
ion coo dina ion s a egies o homogeneous senso ne wo ks using dis ance-only mea-
su emen s, whe e he emphasis is on he op imal senso placemen o (non andom) s a ic
a ge posi ion es ima ion. The objec i e is o minimize he de e minan o he co a iance
ma ix. The esul ing con ol law equi es ha he senso s mo e on a polygon su ound-
ing he a ge so ha he ec o s om he a ge o he senso s a e uni o mly (in e ms o
di ec ion) spaced.
Recen ly, F ew [104] has in es iga ed he p oblem o single senso ajec o y gene a-
ion o a ge acking using bea ing measu emen s. In hei discussed p oblem, mo ion
cons ain s on he senso ’s ajec o y a e explici ly inco po a ed in he p oblem o mu-
la ion and he objec i e unc ion (de e minan o he a ge ’s co a iance ma ix) is min-
imized o e a ini e ime ho izon using exhaus i e sea ch h ough a disc e ized se o
candida e senso headings.
Ol a i-Sabe [23] add esses he p oblem o dis ibu ed a ge acking o mobile sen-
so ne wo ks wi h a dynamic communica ion opology. The au ho ackles he ne wo k
connec i i y issue using a locking-based mobili y model and p esen s a modi ied e sion
o he dis ibu ed Kalman il e algo i hm o es ima ing he a ge ’s s a e. In his case,
he senso s use bo h dis ance and bea ing measu emen s o a a ge ha mo es in 2D
wi h cons an eloci y d i en by ze o-mean Gaussian noise, and seek o minimize hei
dis ances o he a ge , while a oiding collisions.
Chung e al. [22] p esen a decen alized mo ion planning algo i hm o sol ing he

2.5 Discussion 23
mul i-senso a ge acking p oblem using bo h dis ance and bea ing measu emen s. The
au ho s employ he de e minan o he a ge ’s posi ion co a iance ma ix as he cos
unc ion. The decen alized con ol law in his case is based on he g adien o he cos
unc ion wi h espec o each o he senso ’s coo dina es wi h cons an s ep-size o 1.
The main d awback o he p e ious app oaches is ha no physical cons ain s on he
mo ion o he senso s a e conside ed. The only excep ions a e he wo ks p esen ed in
[105] o dis ance-and-bea ing obse a ions, and in [104] o bea ing-only obse a ions.
Howe e , in bo h cases, he p oposed g id-based exhaus i e sea ch algo i hm, when ex-
ended o he mul i-senso case, has compu a ional complexi y exponen ial in he numbe
o senso s, which becomes p ohibi i e when he numbe o he senso s is la ge and/o he
size o he g id cell is small. In addi ion, eams o he e ogeneous senso s using mixed
(i.e., dis ance and/o bea ing) ela i e obse a ions a e only conside ed in [24], whose
g adien -based algo i hm can only gua an ee achie ing local minimum, while i s con e -
gence a e is no add essed. Fu he mo e, none o he p e ious wo ks con empla es issues
ega ding he mobile obo s’ o ma ion wi h espec o he mobili y such as obs acles and
ma es collision a oidance, ene gy issues, online con ol law changing and so on.
2.5 Discussion
This chap e p esen ed wo dis inc p oblems: he o ma ion con ol p oblem and he ac-
i e a ge acking p oblem. S udies usually conside one p oblem o he o he sepa a ely.
Ne e heless, in some si ua ions hese wo p oblems can appea oge he , making he solu-
ion o ei he one o hem ha d o concei e. Fo example, in he o ma ion con ol p oblem
(wi h he leade - ollowing app oach) usually he o ma ion mus be wi h p e-de ined dis-
ances be ween he obo s ( igid o ma ion) and he con olle is said o be wo king i he
o ma ion can be kep du ing a p e-de ined ajec o y. In he ac i e a ge acking p ob-
lem he issue lies wi hin he obse a ion o a a ge dis ega ding a o ma ion be ween he
obo s. An op imal obse a ion is he main ask in his las p oblem.
The e o e, he p oblem add essed in his hesis lies in he bounda ies be ween he o -
ma ion con ol p oblem and he ac i e a ge acking p oblem. The p oblem he e is o
concei e a o ma ion con olle capable o con ol a mul i- obo sys em in a dis ibu i e
ashion conside ing obs acle and ma es a oidance, he o ma ion i sel and he maximiza-
ion o he a ge ’s obse a ion by he g oup o obo s in o ma ion.
A e a ca e ul analysis o he s a e o he a , he nonlinea model p edic i e con ol
heo y was conside ed as he main heo e ical app oach. Amongs he easons ha makes
he nonlinea model p edic i e con ol heo y he mos sui able o sol e he add essed
p oblem, he ollowing a e he ele an ones:
24 S a e o he A
•Some imes he u u e e e ence beha io is known and i is possible o use his
in o ma ion o p edic u u e beha io s o he sys em;
•By ob aining he same gene ic s uc u e o he NMPC, he con ol o di e en ypes
o obo s becomes possible while maximizing he obse a ion wi h di e en ypes
o senso s and mobili y in he o ma ion;
•The nonlinea i ies can be included in he p edic ion model. As an example, he o al
amoun o unce ain y was p edic ed o each obo and minimized in he con olle ’s
cos unc ion;
•A change in he model o he a ge ’s obse a ion can be easily made in he con-
olle ’s code gene alizing he applica ion o di e en senso s;
•Ene gy cos s, a oidance o obs acles, unce ain ies and o ma ion cha ac e is ics can
be conside ed in a single op imiza ion p oblem;
•Di e en beha io s and ac ions can be changed by a simple modi ica ion in he cos
unc ion pa ame e s allowing o be e obse a ion and a change in he o ma ion;
•Rega ding he o ma ion geome y, a ailable o ma ion con ol me hods o en gi e
li le ele ance o he equi emen s imposed by a ge localiza ion and/o acking.
Finally, among he disad an ages, he NMPC has he ollowing:
•I is di icul o heo e ically p o e he sys ems’ s abili y;
•I possesses easonable compu a ional cos s in he sys em’s p ocessing;
•In NMPC, he model o he sys em mus be minimally ealis ic (an accu a e model
o he sys em) in o de o gua an ee accu a e p edic ions o he sys em’s beha io .
Chap e 3
Robo s and Expe imen al Se up
Desc ip ion
This chap e p esen s he obo s used in his hesis. I also explains he simula ion and
expe imen al se ups. Finally, he simula o SimTwo and he alida ion o he obo model
in his en i onmen a e p esen ed.
3.1 In oduc ion
The s udy on mobile obo ics is one o he mos g owing and in es iga ed opics in
obo ics. I s applica ion is ele an in a la ge se o en i onmen s such as indus ial,
medical and domes ic. In his chap e , aspec s o he g ound mobile obo s used as a
mechanism o alida e he p oposed con olle will be p esen ed.
The espec i e used models a e u he b ie ly desc ibed in his chap e . The e o e, his
chap e is s uc u ed p esen ing a b ie conside a ion abou mobile obo s in he ollowing
sec ion. The mul i- obo sys em used is p esen ed in sec ion 3.3. Then, he SimTwo
en i onmen is explained in sec ion 3.4. In sec ion 3.5 he model o he 5dpo obo is
alida ed in he SimTwo en i onmen and he conclusion is p esen ed in he inal sec ion.
3.2 Mobile Robo s
A mobile obo mus be capable o mo e i sel h ough an en i onmen , a oiding colli-
sions wi h s a ic o mo ing obs acles in i s ajec o y o each he objec i e. The obo
can be a wa e obo , an ai obo o a g ound mobile obo . The au onomous g ound
mobile obo s (o simple mobile obo s) can ha e a a ie y o locomo ion elemen s such
as wheels, olle balls, legs o c awle s.
25
26 Robo s and Expe imen al Se up Desc ip ion
This wo k uses wo kinds o mobile obo s wi h wheels, called holonomic and non-
holonomic obo s. The e a e a a ie y o wheels classi ied in i e ca ego ies by [106]:
no mal s anda d wheels, s anda d maneu e wheels, bea e wheels, sphe ic wheels and
omnidi ec ional wheels. Each ype o wheel has a di e en deg ee o eedom. The non-
holonomic mobile obo s p esen some cons ains on hei wheels ha holonomic obo s
do no . The omnidi ec ional wheels a e used in holonomic obo s and he no mal s anda d
wheels a e used in mos indus ial AGVs (Au oma ed Guided Vehicles).
The mechanical s uc u e o an holonomic mobile obo is designed o con ain h ee
o mo e mo o s connec ed o an omnidi ec ional wheel (see Fig. 3.1). In his hesis,
he omnidi ec ional obo s ha e h ee wheels dis ibu ed in 120 deg ees. These wheels
ha e small olle s on hei su ace which lowe he ic ion o la e al slippe ing enabling
o hogonal mo emen o he adi ional di ec ion.
Figu e 3.1: Omni-di ec ional Wheel
In a nonholonomic mobile obo , he mechanical s uc u e can assume a much la ge
a ie y o o ms. The ones mos used ha e ou wheels, as i can be seen in [107]. The ge-
ome y, he numbe o wheels and hei geome ic posi ions on he chassis o he obo a e
esponsible o he obo ’s mechanical s abili y, capaci y o maneu e (maneu e abili y)
and i s deg ee o mobili y.
3.3 The Mul i-Robo Sys em
Se e al obo ic asks equi e o bene i om he coope a ion o mul iple obo s: ans-
po a ion o la ge-sized objec s, la ge a ea co e age (e.g., o cleaning) o su eillance
(e.g., o i e de ec ion), pollu an plume acking, o a ge de ec ion and acking, o
name bu a ew. Usually, in mul i- obo sys ems, he o ma ion can be igid o no , and
he con ol s a egy can be cen alized o dis ibu ed. Also, he o ma ion mus ha e low
compu a ional and powe equi emen s and high communica ion e iciency.
This esea ch used an indoo highly dynamic and ad e sa ial en i onmen o he e o-
geneous and homogeneous mul i- obo sys em wi h a common a ge ( acking a ball),
o en subjec o occlusions and kidnappings. The used obo s a e omnidi ec ional obo s
om he middle size league (see Fig. 3.2) o obo socce championships. Fu he mo e,
a di e en ial obo is used o demons a e he applica ion o he same con olle in a non-
holonomic obo (see Fig. 3.3).
3.3 The Mul i-Robo Sys em 27
Figu e 3.2: The 5DPO Mobile Socce Robo s
3.3.1 Mini-AGV Robo
In Fig. 3.3 he au oma ed guided ehicle (AGV) obo also used in his wo k, he e called
Mini-AGV, can be seen. The main cha ac e is ic o he non-holonomic Mini-AGV mobile
obo is he ac ha i possesses a di e en ial mechanical ac ion. This sys em consis s
o a se o wo no mal s anda d wheels coupled in wo con inuous cu en (CC) mo o s,
se in he same cen al axis and symme ic opposi e o one ano he . To gi e s abili y
and suppo o he s uc u e, wo ee wheels a e also placed in he obo ’s s uc u e.
This con igu a ion can be seen in a simple locomo ion mechanism, allowing he pla o m
eedom in i s mo emen in compa ison o i s size, such as he abili y o o a e in i s own
axis. The kinema ic and dynamic models o his mobile obo , as well as he use o he
SimTwo simula o , can be seen in [107].
Figu e 3.3: The Mini-AGV Robo
3.3.2 The Middle-Size League 5dpo Robo
The 5dpo is he RoboCup middle size league socce eam o he FEUP and INESC TEC,
Po ugal. The cons uc ion o his obo was he esul o a ious people wo king on se -
e al di e en a eas which con ibu ed o he de elopmen o he mechanical s uc u e o

28 Robo s and Expe imen al Se up Desc ip ion
he obo , i s ha dwa e a chi ec u e and con olle s, as well as he so wa e de elopmen in
a eas such as image analysis and p ocessing [108], [109], eal- ime pa h planning [110],
modeling [111], [112] and con ol [113] [114]. Ne e heless, i is impo an o men ion
ha , despi e hese obo s a e used in his hesis as a es bed o he con olle p oposed
he e, many o he ea u es desc ibed bellow such as he RTDB and he o ma ion con-
olle i sel a e no used in he RoboCup games, bu in he p ojec o which his hesis is
a pa o (FCT p ojec PTDC/EEA-CRO/100692/2008 - Pe cep ion-D i en Coo dina ed
Mul i-Robo Mo ion Con ol).
Figu e 3.4: The Gene al A chi ec u e
Fig. 3.4 shows ha he Dec is he con ol so wa e, HAL is he ision sys em applica-
ion, FlashBus is he ha dwa e communica ion applica ion, Coach is a cen al compu e
applica ion o isualize and log he o ma ion ac i i ies and posi ions, RTDB is he com-
munica ion p o ocol, COMM is he communica ion applica ion, Enciis he encode o
mo o iand Miis he mo o i.
The gene al a chi ec u e o he 5dpo obo is p ima ily based on a mul i-agen sys em.
Each agen is placed in a main p ocessing uni (a lap op), which is esponsible o he
high-le el coo dina ion. This main p ocessing uni handles ex e nal communica ion wi h
o he obo s, in e media ed by he sha ed eal ime da a base (RTDB) h ough wi eless
communica ion. All obo s ead and w i e hei sha ed a iables on hei local eal ime
da a base (RTDB), which is hen b oadcas ed by an ad-hoc communica ion sys em [115].
This uni ( he abo e men ioned lap op), besides ha ing a ision sys em di ec ly a ached
o i , ecei es also a low bandwid h sensing in o ma ion and sends ac ua ing commands
o con ol he obo ac ions by means o a dis ibu ed low-le el sys em (Fig. 3.4).
The low le el sensing/ac ua ing sys ems ollows a dis ibu ed model whe e mos o
3.3 The Mul i-Robo Sys em 29
elemen a y unc ions (closed-loop PID con ol o he ac ua o s) a e encapsula ed in small
mic ocon olle based nodes connec ed o a mo he boa d by a RS485 elec ic speci ica-
ion. Fo his pu pose, a p o ocol called FlashBus was c ea ed o suppo communica ion
o exis be ween he main p ocessing uni and he mic ocon olle based nodes.
3.3.2.1 Vision Sys em
The ision applica ion is called HAL (Ha dwa e Abs ac ion Laye ) and i communica es
wi h he Dec (con ol so wa e o he obo s) by local UDP p o ocol. The HAL in e ace
can be seen in Fig. 3.5. The unc ion o his applica ion is o ea he images cap u ed by
he came a and send o he ea u es o he Dec so wa e o be p ocessed. The ea u es a e
ela ed o ield lines, ball and obs acles.
Figu e 3.5: The HAL So wa e
3.3.2.2 The Coach So wa e
The Coach so wa e (Fig. 3.6) se s he oles o each obo in he eam and pe o ms
he selec ion o he "bes " ball. In he Coach was inse ed he in elligen s a e machine
desc ibed in chap e 6. This s a e machine ac i a es he oles ha each obo mus execu e.
The Coach sha es he oles ha each should ake and which ball om he obse ed balls
is he "bes ", o he ue ball. In his same ashion, i collec s he in o ma ion om he
obo s abou hei posi ion wi h espec o he ball.
As i can be seen in Fig. 3.6, he colo s a us is g een when he obo is ac i e and
eady. I changes o ed when he obo is inac i e. Fu he mo e, he Coach map shows
he posi ion o each obo in he ield he loca ion o he ue ball.
30 Robo s and Expe imen al Se up Desc ip ion
Figu e 3.6: Coach Applica ion
3.3.2.3 The Dec So wa e
The Dec so wa e compu es each obo ’s decision. He e a e implemen ed he Roles, Tasks
and Ac ions laye s. Th ough he command ecei ed om he Coach using he RTDB, he
obo akes a speci ic Role, and hen execu es he p ope Task and Ac ion p og ammed
o ha unc ion (Role). The Dec en i onmen can be seen in Fig. 3.7.
Figu e 3.7: The Dec So wa e
In Dec so wa e all he in o ma ion om he senso s (odome y, ea u es ecei ed om
he ision so wa e, compass, e c.) is p ocessed. The p og am also obse es which Role
3.4 The SimTwo Simula ion En i onmen 31
is gi en by he Coach. This Role is ele an o a se ies o asks ye o be done (pa h
gene a ion, ajec o y acking con olling , obs acle a oidance, kicking s eng h, e c.).
The Roles a e he designed unc ions o each playe . The unc ions ha each playe
ecei es a e gi en by he Coach so wa e in ag eemen wi h a se o condi ions, such
as: i s physical cha ac e is ics, i s posi ion in he ield, e c. Examples o oles a e he
ollowing: o wa d, de ense, keepe , e c. In his wo k only h ee oles a e used: Sea ch,
Sea chFollowe and Fo ma ion u he explained in chap e 6.
Each oles execu e a se o Tasks. Tasks a e jobs ha each playe mus execu e, de-
pending on i s unc ion. The ac o passing he ball o a eamma e o o block an ad e sa y
playe a e good examples o asks. Fu he mo e, each Task means he execu ion o a se
o Ac ions. The Ac ions a e esponsible o he ajec o y acking con ol, ajec o y
gene a ion, obs acle de ec ion, kicking s eng h, e c.
3.4 The SimTwo Simula ion En i onmen
The SimTwo simula o c ea es he eplica ion o he en i ies ha popula e he wo ld, based
on a se o .xml (Ex ensible Ma kup Language) iles ha a e ead a he ime o he appli-
ca ion s a up. These s a es can be ead om a sc ip , hus gi ing ise o he mo emen o
he wo ld.
Figu e 3.8: The SimTwo Main Windows
The simula ion pla o m has been de eloped wi hin he g oup o mobile obo ics 5dpo
by [116] and i is a ee so wa e a ailable o download [117]. This pla o m shows a
high le el o ealism, especially in e ms o dynamic models. These include nonlinea
cha ac e is ics, which a e p o en impo an o he beha io o eal objec s. The simula o
also allows he iewing o eal- ime 3D simula ion, which is bene icial o he supe ision
o he simula ion. A sc een sho o he simula o windows is shown in Fig. 3.8. The
38 Robo s and Expe imen al Se up Desc ip ion
3.5 Model Valida ion
In his sec ion, he me hodology o he 5dpo obo model alida ion is p esen ed. The
5dpo obo was se o each wo poin s in he ield, pe o ming a di icul ajec o y in a
desi ed eloci y (1m/s) o alida e he model. A PI (P opo ional plus In eg al) con olle
was used o he mo o s’ eloci y in his wo se s o expe imen s [113]. Acco ding o Fig.
3.14, he ajec o y de e mined o he pe o mance o hese es s is L-shaped.
Figu e 3.14: T ajec o y o Model Valida ion
2.6 2.8 3 3.2 3.4 3.6 3.8 4 4.2
−2.4
−2.2
−2
−1.8
−1.6
−1.4
−1.2
X [m]
Y [m]
Model Valida ion wi hou Ta ge O ien ed and V=1 m/s (X/Y)
Simula ion Da a
Real Da a
Figu e 3.15: Tes wi h 1m/sand θ=0◦
The 5dpo obo was se o pe o m he abo e desc ibed ajec o y en imes. The en
pe o mances we e di ided in o wo se s. A oo came a was used as g ound u h ( eal
posi ion da a), o i s p ecision in he wo ld ame posi ion o he obo is much highe
han he one o he obo ’s senso s ( he e o o he oo came a is less han 0.5cm). The
i s se o es s we e conduc ed wi hou a ying he obo ’s angle (wi h θ=0◦). The
a e age esul achie ed om he i s se o es s can be seen in Fig. 3.15.
In Fig. 3.15, he compa ison be ween he beha io o he obo in SimTwo en i on-
men (blue aced line) and he eal obo en i onmen (black ull line) can be wi nessed.

3.6 Conclusion 39
2.6 2.8 3 3.2 3.4 3.6 3.8 4 4.2
−2.4
−2.2
−2
−1.8
−1.6
−1.4
−1.2
X [m]
Y [m]
Model Valida ion wi h Ta ge O ien ed and V=1 m/s (X/Y)
Simula ion Da a
Real Da a
Figu e 3.16: Tes wi h 1m/sand θ=Va iable
The beha io o he wo esul s (simula ed and eal), dis ega ding con ol issues, is simi-
la ; his ac can be wi nessed by analyzing he p oximi y o bo h ajec o ies.
The second se o es s we e conduc ed while a ia ing he θin a way ha he obo
aced i s mo ing di ec ion. The accu acy o he model can be no ed also in Fig. 3.16.
This igu e also shows he compa ison be ween he beha io o he obo in SimTwo en i-
onmen (blue aced line) and he eal obo en i onmen (black ull line). The p oximi y
o bo h ajec o ies is a poin which is ele an o be obse ed in bo h igu es.
Al hough he g aphs show he p oximi y be ween he ajec o ies, o mal analysis mus
be pe o med o compa e simula ion and eal esul s in bo h g oups. The e o e, he mean
squa e e o was used o analyze he model beha io . A e small adjus men s in he
pa ame e s, he minimum alues ound du ing all he es s o bo h g oups can be seen in
able 3.4.
Table 3.4: Mean Squa e E o o Model Valida ion T ajec o ies
T a jec o y MSE in x MSE in y
θ=0 0.1368 0.0428
θ= a ing 0.0856 0.0265
This model was hen used in he simula ions wi h he con olle which can be seen in
he ollowing chap e s.
3.6 Conclusion
This chap e p esen ed he mobile obo s used in his esea ch: he middle size league
omnidi ec ional 5dpo obo and he di e en ial mobile obo . A de ailed desc ip ion o
he simula o SimTwo was also p esen ed while demons a ing i s ad an ages. Finally,
40 Robo s and Expe imen al Se up Desc ip ion
he 5dpo mobile obo model implemen ed in he SimTwo simula o was alida ed by
compa ing he simula ions wi h eal expe imen s wi h he eal obo .
Chap e 4
Fo ma ion Con ol in Ac i e Ta ge
T acking
This chap e desc ibes a no el app oach in o ma ion con ol o mobile obo s in he ac-
i e a ge acking p oblem. A nonlinea model p edic i e o ma ion con olle (NMPFC)
o a ge pe cep ion was implemen ed o con e ge a g oup o mobile obo s owa ds a de-
si ed a ge . The eam mus also main ain a desi ed o ma ion ollowing a a ge while i
is mo ing, o ollow a leade in he case o a ge absence. The s uc u e de ails o he
con olle , as well as a ma hema ical analysis o he o ma ion model used, a e p esen ed
in his chap e . As a inal objec i e o his chap e , he weigh uning is add essed in o de
o minimize an objec i e unc ion ha e lec s he con olle ’s e iciency wi h espec o
a gi en c i e ia. Fu he mo e, esul s o simula ions and expe imen s wi h eal obo s a e
p esen ed and discussed.
4.1 In oduc ion
The main objec i e o his chap e is o p esen a no el nonlinea model p edic i e o -
ma ion con ol (NMPFC). This wo k is inse ed in he ac i e a ge acking [27], [28],
[29], [30] and o ma ion con ol p oblems [32], [33], [13], [34], [35], [36]. The e o e,
his con olle was concei ed o con e ge and keep he o ma ion o a mobile obo eam
owa ds a a ge . The examples and es s will be pe o med by using a homogeneous
g oup o obo s o med om 5dpo omnidi ec ional mobile socce obo s [123] (Fig. 3.2),
and also using he e ogeneous g oup o obo s o med om 5dpo holonomic obo s and
he nonholonomic obo seen in Fig. 3.3. This con olle can be applied o N obo s in
o ma ion.
41
42 Fo ma ion Con ol in Ac i e Ta ge T acking
Mos o he pas and cu en wo k on mo ion coo dina ion o mul iple (possibly he -
e ogeneous) ehicles ocuses on con olling a ehicle o ma ion wi h a gi en nominal ge-
ome y and a p e-de e mined ajec o y o a s a ic des ina ion loca ion, possibly complian
wi h he p esence o obs acles on he o ma ion ajec o y [95]. Such me hods ypically:
•assume ull knowledge o he o ma ion s a e, exp essed as he ela i e dis ances
and bea ings among all he ehicles, and/o
• ely on local memo y-less in e ac ions, o en jeopa dizing global o ma ion s abili y.
The o ma ion used in his hesis di e s om he usual igid o ma ion whe e he
ela i e pose o a eam elemen mus be p ecisely main ained. He e, he ideal con igu-
a ions a e he ones ha minimize a cos unc ion whe e i penalizes he o al amoun o
unce ain y in he eam pe cep ion o an elemen ( a ge ), con e ges he obo s owa ds
his elemen in a desi ed pose, a oids collisions wi h ma es and obs acles and penalizes
oscilla ions in he con ol e o .
A ehicle o ma ion is supposed o se e one o mo e mission objec i es [124], such as
sea ch escue missions and land mine emo al. One such in e es ing case conce ns local-
izing o acking ele an objec s, he e and hence o h denomina ed as a ge s. A ailable
o ma ion con ol me hods o en gi e li le ele ance o he equi emen s imposed by a -
ge localiza ion and/o acking o he o ma ion geome y, so as o imp o e he a ge
de ec ion and acking quali y (e.g., accu acy). Many con ol s a egies ha e been in es-
iga ed in o de o sol e o ma ion con ol p oblems (e.g., [20], [96] and [125]), some o
which p esen nonlinea model p edic i e con ol (NMPC) solu ions.
The NMPC is based on a ini e ho izon con inuous ime minimiza ion o nonlinea
p edic ed acking e o s wi h cons ain s on he con ol inpu s and he s a e a iables. I
p edic s sys em ou pu s based on cu en and u u e s a es using a model o he sys em.
Then, i inds an open-loop con ol p o ile by nume ical op imiza ion, and applies he i s
con ol signal in he op imized con ol p o ile o he sys em. Howe e , due o he use o a
ini e ho izon, con ol s abili y becomes one o he main p oblems. To gua an ee con ol
s abili y, many app oaches ha e been in es iga ed, e.g., using e minal egion cons ain s
and/o a e minal penal y e m [126], [127].
In [98], a wo laye s p edic i e con olle ha con ols he o ma ion o nonholonomic
mobile ehicles was p oposed. In hei s udy, he au ho s conside ed ha he e a e wo
sub-p oblems o be sol ed o ul ill he main goal: he ajec o y con ol p oblem and
he o ma ion con ol p oblem. To sol e he i s sub-p oblem a nonlinea con olle was
p oposed o con ol he ajec o y, while a linea model p edic i e con olle was p oposed
o sol e he second sub-p oblem and con ol he o ma ion.
Mo eo e , he au ho s in [128] p opose a dis ibu ed model p edic i e con ol scheme
based on a coope a i e game in which wo di e en agen s communica e in o de o ind
4.2 Con ol A chi ec u e 43
a solu ion o he p oblem o con olling wo cons ained linea sys ems coupled h ough
he inpu s. They assume ha each agen only has pa ial in o ma ion o he model and
he s a e o he sys em. In hei p oposed scheme, he agen s communica e wice each
sampling ime in o de o sha e enough in o ma ion o ake a coope a i e decision. The
heo e ical esul s and he design p ocedu e a e illus a ed using wo di e en examples.
Usually, he s a e o he a esea che s s udy igid o ma ion on eams o obo s. In
said s udies [58], he ela i e poses be ween obo s a e ixed. This chap e p esen s an
app oach whe e he desi ed o ma ion is no igid due o he need o he obo s o epo-
si ion and eo ien hemsel es o be e es ima e a a ge and simul aneously a oid ma es,
obs acles and so on.
The e o e, he p oblem add essed in his chap e lies in he on ie be ween he o -
ma ion con ol p oblem and he ac i e a ge acking p oblem. A e a ca e ul analysis o
he s a e o he a , he nonlinea model p edic i e con ol heo y, as s a ed and explained
in chap e 2, was conside ed as he main heo y app oach o his wo k. This chap e is
hen s uc u ed as ollows: he nex sec ion explains he adop ed con ol a chi ec u e. The
nonlinea model p edic i e o ma ion con ol is p esen ed and explained in sec ion 4.3,
he op imiza ion algo i hm is p esen ed in sec ion 4.4, he ollowing sec ion p esen s a
me hodology o weigh uning. The esul s a e hen p esen ed in sec ion 4.6 and he
conclusion is p esen ed in he las sec ion.
4.2 Con ol A chi ec u e
The nonlinea model p edic i e con ol is usually implemen ed in a cen alized ashion.
I holds ull knowledge o he en i e sys em and compu es all he con ol ou pu s. A
cen alized con ol using a non-con ex op imiza ion scheme applied in la ge-scale in e -
connec ed sys ems, such as wa e dis ibu ion sys ems, a ic and powe sys ems, man-
u ac u ing sys ems and economic sys ems, may be a oo complex solu ion o no e en
easible. Wi h he quick de elopmen o communica ion ne wo ks, cen alized con ol
has been g adually eplaced by dis ibu ed con ol such as in mul i- obo sys ems and
applica ions in manu ac u ing and p ocess indus ies whe e mul iple uni s coope a i ely
p oduce a good. In dis ibu ed con ol schemes, agen s sha e in o ma ion in o de o im-
p o e closed-loop pe o mance, obus ness and aul - ole ance [128]. In he app oach
desc ibed in his hesis, he agen s ( obo s) sha e hei local measu emen s (s a es) and
ecei e he o he agen s’ s a es, compu ing a con ol inpu using a educed o de model o
he o ma ion sys em dynamics. The challenge in his case is o o mula e a simple and
decen alized p oblem which leads o a beha io ha is simila o he one ob ained using
a cen alized app oach [38].
The objec i e o his chap e is o o mula e a nonlinea model p edic i e o ma ion
con olle (NMPFC) o a mul i- obo sys ems o ma ion con ol. The gene al s uc u e

44 Fo ma ion Con ol in Ac i e Ta ge T acking
o a coo dina ed mul i- obo sys em can be classi ied in h ee ca ego ies: dis ibu ed,
cen alized, o hyb id (pa ially dis ibu ed). These classi ica ions a e based on how he
con ol signals o each obo a e calcula ed. In his case, he con ol a chi ec u e is ully
dis ibu ed [128].
Figu e 4.1: S uc u e o he Nonlinea Model P edic i e Fo ma ion Con olle Applied o obo 1
Fig. 4.1 illus a es he s uc u e o he NMPFC used in his wo k, whe e U(k|k) =
U(k) = h e (k) n e (k)w e (k)iTis he ou pu con ol (see also Fig. 4.3) signal in
he i s p edic ion s ep, ˆ
U(k+i|k)wi h i=0...Nc−1 is he ou pu con ol signal om
he op imize sen o he p edic o , and ˆ
P(k+i|k)wi h i=1...Npis he esponse o he
p edic o block o each ˆ
U(k+i|k). He e, a an ins an k, obo 1 (R1) sends i s pose
PR1(k) = hxR1(k)yR1(k)θR1(k)iT o he NMPFC. Fu he mo e, he NMPFC also
ecei es he o he obo s’ poses [PR2(k)...PRN(k)], he posi ion o he a ge in he wo ld
ame wP (k) = hwx (k)wy (k)iTand he eloci y o he a ge in he wo ld ame
wV (k) = hw x (k)w y (k)iT.
The NMPFC’s abili y o c ea e and main ain a o ma ion is due o he ac ha he cos
unc ions used by he con olle s o each obo in he eam a e coupled. The abo e men-
ioned coupling occu s when he eamma es’ s a es a e used in he cos unc ion o each
obo ’s con olle o penalize he geome y o he de ia ion om he desi ed objec i e.
This means ha he ac ions o each obo a ec e e y o he eamma e. Each obo keeps
he o ma ion s a e (pose and speed o he obo s in o ma ion, and posi ion and speed
o any a ge ha should be ollowed), upda ing hem in each con ol loop. This in o -
ma ion is ecei ed by he con olle o each obo in he o ma ion which in u n c ea es
he o ma ion geome y whe e he ac ions o each obo a ec he o he eamma es. The
4.2 Con ol A chi ec u e 45
NMPFC can be di ided in o wo sub-blocks:
•Op imize - This sub-block uses an online nume ic op imiza ion me hod o min-
imize he cos unc ion and gene a e he signals o op imal con ol. The esilien
p opaga ion (RPROP) me hod is used he e and i gua an ies quick con e gence [99];
•P edic o - The p edic o pe o ms he s a e e olu ion o he obo i sel , he eam-
ma es and he a ge based on p e-de ined models. I uses a simpli ied dynamic
model o emula e he obo ’s e olu ion. The eloci ies o he eamma es and a ge
a e assumed o be cons an and equal o he las known eloci ies du ing he en i e
p edic ion ho izon. The obs acles (mo ing o s a ic) a e assumed o ha e ze o e-
loci ies du ing he con ol loop and he e olu ion o he ela i e dis ance be ween
he obs acle and he obo is p edic ed. The p edic o also emula es he e olu ion o
he a ge ’s me ged s a e co a iance ma ix.
A e ecei ing he s a es o he obo , eamma es, obs acles and a ge , he con olle ’s
op imize sub-block p o ides he con ol inpu ˆ
U(k+i|k), in a limi ed con ol ho izon, o
he p edic o sub-block, which hen p edic s he o ma ion s a e e olu ion ˆ
P(k+i|k) o
Nps eps (p edic ion ho izons), and p o ides a cos alue o he op imize in acco dance
wi h ˆ
U(k+i|k). The i e a i e minimiza ion p ocess is epea ed in cyclic ashion. Finally,
he con ol ou pu in he i s s ep U(k)is sen o he obo .
Figu e 4.2: Con olle Diag am
46 Fo ma ion Con ol in Ac i e Ta ge T acking
4.3 Nonlinea Model P edic i e Fo ma ion Con ol
The nonlinea model p edic i e o ma ion con olle (NMPFC) p oposed in his s udy was
used o con ol he o ma ion o a mul i- obo sys em in he ac i e a ge acking p oblem.
Fig. 4.2 shows he block diag am o he p oposed o ma ion con ol amewo k o
obo R1in a o ma ion wi h N obo s. He e, he subsc ip ed Rnis used o deno e he obo
nwhe e 1 ≤n≤Nand Nis he o al numbe o obo s in o ma ion and he subsc ip ed
o deno e he a ge . Each obo has a NMPFC, a coope a i e a ge es ima o (CTE) [129]
and a eal ime da a base (RTDB) communica ion applica ion [115]. O he unc ions such
as localiza ion and he ision sys em a e embedded in o he so wa e modules ep esen ed
he e as he block O he Modules om Robo 1. A each ins an k he obo R1sends i s
pose (PR1(k)) o he con olle and o he RTDB o be sha ed wi h he o ma ion. The obo
also sends i s pose (PR1(k)), he posi ion o he a ge in i s local ame (R1P (k)) and he
eloci y o he a ge in i s local ame (R1V (k)) o he CTE. Then, he CTE, ep esen ed
as a single block in Fig. 4.2, ecei es om he RTDB he pose, as well as he posi ion
and eloci y o he a ge in he local ame o each o he obo in o ma ion, and also
communica es locally o he NMPFC he in o ma ion on he used a ge posi ion (wP (k))
and he used a ge eloci y (wV (k)), bo h in he wo ld ame. An addi ional laye in
communica ion using a eal ime da a base (RTDB) sha ed memo y makes i possible o
con ey in o ma ion. Then, each obo w i es i s own pose and eads he eamma es’. The
NMPFC also ecei es he pose o each obo in o ma ion ([PR2(k)...PRN(k)]) om he
obo ’s RTDB and hen sends an ou pu con ol signal U(k) o he obo ( he angen ial
and no mal componen s o linea eloci y and angula eloci y).
The obo ’s pose and eloci y es ima ion is pe o med using a localiza ion algo i hm
ha ecei es da a om he odome y, om a digi al compass, and om he omnidi ec-
ional came a. Then, using he omnidi ec ional came a o de ec he whi e lines o he
ield combined wi h a map ma ching algo i hm, he obo is localized. Du ing he mo e-
men , he eloci y is es ima ed also using da a om odome y.
To achie e con e gence in he o ma ion, and hence cos unc ion minimiza ion, he
NMPFC’s p edic o sub-block p oduces he e olu ion o he o ma ion’s beha io , as well
as he beha io o he a ge ’s me ged s a e and co a iance ma ix which is used by he
NMPFC’s op imize and p edic o sub-blocks o he cyclic minimiza ion p ocess. A e
p ocessing he con ol calcula ions, he NMPFC sends he desi ed con ol ou pu back o
he obo (con olle ’s e e ence eloci ies).
In he NMPFC con ol mode, he goal is o c ea e and main ain a o ma ion o mul iple
obo s in o de o imp o e he me ged pe cep ion o he a ge by he eam while a oiding
eamma es, obs acles and so on. The s a egy consis s o ha ing each obo awa e o he
cu en pose o e e y eamma e in he o ma ion, while con igu ed in a ully dis ibu ed
a chi ec u e. Simila ly o he s a egy used in ajec o y acking, he e:
4.3 Nonlinea Model P edic i e Fo ma ion Con ol 47
•Each obo p edic s he e olu ion o he en i e o ma ion, by using he simpli ied
nonlinea dynamic model o i sel and pu ely kinema ic models o he o he obo s
o mo ing a ge s in he o ma ion;
•The cos unc ion is minimized in o de o ind he con ol ou pu s ha keep he
obo in o ma ion;
•The i s con ol inpu is applied o he obo and he p ocess is epea ed when new
measu emen s a e a ailable.
Each o ma ion is de ined by a se o c oss-coupled cos unc ions which penalize
de ia ions om he desi ed o ma ion. One o hese cos unc ions is a ibu ed o each
obo in he o ma ion, which p opels he NMPFC con olle o gene a e i s own con ol
inpu s. The ollowing subsec ion desc ibes he model used in he p edic o o pe o m he
e olu ion o he o ma ion du ing each s ep (p edic ion ho izon).
Figu e 4.3: The 5dpo obo g aph ep esen a ion.
4.3.1 The Model
In a obo Rn(whe e 1 ≤n≤Nand Nis he o al numbe o obo s in he o ma-
ion) he wheel angula eloci ies, ω Rn(k) = hω1(k)ω2(k)ω3(k)iTis gi en by he
odome y and he wheel linea eloci y is gi en by V Rn(k) = h 1(k) 2(k) 3(k)iT=
.hω1(k)ω2(k)ω3(k)iT. Th ough a geome ic analysis o Fig 4.3 he obo eloci-
ies can be ound using he equa ion below.



Rn(k)
nRn(k)
wRn(k)


= (B)−1.


1(k)
2(k)
3(k)


(4.1)
54 Fo ma ion Con ol in Ac i e Ta ge T acking
Np
∑
i=N1
λ1×|δ(θRn(k),θRn
(k+i))|(4.22)
4.3.2.4 The Posi ion Te m
This nex e m will in luence he obo ’s posi ion wi h espec o he a ge ’s eloci y
ec o . I penalizes he obo in he w ong posi ion du ing he a ge ’s mo emen . He e,
he P al will change his posi ion and i mus ha e a alue be ween 1 and -1. No e ha
his ange o alues can allow he obo o be in on o he a ge , behind i , o a i s side.
He e, λ2is he unc ion’s penaliza ion weigh .
Np
∑
i=N1
λ2×|P al +( ˜
PRn
(k+i)·˜
V (k+i))|(4.23)
4.3.2.5 The Ma es A oidance Te m
The i h e m is a po en ial unc ion ha penalizes he p oximi y be ween he obo and i s
a ailable ma es (NM). This is a nega i e linea unc ion o dis ance. In he e m, he gi en
alue whe e small dis ances a e no penalized is DM=1.5m. The e o e, he obo s mus
keep a ela i e dis ance be ween hem g a e han DM. He e, λ3is he e m’s penaliza ion
weigh .
Np
∑
i=N1
NM
∑
j=1
λ3×max(1−||PRj
Rn(k+i)||
DM,0)(4.24)
4.3.2.6 The Obs acle A oidance Te m
The six h e m wo ks he same way as he p e ious unc ion. Al hough i is used he e o
a oid obs acles. While in he las unc ion he second sum adds o he maximum numbe
o a ailable ma es, his e m sums all obs acles in he obo ’s senso ange (NO). He e,
λ4is he e m’s penaliza ion weigh and DO=1.5mhas he same pu pose o DMbu
ega ding obs acles.
Np
∑
i=N1
NO
∑
l=1
λ4×max(1−||POl
Rn(k+i)||
DO,0)(4.25)
In he o ma ion, he obs acles a e de ec ed as black blobs (e e y hing inside he ield
which is no g een, whi e o he a ge ’s colo is conside ed black blob). Ne e heless, he

4.3 Nonlinea Model P edic i e Fo ma ion Con ol 55
eamma es a e no conside ed as obs acles once he eamma es’ pose a e sha ed and any
black blob ha comes om he ma es isual obse a ion is il e ed.
4.3.2.7 The Con ol E o Te m
Finally, he las e m penalizes he con ol e o . In his e m, he a ia ion in he ou pu
con ol signal is penalized ins ead o i s absolu e alue. Penalizing he ou pu con ol
signal would c ea e s eady-s a e e o in non-ze o eloci ies ( o example when pu suing
a mo ing a ge ).
Nc
∑
i=1
λ5×|∆U(k+i−1)|(4.26)
In his exp ession, ∆U(k+i−1)is he a ia ion o he ou pu con ol signals, whe e
∆U(k+i−1) = h∆ (k+i−1)∆ n(k+i−1)∆w(k+i−1)iT. I is impo an o no-
ice ha he NMPFC does no ac di ec ly in o he ac ua o s, bu in he eloci ies o he
obo . To pe o m his ask, a change should be applied o he model inside he NMPFC
used o p edic he obo ’s beha io . The changes in he model could be implemen ed
acco dingly o he o ma ion necessi y, conside ing as many models (di e en mobile
obo s) as he o ma ion would need.
The e o e, aking in o accoun all he e ms p e iously desc ibed, he weigh s gi en o
each one, and a penaliza ion e m o he a ia ion o con ol e o , he cos unc ion ha
has o be minimized o con ol each obo is:
56 Fo ma ion Con ol in Ac i e Ta ge T acking
J(N1,Np,Nc) =
Np
∑
i=N1
λa×|de (Σ⊥
Me ged(k+i))|+
Np
∑
i=N1
λ0×|(D al −||PRn
(k+i)||)|+
Np
∑
i=N1
λ1×|δ(θRn(k),θRn
(k+i))|+
Np
∑
i=N1
λ2×|P al +( ˜
PRn
(k+i)·˜
V (k+i))|+
Np
∑
i=N1
NM
∑
j=1
λ3×max(1−||PRj
Rn(k+i)||
DM,0)+
Np
∑
i=N1
NO
∑
l=1
λ4×max(1−||POl
Rn(k+i)||
DO,0)+
Nc
∑
i=1
λ5×|∆U(k+i−1)|
(4.27)
whe e N1,Npa e he p edic ed ho izon limi s in disc e e ime, and Ncis he con ol ho i-
zon.
In his cos unc ion, se e al impo an pa s mus be conside ed o e alua e he p o-
posed con ibu ions o his wo k:
•The e is a penaliza ion unc ion o he o al amoun o unce ain y o he a ge ’s
pe cep ion, which allows he obo s o be in he be e posi ion while con e ging o
ollowing a a ge ;
•In he con ol e o penaliza ion e m, he 1-no m is used, which gi es he con olle
mo e e iciency [88]. Howe e , he disad an age o 1-no m is i s high nonlinea i y.
Gi en ha i was adop ed RPROP [99] and ha i is a heu is ic op imize , i can
handle he nonlinea i ies in oduced by he use o he 1-no m;
•The e is an obs acle a oidance unc ion using a po en ial ield app oach and he A*
pa h planne . This unc ion will be be e explained in he nex chap e ;
•The con olle ’s ou pu is he obo ’s eloci ies and no he ac ua o ’s ol age o e-
loci y, making he con olle independen om he obo ’s dynamics. The e o e, his
is an app oach ha can be applied o di e en obo s’ con igu a ions by changing
hei kinema ic model, such as: holonomic, nonholonomic, and so o h.
4.3 Nonlinea Model P edic i e Fo ma ion Con ol 57
4.3.3 The Cos Func ion Du ing Ta ge Absence
The case o he a ge ’s absence is an impo an special case o be conside ed. To add ess
his issue, ano he cos unc ion simila o he p e ious one was c ea ed. This cos unc ion
is used only in case o he absence o a a ge in which case each obo has o ollow a
obo leade ha pe o ms a sea ch in a p e-se a ea using a obus eac i e con olle [113]
wi h he pa h planne u he explained in chap e 5. No e ha his cos unc ion is used
only by he ollowe obo s. The case o absence/p esence o a a ge c ea es some issues
ha will be discussed in chap e 6, whe e a solu ion will be p oposed.
The o ma ion geome y, in his case, selec s a ixed obo as he leade , while he o he
wo obo s become he ollowe s. All he assump ions made o he las cos unc ions
wi h espec o he a ge shall be made in his case wi h espec o he obo leade . An
excep ion is he i s e m due o he ac ha he leade ’s pose is passed o he o he obo s
(no es ima ed). The e o e, his cos unc ion is a composi ion o six e ms. The i s e m
penalizes he dis ance be ween a leade obo RLand he ollowe obo ||PRL
Rn(k)||. To
a oid collision, he penaliza ion also does no ake in o accoun a ixed dis ance ha he
obo mus main ain be ween i and he leade obo (D al).
Np
∑
i=N1
λ0×|(D al −||PRn
RL(k+i)||)|(4.28)
The second e m penalizes he di e ence be ween he angle o he obo (a ollowe )
in he wo ld ame and he angle be ween he ollowe obo and he leade obo . This
will allow he obo o ace he leade while i is in mo emen .
Np
∑
i=N1
λ1×|δ(θRn(k),θRn
RL(k+i))|(4.29)
This nex e m will in luence he obo ’s pose wi h espec o he leade ’s eloci y
ec o as can be seen in Fig. 4.5. I penalizes he obo in he w ong pose. He e, he P al
will change his pose and i mus ha e a alue be ween 1 and -1.
Np
∑
i=N1
λ2×|P al +( ˜
PRn
RL(k+i)·˜
VRL(k+i))|(4.30)
The ou h, i h and las e ms o his cos unc ion a e he same as he i h, six h, and
se en h e ms o he cos unc ion (4.27). The e o e, aking in o accoun all he elemen s
p e iously desc ibed, he weigh s gi en o each one o hem, and a penaliza ion e m o
58 Fo ma ion Con ol in Ac i e Ta ge T acking
Figu e 4.5: Fo ma ion Following a Leade
he a ia ion o con ol e o , he cos unc ion ha ep esen s all his, embedded in all
obo s is as ollows:
J(N1,Np,Nc) =
Np
∑
i=N1
λ0×|(D al −||PRn
RL(k+i)||)|+
Np
∑
i=N1
λ1×|δ(θRn(k),θRn
RL(k+i))|+
Np
∑
i=N1
λ2×|P al +( ˜
PRn
RL(k+i)·˜
VRL(k+i))|+
Np
∑
i=N1
NM
∑
j=1
λ3×max(1−||PRj
Rn(k+i)||
DM,0)+
Np
∑
i=N1
NO
∑
l=1
λ4×max(1−||POl
Rn(k+i)||
DO,0)+
Nc
∑
i=1
λ5×|∆U(k+i−1)|
(4.31)
4.4 The Op imiza ion Algo i hm
RPROP, sho o esilien p opaga ion, is a lea ning scheme, ha pe o ms a di ec adap-
a ion o he weigh s ep based on local g adien in o ma ion. A e he cascade co ela ion
algo i hm and he Le enbe g−Ma qua d algo i hm, RPROP is one o he as es weigh
upda e mechanisms. The main eason o he success o his algo i hm oo s in he concep
4.4 The Op imiza ion Algo i hm 59
o di ec adap a ion o he size o he weigh -upda e. In con as o all o he algo i hms,
only he sign o he pa ial de i a i e is used o pe o m bo h lea ning and adap a ion. This
leads o a anspa en and ye powe ul adap a ion p ocess, ha can be s aigh o wa d
and e y e icien ly compu ed wi h espec o bo h ime and s o age consump ion [99].
To o e come he inhe en disad an ages o pu e g adien -descen , RPROP pe o ms
a local adap a ion o he weigh upda es acco ding o he beha io o he e o unc ion.
In subs an ial di e ence o o he adap i e echniques, he e o o he RPROP adap a ion
p ocess is no blu ed by he un o eseeable in luence o he size o he de i a i e bu
only dependen on he empo al beha io o i s sign. These cha ac e is ics mo i a ed he
selec ion o RPROP o deal wi h he NMPFC op imiza ion p oblems [20].
Whe eas s anda d backp opaga ion (BP) uses ixed p opo ions ( he lea ning a e) o
he e o g adien o adjus weigh s, RPROP in oduces a ime a ying weigh s ep ∆i ha
de e mines he size o he weigh -upda e. This adap i e upda e alue e ol es du ing he
lea ning p ocess based on i s local sigh on he e o unc ion (·).
E e y ime he pa ial de i a i e o he co esponding weigh wi( )changes i s sign,
which indica es ha he las upda e was oo big and he algo i hm jumped o e a local
minimum, he upda e alue ∆iis dec eased by he ac o η−. I he de i a i e e ains
i s sign, he upda e alue inc eases sligh ly in o de o accele a e con e gence in shallow
egions.
Once he upda e alue o each weigh is adap ed, he weigh upda e i sel ∆wi ollows
a e y simple ule: i he de i a i e is posi i e (inc easing e o ), he weigh is dec eased
by i s upda e alue, i he de i a i e is nega i e, he upda e alue is inc eased. Howe e , i
he pa ial de i a i e changes sign (i.e. he p e ious s ep was oo la ge) and he minimum
was missed, he p e ious weigh upda e ∆wiis e e ed.
Due o ha back acking weigh s ep, he de i a i e is supposed o change i s sign
once again in he ollowing s ep. In o de o a oid a double punishmen o he upda e
alue, he e should be no adap a ion o i in he ollowing s ep. The e o e, he alue o
∂
∂wi( −1)is se o ze o in he ∆iadap a ion- ule.
Mo eo e , wi h η+and η−as he op imize pa ame e s, he RPROP algo i hm as
implemen ed in his hesis is de ined as ollows:

60 Fo ma ion Con ol in Ac i e Ta ge T acking
Resilien P opaga ion Algo i hm
o i:=0 o Nido
i ∂
∂wi( −1).∂
∂wi( )>0 hen
∆i( ) = min(∆i( −1)∗η+,∆max)
∆wi( ) = −sign(∂
∂wi( ))∗∆i( )
wi( +1) = wi( )+∆wi( )
else i ∂
∂wi( −1).∂
∂wi( )<0 hen
∆i( ) = max(∆i( −1)∗η−,∆min)
wi( +1) = wi( )−∆wi( −1)
∂
∂wi( ) = 0
else i ∂
∂wi( −1).∂
∂wi( ) = 0 hen
∆wi( ) = −sign(∂
∂wi( ))∗∆i( )
wi( +1) = wi( )+∆wi( )
In his hesis, he same alues as [20] we e used (η+=1.2, η−=0.8 and Ni=20,
whe e Niis he maximum numbe o i e a ions), excep o ∆0and S p, whe e S pis he
s op c i e ia o he op imize . He e, ∆0=0.15 and S p=0.1 a e used. The s op c i e ia
is he maximum accep able alue o he cos unc ion minimiza ion du ing he op imiza-
ion p ocess. Ne e heless, i he op imiza ion p ocess does no con e ge in less hen 20
i e a ions, he algo i hm assumes a sub-op imal solu ion and also s ops he op imiza ion
p ocess. In almos all si ua ions om he es s made by [20], he RPROP algo i hm in
he obo s con e ges in less han 10 i e a ions and he e iciency o he RPROP algo i hm
could be p o en in his wo k. Mo eo e , his algo i hm is much mo e powe ul han p e-
iously hough , handling e en nonlinea i ies such as he use o he 1-no m in he cos
unc ion o he NMPFC con olle .
4.5 Weigh Tuning Me hodology
This sec ion in oduces a me ic o e i y he op imal alues o he cos unc ion’s weigh s.
The op imal alues a e he ones ha minimize an objec unc ion ha e lec s he con-
olle ’s e iciency wi h espec o a gi en c i e ia. I is possible o empi ically ind he
ini ial alues o hese weigh s by analyzing he impo ance and penaliza ion p io i y o
each weigh . Ne e heless, o be co ec and accu a e, i is necessa y o p o ide a c i e-
ion o an objec i e unc ion and use, o example, a nume ic g adien descen me hod in
o de o sea ch o he op imal alue wi h espec o hese weigh s (lambdas). The e o e,
o achie e a inal weigh uning his sec ion p esen s:
1. An empi ical me hodology which allows he se ing o he ini ial cos unc ion’s
weigh s;
4.5 Weigh Tuning Me hodology 61
2. A g adien sea ch o he alues o λi, whe e λi=hλa,λ1,...,λ5iT, which e al-
ua es he con olle by minimizing an objec i e unc ion Vop wi h espec o he
adop ed c i e ia u he explained.
4.5.1 Ini ial Analysis o he Te ms and Weigh s o he Cos Func ion
The main objec i e o his chap e is o concei e a NMPFC ha con e ges a o ma ion
owa ds a a ge , minimizing he o al amoun o unce ain y o he eam o obo s’ pe -
cep ion o he a ge , while a oiding collision wi h eamma es and obs acles, o ien ing he
obo s owa ds he a ge , and penalizing oscilla ions in he con ol e o . While analyz-
ing his main objec i e, i is necessa y o s udy he cos unc ion be o ehand (4.27), as well
as he mul i- obo sys em and he desi ed o ma ion pe o med by he eam o obo s. In
his cos unc ion, in o de o minimize he o al amoun o unce ain y in he a ge pe -
cep ion, he obo s in o ma ion i s ha e o app oach he a ge . This app oach has o
be execu ed by conside ing bo h he bes posi ion a ound he a ge and he a oidance o
eamma es o obs acles du ing he obo s’ mo emen s. The e o e, i is clea ha in a scale
o minimiza ion p io i y, he dis ance e m o he cos unc ion should possess he highes
weigh ollowed by he co a iance e m, he posi ion e m, he obs acle a oidance e m
and he eamma e’s a oidance e m. These las ou e ms a e equally impo an o he
pe o mance o he o ma ion, implying an equal weigh o all ou e ms. The o ien a ion
o he obo owa ds he a ge comes a e he p e iously ci ed i ems in he scale o p io -
i y because i does no in luence he obo s’ mo emen while he o ma ion is con e ging,
bu only a e he posi ion has been achie ed he beha io is a ec ed. Finally, he e mus
be a small weigh in he con ol e o e m in o de o p e en he obo s om shaking in
he con e gence a ea, which would esul in an uns able o ma ion.
When analyzing he cos unc ion (4.27), i is necessa y o decide whe he he e ms
in he cos unc ion should also be ac i e du ing he en i e ime. By analyzing he cos
unc ion (4.27), i is clea ha he posi ion (λ1) and he o ien a ion (λ2) a e he only e ms
ha do no need o be ac i e a all imes du ing he obo s’ mo emen s, due o he ac ha
hey only in luence he obo ’s mo emen when i is nea he a ge (less hen 4min he
obo s’ se ing).
In o de o ac i a e hese e ms, which penalize he e o in posi ion and o ien a ion o
each obo , an ac i a ion unc ion based on hei ini ial gi en alues was c ea ed.
(||PRn
(k)||) = −(λimax
(Dis max −Dis min))×||PRn
(k)||+( Dis maxλimax
(Dis max −Dis min))(4.32)
whe e λimax is he ini ial signed alue o λ1o λ2(depending on he applied e m),
Dis max =4mis he maximum dis ance in which a obo can see a a ge and Dis min =
62 Fo ma ion Con ol in Ac i e Ta ge T acking
1.5mis he dis ance h eshold be ween he obo and he a ge ha ac i a es he weigh .
This unc ion is ac i a ed e e y ime he obo places i sel less han 4maway om he
a ge . I i s dis ance is g ea e han 4m he weigh is au oma ically ze o. I i is less han
1.5m, he alue is equal o λimax. The e o e, he alue o he weigh λiwill be as ollows:
λi=




(||PRn
(k)||)i 1.5<||PRn
(k)||≤4
λimax i ||PRn
(k)||≤1.5
0 i ||PRn
(k)||>4
(4.33)
4.5.2 The Tuning
Finally, an objec i e unc ion was c ea ed o measu e he NMPFC’s e iciency. This unc-
ion is gi en by he equa ion (4.34) and i se es as a me ic o e alua e he pe o mance
o he obo ’s con olle in o ma ion wi h he gi en cos unc ion’s weigh s. The e o e,
he objec i e unc ion is as ollows:
Vop =βT a j ×εT aj +βTime ×εTime +βOR ×εOR +βJ×J(4.34)
wi h
εT a j =
Na
∑
i=1||∆s||,εTime =τ2%
εOR =∑Na
i=1|(||PRn
(i)||−DOR
al )|
Na
(4.35)
and
βT a j =0.1βTime =0.01 βJ=0.00001 DOR
al =1.2
βOR =(0 i ||PRn
(i)||>1.5
0.7 i ||PRn
(i)||≤1.5
whe e Jis he inal alue o he cos unc ion in equa ion (4.27), DOR
al is he dis ance
desi ed alue and τ2% is de e mined based on he g aph o dis ance be ween he obo
and he a ge . The e o e, τ2% is he ime o con e gence when he alue o dis ance is in
+/−2% o i s inal alue. εTime is he unc ion ha penalizes he ime o con e ge, εT a j
is he unc ion ha penalizes a bigge ajec o y and εOR is he unc ion ha penalizes
he o e shoo egion made by he obo s whene e hey pass o e he h eshold dis ances,
4.5 Weigh Tuning Me hodology 63
whe e Na is he o al numbe o s eps. Finally, β ep esen s he weigh s o each e m o
he objec i e unc ion (4.34).
Figu e 4.6: Tuning Tes Se up
Fu he mo e, a g adien sea ch wi h a s ep o +/−2% o he ini ial alue was pe -
o med wi h espec o he me ic desc ibed in equa ion (4.34) in o de o sea ch o a se
o weigh s ha could gi e he minimum alue o his objec i e unc ion. This g adien
sea ch uses he alues o a simula ion en i onmen used as es bed whe e i s con igu a ion
is se wi h obo s R1and R2depa ing om coo dina es (0.3,3.9) and (-0.3,3.9), espec-
i ely, as illus a ed in Fig. 4.6. Bo h obo s we e 90◦(deg ees) o ien ed in he wo ld
ame. In he pa h, he e we e also wo ixed obs acles wi h coo dina es (-0.4,2.2) and
(0.4,2.2). The a ge (ball) was placed in he coo dina es (0,0) o he ield. This es bed
pushes he o ma ion o i s limi s, o cing all e ms o be ac i a ed a some ins an in ime
du ing he pe o med ajec o y.
4.5.3 The In luence o he Numbe o Te ms
The p ocess o uning he pa ame e s o he NMPFC cos unc ion is highly dependen
on he c i e ia adop ed and he numbe o e ms in he cos unc ion. This subsec ion
exempli ies his dependency h oughou he uning o he weigh s. I also demons a es
he change in he inal weigh s alues a each ime a e m is added in he cos unc ion.
Fu he mo e, le ’s begin assuming ha he cos unc ion in he equa ion 4.27 possesses
ini ially only he e ms ela ed o dis ance, eamma es a oidance, and obs acle a oidance.
Le ’s emembe ha he weigh o he e m ha penalizes he oscilla ions in he con ol
e o (λ5) is always s a ic and o he uning p ocess, due o he ac ha a leas one e m
mus be always ac i e (di e en om ze o).
The e o e, he NMPFC cos unc ion would be such as:
70 Fo ma ion Con ol in Ac i e Ta ge T acking
−5 −4 −3 −2 −1 0 1
−1.5
−1
−0.5
0
0.5
1
1.5
2
2.5
3
3.5
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 4.13: Simula ion 2: Homogeneous Fo ma ion Con e gence - Plo XY
0 1 2 3 4 5 6 7
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
5.5
6
Time [s]
Dis ance o he Ball [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
Robo 3
0 1 2 3 4 5 6 7
0
1
2
3
4
5
6
Time [s]
de (Co ) [m2]
To al Amoun o Unce ain y
Figu e 4.14: Simula ion 2: Homogeneous Fo ma ion Con e gence - Dis ance be ween Robo and
De e minan o Σ⊥
Me ged(k)
g aph wi h he dis ance be ween he obo as well as he minimiza ion o he me ged
co a iance’s de e minan can be seen in Fig. 4.14.
4.6.1.4 Simula ion 2: He e ogeneous Fo ma ion Con e gence
Like in he p e ious simula ion, he obo s he e a e placed like demons a ed in Fig. 4.15.
Once mo e he objec i e he e is o do a o ma ion con e gence wi hou he collision be-
ween he obo s wi h an ini ial con igu a ion o ming an ho izon al line. The di e ence
he e is ha obo 3 is now a nonholonomic obo and i was se in he closes posi ion o
he a ge o inc ease he di icul y in his simula ion.
In Fig. 4.15 i can be seen he plo XY o he obo ’s mo emen . No e in he ideo
C3T2Con ha despi e he cons ain s in he mo emen o he nonholonomic obo , no
collisions happened and he o ma ion con e ged. In his simula ion, he mo emen o
obo 3 is mo e smoo h while he mo emen s o he holonomic obo a e mo e o uous
due o he necessi y o a oid collision wi h he nonholonomic obo . A g aph wi h he
dis ance be ween he obo and he ball as well as he minimiza ion o he me ged co-
a iance’s de e minan can be seen in Fig. 4.16 demons a ing he o ma ion con e gence
owa ds he a ge .

4.6 Resul s 71
−6 −5 −4 −3 −2 −1 0 1
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
2.5
3
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 4.15: Simula ion 2: He e ogeneous Fo ma ion Con e gence - Plo XY
0 1 2 3 4 5 6 7
1
2
3
4
5
6
Time [s]
Dis ance o he Ball [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
Robo 3
0 1 2 3 4 5 6 7
0
1
2
3
4
5
Time [s]
de (Co ) [m2]
To al Amoun o Unce ain y
Figu e 4.16: Simula ion 2: He e ogeneous Fo ma ion Con e gence - Dis ance be ween Robo and
Ball and he De e minan o Σ⊥
Me ged
4.6.1.5 Simula ion 3: Homogeneous Leade Following
This simula ion places he obo s ini ially as demons a ed in Fig. 4.17 and has a main
objec i e o analyzing he leade - ollowing en i onmen . The e o e, wo obo s will ol-
low a obo leade depa ing om he coo dina es (6.3,-3.1), (6.3,0), (6.3,3.1) o obo s
1, 2 and 3, espec i ely. The ollowe obo s shall con e ge o he leade obo and keep
a o ma ion while ollowing i . In his case, he a ge is no seen in he ield, so obo 1
(as he leade obo ) has an A* pa h planne seen in [134] wi h a eac i e con olle [113]
while he ollowe obo s ( obo s 2 and 3) possess he second cos unc ion p esen ed in
his s udy and he posi ion e m is now aken in o accoun .
In Fig. 4.17, he plo XY o he obo ’s mo emen and a g aph wi h he dis ance
be ween he ollowe s and he obo leade is p esen ed. The ideo C1T3Con also shows
he obo s’ mo emen as p esen ed in he plo XY. No collisions be ween obo s occu s in
his simula ion.
This case used he same alues ob ained du ing he weigh uning me hodology demon-
s a ing he obus ness o he con olle . Howe e , i analyzed ca e ully, he objec i e
unc ion (4.34) used o pe o m he g adien sea ch did no con empla e his case. The
72 Fo ma ion Con ol in Ac i e Ta ge T acking
−4 −3 −2 −1 0 1 2 3 4 5 6
−4
−3
−2
−1
0
1
2
3
4
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
0 2 4 6 8 10 12 14 16 18
1
2
3
4
5
6
Time [s]
Dis ance o he Leade [m]
Dis ance Be ween Robo and Leade
Robo 2
Robo 3
Figu e 4.17: Simula ion 3: Homogeneous Leade Following - Plo XY and Dis ance be ween he
Leade Robo and he Followe s
0 2 4 6 8 10 12 14 16 18
−1
−0.5
0
0.5
1
1.5
2
Time [s]
In e nal P oduc [m]
In e nal P oduc : Leade Vel. and Robo Pos. w. . he Leade
Robo 2
Robo 3
0 2 4 6 8 10 12 14 16 18
−pi
−pi/2
0
pi/2
pi
Time [s]
Angle [ ad]
E o Angle o he Followe s O ien a ion Facing he Leade
Robo 2
Robo 3
Figu e 4.18: Simula ion 3: Homogeneous Leade Following - In e nal P oduc Be ween Leade
and Followe s and E o Angle o he Followe s O ien a ion Facing he Leade
c i e ia was composed by he ajec o y made, he o e shoo in he con e gence, he ime
o con e gence and he cos unc ion i sel . The in e nal p oduc o example was no
con empla ed and ha can explain he beha io o obo 2 in Fig. 4.18 whe e i shows
he g aphs o he in e nal p oduc be ween he leade and he o he obo s. The gi en
P al o obo 2 was P al =0 and o obo 3 was P al =−1. This g aph shows wo alls
in he in e nal p oduc con e gence due o he leade ’s change o o ien a ion. When he
leade obo u ns, he ollowe s became o ien ed in 90◦wi h espec o he leade ’s o ien-
a ion un il hey con e ge again, also explaining he "jumps" o o ien a ion in Fig. 4.18.
Fu he mo e, ega ding he in e nal p oduc , obo 3 con e ged success ully while obo
2 did no . Ne e heless, he e o angle be ween he obo leade and he ollowe s as
success ully minimized as demons a ed in he g aph o Fig. 4.18.
In his wo k, he choice o c ea e ano he c i e ia jus o his si ua ion o e- une he
pa ame e s was no aken. Tha could cause a dis u bance in he uning o o he pa ame e s
due o he ac ha he e ms o he cos unc ion a e coupled (one penaliza ion e m
in luences he o he indi ec ly). Ne e heless, he uning could be done wi h ano he
c i e ia ha could con empla e a be e o ma ion beha io ega ding he in e nal p oduc
con e gence.
4.6 Resul s 73
−4 −3 −2 −1 0 1 2 3 4 5 6
−4
−3
−2
−1
0
1
2
3
4
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
0 2 4 6 8 10 12 14 16 18 20
0
1
2
3
4
5
6
7
Time [s]
Dis ance o he Leade [m]
Dis ance Be ween Robo and Leade
Robo 2
Robo 3
Figu e 4.19: Simula ion 3: He e ogeneous Leade Following - Plo XY and he Dis ance be ween
he Leade Robo
4.6.1.6 Simula ion 3: He e ogeneous Leade Following
This simula ion has he same objec i e as he p e ious one whe e he con e gence owa ds
a leade obo and he o ma ion keeping by wo ollowe obo s du ing i s mo emen is
pe o med. The di e ence he e is once mo e he subs i u ion o obo 3 by a nonholo-
nomic obo .
0 2 4 6 8 10 12 14 16 18 20
−1
−0.5
0
0.5
1
1.5
Time [s]
In e nal P oduc [m]
In e nal P oduc : Leade Vel. and Robo Pos. w. . he Leade
Robo 2
Robo 3
0 2 4 6 8 10 12 14 16 18 20
−pi
−pi/2
0
pi/2
pi
Time [s]
Angle [ ad]
E o Angle o he Followe s O ien a ion Facing he Leade
Robo 2
Robo 3
Figu e 4.20: Simula ion 3: He e ogeneous Leade Following - In e nal P oduc Be ween Leade
and Followe s and Angle Be ween he Leade and he Followe s
The XY plo o he obo ’s mo emen , as well as a g aph wi h he dis ance be ween
obo and i s leade , can be seen in Fig 4.19. No e ha due o he cons ain s in he mobili y
o obo 3, i possesses a smoo he ajec o y wi h a be e beha io o obo 3 han in he
p e ious simula ion. The con e gence o obo 3 and he beha io o obo 2 ega ding
he in e nal p oduc wi h he leade obo can be seen in Fig. 4.20. The angle be ween he
ollowe s and he leade can also be seen in his igu e, whe e he con e gence o his e m
is also shown. Despi e he cons ain s in he nonholonomic obo ’s mo emen , i can be
no ed in he ideo C3T3Con ha he simula ion p o es he success o he o ma ion in
a leade ollowing app oach and he con olle ’s obus ness. Howe e , like he simula ion
74 Fo ma ion Con ol in Ac i e Ta ge T acking
be o e, a uning could be also applied wi h a di e en c i e ion in o de o imp o e, o
example, he in e nal p oduc .
4.6.1.7 Simula ion 4: Homogeneous Keeping Fo ma ion
The main objec i e in his simula ion is o obse e he conse a ion o he o ma ion
du ing he a ge ’s mo emen in a s aigh ajec o y. Robo s 1, 2, 3 and he a ge ’s
ini ial coo dina es a e (0,-1.5), (0,1.5), (1.5,0) and (0,0), espec i ely, as demons a ed
in Fig. 4.21. Once mo e, he posi ion e m is aken in o accoun , bu his ime in he
i s cos unc ion. The only e m ha is no ac i e is he obs acle a oidance e m. The
ball was guided in a s aigh line o cing i o possess a cons an non ze o eloci y. In
Fig. 4.21 he plo XY o he obo ’s mo emen can be seen. The ideo C1T1Man also
shows he obo s’ mo emen as p esen ed in he plo XY. No collisions be ween he obo s
occu s and he o ma ion is kep success ully du ing he simula ion. A g aph wi h he
dis ance be ween he obo and he ball and he minimiza ion o he me ged co a iance’s
de e minan can be seen in Fig. 4.22.
−3 −2.5 −2 −1.5 −1 −0.5 0 0.5 1
−1.5
−1
−0.5
0
0.5
1
1.5
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 4.21: Simula ion 4: Homogeneous Keeping Fo ma ion - Plo XY
0 2 4 6 8 10 12 14 16
1
1.1
1.2
1.3
1.4
1.5
1.6
1.7
1.8
1.9
2
Time [s]
Dis ance o he Ball [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
Robo 3
0 2 4 6 8 10 12 14 16
0.06
0.08
0.1
0.12
0.14
0.16
0.18
Time [s]
de (Co ) [m2]
To al Amoun o Unce ain y
Figu e 4.22: Simula ion 4: Homogeneous Keeping Fo ma ion - Dis ance be ween Robo and Ball
and De e minan o Σ⊥
Me ged(k)
By analyzing he g aph dis ances, i can be wi nessed ha obo 3 ( he one behind he
ball) has mo e di icul y ollowing he a ge . I s lag in ela ion o he balls mo emen is
4.6 Resul s 75
also summed o i s accele a ion. The e o e, he ini ial dis ance be ween obo 3 and he
ball is g a e hen he o he obo s in o ma ion.
0 2 4 6 8 10 12 14 16
−0.5
0
0.5
1
1.5
Time [s]
In e nal P oduc [m]
In e nal P oduc : Ball Vel. and Robo Pos. w. . . he Ball
Robo 1
Robo 2
Robo 3
0 2 4 6 8 10 12 14 16
−pi
−pi/2
0
pi/2
pi
Time [s]
Angle [ ad]
E o Angle o he Robo s O ien a ion Facing he Ta ge
Robo 1
Robo 2
Robo 3
Figu e 4.23: Simula ion 4: Homogeneous Keeping Fo ma ion - In e nal P oduc Be ween he
Robo s and he Ball and E o Angle o he Robo s O ien a ion Facing he Ta ge
Fig. 4.23 shows he g aphs o he in e nal p oduc be ween he obo s and he ball as
well as he angle be ween he obo s’ and he ball’s eloci y ec o . The gi en P al o
obo s 1, 2 and 3 we e P al =0, P al =0 and P al =−1, espec i ely. No e ha du ing
he ball’s mo emen (V (k)6=0) he in e nal p oduc is di e en om ze o in obo 3. As
soon as he eloci y ends o ze o, he in e nal p oduc also d ops due o he absence o
he ball’s eloci y ec o .
4.6.1.8 Simula ion 4: He e ogeneous Keeping Fo ma ion
This simula ion aims o obse e he he e ogeneous beha io o he o ma ion du ing he
ac i e a ge acking p oblem. As occu ed in he p e ious simula ion, he obo s a e
placed ini ially like demons a ed in Fig. 4.24. The ideo C3T1Man also shows he
obo s’ mo emen as p esen ed in he plo XY.
−4 −3 −2 −1 0 1
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 4.24: Simula ion 4: He e ogeneous Keeping Fo ma ion - Plo XY
A g aph o he dis ance be ween he obo and he ball as well as he minimiza ion
o he me ged co a iance’s de e minan can be seen in Fig. 4.25. No e he e ha he e is

76 Fo ma ion Con ol in Ac i e Ta ge T acking
0 2 4 6 8 10
1
1.2
1.4
1.6
1.8
2
2.2
Time [s]
Dis ance o he Ball [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
Robo 3
0 2 4 6 8 10
0.08
0.1
0.12
0.14
0.16
0.18
0.2
Time [s]
de (Co ) [m2]
To al Amoun o Unce ain y
Figu e 4.25: Simula ion 4: He e ogeneous Keeping Fo ma ion - Dis ance be ween Robo and
De e minan o Σ⊥
Me ged
0 2 4 6 8 10
−1
−0.5
0
0.5
1
1.5
2
Time [s]
In e nal P oduc [m]
In e nal P oduc : Ball Vel. and Robo Pos. w. . . he Ball
Robo 1
Robo 2
Robo 3
0 2 4 6 8 10
−pi
−pi/2
0
pi/2
pi
Time [s]
Angle [ ad]
E o Angle o he Robo s O ien a ion Facing he Ta ge
Robo 1
Robo 2
Robo 3
Figu e 4.26: Simula ion 4: He e ogeneous Keeping Fo ma ion - In e nal P oduc Be ween he
Robo s and he Ball and Angle Be ween he Robo s and he Ball’s Veloci y Vec o
ini ially a g ea e di e ence in he dis ance be ween he a ge and obo 3 ( he ollowe )
as soon as he a ge s a s mo ing. This dis ance is g ea e he e han in he simula ion
o he homogeneous g oup due o he nonholonomic obo ’s cons ain s and he lowe
maximum eloci y his obo has. Ne e heless, i eaches he a ge and hen i eadjus s
i s pose ( obo 3) pu ing i sel a he desi ed dis ance and o ien a ion wi h espec o he
a ge . Fig. 4.26 shows he g aphs o he in e nal p oduc be ween he obo s and he ball
as well as he angle be ween he obo s and he ball’s eloci y ec o e lec ing he same
beha io s as he p e ious simula ion. Finally, i can be seen ha he o ma ion is kep
success ully while he a ge mo es.
4.6.2 Resul s o he Expe imen s wi h Real Robo s
A se up was c ea ed o pe o m expe imen s wi h eal obo s in o de o analyze he be-
ha io o h ee omnidi ec ional mobile obo s wi h he NMPFC which is p esen ed in Fig.
3.2. An ex e nal compu e unning he Coach applica ion and connec ed o a ou e by
cable was needed in o de o se e as a b idge o sha ed in o ma ion be ween he obo s,
4.6 Resul s 77
whe e he log o he o ma ion could be collec ed. The e o e, o un he expe imen s a
wo ks a ion (In el Co e i7 3Ghz/Co e wi h 8Gb RAM) wi h Ubun u 9.04 ha uns said
b idge was used. Each obo had a compu e , a No ebook (In el Dual Co e 2Ghz/Co e
wi h 2Gb RAM) wi h Ubun u 9.04, unning i s own NMPFC and applica ions p e iously
seen in Fig. 4.1. Finally, he expe imen s wi h eal obo s we e execu ed in o de o
epea he en i onmen c ea ed in he simula ions wi h wo expe imen s in o ma ion con-
e gence, one expe imen in leade ollowing and one expe imen in keeping o ma ion
case. This con igu a ion can be seen in Fig. 4.27.
Figu e 4.27: Expe imen Se up
In he eal expe imen , he SimTwo is subs i u ed by he ision so wa e (HAL) send-
ing locally, o Dec, in o ma ion o calcula e he pose o i s obo and he posi ion o he
a ge . Then, each Dec sends he con ol inpu s o he obo . This main p ocessing uni
(Dec) handles ex e nal communica ion wi h o he obo s h ough wi eless communica-
ion. Finally he Coach collec s he in o ma ion om he obo s wi h hei pose and hei
obse ed ball s a e (posi ion and speed o he ball). The Coach hen uses i s s a e ma-
chine o ac i a e he oles ha each obo mus execu e, sending back also he in o ma ion
abou which ball om he obse ed ball is he "bes " ball, o he ue ball, ha will be
used by he o ma ion. In hese esul s he CTE was no implemen ed and was subs i-
u ed by he Coach so wa e. The main eason is ha he CTE is ano he hesis wo k
om he FCT p ojec (PTDC/EEA-CRO/100692/2008 - Pe cep ion-D i en Coo dina ed
Mul i-Robo Mo ion Con ol), which in u n is no he pu pose o his hesis o e alua e.
The e o e, as he Coach chooses he "bes " measu emen be ween he obo s measu e-
men , and he e o e his becomes he "bes ball", i is possible o see a "s ange" beha io
o he ball in he expe imen s when i "jumps" om a posi ion o ano he in an ins an o
ime. This is due o he change he coach pe o ms when choosing he "bes " ball be ween
he obo s measu emen .
78 Fo ma ion Con ol in Ac i e Ta ge T acking
Finally, i is impo an o no ice ha in all expe imen s wi h eal obo s he e is noise in
he localiza ion and pe cep ion o he ball. The localiza ion noise has a s anda d de ia ion
in X and Y o 0.05m, and in θo 0.045mand he noise in pe cep ion o he ball has a s an-
da d de ia ion in X and Y o 0.316m. When compa ing he esul s om he simula ions
wi h hose wi h he eal obo s h ough he analysis o he ideos o bo h cases i can be
seen ha his noise in luences he obo s mainly in he inal con e gence. This in luence
pu s he obo s o shake a li le when con e ging, ne e comple ely s opping ( hey s op
because he expe imen was o e ). This shaking can be no iced in ha li le oscilla ion
a ound he con e ging inal alues p esen ed in he g aphics o o ien a ion, co a iance
penaliza ion and so on. Ne e heless, i also shows he con olle ’s obus ness due o he
ac ha despi e he noise, he o ma ion con e ged o he desi ed beha io .
4.6.2.1 Real Expe imen 1: Fo ma ion Con e gence
The eal expe imen s we e conduc ed wi h obo s numbe 1, 2 and 3. The i s expe imen
placed he obo s ini ially like demons a ed in Fig. 4.28. In his igu e, he XY plo 0
o he obo ’s mo emen can also be seen. As i was in he i s simula ion, he e a e no
obs acles in his expe imen and he objec i e is o see he con e gence owa ds a a ge
placed in he cen e o he ield. Robo s 1, 2 and 3 a e placed a om each o he and hei
posi ions a e (-3,1.7), (0,-2) and (3,1.7), espec i ely. All obo s we e 270◦o ien ed in
he wo ld ame. The a ge ’s coo dina es a e (0,0). In Fig. 4.29 he g aph o he dis ance
be ween he obo and he ball as well as he g aph o he o al amoun o unce ain y
(me ged co a iances) minimiza ion can be seen. The ideo C1T1Con Real shows he
obo s’ mo emen as p esen ed in he plo XY.
−2.5 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 2.5
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 4.28: Real Expe imen 1: Fo ma ion Con e gence
No e he e ha no collisions be ween he obo s occu and ha he obo always s ops
while acing he ball (wi h he ca ed pa owa ds he ball) as shown in he simula ions.
As i can be seen, he ball "jumps" om one coo dina e o ano he because he o ma ion
chooses he "bes " ball amongs he balls’ measu emen which come om all h ee obo s.
Ne e heless, he obo s mo e in a pe ec ci cle a ound he ball while ying o minimize
4.6 Resul s 79
0 2 4 6 8 10 12
0.5
1
1.5
2
2.5
3
3.5
4
Time [s]
Dis ance o he Ball [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
Robo 3
0 2 4 6 8 10 12
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Time [s]
de (Co ) [m2]
To al Amoun o Unce ain y
Figu e 4.29: Real Expe imen 1: Fo ma ion Con e gence - Dis ance be ween he obo and he
ball and De e minan o Σ⊥
Me ged(k)
he co a iance. Tha is explained by he ac ha e en i he bes ball changes, o "jumps"
om one coo dina e o ano he , he o al cos o hese e ms (co a iance and dis ance)
a e kep e y simila as i can be seen in Fig. 4.29. Howe e , he o ma ion con e ges
success ully.
4.6.2.2 Real Expe imen 2: Fo ma ion Con e gence
The second expe imen places he obo s ini ially a he coo dina es (2,0), (3,0), (4,0)
and (0,0) o obo s 1, 2, 3 and a ge espec i ely, like demons a ed in Fig. 4.30. The
objec i e he e is o obse e he obo s con e ging o he ball a oiding collisions be ween
hem. Simila ly o he second pai o simula ions, he obo s a e placed in a ho izon al
line o inc ease he di icul y in he con e gence o cing he obo s o a oid each o he .
The ideo C1T2Con Real shows he obo s success ully con e ging owa ds he a ge
while a oiding collision be ween hem.
−1 −0.5 0 0.5 1 1.5 2 2.5 3 3.5
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 4.30: Real Expe imen 2: Fo ma ion Con e gence
In Fig. 4.31, a g aph wi h he dis ance be ween he obo and he ball as well as he
g aph o he o al amoun o unce ain y (me ged co a iances) minimiza ion can also be
seen. No e ha his ime, he e is no "jump" on he "bes " ball.
86 Obs acle A oidance in Fo ma ion Con ol
de ined a ac i e and epulsi e po en ial ield unc ions. The subsequen s udies can be
ound in [138], [131], and [139]. None heless, he po en ial ield me hod is no s aigh -
o wa dly applicable o mobile ehicles wi h kinema ic cons ain s since, in he po en ial-
ield design, he obo is usually ea ed as a simple pa icle. Ano he majo p oblem is he
ac ha since he me hod is an essen ially as e descen op imiza ion me hod, he obo
can ge apped in o local minima o he po en ial unc ion o he han he goal con igu a-
ion [140].
Among he mos amous he e is also he Roadmap me hod. A compu a ional geome-
y da a s uc u e was p oposed in [141] o sol e he p oblem o an op imal pa h gene a ion
be ween a sou ce and a des ina ion in he p esence o simple disjoin polygonal obs acles.
In [142] a good applica ion o he Roadmap me hod is applied whe e he use o mul i-
ple mobile obo s in a common en i onmen , such as unde g ound mining and wa ehouse
managemen p oblems, a e conside ed despi e he ac ha no andomly mo ing obs acles
a e used. The Roadmap me hod is applied in low-dimension con igu a ion spaces and i
is no easy o be implemen ed, depending o he app oach [140].
Finally, he las me hod among he mos classic algo i hms o pa h planning is he
Cell Decomposi ion [140]. In his ca ego y a e amous and e icien algo i hms such as
A*, D*, ARA* and AD*. The A* algo i hm is he oldes . I is well applied wi h s a ic
[143] and dynamic obs acles [136]. I can be applied also o UAV obs acle a oidance
[135] in unknown en i onmen s. In [144] an app oxima e cell decomposi ion me hod
was de eloped in which obs acles, a ge s, senso ’s pla o m, and FOV (Field o View) a e
ep esen ed as closed and bounded subse s o an Euclidean wo kspace. A good o e iew
abou he ad an ages and disad an ages o hese algo i hms cam be seen in [145] and
[146].
One o he me hods ha has e ol ed in ecen yea s is he Veloci y Obs acles, i s
used in [147]. This me hod de ines he se o all he eloci ies o a obo ha will esul in
a collision a some poin in ime, assuming ha he obs acle main ains he cu en speed.
The e o e, i s mo emen planning aims a inding he speeds ha all ou side hese g oups
o ensu e ha he e will be no collisions. This me hod is widely used in simula ions o
c owds, howe e i has wo p oblems: i s , when dealing wi h s a ic obs acles, he obo
ci cum en s he edges o he obs acle making he obo slowe . The second p oblem is
he p ocessing ime, as no ed in [148].
In his chap e , a deepe explana ion is p esen ed abou he obs acle a oidance p oblem
conside ed by he nonlinea model p edic i e o ma ion con ol. In he nex sec ion he
p oblem is o mula ed desc ibing he issue s udied. The obs acle a oidance solu ion wi h
po en ial ield app oach and he NMPFC is p esen ed in he ollowing sec ion. Then, he
A* pa h planning algo i hm is explained in sec ion 5.4. The esul s o he expe imen s
wi h eal obo s and simula ions a e shown in sec ion 5.5 and inally he conclusion is
shown in sec ion 5.6.

5.2 P oblem Fo mula ion 87
5.2 P oblem Fo mula ion
Le Abe a single igid objec ( he obo ) mo ing in a euclidean space W, called wo kspace,
whe e W⊂ℜ2. The simula ion se s a mobile obo (5dpo middle size league socce obo
o he nonholonomic Mini-AGV) as he igid objec A. Le "q" be a posi ion o obo Ain
he wo kspace W. The e o e, in his wo kspace he obo is placed in an ini ial posi ion
qini and in he a side o he wo kspace W, he e is he a ge poin q a ge . In be ween
he ini ial and a ge poin s he e is a bigge ci cle cen e ed in he middle o he ield ha
symbolizes any c owded en i onmen such as a shopping mall’s hall o a ac o y co ido ,
whe e people ha e o pe o m andom ajec o ies.
Le B1,...,Bnbe also, in a i s case, ixed igid objec s dis ibu ed in W. Le he space
Cbe he con igu a ion space o obo A, o in o he wo ds, all he con igu a ion eachable
by Ain he wo kspace W. The e o e, all obs acles Biin he wo kspace Wa e mapped
in Cin o a egion CBi={q∈C/A(q)∩Bi6=0}which is called C-obs acle. The union
o all C-obs acles is called C-obs acle egion, o he egion occupied by he obs acles.
Mo eo e , he ee space in he wo kspace Wis gi en by:
C ee =C
n
[
i=1
CBi={q∈C/A(q)∩(
n
[
i=1
CBi) = 0}(5.1)
The e o e, he p oblem becomes: gi en an ini ial posi ion o Aand a a ge posi ion
in W, con e ge o he a ge poin q a ge a oiding con ac wi h he CBi’s du ing all he
ajec o y.
When he C-obs acles a e no longe s a ic, in ano he wo ds hey a y hei posi ion
wi h ime, i can no longe be ep esen ed as in he p oblem be o e. This issue is sol ed
by simply adding a dimension o C, ob aining he e o e, CT =Cx[0,+∞), which is called
con igu a ion space- ime o A. Thus, e e y obs acle Bimaps in CT o a s a iona y egion
CTBi, called a CT-Obs acle, de ined by:
CTBi={(q, )/A(q)∩Bi( )6=0}(5.2)
5.3 Po en ial Field App oach in NMPFC
The NMPFC is inse ed in he op imal con ol g oup. I means ha his kind o con olle
uses an op imize algo i hm o ind he bes solu ion, o op imal con ol ou pu . When
conside ing such con olle s in eal ime op imiza ion many app oaches a e a ailable.
Ne e heless, he obs acle a oidance app oach ha mos apidly and easily i s in o his
kind o con ol is he a i icial po en ial ields (APF) app oach. In [88], he au ho uses he
88 Obs acle A oidance in Fo ma ion Con ol
APF embedded in a nonlinea model p edic i e con olle as an example o a oid s a ic
obs acles.
The po en ial ield app oach uses a po en ial unc ion o na iga e he obo (a ac-
ion unc ion) ha d i es he obo owa ds he a ge , and an a oidance unc ion ( epulse
unc ion) ha epels he obo when i is nea an obs acle. I he NMPFC is conside ed,
hen he a ac ion unc ion could be seen as he i s and second e ms o he NMPFC
cos unc ion in equa ion 4.27. The e o e, a epulsi e unc ion had o be made in o de o
conside he obs acle a oidance p oblem.
The main idea unde lying he de ini ion o he epulsi e po en ial is o c ea e a po en-
ial ba ie a ound he C-obs acle egion ha canno be a e sed by he obo s’ con igu-
a ion [140]. In addi ion, i is usually desi able ha he epulsi e po en ial does no a ec
he mo ion o he obo s when i is su icien ly a away om he C-obs acles. One way
o achie e hese cons ain s is o de ine he epulsi e po en ial unc ion as ollows:
U ep(q) = (1
2η(1
ρ(q)−1
ρ0)2i ρ(q)≤ρ0
0 i ρ(q)>ρ0
(5.3)
whe e ηis a posi i e scaling ac o , ρ(q)deno es he dis ance om q o he C-obs acle
egion CB, i.e.:
ρ(q) = min
q0∈CB||q−q0|| (5.4)
and ρ0is a posi i e cons an called he dis ance o in luence o he C-obs acles. The
unc ion U ep is posi i e o null, i ends o in ini y as qge s close o he C-obs acle
egion, and is null when he dis ance o he obo s’ con igu a ion o he C-obs acle egion
is g ea e hen ρ0.
This hesis di ided he p oblem o obs acle a oidance in wo epulse unc ions. The
i s conside s he ma e a oidance, p e en ing he obo s om colliding wi h hemsel es.
The second unc ion conside s he obs acle a oidance, p e en ing he obo s om collid-
ing wi h s a ic o mo ing obs acles which may, o may no appea .
5.3.1 Ma e A oidance Func ion
The i s idea o a e m in a nonlinea model p edic i e con olle ha penalizes he ap-
p oxima ion be ween obo s in a o ma ion was p esen ed in [20]. In his wo k, he au ho
c ea ed a sub- unc ion in his nonlinea model p edic i e con olle such as in he equa ion
5.5.
5.3 Po en ial Field App oach in NMPFC 89
Np
∑
i=N1
λ3×(( 1
||PRm1
Rn(k+i)||−DM
)2+( 1
||PRm2
Rn(k+i)||−DM
)2)(5.5)
Whe e ||PRm1
Rn(k+i)|| is he dis ance be ween obo Rnand he ma e 1. This unc ion
has a nonlinea dec easing beha io as shown in Fig. 5.1
Figu e 5.1: Beha io o unc ion in 5.5
As i can be no iced, he i s p oblem is ha his app oach does no conside a gen-
e alized numbe o ma es, only wo. Howe e , a mo e impo an issue is add essed when
analyzing he beha io o his unc ion. The a oidance unc ion o i s nonlinea i y, akes
mo e ime o inc ease he penaliza ion by p oximi y, allowing he obo s o ge oo nea
each o he be o e penalizing i .
A simple solu ion is p oposed he e o a oid hese p oblems. The p oposed unc ion
o ma e a oidance can be seen in equa ion 5.6.
Np
∑
i=N1
NM
∑
j=1
λ3×max(1−||PRj
Rn(k+i)||
DM,0)(5.6)
Remembe ing ha NM is he maximum numbe o ma es, ||PRj
Rn(k+i)||is he dis ance
be ween obo Rnand he ma e Rjand DM is he gi en alue whe e small dis ances a e
no penalized. The p oposed unc ion was changed o a linea unc ion which inc eases
he penaliza ion wi h p oximi y much mo e apidly. The gene aliza ion o ma es was also
conside ed wi h a second sum ha gi es scalabili y o he NMPFC con olle in his hesis.
Finally, an ex eme case had o be conside ed when using po en ial unc ions. This
ex eme case, also s udied among he po en ial ield app oach, akes in o accoun he
possibili y o he obo s ge ing oo close o each o he much mo e apidly hen allowed.
This beha io can occu i he obo s a e mo ing in high eloci ies o ins ance. To a oid
collision in hese cases a p o ec ion zone is c ea ed a ound he obo s whe e he weigh
o he a ac ion and epulsion unc ions ( e ms o he NMPFC cos unc ion) a e apidly
swi ched so he obo gi es p io i y o penalize he ma e a oidance a he hen ge o he
90 Obs acle A oidance in Fo ma ion Con ol
a ge . When he obo s a e ou side his zone once again, he weigh s o he cos unc ion
a e se back o no mal.
5.3.2 Obs acle A oidance Func ion
An obs acle a oidance unc ion was c ea ed based on he idea o ma es a oidance unc ion
and he po en ial ield app oach. The epulsion unc ion p oposed in his hesis can be seen
in equa ion 5.7.
Np
∑
i=N1
NO
∑
l=1
λ4×max(1−||POl
Rn(k+i)||
DO,0)(5.7)
Remembe ing also ha NO is he maximum numbe o obs acles, ||POl
Rn(k+i)|| is he
dis ance be ween obo Rnand he obs acle Oland DO is simila o DM.
Figu e 5.2: Local Minima P oblem Case
This unc ion’s beha io is simila o he unc ion p oposed in he ma e a oidance
p oblem. In he obs acle a oidance p oposed unc ion, all obs acles (s a ic o mo ing)
a e conside ed o be s opped du ing he 40ms loop con ol. This assump ion speeds up
he calcula ions in he p edic ion o he NMPFC by calcula ing only he obo -obs acle
dis ance e olu ion in a simpli ied ashion. Ne e heless, he s a ic obs acles ha e a majo
issue in po en ial ields app oach: he local minima p oblem. The mos common p oblem
o APF lies in a obo being s uck in a local minima. I is he case ha occu s when he
5.3 Po en ial Field App oach in NMPFC 91
o al o ce ac ing on he agen is summed up o ze o e en hough he obo has no eached
i s goal posi ion ye . Fig. 5.2 shows he local minima p oblem case.
In o de o sol e his p oblem, a swi ching app oach was implemen ed inside he ask
Fo ma ion, which in u n is inside he ole Fo ma ion. In his app oach, he NMPFC
wi h he po en ial unc ion e m is subs i u ed by a con olle wi h a modi ied A* pa h
planning algo i hm i he obo s ge s apped in a local minima (i he pa h owa ds he
a ge is obs uc ed o i he e a e obs acles in one o he obo ’s side). In mos o his
local minima p oblems, he obo encoun e s obs acles a leas in wo sides ( o example
in on o i and by one side making an L shape). In his case, i u ns o he po en ial
ield e m and u ns on he A* algo i hm. Once he obo is ou side he en apmen egion
( he e is no mo e obs uc ion be ween he obo and he a ge ), he ask Fo ma ion u ns
o he A* algo i hm and u ns back on he po en ial unc ions. The swi ching app oach
is implemen ed in he o ma ion ask as illus a ed in Fig. 5.3. This swi ch is due o he
ac ha he po en ial unc ions ha e much less compu a ional cos s hen using A* pa h
planning and when he A* is execu ed i is necessa y o s op p ocessing some ea u es o
he con olle in o de o speed up he p ocessing.
Figu e 5.3: Swi ching app oach
Finally, he same conside a ion made in he ma e a oidance unc ion has o be consid-
e ed he e by c ea ing a secu i y zone. An obs acle may appea in he isible zone owa ds
he obo oo apidly o he obo o a oid i . To a oid collision in hese cases a p o ec ion
zone is c ea ed a ound he obo s whe e he weigh o he a ac ion and epulsion unc-
ions ( e ms o he NMPFC cos unc ion) a e apidly swi ched so he obo gi es p io i y
o penalize he ma e a oidance a he hen ge o he a ge . When he obo s a e ou side
his zone once again, he weigh s o he cos unc ion a e se back o no mal.

92 Obs acle A oidance in Fo ma ion Con ol
5.4 The Modi ied A* Pa h Planning Algo i hm
I is known ha mos en i onmen s a e highly dynamic, highly complex and ha e ob-
s acles mo ing andomly. Conside ing he dynamic cons ains o he obo , he si ua ion
s udied is o en common in he eal wo ld, which is o ind he as es solu ion be ween
he ini ial s a e qini and he goal s a e q a ge , a oiding as many collisions as possible.
The e o e, one o he concep s ha is necessa y o highligh is ha he bes solu ion, in
mos cases, is no gi en by he sho es pa h (op imal pa h) and can lead o undesi ed col-
lisions. In ano he wo ds, he bes solu ion is no he sho es pa h ( he op imal one), bu
he as es pa h (usually he subop imal one). Tha is because he eloci y o he obo is
no cons an ( he obo has limi ed accele a ion) and he obo con olle has di icul y in
ollowing ajec o ies wi h ab up changes in di ec ion. To accomplish ha , wo modi i-
ca ions we e made in he A* algo i hm o achie e an op imal solu ion. The A* algo i hm
can be seen below.
Algo i hm A*
1: Add he ini ial node o O-lis
2: Do
3: Choose n∗ om O-lis in which
4: F(n∗)≤F(n)∀n∈O-lis
5: Remo e n∗ om O-lis and pu in o he C-lis
6: Fo all n∈S a (n∗), which n/∈C-lis , do:
7: i (n/∈O-lis ) hen
8: add nnode o O-lis
9: else i (g(n∗)+c(n∗,n)<g(n)) hen
10: change he a he node om n o n∗
11: end
12: end
13: While (O-lis 6=0) o (n∗= end node)
whe e:
1. S a (n∗) = The se o neighbo s o node n∗
2. c(n1,n2) = Cos om going om node n1 o node n2
3. n∗= Bes no e in he neighbo hood
The e o e, A* is a g aph sea ch algo i hm which calcula es he sho es pa h h ough
a g aph be ween he ini ial and inal node. This algo i hm uses a heu is ic unc ion
F(n) = g(n)+h(n)(5.8)
5.4 The Modi ied A* Pa h Planning Algo i hm 93
ha es ima es he lowes cos o going om he ini ial o he a ge poin while passing
h ough node n. This se s he sea ch p io i y o nodes o ind he bes pa h as soon as
possible. This unc ion is he sum o wo o he unc ions.
1. g(n)= Cos om he o igin o node n;
2. h(n)= An heu is ic o es ima e he cos o he pa h om node n o he a ge node.
In his algo i hm he e a e wo lis s: he O-lis and he C-lis . The open lis , known as
he O-lis , con ains he nodes ha a e candida es o explo a ion. The closed lis , known
as he C-lis , con ains he al eady explo ed nodes. The nodes om C-lis whe e p e iously
in he O-lis bu as hey whe e explo ed, hey we e mo ed o he C-lis . The nodes in hese
lis s s o e he " a he " node, which is he node used o op imally each hem. This is he
node ha lies in he sho es pa h om he o igin o cu en node.
Once p esen ed he algo i hm, i becomes impo an and necessa y o emphasize some
conside a ions. Mainly, i ’s known ha he pa h τis op imal i an heu is ic unc ion
h(n)is admissible. This happens i he unc ion ne e o e es ima es he cos o each he
des ina ion, o in ano he wo ds i
h(n) <hm(n)∀n(5.9)
whe e hm(n)is he lowes cos om n un il he des ina ion.
To use he A* algo i hm in he calcula ion o a obo ’s pa h, i is necessa y o di ide
he en i onmen map in cells, as s a ed in he me hod app oxima e cell decomposi ion.
He e, each cell ep esen s a node. Each node can be connec ed o o he nodes and mo ing
om one node o he o he has an associa ed cos . In his case, he cos is he me ic
dis ance be ween he cell cen e s. The A* can calcula e he pa h ha minimizes he cos
om mo ing om he s a ing cell o he a ge cell. In Fig. 5.4 om [110], he black
cells ep esen he obs acles, he yellow cell ep esen s he ini ial posi ion (node) and he
blue cell ep esen s he des ina ion poin (node).
Finally, i is conside ed ha obo Ain he wo kspace Wis ep esen ed by he ini ial
node and ha occupies only a single node, being his las one he geome ic cen e o
he objec s o m an ae ial iew. The des ina ion node is he a ge poin q a ge . All
o he mo ing objec s a e conside ed C-obs acles (CBi). As he obo is ep esen ed by
a single cell, he obs acles ha e o be bigge in a way o ep esen bo h obs acle and he
obo ’s body. Each obs acle is ep esen ed by a ci cle wi h adius equal o he sum o he
obs acle’s adius and he obo ’s adius. This ep esen a ion can be seen in Fig. 5.5 om
[110].
94 Obs acle A oidance in Fo ma ion Con ol
Figu e 5.4: Map Cell Decomposi ion [110]
Figu e 5.5: Obs acle’s o al adius [110]
5.4.1 The Modi ica ions
The cell decomposi ion algo i hms such as D* (and i s e olu ions such as D*-Li e and
E*), ARA* and AD* a e based in he A* and we e de eloped o sol e p oblems o com-
pu a ional cos , p ocessing ime o memo y use. The modi ica ions p oposed by [110]
and [119] a e in he con igu a ion space and no in he algo i hm co e i sel . The e o e, in
he ma e o con igu a ion space, all he p e ious algo i hms o m A* amily should gi e
an equal o simila solu ion o he A* algo i hm. When applying he modi ica ion in any
algo i hm om he A* amily, he inal solu ion is be e .
Finally, in his app oach he au ho s in [110] and [119] base he modi ica ions in he
me hod o Cell Decomposi ion whe e he modi ica ions a e no in he A* algo i hm bu
in he con igu a ion space o la e un A* algo i hm o ind he bes pa h. The ad an age
comes wi h he ac ha in he Cell Decomposi ion he e a e no local minimums, while
in he VFH o in o he simila app oaches he local minima can become a p oblem when
ying o a oid na ow a eas. The global modi ied A* pa h planned p oposed in [110]
and [119] was he one used he e in he leade obo and in he case o exi ing en apmen
si ua ions (singula i ies).
5.5 Resul s 95
5.5 Resul s
Bo h simula ion and expe imen s wi h eal obo s esul s ollow he same se up done in
he p e ious chap e . The esul s ega ding he obs acle and ma e a oidance esul s a e
di ided in simula ion and expe imen wi h eal obo s. He e, h ee si ua ions a e p esen ed
in simula ion en i onmen and a e also used in expe imen s wi h eal obo s. Fu he mo e,
he i s wo simula ions a e pe o med wi h bo h homogeneous and he e ogeneous g oups
o obo s.
5.5.1 Simula ions
The simula ion se up ollows he same con igu a ion as in chap e 4wi h wo compu -
e s and he SimTwo simula o en i onmen . The s a ic obs acles in he simula ion we e
conside ed o be boxes wi h 0.75x0.75x0.5m(dep h x wid h x heigh ) in dimension. The
mo ing obs acles a e conside ed as sel mo ing sphe es in a semi- andom pa h.
−5 −4 −3 −2 −1 0 1 2 3 4 5
−5
−4
−3
−2
−1
0
1
2
3
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Figu e 5.6: Simula ion 1: Homogeneous Fo ma ion - Plo XY
5.5.1.1 Simula ion 1: Homogeneous Fo ma ion
The i s pai o simula ions add esses he co ido p oblem in obs acle a oidance whe e
he obo s ha e o pass h ough a co ido while changing he o ma ion in o de o a oid
he obs acles (co ido ’s walls). The objec i e he e is o sea ch o he ball depa ing om
he coo dina es (4.3,-1.9), (4.3,-3.1) and (5.5,-3.1) o he obo s 1, 2 and 3, espec i ely.
The obs acles’ walls a e made by six blocks in he coo dina es (-2.8,-1.7), (-2,-1.7), (-1.2,-
1.7), (-2.8,-4.5), (-2,-4.5) and (-1.2,-4.5) as p esen ed in Fig. 5.6. Robo 2 is he leade
obo in his simula ion. A g aph wi h he dis ance be ween he ollowe obo and he
leade obo as well as he angle be ween hem can be seen in Fig. 5.7.
By analyzing he simula ion g aphs and ideo C2T1 a ached, i can be said ha he
obo s succeed b eaking he o ma ion no by swi ching i , bu by a minimiza ion c i e ia
o a oiding obs acles and ma es du ing he expe imen . Du ing he passage h ough he
102 Obs acle A oidance in Fo ma ion Con ol
0 2 4 6 8 10
0
1
2
3
4
5
6
Time [s]
Dis ance o he Leade [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
3 4 5 6 7 8 9 10 11
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Time [s]
de (Co ) [m2]
To al Amoun o Unce ain y
Figu e 5.17: Real Expe imen 1 - Dis ance be ween Robo and he Ball and De e minan o Σ⊥
Me ged
5.5.2.2 Real Expe imen 2
This second expe imen add esses he en apmen p oblem in obs acle a oidance whe e
he obo has o a oid an U shape obs acle by changing he o ma ion in o de o a oid
he obs acle. Simila ly o simula ion 2, he leade obo ( obo 1) is nea he ball, sending
he in o ma ion o he o he obo ( obo 2) o whe e he a ge is loca ed. The e o e, he
objec i e he e is o con e ge o he ball depa ing om he coo dina es (3,1.2), (-3,-0.2)
and (3,0) o he obo s 1, 2 and a ge , espec i ely. The NMPFC a ac s he obo and
una oidably pu s he obo s in a local minima jus as he po en ial unc ion would beha e.
Howe e , he A* pa h planne s a s ins an ly when he obo s en e his si ua ion and
places he obo s ou side he U shape obs acle. The walls and he obo s’ mo emen s a e
p esen ed in Fig. 5.18 and in he ideo C2T2Real ha shows ha obo 2 was success ul
in lea ing he en apmen si ua ion and ha no collisions be ween obo s occu ed.
−2 −1 0 1 2 3 4
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
2.5
3
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Ball
Figu e 5.18: Real Expe imen 2 - En i onmen and Plo XY
A g aph wi h he dis ance be ween he obo and he ball and he minimiza ion o he
me ged co a iance’s de e minan can be seen in Fig. 5.11. I is no iceable he e he same
beha io om he NMPFC as in he p e ious expe imen and due o he same easons.

5.5 Resul s 103
0 1 2 3 4 5 6 7 8 9 10 11
0
1
2
3
4
5
6
Time [s]
Dis ance o he Ball [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
3 4 5 6 7 8 9 10 11
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Time [s]
de (Co ) [m2]
To al Amoun o Unce ain y
Figu e 5.19: Real Expe imen 2 - Dis ance be ween Robo and De e minan o Σ⊥
Me ged
Analyzing bo h ideo and he Plo XY p esen ed in he Fig. 5.18 he pa h made by
obo R2can be unde s ood. Du ing he con e gence, obo R2ge s apped be ween he
obs acle, he end o he ield and obo R1, which s ood in on o obo R2. Then, obo
R2s a s sea ching o a way h ough, always ying o con e ge o he a ge . As he
minimiza ion o he co a iance is coope a i e, obo R1mo es i sel in o ano he pose al-
lowing obo R2 o pass and con e ge o he a ge . I can be seen h ough Fig. 5.18 ha in
he eal expe imen he con e gence o bo h obo s owa ds he a ge was accomplished.
In he con e gence, i is common o see small e o s in he dis ance be ween he obo s
and he a ge . These e o s a e explained by a ew small ision p oblems ha exis in he
sys em (e en a e calib a ion) and which c ea e e o s in he localiza ion o he a ge in
he obo ’s ame. Fu he mo e, each obo has small localiza ion e o s, which, summed
wi h he ision e o s, gene a e he e o s in he wo ld ame and he e o e each obo
sees i s ball in a di e en posi ion. Finally, all he balls a e hen used in he CTE and a
used a ge is gene a ed and sp ead h ough RTDB, no elimina ing he dis ance e o s
comple ely.
5.5.2.3 Real Expe imen 3
This las expe imen epea s he c owded en i onmen in simula ion 3. The objec i e is o
con e ge obo 2 owa ds he ball. The obo leade ( obo 1) is nea he ball and i passes
he a ge ’s coo dina es o he o he obo . The wo pe sons passing h ough he obo ’s
pa h a e conside ed as mobile obs acles. The obo sees hei legs and conside ed as black
blobs a oiding hem as any o he obs acle. The pa h made by he obo s can be seen in
Fig. 5.20. The g aph o he dis ance shows he obo s’ con e gence owa ds he a ge .
The co a iance g aph, s a ing om a dis ance whe e bo h obo s see he ball ( om 2s on)
is also p esen ed con aining some noise due o he empo a ily obs uc ion made by he
obs acles in obo 2 a ge obse a ion.
104 Obs acle A oidance in Fo ma ion Con ol
−3 −2 −1 0 1 2 3
−3
−2.5
−2
−1.5
−1
−0.5
0
0.5
1
1.5
2
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Ball
Figu e 5.20: Real Expe imen 3 - Plo XY
0 2 4 6 8 10 12
1
1.5
2
2.5
3
3.5
4
4.5
5
5.5
6
Time [s]
Dis ance o he Ball [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
2 4 6 8 10 12
0.2
0.3
0.4
0.5
0.6
0.7
0.8
Time [s]
de (Co ) [m2]
To al Amoun o Unce ain y
Figu e 5.21: Real Expe imen 3 - Dis ance be ween Robo and De e minan o Σ⊥
Me ged
Figu e 5.22: Real Expe imen 3
5.6 Conclusion 105
A g aph wi h he dis ance be ween he obo and he ball as well as he minimiza ion
o he me ged co a iance’s de e minan can be seen in Fig. 5.21. Fu he mo e, he sc een
sho s o he expe imen s showing he obo ’s mo emen and he obs acles’ mo emen a e
p esen ed in Fig. 5.22. Mo eo e , he ideo C2T3Real also shows ha no collisions
happened. No e ha in his expe imen , as he e is no obs acles pushing he obo s a a ,
hey pu hemsel es in he ideal posi ion o he me ged co a iance minimiza ion.
5.6 Conclusion
This chap e had he main objec i e o be e explaining he use o he app oach while
u ilizing a i icial po en ial ields (APF) conside ed as a e m o he nonlinea model p e-
dic i e o ma ion con ol’s cos unc ion, in o de o a oid s a ic o mo ing obs acles.
Rega ding he singula i y p oblems ha a e p esen when using he APF, he modi ied A*
algo i hm was conside ed as a complemen o he po en ial ield app oach du ing hese
cases.
As he NMPFC al eady minimizes a cos unc ion, he APF app oach made easible
he solu ion o he obs acles and ma es a oidance p oblem as a e ms o he NMPFC cos
unc ion. Ano he con ibu ion was in oduced as a swi ching app oach ha , in case o
singula i ies, uses a modi ied e sion o he A* pa h planne in o de o exi he en apmen
si ua ion.
The esul s demons a ed he e iciency o his app oach in o ma ion con ol when
embedded in he NMPFC cos unc ion by success ully a oiding he obs acles in expe -
imen s bo h done in simula ion and wi h eal obo s. In he co ido case, he o ma ion
o mul iple obo s could be seen a oiding wo walls o (s a ic) obs acles, b eaking he
desi ed o ma ion by he obs acle a oidance penaliza ion e m and, a e passing h ough
he co ido , con e ging back in o ma ion. The c owded en i onmen demons a ed he
same obs acle a oidance capabili y ega ding mobile obs acles.
Howe e , he APF app oach also has i s d awbacks ega ding conca e obs acles whe e
local minima may occu . In such si ua ions, he modi ied A* algo i hm p o ed o be a
easible empo a y al e na i e.
The main ad an age o his app oach is o conside in he same minimiza ion p oblem
bo h con olle and obs acle/ma e a oidance p oblem. This app oach excludes he need
o a pa h planne in he ac i e a ge acking p oblem. Howe e , as disad an age his
app oach needs o swi ch o a global pa h planne , in some special occasions (singula i y),
in o de o lea e an en apmen si ua ion ha may occu .
Finally, i can be concluded ha despi e he ac ha in a single obo si ua ion, a global
pa h planne would ha e be e esul s in obs acle a oidance p oblem, when conside ing
a o ma ion o a mul i- obo sys em, his app oach p o ed o be a be e choice. This is
106 Obs acle A oidance in Fo ma ion Con ol
due o he ac ha he o ma ion uses only one cos unc ion o minimize bo h o ma-
ion beha io e ms ha pe o m he con e gence owa ds a a ge and he obs acle/ma e
a oidance e ms.
Chap e 6
In elligen S a e Changing Applied o
Mul i-Robo Sys ems
The a ge sea ching p oblem is a si ua ion whe e a o ma ion o mul i- obo sys ems is se
o sea ch o a a ge and con e ge owa ds i when i is ound. This p oblem lies in he ac
ha he a ge is ini ially absen and he o ma ion mus sea ch o i in he en i onmen .
Du ing he a ge sea ch, alse a ge s may appea d agging he o ma ion owa ds hem.
The e o e, in o de o a oid he o ma ion ollowing a alse a ge , his chap e p esen s
a new me hodology using he Takagi-Sugeno ype uzzy au oma on (TS-TFA) in he a ea
o o ma ion con ol o sol e he a ge sea ching p oblem. The TS uzzy sys em is used
o change he o ma ion h ough he modi ica ions in he s a es o he au oma on. This
change does no only swi ch he ules and he e o e he s a e o each obo , bu also he
con olle s and cos unc ions. This app oach ampli ies he e sa ili y o he o ma ion
o mobile obo s in he a ge sea ching p oblem. In his wo k, he TS-TFA is p esen ed
and i s implica ions in he o ma ion a e explained. Simula ions and esul s wi h eal
obo a e p esen ed whe e i can be no iced ha he o ma ion is b oken o maximize he
pe cep ion ange based on each obo ’s obse a ion o a possible a ge . Finally his wo k
is concluded in he las sec ion.
6.1 In oduc ion
Fuzzy sys ems and au oma ons a e o en applied in obo ics. Se e al esea ch p opos-
als use uzzy sys ems o con ol, na iga e o e alua e obo sys ems and he app oach
mos equen ly used is he Mamdani uzzy sys em [149], [150], [151] and [152]. Mo e-
o e , obo ic socce has become a good wo k-bench o es ing a i icial in elligence al-
go i hms and i plays an impo an ole in he p og ess o he in elligen con ol algo i hm
107

108 In elligen S a e Changing Applied o Mul i-Robo Sys ems
ield [153], [154], [155] and [156]. In o de o a oid de ia ing he o ma ion om ol-
lowing a alse a ge , his chap e p esen s he use o a ecen ly p esen ed echnique, he
Takagi-Sugeno ype uzzy au oma on (TS-TFA) [157]. The TS-TFA selec s he oles (be-
ha io s) applied o he o ma ion con ol o h ee 5dpo socce obo s [123]. These obo s,
usually pa icipan s o RoboCup [158], a e used he e as a pla o m o demons a e he
pe o mance o a o ma ion du ing he sea ch o a a ge when he e is he commu ing
absence/p esence o his a ge . The p oblem he e lies in he ac ha he a ge is ini ially
absen . Du ing he a ge sea ch, alse a ge s may appea d agging he o ma ion owa ds
hem. The e o e, as a p oposed solu ion o his p oblem, he uzzy au oma on con ains
h ee s a es which a e su icien in o de o sol e he p oblem in he p oposed case o
s udy. The uzzy au oma on does no only change he obo s’ ole bu i changes also
he obo s’ cos unc ion o con olle (depending on he s a e change), while pu suing o
sea ching o a a ge . A s a ic leade app oach was chosen o his wo k; ne e heless,
he p oposed con ibu ion also wo ks wi h a non-s a ic leade app oach. Simula ions and
expe imen s wi h eal obo s will also be p esen ed in his chap e .
The posi ioning o obo s in a dynamic en i onmen is also ano he issue ega ding
changes o oles in mul i- obo sys ems [159] and [160]. In he wo k o Akiyama e .
al. [161], he au ho s p oposed a no el agen posi ioning mechanism o he dynamic
en i onmen s. They s a e ha because he eal-wo ld p oblem is gene ally dynamic, sui -
able posi ions o each agen should be de e mined acco ding o he cu en s a us o he
en i onmen . The e o e, he au ho s in [161] o malized his issue wi h a map om a
ocal poin (like a ball posi ion in a socce ield) o a desi able posi ioning o each playe
agen . Fu he mo e, hey p oposed a me hod o app oxima e his map using Delaunay
T iangula ion. The pe o mance o he me hod was e alua ed in RoboCup socce simu-
la ion en i onmen and compa ed o o he unc ion app oxima ion me hods such as he
no malized gaussian ne wo k (NGN).
Fu he mo e, S one and Veloso [162] in oduced pe iodic eam synch oniza ion (PTS)
domains as ime-c i ical en i onmen s in which agen s ac au onomously wi h low com-
munica ion, bu in which hey can pe iodically synch onize in a ull-communica ion se -
ing. The wo main con ibu ions o his a icle we e a lexible eam agen s uc u e
and a me hod o in e -agen communica ion in domains wi h un eliable, single-channel,
low-bandwid h communica ion. In said pape , homogeneous agen s can lexibly swi ch
oles wi hin o ma ions, and agen s can change o ma ions dynamically, acco ding o
p e-de ined igge s which a e e alua ed a un- ime. This lexibili y inc eases he pe -
o mance o he o e all eam. Ou eamwo k s uc u e u he includes p e-planning o
equen si ua ions. Second, he no el communica ion me hod is designed o use du ing
he low-communica ion pe iods in PTS domains. Finally, hey ully implemen ed bo h he
lexible eamwo k s uc u e and he communica ion me hod in he domain o simula ed
6.1 In oduc ion 109
obo ic socce , and conduc ed con olled empi ical expe imen s o e i y hei e ec i e-
ness.
Rega ding he dynamic change o oles, he wo k o Reis e . al. [163], p esen s an
in e es ing app oach. In hei pape , hey p oposed an app oach o coo dina ing a eam
o homogeneous agen s based on a lexible common eam s a egy as well as on he con-
cep s o si ua ion based s a egic posi ioning and dynamic posi ioning and ole exchange.
The au ho s also in oduced an agen a chi ec u e including a speci ic high-le el decision
module capable o implemen ing his s a egy. Thei p oposal was based on he o mal-
iza ion o he idea o wha a eam s a egy is o compe ing wi h an opponen eam ha ing
opposi e goals. Agen ’s eac i i y was also in oduced o app op ia e esponse o he dy-
namics o he cu en si ua ion. Howe e , in hei app oach his was done in a way ha
p ese es eam cohe ence ins ead o pe mi ing uncoo dina ed agen beha io .
The au ho s in [164] in es iga e he leade ollowe mo ion coo dina ion o mul iple
nonholonomic mobile obo s. A combina ion o he i ual ehicle and ajec o y acking
app oach was used o de i e he o ma ion a chi ec u e. A i ual ehicle was s ee ed in
such a way i s abilizes o a shi ed e e ence posi ion/heading de ined by he leade , he
eloci y o he i ual ehicle was p o ided o u he use in designing con ol law o
he ollowe independen om he measu emen o leade ’s eloci y. Posi ion acking
con ol was hen cons uc ed by he au ho s o he ollowe o ack he i ual ehicle
using he backs epping and Lyapuno di ec design echnique. Fu he mo e, and o en-
su e he sa e y o obo s while mo ing in a dynamic en i onmen , an obs acle a oidance
scheme based on sensing he ela i e dis ance be ween ollowe obo s and obs acles was
in oduced by [164] using uzzy logic.
Changing oles in a mul i- obo o ma ion sys em equi es a high le el o na iga ion
and con ol. Role assignmen , acco ding o he obo ’s ea u es, is a c ucial s ep in he
coo dina ion o mul iple he e ogeneous obo s. The esea ch o [165] p esen s a s a egy
o picking he e ogeneous playe s and o ming a socce eam in he RoboCup simula ion
en i onmen , which is a mul i- obo coo dina ion esea ch pla o m. Using uzzy e alua-
ion and uzzy in e ence, he au ho s iden i y he mos sui able ole in a socce eam o
a gi en he e ogeneous obo . In he games, he eam ha possessed his s a egy as i s
bases showed a signi ican ly be e pe o mance when compa ed o a eam based on he
p e ious hand- uned solu ion.
The au ho s in [166] p esen ed a uzzy logic based sys em o o ma ion con ol
o mul iple mobile obo s. Two main p oblems o o ma ion con ol a e in es iga ed -
main aining co ec o ma ion posi ion and in e - o ma ion collision a oidance. A leade -
ollowe app oach wi h minimal communica ion be ween obo s was p esen ed he e. Sep-
a a e uzzy logic con olle s we e de eloped by he au ho s o o ma ion posi ion con ol
and in e nal collision a oidance wi h a highe le el uzzy coo dina o used o use hei
ou pu s. A majo issue in o ma ion con ol is he p esence o unce ain ies in he eal
110 In elligen S a e Changing Applied o Mul i-Robo Sys ems
wo ld, in he o m o noisy senso da a and delay in leade posi ion ansmission. Noise
was added o he simula ed da a o p o e ha he sys em was capable o ole a ing such
dis u bance.
In [167], he au ho s ocus on he middle size socce obo league (MSL) as well
as on new hie a chical hyb id uzzy me hods o decision making and ac ion selec ion
o a obo . In a icle [167], he beha io o an agen was in oduced, implemen ed and
classi ied in wo laye s, he Low Le el Beha io and he High Le el Beha io . In phase
one, he obo ’s si ua ion is checked in o de o a decision o be make on how o pe o m
he equi ed beha io . In he second phase, he eam s a egy, eam o ma ion, obo ’s ole
and he obo ’s posi ioning sys em a e in oduced. A uzzy logic app oach is adop ed o
p o ide he playe wi h he bes posi ion o mo e based on he in o ma ion gi en by he
cu en s a e.
The inhe en unce ain y p esen in obo ics in gene al and RoboCup in pa icula
demands he use o p obabilis ic me hods. Wi h i s uzzy cons uc s, Fuzzy Logic has
been used as an app oach o ace he cu en p oblems o obo ics including unce ain y.
Ex ending he use o uzzy logic wi h ype-2 sys ems and high le el wo ld models should
p esen new solu ions o he obo ics domain. In [168] he au ho s in oduce an a emp
o p esen a solu ion o he no ully esol ed ques ion o using ac ions oge he , using a
uzzy ype-2 app oach wi h he added la o o semi quali a i e wo ld model.
An obs acle a oidance mechanism was de eloped by [169] o o ma ion con ol o
mul iple mobile obo s. This mechanism is designed based on he sequen ial si ua ions
when he obo s likely ha e a chance o collide wi h each o he . Fi s , he obs acle a oid-
ance is enabled i he dis ance be ween obo and obs acle is smalle han a designed
dis ance and he obs acle is loca ed on he way whe e he obo is mo ing o. Second, he
ime o u n on he mechanism o obs acle a oidance is decided by a p ede ined dis ance.
Finally, an al e na i e pa h is gene a ed o he obo o a oid obs acles by using a ec-
o ope a ion. Meanwhile, a obo beha io -based o ma ion con ol was also cons uc ed
applying a h ee le el a chi ec u e. The de eloped mechanism was hen applied o he
o ma ion con ol o a g oup o omnidi ec ional obo s.
The o ma ion p oblem o a g oup o obo s is one o challenging esea ch di ec ions
o mul i- obo sys em. Aiming a he o ma ion con ol p oblem, [170] p oposed a kind
o hyb id a chi ec u e i s ly, which combined he eac i e a chi ec u e based on mo o
schema wi h he hie a chical a chi ec u e. Secondly ou kinds o basic beha io s based
on he eac i e a chi ec u e had been designed, each beha io implemen ed by a uzzy
neu al ne wo ks. Las ly, i was used ne e cell o use he ou pu s o each o beha io o
ob ain he inal ou pu s o he sys em.
Usually, app oaches o selec oles use Mamdani uzzy sys ems [171]. The pape w i -
en by [172] p esen ed a new uzzy-gene ic analy ical model o he p oblem o p ojec
eam o ma ion. I buil on p e ious quan i a i e app oaches, bu added se e al modeling
6.2 The S a e Machine 111
enhancemen s like de i a ion o pe sonnel a ibu es om dynamic quan i a i e da a and
complex a ibu e modeling. The au ho s imp o ed he lexibili y o equi emen speci-
ica ion using a special o ma ha exp esses he equi ed eam capabili ies using uzzy
desc ip o s. A single compound objec i e unc ion was hen de ined, which inco po a ed
mul iple c i e ia ha he solu ion should maximize. To op imize he selec ion o mul i-
ple p ojec eams wi h possibly con lic ing equi emen s, he au ho s p oposed a special
adap a ion o island gene ic algo i hm wi h mixed c osso e whe e he i ness o common
solu ion was used o d i e he selec ion wi hin he islands.
In [173], an app oach ha ga e he s a ing poin o his con ibu ion was p esen ed.
In [173], a new hie a chical hyb id uzzy-c isp me hod o decision making and ac ion
selec ion o an agen in socce simula ion 3D en i onmen was p esen ed. Fi s , he skills
o an agen we e in oduced, implemen ed and classi ied in wo laye s, he basic-skills
and he high-le el skills. In he second laye , a wo-phase mechanism o decision making
was in oduced. In phase one, some use ul me hods we e implemen ed which check he
agen ’s si ua ion o pe o ming equi ed skills. In he nex phase, he eam s a egy,
eam o ma ion, agen ’s ole and he agen ’s posi ioning sys em we e in oduced. A uzzy
logical app oach was employed o ecognize he eam s a egy and u he mo e o ell he
playe he bes posi ion o mo e.
In all p e iously men ioned app oaches i is assumed ha he ball ( a ge ) is in he
g oup obse a ion ange, whe e a leas one obo is seeing he a ge . This si ua ion
does no occu in he a ge sea ching p oblem. To sol e his p oblem, he TS-TFA was
p oposed and implemen ed. To desc ibe he TS-TFA implemen ed he e, his chap e is
o ganized in he ollowing o de : The nex sec ion p esen s he s a e machine ha ep-
esen s he obo ’s possible beha io . Sec ion 6.3 p esen s he TS ype uzzy au oma on.
The p oblem is o mula ed in he ollowing sec ion and in sec ion 6.5 he esul s ob ained
bo h in simula ion and wi h eal obo s a e p esen ed. Finally, he conclusion is p esen ed
in he las sec ion.
6.2 The S a e Machine
The TS-TFA combines uzzy se s and au oma on heo y. In one o he mos popula
applica ions o he au oma on heo y in obo ics, a s a e machine is used o de ine a high
le el con ol (o na iga ion). This na iga ion sys em is based on he obo ’s change in
beha io (e.g. changing om a s ands ill s a e o a mo ing s a e). In his chap e , he
mul i- obo sys em pe o ms a sea ch and ack ask o a a ge (ball). In his ask, he ball
can be wi hin he obo ’s senso ange o no . The e o e, he mul i- obo sys em changes
i s beha io om h ee s a es ha can be seen in he s a e machine shown in Fig. 6.1.
118 In elligen S a e Changing Applied o Mul i-Robo Sys ems
ma ion’s beha io . He e, obo 1 is he leade obo , while obo s 2 and 3 a e he ollowe s.
Robo s 1, 2 and 3 depa om coo dina es (4.5,-3), (4.5,-1) and (6.5,-3), espec i ely. To
simula e he appea ance o a alse a ge du ing he sea ch p ocess, he ball was ini ially
placed in he coo dina es (2,2.9). A e a while du ing he simula ion he ball is changed
o he coo dina es (-4.5,6) whe e he " ue" ball would be loca ed.
−6 −4 −2 0 2 4 6
−3
−2
−1
0
1
2
3
4
5
6
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 6.3: The P oblem Simula ion: Fo ma ion Beha io wi hou he TS-TFA app oach
This simula es a si ua ion whe e he obo o he whole o ma ion sees a alse ball
and, ge ing close o he obse ed objec , disca ds i as he a ge (ei he by he objec
o ma o any o he ision p ocess ha occu s wi h he colo segmen a ions p ocess used
o ecognize he ball). In his case, wi hou he p oposed app oach, he o ma ion would
be as shown in Fig. 6.3.
−6 −4 −2 0 2 4 6
−3
−2
−1
0
1
2
3
4
5
6
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 6.4: The Solu ion Simula ion: Fo ma ion Beha io wi h he TS-TFA app oach
No e ha in his example, one o he obo s sees he alse a ge and sends he in-
o ma ion o he o he obo s. Then, all obo s in o ma ion y o con e ge owa ds his
alse a ge . When he o ma ion app oaches he obse ed objec , he obo s conside his
as a alse a ge and ha e o es a he sea ch once mo e. Howe e , when applying he
p oposed echnique, he beha io o he o ma ion is imp o ed. Ins ead o all he o ma-
ion con e ging o a alse a ge , only he obo ha sees i weigh s be ween he quali y
o i s obse a ion and he dis ance ha i is om he obse ed objec . Then, his obo
b eaks i sel om he o ma ion ying o con e ge owa ds he possible a ge while he

6.5 Resul s 119
o he obo s keep sea ching. By analyzing Fig. 6.4, i can be seen ha when he ob-
se ed objec is conside ed as a alse a ge , he obo con e ges back o he he o ma ion
which was s ill sea ching o a " ue" a ge . Once he o ma ion inds he " ue" a ge , i
con e ges success ully.
6.5 Resul s
The esul s a e p esen ed in wo sec ions. The i s ou lines he simula ion esul s using he
SimTwo simula o [117]. In he simula ions he obo s ( h ee omnidi ec ional 5dpo socce
obo s) should achie e a inal o ma ion con igu a ion a ound he ball, o ming a 120◦
(deg ee) angle be ween hem. The h ee simula ions p esen ed he e a e h ee di e en
s a ing si ua ions: Simula ion 1: Robo 3 is ela i ely nea o he a ge and obo s 1
and 2 a e ela i ely a om he a ge ,Simula ion 2: Robo 2 is ela i ely nea o he
a ge and obo s 1 and 3 a e e y a om he a ge and Simula ion 3: Robo s 1, 2
and 3 a e e y a om he a ge . Fu he mo e, he subsec ion 6.5.2 p esen s he esul s
o he expe imen wi h eal obo s, whe e he eal 5dpo socce obo s we e used. The
expe imen was only conduc ed wi h he las simula ion case o alida e his heo y. As a
sa e y measu e, he maximum obo eloci y allowed was 0.7m/s.
6.5.1 Simula ions
This sec ion p esen s all h ee simula ions. I is impo an o emembe ha he e he ball
is always s opped and obo 1 is he obo leade . All he se up made in chap e 4 o he
simula ion en i onmen was also made he e.
6.5.1.1 Simula ion 1
In his simula ion, he ball was placed a he coo dina es (2,3.5) and obo s 1, 2 and 3
we e placed a he coo dina es (4.3,-3.1), (4.3,-1.8) and (4.3,1.6), espec i ely. All obo s
ha e an ini ial θ=270oin wo ld ame. Fig. 6.5 shows ha obo 3 main ained i s ini ial
s a e Fo ma ion and mo ed owa ds he ball using he NMPFC, wi hou e e changing i s
s a e. Then, obo 1 ecei es Sea ch as i s ini ial s a e wi h a modi ied A∗pa h planne and
a eac i e con olle and i kep his s a e un il i eached he coo dina es (-3.2,2.8) whe e
i s con idence was high enough o change i s s a e o Fo ma ion.
Finally, obo 2 ecei es Sea chFollowe as i s ini ial s a e wi h a NMPFC con olle
wi h he cos unc ion o ollow a leade . Meanwhile, as he o ma ion mo es, obo 2
passes h ough coo dina es (3.5,-2) whe e i was close enough o he a ge o change i s
s a e o Fo ma ion. I o he obo s see a a ge , each obo weighs up he global quali y o
he ball pe cep ion and i s dis ance om he a ge . I he obo is oo a om he a ge ,
120 In elligen S a e Changing Applied o Mul i-Robo Sys ems
0 5 10 15 20
0
1
2
3
4
5
6
7
8
9
10
11
Time [s]
Dis ance o he Ball [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
Robo 3
−4 −3 −2 −1 0 1 2 3 4
−3
−2
−1
0
1
2
3
4
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 6.5: Simula ion 1: Dis ance Robo -Ball and Plo XY o he Robo s Pa h
i will no change i s s a e, as shown in able 6.1. He e, he change in s a e co esponds o
a change in he con olle .
By analyzing Fig. 6.5, i can be seen ha he e is no dis u bance in he obo s’ ajec-
o ies o any p esence o ins abili y in he s a e changes. The con olle is he same bu he
cos unc ions a e di e en and once again, no ins abili y could be no ed. The dis ance o
he a ge and hei pa h in an XY plo can be seen in Fig. 6.5, espec i ely. The beha io
o he obo s in his si ua ion can be seen in Fig. 6.6 whe e i is possible o see he change
in s a es du ing each ins an aken om he Coach’s iew o he simula ions.
6.5.1.2 Simula ion 2
He e, Fig. 6.7 shows, h ough he Plo XY, ha he ball was placed a he poin wi h
coo dina es (-5.3,0) and obo s 1, 2 and 3 we e placed a he coo dina es (4.3,-3.1), (-
6.3,0) and (4.3,3.1), espec i ely. Fu he mo e, all obo s ha e an ini ial θ=90◦in wo ld
ame. No e in he XY Plo om Fig. 6.7 ha he e is no dis u bance in he obo ’s
ajec o y and he e is no p esence o ins abili y in he s a e change. In he dis ance g aph
om Fig. 6.7, i can be seen ha he obo con e ged success ully owa ds he a ge .
The XY Plo om Fig. 6.7 demons a es ha obo 2 main ained i s ini ial s a e Fo ma-
ion and mo ed owa ds he ball using he nonlinea model p edic i e o ma ion con olle
(NMPFC). Fu he mo e, obo 1 ecei ed Sea ch as i s ini ial s a e wi h an A∗pa h plane
and a eac i e con olle and i main ained his s a e un il i eached a posi ion whe e i s
con idence was high enough o change i s s a e o Fo ma ion (coo dina es (1.1,-3)). I is
impo an o no e ha he change in s a e is also a change in he con olle and no ins abil-
i y could be seen in he obo ’s pa h. Howe e , he "i egula i y"’ seen in he pa h o obo
1 is due o he change in i s o ien a ion a ound 5 seconds a e he s a o he expe imen ,
as i can be seen in Fig. 6.8. The o a ion is decided and go e ned by he low-le el con ol
sys em.
6.5 Resul s 121
Figu e 6.6: Simula ion 1
122 In elligen S a e Changing Applied o Mul i-Robo Sys ems
0 2 4 6 8 10 12
0
1
2
3
4
5
6
7
8
9
10
11
Time [s]
Dis ance o he Ball [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
Robo 3
−6 −5 −4 −3 −2 −1 0 1 2 3 4
−4
−3
−2
−1
0
1
2
3
4
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 6.7: Simula ion 2: Dis ance Robo -Ball and Plo XY o he Robo s Pa h
0 2 4 6 8 10 12
−pi
−pi/2
0
pi/2
pi
Time [s]
Angle [ ad]
O ien a ion o he Robo
Robo 1
Robo 2
Robo 3
Figu e 6.8: Simula ion 2: O ien a ion o he obo s
Finally, obo 3 ecei es Sea chFollowe as i s ini ial s a e and i s a s mo ing owa ds
he obo leade using a NMPFC wi h a cos unc ion o ollow he leade . As i can be seen
in Fig. 6.7, when obo 3 eached he poin (0,-1), i places i sel wi h enough dis ance o
he a ge in o de o change i s s a e o Fo ma ion. He e, he con olle is he same bu
he cos unc ions a e di e en and once again, no ins abili y can be no iced.
6.5.1.3 Simula ion 3
The las simula ion case s a ed wi h obo s 1, 2 and 3 on one side o he ield placed
a coo dina es (4.3,-2), (4.3,0) and (4.3,2), espec i ely. Robo 1 is he leade obo he e
and he ball was posi ioned a coo dina es (-3,1). Fu he mo e, all obo s ha e an ini ial
θ=270◦in wo ld ame. No e in Fig. 6.9 ha he e is no dis u bance in he obo ’s
ajec o y and he e is no p esence o ins abili y in he s a e change.
By analyzing Fig. 6.9 his expe imen shows ha he obo s s a ed pe o ming he
sea ch o he a ge by mo ing a ound he ield. When obo s 1 and 3 eached he co-
o dina es (-0.4,-3.1) and (-0.1,-1.4) espec i ely, hey began o see he a ge , which in
u n changed he s a e o each obo . The e o e, hese wo obo s s a ed he p ocess o
pu suing he a ge . A his ins an obo 2 could no see he a ge ye . Meanwhile, obo
6.5 Resul s 123
0 2 4 6 8 10 12 14
0
1
2
3
4
5
6
7
8
9
Time [s]
Dis ance o he Ball [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
Robo 3
−4 −3 −2 −1 0 1 2 3 4
−3
−2
−1
0
1
2
3
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 6.9: Simula ion 3: Dis ance Robo -Ball and Plo XY o he Robo s Pa h
2 was a li le bi behind and con inued looking o he a ge and ollowing obo 1 un il
he g oup gained mo e con idence in he obse a ion while in u n obo 2 saw he a ge
as well.
6.5.2 Resul s o he Expe imen s wi h Real Robo s
This case was simila o he las simula ion case whe e obo 1 was he leade and he
sea ch should be pe o med by depa ing om an a ge absen s a e.
5 10 15 20 25
0
1
2
3
4
5
6
Time [s]
Dis ance o he Ball [m]
Dis ance Be ween Robo and Ball
Robo 1
Robo 2
Robo 3
−4 −3 −2 −1 0 1 2 3
−2
−1
0
1
2
3
X [m]
Y [m]
Fo ma ion Pa h (X/Y)
Robo 1
Robo 2
Robo 3
Ball
Figu e 6.10: Real Expe imen : Dis ance Robo -Ball and Plo XY o he Robo s Pa h
Howe e , as he eal ield o expe imen s is small and he ac ha he sea ch ou ine
makes he obo leade pe o m a squa e pa h in he ield independen om i s size he
ini ial coo dina es o obo s 1, 2 and 3 we e espec i ely (3,-2), (3,0) and (3,2). The
small di e ence be ween his expe imen and he las simula ion is ha he obo s depa
wi h a g ea e dis ance in he X axis which in u n does no ep esen any signi ican di -
e ence o he compa ison be ween simula ion and eal expe imen . Mo eo e , Fig. 6.10
shows he dis ance om he obo o he ball du ing bo h s a es Sea ch/Sea chFollowe
and Fo ma ion.

124 In elligen S a e Changing Applied o Mul i-Robo Sys ems
Figu e 6.11: Real Expe imen : Coach View
6.6 Conclusion 125
I is impo an o no e ha be o e changing o s a e Fo ma ion, he quali y o he a -
ge ’s pe cep ion is a ec ed by a ious ision noises due o he la ge dis ance be ween he
obo and i s a ge and he came a p ecision. This ac is demons a ed by he XY Plo
and by he dis ance g aph om Fig. 6.10, and i makes he impo ance o his con ibu ion
mo e no able. In his expe imen , he obo s s a ed pe o ming he sea ch o he a ge
by mo ing a ound he ield. When he obo s eached a ce ain poin in he ield (coo di-
na es (0.2,-1.5) o obo 1 and (0.5,0.2) o obo 3 which we e he i s s o see he a ge
as seen in Fig. 6.10), he obo s ha eached his speci ic dis ance changed hei s a e
and s a ed o pu sue he a ge . Meanwhile, obo 2 was a li le bi (1.2m) behind and
con inued looking o he a ge and ollowing obo 1 un il i saw he a ge as well. By
analyzing he dis ance g aph in Fig. 6.10, he main di e ence be ween he simula ion and
he eal obo expe imen a e no iceable. He e, he a ia ion in he pe cep ion o he a ge
can be seen while he obo s a e sea ching and coinciden ally app oaching he a ge . I is
he a ia ion in he quali y o he pe cep ion ha makes he uzzy sys em disca d he ball
posi ions. The poin whe e he a ge is easonably isible is eached in app oxima ely
ou seconds when i can also be no iced ha he pe cep ion quali y has imp o ed. The
sys em wo ks apidly and smoo hly, p e en ing he obo s om colliding, which is possi-
ble a high eloci y. Howe e his is no he case when ision pe cep ion is no o a high
quali y. The XY plo o he mo emen o he obo s is shown in Fig. 6.10. In his igu e,
he posi ions o he ball do no signi y ha he a ge mo ed, hey only mean ha he
posi ion o he a ge measu ed by he obo s was no p ecise un il he obo app oached
he a ge . This lack o p ecision in measu emen s makes he ball pe cep ion mo e in his
plo . The beha io o he obo s in his si ua ion can be seen in Fig. 6.11.
6.6 Conclusion
This chap e p esen ed a new me hodology, he Takagi-Sugeno ype uzzy au oma on (TS-
TFA), applied o he o ma ion con ol o mul i- obo sys ems o sol e he a ge sea ching
p oblem whe e he o ma ion is pu o sea ch o a a ge and hey con e ge o i when i
is ound. This p oblem lies in he ac ha he a ge is ini ially absen and he o ma ion
o a mul i- obo sys em mus sea ch o his a ge in he en i onmen . Du ing he a ge
sea ch, alse a ge s may appea . The e o e, in o de o a oid he o ma ion ollowing a
alse a ge , his chap e p esen ed he use o a ecen ly p esen ed echnique, he TS-TFA.
This echnique selec s one o he h ee oles applied o o ma ion ega ding he a ge
sea ching p oblem. The uzzy au oma on no only changed he obo s’ oles (au oma on
s a es) bu i also changed he obo s’ cos unc ion o con olle (depending on he s a e
change), while sea ching o a a ge .
The TS-TFA was modi ied sligh ly in o de o adap o he o ma ion con ol heo y
wi h he leade - ollowing app oach. F om he simula ions and he esul s wi h eal obo s
126 In elligen S a e Changing Applied o Mul i-Robo Sys ems
i can be no ed ha he o ma ion is b oken o maximize he pe cep ion ange based on
each obo ’s obse a ion o a possible a ge . I can also be no ed ha he change in he
con olle and he change in he cos unc ion do no make he sys em uns able in he
ep esen a i e cases p esen ed. Finally, his applica ion can be gene alized and he o ma-
ion can conside all so s o en i onmen applica ions, including an ou doo en i onmen
whe e in many ci cums ances, a alse a ge can be ound.
Chap e 7
Conclusions and Fu u e Wo k
This chap e concludes he wo k o his hesis. A inal obse a ion and commen a ies a e
made h oughou he conclusion. Some sugges ions o u u e wo ks a e also made in
his chap e in o de o gi e con inui y o he esea ch in o ma ion con ol o mul i- obo
sys ems.
7.1 Conclusion
The main objec i e o his hesis was he o ma ion con ol o a g oup o mobile obo s;
said con ol minimizes he o al amoun o unce ain y o a a ge ’s pe cep ion con e ging
owa ds a desi ed posi ion while a oiding collisions wi h he ma es, obs acles and he a -
ge . The hesis was ocused on he o ma ion wi h holonomic and nonholonomic mobile
obo s and ollowed a esea ch line ha modeled he o ma ion sys em, selec ed and con-
cei ed a o ma ion con olle and app oached he add essing oles o b eaking o ma ion.
This wo k was s uc u ed o p esen a summa y in he beginning o each chap e and he
ob ained esul s and conclusions in de ailed ashion in he end o each chap e . The e o e,
he summa y o his wo k and i s main conclusions a e as ollows:
•The i s pa o his wo k modeled he 5dpo middle size league omnidi ec ional
mobile socce obo in he SimTwo en i onmen . This ask was impo an o une
and alida e he heo y p oposed in his wo k. The modeling and alida ion we e
p o en accu a e;
•A nonlinea model p edic i e o ma ion con ol (NMPFC) was concei ed o pe -
o m a o ma ion con ol on a g oup o mobile obo s as he ollowing s ep. The
dis ibu i e con igu a ion was c ea ed using wi eless communica ion, whe e each
obo ecei ed da a om he o he obo s and p ocessed i s own ask in he o ma-
ion wi hou he need o any kind o supe iso .
127