Potential Fields in Cooperative Motion Control and Formations

Transcription

1 Pepaed by F.L. Lews and E. Stngu Updated: Satuday, Febuay 0, 03 Potental Felds n Coopeatve Moton Contol and Fomatons Add dscusson. Refe to efs.. Potental Felds Equaton Chapte Secton The potental s a scala feld whose negatve gadent s a vecto feld of consevatve foces. Consde a plana feld of play havng some tagets and some obstacles. A sample -D playng feld s shown n Fgue.-. The tagets o goals ae endowed wth attactve potentals Fg..-. Sample -D playng feld fo vehcle moton contol and the obstacles wth epulsve potentals. Fom the potentals, one computes the assocated foces. At any pont ( x, y) n the plane, the sum of foces at that pont gves a net foce poduced by all the tagets and all the obstacles. The net foce as a functon of poston s a vecto foce feld that epesents the esultant foce obtaned by summng the effects of the attacton foce of the taget and the epulson foces of the obstacles n F( ) F ( ) F ( ) V ( ) (..) t ag et obs whee aows denotes vectos. In potental feld moton contol, t s mpotant to eep tac of foces, the components, and the dectons. Fo moble gound obot applcatons, the potental T felds ae usually used n a -dmensonal space. Then x y R and the foce gadent s T V V T V( ) Fx F y R x y wth x and y components (..)

2 V( x, y) Fx ( x, y) x V( x, y) Fy ( x, y) y (..3) The taectoy of moton of a gound obot s geneated contnuously fo any poston ( x, y) of the obot by followng the decton of the steepest descent n the potental feld, snce that s the decton of the net foce at that pont. Ths can be consdeed an optmzaton poblem that seaches fo the pont of mnmum potental. Some sample potental felds and the foces they geneate ae gven now. Suppose t s desed to each a taget located at ( x, y ) and the cuent vehcle poston s ( x( t), y( t )). A lnea attactve potental s V() (..4) / whee the dstance to the taget s [( xx) ( y y) ]. Then by the chan ule the gadent foce geneated s V / ( x x) Fx [( xx) ( yy) ] ( xx) (..5) x V / ( y y) Fy [( xx) ( yy) ] ( yy) (..6) y Note that ( x x ) ( ) x, y e e y y (..7) ae the components of a unt vecto and so the attactve foce s constant ndependent of the dstance to the taget. Ths means that the attactve effects of the taget have an unbounded ange of nfluence. Suppose, on the othe hand, that t s desed to avod an obstacle located at ( x, y ) and the cuent vehcle poston s ( x( t), y( t )). A epulsve potental s V() (..8) / whee the dstance to the obstacle s [( xx) ( y y) ]. Then by the chan ule the gadent foce geneated s V / ( x x) Fx ( ) [( xx) ( yy) ] ( xx) x x (..9) V / ( y y) Fy ( ) [( xx) ( yy) ] ( yy) (..0) y y Note that ( x x ) ( ) x, y e e y y (..) ae the components of a unt vecto and so the foce follows a epulsve squae law. Ths s

