Distributed Control and Stochastic Analysis of Hybrid Systems Supporting Safety Critical Real-Time Systems Design

Transcription

1 Dstrbuted Control and Stochastc Analyss of Hybrd Systems Supportng Safety Crtcal Real-Tme Systems Desgn WP6: Decentralzed Conflct Predcton and Resoluton Report on Global Decentralzed Conflct Resoluton Dmos V. Dmarogonas 1 and Kostas J. Kyrakopoulos March 15, 2005 Verson: 0.2 Task number: 6.2 Delverable number: D6.2 Contract: IST of European Commsson 1 Natonal Techncal Unversty of Athens NTUA).

2 Ttle of document: Authors of document: Delverable number: D6.2 Contract: Project: DOCUMENT CONTROL SHEET Report on Global Decentralzed Conflct Resoluton Dmos V. Dmarogonas and Kostas J. Kyrakopoulos IST of European Commsson Dstrbuted Control and Stochastc Analyss of Hybrd Systems Supportng Safety Crtcal Real-Tme Systems Desgn HYBRIDGE) DOCUMENT CHANGE LOG Verson # Issue Date Sectons affected Relevant nformaton All Frst draft ,4 Comments of NLR ncorporated Verson 2.0 Organsaton Sgnature/Date Authors Dmos V. Dmarogonas NTUA Kostas J. Kyrakopoulos NTUA Internal revewers Henk Blom NLR

3 HYBRIDGE, IST Work Package WP6, Delverable D6.2 Report on Global Decentralzed Conflct Resoluton Dmos V. Dmarogonas and Kostas J. Kyrakopoulos Control Systems Laboratory,Mechancal Eng. Dept. Natonal Techncal Unversty of Athens,Greece

4 Contents 1 Global Decentralzed Conflct Resoluton Part 1: Holonomc Knematcs The case of Global Sensng Capabltes Decentralzed Navgaton FunctonsDNF s) Control Strategy Constructon of the G functon The f functon Proof of Correctness Input Constrants The Case of Lmted Sensng Capabltes Proof Sketches Proof of Lemma Proof of Lemma Proof of Lemma Proof of Lemma Proof of Lemma Proof of Proposton Smulatons Global Decentralzed Conflct Resoluton Part 2: Nonholonomc Knematcs The case of Global Sensng Capabltes Decentralzed Dpolar Navgaton FunctonsDDNF s) Nonholonomc Control The Case of Lmted Sensng Capabltes Smulatons Global Decentralzed Conflct Resoluton Part 3: Dynamc Models Holonomc Dynamcs Stablty Analyss Nonholonomc Dynamcs Elements from Nonsmooth Analyss Nonholonomc Control and Stablty Analyss

5 3.3 Smulatons Conclusons and Future Research Issues 51 2

6 Introducton Ths s the fnal delverable of HYBRIDGE WP6, summarzng the work held under ths work package on global decentralzed conflct resoluton. It presents the extenson of navgaton functons, whch have been proven a very powerful tool for centralzed navgaton and collson avodance, to decentralzed navgaton, for both the cases where the arcraft dynamcs are consdered holonomc or nonholonomc. The underlyng work s held under the gudance of Task 6.2 of WP6. Decentralzed navgaton approaches are more appealng to centralzed ones, due to ther reduced computatonal complexty and ncreased robustness wth respect to agent falures. The man focus of work n ths doman has been cooperatve and formaton control of multple agents, where so much effort has been devoted to the desgn of systems wth varable degree of autonomy [15],[40],[34], [42]). There have been many dfferent approaches to the decentralzed moton plannng problem. Open loop approaches use game theoretc and optmal control theory to solve the problem takng the constrants of vehcle moton nto account; see for example [2],[13], [21],[18], [6], [41]. On the other hand, closed loop approaches use tools from classcal Lyapunov theory and graph theory to desgn control laws and acheve the convergence of the dstrbuted system to a desred confguraton both n the concept of cooperatve [14], [24], [25], [22]) and formaton control [1], [17], [43], [32], [39]). A few approaches use computer scence based tools to treat the problem;see for example [20], [29], [30]. However, the latter fal to guarantee convergence of the mult-agent system. Closed loop strateges are apparently preferable to open loop ones, manly because they provde robustness wth respect to modellng uncertantes and agent falures and guaranteed convergence to the desred confguratons. However, a common pont of most work n ths area s devoted to the case of pont agents. Although ths allows for varable degree of decentralzaton, t s far from realstc n real world applcatons. For example, n conflct resoluton n Ar Traffc Management, two arcraft are not allowed to approach each other closer than a specfc alert dstance. The constructon of closed loop methods for dstrbuted non-pont mult-agent systems s both evdent and appealng. A closed loop approach for sngle robot navgaton was proposed by Kodtschek and Rmon n ther semnal work [23]. Work under WP6 has extended ths navgaton functons framework to the case of multple non-pont holonomc and nonholonomc agents. 3

7 In Ar Traffc Management Systems, decentralzed conflct detecton and resoluton nvolves reassgnment of the control tasks from the central authorty,.e. the Ar Traffc Controllers, to the agents,.e. the cockpt. The level of decentralzaton depends on the knowledge an agent has on the other agents actons and objectves. In the frst approaches to the problem, the decentralzaton factor led n the fact that each agent/arcraft had knowledge only of ts own desred destnaton, but not of the desred destnatons of the others. In ths delverable the method s extended to take nto account the lmted sensng capabltes of each arcraft. Specfcally, each agent s capable of knowng the postons and/or veloctes only of arcraft wthn ts sensng zone at each tme nstant. The rest of the report s organzed as follows: chapter 1 presents the decentralzed navgaton functons method for multple holonomc agents for the cases of global and lmted sensng capabltes. The counterparts for multple nonholonomc agents are presented n chapter 2. Chapter 3 deals wth dynamc models of vehcle movement. Nontrval computer smulatons are presented throughout the report to ndcate the effectveness of the methodology. 4

8 Chapter 1 Global Decentralzed Conflct Resoluton Part 1: Holonomc Knematcs In ths chapter, we revew the decentralzed conflct resoluton algorthm developed under WP6 for the case when the dynamcs of each arcraft are consdered purely holonomc. We frst present the fundamental approach usng Decentralzed Navgaton Functons DNF s) for agents wth global sensng capabltes. We proceed by showng how ths methodology has been successfully extended to take nto account the lmted sensng capabltes of each agent. A dscusson on handlng velocty constrants s also ncluded. 1.1 The case of Global Sensng Capabltes In ths secton, we consder the case where each agent has global knowledge of the postons of the others at each tme nstant. The decentralzaton factor les n the assumpton that each agent does not need to know the desred destnatons of the others n order to navgate to ts goal confguraton. A provable way to extend ths method to the case of lmted sensng capabltes s presented n the next subsecton. Consder a system of N agents operatng n the same workspace W R 2. Each agent occupes a dsc: R = {q R 2 : q q r } n the workspace where q R 2 s the center of the dsc and r s the radus of the agent. The confguraton space s spanned by q = [q 1,..., q N ] T. The moton of each agent s descrbed by the sngle ntegrator: q = u, N = [1,..., N] 1.1) The desred destnatons of the agents are denoted by the ndex d: q d = [q d1,..., q dn ] T The followng fgure shows a three-agent conflct stuaton: 5

9 q d3 r q 2 2 u 2 q 1 r 1 u 1 u 3 qd 1 q d2 r 3 q 3 Fgure 1.1: A conflct scenaro wth three agents. The mult agent navgaton problem can be stated as follows: Derve a set of control laws one for each agent) that drves the team of agents from any ntal confguraton to a desred goal confguraton avodng, at the same tme, collsons. Each agent has global knowledge of the team confguraton but s unaware of the other agents desred destnatons. In ths secton we make the followng assumptons: Each agent has global knowledge of the poston of the others at each tme nstant. Each agent has knowledge only of ts own desred destnaton but not of the others. We consder sphercal agents. The workspace s bounded and sphercal. Our assumpton regardng the sphercal shape of the agents does not constran the generalty of ths work snce t has been proven that navgaton propertes are nvarant under dffeomorphsms [23]). Arbtrarly shaped agents dffeomorphc to spheres can be taken nto account. Methods for constructng analytc dffeomorphsms are dscussed n [38] for pont agents and n [35] for rgd body agents. The second assumpton makes the problem decentralzed. Clearly, n the centralzed case a central authorty has knowledge of everyones goals and postons at each tme nstant and t coordnates the whole team so that the desred specfcatons destnaton convergence and collson avodance) are fulflled. In the current stuaton no such authorty exsts and we have to deal wth the lmted knowledge of each agent. Ths s of course the frst step towards a varable degree of decentralzaton. The frst assumpton, regardng the global knowledge each agent has about the state space, s overcome n the next secton, where we 6

