IEEETRANSACTIONSONROBOTICS,VOL.25,NO.4,AUGUST2009851Vision-Based,DistributedControlLawsforMotionCoordinationofNonholonomicRobotsNimaMoshtagh,Member,IEEE,NathanMichael,Member,IEEE,AliJadbabaie,SeniorMember,IEEE,andKostasDaniilidis,SeniorMember,IEEEAbstractInthispaper,westudytheproblemofdistributedmo-tioncoordinationamongagroupofnonholonomicgroundrobots.Wedevelopvision-basedcontrollawsforparallelandbalancedcir-cularformationsusingaconsensusapproach.Theproposedcon-trollawsaredistributedinthesensethattheyrequireinformationonlyfromneighboringrobots.Furthermore,thecontrollawsarecoordinate-freeanddonotrelyonmeasurementorcommunica-tionofheadinginformationamongneighborsbutinsteadrequiremeasurementsofbearing,opticalflow,andtimetocollision,allofwhichcanbemeasuredusingvisualsensors.Collision-avoidancecapabilitiesareaddedtotheteammembers,andtheeffectivenessofthecontrollawsaredemonstratedonagroupofmobilerobots.IndexTermsCooperativecontrol,distributedcoordination,vision-basedcontrol.I.INTRODUCTIONCOOPERATIVEcontrolofmultipleautonomousagentshasbecomeavibrantpartofroboticsandcontroltheoryresearch.Themainunderlyingthemeofthislineofresearchistoanalyzeand/orsynthesizespatiallydistributedcontrolar-chitecturesthatcanbeusedformotioncoordinationoflargegroupsofautonomousvehicles.Someofthisresearchfocussesonflockingandformationcontrol9,14,16,22,31,andsynchronization2,39,whileothersfocusonrendezvous,distributedcoverage,anddeployment1,5.Akeyassump-tionimpliedinallofthepreviousreferencesisthateachvehicleorrobot(hereaftercalledanagent)communicatesitspositionand/orvelocityinformationtoitsneighbors.Inspiredbythesocialaggregationphenomenainbirdsandfish6,30,researchersinroboticsandcontroltheoryhaveManuscriptreceivedFebruary23,2008；revisedJanuary31,2009.Firstpub-lishedJune10,2009；currentversionpublishedJuly31,2009.ThispaperwasrecommendedforpublicationbyAssociateEditorZ.-W.LuoandEdi-torJ.-P.Laumonduponevaluationofthereviewerscomments.TheworkofA.JadbabaiewassupportedinpartbytheArmyResearchOfficeMultidisciplinaryUniversityResearchInitiative(ARO/MURI)underGrantW911NF-05-1-0381,inpartbytheOfficeofNavalResearch(ONR)/YoungIn-vestigatorProgram542371,inpartbyONRN000140610436,andinpartunderContractNSF-ECS-0347285.TheworkofK.DaniilidiswassupportedinpartunderContractNSF-IIS-0083209,inpartunderContractNSF-IIS-0121293,inpartunderContractNSF-EIA-0324977,andinpartunderContractARO/MURIDAAD19-02-1-0383.N.MoshtaghwaswiththeGeneralRobotics,Automation,Sensing,andPer-ceptionLaboratory,UniversityofPennsylvania,Philadelphia,PA19104USA.HeisnowwithScientificSystemsCompany,Inc.,Woburn,MA01801USA(e-mail:).N.Michael,A.Jadbabaie,andK.DaniilidisarewiththeGeneralRobotics,Automation,Sensing,andPerceptionLaboratory,UniversityofPennsylva-nia,Philadelphia,PA19104USA(e-mail:；；).Colorversionsofoneormoreofthefiguresinthispaperareavailableonlineat.DigitalObjectIdentifier10.1109/TRO.2009.2022439developedtools,methods,andalgorithmsfordistributedmo-tioncoordinationofmultivehiclesystems.Twomaincollectivemotionsthatareobservedinnatureareparallelmotionandcircularmotion21.Onecaninterpretstabilizingthecircularformationasanexampleofactivityconsensus,i.e.,individualsare“movingaround”together.Stabilizingtheparallelforma-tionisanotherformofactivityconsensusinwhichindividuals“moveoff”together33.Circularformationsareobservedinfishschooling,whichisawell-studiedtopicinecologyandevolutionarybiology6.Inthispaper,wepresentasetofcontrollawsforcoordinatedmotions,suchasparallelandcircularformations,foragroupofplanaragentsusingpurelylocalinteractions.Thecontrollawsareintermsofshapevariables,suchastherelativedistancesandrelativeheadingsamongtheagents.However,theseparam-etersarenotreadilymeasurableusingsimpleandbasicsensingcapabilities.Thismotivatestherewritingofthederivedcontrollawsintermsofbiologicallymeasurableparameters.Eachagentisassumedtohaveonlymonocularvisionandisalsocapableofmeasuringbasicvisualquantities,suchasbearingangle,opti-calflow(bearingderivative),andtimetocollision.Rewritingthecontrolinputsintermsofquantitiesthatarelocallymeasurableisequivalenttoexpressingtheinputsinthelocalbodyframe.Suchachangeofcoordinatesystemfromaglobalframetoalocalframeprovidesuswithabetterintuitiononhowsimilarbehaviorsarecarriedoutinnature.Verificationofthetheorythroughmultirobotexperimentsdemonstratedtheeffectivenessofthevision-basedcontrollawstoachievethedesiredformations.Ofcourse,inreality,anyformationcontrolrequirescollisionavoidance,andindeed,collisionavoidancecannotbedonewithoutrange.Inordertoimprovetheexperimentalresults,weprovidedinteragent-collision-avoidancepropertiestotheteammembers.Inthispaper,weshowthatthetwotasksofformationkeepingandcollisionavoidancecanbedonewithdecoupledadditivetermsinthecontrollaw,wherethetermsforkeepingparallelandcircularformationsdependonlyonvisualparameters.Thispaperisorganizedasfollows.InSectionII,wereviewanumberofimportantrelatedworks.Somebackgroundinfor-mationongraphtheoryandothermathematicaltoolsusedinthispaperareprovidedinSectionIII.TheproblemstatementisgiveninSectionIV.InSectionsVandVI,wepresentthecontrollersthatstabilizeagroupofmobileagentsintoparallelandbalancedcircularformations,respectively.InSectionVII,wederivethevision-basedcontrollersthatareintermsofthevisualmeasurementsoftheneighboringagents.InSectionVIII,collision-avoidancecapabilitiesareaddedtothecontrollaws,andtheireffectivenessistestedonrealrobots.1552-3098/$26.002009IEEEAuthorizedlicenseduselimitedto:NanchangUniversity.DownloadedonJanuary12,2010at20:02fromIEEEXplore.Restrictionsapply.852IEEETRANSACTIONSONROBOTICS,VOL.25,NO.4,AUGUST2009II.RELATEDWORKANDCONTRIBUTIONSTheprimarycontributionofthispaperisthepresentationofsimplecontrollawstoachieveparallelandcircularformationsthatrequireonlyvisualsensing,i.e.,theinputsareintermsofquantitiesthatdonotrequirecommunicationamongnearestneighbors.IncontrastwiththeworkofJusthandKrishnaprasad17,MoshtaghandJadbabaie27,Paleyetal.32,33,andSepulchreetal.35,whereitisassumedthateachagenthasaccesstothevaluesofitsneighborspositionsandvelocities,wedesigndistributedcontrollawsthatuseonlyvisualcluesfromnearestneighborstoachievemotioncoordination.Ourapproachonderivingthevision-basedcontrollawscanbeclassifiedasanimage-basedvisualseroving41.Inimage-basedvisualservoing,featuresareextractedfromimages,andthenthecontrolinputiscomputedasafunctionoftheimagefeatures.In8,12,and38,authorsuseomnidirectionalcam-erasastheonlysensorforrobots.In8and38,inputoutputfeedbacklinearizationisusedtodesigncontrollawsforleader-followingandobstacleavoidance.However,theyassumethataspecificverticalposeofanomnidirectionalcameraallowsthecomputationofbothbearinganddistance.IntheworkofPrattichizzoetal.12,thedistancemeasurementisnotused；however,theleaderusesextendedKalmanfilteringtolocalizeitsfollowers,andcomputesthecontrolinputsandguidestheformationinacentralizedfashion.Inourpaper,thecontrolar-chitectureisdistributed,andwedesigntheformationcontrollersbasedonthelocalinteractionamongtheagentssimilartothatof14and22.Furthermore,forourvision-basedcontrollers,nodistancemeasurementisrequired.In25and34,circularformationsofamultivehiclesys-temundercyclicpursuitisstudied.Theirproposedstrategyisdistributedandsimplebecauseeachagentneedstomeasuretherelativeinformationfromonlyoneotheragent.Itisalsoshownthattheformationequilibriaofthemultiagentsystemaregeneralizedpolygons.Incontrastto25,ourcontrollawisanonlinearfunctionofthebearingangles,andasaresult,oursystemconvergestoadifferentsetofstableequilibria.III.BACKGROUNDInthissection,webrieflyreviewanumberofimportantcon-ceptsregardinggraphtheoryandregularpolygonsthatweusethroughoutthispaper.A.GraphTheoryAn(undirected)graphGconsistsofavertexsetVandanedgesetE,whereanedgeisanunorderedpairofdistinctverticesinG.Ifx,yVand(x,y)E,thenxandyaresaidtobeadjacent,orneighbors,andwedenotethisbywritingxy.Thenumberofneighborsofeachvertexisitsdegree.Apathoflengthrfromvertexxtovertexyisasequenceofr+1distinctverticesthatstartwithxandendwithysuchthatconsecutiveverticesareadjacent.IfthereisapathbetweenanytwoverticesofagraphG,thenGissaidtobeconnected.TheadjacencymatrixA(G)=aijofan(undirected)graphGisasymmetricmatrixwithrowsandcolumnsindexedbytheverticesofG,suchthataij=1ifvertexiandvertexjareneighbors,andaij=0otherwise.Wealsoassumethataii=0foralli.ThedegreematrixD(G)ofagraphGisadiagonalmatrixwithrowsandcolumnsindexedbyV,inwhichthe(i,i)-entryisthedegreeofvertexi.ThesymmetricsingularmatrixdefinedasL(G)=D(G)A(G)iscalledtheLaplacianofG.TheLaplacianmatrixcapturesmanytopologicalpropertiesofthegraph.TheLaplacianLisapositive-semidefinitematrix,andthealgebraicmultiplicityofitszeroeigenvalue(i.e.,thedimensionofitskernel)isequaltothenumberofconnectedcomponentsinthegraph.Then-dimensionaleigenvectorassociatedwiththezeroeigenvalueisthevectorofones,1n=1,.,1T.Formoreinformationongraphtheory,see13.B.RegularPolygonsLetd1andnanddarecoprime,thentheedgesintersect,andthepolygonisastar.Ifnanddhaveacommonfactorl1,thenthepolygonconsistsofltraversalsofthesamepolygonwithn/lverticesandedges.Ifd=n,thepolygonn/ncorrespondstoallpointsatthesamelocation.Ifd=n/2(withneven),thenthepolygonconsistsoftwoendpointsandalinebetweenthem,withpointshavinganevenindexononeendandpointshavinganoddindexontheother.Formoreinformationonregulargraphs,see7.IV.PROBLEMSTATEMENTConsideragroupofnunit-speedplanaragents.Eachagentiscapableofsensinginformationfromitsneighbors.Theneigh-borhoodsetofagenti,thatis,Ni,isthesetofagentsthatcanbe“seen”byagenti.Theprecisemeaningof“seeing”willbeclarifiedlater.Thesizeoftheneighborhooddependsonthechar-acteristicsofthesensors.TheneighboringrelationshipbetweenagentscanbeconvenientlydescribedbyaconnectivitygraphG=(V,E,W).Definition1(Connectivitygraph):TheconnectivitygraphG=(V,E,W)isagraphconsistingof1)asetofverticesVindexedbythesetofmobileagents；2)asetofedgesE=(i,j)|i,jV,andij；3)asetofpositiveedgeweightsforeachedge(i,j).TheneighborhoodofagentiisdefinedbyNi.=j|ijVi.Letrirepresentthepositionofagenti,andletvibeitsvelocityvector.Thekinematicsofeachunit-speedagentisAuthorizedlicenseduselimitedto:NanchangUniversity.DownloadedonJanuary12,2010at20:02fromIEEEXplore.Restrictionsapply.MOSHTAGHetal.:VISION-BASED,DISTRIBUTEDCONTROLLAWSFORMOTIONCOORDINATIONOFNONHOLONOMICROBOTS853Fig.1.TrajectoryofeachagentisrepresentedbyaplanarFrenetframe.givenbyri=vivi=ivivi=ivi(1)whereviistheunitvectorperpendiculartothevelocityvectorvi(seeFig.1).Theorthogonalpairvi,viformsabodyframeforagenti.Werepresentthestackvectorofallthevelocitiesbyv=vT1,.,vTnTR2n1.Thecontrolinputforeachagentistheangularvelocityi.Sinceitisassumedthattheagentsmovewithconstantunitspeed,theforceappliedtoeachagentmustbeperpendiculartoitsvelocityvector,i.e.,theforceoneachagentisagyroscopicforce,anditdoesnotchangeitsspeed(andhence,itskineticenergy).Thus,iservesasasteeringcontrol16foreachagent.Letusformallydefinetheformationsthatwearegoingtoconsider.Definition2(Parallelformation):Theconfigurationinwhichtheheadingsofallagentsarethesameandvelocityvectorsarealignediscalledtheparallelformation.Notethatinthisdefinition,wedonotconsiderthevalueoftheagreeduponvelocitybutjustthefactthattheagreementhasbeenreached.Attheequilibrium,therelativedistancesoftheagentsdeterminetheshapeoftheformation.Anotherinterestingfamilyofformationsisthebalancedcircularformation.Definition3(Balancedcircularformation):Theconfigurationwheretheagentsaremovingonthesamecirculartrajectoryandthegeometriccenteroftheagentsisfixediscalledthebalancedcircularformation.Theshapeofsuchaformationcanberepresentedbyanappropriateregularpolygon.Inthefollowingsections,westudyeachformationanddesignitscorrespondingdistributedcontrollaw.V.PARALLELFORMATIONSOurgoalinthissectionistodesignacontrollawforeachagentsothattheheadingsofthemobileagentsreachanagree-ment,i.e.,theirvelocityvectorsarealigned,thusresultinginaswarm-likepattern.ForanarbitraryconnectivitygraphG,con-sidertheLaplacianmatrixL.We,therefore,defineameasureofmisalignmentasfollows27,35:w(v)=12summationdisplayij|vivj|2=12v,Lv(2)wherethesummationisoverallthepairs(i,j)E,andL=LI2R2n2n,withI2beingthe22identitymatrix.Thetimederivativeofw(v)isgivenbyw(v)=nsummationdisplayi=1vi,(Lv)i=nsummationdisplayi=1ivi,(Lv)iwhere(Lv)iR2isthesubvectorofLvassociatedwiththeithagent.Thus,thefollowinggradientcontrollawguaranteesthatthepotentialw(v)decreasesmonotonically:i=vi,(Lv)i=summationdisplayjNivi,vij(3)where0isthegain,andvij=vjvi.Remark1:Letirepresenttheheadingofagentiasmeasuredinafixedworldframe(seeFig.1).Theunitvelocityvectorvianditsorthogonalvectorviaregivenbyvi=cosisiniTandvi=sinicosiT.Thus,thecontrolinput(3)becomesi=summationdisplayjNisin(ij),0.(6)Thefollowingtwotheorems28presenttheresultswhenbalancedcircularformationsareattainedforagroupofunit-speedagentswithfixedconnectivitygraphs.Theorem2isforthecasewhenGisacompletegraph,andTheorem3isfortheringgraph.Theorem2:Considerasystemofnagentswithkinematics(5).GivenacompleteconnectivitygraphGandapplyingcontrollaw(6),then-agentsystem(almost)globallyasymptoticallyconvergestoabalancedcircularformation,whichisdefinedinDefinition3.Proof:See28fortheproof.squaresolidThereasonfor“almostglobal”stabilityofthesetofbal-ancedstatesisthatthereisameasure-zerosetofstateswheretheequilibriumisunstable.Thissetischaracterizedbythoseconfigurationsthatmagentsareatantipodalpositionfromtheothernmagents,where1mn/2.Next,weconsiderthesituationthattheconnectivitygraphhasaringtopologyGring.Theorem3:Considerasystemofnagentswithkinematics(5).SupposetheconnectivitygraphhastheringtopologyGringandthateachag