Dynamic State Feedback Control of Robotic Formation System

Transcription

1 so he 00 EEE/RSJ ntenatonal Confeence on ntellgent Robots and Systems Octobe 8-, 00, ape, awan Dynamc State Feedback Contol of Robotc Fomaton System Chh-Fu Chang, Membe, EEE and L-Chen Fu, Fellow, EEE Abstact hs pape poposes a constuctve contol appoach, dynamc state feedback fomaton contol, fo achevng the ealzaton of the Mult-Robot Fomaton System (MRFS) wth espect to the poblem of dlaton of a fomaton shape and stablzaton ssue n nonholonomc system smultaneously. Combnng wth theoetcal analyss, the poposed contol appoach has been successfully to deal wth the followng desgn ssues: one s to elude the swtch contol of the nonholonomc MRFS fo peventng the dvegence of the MRFS; anothe one s to stablze the MRFS whch s allowed to change the nteconnecton stuctue dynamcally. ndex ems Fomaton Contol, Dynamc Feedback Contol, Mult-Robotc System, Stablzaton. A. NRODUCON wdely studed desgn concen about the MRFS s ts nteconnecton stablty, whch s defned as the capablty to ceate o mantan the desed fomaton shape whle pefomng fomaton contol. Anothe concen, followng the nteconnecton stablty ssue of the MRFS, s how to desgn a fomaton contol n tems of the nonholonomc subsystem, whch seems to have become moe hghly egaded than befoe. heefoe we have to nvestgate the followng desgn poblems:. Fnd the stablty ctea among subsystem stablty, nteconnecton stablty and fomaton system stablty.. Stablze the MRFS wth espect to nonholonomc subsystems n tems of lmted senso/communcaton capablty. Fo the second statement of the poblem, t patculaly epesents that the global nonholonomc path plannng appoach cannot be appled. Numeous eseaches have concened the nonholonomc subsystem and nteconnecton stuctue of the MRFS. A feedback lneazaton contol appoach fo the nonholonomc MRFS wth Leade-Followe s poposed n [] and late, n [], the autho consdes seveal fomaton stateges wth dffeent hs wok was suppoted n pat by the Natonal Scence Councl, awan, R.O.C. unde Gant NSC95-75-E PAE. Chh-Fu Chang s wth the Electcal Engneeng Depatment, Natonal awan Unvesty, (phone: ext. 05; e-mal: L-Chen Fu s wth the Compute Scence nfomaton Engneeng and Electcal Engneeng Depatment, Natonal awan Unvesty, (phone: ; e-mal: lchen@ntu.edu.tw). nteconnecton stuctue appled n ndoo envonment. Wth ths contol stuctue, Das et al.[3] pactcally desgn a vson-based system. he swtchng between decentalzed contolles that allows fo changes n fomaton wth obstacle avodance of the MRFS. Lawton et al.[4] conduct behavo-based fomaton contol wth espect to dynamcs athe than knematcs of a WMR. n addton, fo dealng wth the measuement uncetanty, an obseve based nonlnea Lyapunov based output feedback contol s poposed n [5]. Recently, J et al.[6] focus on the connectedness ssue usng gaph Laplacan n the multobot coodnaton poblem. Also, a constaned foced based contol appoach s poposed by Zou et al.[7] wth espect the dynamcs of a WMR. Ren and Soesen n [8] study the nteconnecton ssues of the MRFS wth espect to the physcal lmtaton of the communcaton bandwdth and the stablty of the nteconnecton stuctue as well as consensus poblem n communcaton system. Do[9] uses the backsteppng contol scheme to contol the educed ode dynamcs of a WMR. hs eseach, based on the exstng eseaches, ntends to go futhe to nvestgate the stablty and contol ssues of the MRFS. Wth ths aspect, seveal desgn challenges shall be ovecome: nteconnecton stablty, subsystem stablty and nonholonomc constants. he fome can be ecast as the gdty condton of a popely defned gaph [6, 0]. Combnng wth the subsystem stablty condton, one can yeld t as a consensus poblem[] wth espect to the communcaton ssue. hs pape s oganzed as follows: ntally, n Secton, the theoetcal analyss of the MRFS s poposed. hen, the stablty analyss of the fomaton system s defned and analyzed n Secton. he dynamc state feedback contol of the MRFS s obtaned n Secton V. n Secton V, an expement of the MRFS s examned. Fnally, the conclusons ae made n Secton V.. HEORECAL ANALYSS n ths secton, the geneal model of a MRFS s ntally establshed. n addton to smplfyng the poblem, we tempoaly assume that the gdty condton of the nteconnecton stuctue of the MRFS s satsfed. Late the algebac dffeental topology of the nonholonomc MRFS s obtaned and the esult s sgnfcantly helps us to undestand the dffeental stuctue of the nonholonomc MRFS /0/$ EEE 3574

2 A. Geneal Fomula of MRFS Consde an MRFS wth the fomaton state: z = { z ;, n} whee n denotes the numbe of WMRs n the fomaton team, along wth and beng the sub-ndces whch epesent the th connecton of the th WMR. Followng the defnton of the fomaton vaables n [7], the component fom of the fomaton states defned n the Eucldean space of the global coodnate can be geneally descbed as: l qp q p z = Q ϕ. () qθ qθ k k Hee, q = qp q θ and q = qp q θ whee qp N and qp N denote the poston state of the WMR wth N and N beng the subspace of the base space B wth N, N B, q θ and q θ denote the oentaton state of the m WMR, and Q denotes the abstacted space wth k m. Addtonally, the followng popety s satsfed: N N B fo all,. Followng the defnton, z s denoted as the sum of the th connecton to the th WMR z = z fo all. Followng the defnton, f thee exsts a smooth and dffeentable map h: N Q, N = N fo all, wth espect to the nteconnected stuctue Ω of the MRFS, we say h s a dffeomophsm when k = m and smlaly, we say h s a suecton when k > m such that z = z z h q Ω wth [ n ] (, ) kn n [ ] ; q = q q n dmenson of the WMR contol. Suppose that a vtual cente of the MRFS moves along a desed taectoy c() t n two-dmensonal space so that kn kn Ω wth beng the the MRFS s dven fom an ntal state q c( t ) state qcf c( tf ) = to a fnal c0 0 = whee t 0 and t f denote the ntal and fnal tme, espectvely. he contol goal, thus, s to move the MRFS to follow c() t n addton to mantanng the gven nteconnecton stuctue (esp. fomaton shape) smultaneously. heefoe, ntal confguatons of the MRFS eque the desed nteconnecton stuctue Ω d, the desed fomaton state z d, and a vtual fomaton cente qc, whch s located nsde the closed egon of the fomaton shape. he knematcs of the th WMR fo all can be geneally egaded q = g q u wth as a dftless affne contol system =,, n o we can geneally wte q = g ( q ) u wth [ ] m and g g g u. Geneally, consde a MRFS: qt = f qt, ut. (a) () ( () ()) z = h( q, Ω ). (b) kn n n kn whee f : s a smooth functon. We fomally pose the poblem of stablzng the fomaton state z by means of state feedback. he desed states of the subsystems ae to deve natually va fowadng geometcal constants such that the set of the desed state { q,, d q } d of n the subsystems s well defned f the ntal confguatons of the MRFS ae gven,.e., qd = qc0 + ldc [ cosϕdc snϕdc ] fo all whee l dc and ϕ dc denotes the fomaton states fom th WMR to the vtual cente of the MRFS. Now we defne the geneal contol system n tems of the nonholonomc MRFS n Eq. (). Defnton. Contol System: A contol system S = ( B, F) conssts of the followng. A fbe bundle π : M B called the contol bundle. A smooth map F : M B whch s fbe pesevng. A map s fbe pesevng f πb F = π, whee π b : B B s the tangent bundle poecton. Consdeng the ntegal flow of the contol system, an extensve defnton s made n the sense of the fbe bundle stuctue. Defnton.: A smooth cuve cm : M wth s called the taectoy of the contol system ( BF, ) f thee exsts a cuve c: B such that π ( cm ) = c and c () t = F( cm () t ). n local coodnates, ths defnton s tanslated nto the well-known concept of taectoes n a contol system by q = f ( q, u, t). Regadng the knematcs of the MRFS, the n n fomaton state z s educed fom to f we consde the elatve length only so that the constant fo the elatve angle s embedded va a dffeental topology of the nonholonomc system. We eaange Eq. (a) and Eq. (b): q p = fp( qp, u ) (3) z = h( qp, Ω). Accodng to Eq. (3b), the dffeental equaton n the oented angle s able to be combned. z = Lf p h (4) q = u θ 3575

3 whee L denotes the Le devatve wth L h f, h q. Notce that the MRFS s stll nonholonomc and cannot be stablzed va a smooth statc feedback contol. Also, we may apply a smooth dynamc state feedback contol fo stablzng the system n Eq. (4). f one gves the desed fomaton state z d, Eq. (4) can be futhe obtaned as: z = Lf h z p d q u. (5) θ = he contol pupose of the MRFS n Eq. (5) s thus expessed to desgn the contol u and u fo zeong the output of z wthout settng the contol output as zeo only. S = B, F and Remak.3: Consde two contol systems, (, ) fp B B B SQ = BQ FQ, wth espect to some submeson Φ: B Q. hen, the vecto felds n S B maps fom vecto felds n S Q and ae found by the followng pocess: q,, p q u M θ F B ( X, p X p, u ) B Φ ( Z, [ Zu, ]) Q. Paallel to the desgn a contol of the MRFS, f we emove the assumpton of the gdty condton of the MRFS n ths secton, one yeld a challenge n selecton of the nteconnecton stuctue fo achevng the gdty condton of the fomaton shape. Consequently, we ty to nvestgate the elatonshp among subsystem stablty, nteconnecton stablty, and fomaton system stablty.. SABLY OF MRFS Now, we elease the assumpton of the gdty condton n Secton and begn by tanslatng the stablty poblem nto puely algebac tems n a local sense. n the neghbohood of q V and z U wth V B; U Q, suppose thee exsts δ q > 0 aound q and smlaly, thee exsts δ z > 0 aound z so that small enough balls B( q, q ) and B( z, z ) wth q > 0 ; z > 0 exst on V and U, espectvely. hus, the sup B q, = and uppe bound of the open balls ( q ) q sup B( z, z ) = z, wth z and of the balls can be found. Now, we set q mn ( δ, ) q q z mn ( δz, z ) q beng the maxmum adus = and =, and the followng defntons can be gven. p Defnton 3. (nteconnecton stable): Let z be pecewse contnuous n t, and suppose that z d s gven such that the fomaton state eo s defned as z () t z () t zd () t. he system s nteconnecton stable f fo evey z > 0 thee exsts a δ z > 0 such that f z ( t0 ) δ then z lm () z t z fo all. n addton, the state eo of the th WMR of the MRFS, q () t q () t qd () t, s defned fo fndng the condtons of the subsystem stablty. Defnton 3. (Subsystem stable): Let z be pecewse contnuous n t and zd be a gven. he equlbum pont z ( 0) = 0 and q ( 0) = 0 fo all, n the fomaton eo state and subsystem eo state espectvely s subsystem system stable: f thee exsts z ( t0 ) δ and z q t δ then lm q () t, fo all ; 0 q q asymptotcally subsystem system stable: f thee exsts z t δ q t δ then lm q () t 0, ( 0 ) and z 0 q t fo all ; subsystem system unstable: f t s not subsystem stable. he followng lemma dentfes the elatonshp between the nteconnecton stablty and the subsystem stablty of the MRFS. Lemma 3.3: he followng statements ae tue: he MRFS s nteconnecton stable f the subsystem stablty s satsfed; he MRFS s subsystem unstable f and only f the nteconnecton stablty s not held. Poof: Fo the fst statement, we shall pove that, f lm ( q() t + q () t ) q + q, then lm () z t fo z all. Fom Defnton., we have 0 q 0 q q t δ and q t δ. Hence, accodng to Fgue, the nequalty can be wtten as δq + zd + δq z wth δq + δ 0 q > such z t δ + δ whch can be epesented as that we have ( 0 ) q q lm z ( t ) lm q ( t ) + q ( t ) by Defnton 3. and Defnton 3.. Wth ths esult, the bounday z q + q can be found such that lm z () t fo all. z 3576

4 Fg.. he dagam fom fomaton state space to the ndvdual state space. Fo the second statement, we fst clam that the subsystem s unstable and the nteconnecton stablty s held. Fom the pecedng poof, the unstable subsystem mples that t s able to q t + q t q + q, whch mples that q + q s unbounded. Hence, the bounded z does not exst, so the clam s not tue. On the contay, we clam that the MRFS s nteconnecton unstable but the subsystems ae locally stable. Mathematcally, the nteconnecton beng unstable demonstates z s unbounded such that q + q s unbounded, whch ndcates that the subsystem s unstable, so the clam cannot be tue. Lemma 3.3 clealy ndcates that, f the subsystem s stable, the nteconnecton stablty s guaanteed and thee also exsts a be wtten as () () lm unque map Φ ( q p ) fom B to Q fo all. Followng ths esult, a moe stct defnton wth espect to the fomaton stablty s needed. Defnton 3.4 (Fomaton system stable): Let z be pecewse contnuous n t and zd be a gven. he equlbum pont z ( 0) = 0 and q ( 0) = 0 n the fomaton eo state and subsystem eo state fo all, s Fomaton system stable: f thee exst z ( t0 ) δ and z q ( t0 ) δ q then lm () z t and z () lm q t fo all ; q asymptotcally fomaton system stable: f thee exst z t δ q t δ then ( 0 ) and z () t 0 q lm z 0 and lm q () t 0, fo all ; fomaton system unstable: f the afoementoned condtons do not exst. V. DYNAMC SAE FEEDBACK CONROL n ths secton, a dynamc state feedback fomaton contol wth espect to the nteconnecton stuctue s poposed fo the nonholonomc MRFS based on Lyapunov theoy. Fo connectng the gdty matx and the fomaton dynamcs, we may ntoduce an adacency matx to descbe the nteconnected stuctue of the MRFS. he adacency matx[] (o so-called nteconnecton matx), A G, s mposed and epesented as a bnay matx whch mples q acts on q f the element n th ow and th column of the matx equals denoted as A G. t s the fact that all of the connectons of the th WMR to all neghbo nteconnectons ae able to fom a set: a = { AG (, ) n} whee and denotes the th aw and th column n the adacency matx. heefoe, egadng wth the nteconnecton stuctue, the knematcs of the MRFS could be ewtten as: qpω q p 0 z = J qpω q p q (6) θ a aj J 0 0 wth Ω = ; Ω = ; = l l 0 ; J = 0 ; p p p whee q q q l denotes the elatve length andϕ denotes the elatve oented angle. We select the Lyapunov functon L = aqq n each of the subsystems. Addtonally, we lmt the output of the angula velocty to pevent the behavos of sldng and slppng. Nevetheless, ths may lead the satuaton n the contol, so we have to caefully choose the opeaton pont n the contol desgn by means of manpulatng the contol gan. o ad n the udgement of the stablty ctea, we also mpose an nteconnecton Lyapunov functon: L = waz z wth w beng an element of the weght : 3n 3n matx W. By the addtve popety of the enegy, the fomaton Lyapunov functon, L can be smply splt nto two pats: the ndvdual Lyapunov functon of the th WMR and the nomalzed nteconnecton Lyapunov functons of the th WMR whch acts on the th WMR F L = L + L n (7) n Eq. (7), L s geneated fom the th subsystem and L s poduced by the nteconnecton of the MRFS fo the th subsystem. n the component fom, t can be wtten as: F q p L = p q q θ P q θ (8) + L G z z zn z n 3 3 whee P s postve defnte; L G denotes the th aw of the nomalzed weghted laplacan matx wth LG W AG wth W beng a weghted postve n F 3577

5 defnte matx. Hence, the necessay condton fo the asymptotcally fomaton stablty s establshed va the followng theoem: heoem 4.: Consdeng the MRFS descbed n Eq. (-), the system s sad to be asymptotcally nteconnecton stable f FW Ω + W Ω F= G wth [ ] F = F F n whee F = fp q denotes a Jacoban matx whch s poceeded wth the lneazaton scheme fom the nonlnea functon f p n Eq. (3) ; G beng postve matx. Poof. he tme devatve of Equaton (8) can be educed as the followng fomulaton: L = q p ( W Ω ) qp + qp ( W Ω ) qp (9) = qp ( F ( W Ω ) + ( W Ω ) F ) qp hus, we efomulate the esult n Eq. (9) n assocated wth a matx fomula: FW ( Ω ) + ( W Ω ) F= G (0) whee G s a postve matx. Accodng to the Lyapunov stablty theoem, f Ω and G ae postve defnte, then the MRFS s asymptotcally stable. heoem 4. also povdes the necessay condton of the fomaton system stablty n assocaton wth the nteconnecton matx. he condton s that the nteconnecton matx Ω shall have full dmenson. As a consequence, Ω has to be a non-sngula matx fom the lnea system theoy. Pactcally, let us now consde a MRFS n tems of t nteconnecton stuctue n Eq. (6). he Lyapunov functon n Eq. (9) can be futhe taken as the patal devatve: F L L L = + q q = f ( a, z, qp, qp, qθ ) () + qps + f ( a, z, qp, qp, qθ) v + qθw ; wth f = ( ρz)( zd cosγ) f = ρ z cosϕ wth θ cos qθ sn q 0 γ = ϕ + qθ qθ ; S = 0 0. heefoe, the fomaton contol can be chosen by the followng theoem: F f KpL ; qps + f v = 0 f qps + f = 0; w = K q. θ θ () then the MRFS s asymptotcally stable whee Kp K θ 0 denotes the postve contol gan. Poof: Afte puttng the contolle n Eq. () nto Eq. (), the Lyapunov equaton s obtaned: F F F L = KpL ( Kp Kθ ) qθ KpL (3) Obsevng Eq. (3), the MRFS wth the contol of the th WMR fo all n Eq. () s exponentally stable. he esult fom heoem 4. cetanly eveals that, f the enegy s exponentally decayng, then the eachablty of the subsystems of the MRFS s guaanteed. Now, the oveall contol ssues can be lnked togethe: ntenal dynamcs and the nteconnected stuctue. Remak 4.3 Accodng to heoem 4., the contol of the MRFS satsfes the fomulaton of the smooth dynamc feedback contol desgn u = α ( q, z) (4) z = h( q, z) so that Bockett s necessay condton fo the pecewse contol desgn by means of the smooth statc feedback of the nonholonomc system can be ovecome by the poposed dynamc state feedback contol desgn n the MRFS n heoem 4.. n the next secton, an expement s conducted fo evaluatng the theoetcal esults. V. EXPERMENAL RESUL An expement s set up to evaluate the poposed appoach. he MRFS wth thee WMRs that move on a fee D space s the mao applcaton scenao. heoem 4.: Consdeng that the MRFS follows Eq. (-), f the velocty and angula velocty s chosen by Fg.. Physcal layout of the setup of the expement. 3578

6 Each of the moble obots s equpped wth encodes and s connected vtually wth a weless communcaton devce shown n Fg.. hus, thee moble obots (P3DX) made by Actve Meda nc. play a ole as an expemental platfom. he aveage speed s 0.5m sec and the aveage acceleaton s m sec. n the ntal settng, we put the thee WMRs n the specfed places and espectvely egad the postons as the statng ponts. Also, the elatve dstance wth each othe s ntally set to.5 (m). Afte 0 sec fom the statng tme, the desed elatve dstance s set to be 3.5 (m). 60 sec late, the elatve dstance s set back to.5 (m). y(m) y(m) m he taectoes of the WMRs x(m) he taectoes of the fomaton system x(m) Fg. 3. he taectoes of the MRFCS. Relatve Dstances sec Fg. 4. he elatve dstances ( L, L 3, L 3 ) of the MRFS. he expemental esults ae dawn n Fg. 3 and Fg. 4, espectvely. he fst dawng descbes both the taectoy of the ndvdual WMR and the elatve dstance fom WMR to WMR whch s denoted as L wth, = {,,3}. n Fg. 4, t s sgnfcant that the elatve dstance between WMR and WMR 3 s elatvely moe stable than the one fom WMR to WMR and WMR to WMR 3 due to the popety of the nonholonomc system. he fomaton contol paamete n K p and K θ ae both selected as 0.0 and hs follows the analyss esult n heoem 4. pecsely. V. CONCLUSON n ths eseach, the poblem of dlaton fomaton shape s successfully solved by means of smooth dynamc state stable WMR WMR WMR3 WMR WMR WMR3 L L3 L3 feedback contol. By the way of ntoducng the hghe dmenson vaable, the poposed contol appoach of the MRFS s essentally used solve the followng desgn poblem: the stable contol fo the nonholonomc MRFS wth espect to the nteconnecton stuctue fo peventng the dvegence of the MRFS; anothe one s that the global path plannng s mpossble to pefom n lage scale subsystems of a MRFS. Futhemoe, the theoetcal analyss s set up so that the necessay condton fo the fomaton system stablty s dstnctly obtaned wth espect to the nonholonomc subsystems. Fnally, the expement s pefomed to evaluate the poposed contol appoach. REFERENCES [] J. P. Desa, et al., "Contollng fomatons of multple moble obots," pesented at the Poceedngs of the 998 EEE ntematonal Confeence on Robotcs & Automaton, Leuven, Belgum, 998. [] J. P. Desa, et al., "Modelng and Contol of Fomaton of Nonholonomc Moble Robots," EEE RANSACONS ON ROBOCS AND AUOMAON, vol. 7, 00. [3] A. K. Das, et al., "A Vson-Based Fomaton Contol Famewok," EEE RANSACONS ON ROBOCS AND AUOMAON, vol. 8, pp , 00. [4] J. R.. Lawton, et al., "A Decentalzed Appoach to Fomaton Maneuves," EEE ansacton on Robots and Automaton, vol. 9, pp , 003. [5] O. A. A. Oqueda and R. Feo, "A Vson-based Nonlnea Decentalzed Contolle fo Unmanned Vehcles," pesented at the Poceedngs of the 006 EEE ntenatonal Confeence on Robotcs and Automaton, Olando, Floda, 006. [6] M. J and M. Egestedt, "Dstbuted Coodnaton Contol of Multagent Systems Whle Pesevng Connectedness," EEE RANSACONS ON ROBOCS, vol. 3, pp , 007. [7] Y. Zou, et al., "Fomaton of a Goup of Vehcles wth Full nfomaton Usng Constant Foces," Jounal of Dynamc Systems, Measuement and Contol, vol. 9, pp , 007. [8] W. Ren and N. Soensen, "Dstbuted coodnaton achtectue fo mult-obot fomaton contol," Robotcs and Automated System, vol. 56, pp , 008. [9] K. D. Do, "Output-feedback fomaton tackng contol of uncycle-type moble obots wth lmted sensng anges," Robotcs and Autonomous Systems, 008. [0] Z. Ln, et al., "Necessay and Suffcent Gaphcal Condtons fo Fomaton Contol of Uncycles," EEE ansacton on Automatc Contol, vol. 50, pp. -6, 005. [] R. Olfat-Sabe and R. M. Muay, "Consensus Poblems n Netwoks of Agents wth Swtchng opology and me-delays," EEE Rans on Automatc Contol, vol. 49, pp , 004. [] F. R. K. Chung, Spectal Gaph heoy: Amecan Mathematcal Socety,