10 dscuss how the methodology presented n the next subsectons, can be extended to the case of lmted sensng capabltes Decentralzed Navgaton FunctonsDNF s) Prelmnares In ths secton we revew the navgaton functon method ntroduced n the semnal paper of Kodtscheck and Rmon [23] for sngle pont robot navgaton. Navgaton functons NF s) are real valued maps realzed through cost functons ϕq), whose negated gradent feld s attractve towards the goal confguraton and repulsve wrt obstacles. It has been shown by Kodtscheck and Rmon that strct global navgaton.e. the system q = u under a control law of the form u = ϕ admts a globally attractng equlbrum state) s not possble, and a smooth vector feld on any sphere world wth a unque attractor, must have at least as many saddles as obstacles [23]. A navgaton functon can be defned as follows: Defnton 1.1 Let F R 2N be a compact connected analytc manfold wth boundary. A map ϕ : F [0, 1] s a navgaton functon f:1) t s analytc on F, 2) t has only one mnmum at q d ntf ), 3) ts Hessan at all crtcal ponts zero gradent vector feld) s full rank, and 4) lm q ϑf ϕq) = 1. Strctly speakng, the contnuty requrements for the navgaton functons are to be C 2. The frst property of the above defnton follows the ntuton provded by the authors of [23], that s preferable to use closed form mathematcal expressons to encode actuator commands nstead of patchng together closed form expressons on dfferent portons of space, so as to avod branchng and loopng n the control algorthm. Analytc navgaton functons, through ther gradent provde a drect way to calculate the actuator commands, and once constructed they provde a provably correct control algorthm for every envronment that can be dffeomorphcally transformed to a sphere world. In our approach, we further relax ths requrement by usng a non-analytc, merely C 1 navgaton functon. The dscontnuty however, takes place outsde of the regon where crtcal ponts of the potental functon occur, so t does not affect the navgaton propertes of the proposed functon. A functon ϕ that has a unque mnmum on F s called polar. By usng a polar functon on a compact connected manfold wth boundary, all ntal condtons wll ether be brought to a saddle pont or to the unque mnmum of the functon. A scalar valued functon ϕ whose Hessan at all crtcal ponts s full rank s called Morse. The correspondng crtcal ponts are called non-degenerate. The requrement n Defnton 1 that a navgaton functon must be a Morse functon, establshes that the ntal condtons that brng the system to saddle ponts are sets of measure zero [31]. In vew of ths property, all ntal condtons away from sets of measure zero are brought to the unque mnmum. 7

11 The last property of defnton 1.1 guarantees that the resultng vector feld s transverse to the boundary of F. The set F represents the free space of the agent movement,.e. the subset whch s free of collsons. Ths establshes that the system wll be safely brought to q d, avodng collsons. DNF s vs MRNF s In [26], the navgaton functons method has been extended to the case of multple moble robots wth the use of Mult-Robot navgaton functons MRNF s). In the form of a centralzed setup [26], where a central authorty has knowledge of the current postons and desred destnatons of all agents, the sought control law s of the form: u = K ϕq) where K s a gan. In the decentralzed case addressed n ths work, each agent has knowledge of only the current postons of the others, and not of ther desred destnatons. Hence each agent has a dfferent navgaton law. Followng the procedure of [23],[26], we consder the followng class of decentralzed navgaton functonsdnf s): ϕ = σd σ ˆϕ = γ γ + G ) 1/k 1.2) whch s a composton of σ d = x 1/k, σ = x 1+x and the cost functon ˆϕ γ G,where γ 1 0) denotes the desrable set.e. the goal confguraton) and G 1 0) the set that we want to avod.e. collsons wth other agents).a sutable choce s: γ = γ d + f ) k 1.3) where γ d = q q d 2, s the squared metrc of the current agent s confguraton q from ts destnaton q d. The defnton of the functon f wll be gven later. Functon G has as arguments the coordnates of all agents,.e. G = G q), n order to express all possble collsons of agent wth the others. The proposed navgaton functon for agent s ϕ q) = = γ d + f γ d + f ) k + G ) 1/k 1.4) By usng the notaton q = [q1,..., q 1, q +1,..., q N ] T, the decentralzed NF can be rewrtten as ϕ = ϕ q, q ) = ϕ q, t) that s, the potental functon n hand contans a tme-varyng element whch corresponds to the movement n tme of all the other agents apart from. Ths element s neglected n the case of a sngle agent movng n an envronment of statc obstacles [23]), but n ths case the term ϕ t s nonzero. 8

12 1.1.2 Control Strategy The proposed feedback control strategy for agent s defned as where K > 0 a postve gan Constructon of the G functon u = K ϕ q 1.5) In the proposed decentralzed control law, each agent has a dfferent G whch represents ts relatve poston wth all the other agents. In contrast to the centralzed case, n whch a central authorty has global knowledge of the postons and desred destnatons of the whole team and plans a global G functon accordngly, n the decentralzed case, each member of the team has ts own G functon, whch encodes the dfferent proxmty relatons wth the rest. The man dfference of the DNF s and the MRNF s n [26] from the NF s ntroduced n [23] les n the structure of the functon G. Whle there were attempts to prove convergence and collson avodance to the straghtforward extenson of [23] to the multple movng agents case, only collson avodance propertes were establshed. Furthermore smulaton results motvated us to consder a dfferent approach to [26] for the decentralzed setup. We revew now the constructon of the collson functon G for each agent. The Proxmty Functon between agents and j s gven by β j = q q j 2 r + r j ) 2 1.6) Consder now the stuaton n fgure 1.2. There are 5 agents and we proceed to defne the functon G R for agent R. Defnton 1.2 A relaton wth respect to agent R s every possble collson scheme that can occur n a multple agents scene wth respect R. Defnton 1.3 A bnary relaton wth respect to agent R s a relaton between agent R and another. Defnton 1.4 The relaton level n the number of bnary relatons n a relaton. We denote by R j ) l the jth relaton of level-l wth respect to agent R. Wth ths termnology n hand, the collson scheme of fgure 1.2a) s a level-1 relaton one bnary relaton) and that of fgure 1.2b) s a level-3 relaton three bnary relatons), always wth respect to the specfc agent R. We use the notaton R j ) l = {{R, A}, {R, B}, {R, C},...} 9

13 to denote the set of bnary relatons n a relaton wth respect to agent R, where {A, B, C,...} the set of agents that partcpate n the specfc relaton. For example, n fgure 1.2b: where we have set arbtrarly j = 1. R 1 ) 3 = {{R, O 1 }, {R, O 2 }, {R, O 3 }} O 2 O 3 O 4 O 2 O 4 R O 1 O 1 R O 3 a b Fgure 1.2: Part a represents a level-1 relaton and part b a level-3 relaton wrt agent R. The complementary set Rj C) l of relaton j s the set that contans all the relatons of the same level apart from the specfc relaton j. For example n fgure 1.2b: ) R C 1 3 = {R 2) 3, R 3 ) 3, R 4 ) 3 } where R 2 ) 3 = {{R, O 1 }, {R, O 2 }, {R, O 4 }} R 3 ) 3 = {{R, O 1 }, {R, O 3 }, {R, O 4 }} R 4 ) 3 = {{R, O 2 }, {R, O 3 }, {R, O 4 }} A Relaton Proxmty Functon RPF) provdes a measure of the dstance between agent and the other agents nvolved n the relaton. Each relaton has ts own RPF. Let R k denote the k th relaton of level l. The RPF of ths relaton s gven by: b Rk ) l = j R k ) l β {R,j} 1.7) where the notaton j R k ) l s used to denote the agents that partcpate n the specfc relaton of agent R. In the proofs, we also use the smplfed notaton b r = j P r β j for smplcty, where r denotes a relaton and P r denotes the set of agents partcpatng n the specfc relaton wrt agent. For example, n the relaton of fgure 1.2b) we have b R1 ) 3 = m R 1 ) 3 β {R,m} = β {R,O1} + β {R,O2} + β {R,O3} 10

14 A Relaton Verfcaton Functon RVF) s defned by: where λ, h are postve scalars and λb Rk ) l g Rk ) l = b Rk ) l + b Rk ) l + B R C ) 1/h k l B R C ) l = b m ) l k m R C k ) l 1.8) where as prevously defned, R C k ) l s the complementary set of relatons of levell,.e. all the other relatons wth respect to agent that have the same number of bnary relatons wth the relaton R k. Contnung wth the prevous example we could compute, for nstance, B R C1 ) 3 = b R2 ) 3 b R3 ) 3 b R4 ) 3 whch refers to level-3 relatons of agent R. For smplcty we also use the notaton B R C ) l b = m R C k ) b m. The l RVF can be wrtten as g = b + λb b + b 1/h k It s obvous that for the hghest level l = n 1 only one relaton s possble so that Rk C) n 1 = and g Rk ) l = b Rk ) l for l = n 1. The basc property that we demand from RVF s that t assumes the value of zero f a relaton holds, whle no other relatons of the same or other levels hold. In other words t should ndcate whch of all possble relatons holds. We have he followng lmts of RVF usng the smplfed notaton): a) lm lm g b, b ) b 0 b 0 = λ b) lm g b, b ) = 0. These lmts guarantee that b 0 b 0 RVF wll behave n the way we want t to, as an ndcator of a specfc collson. The functon G s now defned as n R l n L G = g Rj ) l 1.9) l=1 j=1 where n L the number of levels and n R l the number of relatons n level-l wth respect to agent. The defnton of the G functon n the multple movng agents stuaton s slghtly dfferent than the one ntroduced by the authors n [23]. The collson scheme n that approach nvolved a sngle movng pont agent n an envronment wth statc obstacles. A collson wth more than one obstacle was therefore mpossble and the obstacle functon was smply the product of the dstances of the agent from each obstacle. In our case however, ths s napproprate, as can be seen n the next fgure. The control law of agent A should dstngush when agent A s n conflct wth B, C, or B and C smultaneously. Mathematcally, the frst two stuatons are level-1 relatons and the thrd a level-2 relaton wth 11