3 smla to the electostatc foce o gavtatonal foce and means that the epulsve effects of the obstacles have a lmted ange of nfluence. Sample potental felds ae shown n Fgue.-. Shown s an envonment on the squae [0,]x[0,] n the ( x, y) plane wth a goal o taget at ( x, y G G ) (0,0) and obstacles at x, y ) (3,3), x, y ) (7,). ( ( Fg..-. Potental Felds fo taget and two obstacles.. Potental Felds fo Vehcle Contol Equaton Secton (Next) Consde the model of a ca-le vehcle. Ths s a nonholonomc system, so t can only tun wth a fnte tun adus. Thus, t may not be able to follow the deal taectoy esultng fom the potental feld. Howeve, the potental feld mplctly povdes a feedbac mechansm. That s, the foce F s geneated fo any poston of the obot n such a way that t wll etun close to the deal taectoy. Vehcle Dynamcs The obot vehcle shown n Fgue.- has the dynamcs x vt cos cos y vt cos sn (..) vt sn L wth ( x, y) the poston, the headng angle, v T the fowad wheel speed, L the wheel base, and the steeng angle. In ths model, the contol nput s the steeng angle (t). A smplfed model of a vehcle s gven by x vt cos (..) y v sn T y L v T Fgue.-. Model of a ca-le obot In ths model, the contol nput s the headng angle (t). Ethe model can be used fo smulaton dependng on the degee of accuacy to eal lfe wanted. v 3 x

4 Geneatng the Potental Felds Consde the envonment on the squae [0,]x[0,] n the ( x, y) plane shown n Fgue.- wth a goal o taget at ( x, y G G ) (0,0) and obstacles at ( x, ) (3,3 y ), ( x, y ) (7,). The potental felds fo moton towads the goal whle avodng the two obstacles ae geneated as follows. The taget potental feld (..4) yelds a constant foce ndependent of dstance that attacts the obot to the taget. The attactve foce when the vehcle s at poston ( x, y) s defned n tems of the ange to the taget G ( xg x) ( yg y) (..3) as xg x yg y FGx ( x, y) KG, FGy ( x, y) KG (..4) G G Computng the obstacle foces based on the potental feld (..8) yelds the epulsve squae law foces x x F ( x, y) K (..5) whee x y y F ( x, y) K (..6) y ( x x) ( y y) s the dstance between the obot and the -th obstacle. Thee ae many othe methods fo computng potental foces fo epulson and attacton. Fo example, assumng the obstacles ae ccula, the foce fo the -th obstacle can be taen as x x y y Fx ( x, y) K, Fy ( x, y) K, (..7) max ( a ), b max ( a ), b whee a s the adus of the -th obstacle and b lmts the elatve heght of the -th obstacle potental feld. The total foce that acts on the obot s Fx FGx Fx Fx (..8) Fy FGy Fy Fy The angle of the foce ndcates the desed decton of moton of the vehcle. The angle of the foce as seen fom the obot s F y tan (..9) Fx Feedbac Contol System fo Potental Feld Moton 4

5 The angle of the summed foces s ntepeted as a desed headng angle des fo the vehcle. To follow ths angle one can desgn the feedbac contol system shown n Fgue.-. The Fg..-. Feedbac system fo vehcle moton contol contol nput to the vehcle s the steeng angle () t. The speed v T can be ept constant, o educed as one appoaches nea an obstacle. A smulaton s now 4000 pefomed. The ntal poston 3000 fo the obot s ( x 0, y0 ) (0,0) 000 Goal and the ntal headng angle s ad. The speed v T s ept 6 0 constant. The steeng angle s geneated by the contolle based 8 Stat on the dffeence between the 6 headng angle and the angle y 4 of the esultant foce actng on the 0 obot (Fg. ). In ths example, a popotonal contolle s x mplemented: Fgue.-3. The taectoy of the obot usng potental felds K( ) The esultng moton s shown n Fgue.-3, along wth the potental felds geneated. In ths smulaton, the potental felds coespondng to (..7) ae used. V Poblems wth Potental Feld Methods It can be seen fom the potental feld epesentaton that the obot wll follow the decton of the steepest descent, whch s the decton of the esultant foce. Thee ae a few ssues that can pevent the obot to each the taget. The sum of foces s equal to zeo at any pont ( x, y) whee the net potental has a local mnmum. At these ponts, the vehcle wll stop. One stuaton whee ths can occu s f thee ae obstacles spaced too close togethe. The vehcle wll attempt to pass between the obstacles due to the attactve foce towads the taget, but wll not be able to pass between the obstacles. Such poblems can be solved by addng a dthe to the foce whch esults n small andom motons that can help get out of local mnma. Altenatvely, one can modfy the heght o 5

6 dmenson of the obstacle potental felds, o change the type of foce used to epesent them, o use an ntellgent contolle n the oute loops of contolle shown n Fgue.-. Anothe poblem ases wth nonholonomc vehcles. These vehcles have a fnte tunng adus and cannot mae abtaly tght tuns. If the vehcle msses the taget by a small amount, t wll ty to tun towads the taget. If the attactve foce geneates a desed headng decton that cannot be eached wth the vehcle s tun adus, the vehcle wll ccle the taget and neve each t..3 The Gaph Laplacan Potental and Gadent-Based Coopeatve Contol Equaton Secton (Next) In ths secton we study a potental functon that s natually assocated wth a gaph, the Gaph Laplacan Potental, and show how to use t to geneate local coopeatve contol potocols. Gaph Laplacan Potental Consde a dynamc gaph G ( V, A) wth N nodes v V, the state of -th node gven by x, and connectvty matx A [ a ]. The n-degee s the -th ow sum of A d a. The gaph Laplacan s L D A wth D dag[ d ]. Defne x [ x x ] T N, wth x ( t ) a state assocated wth each node. The gaph Laplacan potental VG a( x x) (.3.), s a measue of the enegy stoed n the gaph. Clealy VG a( x x) 0. (.3.), The Laplacan potental s useful n coopeatve contol on any gaph, ncludng geneal dgaphs. The next esult shows that t taes a specal fom fo undected gaphs. Lemma.3-. Let the gaph be undected. Then T VG a( x x) x Lx (.3.3) Poof: G,, V a ( x x ) x a( x x) x a( x x) Fst tem on the ght s x a ( x x ) x a x a x Intechange ndex vaables n fst tem hee to get x a ( x x ) x a a x x 6

7 Howeve, the -th ow sum f the gaph s undected. Theefoe, and a (n-degee) s equal to the -th column sum a x a ( x x ) x a a x x x a ( x x ) ( ) T G, V a x x x d x a x x Lx (out-degee) Ths esult shows that fo undected gaphs the Laplacan matx s postve sem-defnte L 0. Fo geneal dgaphs t s not possble to expess the gaph potental V G n tems of the Laplacan matx, and the defnton of V G must be modfed. Ths ssue s studed n a subsequent chapte. The next execse shows that fo balanced gaphs the potental as defned n (.3.) also has a specal fom n tems of the Laplacan matx. Execse.3-. Laplacan Potental fo Balanced Gaphs. The -th ow sum a (ndegee) s equal to the -th column sum a (out-degee) f and only f the gaph s balanced. Show that fo balanced gaphs T T VG a( x x) x ( LL ) x (.3.4), Ths poves that fo balanced gaphs ( L L T ) s postve semdefnte. Gadent-Based Contol fo Fst-Ode Scala Dynamcs Consde a geneal dected gaph wth agent moton n -D along the lne and the agent dynamcs x u (.3.5) wth x, u R. Select the contol nput gven by the gadent of the Laplacan potental as VG ( x) u a( x x) a( x x) (.3.6) x N Ths s the standad local votng potocol we have analyzed at length. Note that sums ove can be taen as sums ove the neghbo set N of snce only fo these values of ae a 0. In tems of the global contol u [ u u ] T N N R and state one has u Lx (.3.7) x Lx (.3.8) It s nteestng to note that the global dynamcs of the agents s gven n tems of the Laplacan matx fo any gaph even though, fo geneal dgaphs, the dentty (.3.3) does not hold. Fst-Ode Moton Dynamcs n the -D Plane 7

8 Consde now moton n the ( x, y) wth velocty contol dynamcs (.3.5) and states x [ ] T p q R whee ( p( t), q( t )) s the poston of node n the ( x, y) -plane. Now the Laplacan potental s V a ( x x ) T ( x x ) a whee the dstance between nodes and s (.3.9) G,, ( p p ) ( q q ) (.3.0) Compute the gadents to get the foces n the x and y dectons as V G a a ( p p ) p p N (.3.) V G a a ( q q ) (.3.) q q N The gadent-based contol now gves the x and y velocty nputs VG up a ( p p ) (.3.3) p N VG uq a ( q q ) (.3.4) q N That s, fo -D moton systems (.3.5) one taes ndependent standad local votng potocols n each component of the moton x and y. The x and y moton potocols may be combned to wte VG u a ( x x ) (.3.5) x N whee x, u R. Note that ths stuaton of fst-ode moton dynamcs n the -D ( x, y) -plane s not the same as second-ode consensus consdeed n Chapte. Second-ode consensus taes the dynamcs fo coupled systems that satsfy Newton s law x u, whch yelds the second-ode node dynamcs x v (.3.6) v u wth poston x R, velocty v R, and acceleaton nput u R. The standad local votng potocol s a gadent-based contol usng the gaph Laplace potental (.3.). We have seen n pevous chaptes that ths potocol guaantees consensus f the gaph has a spannng tee. Othe local potocols can be deved usng dffeent potental functons, as ndcated by the next execse. Execse.3-. Local Potocol Based on a Lnea Potental Functon: Fnte-Tme Consensus. Consde plana moton gven by the velocty contol dynamcs (.3.5) wth x [ p q ] T R whee ( p ( t), q ( t )) s the poston of node n the ( x, y) -plane. Defne the 8

9 lnea potental V G a (.3.7), whee the dstance between nodes and s gven by (.3.0). Show that the gadent-based local potocol s V ( x x) ( x x) G u a a (.3.8) x N N x x Does ths potocol guaantee consensus? Ague that ths law guaantees convegence to consensus n fnte tme egadless of the gaph topology, as long as the gaph has a spannng tee. We shall dscuss fnte-tme consensus n a late chapte. Note that vehcle dynamcs fo moton n n dmensons ae actually of second ode n the Newton s law fom x v (.3.9) v u n n n wth vecto poston x R, speed v R, and acceleaton nput u R. Potental feld methods can also be used to deve the acceleaton contols fo such systems..4 Potental Feld Analyss of Collectve Moton Equaton Secton (Next) Many eseaches have used potental felds to analyze the collectve moton of flocs, heds, swams, fomatons and othe anmal and human goups. Gaz and Passno Gaz and Passno [003, 004] consdeed swams of N agents wth the ntegato dynamcs wth contol gven by x gx ( x), x n R. (.4.) Ths amounts to communcaton ove a fully connected undected gaph. Functon g(.) s the gadent of a potental functon consstng of a epulsve pat and an attactve pat, wth a unque value whee g( ) 0. The example used was y g( y) yabexp, (.4.) c 9

10 whch s equal to zeo fo cln( b/ a), epulsve fo y, and attactve fo y. Ths potental functon s shown n Fgue.4-. The followng esults ae shown: Defne the cente of the swam as N x N x. Then the cente s statonay, x 0. Let an agent satsfy x x,,.e. t expeences puely attactve foces. Then ts moton s towads the cente. The poof uses the Lyapunov functon ( ) T V ( ) x x x x. As tme passes, the agents wll convege to a hypeball xx, wth Fg..4-. Potental feld functon. Coutesy of (Gaz and Passno 003). b c adus a exp( / ). Ths s ndependent of the numbe of agents N, so densty nceases wth N. The moton conveges wth tme to x 0,.e. all agents come to elatve est. The poof s based on the Lyapunov potental feld functon V( x) a x x bcexp M M x x. (.4.3) c Then the gadent gves foce and hence the closed-loop system. Defne the local potental functon V x a x x bc M x x ( ) exp. (.4.4) c Then each agent mnmzes ts local potental functon, and the global mnmum s acheved. If each agent has fnte body sze of adus, then the swam sze s bounded below by n N. Now the sze depends on the numbe of agents. The densty s now bounded above by /. Gaz [005] extends the above ntegato dynamcs esults to vehcles wth Lagangan dynamcs M x f u by defnng the sldng manfold s x x V( x) 0 wth V(x) a potental functon. On the sldng manfold one has x V( x) as above. x Leonad and Foell In [Leonad and Foell 00] potental feld contol of poston/speed dynamcs p v, v u 0

11 s consdeed. Defne a potental functon V( ) wth the dstance fom node to node. Defne the gadent f ( ) V( ). Contol s taen as f u f (.4.5) v wth the last tem a dampng tem, e.g. fv av o fv ap. Ths assumes a fully connected undected gaph communcaton topology. cos Defne pola coodnates sn. Then the closed loop system (wthout dampng) s f cos( ) ( ) (.4.6) f ( ) sn( ) (.4.7) The last equaton s vey smla to the Kuamoto oscllatos [Stogatz, Kuamoto]. Flocng s consdeed. Also consdeed s schoolng, whee a desed velocty s to be followed by the swam. Then, f av ( v) wth v d the desed velocty. v d Olfat-Sabe and Muay [Olfat-Sabe and Muay 004] consde the moton dynamcs p v, v u and gaph theoy s used. A floc s defned as a wealy connected dgaph, o a connected undected gaph. Defne the c.g. as p ave( p) p and the elatve poston as p p p. Then the tanslatonal dynamcs ae p v, v u and the elatve dynamcs ae p v, v u. A sepaaton pncple s shown. The zeo egenvalue of the Laplacan matx coesponds to the absolute moton of the c.g. The flocng potocol used s o ( ) ( ) f p p d u p p c v v (.4.8) d N p p u s( p p) cd( v v) (.4.9) N wth s the edge stess weghts, d the desed sepaaton, and c d the dampng. The foce f s the gadent of a potental functon. Dang and Lews [006] used potental feld methods fo dynamc localzaton n weless senso netwos. They defned a Lyapunov functon smla to (.3.9) n tems of an eo potental, and too the gadent-based contol to update estmates n an obseve fo estmaton

12 of the postons of nodes n a netwo..5 Potental Feld Contol of Nonholonomc Vehcles Equaton Secton (Next) Uncycle Vehcles In [Ln, Fancs, Maggoe 005] uncycle agents wee consdeed x vcos y vsn o z ve wth z x y. The contol s taen as v ( z z ) N cost (.5.) (.5.) (.5.3) n tems of the dot poduct, wth z [ ] T x y and the Fenet-Seet vecto tangent to the taectoy. Then the closed-loop dynamcs s z H( ( t)) LI z wth H ( ( t)) dag{ T } and the Konece matx poduct. It was shown that the fomaton stablzes to a pont (.e. all vehcles convege to the same value of z ) fo all ntal ( x, y, ) f and only f the gaph has a eachable node. They defne gaph edge aows bacwads, so that agent obtans nfomaton fom agent f thee s an edge fom to. Theefoe, n ou temnology, a eachable node coesponds to exstence of a node connected to all othe nodes,.e. a spannng tee. It was shown that the fomaton stablzes to a lne f and only f thee ae at most two dsont closed sets of nodes n the contol dgaph. Nonlnea Fomaton Contol Law. In [Justh and Kshnapasad 00] ae consdeed nonholonomc vehcle dynamcs n nematc Fenet-Seet fom x x yu (.5.4) y xu wth the poston and x,y othogonal vectos specfyng the oentaton. The contol law s

13 u ( ). x. y f( ). y ( ) x. y (.5.5) wth and usng the dot poduct. It s shown usng Lyapunov methods that the system conveges to the equlbum ponts. In the contol, the last tem foces a common oentaton. Usng ths tem only yelds u N x. y. Defne x exp( ). Then * y exp( ), and ths becomes u sn( ). Ths s the Kuamoto oscllato [Stogatz, Kuamoto], and s the gadent system wth espect to the enegy functon Voent cos( ). The functon f(.) allows one to acheve a desed sepaaton, fo t s shown that equlbum ponts have f ( ) 0., 3

14 Refeences P. Dang, F.L. Lews, and D.O. Popa, Dynamc Localzaton of A-Gound Weless Senso Netwos, Poc. Medteanean Conf. Contol and Automaton pape WM-, Ancona, June 006. Gaz V., Passno K.M., Stablty analyss of swams, IEEE Tans. Automatc Contol, 48(4), (003). Gaz V., Passno K.M., Stablty Analyss of Socal Foagng Swams, IEEE Tans. on Systems, Man, and Cybenetcs-Pat B: Cybenetcs, Vol. 34, No.. Feb. pp , 004. [Justh and Kshnapasad 00] N.E. Leonad and E. Foell, Vtual leades, atfcal potentals, and coodnated contol of goups, Poc. IEEE Conf. Decson and Contol, pp , Dec. 00. [Ln, Fancs, Maggoe 005] R. Olfat-Sabe and R.M. Muay, Consensus poblems n netwos of agents wth swtchng topology and tme-delays, IEEE Tans. Automatc Contol, vol. 49, no. 9, pp , Sept Y. Kuamoto, Chemcal Oscllatons, Waves, and Tubulence, Spnge, Beln, 984. E. Stngu and F.L. Lews, Neuo Fuzzy Contol of Autonomous Robotcs, n Encyclopeda of Complexty and Systems Scence, ed. R.A. Meyes, Spnge, Beln, 009. S.H. Stogatz, Fom Kuamoto to Cawfod: explong the onset of synchonzaton n populatons of coupled oscllatos, Physca D, vol. 43, pp. -0,