15 B B B A C A C A C I II III Fgure 1.3: I,II are level-1 relatons wth respect to A, whle III s level-2. The RVFs of the level-1 relatons are nonzero n stuaton III. respect to A. Whenever the latter occurs, the RVF of the level-2 relaton tends to zero whle the RVFs of the two separate level-1 relatons A,B and A,C) are nonzero. The key property of an RVF s that t tends to zero only when the correspondng relaton holds. Hence t serves as an analytc swtch that s actvated tends to zero) only when the relaton t represents s realzed. An example As an example, we wll present steps to construct the functon G wth respect to a specfc agent n a team of 4 agents ndexed 1 through 4. We construct the functon G 1 wrt agent 1. We begn by defnng the Relaton Proxmty Functons n every level Table 1): Relaton Level 1 Level 2 Level 3 b 1 b 1 ) 1 = β 12 b 1 ) 2 = β 12 + β 1 ) 3 = β β 13 + β 14 2 b 2 ) 1 = β 13 b 2 ) 2 = β 12 + β 14-3 b 3 ) 1 = β 14 b 3 ) 2 = β 13 + β 14 - Table 1 It s now easy to calculate the Relaton Verfcaton Functons for each relaton based on equaton 1.8). For example, for the second relaton of level 2, the complement term B R C ) l n eq.1.8)) s gven by B k 2 C ) 2 = b 1 ) 2 b 3 ) 2 and substtutng n 1.8), we have g 2 ) 2 = b 2 ) 2 + λ b 2 ) 2 b 2 ) 2 + b 1 ) 2 b 3 ) 2 ) 1/h The functon G 1 s then calculated as the product of the Relaton Verfcaton Functons of all relatons The f functon The key dfference of the decentralzed method wth respect to the centralzed case s that the control law of each agent gnores the destnatons of the others. By usng ϕ = as a navgaton functon for agent, there s no γ d γ d ) k +G ) 1/k 12

16 potental for to cooperate n a possble collson scheme when ts ntal condton concdes wth ts fnal destnaton. In order to overcome ths lmtaton,we add a functon f to γ so that the cost functon ϕ attans postve values n proxmty stuatons even when has already reached ts destnaton. A prelmnary defnton for ths functon was gven n [12], [44]. Here, we modfy the prevous defntons to ensure that the destnaton pont s a non-degenerate local mnmum of ϕ wth mnmum requrements on assumptons. We defne the functon f by: a a j G j f G ) =, G X j=1 1.10) 0, G > X where X, Y = f 0) > 0 are postve parameters the role of whch wll be made clear n the followng. The parameters a j are evaluated so that f s maxmzed when G 0 and mnmzed when G = X. We also requre that f s contnuously dfferentable at X. Therefore we have: a 0 = Y, a 1 = 0, a 2 = 3Y X 2, a 3 = 2Y X 3 The parameter X serves as a sensng parameter that actvates the f functon whenever possble collsons are bound to occur. The only requrement we have for X s that t must be small enough to guarantee that f vanshes whenever the system has reached ts equlbrum,.e. when everyone has reached ts destnaton. In mathematcal terms: X < G q d1,..., q dn ) 1.11) That s the mnmum requrement we have regardng knowledge of the destnatons of the team. The resultng navgaton functon s no longer analytc but merely C 1 at G = X. However, by choosng X large enough, the resultng functon s analytc n a neghborhood of the boundary of the free space so that the characterzaton of ts crtcal ponts can be made by the evaluaton of ts Hessan. Hence, the parameter X must be chosen small enough n order to satsfy 1.11) but large enough to nclude the regon descrbed above. Clearly, ths s a tradeoff the control desgn has to pay n order to acheve decentralzaton. Intutvely, the destnatons should be far enough from one another Proof of Correctness Let ε > 0. Defne B j,l ε) {q : 0 < g R j ) l < ε}. Followng [23],[26] we dscrmnate the followng topologes for the functon ϕ : 1. The destnaton pont: q d 2. The free space boundary: F q) = G 1 δ), δ 0 13

17 3. The set near collsons: F 0 ε) = n L l=1 n R,l j=1 B j,l ε) {q d} 4. The set away from collsons: F 1 ε) = F {q d } F F 0 ε)) The followng theorem allows us to derve results for the functon ϕ by examnng the smpler functon ˆϕ q) = γ G : Theorem 1.1 [23]Let I 1, I 2 be ntervals, ˆϕ : F I 1 and σ : I 1 I 2 be analytc. Defne the composton ϕ : F I 2 to be ϕ = σ ˆϕ. If σ s monotoncally ncreasng on I 1, then the set of crtcal ponts of ϕ and ˆϕ concde and the Morse) ndex of each crtcal pont s dentcal. A key pont n the dscrmnaton between centralzed and decentralzed navgaton functons s that the latter contan a tme-varyng part whch depends on the movement of the other agents. Usng the same procedure as n [23],[26] we frst prove that the constructon of each ϕ guarantees collson avodance: Proposton 1.1 For each fxed t, the functon ϕ q, ) s a navgaton functon f the parameters h, k assume values bgger than a fnte lower bound. Proof Sketch: For the complete proof see [8]. The set of crtcal ponts of ϕ s defned as C ϕ = {q : ϕ / q = 0}. A crtcal pont s non-degenerate f 2 ϕ / 2 q has full rank at that pont.the statement of the proposton s guaranteed by the followng Lemmas: Lemma 1.2 If the workspace s vald, the destnaton pont q d s a non-degenerate local mnmum of ϕ. Lemma 1.3 All crtcal ponts of ϕ are n the nteror of the free space. Lemma 1.4 For every ε > 0, there exsts a postve nteger Nε) such that f k > Nε) then there are no crtcal ponts of ˆϕ n F 1 ε). Lemma 1.5 There exsts an ε 0 > 0 such that ˆϕ has no local mnmum n F 0 ε), as long as ε < ε 0. Lemma 1.6 There exst ε 1 > 0 and h 1 > 0, such that the crtcal ponts of ˆϕ are non-degenerate as long as ε < ε 1 and h > h 1. The complete proofs of the Lemmas can be found n [8]. Sketches of the proofs are found n secton 1.4. Lemmas guarantee the polarty of the proposed DNF, whlst Lemma 1.6 guarantees the non-degeneracy of the crtcal ponts. By choosng k, h that satsfy the above Lemmas, the statement of Proposton 1.1 s proved. Ths however does not guarantee global convergence of the system state to the destnaton confguraton. Ths s acheved by usng a Lyapunov functon for the whole system whch s tme nvarant that s a functon that depends on the postons of all the agents. The canddate Lyapunov functon that we use n ths paper s smply the sum of the DNF s of all agents. Specfcally we prove the followng: 14

18 Proposton 1.7 The tme-dervatve of ϕ = N =1 ϕ s negatve defnte across the trajectores of the system up to a set of ntal condtons of measure zero f the parameters h, k assume values bgger than a fnte lower bound. A rather detaled proof based on matrx calculus be found n [8] whle a proof sketch n secton Input Constrants Handlng of nput constrants can also be ncorporated n the proposed set-up. Suppose that the velocty specfcatons for each agent are gven by: u V max N = [1,..., N] 1.12) It s straghtforward to see that ths s equvalent to K V max ϑϕ /ϑq. Hence, max a small enough K can always be chosen to meet the desred specfcatons. What remans s to show that ϑϕ ϑq max s fnte. Ths s guaranteed by the followng Lemma: Lemma 1.8 The term ϑϕ ϑq admts a fnte upper bound. Proof : Followng the proof of Proposton 1.7 n [8] we have γ ϕ G d G q = + σ γ q q ) 1+1/k ϕ G d G q + σ q γ d + f ) k q + G = )max ) ) 1+1/k max γ d + f ) k + G The denomnator admts a strctly postve lower bound because even when γ d = 0, G 0, f Y whch s strctly postve. For the nomnator we have ) ) γ d G G G + σ G ) q q max q q d max + σ max q max The frst term s always bounded n a bounded workspace whle the second term s also bounded by vrtue of Lemma 2.3 n [8]. Ths establshes the boundedness of ϑϕ ϑq. 1.2 The Case of Lmted Sensng Capabltes In the prevous secton, t was shown how wth a sutable choce of the parameters h, k the proposed control law can satsfy the collson avodance and destnaton convergence propertes n a bounded workspace. The decentralzaton feature of the whole scheme led n the fact that each agent ddn t have knowledge of the desred destnatons of the rest of the team. On the other hand, each one had global knowledge of the postons of the others at each tme nstant. Ths s far from realstc n real world applcatons. max mn 15

19 In ths secton we provde the necessary machnery to take the lmted sensng capabltes of each agent nto account. Specfcally, we alter the defnton of nter-agent proxmty functons n order to cope wth the lmted sensng range of each agent. We consder a bounded workspace wth n agents. Each agent has only local knowledge of the postons of the others at each tme nstant. Specfcally, t only knows the poston of agents whch are n a cyclc neghborhood of specfc radus d C around ts center. Therefore the Proxmty Functon between two agents has to be redefned n ths case. We propose the followng nonsmooth functon: { q q β j = j 2 r + r j ) 2, for q q j d C d 2 C r + r j ) ), for q q j > d C The whole scheme s now modelled as a determnstc) swtched system n whch Proxmty Functon of Agents,j d C Dstance of Agents,j Fgure 1.4: The functon β j for r + r j = 1, d C = 4. swtches occur whenever a agent enters or leaves the neghborhood of another. In [8], we have used ϕ = n =1 ϕ as a Lyapunov functon for the whole system. In ths case ths functon s contnuous everywhere, but nonsmooth whenever a swtchng occurs,.e. whenever q q j = d c for some, j. We defne the swtchng surface as: S = {q :, j, j q q j = d c } 1.14) We have proved that the system converges whenever q / S. On the swtchng surface the Lyapunov functon s no longer smooth so classc stablty theory for smooth systems s no longer adequate. In [7], we prove the valdty of Proposton 2 under the nonsmooth modfcaton of the Proxmty Functons. We make use of tools form nonsmooth stablty theory [5],[36]). It s shown than the nonsmooth alternatve of the navgaton functon does not affect the stablty and convergence propertes of the system. The prescrbed control strategy s another step towards decentralzaton of the navgaton functons methodology. Although each agent must be aware of 16

20 the number of agents n the entre workspace, t only has to know the postons of agents located n ts neghborhood. The next step towards global decentralzaton s to consder the case where each agent s unaware of the global number of agents n the workspace, but only knows what s gong on n ts neghborhood. 1.3 Proof Sketches Before proceedng wth our proof, we ntroduce some smplfcatons concernng termnology. To smplfy notaton we denote by q nstead of q the current agent confguraton, by q d nstead of q d ts goal confguraton, by G nstead of ts G functon and by q j the confguratons of the other agents. In the proof sketches of Lemmas we use the notaton Proof of Lemma 1.2 q ) = ) and 2 ) = 2 ) q 2 At steady state, the functon f vanshes due to the constrant X < G q d1,..., q dn ). Takng the gradent of the defnton of ϕ we have: ϕ q d ) = γ k d + G ) 1/k γd γ d γ k d + G) 1/k γ k d + G ) 2/k = 0 snce both γ d and γ d ) vansh by defnton at q d. The Hessan at q d s whch s non-degenerate. 2 ϕ q d ) = γk d +G)1/k 2 γ d γ d 2 γ k d +G)1/k γ k d +G)2/k = = G 1/k 2 γ d ) = 2G 1/k I Proof of Lemma 1.3 Let q 0 be a pont n ϑf and suppose that g Ra ) b q 0 ) = 0 for some relaton a of level b. If the workspace s vald: g Rj )l q 0) > 0 for any level-l and j a snce only one RVF can hold at a tme. Usng the termnology prevously defned, and settng g g Ra ) b q 0 ) = 0, t follows that ḡ > 0. Takng the gradent of ϕ at q 0, we obtan: ϕ q 0 ) = γ d+f) k +G) 1/k γd +f) γ d +f) γ d +f) k +G) 1/k γ d +f) k +G) 2/k Gq 0)=0 = γ d+f) γ d +f) γ d +f) γ d +f) 1 k γ d+f) 2 k G γ d +f) 2 = = 1 k γ d + f) k G = 1 k γ d + f) k ḡ g 0 q0 17

21 1.3.3 Proof of Lemma 1.4 At a crtcal pont q C ˆϕ F1 ε) we have: ˆϕ = γ G ˆϕ = 1 G 2 G γ γ G) ˆϕ=0 G γ = γ G G γ d + f) k = γ d + f) k G kg γ d + f) = γ d + f) G Takng the magntude of both sdes yelds: kg γ d + f) = γ d + f) G A suffcent condton for the above equalty not to hold s gven by: γ d + f) G G γ d + f) < k, q F 1ε) An upper bound for the left sde s gven by: snce: g Rj )l ε. g Rj ) γ d +f) G G γ d +f) < γ d+f) γ d +f) nl n R,l G j,l G l l=1 j=1 ) n n L R,l max{γ d }+max{f} max grj ) < 1 ε W W W l l=1 j=1 mn γ d +f) = W ) n n L R,l max{γ d }+Y max grj ) = 1 ε W W l l=1 j=1 γ d +f) mn W Proof of Lemma 1.5 < If q F 0 ε) C ˆϕ, where C ˆϕ s the set of crtcal ponts, then q B L ε) for at least one set {L, }, {1...n R,L }, L {1...n L }, wth n L the number of levels and n R,L the number of relatons n level L. We wll use a unt vector as a test drecton to demonstrate that 2 ˆϕ ) q) has at least one negatve egenvalue. At a crtcal pont, Hence, ˆϕ) q) = kg γ d + f) k 1 γ d + f) γ d + f) k G G 2 = 0 The Hessan at a crtcal pont s: k G γ d + f) = γ d + f) G 1.15) 2 ˆϕ ) q) = 1 ) G 2 G 2 γ d + f) k γ d + f) k 2 G 18

22 and expandng 2 ˆϕ ) q) = γ d+f) k 2 { [ G 2 γd + f) 2 γ d + f) + kg k 1) γ d + f) γ d + f) T ] } γ d + f) 2 2 G Takng the outer product of both sdes of equaton A.1), we get: kg) 2 γ d + f) γ d + f) T = γ d + f) 2 G G T 1.16) Substtutng equaton A.2) n equaton A.1), we get: 2 ˆϕ ) q) = γ d + f) k 1 kg 2 γ d + f) + G 2 + ) 1 1 γd +f) k G G GT γ d + f) 2 G We choose the test vector unt magntude) to be:û = bqc) b q c). By ts defnton û s orthogonal to b at a crtcal pont q c, and so the followng propertes hold:û T b = 0 and b T û = 0. Wth 2 γ d + f) = 2 I + 2 f, we form the quadratc form: G 2 û T 2 ˆϕ ) q) û = 2kG + kgû T 2 fû γ d +f) k 1 + ) 1 1 γd +f) k G ût G G T û γ d + f) û T 2 Gû After many nontrval calculaton we get G 2 û T 2 ˆϕ ) q) û = γ d +f) k 1 ) ḡ c 1 + a 0 1 γ d 2 bt γ ) d υ γ d kḡ û T 2 fû + γ d + f) η γ d + f) ψ + z 2 ) +g 3 υ ḡ c j=1 a j g j 1 ḡ j ζ 2γ d 1.17) where c = 1 + λ b + b 1/h,υ = 2 l,l the relaton level, û η = 1 1 ) T ḡ ḡ T û ḡ 2λ ût ḡ k +λ 2 û T b 1/h ḡ ) b 1/h T û c b + b 1/h ) b 1/h T û ) c b 2 + b 1/h 4 ) 2 + ψ = û T 2 ḡ û + ḡ c û T B û 2 ) 2 û T b 1/h ḡ û λ c b + b 1/h 2 B = λ ) b + b ) b 1/h + b 1/h T b + b 1/h 2 b + 2 b1/h ) b + b 1/h 2 19 ) 3 )

23 z 2 g, ḡ, g, ḡ ) = γ d ḡ T γ d + f ḡ T γ d +... kḡ 2 γ T d f f T f ) λḡ ζ = 2c b + b ) 2 b + b 1/h) T γd 1/h Settng: where: µ = 1 + a ) 0 µ γ d µ = 1 2 bt γ d υ γ d equaton A.3) becomes: G 2 û T 2 ˆϕ ) q) û = ḡ γ d +f) k 1 c µ + k ḡ û T 2 f û + γ d + f) ) η γ d + f) ψ g 3 + z2 2γ d υ ḡ c a j g j 1 ḡ j ζ j=1 1.18) The second term s proportonal to g and can be made arbtrarly small by a sutable choce of ε but can stll be postve, so the frst term should be strctly negatve. From the result of Lemma 7 n [8], we have: max {µ } = q F 0 = 2 1 l q j 2 l ) q j 2 + l r + rj ) 2 + ε l lq d q j lq d q j ) For ε small enough, max {µ } s negatve. Moreover, the term 1 + a 0 q F γ d s always 0 greater than one, snce we have assumed that a 0 > 0, and γ d > 0 for q F 0 ε). Thus for ε small enough, µ s also negatve. So, for µ, accordng to Lemma 1, t s suffcent to make sure that: q j 2 l ) q j 2 + l r + rj ) 2 + ε < 1 l < l q d q j ε < l l q d q j 2 + q j 2 1 l q j 2 r + r j ) 2 ε 0 An other constrant arses from the fact that ε > 0.. So for a vald workspace t wll be: l l q d 2 q j + qj 2 1 l 2 q j > r + rj ) 2 20

24 1.3.5 Proof of Lemma 1.6 From the proof of the prevous Lemma, we have at a crtcal pont We also have and At a crtcal pont: G 2 2 ˆϕ ) = kg 2 γ γ d +f) k 1 d + f) + ) 1 1 γd +f k G G GT γ d + f) 2 G 3 f = j=1 ja j G j 1 } {{ } σg) 2 f = σ 2 G + σ G G T, σ = G 3 jj 1)a j G j 2 j=2 kg γ d + f) = γ d + f) G kg γ d = γ d + f) G kg f kg γ d = γ d + f kgσg)) G γ d + f G γ d = GσG) G } k {{ } σ Takng the magntude from both sdes we have 2kG = k σ 2 2Gγ d G 2 Choosng ũ = b as a test drecton and after some manpulaton we have G 2 ũ T 2 ˆϕ ) ũ = σ 2 G 2 kγ d +f) k 1 2Gγ d ξũ T G G T ũ }{{} M + σ ũ T 2 Gũ }{{} N } {{ } L + where ξ = 1 1 ) γd + Y k kg + 3 j=2 { kjj 1) )} aj k k Gj 1 21

25 After some manpulaton, we have { g 2 L + M + N σ 2 ḡ 2 + ḡ 2 g 2 2Gγ d 2G ḡ g 2 ) ũ T g ũ σ 2 ) ũt ) +2 γ d + ξg + σ g ḡ ũ) +ξḡ 2 ũt ) 2 g + σ ũ ) T g 2 ḡ + ḡ 2 g u } But g 2 ũ T g ) ũ 2 = g 2 so that so that g 2 ḡ 2 + ḡ 2 g 2 2G ḡ g 2 ũ T g ) ũ = g ḡ ḡ g ) 2 ) L + M + N 2 σ 2 ũt ) γ d + ξg + σ g ḡ ũ) +ξḡ 2 ũt ) 2 g + σ ũ ) T g 2 ḡ + ḡ 2 g u It s shown n [8] that the second term, whch s strctly postve, domnates the thrd and the frst term for suffcently small ε Proof of Proposton 1.7 In the proof sketch of Proposton 1.7, the terms ), 2 ) have ther usual meanng and refer to the whole state space and not a sngle agent, namely [ ) = T [ ] q 1 ),..., q N )] and 2 ) = 2 q j ). We mmedately note that the followng proof s exstental rather than computatonal. We show that a fnte k that renders the system almost everywhere asymptotcally stable exsts, but we do not provde an analytcal expresson for ths lower bound. However, practcal values of k have been provded n the smulaton secton. Let us recall that the Proxmty functon between agents and j s gven by: β j q) = q q j 2 r + r j ) 2 = q T D j q r + r j ) 2 where the 2N 2N matrx D j s defned n [26]: D j = O 2 1) 2N O 2 2 1) I 2 2 O 2 2j 1) I 2 2 O 2 2N j) O 2j 1) 2N O 2 2 1) I 2 2 O 2 2j 1) I 2 2 O 2 2N j) O 2N j) 2N We can also wrte b r = q T Prq r + r j ) 2,where Pr = D j, and P r j P r j P r denotes the set of bnary relatons n relaton r. It can easly be seen that 22

26 b r = 2P rq, 2 b r = 2P r. We also use the followng notaton for the r-th relaton wrt agent : g r = b r + b r = s S r s r λb r b r + b r) 1/h, b r = t S r t s,r b t }{{} b s,r 2P sq s S r s r b s, where S r denotes the set of relatons n the same level wth relaton r. An easy calculaton shows that [ gr =... = 2 d rpr wr P ] r q = Q rq, P r = b s,r Ps s S r s r where d r = the G functon s gven by: We defne G = b r λ ) b r + b r )1/h b r + N G = gr G = r=1 q = G 1. G N = b r )1/h, w r = N N r=1 l=1 l r Q 1. Q N g l }{{} g r q = Qq λb r b r ) h 1 1. The gradent of hb r + b r )1/h ) 2 N gr = g rq rq = Q q r=1 γ d +f ϕ Rememberng that u = K q and that ϕ =, f γ d +f ) k +G ) 1/k = the closed loop dynamcs of the system are gven by: { } K 1 A 1+1/k) γ 1 G d1 G 1 q 1 + σ 1 1 q 1. K N A 1+1/k) N = A K G γ d ) A K ΣQq { γ G dn G N q N + σ N N q N } =... 3 j=0 a G j [ ] T where γ d ) = γd1 q 1... γ dn q N, σ = G σg ) γ d+f k, σg ) = 3 ja j G j 1,A = j=1 γ d + f ) k + G and the matrces G = dag G 1, G 1,..., G N, G N ) }{{} 2N 2N 23

27 By usng ϕ = ) K A K = dag 1 A 1+1/k) 1, K 1 A 1+1/k) 1,..., K N A 1+1/k) N, K N A 1+1/k) N }{{} Σ = Σ 1 }{{} 2N 2N 2N 2N,..., Σ N }{{} 2N 2N, } {{ } 2N 2N 2 Σ = dag 0, 0,..., σ, σ }{{},..., 0, 0 2 1,2 ϕ as a canddate Lyapunov functon we have ϕ = { } ϕ ϕ = ϕ ) T q, ϕ = A 1+1/k) {G γ d + σ G } and after some trval calculaton ϕ ) T =... = γ d ) T A G + q T Q T A Σ where So we have ) G A G = dag 1 A 1+1/k) 1, G 1 A 1+1/k) 1,..., G N A 1+1/k) N, G N A 1+1/k) N }{{} A Σ = A Σ1 }{{} 2N 2N.. A ΣN }{{} 2N 2N } {{ } 2N 2 2N 2N 2N, A Σ = dag A 1+1/k) A 1+1/k) σ σ,..., ) } {{ } 2N 2N { } ϕ = ϕ ) T q =... = [ ] [ ] [ ] = γ d ) T q T M 1 M 2 γd M 3 M 4 q }{{} M where M 1 = A G A K G, M 2 = A G A K ΣQ, M 3 = Q T A Σ A K G, M 4 = Q T A Σ A K ΣQ. In [8], we provde an analytc expresson for the elements of the matrx Q. We examne the postve defnteness of the matrx M by use of the followng theorems: 24

28 Theorem 1.9 [19]: Gven a matrx A R n n then all ts egenvalues le n the unon of n dscs: n n n z : z a a j = R A) = RA) =1 =1 j=1 j Each of these dscs s called a Gersgorn dsc of A. Corollary 1.10 [19]: Gven a matrx A R n n and n postve real numbers p 1,..., p n then all ts egenvalues of A le n the unon of n dscs: n =1 z : z a 1 p n j=1 j p j a j A key pont of Corollary 1.10 s that f we bound the frst n/2 Gersgorn dscs of a matrx A suffcently away from zero, then an approprate choce of the numbers p 1,..., p n renders the remanng n/2 dscs suffcently close to the correspondng dagonal elements. Hence, by ensurng the postve defnteness of the egenvalues of the matrx M correspondng to the frst n/2 rows, then we can render the remanng ones suffcently close to the correspondng dagonal elements. Ths fact wll be made clearer n the analyss that follows. Some useful bounds are obtaned by the followng lemma: Lemma 1.11 : The followng bounds hold for the terms Q, Qj, σ and Proof : See [8]. σ ε) Y 1 k + ) 8 9 γ d Y 1 k + ) 8 9 γ d k, Y k γ d }{{ k } k, γ d k σ 0) }{{} σ X) 0 < Q < Q max < 0 < Q j Q < j < max, 0 ε ε, X ε ε Let us examne the Gersgorn dscs of the frst half rows of the matrx M. We denote ths procedure as M 1 M 2, as the man dagonal elements of M 1 are compared wth the correspondng raw elements of M 2. Note that the 25

29 submatrces M 1, M 2 are both dagonal, therefore the only nonzero elements of raw of the 4N 4N matrx M are the elements M, M,2N+ where of course 1 2N as we calculate the Gersgorn dscs of the frst half rows of the matrx M. We have: z M 1 p p j M j, 1 2N j z A 21+1/k) K G 2 p 2N+ A 21+1/k) σ K G Q z A 21+1/k) K G 2 p 2N+ p We examne the followng three cases: p A 21+1/k) σ K G Q G < ε At a crtcal pont n ths regon, the correspondng egenvalue tends to zero, so that the dervatve of the Lyapunov functon could acheve zero values. However, the result of Lemma 1.6 ndcates that ϕ s a Morse functon, hence ts crtcal ponts are solated [23]. Thus the set of ntal condtons that lead to saddle ponts are sets of measure zero[31]. G > X The correspondng egenvalue s guaranteed to be postve as long as: z > 0 A 21+1/k) K G p 2N+ p σ Q ) > 0 G X > p 2N+ p σ Q = γ d p 2N+ k p Q k > γ d) max X Q p 2N+ p max 0 < ε G X z > 0 ε > { Y 1 k γ d } p 2N+ Q max k p { { 2 max Y k ε > 2 max, } } 8Y 9, γ p d) max 2N+ k p Q max { } Y Θ 1 Θ k 2 1 k > 2 max ε, 16Θ 1 9ε, Q max γ d ) max ε p 2N+ p A key pont s that there s no restrcton on how to select the terms p 2N+ p. Ths wll help us n dervng bounds that guarantee the postve defnteness of the matrx M. Let us examne the Gersgorn dscs of the second half rows of the matrx M. Lkewse, we denote ths procedure as M 3 M 4. The dscs of Corollary 1.10 are evaluated: z M p j p M j, 2N + 1 4N, 1 j 4N j z M 4 ) R M 3 ) + R M 4 ) where M 4 ) = j K A 1+1/k) A 1+1/k) j σ j σ Q Q j 26

30 and R M 3 ) = 2N j=1 j=1 = 2N p j p R M 4 ) = l p j p M3 ) j = A 1+1/k) l 4N j=2n+1 j σ l A 1+1/k) j p j p M4 ) j = K j G j Q l j = p j p A l A j ) 1+1/k) σ l σ j K j Q l j Qj jj j l A suffcent condton for the postve defnteness of the correspondng egenvalue for raw s then: M 4 ) > R M 3 ) + R M 4 ) M 4 ) > max {2R M 3 ), 2R M 4 )} We frst show that we always have R M 3 ) R M 4 ). By takng nto account the relatons Q jk = Q kj = 0, Q j = Q jj, j k j and expandng t s easy to see that { } 2N R M 3 ) = 1 A 21+1/k) j σ p p j K j G j Q j + j j=1 A j A ) 1+1/k) = σ K j G j Q jj A 21+1/k) j σ j K j G j Q j + = 2N }{{} p j I) p j=1 A j A ) 1+1/k) σ K j G j Q jj j }{{} II) 2 p σ K G Q p A 21+1/k) where wthout loss of generalty we choose p = p, 2N + 1 4N.We also have A 21+1/k) j σj 2 K j Q j R M 4 ) = Qj jj + }{{} I) A j A j ) 1+1/k) σ σ j K j Q jjq j jj }{{} II) 27

31 By comparng the terms I) and II) n the last two equatons we have: I) : p j p A 21+1/k) j σ j K j G j Q j A 21+1/k) j σj 2K jq j ) Qj jj pj p σ jg j σj 2Qj jj σ j σ j Q j jj + pj p G j 0 σ j <0 σ j Q j jj + pj p G j 0 II) : pj p A ja ) 1+1/k) σ K j G j Q jj A A j ) 1+1/k) σ σ j K j Q jj Qj jj ) pj p σ G j σ σ j Q j jj σ σ j Q j jj + pj p G j 0 σ <0 σ j Q j jj + p j p G j 0 Thus, the condton σ j Q j jj + pj p G j 0 guarantees that R M 3 ) R M 4 ). Hence t suffces to show that M 4 ) > 2R M 3 ). The fact that σ j Q j jj + p j p G j 0 s a drect concluson of the results of procedure M 1 M 2. For example, by the last bound on k we have: { k > 2 max 2 Y Θ 1 k G j ε G j > 2 max G j > { Y 1 }, γ dj) max p Q j ε p j jj max { 2 max { Y k, } } 8Y γ 9, dj ) max k p Q j p j + γ dj } p Q j k p j jj max jj σ j Q j jj + p j p G j > 0 max Θ 1 ε, 16Θ 1 9ε k p j p G j > σ j max Q j jj max The fact that M 4 ) > 0 s guaranteed by Lemma 5.4. Ths lemma also guarantees that there s always a fnte upper bound on the terms M 3 ) j = A 1+1/k) l σ l A 1+1/k) j K j G j Q l j l We have p j p j=1 } M3 {p j ) j M 4 ) > 2R M 3 ) = 2 2N 4N p > M 4) max, j 2N + 1 4N, 1 j 2N M 3 ) j 1.4 Smulatons To demonstrate the navgaton propertes of our decentralzed approach, we present two smulatons of multple holonomc agents that have to navgate from an ntal to a fnal confguraton, avodng collson wth each other and 28

32 satsfyng velocty bounds. The chosen confguratons consttute non-trval setups snce the straght-lne paths connectng ntal and fnal postons of each agent are obstructed by other agents. The frst smulaton nvolves 8 holonomc agents wth global sensng and the second four agents wth local sensng capabltes. Fgure 1.5: 8 holonomc agents 29

33 Pc.1 Pc.2 T4 A3 T2 T1 A1 T3 A2 A4 Pc.3 Pc.4 Pc.5 Pc.6 Fgure 1.6: 4 holomonc agents wth lmted sensng capabltes 30

34 Chapter 2 Global Decentralzed Conflct Resoluton Part 2: Nonholonomc Knematcs In ths chapter, we revew the decentralzed conflct resoluton algorthm developed under WP6 for the case when the dynamcs of each arcraft are consdered nonholonomc. Nonholonomc constrants cannot be wrtten as an algebrac constrant n the confguraton space. When the constrants are explctely ntegrable, then they can take the form of an algebrac constrant. Hence one can relate the words holonomc and nonholonomc to ntegrable and non-ntegrable respectvely. We frst present the fundamental approach usng Decentralzed Navgaton Functons DNF s) for agents wth global sensng capabltes. We proceed by showng how ths methodology has been successfully extended to take nto account the lmted sensng capabltes of each agent. A dscusson on handlng velocty constrants s made n the end of ths chapter. 2.1 The case of Global Sensng Capabltes In ths secton, we consder the case where each agent has global knowledge of the postons and veloctes of the others at each tme nstant. The decentralzaton factor les n the assumpton that each agent does not need to know the desred destnatons of the others n order to navgate to ts goal confguraton. The means to extend ths method to the case of lmted sensng capabltes s presented n the next subsecton. Consder the followng system of N nonholonomc vehcles: ẋ = u cos θ ẏ = u sn θ θ = ω 2.1) 31

35 wth {1... N}. x, y, θ ) are the poston and orentaton of each robot, u and w are the translatonal and rotatonal veloctes respectvely. The problem can be now stated as follows: Gven the N nonholonomc systems, derve a control law that steers every system from any feasble ntal confguraton to ts goal confguraton avodng collsons. The control law must be decentralzed n the sense that each system has no knowledge of the targets of other systems. In ths secton we make the followng assumptons: Each agent has global knowledge of the poston and velocty of the others at each tme nstant. Agents have no nformaton about other agents targets. Around the target of each agent A there s a regon called the agent s A safe regon Agent s A safe regon s only accessble by agent A, whle regarded as an obstacle by other agents Decentralzed Dpolar Navgaton FunctonsDDNF s) In ths secton, we show how the DNF s of the prevous secton have been redefned n [27] n order to provde trajectores sutable for nonholonomc navgaton. Ths s accomplshed by a enhancng a dpolar structure [37] to the navgaton functons. Dpolar potental felds have been proven a very effectve tool for stablzaton [38] of nonholonomc systems as well as for centralzed coordnaton of multple agents wth nonholonomc constrants [28]. The key advantage of ths class of potental felds s that they drve the controlled agent to ts destnaton wth desred orentaton. The navgaton functon of the prevous secton s modfed n the followng manner n order to be able to produce a dpolar potental feld: ϕ = γ d γ k d + H nh G b t ) 1/k 2.2) where b t = j q q dj 2 ε + r ) 2 ). The term ε > 0 s the radus of the safe regon of ts agent. H nh has the form of a pseudo-obstacle and s defned as H nh = ε nh + η nh wth ε nh > 0, η nh = q q d ) n d 2 and n d = [cosθ d ), snθ d )] T. Moreover γ d = q q d 2,.e. the headng angle s not ncorporated n the dstance to the destnaton metrc. The next fgure shows a 2D dpolar navgaton functon. An mportant feature that should be notced s the fact that ths navgaton functon does not have to nclude the f functon as each agent treats the other agents targets as statc obstacles. 32

36 Fgure 2.1: A dpolar potental feld Nonholonomc Control Thus far we have establshed that the dpolar functon ϕ has navgaton propertes. We consder convergence of the mult-agent system as a two-stage process: In the frst stage agents converge to a ball of radus ε called safe regon, contanng the desred destnaton of each agent. Each agent can get n ts own safe regon but not n others. The safe regon of one agent s regarded as an obstacle from the other agents. Once an agent gets n ts own safe regon, t remans n the set and asymptotcally converges to the orgn. Before defnng the control we need some prelmnary defntons: We defne by 2 ϕ q 2 q, t) = 2 ϕ q, t) the Hessan of ϕ. Let λ mn, λ max be the mnmum and maxmum egenvalues of the Hessan and ˆυ λmn, ˆυ λmax the unt egenvectors correspondng to the mnmum and maxmum egenvalues of the Hessan. Snce navgaton functons are Morse functons [31], ther Hessan at crtcal ponts s never degenerate,.e. ther egenvalues have always nonzero values. As dscussed before,ϕ s a dpolar navgaton functon. The flows of the dpolar navgaton feld provde feasble drectons for nonholonomc navgaton. What we need now s to extract ths nformaton from the dpolar functon. To ths extend we defne the nonholonomc angle : θ nh = { arg ϕ x s + ϕ, P 1 arg d s υλ x mn + υ y ) λ mn, P1 y s ) where condton P 1 s used to dentfy sets of ponts that contan measure zero sets whose postve lmt sets are saddle ponts: where ε 1 < P 1 = λ mn < 0) λ max > 0) ˆυ λmn ϕ < ε 1 ) mn ϕ C) ), s = sgnq q d ) η d ) C={q : q q d =ε} 33

37 d = sgnυ λmn ϕ ), η d = [cosθ d ) snθ d ) ] T η = [cosθ ) snθ ) ] T Before proceedng we need the followng: Lemma 2.1 If ˆυ λmn ϕ = 0 then P 1 conssts of the measure zero set of ntal condtons that lead to saddle ponts. For a proof of ths lemma the reader s referred to [27]. In vew of Lemma 2.1, ε 1 can be chosen to be arbtrarly small so the sets defned by P 1 eventually consst of thn sets contanng sets of ntal condtons that lead to saddle ponts. The followng provdes a sutable nonholonomc controller for the frst stage: Proposton 2.2 The system under the control law ) u = sgn ϕ x cos θ + ϕ y sn θ )) ϕ K u K z + c / t ϕ ϕ x cos θ + ϕ y sn θ tanh x cos θ + ϕ 2 y sn θ ω = θ ϕ nh + θ nh θ ) K θ + c / t tanh θ 2θ nh θ ) 2 nh θ 3)) converges to the set B = {p : q q d ε δ, θ π, π]} 2.3) up to a set of measure zero of ntal condtons where 0 < δ < ε. Here K z = ϕ 2 + q q d 2, K u, K θ are postve constants, c > ε 2+1 ε 2 where ε 2 = 2π 3 ε 2 1 4ε π 2) 3/ and. ϕ t = { ϕ cos θ j + ϕ ) } sn θ j u j x j y j j Proof[27]:We form the followng Lyapunov functon: V = ϕ x, y, t) + θ nh x, y, t) θ ) 2 and take t s tme dervatve: V = ϕ t + u η ϕ + +2 θ nh θ ) w + θ nh t + u η θ ) nh After substtutng the control law u and w, we get: V = ϕ t ϕ η ϕ K z + c / t ϕ η tanh )) ϕ η 2 2 θ nh θ ) 2 ϕ K θ + c / t tanh θ 2θ nh θ ) 2 nh θ 3)) ϕ ϕ t c ) t tanh ϕ η 2 + tanh θ nh θ 3)) 2.4) 34

38 Snce the set P 1 s by constructon repulsve for ε 1 suffcently small, we only need to consder the set P 1. Then: ϕ η 2 = ϕ 2 cos 2 θ nh θ ). Let θ = θ nh θ. After substtutng we get: V ϕ t c ϕ ) t tanh ϕ 2 cos 2 θ) + tanh θ 3)) Before proceedng we need the followng: Lemma 2.3 The followng nequaltes hold: 1. tanh x) x x+1, x 0 x 2. x+1 + y y+1 x+y x+y+1, x, y 0 3. cos 2 θ 8 θ π π ) θ [ ] 0, π 2 Proof : 1. For x 0 we have that e 2x 1 2x 0. Hence x + 1) e x e x ) x e x + e x ) and we get the result: tanh x) x x+1. The equalty holds at x = 0. x+1 + y y+1 = 2xy+x+y xy+x+y+1 xy+x+y xy+x+y+1 x+y x+y+1 x 2. Wth x, y 0 we have : and the equalty holds at x = y = 0 3. Denote A θ) = cos 2 θ and B θ) = 8 π θ π ) Solvng A θ) = B θ), for θ [ ] 0, π 2 we get θ = 0 for A = B = 1 and θ = π 2 for A = B = 0. But at A θ θ=0 = 0 > 6 π = B θ and snce A and B θ=0 have no other ntersecton for θ [ ] 0, π 2 t follows that A θ) B θ), for θ [ ] 0, π 2. By use of Lemma we get: V ϕ t ϕ / t By use of Lemma we get: V ϕ t c ϕ 2 cos 2 θ + c θ 3 ϕ 2 cos 2 θ+1 θ 3 +1 ϕ ) / ϕ 2 cos 2 θ+ θ 3 t c ϕ 2 cos 2 θ+ θ 3 +1 and from Lemma we get: V ϕ t ϕ / ϕ 2 8 t c π 3 θ π 2 ). 3 + θ 3 +1 In vew of the fact that the functon has the same extremal ponts wth fx) fx)+1 π 3 θ π 2 ) 3 + θ 3 ϕ 2 8 f x) 0 see [23] for a proof), the mnmum of [ ϕ 2 8 π 3 θ π 2 ) 3 + θ 3 ϕ 2 8 π 3 θ π 2 ) 3 + θ 3 +1 concdes wth the mnmum of m = ϕ 2 8 π θ π ) θ 3. Tryng to mnmze m, we get: m ϕ = 16 π ϕ 3 θ π ) whch means that m s strctly ncreasng n the drecton of ϕ. Examnng m θ = 3 θ π ϕ 3 2 ) θ π 2 ) 2 sgn θ π 2 and requrng m θ = 0 ). 35

39 for an extremum n the drecton of θ, we get: 2 ϕ π 4 θ = ϕ 3 / ± 2 π 2 2 ϕ π 4 ϕ 3 / ± 2 π 2 2 ϕ π θ π / 2 θ > π / 2 The only feasble soluton s: θ = 4 ϕ + 3. Substtutng the soluton / 2 π 2 n m we get: mn m) = 2 ϕ 2 3 π θ 4 ϕ 3 ) 2. Mnmzng the last we get: / + 2 π 2 mnm) θ ϕ = / ϕ π 2 4 ϕ 3 ) 3 0. Actvatng the constrant ϕ ε 1 we / + 2 π 2 2ε 2 1 π3 get: ε 2 = mn m) = 4ε 3 ) 2. Substtutng n the tme dervatve of / 1+ 2 π 2 the Lyapunov functon, we have that: V ϕ t ϕ / t c ε 2 ε 2 +1, so choosng c > ε 2+1 ε 2 we get that V ϕ ) ) / t sgn ϕ / t k 0 snce The equalty holds when ε 2 k = c ε > 1 ) q = q d ) ϕ / t = 0 We assume that the system s ntal condtons are n the set W \S where the set S = { p : ϕ < ε 1 }. ε1 can be chosen to be arbtrarly small such that the set S ncludes arbtrarly small regons only around the saddle ponts and the target. Snce we are consderng convergence to the set B, we have that V < 0, q W free \ { B { q : ϕ q ) < ε 1 }}, where the bar denotes the set nternal. For the second stage each agent s solated from the rest of the system. The dpolar navgaton functon for ths case becomes: ϕ nt x, y, θ ) = γ d,θ γ kd,θ + H nh β nt ) 1/k 2.5) 36

40 where β nt = ε 2 q q d 2, and γ d,θ = q q d 2 + θ θ d ) 2. Defne and = K θ ϕ nt / θ θ nh θ ) K u K z ϕ nt η θ nh ϕnt = arg s + ϕ ) nt s x y Then for each arcraft n solaton we have the followng: Proposton 2.4 Each subsystem under the control law u = sgn ϕnt x ω = K θ θ nh θ ), < 0 ω = K θ ϕ nt θ, 0 cos θ + ϕ nt y sn θ ) K u K z 2.6) converges to p d Proof : Takng V = ϕ nt tme dervatve: as a Lyapunov functon canddate, we have for the V = ẋ ϕ nt = u ) ϕ nt η + w ϕ nt/ y. We can now dscrmnate two cases, dependng on the level of : 1. < 0. Then V = K u K z ϕ nt η + Kθ θ nh θ ) ϕ nt / y = < Then V = K u K z ϕ nt η K θ ϕnt/ y ) 2 0, wth the equalty holdng only at the orgn. The fact that each agent remans n ts safe regon after the frst stage s establshed by the followng lemma whch s a drect applcaton of the propertes of the navgaton functon: Lemma 2.5 For each subsystem under the control law 2.6)the set s postve nvarant. B nt = {p : q q d ε, θ π, π]} Proof : The boundary of 2.5) s the set B nt = {p : β nt q ) = 0} = {p : q q d = ε} = B nt,.e. the workspace boundary, whch s postve nvarant for a navgaton functon [23],[8]. 37

41 2.2 The Case of Lmted Sensng Capabltes In the prevous secton, we presented the nonholonomc control scheme for multple agents wth global sensng capabltes. In ths secton we modfy ths n order to cope wth the lmted sensng range of each agent. It s obvous that each agent takes nto account the other agents only on the frst stage. The nter-agent proxmty functons are modfed accordng to 1.13). However each agent has also only local knowledge of the veloctes of the rest of the team. Therefore the term ϕ t ϕ t = j: q q d d C must be modfed accordng to: { ϕ x j cos θ j + ϕ y j sn θ j ) u j } 2.7) where d C s agan the radus of the sensng zone of each agent. Hence each agent has to take nto account only the postons and veloctes of agents that are wthn each sensng zone at each tme nstant. Ths modfcaton of the control law 2.3) does not affect the stablty results of the prevous secton as the nodes of the determnstc swtched system admt a common Lyapunov functon. Usng arguments from establshed results on stablty for hybrd systems[3],[33]) the convergence n the frst stage s guaranteed for each agent n ths case as well. The nterested reader can refer to [10] for more detals. 2.3 Smulatons To demonstrate the navgaton propertes of our decentralzed approach, we present three smulatons of four nonholonomc agents that have to navgate from an ntal to a fnal confguraton, avodng collsons. The chosen confguratons consttute non-trval setups snce the straght-lne paths connectng ntal and fnal postons of each agent are obstructed by other agents. The followng sequence of fgures verfes the collson avodance and global convergence propertes of our algorthm. In each fgure the crcles denote the targets of each agent whle the rng around each target represents the correspondng transton guard where the transton from the frst to the second stage takes place. 38

42 0.3 Bnt epslon regon of agent s target 0.2 transton guard Fgure 2.2: 4 nonholonomc agents Fgure 2.3: 4 nonholonomc agents 39

43 Chapter 3 Global Decentralzed Conflct Resoluton Part 3: Dynamc Models The mathematcal models of the movng vehcles/agents n the prevous chapters were consdered purely knematc. In practce however, real mechancal systems are controlled through ther acceleraton. It s therefore evdent that second order models are consdered as well n the navgaton functons approach. The next two sectons present the extenson of the DNF s approach of the prevous paragraphs to the cases of dynamc models for holonomc and nonholonomc systems, respectvely. 3.1 Holonomc Dynamcs In ths secton, we present the decentralzed control scheme for a mult-agent system wth double ntegrator dynamcs. The followng dscusson s based on [9]. We consder the followng system of n agents wth double ntegrator dynamcs: q = v v = u, {1,..., N} 3.1) We wll show that the system s asymptotcally stablzed under the control law ϕ u = K + θ v, ϕ ) g v 3.2) q t where K, g > 0 are postve gans, θ v, ϕ ) cv = t tanh v 2) ϕ t 40

44 and ϕ t = ϕ q j q j j The frst term of equaton 3.2) corresponds to the potental feld decentralzed navgaton functon) descrbed n chapter 1. The second term explots the knowledge each agent has of the veloctes of the others, and s desgned to guarantee convergence of the whole team to the desred confguratons. The last term serves as a dampng element that ensures convergence to the destnaton pont by suppressng oscllatory moton around t. By usng the notaton x = [ ] x T 1,..., x T T N, x T = [ ] q T v T the closed loop dynamcs of the system can be rewrtten as ẋ = ξx) = [ ξ T 1 x),..., ξ T Nx) ] T 3.3) wth ξ x) = [ K ϕ q v cv ϕ tanh v 2 ) t ] g v We wll use the functon V = K ϕ v 2 as a canddate Lyapunov functon to show that the agents converge to ther destnatons ponts. We wll check the stablty of the mult-agent system wth LaSalle s Invarance Prncple Stablty Analyss In the followng we prove the followng theorem: Theorem 3.1 The system 4) s asymptotcally stablzed to [ qd T 0 ],q d = [q d1,..., q dn ] T up to a set of ntal condtons of measure zero f the exponent k assumes values bgger than a fnte lower bound and c > max K ). Proof: The canddate Lyapunov Functon we use s V = K ϕ v 2 and by takng ts dervatve we have V = K ϕ v 2 V = K ϕ + v T v = ) K ϕ t + v T ϕ q + ) ) v T ϕ K q + θ v, ϕ t g v V = ) ϕ K t + v T θ v, ϕ t g v 2) 41

45 ) Usng the notaton B = ϕ K t + vt θ v, ϕ t we frst show that c > max K ): B 0 f Of course, K ϕ t ϕ t > 0 : tanh v c > max K ) c > K 2 ) v ) 2 K > c v 2 tanh v 2 ) sgn ϕ t ϕ K + v T θ t v, ϕ t ) < 0 : ϕ t > 0 ϕ t < 0 : tanh v c > 0 c > K 2 ) v 2 ) K > c v 2 tanh v 2 ) sgn ϕ t ) ϕ K t + v T θ v, ϕ t < 0 : ϕ t < 0 ) + v T θ v, ϕ t we used the fact that 0 tanhx) x holdng only when ϕ t = 0. We have V = = 0 for ϕ t 1 x 0. So we have B = 0. In the precedng equatons g v 2 0 B 0 wth equalty Hence, by LaSalle s Invarance Prncple, the state of the system converges to the largest nvarant set contaned n the set { ) } S = q, v : ϕ t = 0 v = 0) = = {q, v : v = 0) } { ) } because by defnton the set q, v : ϕ t = 0 s contaned n the set {q, v : v = 0) }. For ths subset to be nvarant we need v = 0 ϕ q = 0 The analyss of chapter 1 revealed that ths stuaton occurs whenever the potental functons ether reach the destnaton or a saddle pont. By boundng the parameters k, h from below by a fnte number, ϕ becomes a navgaton functon, hence ts crtcal ponts are solated [23]). Thus the set of ntal condtons that lead to saddle ponts are sets of measure zero [31]). Hence the largest nvarant set contaned n the set ϕ q = 0 s smply q d 3.2 Nonholonomc Dynamcs In chapter 2, we presented the decentralzed navgaton functons methodology for multple agents wth nonholonomc knematcs. Although each agent had no 42

46 specfc knowledge about the destnatons of the others, t treated a sphercal regon around the target of each agent as a statc obstacle. In ths secton we modfy the proposed control law n order to allow each agent to neglect the destnatons of the others. Furthermore, the control nputs are the acceleraton and rotatonal velocty of each vehcle, copng n ths way wth realstc classes of mechancal systems. The followng dscusson s based on [11]. We consder the followng system of n nonholonomc agents wth the followng dynamcs ẋ = v cos θ ẏ = v sn θ, {1,..., N} 3.4) θ = ω v = u where v, ω are the translatonal and rotatonal veloctes of agent respectvely, and u ts acceleraton. The problem we treat n ths paper can be now stated as follows: Gven the N nonholonomc agents 3.4),consder the rotatonal velocty ω and the acceleraton u as control nputs for each agent and derve a control law that steers every agent from any feasble ntal confguraton to ts goal confguraton avodng, at the same, collsons. We make the followng assumptons: Each agent has global knowledge of the poston of the others at each tme nstant. Each agent has knowledge only of ts own desred destnaton but not of the others. We consder sphercal agents. The workspace s bounded and sphercal Elements from Nonsmooth Analyss In ths secton, we revew some elements from nonsmooth analyss and Lyapunov theory for nonsmooth systems that we use n the stablty analyss of the next secton. We consder the vector dfferental equaton wth dscontnuous rght-hand sde: ẋ = fx) 3.5) where f : R n R n s measurable and essentally locally bounded. Defnton 3.1 [16]: In the case when n s fnte, the vector functon x.) s called a soluton of 3.5) n [t 0, t 1 ] f t s absolutely contnuous on [t 0, t 1 ] and there exsts N f R n, µn f ) = 0 such that for all N R n, µn) = 0 and for almost all t [t 0, t 1 ] ẋ K[f]x) co{ lm x x fx ) x / N f N} 43

47 The above defnton along wth the assumpton that f s measurable guarantees the unqueness of solutons of 3.5) [16]. Lyapunov stablty theorems have been extended for nonsmooth systems n [36],[4]. The authors use the concept of generalzed gradent whch for the case of fnte-dmensonal spaces s gven by the followng defnton: Defnton 3.2 [5]: Let V : R n R be a locally Lpschtz functon. The generalzed gradent of V at x s gven by V x) = co{ lm x x V x ) x / Ω V } where Ω V s the set of ponts n R n where V fals to be dfferentable. Lyapunov theorems for nonsmooth systems requre the energy functon to be regular. Regularty s based on the concept of generalzed dervatve whch was defned by Clarke as follows: Defnton 3.3 [5]: Let f be Lpschtz near x and v be a vector n R n. The generalzed drectonal dervatve of f at x n the drecton v s defned Regularty of a functon s defned: f 0 fy + tv) fy) x; v) = lm sup y x t 0 t Defnton 3.4 [5]: The functon f : R n R s called regular f 1) v, the usual one-sded drectonal dervatve f x; v)exsts and 2) v, f x; v) = f 0 x; v) The followng chan rule provdes a calculus for the tme dervatve of the energy functon n the nonsmooth case: Theorem 3.2 [36]: Let x be a Flppov soluton to ẋ = fx) on an nterval contanng t and V : R n R be a Lpschtz and regular functon. Then V xt)) s absolutely contnuous, d/dt)v xt)) exsts almost everywhere and d dt V xt)) Ṽ a.e. x) := ξ V xt)) ξ T K[f]xt)) We shall use the followng nonsmooth verson of LaSalle s nvarance prncple to prove the convergence of the prescrbed system: Theorem 3.3 [36] Let Ω be a compact set such that every Flppov soluton to the autonomous system ẋ = fx), x0) = xt 0 ) startng n Ω s unque and remans n Ω for all t t 0. Let V : Ω R be a tme ndependent regular functon such that v 0 v Ṽ f Ṽ s the empty set then ths s trvally satsfed). Defne S = {x Ω 0 Ṽ }. Then every trajectory n Ω converges to the largest nvarant set,m, n the closure of S. 44

48 3.2.2 Nonholonomc Control and Stablty Analyss We wll show that the system s asymptotcally stablzed under the control law v u = v { ϕ η + M } g v tanh v ) K v K z ω = K θ θ θ d θ nh ) + θ 3.6) nh where K v, K θ, g > 0 are postve gans, θ nh = arg ϕ x s + ϕ y s ) s = sgnq q d ) η d ) η = [ cos θ sn θ ] T η d = [ cos θ d sn θ d ] T K z = ϕ 2 + q q d 2 M > j ϕ j η max ϕ j = [ ϕj In partcular, we prove the followng theorem: x ϕ j y ] Theorem 3.4 Under the control law 3.6), the system s asymptotcally stablzed to p d = [p d1,..., p dn ] T. Proof: Let us frst consder the case v > 0. We use V = V, V = ϕ + v θ θ d θ nh ) 2 as a Lyapunov functon canddate. For v > 0 we have V = V = { v j j ϕ ) η j + sgnv ) v + j + θ θ d θ nh ) θ θ nh ) } and substtutng V = { } v j j ϕ ) η j v ϕ ) η + M ) j v tanh v K ) v K z g v K θ θ θ d θ nh ) 2 45

49 The frst term of the rght hand sde of the last equaton can be rewrtten as { } v j j ϕ ) η j v ϕ ) η + M ) = j = { } v ϕ ) η + v ϕ j ) η j 0 v ϕ ) η + M ) so that V K v K z g v K θ θ θ d θ nh ) 2 x where the nequalty tanh x 1 for x 0. The canddate Lyapunov functon s nonsmooth whenever v = 0 for some. The generalzed gradent of V s gven by 1 ϕ. N ϕ v 1. V = v N 1 2 θ 1 θ 1 θ d1 θ nh1 ) θ N θ N θ dn θ nhn ) θ nh1 θ 1 θ d1 θ nh1 ) θ nhn θ N θ dn θ nhn ) 2 46

50 and the Flppov set of the closed loop system by K [f] = v 1 cos θ 1 v 1 sn θ 1. v N cos θ N v N sn θ N u 1. u N ω 1. ω N θ nh1. θ nhn = v 1 cos θ 1 v 1 sn θ 1. v N cos θ N v N sn θ N K [u 1 ]. K [u N ] ω 1. ω N θ nh1. θ nhn We denote by the dscontnuty surface and D = {x : {1,... N} s.t.v = 0} D S = { {1,... N} s.t.v = 0} the set of ndces of agents that partcpate n D. We then have Ṽ = ξ T K [f] = ξ V ) ) v 1 1 ϕ η v N N ϕ η N + ξ T K [u 1 ] ξ T K [u N ] ξ v 1 ξ v N + θ θ d θ nh ) ω θ ) nh Ṽ = { ) } v ϕ j η + sgn v ) u / D S + ξ T K [u ] K θ θ θ d θ nh ) 2 ξ v D S For D S we have v v =0 = [ 1, 1] and K [u ] v =0 = [ K vk z, K v K z ] so that ξ v ξ T K [u ] = 0 47

51 From the prevous analyss we also derve that { ) } v ϕ j η + sgn v ) u / D S {K v K z + g v } / D S Gong back to Theorem 3.4 t s easy to see that v 0 v Ṽ. Each functon V s regular as the sum of regular functons [36]) and V s regular for the same reason. The level sets of V are compact so we can apply ths theorem. We have that S = {x 0 Ṽ } = {x : v = 0 ) θ θ d = θ nh )}. The trajectory of the system converges to the largest nvarant subset of S. For ths subset to be nvarant we must have v = 0 K v K z = 0 ϕ = 0) q = q d ) For ϕ = 0 we have θ nh = 0 so that θ = θ d. 3.3 Smulatons The navgaton propertes of the proposed control scheme are verfed n the dynamc case as well through the followng non-trval smulatons nvolvng four holonomc and nonholonomc agents respectvely. The smulatons of dynamc models of the next two fgures have ther own mportance as they deal wth mathematcal models of real world applcatons, such as arcraft and mechancal systems. The smulaton of fgure 3.2 s more closely related to realstc arcraft movement than the smulaton of the knematc nonholonomc model of fgures 2.3,2.4 both from the modellng dynamc model) as well as from the curvature constrants vewpont. A formal proof of the last dervaton s a topc of ongong research. 48

52 Fgure 3.1: 4 dynamc holonomc agents 49