ALIGNMENTCONTROLFOROPTICALCOMMUNICATIONBETWEEN UNDERWATERROBOTS By PratapBhanuSolanki ADISSERTATION Submittedto MichiganStateUniversity inpartialful˝llmentoftherequirements forthedegreeof ElectricalEngineeringDoctorofPhilosophy 2021 ABSTRACT ALIGNMENTCONTROLFOROPTICALCOMMUNICATIONBETWEEN UNDERWATERROBOTS By PratapBhanuSolanki Light-emittingdiode(LED)-basedopticalcommunicationisemergingasapromisinglow-power, low-cost,andhigh-data-ratealternativetoacousticcommunicationforunderwaterapplications. However,itrequiresaclose-to-line-of-sight(LOS)linkbetweenthecommunicatingparties. AchievingandmaintainingtheLOSischallengingduetotheconstantmovementofunderly- ingmobileplatformscausedbypropulsionandunwanteddisturbances.Inthisdissertation,anovel, compactLED-basedwirelesscommunicationsystemwithactivealignmentcontrolispresentedthat maintainstheLOSdespitethemovementoftheunderlyingplatform.Multiplealignmentcontrol algorithmsaredevelopedforscenariosthatrangefromasimpleone-waytwo-dimensional(2D) settingtoapracticalthree-dimensional(3D)bi-directionalunderwatersetting. AnextendedKalman˝lter(EKF)-basedapproachis˝rstproposedtoestimatetherelative orientationbetweentheheadingangleandtheLOSdirection,whichissubsequentlyusedfor alignmentcontrol.TheEKFusesonlythemeasurementoflightintensityfromasinglephoto- diode,wheresuccessivemeasurementsareobtainedviaascanningtechniquethatensuresthefull observabilityoftheunderlyingsystem.Theapproachis˝rstexaminedina2Dsetting,andthen extendedtothe3Dscenariowithimprovementsinboththehardwareandthealgorithm.The amplitudeofthescanningismodulatedaccordingtothealignmentperformancetoachieveasound trade-o˙betweenestimationaccuracy,signalstrength,andenergyconsumption.Thee˚cacyof theapproachistestedandveri˝edviasimulationandonanexperimentalsetupinvolvingtworobots withrelative3Dmotion. TheEKFapproachusesanassumptionthattherelativemotionbetweentherobotsissmall, andconsequently,requiresthecommunicatingrobotstotakethescanninginanalternatingfashion fortheconvergenceoftheestimator.Analternativeapproach,˝rstexploredinthe2Dsetting,is developedthatallowssimultaneous,bi-directionalalignmentcontrolforbothparties.Becauseof theconvexnatureofthemeasuredintensityfunctions,model-freeapproaches,includingbothhill- climbing(HC)andextremum-seeking(ES),areexplored.Thehill-climbingapproachisfoundtobe superiortotheESapproachintermsofconvergencetimeandcomputationale˚ciency.Theoretical analysisisprovidedforthehill-climbingapproachthatguarantees˝nitetimeconvergencetoan $ ( X ) neighborhoodoftheLOS,forcontrolstepsize X . Finally,amodel-freeapproachforthe3Dsettingisproposedthatmaximizeslightintensitybased onthreeconsecutiveintensitymeasurementsfromanequilateraltrianglecon˝guration.Thee˚cacy oftheapproachisdemonstratedexperimentally,˝rstwithanunderwaterrobotcontrolledbya joystickviaLEDcommunicationandthenwithtworobotsperformingbi-directionalcommunication andtrackinginanunderwatersetting. Copyrightby PRATAPBHANUSOLANKI 2021 Dedicatedtomyparentsandmybrother fortheirloveandsupportthroughthisjourney. v ACKNOWLEDGEMENTS ThroughoutmyPh.D.journey,Ihavereceivedagreatdealofsupportandassistance. Firstly,Iwouldliketoexpressmysinceregratitudetomyadvisor,Prof.XiaoboTan,for thecontinuoussupportofmyPh.D.studyandrelatedresearch,forhispatience,motivation,and guidance.Besidesmyadvisor,Iwouldliketothankmyentirethesiscommittee:Prof.Hassan Khalil,Prof.RanjanMukherjee,andProf.DanielMorris,fortheirinsightfulcommentsand encouragement.MysincerethanksalsogoestoProf.ShaunakBopardikarforthestimulating discussionsandinsights. IthankJohnThonforhisfabricationexpertiseandhishelpinpreparingmechanicalparts, especiallymakingwaterproofenclosuresfortherobots.IalsothankAustinCoha,ShaswatJoshi, JasonGreenberg,andMaxVerboncoeurfortheirhelpinpreparingtheexperimentalsetupandin performingexperimentsthatarepresentedinthiswork.IthankBrianFickiesforhiscoordination andgrantingusseamlessaccesstotheIMCircleswimmingpoolfacilityatMichiganStateUni- versity.Ithankmyfellowlabmatesfortheirsupportandforallthefunwehavehadinthelastfew years.Thanksalsotoalloftheadministratorsandsta˙membersintheECEdepartmentwhohave helpedmeovertheyears. IalsowanttoacknowledgethefundingsupportfromNationalScienceFoundation(IIS1319602, IIP1343413,CCF1331852,ECCS1446793,IIS1734272,IIS1848945)thatmadethiswork possible. Mostofall,Iwouldliketothankmyfamily:myparentsandmybrotherforsupportingmy decisiontocometotheUSAforpursuingthePh.D.,theirpatience,andprovidingmelove,support, andencouragementthroughoutthisjourney.IwanttopraiseandthankGod,thealmighty,who hasgrantedcountlessblessings,knowledge,opportunity,strength,andperseveranceinlifeand especiallyduringthisperiodofthepandemic. vi TABLEOFCONTENTS LISTOFTABLES ....................................... ix LISTOFFIGURES ....................................... x CHAPTER1INTRODUCTION ............................... 1 1.1LEDOpticalCommunicationandtheProblemofDirectionality...........1 1.2OverviewofContributions..............................4 1.2.1AlignmentControlinthe2DSpace.....................5 1.2.2ActiveAlignmentControlin3DSpace....................6 1.2.3Bi-directionalAlignmentApproachfor2DSpace..............8 1.2.4Bi-directionalAlignmentApproachfor3DSpaceandExperimentsin UnderwaterScenario.............................10 CHAPTER2EKF-BASEDALIGNMENTCONTROLINTHE2DSPACE ........ 12 2.1SystemDesignandImplementation..........................12 2.2Modeling.......................................13 2.2.1LightIntensityModel.............................15 2.2.2State-spaceProblemFormulation.......................18 2.3EstimationandAlignmentAlgorithms........................19 2.4SimulationResults..................................25 2.5ExperimentalResults.................................30 2.6ChapterSummary...................................34 CHAPTER3EKF-BASEDALIGNMENTCONTROLINTHE3DSPACE ........ 36 3.1SystemSetupandModeling..............................37 3.1.1SystemSetup.................................37 3.1.2CoordinateFramesandReceivedLightIntensityModel...........37 3.1.3State-spaceProblemFormulation.......................40 3.2EstimationandAlignmentAlgorithms........................42 3.2.1ObservabilityoftheSystem.........................42 3.2.2ImplementationofExtendedKalmanFilter.................44 3.2.3ExtensiontotheBidirectionalScenario...................47 3.2.4BenchmarkApproach:Extremum-seeking(ES)Control..........48 3.3SimulationResults..................................49 3.4ExperimentalResults.................................55 3.5ChapterSummary...................................58 CHAPTER4SIMULTANEOUSBI-DIRECTIONALALIGNMENTCONTROLIN THEFOR2DSPACE ............................. 60 4.1SystemSetupandProblemFormulation.......................61 4.1.1SystemSetupandModeling.........................61 4.1.2State-spaceproblemformulation.......................64 vii 4.1.3GeneralizedProblemFormulation......................64 4.2MainResults.....................................66 4.2.1ProposedControlLaw............................66 4.2.2De˝nitionofEssentialGeometricTerms...................67 4.2.3KeyResults..................................69 4.3ProofofTheorem4.2.1................................71 4.3.1LocalBehavior................................72 4.3.2Calculationofpathlengthandthenumberofsteps.............79 4.3.3LimitingCon˝guration............................80 4.4Simulationresults...................................81 4.4.1CasewithNon-contiguous D [ ........................82 4.4.2CaseCorrespondingtoOpticalTrackingSetup...............84 4.4.2.1Idealscenario............................86 4.4.2.2Practicalscenario..........................88 4.5ExperimentalResults.................................99 4.6ChapterSummary...................................103 CHAPTER5SIMULTANEOUSBI-DIRECTIONALALIGNMENTCONTROLIN THE3DSPACE ................................ 105 5.1ReviewofSystemSetupandModeling........................106 5.2ATriangularExplorationAlgorithm.........................107 5.3SimulationResults..................................109 5.4In-airExperimentresults...............................113 5.5UnderwaterExperimentsandResults.........................115 5.6ChapterSummary...................................123 CHAPTER6SUMMARY&FUTUREWORK ....................... 125 6.1Summary.......................................125 6.2FutureWork......................................126 APPENDICES ......................................... 128 APPENDIXAGENERATIONOFMOTORCOMMANDS ............. 129 APPENDIXBCALCULATIONOFTHEHEADINGOFFSETANGLEAND ITSDERIVATIVES .......................... 132 BIBLIOGRAPHY ........................................ 133 viii LISTOFTABLES Table2.1:ParametersassociatedwithEKFimplementationinthesimulation.........27 Table3.1:Parametersusedinthesimulation.Thevaluesofparametersmarkedas' « ' arechosenempirically...............................55 Table4.1:Summaryoftheoutcome u : +1 basedon u : andgradientangles.Theterm u : denotesthecomplexconjugateof u : ........................75 Table4.2:Parametersusedinsimulation............................92 ix LISTOFFIGURES Figure1.1:Illustrationofapplicationofunderwaterwirelessopticalcommunicationin diversescenarios(Source:UniversitàdegliStudi,Italy[42])...........2 Figure1.2:Illustrationoftheextractionofsignalstrengthandinformationfromoptical signalincidentonaphoto-diode..........................4 Figure2.1:AprototypeofLEDopticalcommunicationmodulewitharotationalbase...14 Figure2.2:Illustrationoftherelativepositionandorientationbetweenthetransmitter andthereceiver...................................15 Figure2.3:Gaussiancurve˝ttingforthefunction 6 forthephotodiodeusedinthiswork..17 Figure2.4:Blockdiagramillustratingtheproposedmethod..................20 Figure2.5:Illustrationofthereceiverscanningsequence,withmean G 2 andlasttwo anglesofscanning k : and k : 1 ..........................21 Figure2.6:SimulationresultsofEKFwhenthe G 2 dynamicscontainsanunknowncon- stantdisturbance V =1 Ł 2 € s ............................26 Figure2.7:SimulationresultsofEKFmethod'sfailurewhentheunknownconstantrate disturbance V isincreasedto 8 € s .........................28 Figure2.8:Simulationresultsoncomparisonofalignmentcontrolperformanceforthe threemethods,fordi˙erentlevelsofmeasurementnoise,whensystemstates areevolvedaccordingtoEq.(2.25)with V =1 Ł 2 .Verticalbarsdenotethe down-scaledstandarddeviationsateachpoint.representsthehill- climbingalgorithm,andointrepresentsthethree-point-averagingalgo- rithm........................................30 Figure2.9:Completesetup:Receiver(left)onarotationbaseandtransmitterLEDona mobilerobot(right).................................31 Figure2.10:Experimentalsetup:Transmitterrobotmovingaroundstaticreceiver,follow- ingthemarkerlines.................................32 Figure2.11:Experimentalresultswhenthetransmitterrobotmovesaroundthereceiver withanangularrateofabout1degree/s.Themeasurementsarecorrupted withthenoiselevelof1.0..............................33 x Figure2.12:Experimentalresultsoncomparisonofalignmentcontrolperformanceforthe threemethods,fordi˙erentlevelsofmeasurementnoisewhentheconstant disturbance( V )is 2 Ł 8 € s .Verticalbarsdenotethedown-scaledstandard deviationsateachpoint...............................35 Figure3.1:Hardwaresetup,wherethetransceiver(transmitterLEDandreceiverphoto- diode)mountedontwo-DOFactivepointingmechanismisshown........38 Figure3.2:Illustrationoftwolocal3Dcoordinateframesandtheassociatedvariablesto de˝netherelativepositionandorientationbetweenthetransceiversoftworobots.39 Figure3.3:LightintensitydataforGaussiancurve˝ttingtoapproximate 5 ( qŒ\ ) .......41 Figure3.4:Illustrationofthecircularscanningsequence,withmeanpointingorientation ¹ G 2 ŒG 3 º andthelastthreeangularpositionsofscanning k : , k : 1 and k : 2 ...44 Figure3.5:Blockdiagramsummarizingtheproposedmethod.Alltheshadedcolor nodesdenotethestepsknowntothealgorithm.Theinformationatwhite colornodesisnotavailabletothealgorithm....................48 Figure3.6:BlockDiagramforextremumseekingcontrol...................49 Figure3.7:Plotofasimulationrunillustratingtheevolutionofthethreestatesand measuredintensityfortheEKFandtheESapproach,andtheirEKF-estimates, andscanning/perturbationamplitudeforeachrobot,whentherobotsare stationary.Theangularstates: G 2 , G 3 ,andtheirestimatesareaugmentedwith scanningterms V and U toillustrateafaircomparisonwiththeangularstates oftheESapproach.................................51 Figure3.8:Trackingperformanceintermsofaverageintensity I andaverageerror E in simulationoverarangeofdistancesbetweentherobots.Theerrorbarsdenote thestandard-deviation.TheintensityatLOS( I LOS )isalsoshownforreference.53 Figure3.9:Illustrationoftheinitialcon˝gurationofmovingrobots(denotedbyspheres) placed 3 <8= distanceapartinsimulation.Theelevatorrobotmovesupward, andtheroverrobotmoveshorizontallyinadirectionorthogonaltotheline joiningtherobots'initiallocations.........................54 Figure3.10:Trackingperformanceintermsofaverageerror E insimulationoverarange ofspeedsoftherobots...............................54 Figure3.11:Experimentalsetupwithtwomovingrobotsinadarkroom.Thedashed arrowsdenotethemovingdirectionoftherobots.................56 xi Figure3.12:Plotofanexperimentrunillustratingtheevolutionoftheangularstatesand theirestimates(augmentedwithscanningtermsforEKF),intensitymeasure- mentanditsestimate,andscanning/perturbationamplitudeforeachrobot, whentherobotsarestationary...........................56 Figure3.13:Trackingperformanceofthealgorithmsintermsofaveragepointingerror E overarangeofspeedsoftherobotsinexperiments................57 Figure4.1:TwoagentsseekingtoestablishLOSina2Dscenario..............61 Figure4.2:IllustrationofhardwarecomponentsofthetransceiverforLEDcommunica- tion.........................................62 Figure4.3:IllustrationoftheGaussianapproximationofthe˝ttingfunctionsofphoto- diodesensitivitycurve 6 ( ) andLEDintensitycurve 5 ( ) .............63 Figure4.4:Line d x,u withsuper-levelsetsof 8 ( ) ......................67 Figure4.5:Illustrationofthetransitionintervalintermsofmeasurementfunction 8 and itsgradientalongtheline d x,u ..........................68 Figure4.6:Illustrationoftheintersectingtransitionregion D ( u 2 ) forthecontroldirection u 2 .70 Figure4.7:RoadmapoftheproofofTheorem4.2.1......................72 Figure4.8:Illustrationofthegradientsofthetwomeasurementfunctionsandthefour controldirections. \ 1 and \ 2 representtheanglesofthegradientsofthe measurementfunctions 1 and 2 ,respectively,withrespecttothepositive G 1 axis,evaluatedatpoint x : .Theimprovingcontroldirection u 1 isdenoted byathickerarrow.................................74 Figure4.9:Twocon˝gurationsforthecasewhentheimprovingdirectionisbetween therewardfunctiongradientdirections.Thecorrespondingstatemachine diagramisoverlaid,where & representstheimprovingcontroldirection state.Similarly, & , & ˘ and & ˇ arede˝nedrelativelyw.r.t.controldirections andthegradientdirections.............................76 Figure4.10:Con˝gurationwherebothofthegradientsareinbetweentheimprovingdirections.77 Figure4.11:Illustrationoftwosub-casesofthechangeingradientcon˝gurationwhena transitionoccursbetweenstep : and : +1 .....................78 Figure4.12:Illustrationof D ( u 2 ) asaseparateportionof D [ ,andapath ? thatendsin oscillationaround D ( u 2 ) ..............................81 xii Figure4.13:Illustrationofanisolatedportionofset D [ andasamplepath ? .........82 Figure4.14:Illustrationofset D [ forthescenariowhenthemeasurementfunctionsare Gaussiansandcorrespondstothephysicalsetupwith 0 =15 and 1 =14 ...83 Figure4.15:Illustrationofthepathsofsampletrajectoriesforthesystemwithnon- contiguous D .Thetrajectorystartingat ( 1 reachesasubset D 1 whichis notaroundtheorigin.Thetrajectorystartingat ( 2 reachesasubset O around theorigin.Thetrajectorystartingat ( 3 reachesthesubset D 3 andoscillates inthesubsetuntilthenon-zeromeasurementnoisetermsareintroduced.....85 Figure4.16:Evolutionofthestatesofthetrajectoriescorrespondingtothepathsillustrated inFigure4.15....................................86 Figure4.17:Illustrationofthelevelsets,transitionregions,andofasampletrajectoryfor thephysicalsystem.................................87 Figure4.18:Blockdiagramforextremumseekingcontrol...................88 Figure4.19:Comparisonofthetrajectoriesofasamplesimulationrunforthethreealgorithms.89 Figure4.20:Illustrationoftheevolutionofthestatesforthethreealgorithmscorresponding topathsinFigure4.19...............................90 Figure4.21:Illustrationoftheoutputsforthethreealgorithmscorrespondingtothepaths inFigure4.19andstatesinFigure4.20.......................91 Figure4.22:Comparisonofthetrajectoriesofasamplesimulationrunforthethreealgo- rithmsinthepracticalscenario...........................93 Figure4.23:Illustrationoftheevolutionofthestatesforthethreealgorithmsinthepractical scenario.......................................94 Figure4.24:Illustrationoftheoutputsforthethreealgorithmsinthepracticalscenario....95 Figure4.25:Illustrationofperformanceofthealgorithmsintermsofconvergencecount N ˘ andtrackingerror E ˘ overarangeofdisturbancefraction j .Theerror barsfor E ˘ denotethestandard-deviation.....................96 Figure4.26:Illustrationofperformanceofthealgorithmsintermsofconvergencecount N ˘ ,convergencetime T ˘ ,andtrackingerror E ˘ overarangeofmagnitudes ofinitialcondition V .Theerrorbarsfor T ˘ and E ˘ denotesthestandard-deviation.98 Figure4.27:Illustrationofarobotwiththeopticaltransceiversystem.Therobotstands onarotatingdisctoemulatetherelativemotionbetweentworobots.......100 xiii Figure4.28:Illustrationofexperimentalsetupofthetwo-robotsscenario.Theoverhead lightsoftheroomareturnedo˙tominimizetheambientopticalnoise.....101 Figure4.29:Pathofthetrajectoryofasampleexperimentrunwhenthebasediscrotates withtheangularspeedof l =1 € s.........................102 Figure4.30:Evolutionofthesystem'sstatesandtheoutputcorrespondingtothepathof thesampleexperimentrunshowninFigure4.29..................103 Figure4.31:Illustrationofperformanceofthealgorithmsintermsofconvergencecount N ˘ andtrackingerror E ˘ overarangeofdisturbancespeeds.Theerrorbars for E ˘ denotesthestandard-deviation.Thenumberaboveeachofthebar representsthetotalcountofexperimentrunsperformedatthatangularspeed..104 Figure5.1:Hardwaredescriptionoftheactivetransceivermodule...............106 Figure5.2:Illustrationofthetriangular-explorationmethod.................108 Figure5.3:Illustrationofthepathofasimulationrunoftriangular-explorationalgorithm..109 Figure5.4:Illustrationoftheevolutionofstatesandoutputforthesimulationrunof triangular-explorationalgorithmcorrespondingtoFigure5.3...........110 Figure5.5:Illustrationofpathsofthestatesforthethreealgorithmsforeachrobot,fora simulationrunwhentherobotsarestationary...................111 Figure5.6:Illustrationofevolutionofthestatesandoutputforthethreealgorithmsfor eachrobot,forasimulationruncorrespondingtotheFigure5.5.........112 Figure5.7:Trackingperformanceofthethreealgorithmsintermsofaverageerror E and averageintensity I insimulationoverarangeofdistancesbetweentherobots. Theerrorbarsdenotethestandard-deviation.TheintensityatLOS( I !$( )is alsoshownforreference..............................113 Figure5.8:Illustrationoftrackingperformanceofthethreealgorithmsintermsofaverage error E insimulationoverarangeofspeedsoftherobots.............114 Figure5.9:Illustrationofpathsofthestatesforthethreealgorithmsforeachrobot,for experimentrunswhentherobotsarestationary..................115 Figure5.10:Illustrationofevolutionofthestatesandoutputforthethreealgorithmsfor eachrobot,forexperimentrunscorrespondingtotheFigure5.10.........116 Figure5.11:Trackingperformanceofthealgorithmsintermsofaveragepointingerror E overarangeofspeedsoftherobotsinexperiments................117 xiv Figure5.12:Anunderwaterrobotequippedwiththeactivetransceiver............118 Figure5.13:Auni-directionalLED-communicationbasedjoystickcontrollerdesignedfor thiswork......................................119 Figure5.14:Evolutionoflightintensityanddataratesforanexperimentrunoftriangular explorationalgorithmontheexperimentsetup.Thereceiveddata-rateis correlatedwiththesignal-strength.........................120 Figure5.15:UnderwaterrobotbeingcommandedbyahumanoperatorusingtheLED- joystickcontrollerinsideaswimmingpool.....................121 Figure5.16:Setupoftwounderwaterrobotswhicharecommunicatingandactivelyalign- ingwitheachother.................................121 Figure5.17:Illustrationoflightintensitymeasurementsanddataratesforanexperiment runonthesetupshowninFigure5.16.......................122 Figure5.18:Setupoftwounderwaterrobotsforbi-directionalalignmentandcommunica- tionintheswimmingpool.............................123 FigureA.1:Blockdiagramillustratingtherelationbetweencoordinatesystems........129 xv CHAPTER1 INTRODUCTION Withtherecentadvancementsintechnology,theuseofautonomousunderwatervehicles(AUVs) hasbecomeincreasinglypopularforunderwaterexploration[56],withapplicationtomarine sciences,environmentalengineering,andoil/gasexplorationamongothers.Oneessentialattribute oftheserobots,whiletheyaredeployed,istostayconnectedwitheachotherorwithabase stationviaawirelesscommunicationlink.Duetothesubstantialattenuationofradiofrequency signalsinwater[9],acousticcommunicationiscurrentlytheindustrystandardforunderwater communication,witharangeofuptotensofkilometers[53].However,underwateracoustics su˙ersfromshortcomingslikelatency,lowdatarates,andhighpowerconsumption[13].Recently, opticalwirelesscommunicationisemergingasapromisingalternativeorcomplementarysolution totheacousticscommunicationforlow-to-mediumrangedatatransferapplications,duetoits propertiessuchaslowpower,lowcost,andhighdatarate[19,29].Figure1.1illustratesdi˙erent promisingapplicationscenariosofwirelessunderwateropticalcommunicationtechnology. Manyoftherecentworksinopticalcommunicationsystemsfocusedonincreasingthecommu- nicationrangeand/ordatarateusingnarrowbeams(e.g.,laser).Oubei etal .showeda2.3Gbits/s linkoveradistanceof7m[37].Liu etal .demonstrated2.7Gbits/sat34.5musinga520nmgreen laserdiode(LD)[28].Wu etal .useda450-nmblueGaNlaserdiode(LD)directlymodulatedby pre-leveled16-quadratureamplitudemodulation(QAM)toachieve12.2and5.6Gbits/sdatarate atadistanceof1.7mand10.2m,respectively[55]. 1.1LEDOpticalCommunicationandtheProblemofDirectionality Overthepastfewyears,light-emittingdiode(LED)-basedopticalcommunicationhasbeen proposedasapromisinglow-power,low-cost,high-ratesolutionforlow-to-mediumrangeunder- waterdatatransfer[18,19,29].Severalstudiesfocusedonincreasingtherangeanddatarates ofLEDcommunication.BrundagereportedanopticalcommunicationsystemusingaTitanblue 1 Figure1.1: Illustrationofapplicationofunderwaterwirelessopticalcommunicationindiverse scenarios(Source:UniversitàdegliStudi,Italy[42]). lightingLED[8],whichperformederror-freecommunicationover1Mbpsatdistancesupto13m. DoniecandRusdemonstratedabidirectionalunderwaterwirelesscommunicationsystemcalled AquaOpticalII[16],whichused18LuxeonRebelLEDsandanavalanchephotodiode(APD)and operatedoveradistanceof50matadatarateof4Mbps. Aninherentchallengeinwirelessopticalcommunicationisthatlightsignalsarehighlydirec- tionalandthusclose-to-line-of-sightlinksarerequired.Formanyintendedapplicationsinvolving mobileplatforms(inparticular,underwaterrobots),maintaininglineofsight(LOS)isdi˚cult duetoconstantmovementoftheplatformcausedbypropulsionorambientdisturbances.Several approacheshavebeenproposedtoaddresstheline-of-sightrequirementinopticalcommunication systems.Pontbriand etal .increasedthe˝eldofview(FOV)ofthereceiverbyusinglarge-area 2 photomultipliertubes( ˘ 20inch),toavoidtheneedforactivepointingduringcommunication[38]. Anguita etal .implementedatransmitterthatused12LEDsarrangedonacircletotransmit omnidirectionallyintheplane[4],[5].Theytestedtheirsystematarateof100kbpsfordistances ofupto2m.RustandAsada[41]usedahigh-powerLEDand8photodiodesarrangedinacircle tocontrolanunderwaterrobot,wherethesystemwasabletotransmitat100kbpsatadistance of23m.Simpson etal .reportedasystemwherethereceiverhada3-Dsphericalarrayof7 lensesallfocusingona2-Dplanararrayof7photodiodes,andthetransmitterconsistedofa truncatedhexagonalpyramidwith7LEDsand7lenses[44].Mostoftheaforementionedsystems achievedtheline-of-sightthroughredundancyintransmittersand/orreceivers,whichresultedin largerfootprint,highercost,andhighercomplexity. Modulating-retro-re˛ector(MRR)-basedasymmetricpointingsystemshavebeendemonstrated [14,17,21,36],whereanMRRterminalisattachedtoamobilerobot.Galvanic[33,57]andMEMS- based[21]laserscannershavebeenusedattheactivestationarynodesfortheinitialsearchofthe mobileplatform.Theapproachesmentionedaboveworkwellwhenoneofthecommunicating agentsisstationaryandhasaccesstohighpower;however,thiscon˝gurationdoesnotapplytotwo mobilecommunicatingrobots,especiallywhentheyhavelimitedcomputingandpowerresources. Soysal etal. usedquadrantphoto-detector[52]tosimultaneouslyobtainazimuthalandelevation errors,andusedKalmanFiltertopredicttheerrorsandhenceforwardcontrolthealignment. However,aquadrantdetectorisacombinationof4photo-diodesarrangedinaquadrantmanner, whichisalso,inawayredundancyinthehardwareofthesystem.Theoreticalworkinvolving formulationofadoublystochasticspace-timePoissonprocessmodeloflightwasusedtoderive alinear-quadratic-Gaussian(LQG)controllerforactivepointinginthesingle-directional[24]and bi-directional[23]communicationsettings.However,thecontrollerneedsaccesstotheoutputs fromeachunitofa2-dimensionalgridthatisobtainedbypartitioningtheapertureofareceiver photo-diode.Arecentstudy[10]involvesusingextremum-seekingcontrolforalignmentbetween alasertransmitterandaphoto-diodemountedonmobile-robots,wherethepointingmechanismis assistedbyacamerathatcomputedthealignmenterror. 3 1.2OverviewofContributions Thecontributionsofthisresearchresideinthedesignanddevelopmentofanovel,compact LED-basedwirelesscommunicationsystemthatmaintainstheLOSdespitethemovementofthe underlyingplatform.Thenoveltyofthesystemliesinitssimpledesignthatusesonlyasingle photo-diodeandasingleLED.Thesignalfromthephoto-diodeisusedforbothalignmentcontrol andcommunication.Figure1.2illustratestheextractionofsignalstrengthandinformationfrom asignalfromthephoto-diodecircuit.Theoutputfromthephoto-diodeisfedintoavoltage comparatorandalowpassaveraging˝lter.Theoutputofthecomparatoristhenusedasthe receivedsignalforcommunication,whichcontainstheencodedinformationinabitsequence format.Theoutputofthe˝ltergivesanaverageintensitymeasurementthatservesasthesignal strength,whichisusedinalignmentalgorithms. Figure1.2: Illustrationoftheextractionofsignalstrengthandinformationfromopticalsignal incidentonaphoto-diode. Al-RubaiaiandTan[1,2]achievedactivealignmentcontrolusingsinglepairofLEDandphoto- diode,eachofwhichwasmountedonabasethatcouldrotateindependentlyoftheunderlying roboticplatform.Thissystemalsousedredundancyintimebytakingthreemeasurementsat 4 threedi˙erentorientationsandthencomputingadesiredorientationbasedontheinterpolation ofthemeasurements.Thisalignmentcontrolapproachisrudimentaryandrequiresabruptand largerotationofthecommunicationdeviceforthesignalstrengthprobing,andthusisnotenergy- e˚cientandcreatesunwantedmechanicalvibrations.Inthiswork,weuseaprincipledapproach foractivealignmentwherethesystemoftransmitterandreceiver˝rstismodeledasadynamical system.Multiplealignmentcontrolalgorithmsarethendevelopedforthesystemforscenariosthat rangefromasimpleone-waytwo-dimensional(2D)settingtoapracticalthree-dimensional(3D) bi-directionalunderwatersetting.Followingarethedetailsofthecontributionsofthiswork. 1.2.1AlignmentControlinthe2DSpace Firstly,westartwithatwodimensionalsetting,wherethereceiverandthetransmittercanonly moveonaplane.WeproposeanextendedKalman˝lter(EKF)forestimatingtheanglebetween thereceiverorientationandthelineconnectingthereceiverandthetransmitter,whichisthenused toadjustthereceiverorientationtowardstheLOS.WenotethatKalman˝lterandEKFhavebeen proposedinopticalbeamsteeringinthecontextoflaser-basedfreespaceopticalcommunication, wherethelaserbeamisconsideredasasinglelineandthussimplegeometricrelationshipscanbe usedtorelatethemeasurementtothereceiver/transmittercon˝guration[52,57].Forexample,in theirsimulationstudy,SoysalandEfeconsideredaquadrantphoto-detectorasthemeasurement device,whichwasassumedtoproducesignalsdirectlyproportionaltoazimuthandelevation errors[52].YoshidaandTsujimurausedatwo-dimensionalposition-sensitivedevice(PSD)and thedetectedbeamspotpositionwasgeometricallyrelatedtotherelativepositionandorientation betweenthetransmitterandthereceiver[57].Theseapproachesdonotapplytooursettingbecause ofthedi˙usivenatureofLEDandtheuseofsinglephoto-detectorintheproposedwork. Wenowbrie˛ysummarizeourEKF-basedalignmentcontrolapproach.Basedonalight intensitymodel,we˝rstformulateanestimationproblem,wherethereceiverestimatesbothits relativeorientationtothetransmitterandaquantityrelatedtotheoveralllightintensityatthe receiversite.Therotatingbaseistheninstructedtomovetowardsalignmentbasedontheestimated 5 relativeorientation.Duetothenonlinearnatureoftheobservationfunction,anEKFisadopted forthestateestimation.InordertoensureconvergenceoftheEKF,lightintensitymeasurements takenattwoconsecutivestepsinthescanningmotionareusedineachstateupdate.Thefeedback controlalgorithmthenupdatestheorientationbiasintheangularscanmotionbasedonthestate estimate. Preliminaryversionsofpartsofthisworkwerepresentedatthe2016AmericanControl Conference[45]andthe2016IEEEInternationalConferenceonAdvancedIntelligentMechatronics [48].MorecomprehensiveresultswerepublishedinIEEE/ASMETransactionsonMechatronics [46],wherethetrackingperformanceoftheEKFapproachwascomparedwithtwoalternative schemes,hill-climbingandthree-pointaveragingis,inthepresenceofthemeasurementnoise. Hill-climbingisawidelyused,computationallye˚cientalgorithmforoptimizationthatlocally updatesthesolutioninthedirectionofhigherobjectivefunction[40].Sincebetteralignment betweenthereceiverandtheLEDleadstohighermeasuredlightintensity,thehill-climbing algorithmsimplydirectsthereceivertokeepmovinginthedirectionofhigherlightintensity. Thethree-point-averagingalgorithm[1]computesthenextorientationofthereceiverbasedonthe weighted-averageofthreeorientations:nochange,a˝xedrotationtotheright,anda˝xedrotation totheleft,wherethemeasuredintensitiesattheseorientationsareusedasweights.Aperformance metricisdesignedtoevaluateandcomparethethreealgorithmsintermsoftrackinge˙ectiveness, wherearangeofmeasurementnoiselevelsisconsidered.Foreachofthenoiselevels,multiple runsofsimulationandthecorrespondingexperimentshavebeenperformedtoassesstheaverage performanceandsimultaneouslyalleviatethee˙ectsofstochasticityontheresults.Itisfoundthat theEKFalgorithmsigni˝cantlyoutperformsthealternativesinthepresenceofmeasurementnoise. Chapter2describesthedetailsofthispreliminarywork. 1.2.2ActiveAlignmentControlin3DSpace Sinceinarealisticunderwaterscenario,therobotsmoveinathree-dimensional(3D)space,and eachneedstotransmitandreceive.Hence,weproposeabidirectionalactiveLOS-alignmentsystem 6 formobilerobotsmovingina3Dscenario.Inthissetup,eachrobotisequippedwithatransceiver, consistingofaphoto-diodeandanLED,attachedtoatwo-degree-of-freedom(DOF)rotational system.WeproposeanextendedKalman˝lter(EKF)-basedalignmentcontrolapproach,where theestimatesofazimuthalandelevationanglesbetweenthetransceiverorientationandtheLOS directionareusedtoadjustthetransceiverorientationtowardstheLOS.Duetotheuseofasingle photo-diode,onlyasinglemeasurementisavailableatatime,whichisnotsu˚cienttoestimate allthestatesofthesystem.Toaddressthisissue,weproposeacircularscanningtechniquethat ensuresthesystemobservabilitybytakingthreeconsecutivemeasurementsoflightintensityfrom non-coplanardirections. Oncetheestimateoftherelativeanglebetweeneachagent'sorientationandtheLOSisknown, aproportional-integral(PI)controlalgorithmisthenusedtogeneratethecontrolinputstodrive theorientationofthetransceivertowardstheLOS.Thesecontroltermsaretranslatedintothe correspondingmotorcommandsfortherotationalsystem.Additionally,toincreasesignalstrength atthesteady-stateandtosaveenergy,theamplitudeofthecircularscanningisadjustedbasedonthe estimationcovariance.Furthermore,toenabletwo-wayalignment,theaforementionedapproach isalternatedbetweentherobots,whereeachrobottakesturnstoconductitscircularscanwhile theotherrobotispaused.Thee˚cacyoftheproposedmethodisevaluatedinbothsimulationand experiments,wherewealsoimplementanextremum-seeking(ES)approach[25]forcomparison withtheproposedapproach.TheESapproachiswellstudiedforreal-timeoptimization,anddueto theunimodalnatureoflight-intensityfunction(see(3.2)andFigure3.3fordetails),itisapplicable tooursetupandisthuschosenasthebenchmarkalgorithmforcomparison.Thesimulation resultsdemonstratethefunctionallimitationoftheES-basedapproach,asitbecomesunstableat lowdistances.Furthermore,weperformsimulationsandexperimentsforthescenariowherethe robotsmovewitharangeofrelativespeeds.Theresultsvalidatethee˚cacyofourapproachat relativelylowspeedsbetweentherobotsandillustratethechallengeswhentherelativemotiongets pronounced. ThedetailsofthisworkareprovidedinChapter3.Simulationresultsoftheproposedapproach 7 werepresentedatthe2017ASMEDynamicSystemsandControlConference[51].Amore re˝nedapproachwithexperimentalresultswaspresentedat2018IEEEInternationalConference onRoboticsandAutomation[49]. 1.2.3Bi-directionalAlignmentApproachfor2DSpace TheEKFapproachusesanassumptionthattherelativemotionbetweentherobotsissmall,and consequently,requiresthecommunicatingrobotstotakethescanninginanalternatingfashion fortheconvergenceoftheestimator.Inthiswork,wedemonstratebi-directionalactivebeam trackingbetweentwopartiesina2Dsetting.ItisdesirabletoachieveLOSwithoutrelyingon communicationbetweentheagentsasthequalityofcommunicationlinkitselfdependsonthe LOS.Thisworkproposes,analyzes,andevaluatesahill-climbingbasedcomputationallye˚cient schemefortwoagentsinaplanetoachieveandmaintainLOSfromarbitraryallowableinitial con˝gurations. Inthisworkweconsidertheopticalalignmentquestionasatwo-agentcooperative-control problem.Eachoftheagentsisassumedtohavealocalmeasurementofitsoutputfunctionthat denotesthereceivedopticalpowerintermsofstates G 1 and G 2 oftheunderlyingsystem,with G 8 Œ8 2f 1 Œ 2 g ,denotingtheheadingorientationofanagent 8 .Thetwooutputfunctionsarenon- con˛icting;bothhavetheglobalmaximumatorigin,whichcorrespondstotheLOScon˝guration. Basedonthesetup,theagentsdonothaveaccesstotheirownorientations.Furthermore,sincethe communicationreliesontheLOS,thealignmentalgorithmassumesthattheagentsdonothaveany communicationwitheachother.Additionally,theagentsactindependentlyandsimultaneouslyat eachtimestep,whicheliminatescandidatesolutionsbasedonsequentialactionsoftheagentsthat cangreedilyoptimizetheirinstantaneousoutputfunctions.Weproposeacomputationallye˚cient hill-climbingbasedcontrolschemetoupdatetheheadingorientations G 1 and G 2 oftheagents,which onlyusesthecurrentandpreviousmeasurementsobtainedbyanagent.Furthermore,foraparticular case,whenthemeasurementfunctionscanbeapproximatelycharacterizedinaGaussianform,the proposedschemeguaranteesthattheheadingsoftheagentsarewithinaspeci˝edneighborhoodof 8 theLOScon˝gurationwithina˝nitenumberofsteps. Theprobleminthisworkcanbeconsideredasaspecialcaseofamulti-agentoptimization probleminwhicheachagentseekstooptimizetheirowncost(reward)functionthatdepends onthestateoftheotheragents.Severalreportedapproachesbasedondistributedoptimization relyonassumptionsofthelocalcostfunctionsbeingconvexandtheagentscommunicatingtheir states/estimateofglobalstatevectorsasperatopology[30,34].Severalapproachesrelatedto distributedsensingandestimationwithcooperativecontrolinmulti-agentsystemsarereported in[11,20,26,35].Collaborativesourceseekingviacircularformationcontrolisreportedin[7,32], whereagentssharetheirmeasurementsandlocationswiththeirneighborstoestimatethegradient oftheunderlyingsignalpro˝letosteertheformationtothesourceofthesignal.Passivity-based toolshavealsobeenstudiedformulti-agentsynchronizationandextremum-seekingproblems [6,12].However,theseaforementionedworksrequireconnectednetworkwithinformationexchange betweentheagentsor,incertaincases,accesstothegradientofthelocalcostfunctionwithrespect tothestate.Inmulti-agentgame-theoreticformulations,gradientplayisapopulartechniquethat convergestoaNashequilibriumforthegameundermildtechnicalassumptions[27,43].However, thesetechniquesrequirethateachagenthasaccesstothegradientofitsowncostfunction. Thecontributionsofthisworkarethree-fold.First,weproposeacomputationallye˚cientnovel localcontrollawforaclassofsystemsthatencompassesourfree-spaceopticalcommunication experimentalsetup,andonlyrequirestheinformationofthecurrentandtheimmediatelypreceding rewardfunctionmeasurements.Second,undercertainassumptionsontheformofthemeasurement functions,weprovethat,fromanyadmissibleinitialvaluesofthestates,theproposedcontrollaw ensuresthatbothagentsreachaspeci˝edlimitingset,whichcontainstheglobaloptimal,ina˝nite numberofsteps(cf.Theorem4.2.1foradditionaldetails).ForthechoiceofGaussianmeasure- mentfunctionsderivedfromoursetup,weobtainstrongerconvergenceresults(cf.Theorem4.2.2). Third,theapproach'se˙ectivenessisevaluatedinsimulationandexperiments,wherethealgorithm istestedwithrelativemotionbetweentheagents.Simulationresultsdemonstratethesuperiority oftheproposedapproachintermsofconvergencespeed,robustnesstounknowndisturbances,and 9 handlinglargeinitialconditionsagainsttwoalternativeapproaches.A2Dversionofextremum- seeking(ES)controlalgorithm[25]isusedasabenchmarkapproachforcomparingtheproposed methodinthispaper.Thealgorithmexhibitsrelativelyslowerconvergenceduetosigni˝canttime spentinexplorationbyeachagent.TheotherapproachconsideredforcomparisonistheEKFbased alignmentapproach[46].However,theEKFformulation'sassumptionsofquasi-staticdynamics areviolatedinthebi-directionalsetting,andconsequentially,thealgorithmfailstoconvergefrom arbitrarilylargeinitialconditions. Preliminaryresultsofthisworkwerepresentedatthe2020IEEEInternationalConferenceon AdvancedIntelligentMechatronics[47],wherethespecialcaseofGaussianformmeasurement functionswasconsideredandthestudieswerelimitedtosimulationsetting.Thedetailsofthis workareprovidedinChapter4. 1.2.4Bi-directionalAlignmentApproachfor3DSpaceandExperimentsinUnderwater Scenario Inthiswork,weextendtheapproachtobi-directionalactivebeamtrackingbetweentwopartiestothe 3Dscenario.Inspiredbytheaforementionedmodel-freebidirectionalapproachforthe2Dsetting, wheremaximizingownmeasurementsbyeachagentleadstoconvergencetoaneighborhoodofthe LOS,weproposeanoveltriangular-explorationalgorithmwhereanagentcontinuouslymaximize itsownlocallightintensitymeasurements,toachievetheLOSwiththeothercommunicatingagent inthe3Dsetting. Theproposedalgorithmmovesthetransceiverpointingdirectioninanequilateraltriangular gridpatternandguaranteesthepointingdirectiontobeconsistentwiththelocalgradientdirection. Themethodonlyrequiresthelightintensitytobeaunimodalfunctionoftheangleo˙setsfrom theLOS,anddoesnotrequireanexplicitmodelforimplementation.Furthermore,theapproach worksdirectlyforthesettingofbi-directionalcommunication.Thee˙ectivenessoftheapproach is˝rstevaluatedinasimulationsetupoftworobotsbycomparisonwiththeEKF-basedapproach andtheESapproach.Simulationresultsshowthattheproposedapproachise˙ectiveinterms 10 ofitsconvergencespeedforawiderangeofrelativespeedanddistancebetweentherobots.The performanceoftheapproachisfurtherassessedagainsttheEKFandtheESapproachonan experimentalsetup,wheretheresultsfurthersupportthesuperiorityoftheproposedapproach. Thee˚cacyoftheapproachandtheoverallcommunicationsystemisfurtherdemonstrated intheunderwatersettingwherecommunicationisperformedsimultaneouslywiththealignment control.First,one-waycommunicationandalignmentisdemonstratedonasetupwhereahuman operatorwirelesslycontrolsarobotintheunderwaterscenariousinganLEDcommunication- basedjoystick,followedbydemonstrationofbi-directionalalignmentcontrolandcommunication betweentwounderwaterrobots.Preliminaryresultsfortheuni-directionalsetupwerepresentedat 2020IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems(IROS)[50].Chapter5 providesthedetailsofthiswork. 11 CHAPTER2 EKF-BASEDALIGNMENTCONTROLINTHE2DSPACE Inthischapter,wepresentanovel,compactLED-basedcommunicationsystemwithactivealign- mentcontrol,inatwo-dimensional(2D)setting,thatmaintainstheLOSdespitetheunderlying platformmovement.AnextendedKalman˝lter-basedalgorithmisproposedtoestimatetheangle betweenthereceiverorientationandthereceiver-transmitterline,whichisusedsubsequentlyto adjustthereceiverorientation.Thealgorithmusesonlythemeasuredlightintensityfromasingle photodiode,wheresuccessivemeasurementsareobtainedviaascanningtechniquethatalsoensures theobservabilityofthesystem.Asimpleproportionalcontrollerisdesignedforalignment.Theef- fectivenessoftheproposedactivealignmentalgorithmisveri˝edinsimulationandexperiments.In particular,itsrobustnessinthepresenceofmeasurementnoiseisdemonstratedviacomparisonwith twoalternativealgorithmsthatarebasedonhill-climbingandthree-point-averaging,respectively. Theorganizationofthechapterisasfollows.InSection2.1,thedesignandhardwareim- plementationoftheLEDcommunicationsystemisdescribed.InSection2.2themodelforthe receivedlightintensityispresented,followedbyastate-spacereformulationforthepurposeofal- gorithmdevelopment.InSection2.3theestimationandtrackingcontrolalgorithmsaredescribed. SimulationsetupandresultsarepresentedinSection2.4,whileexperimentalsetupandresultsare discussedinSection2.5.ChaptersummaryisprovideinSection2.6 2.1SystemDesignandImplementation AnLED-basedopticalcommunicationsystemmainlyconsistsoftwoparts,thetransmitterand thereceiver.Thetransmitterconvertstheelectricalsignalintoanopticalsignal.Thatsignalpasses throughthemediumandispickedupbythereceiver.Thereceiverdetectstheopticalsignaland convertsitbackintoanelectricalsignalfordataprocessing.Inadditiontothetransmitterand thereceiver,theproposedsystemincludesamechanismforrotatingthetransmitter/receiver,to maintaincommunicationdespitethemovementoftheunderlyingroboticplatform.Considering 12 theintendedapplications,smallfootprintandlowpowerconsumptionareamongthemajordesign constraints. Theroleoftheopticaltransmitteristoconverttheelectricalsignalintolightpulses.Since thesignalattenuationunderwaterisminimuminthewavelengthrangeof400-500nm[54],an o˙-the-shelfblueLED(CreeXR-ESeriesLEDfromCreeInc)ischosen.whichprovides30.6 lumensat1Aandrequires3.3volts.Itcomesassembledwithaheatsink.Acircuitisdesignedto modulatetheLED(turningitonando˙)incorrelationwithbinarydata[2].Aphotodiodefrom AdvancedPhotonix(partnumberPDB-V107)ischosenforthereceiver,andithashighquantum e˚ciencyat410nm,lowdarkcurrent,andfastrisetime(20ns).A12Vreversebiasacross thephotodiodeisusedtoincreasethebandwidthandquantume˚ciency[3].Atrans-impedance ampli˝erisusedtoconvertthephotodiodecurrentsignalintoavoltagesignal,whichthengoes througha˝lterfornoisereduction. Thecomponentsofthetransmitterandthereceiverareplacedontwoprinted-circuitboards (PCBs).The˝rstPCBboardis2inchesindiameterandhastwoholesinthemiddletoattachset screwhubsforconnectingtoamotorshaft.ThesecondPCBboardhasarectangularshapewith sizeof1inch 2inch,whichholdstheLEDandthephotodiode,anditismountedperpendicularly tothe˝rstcircularboardbyusingfour90-degreeheaderpins(seeFigure2.1). Thereare8pinsinthePCBcircuitsinvolvingthepowersupply,thetransmittedsignal,andthe receivedsignal.Thesepinsareconnectedbywires,whichwouldbetwistedwhenthePCBsare rotated.Toaddressthisproblem,aslipring(MT007fromMOFLON),anelectromechanicaldevice thatallowsthetransmissionofpowerandelectricalsignalsfromastationarytoarotatingstructure, isadopted.Amotorisusedtorotatethedevice.WeinitiallyusedaminiDCmotorequippedwitha shaftencoder,butlaterswitchedtoasteppermotorduetothehighercontrolprecisionofthelatter. 2.2Modeling Inthissection,we˝rstreviewalightintensitymodelandthenformulatethestate-spacemodel foranestimationproblem,where,withoutthelossofgenerality,ascenariooftworobotsis 13 Figure2.1: AprototypeofLEDopticalcommunicationmodulewitharotationalbase. 14 considered.Inaddition,inthiswork,weconsiderthatthecommunicatingpartiesareonthesame plane. 2.2.1LightIntensityModel Themodeladoptedherelargelyfollows[15]withminoradjustmentstosuittheexperimental prototypeusedinthiswork.Themodeltakesintoaccountallstagesofthetransmitterandreceiver circuits,includingLED,lens,photodiode,andampli˝ers.Themodelmainlydescribesthee˙ectof relativepositionandorientationbetweenthetransmitterandthereceiveronthesignalstrength.See Figure2.2foranillustrationofthevariablesofinterest,includingtransmissionangle W ,transmission distance 3 andtheangleofincidence q . Figure2.2: Illustrationoftherelativepositionandorientationbetweenthetransmitterandthe receiver. ThetransmitterLEDhasanangularintensitydistributionwhichisrotationallysymmetricabout theLED'snormal( W =0 ).SoifweknowtheintensityoftheLEDalongthenormal,wecan computetheintensityatotherpointsatthesameradialdistancebasedonspatialintensitycurve ˚ W ,whichrepresentsthelightintensityataunitdistancefordi˙erenttransmitterangles. ˚ W is maximumat W =0 ,anditrollso˙as W increases.Typically, ˚ W canbeobtainedeitherdirectly fromtheLEDvendorormeasuredexperimentally. 15 TodescribetheextinctionofthelightsignalwewilladoptBeer'sLaw[31],whichisused inunderstandingtheattenuationinphysicaloptics.Let 2 betheattenuationcoe˚cientforthe mediuminwhichthelighttransmits.Weassumethatthecoe˚cientisuniformacrosstheentire lengthoftransmission.Beer'slawgivestheexponentialsignaldegradationatdistance 3 causedby absorption: = 4 23 (2.1) Bycombiningthee˙ectofsphericalspreadingwithexponentialdecay,wegettheequationof theirradiancereachingthereceiversite: ˆ W ( 3 )= ˚ W 4 23 €3 2 (2.2) where ˚ W denotestheangularintensitydistributionoftransmitter(combinationoftheLEDand thelens),whichcharacterizesthelightsignalstrengthfordi˙erenttransmissionanglesataunit distance.Finally,weneedtoconsiderthee˙ectofangleofincidence q ,whichisbasicallytheangle madebythereceivernormalwiththelineconnectingthereceivertothetransmitter.From[2], thepowerincidentonthedetectorcanbecomputedbasedonthesignalirradianceatthedetector position: % in = ˆ W ( 3 ) 0 6 ( q ) (2.3) where 0 denotesthedetectorareaand 6 ( q ) characterizesthedependenceofthereceivedlight intensityontheincidenceangle q .Theterm 6 ( q ) issetup-dependent.Forthereceiverusedinthis work,wehavefoundthefunction 6 ( q ) usingGaussiancurve˝ttingofthenormalizedmeasurement data(Figure2.3)collectedatdi˙erentorientationsofthereceiver.Theresulting 6 ( q ) takesthe formofabimodalGaussianfunction: 6 ( q )= 0 1 4 ( q 1 1 2 1 ) 2 + 0 2 4 ( q 1 2 2 2 ) 2 (2.4) where 0 1 =0.6682, 1 1 =7.752, 2 1 =148.8, 0 2 =0.3340, 1 2 =-13.57, 2 2 =325.8arethecurve ˝ttingparameters.Theparameters 1 1 and 1 2 arerelativelyclosetoeachother(overtherangeof 16 Figure2.3: Gaussiancurve˝ttingforthefunction 6 forthephotodiodeusedinthiswork. [ 180 Œ 180 ] ),sotheresultingsumofthetwomodeshasasinglepeak,asshowninFigure2.3. Thecurve˝ttingcouldbedoneusingasingleGaussianmodebuthavingoneextraGaussianmode givessigni˝cantlybetter˝tting. Asthelightarrivesonthereceiverphotodiode,thephotodiodeproducesacurrent,whichgets ˝lteredandampli˝ed,tobeprocessedbyananalog-digitalconverter.Afterallthestages,thefull signalstrengthmodelcanbesummarizedas + 3 = ˘ ? ˚ W 4 23 6 ( q ) €3 2 (2.5) where + 3 isthevoltagesignaland ˘ ? isaconstantofproportionality,whichdependsonthearea ofreceiverphoto-diodeandvariousparametersassociatedwiththe˝lterandampli˝ercircuits. 17 2.2.2State-spaceProblemFormulation FromFigure2.2andEq.(2.5),wecanseethattherearethreeindependentvariables, W , 3 and q , thatcharacterizethereceivedlightintensity.Onecouldtakethesethreevariablesasthestatestobe estimatedbythesystem,andthentrytodrivethemtowardstheirdesiredvaluesthroughcontrol,if thatispossible.However,oftentimestheunderlyingroboticplatformsareengagedinothertasks andmaynotconstrainormodifytheirmotionstoaccommodatecommunication.Forexample,it maynotbepossibletochangedistance 3 inadesirablewayforcommunicationsincethatwould involvethemovementoftherobots.Whatismuchmorepracticalistocontrolthereceiverangle q ,sinceitisacompletelylocaldecisionduetotheindependentrotationbaseforthetransceiver.In atwo-waycommunicationsetting,sincethetransmitterandthereceiveroneachrobotarepointing inthesamedirection,adjusting q tozerooneachrobotautomaticallyalignseachtransmitterwith thelineconnectingtworobots.Inlightofthisdiscussion,wecancombinetermsinvolving W and 3 inasinglevariableandde˝nethestatevariablesas x 4 = " G 1 G 2 # 4 = " ˘ ? ˚ W 4 23 €3 2 q # (2.6) Thevalueof G 1 isdependentonthedistanceandthetransmissionangle.Inatypicalscenario, thereceiverdoesnothaveinformationabouthowthetransmitteranditsunderlyingroboticplatform move.Soinourcase,wewillassumethattherelativedynamicsbetweenthetwocommunicating robotsisslowenough(quasi-static)thatitcanbecapturedwithaGaussianprocess.Inparticular, thedynamicsofthestatesde˝nedin(2.6)canberepresentedinthediscrete-timedomainas x : 4 = " G 1 Œ: G 2 Œ: # = " G 1 Œ: 1 + F 1 Œ: 1 G 2 Œ: 1 + D : + F 2 Œ: 1 # (2.7) where : isthetimeindex,and F 1 Œ: and F 2 Œ: aretheprocessnoises,assumedtobeindependent, white,Gaussiannoises.Thesenoiseterms,tosomeextent,accountfortheslowdynamicsof G 1 and G 2 ,whicharenotmodeledexplicitly.Theterm D : isthecontrolinputthroughwhichthereceiver angleischanged. 18 The : thmeasurement + 3Œ: canbeexpressedintermsofthestatevariables,whereanadditive whiteGaussiannoise E : ,assumedtobeindependentfromtheprocessnoises,isincluded: + 3Œ: = G 1 Œ: 6 ( G 2 Œ: )+ E : (2.8) Giventhemeasurement,thegoalistoestimate G 1 Œ: and G 2 Œ: ,basedonwhichthecontrol D : is designedtodrive G 2 towards 0 ,whichistheorientationwiththemaximumlightintensity. 2.3EstimationandAlignmentAlgorithms Giventhatthemeasurementmodel(2.8)isnonlinear,adiscretetimeextendedKalman˝lter (EKF)[39]isexploredforsolvingtheestimationproblem.Fromthe(linear)stateequation(2.7), the and matricesare: 8 > > > > > > >< > > > > > > > : = " 10 01 # = " 0 1 # (2.9) Sothatthesystemdynamicscanbewrittenas: x : = x : 1 + D : 1 + w : 1 (2.10) with w : = F 1 Œ: ŒF 2 Œ: ) .Giventhattheoutputfunctionin(2.8)isnonlinear,denotingthesystem's linearizedoutputmatrixat k thtimeinstantas ˘ : , ˘ ( x : ) ,onecanexpresstheobservabilitymatrix atthattimeinstantas[39] O : = " ˘ : ˘ : +1 # = " ˘ : ˘ : +1 # (2.11) Iftheobservabilitymatrix O : hasafullrankof2,thestateestimationerrorundertheEKF willbeexponentiallyboundedinmeansquareandboundedwithprobabilityoneunderproper conditions[39].Asu˚cientconditionfor O : tobefullrankistomake ˘ : arank2matrix,whichis onlypossiblewithatleasttwoindependentmeasurementsofthelightintensity.Onecouldusetwo receiverswithdi˙erentorientationstoaddressthisproblem,butthatwouldincreasethecomplexity andcostofthesystem.Instead,weintroduceascanningtechnique,wherethemotoroftherotating 19 baseiscommandedtooscillatearoundameanposition.Thismeanposition,whichiswhatthe controlinputmodulates,isconsideredtobethestatevariable G 2 fromhereon. Figure2.4providesanoutlineoftheproposedmethod.Ateachiteration,thestates G 1 and G 2 areupdatedaccordingtothesystemdynamics.Thescanningtermisaddedtoaccountforthe actualorientationofthereceiver.ThelightintensitymeasuredbythereceiverisusedbytheEKFto updatethestateestimates.Next,theestimate ^ G 2 isusedtocomputethecontrolterm.Wenotethat thefocusofthisworkisontheuseofnonlinearestimationandbasicfeedbackconceptstoenable activealignmentbetweenthereceiverandtheLED.Therefore,explorationofadvancedcontrollers isbeyondthescopeofthiswork;forsimplicityofimplementationandpresentation,aproportional controllerisadopted.The˝nalcommandsenttothemotoristhesumofcontroltermandthe di˙erencebetweenthelasttwoconsecutivescanningterms. Figure2.4: Blockdiagramillustratingtheproposedmethod. Figure2.5illustratesthescanningtechnique.Thereceiveroscillatesthroughade˝nedarrayof angles = f k 1 , k 2 , k 3 ... k = g ,whichcontainsprede˝nedanglesusedforscanning.Inourcase 20 Figure2.5: Illustrationofthereceiverscanningsequence,withmean G 2 andlasttwoanglesof scanning k : and k : 1 . = f 2 , 4 , 6 , 8 , 10 , 8 , 6 , 4 , 2 , 0 , 2 , 4 , 6 , 8 , 10 , 8 , 6 , 4 , 2 , 0 g . Ineachiteration,one k : ischosenfromthisarraysequentially.Themeasurementistakenateach k : ,andthelasttwomeasurementsat k : and k : 1 formouroutputvector y : y : = " G 1 Œ: 6 ( G 2 Œ: + k : )+ E : G 1 Œ: 1 6 ( G 2 Œ: 1 + k : 1 )+ E : 1 # (2.12) Usingthedynamicsequation(2.7)andthemeasurementequation(2.12),anEKFcanbeimple- mented.Thecompletealgorithmisexplainedasfollows. Therearethreecovariancematrices,namely, % , & and ' ,associatedwithanEKF. % isthe conditionalerrorcovariancematrixand % 5 representstheforecastofthecovariancematrix. % 5 needstobeinitializedasapositivede˝nitematrix.Theinitialvalueoftheestimateof G 2 can betakenas 0 .Theinitialvalueoftheestimateof G 1 dependsonthemaximumpossiblevalue ofintensity.Agoodchoiceoftheinitialestimate ^ G 1 wouldbefrom 1 € 3 to 2 € 3 ofthemaximum 21 intensityvalue. & istheprocessnoisecovariancematrix,and ' isthemeasurementnoisecovariance matrix.Atstep : , 1. Predictionphase:Bothstateestimates( ^ G )anderrorco-variancematrix( % 5 )arepredicted: ^ x 5 : 4 = 2 6 6 6 6 4 ^ G 5 1 Œ: ^ G 5 2 Œ: 3 7 7 7 7 5 = " ^ G 1 Œ: 1 ^ G 2 Œ: 1 + D : 1 # (2.13) % 5 : = % : 1 ) + & (2.14) where ^ G 5 <Œ: denotestheestimateofthe < thstateat : thintervalandthesuperscript 5 stands for`forecast'. 2. Estimatedoutput:From(2.12),theestimatedoutputcanbewrittenas ^ y : 4 = " ^ H 1 Œ: ^ H 2 Œ: # 4 = " ^ G 5 1 Œ: 6 (^ G 5 2 Œ: + k : ) ^ G 1 Œ: 1 6 (^ G 2 Œ: 1 + k : 1 ) # (2.15) With(2.13),onecanwrite ^ y : = (^ G 5 1 Œ: Œ ^ G 5 2 Œ: ) , 2 6 6 6 6 4 ^ G 5 1 Œ: 6 (^ G 5 2 Œ: + k : ) ^ G 5 1 Œ: 6 (^ G 5 2 Œ: D : 1 + k : 1 ) 3 7 7 7 7 5 (2.16) Theterm D : 1 wouldbeexpressedintermsofthestatevariableslater.Nowthelinearized observationmatrix ˘ : canbecomputedas: ˘ : = m (^ G 5 1 Œ: Œ ^ G 5 2 Œ: ) m ^ x 5 : = " ˘ :Œ 1 Œ 1 ˘ :Œ 1 Œ 2 ˘ :Œ 2 Œ 1 ˘ :Œ 2 Œ 2 # (2.17) where, ˘ :Œ 1 Œ 1 = 6 (^ G 5 2 Œ: + k : ) ˘ :Œ 1 Œ 2 =^ G 5 1 Œ: 6 0 (^ G 5 2 Œ: + k : ) ˘ :Œ 2 Œ 1 = 6 (^ G 5 2 Œ: D : 1 + k : 1 ) ˘ :Œ 2 Œ 2 =^ G 5 1 Œ: 6 0 (^ G 5 2 Œ: D : 1 + k : 1 ) 22 with 6 0 ( ) beingthederivativeof 6 ( ) withrespecttoitsargument. 3. Update/analysisphase: : = % 5 : ˘ ) : ( ˘ : % 5 : ˘ ) : + ' ) 1 (2.18) ^ x : = ^ x 5 : + : ( y : ^ y : ) (2.19) % : =( ˚ : ˘ : ) % 5 : (2.20) Itistobenotedthat % : and ^ x : withoutanysuperscriptsdenotetheupdatedvaluesafterthe analysisphase. 4. Finally,thecontroliscomputedas D : = ˝ ^ G 2 Œ: (2.21) where ˝ isapositivegain,which,ismotivatedbythegoalofdrivingthemeanofscan G 2 tozero.The˝nalrotationanglesenttothemotoris D : + k : +1 k : ,whichwillbeusedto updatethereceiverangleattime : +1 . SincethealgorithmisbasedonEKF,theconvergencedependsmainlyontwofactors:the fullrankconditionoftheobservabilitymatrix O : ofthelinearizedsystem(recallEq.(2.11),and theinitialconditionsofthestateestimates,whichwerealreadydiscussedearlier.Sincethefull rankconditionof O : isensuredbythenon-singularityoftheoutputmatrix ˘ : ,weconsiderthe determinantof ˘ : j ˘ : j =^ G 5 1 Œ: 6 (^ G 5 2 Œ: + k : ) 6 0 (^ G 5 2 Œ: D : 1 + k : 1 ) ^ G 5 1 Œ: 6 (^ G 5 2 Œ: D : 1 + k : 1 ) 6 0 (^ G 5 2 Œ: + k : ) Using(2.13)and(2.21),weobtain D : 1 = ˝ ^ G 5 2 Œ: 1 ˝ whichimplies ^ G 5 2 Œ: D : 1 = ^ G 5 2 Œ: 1 ˝ 23 andthus j ˘ : j =^ G 5 1 Œ: 6 (^ G 5 2 Œ: + k : ) 6 0 ( ^ G 5 2 Œ: 1 ˝ + k : 1 ) ^ G 5 1 Œ: 6 ( ^ G 5 2 Œ: 1 ˝ + k : 1 ) 6 0 (^ G 5 2 Œ: + k : ) Since G 1 representsthelightintensityatthereceiversite(whichisingeneraldi˙erentfromthe measuredintensitybythereceiver),itisalwayspositiveif G 1 werezero,therewouldnotbeany measuredsignalevenifthereceiverisperfectlypointingatthetransmitterandthealgorithmwould bestopped.Therefore,itisreasonabletoassume ^ G 5 1 Œ: ¡ 0 .Sotheonlypossibilityfor j ˘ : j =0 is then 6 (^ G 5 2 Œ: + k : ) 6 0 ( ^ G 5 2 Œ: 1 ˝ + k : 1 ) 6 ( ^ G 5 2 Œ: 1 ˝ + k : 1 ) 6 0 (^ G 5 2 Œ: + k : )=0 or 6 0 (^ G 5 2 Œ: + k : ) 6 (^ G 5 2 Œ: + k : ) = 6 0 ( ^ G 5 2 Œ: 1 ˝ + k : 1 ) 6 ( ^ G 5 2 Œ: 1 ˝ + k : 1 ) (2.22) Onecanshowthatthefunction G = 6 0 ( ) 6 ( ) ismonotonouslydecreasinginourdomainofinterest: ( 180 Œ 180 ).Hence(2.22)wouldbetrueifandonlyif ^ G 5 2 Œ: + k : = ^ G 5 2 Œ: 1 ˝ + k : 1 (2.23) whichimplies ^ G 5 2 Œ: = (1 ˝ ) ˝ ( k : k : 1 ) (2.24) Since j ^ G 5 2 Œ: j 180 and j k : k : 1 j =2 asu˚cientconditionforguaranteeingthat(2.24)doesnotholdandthus ˘ : isnon-singular,is 24 1 ˝ ˝ 90 Œ or ˝¡ 1 91 Here,wegetaveryrelaxedcriterionon ˝ .Soforoursimulationandexperiments,weused ˝ =0 Ł 5 . 2.4SimulationResults Inthissection,weverifythee˙ectivenessofouralgorithmthroughMatlabsimulation.In addition,weintroducetwoalternativealgorithms,followedbyacomparisonofEKFwiththetwo algorithms. Tofurtherexplorethee˙ectofunmodeledsystemdynamics(whichwereignoredintheal- gorithmdevelopment,consideringthatthereceivertypicallywouldnothaveaccesstothemotion informationofthetransmitter),wehaveincludedsomearbitrarydynamicsforthesysteminthe simulation.Speci˝cally,thesystemstateevolvesaccordingtothefollowing, 8 > > >< > > > : G 1 Œ: = G 1 Œ: 1 + F 1 Œ: 1 G 2 Œ: = G 2 Œ: 1 + V + D : 1 + F 2 Œ: 1 (2.25) where V isanunknownconstantdisturbance.Theterm V correspondstotherelativeangularmove- mentbetweenthetransmitterandthereceiver,anditsimulatesthescenariowherethetransmitter revolvesaroundthereceiverwhileshiningdirectlyatit.Basedonthemodelandthealgorithms describedearlier,thesimulationisconductedwithparameterslistedinTable2.1.Notethatwe haveuseddi˙erent & and ' valuesforEKF,thanthesystem'snoisecovariancematrices,asitis shownearlier[45]thatusingscaled-upnoisecovariancematricesforEKFimplementationgives animprovementinconvergenceperformance. Figure2.6showstheresultsobtainedfromasimulationrunwith V =1.2.FromFigure2.6,it canbeseenthattheestimatedstatesconvergetotheneighborhoodoftheoriginalstatesinabout2 secondsandremaintherethroughouttherun.Wenotethattheestimatedvalueforthestate G 2 is slightlylowerthantheactual.Mostlikelythiscanbeattributedtothepositivebiasterm V inthe systemdynamics,whichconstantlyproducesashiftofreceiverorientationinthepositivedirection. Notethattheoscillationsinthemeasurementsareattributedtothescanningmotionofthereceiver. 25 Figure2.6: SimulationresultsofEKFwhenthe G 2 dynamicscontainsanunknownconstant disturbance V =1 Ł 2 € s . 26 Table2.1: ParametersassociatedwithEKFimplementationinthesimulation. ParameterDescriptionValue ^ x 0 Initialstateestimate » 2 Œ 0 ¼ ) % 5 0 Initialerrorcovariancematrix h 1000 01000 i & BHB System'sprocessnoisecovariancematrix h 0 Ł 00250 00 Ł 01 i & EKF'sprocessnoisecovariancematrix h 0 Ł 250 01 i ' BHB System'smeasurementnoisecovariancematrix h 0 Ł 040 00 Ł 04 i ' EKF'smeasurementnoisecovariancematrix h 10 01 i ˝ Proportionalcontrollergain0.5 Andwenotethattheactualmean(state G 2 )convergestotheneighborhoodofzeroinabout2-3 seconds.Thiscorrespondstothealignmentofthemeandirectionofthereceivertothedirection thatfacesthetransmitter. Toexplorethelimitofalgorithm'sassumptiononquasi-staticdynamics,theconstantdisturbance term V isincreasedtoanextentwherethetrackingfails.Figure2.7showsthestateswhentheEKF algorithmstopsworkingandtheangleofincidence( G 2 )startsgoingunbounded.Thiscorresponds tothereceiver'sdirectionmovingawayfromthetransmitter-facingdirection. Next,wecomparetheperformanceoftheEKF-basedalgorithmwithtwoalternativealgorithms: hill-climbingandthree-point-averaging.Inparticular,weexploretheperformanceofthealgorithms inthepresenceofmeasurementnoises.Inhill-climbing,thereceiverstartswithanorientation, measuresthelightintensityandrotatesbyangle ^ =2 ineithertheclockwiseorcounterclockwise direction.Itthenmeasuresthenewlightintensity.Ifthelatterishigherthanthepreviousvalue,it willrotateby ^ againinthesamedirection;otherwiseitwillrotateintheoppositedirection. Forthethreepoint-averagingalgorithm[2],thereceiverperformsaclockwiserotationandthen acounter-clockwiserotationbyanangle ^ fromtheoriginallocation.Meanwhile,ittakesthelight intensitymeasurementsateachstep( + 1 Œ+ 2 Œ+ 3 ),where + 1 , + 2 , + 3 representthevoltagesatthe 27 Figure2.7: SimulationresultsofEKFmethod'sfailurewhentheunknownconstantratedisturbance V isincreasedto 8 € s . clockwiserotation,counter-clockwiserotation,andoriginallocation( ^ =0 ),respectively.Thenew turningangle ^ ? oftherotatingbaseiscalculatedbytakingaweightedaverageofthesesignalsat threesteps: ^ ? = ^+ 1 ^+ 2 + 1 + + 2 + + 3 (2.26) ThealgorithmisimplementedinMatlabforsimulation,withturningangle ^ =2 .Toquantify thealignmentcontrolperformance,twometricsareconsideredbasedontheangleofincidence( q ) andthecleanlightintensitymeasurement(lightintensityvalueuncorruptedbynoise E : ,de˝nedas `cleanmeasurement'),respectively.Notethatthealgorithmsusethenoise-corruptedmeasurement 28 foralignmentcontrol,andthecleanmeasurementisusedonlyforperformanceevaluation.We de˝nethe`trackingpercentage'asthefractionoftimewhenthesystemisinthetrackingzone,where thelattercouldbedeterminedbasedoneithertheangleofincidenceorthecleanmeasurement.In particular,weconsiderathresholdof 15 fortheangleofincidence q :if q isoutsidetherange [ 15 Œ 15] ,itwouldbeconsideredoutofthetrackingzone.FromFigure2.3,wecanseethatthis angularthresholdcorrespondstoabout 20% ofthemaximumleveloftheintensity,whichwewill useasathresholdforthe`cleanmeasurement'fordeterminingwhetherthereceiverorientationis inthetrackingzone.Foramaximumintensityof3V,thecorrespondingintensitythresholdfor trackingis0.6V. Forthecomparisonofalignmentcontrolperformance,eachalgorithmisrunwithaseriesof noiselevelsforthemeasurement.Asthenoiselevelincreases,thestochasticityinthetracking percentageincreases.Henceinsimulation,foreachnoiselevel,resultsof1000runshavebeen combined.Ineachrun,thetrackingpercentageiscomputedandthentheaverageoverthe1000 runsisobtainedtogetanestimateoftheexpectedvalue.Thestandarddeviationineachcaseis alsocomputedtocapturethevariationamongtheruns.Figure2.8showstheplotsforthemean ofeachalgorithmwithstandarddeviationaserrorbars.Forclarity,thestandarddeviationinerror barsisscaleddownby5times.Foreachofthealgorithm,thetrackingpercentageiscomputed bythetwomethods(basedontheangleofincidence( q ),legend`-angular',andthelightintensity measurement,legend`measure')mentionedearlier. Thepurposeofusingtwomethodstocomputetrackingistoshowthatthereisahighamountof correlationbetweenthetrackingpercentagescomputedbythesetwomethods.Sothatinthecase ofexperiments,eveniftheangulardataisnotavailable,wecancon˝dentlyrelyonthetracking percentagegeneratedbytheintensitymeasurementdata.FromFigure2.8,onecaneasilyseethat thetrackingperformanceoftheEKF-basedalgorithmismuchbetterthanthatoftheothertwo.The trackingremains100%evenatthenoiselevelof1.0anditdecreasesgraduallyafterthat,which givesagoodrangeofoperation.Theothertwoalgorithmsperformwellunderlownoiselevels, buttheirperformancedegradesfasterthantheEKFathighernoiselevels.Itcanalsobeobserved 29 Figure2.8: Simulationresultsoncomparisonofalignmentcontrolperformanceforthethree methods,fordi˙erentlevelsofmeasurementnoise,whensystemstatesareevolvedaccordingto Eq.(2.25)with V =1 Ł 2 .Verticalbarsdenotethedown-scaledstandarddeviationsateachpoint. representsthehill-climbingalgorithm,andointrepresentsthethree-point-averaging algorithm. thatthestandarddeviationundertheEKF-basedalgorithmisalsomuchlowerthantheothertwo algorithms,whichfurtherprovesitsreliabilityunderhighernoiselevels. 2.5ExperimentalResults Inthissection,weverifythee˚cacyofouralgorithmbyimplementingcasessimilartothe simulationonanexperimentalsetup.WhiletheLEDcommunicationhardwaredesignallowsus touseboththereceiverandthetransmitterinthesamemoduleatonce,sothattworobots,each equippedwithsuchamodule,cancommunicatewitheachotherboth-ways,inthisparticularwork wearefocusedonimplementationoftrackingalgorithmonthereceiver.Therefore,aseparate lightsourceisusedasatransmitter.Figure2.9showssuchatransmitter-receiverpairusedin theexperiments.Thetransmitterismountedonamobilerobottofacilitaterelativemotionwith respecttothereceiver.ItistobenotedthatthereceiverinFigure2.9isamodi˝edversionofthe 30 deviceshowninFigure2.1.Here,asmentionedearlier,asteppermotorisusedinsteadofaDC motor.Thesteppermotorhasaprecisecontrolovertheangularposition.Forthereal-timeonboard implementationofallcomputations,IntelEdison ® mini-computerboardisused.Itisequipped with500MHzAtom2-CoreCPUand1GBofLPDDR3RAM.Thehardwarespeci˝cationsare su˚cientforthereal-timecomputationrequiredforouralgorithm,andeachEKFiterationtakes about50mstocomplete. Figure2.9: Completesetup:Receiver(left)onarotationbaseandtransmitterLEDonamobile robot(right). Amobilerobotequippedwiththetransmitterrevolvesaroundthestaticreceiveratadistanceof 1.25mwhilefacingthereceiver(Figure2.10).Herethetransmitterrobotishard-codedtofollowthe 31 circularpathcenteredaroundthereceiver,whichnotonlyensuresthedistance 3 tobeconstantbut alsoenablesthetransmittertofocuslightonthereceiverthroughouttherun( W ˇ 0 > ).Therobot's speedis˝xedinsuchawaysothatitrevolvesaroundthereceiverat 1 € s .Onthereceiver'send, anaveraging˝lterisimplementedontheon-boardmeasurementofthelightintensity.Currently, ourexperimentsareconductedintheairsothenoiseisrelativelysmallandcanberemovedby averaging.Theaveragedoutputofthe˝lteristermedas`cleanmeasurement'fortheexperiments. Moreover,toimplementarangeofnoises,anextraarti˝cialGaussiannoisetermisaddedtothe cleanmeasurement. Figure2.10: Experimentalsetup:Transmitterrobotmovingaroundstaticreceiver,followingthe markerlines. Figure2.11showstheevolutionofstateestimatesandmeasurementoutputforaparticularrun withtheadditional,arti˝cialnoiselevelof1.0whentheEKF-basedalgorithmisimplemented. 32 Herewecanseethattheestimateofthemeanofthescan( ^ G 2 )goestoaboundedneighborhoodof zeroaswell.Itistobenotedthatwedonothaveaccesstotheoriginalstatesofthesystem,hence onlytheestimatesareplotted.However,whilerunningtheexperiment,wehavevisuallyobserved thatafterafewiterations,themeanangularpositionofthereceiver-scanstartsaligningitselfwith thelineconnectingthetransmittertothereceiver,whichimpliesthatthereal G 2 alsoconvergesto theneighborhoodof 0 .Moreover,theestimate ^ G 1 stayswithinarelativelynarrowrange(according toourdesignofexperiment),whichshouldbeclosetotheactual G 1 .Hence,accordingtothese observations,theestimatesremainwithinacloserangearoundtheoriginalstates. Figure2.11: Experimentalresultswhenthetransmitterrobotmovesaroundthereceiverwithan angularrateofabout1degree/s.Themeasurementsarecorruptedwiththenoiselevelof1.0. 33 Toconductacomparisonbetweenthethreealgorithms,wefurtherperformmultiplerunsinthe experiment.However,unlikesimulation,itisnotpracticaltodo1000runsintheexperimentsfor eachcase,andwelimitthenumberofexperimentsto10runs.Moreover,whenthetransmitterrobot movesalongthecircularpath,itstrajectoryisnotalwaysconsistent.Forinstance,sometimesit goesalittleclosertothereceiverandsometimesfurther.Thedatageneratedbythismotionisgood forqualitativedemonstrationbuttohaveafairquantitativecomparisonbetweenthealgorithms, weneedaconsistentsystem.Hencewechoosetokeepthetransmitterrobotstaticandintroduce anunknownconstantdisturbanceterm,similarto V inEq.(2.25),intheonboardprogram.This disturbancetermforcesthereceivertorotateawayfromthetransmitterfacingdirection.Figure2.12 showstheplotofthetrackingpercentagewithscaled-downstandarddeviationerror-barsoverthe rangeofnoiselevel.Itistobenotedthatsincetheangulardataisnotavailable,thetrackingis computedbythresholdingoflightintensitymeasurements.Aswecansee,thebehaviorsofthe algorithmsintheexperimentaresimilartothoseobservedinsimulation.andtheEKFalgorithmhas highertrackingperformancewithgradualdegradationascomparedtoothertwoalgorithms.The varianceinthetrackingpercentageofEKFisalsolowerthantheothertwoalternativealgorithms. Otherthanthecomparativeperformanceevaluation,ifweconsiderthenoiselevelof1.0,whichis one-thirdofthemaximumintensityor22dBSNR(Signaltonoiseratio),wehavemorethan95% oftracking. 2.6ChapterSummary Inthiswork,acompactLED-basedcommunicationsystemusingasinglereceiverwithactive alignmentcontrolhasbeenpresented.Fortracking,wehavetestedtheapplicabilityofsimpler algorithmslikethehill-climbingandthethree-pointaveragingmethods.Thesemethodsaregood forlownoiseenvironmentsbuttheirperformancedegradessteeplyathighernoisescenarios. Whereas,aprincipledapproachusingstateestimationandcontrollikeourproposedEKF-based alignmentcontrolalgorithmnotonlygivescomparableresultsinlowernoisecasesbutperforms robustlyinthecaseofhighernoiseenvironment.Inourapproach,themotionofthetransmitter 34 Figure2.12: Experimentalresultsoncomparisonofalignmentcontrolperformanceforthethree methods,fordi˙erentlevelsofmeasurementnoisewhentheconstantdisturbance( V )is 2 Ł 8 € s . Verticalbarsdenotethedown-scaledstandarddeviationsateachpoint. wasassumedtobeunknownandcapturedaspartofawhiteGaussiannoise.Ascanningtechnique isimplementedtosatisfytheobservabilitycriterionrequiredfortheEKF.Asimpleproportional controllerisusedforactivealignment. AsmentionedinSection2.3,themainfocusofthisworkwastodemonstratetheinstrumental roleofnonlinearestimationandfeedbackcontrolintheactivealignmentoftheLEDandthe receiver.Inparticular,thisapproachusesthemeasurementhistorytoestimatethestatevariables andsubsequentlyappliesacontrolactionbasedonthestateestimate.Itoutperformsalgorithms thatsimplyreacttothecurrentmeasurement(suchasthehill-climbingandthree-point-averaging algorithms)inthepresenceofmeasurementnoises,atamodestcostofimplementationcomplexity. 35 CHAPTER3 EKF-BASEDALIGNMENTCONTROLINTHE3DSPACE Inthischapter,weextendtheactivealignmentcontrolapproachtothree-dimensional(3D)space. WeproposeanextendedKalman˝lter(EKF)-basedalignmentapproach,wheretheestimatesof azimuthalandelevationcomponentsoftheheadingbiaswiththeLOSareusedtocorrectthe alignment.Weintroduceandimplementanewcircularscanningtechniqueonatwo-degree- of-freedom(DOF)rotationalsystem,mountedonarobot,thatenablesconsecutiveindependent measurementsfromasinglephoto-diode,whicharenecessarytosatisfytheobservabilityconstraints oftheEKF.Furthermore,weexploreasynchronizedalternatingschemetoextendtheapproachto asystemoftworobotsinabi-directionalsetting,wherebothrobotsparticipateinthealignment scheme.ThescanningamplitudeisfurtheradjustedbasedontheEKFestimationcovariance,to balancethetrade-o˙betweenestimationaccuracyandactuatione˙ort.Wecomparetheproposed approachwithanextremum-seeking(ES)approachinbothsimulationandexperimentation,where asetupoftworobotswithrelative3Dmotionisconsidered.Thepresentedresultssupportthe e˚cacyoftheproposedmethodinthepresenceofslowrelativemotionbetweentherobots,and demonstratethesuperiorityoftheproposedapproachovertheESmethodoverawiderangeof distancesbetweentherobots. Theorganizationfortherestofthechapterisasfollows.InSection3.1,thehardwaresetupof thesystemisdiscussed,followedbyadiscussiononthelightsignalstrengthmodelforstate-space formulation.InSection3.2,theEKF-basedalignmentalgorithmisdescribed,followedbyabrief descriptionoftheES-basedmethod.Discussiononsimulationsetupandresultsarepresentedin Section3.3,whileexperimentalsetupandresultsarereportedinSection3.4.Finally,concluding notesareprovidedinSection3.5. 36 3.1SystemSetupandModeling Inthissection,we˝rstbrie˛ydescribethehardware-setup,whichisessentialinthesubse- quentmathematicalmodelingofthereceivedlightintensity.Wethenformulatethestate-space representationofthesystem,whichisthebasisforEKFalgorithmdevelopment. 3.1.1SystemSetup Figure3.1illustratesthetransceivermountedonthetwo-DOFmechanism,alongwithmiscella- neouscomponentsofthesetup.Thetransceivercomprisesatransmitter,CreeXR-ESeriesLED withprincipalwavelengtharound480nm,andareceiver,photo-diodefromAdvancedPhotonix (partnumberPDB-V107).Lens-opticsismountedoneachofthedevicestoadjustthe˝eldofview. Further,toenabledynamicadjustmentoftransceiverpointingdirectionin3D,atwo-DOFrotation mechanismisused.ThemechanismconsistsoftwoDynamixel ® servomotors;the˝rstone,called baseservo ,adjuststheazimuthalorientationofthetransceiver,andthesecondservomotoradjusts theelevationorientationofthetransceiver.Fortheonboardcomputationandprocessing,weused aBeaglebone-Blue ® board. Inourwork,weconsiderthesignalfromasinglephoto-diode,whichcanbeusedforboth alignmentcontrolandcommunication.Weassumethattheopticalsignalhasanapproximately equaldistributionoflowandhighvalues,andhencetheaverageintensitystaysapproximatelyequal tothehalfofthehighvalueofthesignal.Thischapterfocusesonlyonthealignmentcontrolaspect oftheproblem.PleaseseeChapter5fordetailsonthesimultaneousalignmentandcommunication. Next,webrie˛ydiscussthecoordinateframesrelatedtoourphysicalsetupandde˝nethevariables associatedwiththelightsignalstrengthmodelusedinthiswork. 3.1.2CoordinateFramesandReceivedLightIntensityModel Weconsiderascenariooftworobots,say R 1 and R 2 ,locatedatarbitrarypositionsina3Dspace. Eachrobothasitsownsetoftwolocalframesofreference;withoutthelossofgenerality,we 37 Figure3.1: Hardwaresetup,wherethetransceiver(transmitterLEDandreceiverphoto-diode) mountedontwo-DOFactivepointingmechanismisshown. studytheframesof R 1 thathelpindevelopingtheintensitymodelofthelighttransmittedby R 2 andreceivedby R 1 .Figure3.2illustratestwo3Dcoordinateframesdeterminedbytherelative orientationofthetworobots,atrobot R 1 'sreceiverend.Formodelingpurposes,boththereceiver andthetransmitterofthesamerobotareconsideredtobeonasinglepointcalleda transceiver and theirnormalaxesarealigned.Thecoordinateframesaredenotedasthe baseframe ( > G 0 H 0 I 0 )and the transceiverframe ( > GHI ),respectively.Bothcoordinateframessharethesameorigin,which isthelocationofthetransceiverof R 1 .Thefollowingsequentialrulede˝nestheaxesoftheframes: 38 ‹ H 0 istherotor'saxisofbasesteppermotorof R 1 . ‹ G istheheadingdirection(normal)ofthereceiverof R 1 . ‹ Both I and I 0 areidenticalandarechosenasaperpendicularaxistotheplanecontaining G and H 0 . ‹ G 0 and H aredecidedbytheright-handrule. Figure3.2alsodescribesalltheassociatedvariablesofinterestinouranalysis.Theparameter 3 isthedistancebetweenthetransceiversoftworobots.Thelineconnectingthetworobots' transceiversrepresentsthedesireddirectionofLOS.Theanglebetweenthetransmitterorientation of R 2 andtheLOSisthetransmissionangle W .Theangles q and \ representtwoorthographic projectionsoftheanglebetweentheLOSandthepointingdirection G ,where \ denotestheelevation, and q denotestheazimuthalorientationinthetransceiverframe. Figure3.2: Illustrationoftwolocal3Dcoordinateframesandtheassociatedvariablestode˝ne therelativepositionandorientationbetweenthetransceiversoftworobots. 39 ThelightsignalstrengthmodelhereisderivedfromSection2.2,withminormodi˝cationsto accommodatethe3Dsetting.FromEq.(2.2),consideringthee˙ectoftheareaandorientationof receiver(combinationofthelensandthephoto-diode),the˝nalreceivedpoweris: % in = ˆ W ( 3 ) 0 5 ( qŒ\ ) (3.1) where 0 denotesthelensapertureareaand 5 ( qŒ\ ) characterizesthee˙ectofincidenceangles q and \ onthereceivedintensityoflight.Theterm 5 ( qŒ\ ) dependsonthecombinationofthe lensandthephoto-diodeusedinthereceiver.Tocharacterizethefunction 5 ( qŒ\ ) forourreceiver setup,we˝rstcollectedthemeasurementdataatdi˙erentvaluesof q and \ atadistanceof1m. Figure3.3showsthecollectedlightintensitydata.Thecollecteddataisobservedtobecircularly symmetricaboutthepeak.Henceforth,forpracticalusageandsimplicityintheformulation, 5 ( qŒ\ ) isapproximatedbyaone-dimensionalfunctionofthereceiver'sheadinganglewithrespecttothe LOS,whichistheequivalentanglecombining q and \ : 5 ( qŒ\ )= 6 ¹ arccos ¹ cos q cos \ ºº Œ (3.2) where 6 ( ) istheGaussianfunctionobtainedbyperformingcurve-˝ttingonthenormalizedver- sionmeasurementdata,theexpressionof 6 ( ) isprovidedinEq.(2.4)inChapter2.The˝nal measurementmodelcanbesummarizedas + 3 = C ? ˚ W 4 23 6 (arccos ¹ cos q cos \ º ) €3 2 (3.3) where + 3 isthesignalstrengthinvolts,and C ? isaproportionalityconstant,whichdependson thelensapertureofthereceiveralongwithvariousparametersassociatedwiththephoto-diode's signal˝lterandampli˝erstages. 3.1.3State-spaceProblemFormulation WeobservefromFigure3.2andEq.(3.3),thatthereceivedsignalstrengthdependsonthefour independentvariables, W , 3 , q and \ .InlightofthediscussionprovidedinSection2.2.2,wecan 40 Figure3.3: LightintensitydataforGaussiancurve˝ttingtoapproximate 5 ( qŒ\ ) . mergethetermscomprising W and 3 inasinglevariableandde˝nethestatevariablesas x = 2 6 6 6 6 6 6 4 G 1 G 2 G 3 3 7 7 7 7 7 7 5 4 = 2 6 6 6 6 6 6 4 C ? ˚ W 4 23 €3 2 q \ 3 7 7 7 7 7 7 5 (3.4) Thestatesin x dependonthedistanceandrelativeorientationbetweenthetransceiversofthetwo robots.Inatypicalscenario,arobotdoesnothavetheexactinformationaboutthemotionoftheother robot.Sowewillassumethattherelativemotionbetweenthetwocommunicatingrobotsisslow enoughthataconstantmodelwithGaussiannoisescancapturetherelativedynamics.Inparticular, thedynamicsforthestatevariablesde˝nedin(3.4)canberepresentedinthediscrete-timedomainas x : = 2 6 6 6 6 6 6 4 G 1 Œ: G 2 Œ: G 3 Œ: 3 7 7 7 7 7 7 5 = 2 6 6 6 6 6 6 4 G 1 Œ: 1 + F 1 Œ: 1 G 2 Œ: 1 + D 2 Œ: 1 + F 2 Œ: 1 G 3 Œ: 1 + D 3 Œ: 1 + F 3 Œ: 1 3 7 7 7 7 7 7 5 (3.5) 41 where F 1 Œ: , F 2 Œ: and F 3 Œ: representprocessnoiseterms.Weassumethesenoisestobeinde- pendent,white,andGaussian.Thesenoiseterms,uptoacertainextent,accountfortheslow dynamicsof x : ,whichwedonotexplicitlymodel.Theterms D 2 Œ: and D 3 Œ: arethecontrolinputs foradjustingthetransceiver'sorientation.Thesetwoinputsarede˝nedinthetransceiverframeand thusrequirearotationaltransformationtoconvertthemintothebaseframe,inordertogenerate motorcommands.ThedetailsofthetransformationarediscussedlaterinAppendixA.Thesystem outputattimeinstant : ,themeasurement + 3Œ: ,canbeexpressedas: + 3Œ: = G 1 Œ: 6 (arccos cos G 2 Œ: cos G 3 Œ: )+ E : (3.6) where E : isanadditivewhiteGaussiannoise,assumedtobeindependentoftheprocessnoiseterms. Givenasequenceofmeasurements,theobjectiveistoestimate x : ,whichisthenusedtodesign thecontrolterm u k = D 2 Œ: ŒD 3 Œ: ) ,inordertodrive G 2 and G 3 (termedas angularstates )towards 0 ,thecon˝gurationcorrespondingtotheLOS. 3.2EstimationandAlignmentAlgorithms Inthissection,we˝rstexaminetheobservabilityoftheformulatedstate-spacemodelandthen introducethecircularscanningtechnique.Movingfurther,wediscusstheimplementationofthe EKFalgorithmwithafewre˝nementsandtheextensiontothebi-directionalsetting,followedby abriefdescriptionoftheimplementationoftheextremum-seekingcontrolalgorithm. 3.2.1ObservabilityoftheSystem Forthenonlinearmeasurementmodel(3.6),anextendedKalman˝lter[39]isexploredforstate estimation.Fromthe(linear)dynamicequation(3.5),wede˝ne = 2 6 6 6 6 6 6 4 100 010 001 3 7 7 7 7 7 7 5 Œ = 2 6 6 6 6 6 6 4 00 10 01 3 7 7 7 7 7 7 5 Ł (3.7) Thenequation(3.5)canbere-writtenas: x : = x : 1 + u : 1 + w : 1 Œ (3.8) 42 with w : = F 1 Œ: ŒF 2 Œ: ŒF 3 Œ: ) .Denoting ˘ : , ˘ ( x : ) asthesystem'slinearizedoutputmatrixat the k thtimeinstant,thenonlinearobservabilitymatrixatthattimeinstant,obtainedfrom[39],can beexpressedas O : = 2 6 6 6 6 6 6 4 ˘ : ˘ : +1 ˘ : +2 2 3 7 7 7 7 7 7 5 = 2 6 6 6 6 6 6 4 ˘ : ˘ : +1 ˘ : +2 3 7 7 7 7 7 7 5 Ł (3.9) Thematrix O : needstobefullrankateachtimeinstant : ,forthesystemtobeobservable[39],which isanecessaryconditionforthestabilityoftheEKFestimate.Apossiblewaytosatisfythiscriterion istoensurethelinear-independenceofthreeconsecutive ˘ : 'switheachother,whichrequiresatleast threesuccessivemeasurementsoflightintensitytobeindependent.Inoursystem,weensurethe linear-independencebytakingthemeasurementsfromthreenon-planarpointingdirectionsofthe transceiver.Henceforth,weintroduceacircularscanningtechnique,wherethepointingdirection ofthetransceivermovesinacircularmannercenteredaroundameanorientation.Thismeanorien- tation,whichiswhatthecontrolinputmodulates,isconsideredasthenewmodi˝edstates ¹ G 2 ŒG 3 º . Thescanningpatterndependsontwoparameters:scanningamplitude X A andangularstep X k (seeFigure3.4),where X A modulatestheradialangulardisplacementofthemeasurement orientationfromthemeanorientation.Theparameter X k istheangularseparationbetweenthetwo successivemeasurementorientations.Theterm k : accountsfortherelativeangularpositionofa measurementorientationwithrespecttothemean ¹ G 2 ŒG 3 º orientationatthe : thinstantandithas twoorthogonalcomponents U : and V : asshownin(3.10): 8 > > > > > > >< > > > > > > > : k : = k : 1 + X k V : = X A cos( k : ) Œ U : = X A sin( k : ) Ł (3.10) Figure3.4explainsthescanningtechnique.Thepointingorientationmovesaroundthemean ( G 2 ŒG 3 ) insuccessiveangularstepsofsize X k .Themeasurementobtainedateach k : isconsidered asouroutput y : 2 R 1 : y : = G 1 Œ: 6 b G 2 Œ: ŒG 3 Œ: ŒV : ŒU : + E : Œ (3.11) 43 Figure3.4: Illustrationofthecircularscanningsequence,withmeanpointingorientation ¹ G 2 ŒG 3 º andthelastthreeangularpositionsofscanning k : , k : 1 and k : 2 . where b ( ) computesthenetanglebetweentheLOSdirectionandthecurrentpointingdirection, whichdependsonboththemeanandthescanningterms.Thedetailsofthefunction b ( ) are coveredinAppendixB. 3.2.2ImplementationofExtendedKalmanFilter Withthesystemdynamics(3.8)andtheoutputequation(3.11),anextendedKalman˝lter(EKF) cannowbeimplemented.Thecompletealgorithmdescriptionisasfollows. TheEKFhasthreecovariancematrices: % 2 R 3 3 , & 2 R 3 3 ,and ' 2 R 1 . % denotesthe conditionalerrorcovariancematrix, & denotestheprocessnoisecovariancematrix,and ' isthe measurementnoisecovariance.Atstep : , 44 1. Predictionphase :Thestateestimatesanderrorcovariancematrixarepredictedas: ^ x 5 : = 2 6 6 6 6 6 6 4 ^ G 5 1 Œ: ^ G 5 2 Œ: ^ G 5 3 Œ: 3 7 7 7 7 7 7 5 = 2 6 6 6 6 6 6 4 ^ G 1 Œ: 1 ^ G 2 Œ: 1 + D 2 Œ: 1 ^ G 3 Œ: 1 + D 3 Œ: 1 3 7 7 7 7 7 7 5 Œ (3.12) % 5 : = % : 1 ) + &Œ (3.13) where ^ G 5 =Œ: representstheestimateof = thstateatthe : thtimeintervalandthesuperscript 5 standsfor`forecast'oftheassociatedentities. 2. Outputestimation : From(3.11),weget ^ y : , ( ^ x 5 : )=^ G 5 1 Œ: 6 b ^ G 5 2 Œ: Œ ^ G 5 3 Œ: ŒV : U : (3.14) Nowthematrix ˘ : 2 R 1 3 canbecomputedas: ˘ : , m ( ^ x 5 : ) m ^ x 5 : = 2 6 6 6 6 4 6 ( ^ b 5 : ) Œ ^ G 1 Œ: 6 0 ( ^ b 5 : ) r 1 (^ j 5 : ) 2 " m ^ j 5 : m ^ G 2 Œ: Œ m ^ j 5 : m ^ G 3 Œ: # 3 7 7 7 7 5 Œ (3.15) where 6 0 ( ) indicatesthederivativeof 6 ( ) and ^ b 5 : , b ^ G 5 2 Œ: Œ ^ G 5 3 Œ: ŒV : ŒU : =arccos( j 5 : ) Œ and ^ j 5 : , j ^ G 5 2 Œ: Œ ^ G 5 3 Œ: ŒV : ŒU : Ł Thedetailsofthefunctions j ( ) Œb ( ) ,andtheirderivativesarediscussedinAppendixB. 3. Analysis/updatephase : 8 > > > > > > >< > > > > > > > : : = % 5 : ˘ ) : ( ˘ : % 5 : ˘ ) : + ' ) 1 ^ x : = ^ x 5 : + : ( y : ^ y : ) % : =( ˚ 3 : ˘ : ) % 5 : (3.16) Here : 2 R 3 1 denotesthe˝ltergain.Thematrix ˚ 3 denotesthe 3 3 identitymatrixand itistobenotedthat % : and ^ x : withnosuperscriptsdenotethe˝nalestimatedvaluesofthe : thstep. 45 Usingthestateestimates,thecontroltermscannowbecomputedas 8 > > >< > > > : D 2 Œ: = ˝ % ^ G 2 Œ: ˝ ˚ ^ I 2 Œ: D 3 Œ: = ˝ % ^ G 3 Œ: ˝ ˚ ^ I 3 Œ: (3.17) wheretheterms ^ I 2 Œ: and ^ I 3 Œ: aretheintegralsoftheestimates ^ G 2 Œ: and ^ G 2 Œ: ,respectively,which arede˝nedby: ^ I 8Œ: , : 1 X = =0 ) ^ G 8Œ= =^ I 8Œ: 1 + ) ^ G 8Œ: 1 Œ8 2f 2 Œ 3 g (3.18) where ) isthesamplingtime.Thepositiveconstants ˝ % and ˝ ˚ accountfortheproportionaland integralgainsofthePI-controller,respectively.Thegainsaredesignedtoensuretheclosed-loop stabilityofthetwo-states;formoredetails,see[49]. Notethattheabovecalculationisconductedinthelocaltransceiverframeof R 1 ,andthecontrol termsneedtobetranslatedtothebaseframetogenerate˝nalcommandsformotors.Thedetails ofthecommandtranslationarediscussedinAppendixA. Moreover,whenthemeanofthescanachievesasteadystate,therearestilloscillationsin theintensitymeasurementsduetothescanningmotion.Theseoscillationslimittheaverage intensitybelowtheavailablemaximum.Thisdi˙erenceofintensitycanbecriticalduringactual communication,aslowerlightintensityleadstoaweakersignaltonoiseratio(SNR)andresults inahigherbiterrorrate.Toavoidsuchcasesandadditionally,toreducethepowerconsumedin thescanningmotion[51],weproposeanproportionalscanningtechnique.Here,theamplitudeof scanningismadeproportionaltothetime-averageofthenormofthesub-matrixof % (denotedas % 0=6 )correspondingtotheangularcomponents,where % 0=6 = " % 2 Œ 2 % 2 Œ 3 % 3 Œ 2 % 3 Œ 3 # (3.19) with % 8Œ9 denotestheentryinthe 8 throwand 9 thcolumnofthe % matrix: X AŒ: =max X ; Œ min X : X 8 = : = B +1 k % 0=6Œ8 k 2 ŒX !! (3.20) withthe2-norm kk 2 beingthelargesteigenvalueofamatrix,and X and X ; indicatetheupperand lowerboundsofthescanningamplitude,respectively. X denotestheproportionalgainand = B is 46 thenumberofiterationsinanactivescanningperiod,whichisequalto 360 > X k .Thelowerboundis implementedduetotheconstraintofthethreeindependentmeasurementsfromtheobservability criterionandhence,thescanningamplitudecannotbemadezero. 3.2.3ExtensiontotheBidirectionalScenario Sofar,wehavediscussedtheformulationandapproachforasingledirectionalscenariowherethe receivertriestoalignitselftowardsthedirectionofthemaximumlightintensity.Now,considerthe bidirectionalcaseoftworobots,whereeachoftherobotsisequippedwithatransceiver.Herewe cannotimplementtheaforementionedalgorithmoneachrobotasthesimultaneousscanningwould violatethequasi-staticassumptiononthestate G 1 ofeachoftherobot.Therefore,weimplementan alternatingpausing-scanningapproach.Hereonerobotstartswiththeactivescanningphasewhile theotherwiththepassivepausingphaseandtheyalternateafterward.Theactiveandpassives phasearedescribedasfollows. 1. Activephase: InthisphasetherobotcompletesacircularscanoftheEKFapproachpresented earlier.Thetotalnumberofstepsinonecircularscanequalsto 360 €X k .Afteronecircleof thescan,therobotpointstothecenterofthescanandswitchestothepassivephase. 2. Passivephase: Inthisphasearobot˝xesitsorientation;scanningamplitude( X A )andthe controlgains( ˝ ? , ˝ ˚ )botharezero.Duetothezeroscanningradius,theobservability matrixofthesystemhasrankoneandhencetheoriginalsystemisnotobservable.Therefore, weimplementtheEKFonlyforthestate G 1 inthisphase,detailedasfollows: ^ G 5 1 Œ: =^ G 1 Œ: 1 (3.21) % 5 :Œ 1 Œ 1 = % :Œ 1 Œ 1 + & 1 Œ 1 (3.22) : = % 5 :Œ 1 Œ 1 ˘ :Œ 1 ( % 5 :Œ 1 Œ 1 ˘ 2 :Œ 1 + ' ) 1 (3.23) ^ G : =^ G 5 : + : ( y : ^ y : ) (3.24) % :Œ 1 Œ 1 =(1 : ˘ :Œ 1 ) % 5 :Œ 1 Œ 1 (3.25) 47 Figure3.5: Blockdiagramsummarizingtheproposedmethod.Alltheshadedcolornodesdenote thestepsknowntothealgorithm.Theinformationatwhitecolornodesisnotavailabletothe algorithm. Ateachiteration,theotherentriesof ^ x : and % : arecarriedtothenextiteration.Thiswayat thebeginningofanactivephasetheinitialconditionsfor ^ G 2 and ^ G 3 arepropagatedfromthe endofthepreviousactivephase.Afterwaitingforthepredeterminednumberofiterations requiredtocompleteacircularscan,therobotswitchestotheactivephase. Figure3.5summarizestheoverall˛owofourapproach. 3.2.4BenchmarkApproach:Extremum-seeking(ES)Control Here,webrie˛ydiscusstheimplementationofthediscrete-timeEScontrolmethodthatisusedas abenchmarkforcomparisonwithourmethod[25].Thealgorithmistypicallyusedtooptimizea functioninreal-time.Thelightintensitymeasurementfromeachrobotisusedasthefunctionfor maximization.TheblockdiagraminFigure3.6illustratesthe˛owofthealgorithmimplemented 48 oneachoftherobots.Theplantrepresentstheoverallsystemofthetworobots,theoutputfromthe plant y 8Œ: ispassedthroughahigh-pass˝lter,whichisthenmultipliedbytwoseparateperturbation signals X ˆ( sin(2 c5 ?Œ8 ): ) and X ˆ( cos(2 c5 ?Œ8 ): ) togeneratethecorrespondingbiassignals, b 2 Œ: and b 3 Œ: ,respectively.Theterms X ˆ( and 5 ?Œ8 representtheamplitude,andthefrequencyofthe perturbationsignalsforrobot R 8 ,respectively.Thebiassignalsareeachmultipliedbyagainof ˆ( , andthenaddedtotheircorrespondingperturbationtermstogeneratethecontrolterms D 2 Œ: and D 3 Œ: . Figure3.6: BlockDiagramforextremumseekingcontrol. 3.3SimulationResults Inthissection,wesimulatetheproposedapproachandtheEScontrolmethodforatwo-robot scenarioinMATLAB.TheparametersusedinthesimulationarelistedinTable3.1.ForEKF,the initialconditionforthestateestimatesischosenas » 0 Ł 5 Œ 0 Œ 0 ¼ ) ,wherethe˝rsttermischosenas apositivevalueclosetotheexpectedvoltageattheLOS,andtheothertwotermsareeachchosen tobezeroasitisanunbiasedinitialcondition.Thematrices & and ' arethescaledversionof thenoise-covariancematricesofthesystem.Inourpriorwork[45],wecomparedtheperformance 49 ofEKFovermultiplevaluesofscaling-coe˚cientof & ,anditisshownthatthescaledversions improvetheperformanceoftheEKFalgorithm.Theexactvaluesofthematricesarechosen empirically.Theinitialerror-covariance % 5 0 ischosentobesameas & . First,weconsiderthecasewherethetworobotsarestationaryandseparatedabouttwometers apartinthe3Dspace,suchthatinitially,noneofthemisalignedwiththeLOS.Figure3.7 summarizestheevolutionofthestatesandtheirestimates,themeasuredlightintensityandits estimate,andthescanning/perturbationamplitudebyeachrobotduringthecourseofthealgorithm execution.Itisobservedthat,inthecaseofEKF,forbothrobots,thestates G 2 and G 3 ,andtheir estimatesconvergetoaneighborhoodofzeroinaboutfortyseconds.FortheES,thestatesconverge toaneighborhoodofzeroinabouttwentyseconds.Thebottomsub-˝guresshowthescanning amplitudevalues( X A )forEKF,alongwiththeconstantperturbationamplitude( X ˆ( )forES.They illustratethat,whenarobotisintheactivephase,ithasahighvalueof X A ,whiletheotherrobot's X A iszero,whichsigni˝esitspassivephase.Initially,whentheuncertaintyintheestimatesis high,thevalueof X A ishigh( 4 ),andwhenthestatesreachthesteady-state,thevalueof X A inthe activephasechangesbetween 2 and 3 .Itcanbeinferredfromtheplotsthatthealternatingand proportionalnatureofthescanningtechniqueintheEKFapproachhelpsin ‹ achievinglowersteady-stateerrorandhigh-intensityvalues,and ‹ reducingthee˙ortbytheactuators, ascomparedtotheES-basedapproachwheretheperturbationamplitudeisconstant,whichresults inthehighersteady-stateerrorandcontrole˙ort. Next,toassessthealgorithms'repeatabilityandstudythee˙ectofthedistanceonthetracking performance,wehaveconductedmultiplesimulationrunswithrobotsstaystationaryforarangeof distances.Tocharacterizethee˚ciencyoftracking,weconsiderametriccalled averagepointing error E ,whichistheaverageoftheheadingo˙setangle b duringarunfrombothoftherobots, 50 Figure3.7: Plotofasimulationrunillustratingtheevolutionofthethreestatesandmeasured intensityfortheEKFandtheESapproach,andtheirEKF-estimates,andscanning/perturbation amplitudeforeachrobot,whentherobotsarestationary.Theangularstates: G 2 , G 3 ,andtheir estimatesareaugmentedwithscanningterms V and U toillustrateafaircomparisonwiththe angularstatesoftheESapproach. calculatedafterthesteady-stateisachieved,namely E = 1 2( = <0G = ; +1) 2 X 8 =1 = <0G X : = = ; b 8Œ: Œ where = <0G denotesthetotalnumberofiterationsineachrun(weuse200)and = ; isthetimeindex ofaniterationthatisinsidethesteady-state(161isusedheretocapturethelasttwentypercentof iterations).Furthermore,weconsideranothermetriccalled averageintensity I ,whichconsiders theaverageintensitymeasurementandisde˝nedsimilarlyto E : I = 1 2( = <0G = ; +1) 2 X 8 =1 = <0G X : = = ; y 8Œ: Œ Figure3.8showsthee˙ectoftheincreasingdistanceontheaverageintensityandtheaverage pointingerror.Italsoshowsthecurveof I LOS ,whichisthemaximumattainablelightintensity atadistance,achievedforthecaseofperfectLOSbetweentherobots. I LOS exhibitsaninverse quadraticdecreaseasperEq.(3.3).ForEKF,theaverageintensity I exhibitsasimilardecreasing trendwiththedistance,anditstayscloseto I LOS .Thepointingerror E stayslowforthedistance 51 from1mto3m,andthenitstartsincreasinglinearlywiththedistance.Thisdegradationisdue tothedecreasingsignaltonoiseratio(SNR)astheintensitymeasurementdecreasesbytheinverse squarelawwiththedistancewhilethelevelofthemeasurementnoisestaysconstant. FortheESalgorithm,itisobservedthatatlowdistances,thepointingerrorbecomessigni˝cantly high,andthatresultsinalowaverage-intensityvalue.Atalowdistance,theproductofgain ˆ( and thehigh-pass-˝lteredoutputsignalbecomeshigh,anditresultsintheinstabilityoftheESalgorithm. ItillustratesthattheESapproachwouldrequirevariedvaluesofgain ˆ( atdi˙erentdistances; however,itisnotpracticalforarealscenariowheretheoperatingdistancebetweentherobotscould changeandmaynotbeknownforeitherrobot.InthecaseofEKF,theestimateofthestate G 1 ac- countsforthechangeinintensityduetothechangeinthedistance,andhencetheapproachworksfor awiderangeofthedistances.Forthecurrentvalueof ˆ( ,theESalgorithmshowsminimumpoint- ingerrorbetweenthedistanceof2mand3m,andtheerrorstartsincreasingbeyondthreemeters. Inapracticalscenario,therobotswouldbemoving;however,inourformulationofEKF,we assumedaquasi-staticrelativemotionbetweentherobots.Thisassumptionwouldbeviolated whentherobotsmoveatalargerspeed.Therefore,toexplorethein˛uenceofrobotmovements ontheLOSalignmentperformance,thealgorithmsarefurthertestedwiththerobotsmovingata rangeofspeeds.Figure3.9illustratesthecon˝gurationofthemotionofthetworobots,whichis basedontheexperimentalsetuptobediscussedinSection3.4.Inthesetup,therobot R 1 ,which wecallthe Rover ,movesonahorizontalplane,andtherobot R 2 ,whichwecallthe Elevator ,moves upward.Initially,boththerobotsareplacedatthepointsofminimumdistance 3 min betweenthe linearpathsofthem.Eachoftherobotsmovesatacertainspeed,andsincethedirectionsoftheir motionareorthogonaltoeachother,therelativespeed B iscomputedbytakingthesquare-rootof thesumofsquaresoftheirspeeds( q B 2 1 + B 2 2 )where B 8 denotesthespeedoftherobot R 8 . Next,weconsiderarangeofrelativespeedvaluesfortherobots.Ateachspeed,weperform 1000runsandcomputetheaverageandthestandarddeviationofthepointingerror E .Dueto therobots'movingnature,thedistancebetweenthemchanges,andhencethemaximumattainable intensitychangesthroughthecourseofarun,sotheaverageintensity I isnotconsideredinthese 52 Figure3.8: Trackingperformanceintermsofaverageintensity I andaverageerror E insimulation overarangeofdistancesbetweentherobots.Theerrorbarsdenotethestandard-deviation.The intensityatLOS( I LOS )isalsoshownforreference. setofsimulations.Figure3.10illustratestheaveragepointingerrorovertherangeofspeed E .Itis observedthatatlowspeeds,thepointingerrorforEKFstayslowerthantheerrorforES;however,as thespeedincreases,theerrorforEKFincreasesatahigherrateascomparedtoES.Furthermore,the EKFalgorithmstopsconvergingathighspeedsthatresultsinhighpointingerrors.Thisbehavior illustratesthelimitationoftheEKFalgorithmwhenthequasi-staticassumptionsonthestatesare violated.For,ESalgorithm,theerrorstaysrelativelylowasthegain ˆ( is˝nelytunedforthe distancerangeoftwotothreemeters(thedistance 3 staysinthisrangewhentherobotsmove),soit exhibitsfastconvergenceandhencecantracktheLOS,undertherelativelyfastmotionoftherobots. 53 Figure3.9: Illustrationoftheinitialcon˝gurationofmovingrobots(denotedbyspheres)placed 3 <8= distanceapartinsimulation.Theelevatorrobotmovesupward,andtheroverrobotmoves horizontallyinadirectionorthogonaltothelinejoiningtherobots'initiallocations. Figure3.10: Trackingperformanceintermsofaverageerror E insimulationoverarangeofspeeds oftherobots. 54 Table3.1: Parametersusedinthesimulation.Thevaluesofparametersmarkedas' « 'arechosen empirically. ParameterValueDescription ^ G 5 0 » 0 Ł 5 Œ 0 Œ 0 ¼ ) Initialvalueofthestateestimates % 5 0 diag ( » 10 Œ 900 Œ 900 ¼ ) « Initialerror-covariancematrix f F 0 Ł 009 Standard-deviationofsystem's processnoiseforangularstates G 2 and G 3 & diag ( » 10 Œ 900 Œ 900 ¼ ) « EKF'sprocessnoise-covariance matrix f E 5 Ł 15e 4 Standard-deviationofsystem's measurementnoise ' 1 « EKF'smeasurementnoise- covariancematrix » ˝ % Œ˝ ˚ ¼» 0 Ł 98 Œ 0 Ł 2 ¼ PIcontrollergains X ; ŒX [2 Œ 5] « Scanningamplitudelimits X k 120 « Scanningangularstep-sizeinde- gree X 0 Ł 0025 « Proportionalscanninggain ) 500 Œ 30 > º ¹ 30 > Œ 30 > º )usingthe LatinHyper-cubeSampling(LHS)techniqueinMATLAB,andperformasimulationrunforeach oftheseinitialconditions,foreveryvalueofangularspeed.Itistobenotedthatweconsidered onlythepositivevaluesofthestate G 1 ,asthedisturbanceispositiveandtherefore,itactstomove 92 Figure4.22: Comparisonofthetrajectoriesofasamplesimulationrunforthethreealgorithmsin thepracticalscenario. 93 Figure4.23: Illustrationoftheevolutionofthestatesforthethreealgorithmsinthepractical scenario. thestatesawayfromtheorigin.Asimulationrunislabeledasconvergedif,atanyiteration : ,the stateslieinsidetheset D [ .Theperformanceateachspeedismeasuredbycountingthenumberof convergedsimulationruns( N ˘ )andtheaveragetrackingerroroftheconvergingruns( E ˘ ),which isde˝nedforasimulationrunasfollows: E ˘ = 10 = 5 = 5 X : = = B k x : k 2 Œ with = B = = 5 = 5 € 10+1 where = 5 isthetotalnumberoftime-stepsinonesimulationrunofanalgorithm(thevalueof 94 Figure4.24: Illustrationoftheoutputsforthethreealgorithmsinthepracticalscenario. 5000 isusedinthesimulationruns).The˝nal 10% iterations( = 5 € 10 )areconsideredtoe˙ectively capturetheaverageofsteady-stateoscillatingpoints.Anewtermcalled disturbancefraction denotedby j isintroduced,thatequals l )€X torepresenttherelativestrengthofthedisturbance incomparisonwiththespeedofrobots'actuation. Figure4.25showsthecumulativeperformanceofthethreealgorithmsintermsof N ˘ and E ˘ overarangeof j .Themeantrackingerror E ˘ isplottedforanalgorithmforavalueofdisturbance strengthonlywhenmorethan 10% oftherunsshowconvergence.Allthealgorithmsshowa decreasingtrendofconvergencecountasthedisturbancefraction j increases.Theconvergence countoftheproposedalgorithmremainshighestthroughouttherange,withcloseto 100% runs convergingforthelowrangeofthedisturbances.Furthermore,themeanandstandarddeviation oftheerror E ˘ stayslowestfortheproposedalgorithminthelowrangeofdisturbances.The 95 convergencecountoftheEKFalgorithmstartswithabout 600 thatgraduallydecaystoabout 100 . From j =0 Ł 15 onward,themeantrackingerrorofEKFstaysclosetozerowithconsistentvalues ofthestandarddeviation.Itoutlinesthatthesystemstatesfortheconvergingrunsstaymoderately closetotheoriginthroughouttherun.Thestandard-deviationof E ˘ valuesstayssigni˝cantlyhigh formostofthe j values.TheperformanceoftheESalgorithmispoor;itsconvergencecountstarts withabout 400 anddropsbelow 10% atalow j valueof 0 Ł 10 .Thereforeonlytwodatapointsof E ˘ areplotted.Asdiscussedearlier,theESalgorithmissigni˝cantlyslowerthantheothertwo algorithms,andhenceitfailstoconvergeinthepresenceofdisturbance. Figure4.25: Illustrationofperformanceofthealgorithmsintermsofconvergencecount N ˘ andtrackingerror E ˘ overarangeofdisturbancefraction j .Theerrorbarsfor E ˘ denotethe standard-deviation. Tostudythee˙ectoftheinitialconditionsontheconvergenceperformance,weconsidereda rangeofmagnitudeofinitialconditions.Foragivenmagnitude V , 1000 pointsaregeneratedusing 96 theLHStechniqueonacircleofradius V centeredaroundtheorigin.Itistobenotedthatour domain representsangles,andhencetheradius V isexpressedindegrees.Anewmetriccalled convergencetime T ˘ ,de˝nedasthetimetakentoconvergeinasimulationrun,isconsideredto evaluatetheperformance. Figure4.26showstheconvergenceperformanceofthealgorithmsoverarangeofmagnitude ofinitialconditions.Forlowinitialconditionsuntil 20 ,allofthesimulationrunsresultin convergenceforeachofthealgorithms.At V =30 ,theperformanceoftheEKFandthatoftheES algorithmsstartdegradingwheretheESalgorithmhasahigherconvergencecount.At V =60 ,the convergencecountsofboththealgorithmsdiminishtolessthan 10% .Theconvergencecountof theproposedalgorithmalsodegradesbutsigni˝cantlyslowerthanthetwoalgorithms,anddespite thehighinitialconditionof V =90 ,morethanhalfofthesimulationrunsresultinconvergence. Furthermore,itisobservedthattheconvergencetime( T ˘ )increaseswiththemagnitudeofthe initialconditions V .Additionally,thestandarddeviationof T ˘ alsoincreaseswith V ,which isattributedtotheincreaseinthestochasticityofthemeasurements;athighinitialconditions, themeasurementvaluesbecomesmall,andthemagnitudeofnoisebecomescomparabletothe measurements.Thisdecreaseinsignal-to-noiseratio(SNR)contributestothestochasticityinthe dynamicsandhenceresultsinthepoorperformanceofthealgorithms.Moreover,forhighinitial conditions,thecontrolstepsforeachagentfortheEKFalgorithmarelarge,whichviolatesits quasi-staticassumptiononthesystemdynamics.Thisviolationfurthera˙ectsthefailurerateof convergenceofthesimulationruns,evenfortherelativelysmallerinitialconditions.Onasidenote, itcanbeinferredthattheconvergingrunsoftheEKFalgorithmobservedearlierinFigure4.25 aretheonesthatstartedwithsmallerinitialconditions.Themeantrackingerror E ˘ remainssmall fortheESandtheproposedalgorithm;however,fortheEKF,theerrorincreaseswiththe V for thereasonthatasigni˝cantfractionoftheconvergingrunsdivergesawayfromtheorigindespite comingclosetotheorigin. Henceforth,wehaveseenfromthenumericalresultsthattheproposedapproachsigni˝cantly outperformstheEKFandtheESalgorithmintermsofconvergencespeedandrobustnessto 97 noiseandunknowndisturbancesandlargeinitialconditions.Moreover,itistobenotedthatthe proposedalgorithmiscomputationallyeconomicalthantheothertwoalgorithms,whichbecomes advantageousinreal-timeonboardimplementationasitplaysamajorroleindecidingthesampling time ) . Figure4.26: Illustrationofperformanceofthealgorithmsintermsofconvergencecount N ˘ , convergencetime T ˘ ,andtrackingerror E ˘ overarangeofmagnitudesofinitialcondition V .The errorbarsfor T ˘ and E ˘ denotesthestandard-deviation. 98 4.5ExperimentalResults Inthissection,wetestthee˚cacyoftheproposedalgorithminexperiments.We˝rstdescribe thedetailsoftheexperimentalsetupusedinthisworkandthendiscusstheresults. Figure4.27illustratesthehardwarecomponentsofoneofthetworobots.Forthetransmitter, aCreeXR-ESeriesBlueLEDwithaprincipalwavelengthof480nmisused,andforthereceiver, ablue-enhancedphoto-diodefromAdvancedPhotonix(partnumberPDB-V107)isused.A DYNAMIXEL ® servomotor(modelnumberXL430-W250-T)isusedtocontrolthepointingangle ofthetransceiver.ABeagleboneBlue ® boardisusedasanon-boardcomputerforreal-time processingandcomputation.TherobotinFigure4.27isplacedonametaldiscthatserves asarotatingbasetoemulaterelativeangularmotionbetweenthetworobots.Anotherrobot withthesamecon˝gurationonastaticplatformisusedasthesecondagent.Furthermore,we usedOptiTrack ® motioncapturesystemtoaccessthegroundtruthvaluesofthesystem'sstates. Figure4.27showsthere˛ectivemarkersplacedonthetopoftherobotandonthebasedisc,which areusedbythemotioncapturesystem.Theillustratedrobotislabeledas R 1 ,andthecopyofthis robotislabeledas R 2 ,whichisplacedonastaticbase.Figure4.28,showstheexperimentalsetup consistingofrobots R 1 and R 2 ,thatareplaced2.8metersapart.Theexperimentsareperformed inadarkroomtominimizeambientopticalnoise. ThecodetoimplementtheproposedalgorithmsetupiswritteninPython,whichisthenexecuted ontheon-boardcomputerofeachrobot.Figure4.29andFigure4.30showtheresultsofasample experimentrunwherethediscisrotatingwithanangularspeedof l =1 € s ( j =0 Ł 23) .Thepath ofthetrajectoryofthestatesofthesystemisshowninFigure4.29,andtheevolutionofthestates andtheintensitymeasurementsisshowninFigure4.30.Thestartingpoint S denotestheinitial condition,wherethetransceiversofboththerobotswerepointingawayfromtheLOS.Duringthe courseofthealgorithm,thestatesreachtheneighborhood D [ oftheorigin O ,inabout 10 s,and thentheyoscillatearoundtheoriginfortherestoftheexperimentrun.Theconvergenceofthe statestotheset D [ indicatestheachievementofthenearLOSbythetransceiversoftherobots.The robots'signalstrengthmeasurementsstartfromlowinitialvalues,andthentheyreachsteady-state 99 Figure4.27: Illustrationofarobotwiththeopticaltransceiversystem.Therobotstandsona rotatingdisctoemulatetherelativemotionbetweentworobots. oscillationsataround 0 Ł 4 VwhenthesystemreachestheneighborhoodofLOS. Next,totesttherepeatabilityoftheexperimentsandlimitationsoftheproposedalgorithmon theexperimentalsetup,weconsiderarangeofangularspeedsforthediscsimilartotherangeused inthesimulation.We˝rstperform 10 experimentalrunswithpracticallythesameinitialcondition foreachoftheangularspeeds.Weperformadditional 20 experimentalrunsiftheinitial 10 runs exhibitbothsuccessandfailureinconvergenceinordertoobtainbetterstatisticalmeasures.The signoftheinitialangle G 1 Œ 0 ofrobot R 1 ischosentobethesameasthesignofthe l sothatthe rotationofthediscmovesthepointingdirectionof R 1 awayfromtheLOS.Henceforth,allofthe 100 Figure4.28: Illustrationofexperimentalsetupofthetwo-robotsscenario.Theoverheadlightsof theroomareturnedo˙tominimizetheambientopticalnoise. experimentrunsstartfromtheinitialcondition x 0 = » 16 Œ 23 ¼ ) . Figure4.31showstheresultsoftheexperimentruns,performedoverarangeofrotational speedsofthediscintermsofpercentageofconvergingrunsalongwiththemetrics T ˘ and E ˘ whichwerede˝nedinSection4.4.The G -axisofthesub-plotsrepresentsthediscspeedinterms ofthedisturbancefraction j .Thenumberaboveeachofthebarsrepresentsthetotalcountof experimentrunsperformedatthatvalueof j .Similartothesimulation,the T ˘ and E ˘ areonly plottedataparticularvalueof j ,iftheconvergencecountisgreaterthan 10% ofthetotalcount.It isobservedthatatlowdisturbancevaluesuntil j =0 Ł 19 ,thesuccessrateofconvergenceis 100% , andthenitdecreasesgradually.Theaverageofconvergencetime T ˘ remainsbetween 10 sand 20 still j =0 Ł 44 ,astheinitialconditionisthesameforalltheruns.Basedonconsistentvalues, itisinferredthatwhenanexperimentrunconverges,itislikelytoreachtheLOS-neighborhood 101 Figure4.29: Pathofthetrajectoryofasampleexperimentrunwhenthebasediscrotateswiththe angularspeedof l =1 € s. inacertaintimelimit.Beyondthat,itisextremelyunlikelytoconvergeasduetotheplatform's rotation,thepointingdirectionofthetransceiverofrobot R 1 movesfarawayfromtheLOSwhere thee˙ectofthemeasurementnoiseandthemotoruncertaintybecomesprominent.Theaverage trackingerror E ˘ staysatconsistentlylowvalueofabout 4 degreesuntil j =0 Ł 31 .Beyondthat value,itstartsincreasinggradually,suchthatat j =0 Ł 56 ,itsvalueaveragingat 35 degreeswith signi˝cantlyhighstandard-deviation.Thesehighvaluesathighdisturbancesdepictthatthesystem leavestheLOS-neighborhoodafterconvergencemostofthetime. 102 Figure4.30: Evolutionofthesystem'sstatesandtheoutputcorrespondingtothepathofthe sampleexperimentrunshowninFigure4.29. 4.6ChapterSummary Inthiswork,weformulateabidirectionalopticalbeamtrackingproblemasadiscrete-time dynamicalsystem.Weproposeamodel-freeoutputfeedbackcontrollawforaclassofsystemsthat followcertainassumptionswiththeconstraintthatthecontrolcommandofanagentcandepend onlyontheinformationaccessibletothatagent.Throughrigorousanalysis,weshowthatforany initialcondition,theproposedcontrollawdrivesthesystemtoade˝nedset D [ ina˝nitenumber ofsteps.ForourphysicalsetupwithGaussianmeasurementfunctionchoices,wegetstronger resultsregardingtheconvergencetowards O ( X ) neighborhoodoftheoriginthatcorrespondsto LOS.Theproposedalgorithmiscomputationallyeconomicalthanthetwocontendingapproaches: 103 Figure4.31: Illustrationofperformanceofthealgorithmsintermsofconvergencecount N ˘ andtrackingerror E ˘ overarangeofdisturbancespeeds.Theerrorbarsfor E ˘ denotesthe standard-deviation.Thenumberaboveeachofthebarrepresentsthetotalcountofexperimentruns performedatthatangularspeed. ESandEKF,andsuperiorintermsofconvergencespeed,robustnesstounknowndisturbances,and handlinglargeinitialconditions.Theproposedapproachisalsovalidatedonanexperimentalsetup consistingoftworobotsinthepresenceofaconstantexternaldisturbance. 104 CHAPTER5 SIMULTANEOUSBI-DIRECTIONALALIGNMENTCONTROLINTHE3DSPACE Motivatedbythemodel-freeapproachforthebi-directional2DsettingthatisproposedinChapter4, inthiswork,weexploreamodel-freebi-directionalactivealignmentcontrol-basedapproachforthe 3Dsetting.Utilizingtheuni-modalnatureofthedependenceofthelightsignalstrengthonlocal angles,weproposeanoveltriangularexplorationalgorithm,thatdoesnotrequiretheknowledge oftheunderlyinglightintensitymodel,tomaximizethesignalstrengththatleadstoachievingand maintainingLOS.Themethodmaintainsanequilateraltriangleshapeintheanglespaceforany threeconsecutiveexplorationpoints,whileensuringtheconsistencyofexplorationdirectionwith thelocalgradientofsignalstrength.Theapproachcanbedirectlyimplementedontworobotsfor bi-directionalsetting,withouttheneedofanysynchronizationbetweentherobots. Thee˙ectivenessoftheapproachis˝rstevaluatedinthesimulationsetupoftworobots, whichwaspresentedinSection3.3,bycomparisonwiththeEKF-basedapproachandtheES approach.Simulationresultsshowthattheproposedapproachisoptimalande˙ectiveinterms ofitsconvergencespeedforawiderangeofrelativespeedanddistancebetweentherobots.The performanceoftheapproachisfurtherassessedagainsttheEKFapproachandtheESapproach ontheexperimentalsetup,whichwaspresentedinSection3.4.Theexperimentalresultsfurther supportthesuperiorityoftheapproachwiththeothertwocontendingapproaches. Thee˚cacyoftheapproachandtheoverallcommunicationsystemisfurtherdemonstratedin underwatersettingwherecommunicationisperformedsimultaneouslywiththealignmentcontrol. Onewaycommunicationandalignmentare˝rstdemonstratedonasetupwhereahumanoperator wirelesslycontrolsarobotintheunderwaterscenariousinganLEDcommunication-basedjoystick, followedbydemonstrationofbi-directionalalignmentcontrolandcommunicationbetweentwo underwaterrobots. Theorganizationoftherestofthechapterisasfollows.Section5.1reviewsthebasicproblem setup.ThedetailsofthealignmentalgorithmisdiscussedinSection5.2.Simulationresults 105 arepresentedinSection5.3.In-airexperimentresultsareprovidedinSection5.4,followedby thediscussiononunderwaterexperimentalsetupandresultsinSection5.5.Finally,concluding remarksareprovidedinSection5.6. 5.1ReviewofSystemSetupandModeling Figure5.1: Hardwaredescriptionoftheactivetransceivermodule. Here,we˝rstbrie˛yreviewthehardwaresetupandsystemmodelfromChapter3anddescribe therelevantmathematicalrepresentationofthesystembehavior.Forsimplicityinformulation, weconsiderthemodelofeachrobotseparatelyinthiswork.Figure5.1showsthehardware setupconsistingoftheLED-photodiodepairinthetransceivermodulethatismountedona2DOF activepointingmechanism.Thesamesetupwasusedintheexperimentswhichwerepresentedin Chapter3.Next,wederivethestatesofthesystemforonerobotfromEq.(3.4): x = " G 2 G 3 # = " q \ # Œ (5.1) where q and \ aretheazimuthandelevationcomponentofrelativeanglebetweenLOSandheading directionofthetransceiver.Thestate G 1 fromEq.(3.4)isomittedfromthestatevectoranditis 106 consideredasaconstant C @ inthiswork.Nowthesystemoutput,whichisderivedfromEq.(3.6), canbeexpressedintermsofthestatevariablesas y : = C @ 6 arccos cos G 2 Œ: cos G 3 Œ: Ł (5.2) Theevolutionofsystemcanbeexpressedinthediscretetimeas: " G 2 Œ: +1 G 3 Œ: +1 # = " G 2 Œ: + D 2 Œ: G 3 Œ: + D 3 Œ: Œ # (5.3) where D 2 Œ: and D 3 Œ: denotethecontrolterms.Thenoisetermsareomittedfromthedynamicsand theoutputequationsforsimplicityintheformulation. 5.2ATriangularExplorationAlgorithm Inthissection,wediscussthedetailsoftheproposedalignmentcontrolalgorithmthatistermed asa TriangularExploration algorithm.Givenanyinitialcondition [ G 2 Œ 0 ŒG 3 Œ 0 ] ) ,wechoose " D 2 Œ 0 D 3 Œ 0 # = " X cos( k 0 ) X sin( k 0 ) # Œ " D 2 Œ 1 D 3 Œ 1 # = " X cos( k 0 + k 0 ) X sin( k 0 + k 0 ) # where X¡ 0 isthestep-size,with k 0 chosenrandomlyfrom ( cŒc ] and k 0 = 2 c 3 ,where thesignischosenrandomly.Theaboveinitializationplacesthe˝rstthreevaluesofthestates ( x 0 Œ x 1 Œ and x 2 )inanequilateraltrianglepattern.Now,de˝nethecontrollaw u : =[ D 2 Œ: ŒD 3 Œ: ] ) as follows: u : = 8 > > > >< > > > > : x : 1 x : 2 Œ if y : 0 x : 2 x : Œ if y : 0 Œ (5.4) where y : = y : + y : 1 2 y : 2 .Thecontrolalgorithmwiththeinitializationensuresthatthenext, presentandthepreviousstatesformverticesofonanequilateraltriangle,asillustratedinFigure5.2. Considerthreepoints,illustratedinFigure5.2as, x : Œ x : 1 and x : 2 forminganequilateraltri- anglewithside X .Thereareonlytwopossibilitiesforthenextpoint x : +1 ;when y : 0 ,the nextpoint G 1 : +1 comesbacktothesecondpreviouspoint x : 2 ,andwhen y : 0 thenextpoint G 0 : +1 completestherhombuswiththelastthreepoints.Further,theapproximategradientatpoint 107 Figure5.2: Illustrationofthetriangular-explorationmethod. " : (midpointofsegmentjoining x : 1 and x : )alongthelocalcoordinateaxes ^ G : (directionof x : x : 1 )and ^ H : (directionorthogonalto ^ G : ),computedby˝nitedi˙erenceis r y : = 2 6 6 6 6 4 y : y : 1 X y : + y : 1 2 y : 2 p 3 X 3 7 7 7 7 5 Ł Thesecondcomponentofthegradientisascalarmultipleof y : ,andthenextpointalwayslies onthelocal ^ H : axis,whichisorthogonaltothepreviousdirectionofmotion,intheincreasing directionofthecomponentofthegradientin ^ H : . 108 Figure5.3: Illustrationofthepathofasimulationrunoftriangular-explorationalgorithm. 5.3SimulationResults Inthissection,we˝rstsimulatetheone-sidedTriangularExplorationapproachonMATLAB whereatransceivertracksastaticlightsource.Thenwesimulatethebi-directionalscenarioona systemoftworobots,theapproachisimplementedoneachrobotindependently.Wethencompare theperformanceofthetriangularexplorationapproachwiththeEKFbasedapproachandthe Extremumseekingapproach. 109 Figure5.4: Illustrationoftheevolutionofstatesandoutputforthesimulationrunoftriangular- explorationalgorithmcorrespondingtoFigure5.3. Figure5.4illustratesthepathofthestatesofthesystem,startingfromtheinitialcondition S .Thepathfollowsazigzagpatterninthebeginningforsometimeandthenitcorrectsitself frequentlytofollowthegradientandreachtheneighborhoodoftheorigin.Figure5.4illustratesthe evolutionofstatesandtheoutputofthesystemcorrespondingtothepathinFigure5.3.Thestates andtheoutputconvergestotheneighborhoodinabout20sandthentheycontinuetooscillatein theregion.Theoutputattainsthesteadystatevalueof1Vwithsomeminor˛uctuations. Next,weconsiderthesetupoftworobotsdescribedinSection3.3,wheretherobotsare stationaryandseparatedabouttwometersapartinthe3Dspace.Initially,noneoftherobots'heading directionisalignedwiththeLOS.Thesamesetupisusedtofairlyevaluatetheperformanceof 110 Figure5.5: Illustrationofpathsofthestatesforthethreealgorithmsforeachrobot,forasimulation runwhentherobotsarestationary. theTriangularExploration(TE)AlgorithmincomparisonwiththeEKFandES-basedalgorithms. Fig5.5showsthepathofthestatesofeachrobotsforthethreealgorithms.Thestatesstartfrom initialcondition S andreachesaneighborhoodoftheorigin O .Itcanbeobservedthatforeach oftherobots,thepathofTEapproachinitiallyfollowsazigzagpatternanditgetscorrected frequentlysuchthatit˝nallyreachesaneighborhoodaround O andcontinuouslyoscillateswithin theneighborhood.Figure5.6illustratestheevolutionofstatesandoutputforthethreealgorithms correspondingtoFigure5.5.ItcanbevisuallyobservedthatthespeedofconvergencefortheTE approachbetterthantheothertwoapproaches. Nextwecomparetheperformanceofthealgorithmsonrepeatedsimulationrunsoverarangeof distanceandrelativespeed.Weusethesamesimulationsetupandconsiderthesameperformance metricsforevaluationasdescribedinSection3.3.Figure5.7showsthee˙ectofincreasingdistance ontheaverageintensity I andtheaveragepointingerror E .ItisobservedthattheaverageerrorE fortheTEapproachstaysrelativelyconstantfortherangeofdistances.ForlowdistancestheEKF 111 Figure5.6: Illustrationofevolutionofthestatesandoutputforthethreealgorithmsforeachrobot, forasimulationruncorrespondingtotheFigure5.5. approachshowsminimumerroramongthethreeapproaches;however,theerrorofTEapproach becomestheminimumbeyond4m.Theaverageintensity I oftheTEapproachshowsatrendthat issimilarto I oftheEKF,andisclosetothemaximumattainableintensity I !$( .Theseresults validatetheoperationale˚cacyoftheTEapproachoverawiderangeofdistance. Figure5.8showstheaveragepointingerroroverarangeoftherelativespeed E .Theerror fortheTEapproachstaysconstantandclosetotheerroroftheESapproach.TheEKFapproach haslowerrorforlowerrangeofspeed;however,asthespeedincreases,theerrorstartsincreasing andbecomessigni˝cantlyhighforhigherspeeds.Now,fromtheperformanceoftheTEapproach exhibitedinFigure5.7andFigure5.8,wecanconcludethattheTEapproachissigni˝cantlybetter 112 Figure5.7: Trackingperformanceofthethreealgorithmsintermsofaverageerror E andaverage intensity I insimulationoverarangeofdistancesbetweentherobots.Theerrorbarsdenotethe standard-deviation.TheintensityatLOS( I !$( )isalsoshownforreference. thantheothertwoapproachesintermsofitswiderangeofoperationinbothdistanceandspeed. 5.4In-airExperimentresults Inthissection,weevaluatetheperformanceofthethreealgorithmsontheexperimentalsetup, whichisdiscussedinSection3.4.Figure5.9illustratepathsofthestatesofeachrobotforthethree algorithm.Thecharacteristicsofthepathsissimilartowhatisobservedinsimulation;however, thesizeofthesteady-stateneighborhoodfortheTEapproachisbiggerthanwhatisobservedin simulation.Additionally,fortheelevatorrobot,thepathoftheTEapproachinitiallygoesaway 113 Figure5.8: Illustrationoftrackingperformanceofthethreealgorithmsintermsofaverageerror E insimulationoverarangeofspeedsoftherobots. fromtheoriginandafterfewcorrections,itgetssteeredtoaneighborhoodoftheorigin.Figure5.10 showstheevolutionofthestatesandtheoutputcorrespondingtoFigure5.9.Here,itisobserved thatthespeedofconvergencefortheTEapproachisclosetothespeedoftheESapproach. Next,weconsidertherangeofrelativespeedsoftherobots.Figure5.11showstheplotsof theaveragepointingerror E overtherangeofspeed E forthethreealgorithms.Thequalitative characteristicsoftheplotsaresimilartowhatisobservedinthesimulation.Initially,atlowspeeds theEKFapproachshowminimumerrorandasthespeedincreases,duetotheviolationofthe quasi-staticassumptionofthestates,theEKFfailstoconvergeandproduceshightrackingerror. TheerrorfortheTEapproachstaysrelativelyconstantthroughtherangeofthespeedandstays minimumformid-rangeandhighrangeofspeeds.FortheESapproach,theerrorstaysclosetothe erroroftheTEapproach,butitstartsincreasingmoderatelywiththeincreaseoftherelativespeed. 114 Figure5.9: Illustrationofpathsofthestatesforthethreealgorithmsforeachrobot,forexperiment runswhentherobotsarestationary. Fromthesimulationandtheexperimentalresults,weestablishthattheTEapproachshows optimalperformanceintermsofthemeantrackingerrorovertherangeofspeedanddistance. Therefore,weusedthetriangularexplorationalgorithmforthealignmentcontrolintheunderwater setup,whichisdiscussedinthenextsection. 5.5UnderwaterExperimentsandResults Inthissection,wepresenttheexperimentswithsimultaneousalignmentcontrolandcommu- nicationthatareperformedintherealunderwaterscenario.Firstwepresentauni-directional setupwhichcomprisesanunderwaterrobotandanopticalwirelessjoystick.Thenwepresenta bi-directionalscenariocomprisingoftwo-robots. Figure5.12showsanunderwaterrobotequippedwiththetransceivermodule,describedin Section5.1,insideatransparentcasing.ItconsistsofthreeT100thrustersfromBlueRobotics:two forhorizontalmotionandtheotheroneforverticalmotionoftherobot.Therobotiskeptneutrally buoyantforeaseofoperation.Foron-boardprocessingandcomputation,aBeagleBone Š blue 115 Figure5.10: Illustrationofevolutionofthestatesandoutputforthethreealgorithmsforeach robot,forexperimentrunscorrespondingtotheFigure5.10. computerboardisused. Figure5.13showsanopticalcommunication-basedjoystickcontroller,whereaPS4joystick isintegratedwiththeLEDcommunicationcircuitry.Thejoystickisinspiredby diverinterface module [22],wherehigh-levelcommandsweretransmittedbyahumanscubadivertoarobotic˝sh usingacousticcommunication.Thejoystickinthisworkisintendedtodirecttheaforementioned robotintheunderwaterscenariobysendingcommands "goup","goforward","turnleft" ,etc. Thecommandscorrespondingtothebuttonspressedonthejoystickarereceivedbyanon-board BeagleBone Š bluecomputerthroughaUSBcable.Theboardthengenerateson-o˙keying(OOK) signalsbytheUART(UniversalAsynchronousReceiver/Transmitter)protocolatabaud-rateof 116 Figure5.11: Trackingperformanceofthealgorithmsintermsofaveragepointingerror E overa rangeofspeedsoftherobotsinexperiments. 115 Œ 200 bitspersecond(bps),whicharethentransmittedthroughtheLEDofthejoystick-controller. Inaddition,aprede˝ned dummy stringofcharactersisalwaystransmittedrepetitivelyfromthe LEDtogenerateaconstantaveragelightintensitythatfacilitatesactivealignmentcontrolatthe receiverendoftherobot. Ontherobot'send,theopticalsignalsarereceivedbythephoto-diodeandthetwocomponents, theaveragevalue H ,andtheinformationsignal,arepassedontotheon-boardcomputer,whichthen usesthetriangularexplorationalgorithmtosteerthetransceivertoalignwiththeLOS.Figure5.14 showstheplotsofthereceivedsignalintensity H alongwithbit-ratesofreceptionanderrorrates overthecourseofanalgorithmrun.Forthisexperiment,therobotisheldsteadyunderwater,and theLED-joystickisheldaboutameteraway,pointingatthetransceiver'slocationoftherobot.The controlleronlytransmitsthedummystring,sothatitiseasiertocomputethebiterrorrateonthe otherend.Thereceiveddata-rateiscomputedbydividingthenumberofbitsreceivedbythelength 117 Figure5.12: Anunderwaterrobotequippedwiththeactivetransceiver. oftheiterationinterval(0.5s).Sincetheexpectedincomingstringisknown,thebit-error-rateis computedbycalculatingthehammingdistancebetweenthereceivedandexpectedbitstrings. Atthebeginningoftheexperiment,thetransceiveroftherobotispointingawayfromthe LED-joystick,andhencethereceivedsignalsignal-strengthistoolow(lessthanthethresholdof thecomparator)forenablingcommunication.Astimeprogresses,thesignalstrengthincreases, andataboutthe 21 secondmark,thereceiverstartsreceivingsomebits.However,amajorportion ofthebitshaserroratthispoint.Here,thesignalstrengthmustbecomparabletothecomparator thresholdresultinginahighsignaltonoiseratio(SNR).Movingforward,atabout25seconds,asthe signalstrengthincreasesfurther,thebiterroreventuallydropstozero,andthereceivedinformation 118 Figure5.13: Auni-directionalLED-communicationbasedjoystickcontrollerdesignedforthis work. ratestabilizestoavalueofabout 100 kbps,whichisclosetothebaud-rateoftransmission.The di˙erencebetweentheratesisattributedtoaparitybit,whichisusedforbuilt-inerrorcorrection bytheUARTprotocol.Atabout30seconds,thesignalstrengthgetssaturatedaroundaconstant intensity;atthispoint,itisalsovisuallyobservedthatthetransceiverpointingdirectionoscillates aroundtheLOSwiththeLED-controller. ThesetupisfurthertestedinaswimmingpoolfacilityattheMichiganStateUniversity.The LED-controllerismadeoperationalunderwaterbysealinginsideatransparentplasticbagsothatthe buttonsremainaccessible,andthelightofLEDisnotblocked.Figure5.15showsahumanoperator directingtherobottonavigateinsidethepool.Therobotiscommandedtomoveforward,backward, turn,andmoveupward/downward.Itistobenotedthatthehumansupervisorsometimesmanually adjuststhepointingdirectionofthejoysticktore-establishLOSwiththerobot'stransceiverasthe LOSgetsdisturbedduetofastrelativemotionbetweentherobotandthecontroller. Next,wepresentthebi-directionalalignmentandcommunicationperformance,fortheunder- waterscenario,Figure5.16showstwocopiesoftheunderwaterrobotplacedabout1.5mapart inwater.Thetransmissionbeamsofboththerobotsarevisibleduetothescatteringoflightand 119 Figure5.14: Evolutionoflightintensityanddataratesforanexperimentrunoftriangular explorationalgorithmontheexperimentsetup.Thereceiveddata-rateiscorrelatedwiththe signal-strength. hencetheyareconsideredasvisualindicatorsofthealignment. Figure5.17showstheplotsofreceivedsignalstrength,receiveddata-rateanderror-rate.In thebeginningoftheexperiment,bothoftherobotsarepointingawayfromthedirectionofLOS, andeachoftherobotsistransmittingthedummystringfromitstransceiverat 57 Œ 600 bps.From theplots,itcanbeobservedthat,initiallythesignalstrengthforboththerobotswaslow(lessthan 0.1V)andasthetimeprogressesthestrengthincreasesandoscillatesbetween0.2to0.5V.The receivedsignalrateduringtheseoscillationgetsstablebetween30and40kbps,whichiscloseto thetwo-thirdofthetransmissionrate.Thedi˙erencehereisduetofourextrabits(twoparityand twostopbits)pereight-bitsequence,whichresultsinthee˙ectivedatarateof 38 Œ 400 bps.The observedmagnitudeofoscillationsinlightintensityishigherthanwhatisobservedinthe in-air 120 Figure5.15: UnderwaterrobotbeingcommandedbyahumanoperatorusingtheLED-joystick controllerinsideaswimmingpool. Figure5.16: Setupoftwounderwaterrobotswhicharecommunicatingandactivelyaligningwith eachother. 121 Figure5.17: Illustrationoflightintensitymeasurementsanddataratesforanexperimentrunon thesetupshowninFigure5.16. experiments.Thisdi˙erencecanbeattributedtochangeinlight-intensitymodel,largelydueto refractioncausedbymultiplechangesofmediuminthepathofthetransmittedbeaminitspath beforereachingthephoto-diodeatthereceivingend. Figure5.18showsthesetupoftworobotsinaswimmingpoolsettingatadepthof3.7m.The setupisintendedtoperformthebi-directionalalignmentcontrolandcommunicationexperiments. TherobotsmaintainedtheLOSforsometimebutfailedtomaintainthealignmentthroughoutthe run.Possiblereasonsforthefailureincludetheinterferencefromambientsunlightinthepooland theleakageofwaterinsideoneoftherobotsa˙ectingtheoverallfunctionalityoftherobot. 122 Figure5.18: Setupoftwounderwaterrobotsforbi-directionalalignmentandcommunicationin theswimmingpool. 5.6ChapterSummary Inthiswork,wepresentamodel-freeactivealignmentcontrolapproachthatisdirectlyapplicable tobi-directionalsetting.Giventhatthedependenceofreceivedlightintensityonthelocalanglesis unimodal,weproposeatriangular-explorationalgorithmforreal-timealignmentcontroltoachieve andmaintainLOS.Thealgorithmensuresthatthelastthreepointsalwaysformanequilateral trianglesothatithasagoodestimateofthelocalgradient,anditusesthelastthreemeasurements todecideitsnextstepinthegradientdirectionwhilemaintainingtheequilateraltriangleshape.The trackingperformanceoftheTEapproachiscomparedwiththepreviouslyproposedEKFmethod andtheEScontrolmethodinsimulationandexperiments.TheTEapproachisfoundtobeoptimal ande˙ectiveintermsofitsconvergencespeedforawiderangeofrelativespeedanddistance betweentherobots. Thee˚cacyoftheoverallsystemisalsotestedinunderwaterscenariowherecommunicationis performedsimultaneouslywiththealignmentcontrol,andthedependenceofdata-rateonsignal- strengthisalsodemonstrated.First,anapplicationofuni-directionalsystemisdemonstrated whereahumanoperatorwirelesslycontrolledarobotinanunderwaterscenariousinganLED 123 communication-basedjoystick.Thenbi-directionalalignmentcontrolandcommunicationisshown betweentwounderwaterrobots.Althoughthepresentedresultsarevalidatedonthespecialized hardwaresetup,themodel-freeandgenericnatureoftheapproachmakestheresultswidelyuseful forscenariosofactivealignmentofbeamsignals. 124 CHAPTER6 SUMMARY&FUTUREWORK 6.1Summary Inthisdissertation,anovelactive-alignmentcontrolbasedLEDcommunicationsystemdesign waspresentedwhereeachtransceiveronanagentconsistedofonlyasinglephoto-diodeanda singleLED.Startingwithasimple2Dscenario,anEKF-basedalgorithmwasproposedtoestimate therelativeorientationbetweentheheadingangleandtheLOSdirection,whichwassubsequently usedforalignmentcontrol.Ascanningtechniquewasintroducedtoobtainsuccessiveintensity measurementsthatensuredthefullobservabilityoftheunderlyingsystem.Theapproachwaslater extendedtoa3Dscenariowithimprovementsinboththehardwareandthealgorithm.Anew circularscanningtechniquewasproposedforthe3Dsetting,wheretheamplitudeofthescanning wasmodulatedaccordingtothealignmentperformancetoachieveasoundtrade-o˙between estimationaccuracy,signalstrength,andenergyconsumption.Thee˚cacyoftheapproachwas testedandveri˝edagainstanextremum-seekingapproachviasimulationandexperimentsinvolving tworobotswithrelative3Dmotion.TheresultsestablishedthesuperiorityoftheEKFapproach approachovertheESapproachintermsofe˚cacyacrossarangeofdistances. Movingforward,weformulatedthebidirectionalopticalbeamtrackingproblemasatwo- agentdiscrete-timedynamicalsystem,wheretheorigincorrespondstotheLOSandisthepoint ofmaximumoutputfunctionforeachoftheagents.Weproposedamodel-freeoutputfeedback controllawforaclassofsystemsthatfollowcertainassumptionswiththeconstraintthatthe controlcommandofanagentcandependonlyontheinformationaccessibletothatagent.Through rigorousanalysis,weshowedthatforanyinitialcondition,theproposedcontrollawdrivesthe physicalsystemtoanappropriatelycharacterizedneighborhoodoftheLOSina˝nitenumberof steps.Fromsimulationresults,theproposedalgorithmwasshowntobesuperiorthanthetwo contendingapproachesbasedontheEKFandtheES,intermsofconvergencespeed,robustness 125 tounknowndisturbances,andhandlinglargeinitialcondition.Theproposedapproachwasalso validatedonanexperimentalsetupconsistingoftworobotsinthepresenceofaconstantexternal disturbance. Finally,weproposedamodel-freeapproachforbi-directionalalignmentcontrolinthe3Dset- ting.Theapproachalwaysmaintainsanequilateraltriangleshapeintheangularspacebyitslast threepointingdirections,whichgivesagoodestimateofthelocalgradient,whichisusedtotake thenextstepintheincreasinggradientdirectionwhilemaintainingthetriangleshape.Thee˚cacy oftheproposedtriangularexplorationapproachwastestedagainsttheEKFandtheESapproachin simulationandexperimentsandwasfoundtobebetterintermsofhigherconvergencespeedfora widerangeofrelativespeedanddistancebetweentwounderlyingrobots.Theoverallsystemwas alsotestedintheunderwaterscenariowherecommunicationwasperformedsimultaneouslywith thealignmentcontrol,andthedependenceofdata-rateonsignal-strengthwasalsodemonstrated. First,anapplicationoftheuni-directionalsystemwasdemonstratedwhereahumanoperatorwire- lesslycontrolledarobotinanunderwaterscenariousinganLEDcommunication-basedjoystick. Thensimultaneousbi-directionalalignmentcontrolandcommunicationwasshownbetweentwo underwaterrobots. 6.2FutureWork Forfuturework,additionalexperimentaltrialsofsimultaneousalignmentandcommunications betweentwomovingunderwaterrobotsshallbeconductedtotestthee˚cacyofourproposed approachinmorepracticalscenariosandexploringthelimitofthecommunicationbandwidthfor movingrobots.Itwillalsobeofinteresttoprovideanalyticalresultsandguaranteesofconvergence forthetriangular-explorationapproachonthefour-dimensional(4D)systemoftworobots(two statesofeachrobot). Sincethealignmentapproachespresentedinthisworkdonotrelyontheinformationexchange betweenthecommunicationparties,afuturedirectionofthisresearchcouldincludeexploration ofalgorithmsthatusesomeofthebandwidthofcommunicationtoexchangeusefulinformation 126 betweenthecommunicatingrobots,thatcanbeusedtoenhancethetrackingperformance.Fur- thermore,theframeworkoftworobotscanbeextendedtoamulti-agentsystemthatemulates a˛eetofunderwaterrobotsonamission.Astherobotscancommunicateonlyinpairsbased ontheirphysicallocationandorientationoftheirtransceivers,thustheoverallnetworkbecomes intermittentandtechniqueslikeagreementprotocolandformationcontrolwillbechallengingto analyzeandimplementandwouldrequireexplorationofadvancedalgorithms. 127 APPENDICES 128 APPENDIXA GENERATIONOFMOTORCOMMANDS Inthissection,wediscussthedetailsofgeneratingthemotorcommandsfromcontroltermsateach iteration.Themotorcommandsrepresentthedi˙erencesbetweenthepresentstatesofmotors, andtheirvaluesatthenextstep.ThedetailsofthecontroltermsfortheEKF,theESandtheTE approachareprovidedinEq.(3.17),Figure3.6andEq.(5.4)respectively. " q "Œ: \ "Œ: # = " q "Œ: +1 \ "Œ: +1 # " q "Œ: \ "Œ: # Œ (A.1) where q "Œ: and \ "Œ: aretheazimuthalandelevationanglesofthemotor-systematthe : thiteration respectively. FigureA.1illustratestherelationbetweenmultiplecoordinate-systems,whichshareacommon origin,andarerelatedbyrotationmatrices.The mean and scan termsarespeci˝ctoEKFbased approachandtheirdetailsareincludedinChapter3,Section3.2.Fortheextremum-seeking FigureA.1: Blockdiagramillustratingtherelationbetweencoordinatesystems. 129 approachandthetriangular-explorationapproach,sincethenotionof scan isnotapplicable,the mean and scan coordinatesystemsofeachiterationaresameandcanbemergedtogether.The motorcommands(angles q "Œ: +1 and \ "Œ: +1 )arethedi˙erencesbetweenthecurrentpointing direction( G -axisofthescan( : )co-ordinatesystem)andthenextpointingdirection( G -axisofthe scan( : +1 )co-ordinatesystem)measuredinthecoordinatesystemofbaseframe.Therotational transformation ' isde˝nedas, ' ( UŒVŒW )= ' H ( V ) ' I ( U ) ' G ( W ) Œ where ' H ( V )= 2 6 6 6 6 6 6 4 cos( V )0sin( V ) 010 sin( V )0cos( V ) 3 7 7 7 7 7 7 5 Œ ' I ( U )= 2 6 6 6 6 6 6 4 cos( U ) sin( U )0 sin( U )cos( U )0 001 3 7 7 7 7 7 7 5 Œ ' G ( W )= 2 6 6 6 6 6 6 4 100 0cos( W ) sin( W ) 0sin( W )cos( W ) 3 7 7 7 7 7 7 5 Ł Now,atanygiventimeiteration : ,withoutthelossofgenerality,wecanassumethatthebase( : )- coordinatesystemandthescan( : )-coordinatesystemhavetheir I -axesaligned.Hence,thecurrent scanhasmotoranglesas h 0 Œ\ : i ) .Itisassumedthatthemotorsystemhasaccesstoitselevation angle \ : ateveryinstant;however,theaccesstotheazimuthalangleisnotrequired.Thenextscan directioninthebase-coordinatesystems( s )canbecomputedbythefollowingseriesofrotational transformsontheunitvectorcorrespondingtothe G -axisofthelocalscan( : +1 )coordinatesystems: s = ' (0 Œ\ : Œ 0) ' 1 ( U : ŒV : ŒW 1 ) ' ( D 1 Œ: ŒD 2 Œ: ŒW 2 ) ' ( U : +1 ŒV : +1 ŒW 3 ) h 100 i ) Ł (A.2) Additionally,inallofthelocalcoordinatesystems,thereisarequirementthatthelocal I axisshould beinthe GI plane.Thisconstraintessentiallymeansthatnoneofthecoordinatesystemshasany 130 rollanglewithrespecttothebasecoordinatesystem.Thisconstraintrequiresarollcorrectionat everystage,andhencetherollangle W 1 canbecomputedbysolvingthefollowing: ' (0 Œ\ : Œ 0) ' 1 ( U : ŒV : ŒW 1 ) (2 Œ 3) =0 ) sin( W 1 )cos( \ : )cos( V : )+cos( W 1 )cos( \ : ) sin( U : )sin( V : ) sin( \ : )cos( U : )sin( V : )=0 Ł Once W 1 isknown, W 2 iscomputedbysolving ' (0 Œ\ : Œ 0) ' 1 ( U : ŒV : ŒW 1 ) ' ( D 1 Œ: ŒD 2 Œ: ŒW 2 ) (2 Œ 3) =0 Ł IthasbeennotedthatEq.(A.2)isindependentof W 3 ,hencethecomputationof W 3 isnotrequired. Oncethevector s isobtained,themotoranglesforthenextiterationare q "Œ: +1 =atan2( s I Œ s G ) ,and \ "Œ: +1 =atan2( s H Œ q s 2 G + s 2 I ) Ł NowusingEq.(A.1),onecan˝nallycomputethemotorcommandsas " q "Œ: \ "Œ: # = 2 6 6 6 6 4 atan2( s I Œ s G ) atan2( s H Œ q s 2 G + s 2 I ) \ : 3 7 7 7 7 5 Ł (A.3) 131 APPENDIXB CALCULATIONOFTHEHEADINGOFFSETANGLEANDITSDERIVATIVES Thisappendixdiscussesthecomputationof b (anditsgradient),whichisintroducedinEq.(3.15). Atanygiveninstant,inthemean( : )coordinatesystem, b ( ) computestheangleofthecurrent pointingdirection(sphericalangles- ( VŒU ) )withtheLOS(sphericalangles- ( G 2 ŒG 3 ) ),whichisthe inversecosineofthedotproductoftheunitvectorsinthesetwodirections. b ¹ G 2 ŒG 3 ŒUŒV º =arccos ¹ j ¹ G 2 ŒG 3 ŒUŒV ºº Œ j ¹ G 2 ŒG 3 ŒUŒV º = 2 6 6 6 6 6 6 4 cos G 2 cos G 3 sin G 3 sin G 2 cos G 3 3 7 7 7 7 7 7 5 2 6 6 6 6 6 6 4 cos V cos U sin U sin V cos U 3 7 7 7 7 7 7 5 Ł Fornotationalconvenience,wede˝ne ~ x as » G 2 ŒG 3 ¼ ) , ~ b as b ¹ G 2 ŒG 3 ŒUŒV º and ~ j as j ¹ G 2 ŒG 3 ŒUŒV º . Next,wecomputethegradientof ~ b withrespectto ~ x : m ~ b m ~ x = 1 p 1 j 2 h mj mG 2 Œ mj mG 3 i Œ with m ~ j mG 2 = cos U cos V cos G 2 sin G 3 + cos U sin V cos G 2 cos G 3 Œ and m ~ j mG 3 = cos U cos V sin G 2 sin G 3 +sin U cos G 2 cos U sin V sin G 2 sin G 3 Ł 132 BIBLIOGRAPHY 133 BIBLIOGRAPHY [1] M.Al-RubaiaiandX.Tan.DesignanddevelopmentofanLED-basedopticalcommunication systemwithactivealignmentcontrol.In 2016IEEEInternationalConferenceonAdvanced IntelligentMechatronics ,pagesJuly2016. [2] MohammedAl-rubaiai.DesignanddevelopmentofanLED-basedopticalcommunication system.Master'sthesis,MichiganStateUniversity,Aug2015. [3] StephenB.Alexander. OpticalCommunicationReceiverDesign .SPIEPublications,Belling- ham,1997. [4] DavideAnguita,DavideBrizzolara,andGiancarloParodi.Buildinganunderwaterwireless sensornetworkbasedonopticalommunication:Researchchallengesandcurrentresults. In SensorTechnologiesandApplications,2009.SENSORCOMM'09.ThirdInternational Conferenceon ,pagesIEEE,2009. [5] DavideAnguita,DavideBrizzolara,andGiancarloParodi.Opticalwirelesscommunica- tionforunderwaterwirelesssensornetworks:Hardwaremodulesandcircuitsdesignand implementation.In OCEANS2010 ,pagesIEEE,2010. [6] E.BiyikandM.Arcak.Gradientclimbinginformationviaextremumseekingandpassivity- basedcoordinationrules.In 200746thIEEEConferenceonDecisionandControl ,pages Dec2007. [7] L.Briñón-Arranz,L.Schenato,andA.Seuret.Distributedsourceseekingviaacircular formationofagentsundercommunicationconstraints. IEEETransactionsonControlof NetworkSystems ,2016. [8] H.Brundage.Designingawirelessunderwateropticalcommunicationsystem.Master's thesis,MassachusettsInstituteofTechnology,2010. 134 [9] LloydButler.Underwaterradiocommunication. AmateurRadio ,1987. [10] WenqiCaiandIbrahimaNDoye.Anextremumseekingcontrolbasedapproachforalignment problemofmobileopticalcommunicationsystems.(May),2020. [11] JongeunChoi,SonghwaiOh,andRobertoHorowitz.Distributedlearningandcooperative controlformulti-agentsystems. Automatica ,45(12):28022814,2009. [12] NikhilChopraandMarkW.Spong. Passivity-BasedControlofMulti-AgentSystems ,pages SpringerBerlinHeidelberg,Berlin,Heidelberg,2006. [13] SalvadorCliment,AntonioSanchez,JuanVicenteCapella,NirvanaMeratnia,andJuanJose Serrano.Underwateracousticwirelesssensornetworks:Advancesandfuturetrendsin Physical,MACandroutinglayers. Sensors ,14(1):795,2014. [14] BrandonCochenour,LindaMullen,WilliamRabinovich,andRitaMahon.Underwater opticalcommunicationswithamodulatingretro-re˛ector. OceanSensingandMonitoring , 7317(April2009):73170G,2009. [15] M.Doniec,M.Angermann,andD.Rus.Anend-to-endsignalstrengthmodelforunderwater opticalcommunications. OceanicEngineering,IEEEJournalof ,Oct2013. [16] M.DoniecandD.Rus.BidirectionalopticalcommunicationwithAquaOpticalII.In Com- municationSystems(ICCS),2010IEEEInternationalConferenceon ,pagesNov 2010. [17] PeterG.Goetz,WilliamS.Rabinovich,RitaMahon,JamesL.Murphy,MikeS.Ferraro, MicheleR.Suite,WalterR.Smith,BenB.Xu,HarrisR.Burris,ChristopherI.Moore, WarrenW.Schultz,BarryM.Mathieu,KurtHacker,ShadReese,WadeT.Freeman,Steve Frawley,andMichaelColbert.Modulatingretro-re˛ectorlasercomsystemsattheNaval ResearchLaboratory. Proceedings-IEEEMilitaryCommunicationsConferenceMILCOM , pages2010. 135 [18] R.M.Hagem,DavidV.Thiel,S.G.O'Keefe,A.Wixted,andT.Fickenscher.Low-costshort- rangewirelessopticalFSKmodemforswimmersfeedback.In Proceedingsof2011IEEE SensorsConference ,pagesOct2011. [19] FrankHansonandStojanRadic.Highbandwidthunderwateropticalcommunication. Appl. Opt. ,Jan2008. [20] MahdiJadaliha,JoonhoLee,andJongeunChoi.Adaptivecontrolofmultiagentsystems for˝ndingpeaksofuncertainstatic˝elds. JournalofDynamicSystems,Measurement,and Control ,134(5),072012.051007. [21] SunghoJeonandHiroshiToshiyoshi.MEMStrackingmirrorsystemforabidirectional free-spaceopticallink. AppliedOptics ,56(24):6720,aug2017. [22] RobertK.Katzschmann,JosephDelPreto,RobertMacCurdy,andDanielaRus.Exploration ofunderwaterlifewithanacousticallycontrolledsoftrobotic˝sh. ScienceRobotics ,3(16), 2018. [23] A.Komaee.Cooperativestochasticcontrolforopticalbeamtracking.In 200741stAnnual ConferenceonInformationSciencesandSystems ,pages2007. [24] ArashKomaee,P.S.Krishnaprasad,andPrakashNarayan.Activepointingcontrolforshort rangefree-spaceopticalcommunication. CommunicationsinInformationandSystems , 2007. [25] M.KrsticandK.B.Ariyur. MultiparameterExtremumSeeking ,chapter2,pagesJohn Wiley&Sons,Ltd,2004. [26] NaomiE.Leonard,DerekA.Paley,RussE.Davis,DavidM.Fratantoni,FrancoisLekien, andFuminZhang.Coordinatedcontrolofanunderwaterglider˛eetinanadaptiveocean sampling˝eldexperimentinMontereyBay. JournalofFieldRobotics ,2010. 136 [27] N.LiandJ.R.Marden.Designinggamesfordistributedoptimization. IEEEJournalof SelectedTopicsinSignalProcessing ,April2013. [28] XiaoyanLiu,SuyuYi,RanLiu,LirongZheng,andPengfeiTian.34.5mUnderwateroptical wirelesscommunicationwith2.70GbpsdataratebasedonagreenlaserwithNRZ-OOK modulation.In 201714thChinaInternationalForumonSolidStateLighting:International ForumonWideBandgapSemiconductorsChina,SSLChina:IFWS2017 ,volume2018- January,2018. [29] FengLu,SammyLee,Je˙reyMounzer,andCurtSchurgers.Low-costmedium-rangeoptical underwatermodem:Shortpaper.In ProceedingsoftheFourthACMInternationalWorkshop onUnderWaterNetworks ,WUWNet'09,pagesNewYork,NY,USA,2009.ACM. [30] D.Meng,M.Fazel,andM.Mesbahi.Proximalalternatingdirectionmethodofmultipliers fordistributedoptimizationonweightedgraphs.In 201554thIEEEConferenceonDecision andControl(CDC) ,pagesDec2015. [31] F.P.Miller,A.F.Vandome,andJ.McBrewster. Beer-LambertLaw .VDMPublishing,2009. [32] B.J.MooreandC.Canudas-de-Wit.Sourceseekingviacollaborativemeasurementsbya circularformationofagents.In Proceedingsofthe2010AmericanControlConference ,pages 2010. [33] ShigekiMuta,TakeshiTsujimura,andKiyotakaIzumi.Laserbeamtrackingsystemfor activefree-spaceopticalcommunication. Proceedingsofthe2013IEEE/SICEInternational SymposiumonSystemIntegration ,pages2014. [34] A.NedicandA.Ozdaglar.Distributedsubgradientmethodsformulti-agentoptimization. IEEETransactionsonAutomaticControl ,Jan2009. [35] P.Ogren,E.Fiorelli,andN.E.Leonard.Cooperativecontrolofmobilesensornet- works:adaptivegradientclimbinginadistributedenvironment. IEEETransactionsonAuto- maticControl ,2004. 137 [36] JOnathanFHOlzman.All-opticalretro-modulationforfree-spaceopticalcommunication. 2018. [37] HassanMakineOubei,ChangpingLi,Ki-HongPark,TienKheeNg,Mohamed-SlimAlouini, andBoonS.Ooi.23Gbit/sunderwaterwirelessopticalcommunicationsusingdirectly modulated520nmlaserdiode. OpticsExpress ,23(16):20743,2015. [38] C.Pontbriand,N.Farr,J.Ware,J.Preisig,andH.Popenoe.Di˙usehigh-bandwidthoptical communications.In OCEANS2008 ,pagesSept2008. [39] K.Reif,S.Gunther,E.Yaz,andR.Unbehauen.Stochasticstabilityofthediscrete-time extendedKalman˝lter. IEEETransactionsonAutomaticControl ,Apr1999. [40] StuartJ.RussellandPeterNorvig. Arti˝cialIntelligence:AModernApproach .Pearson Education,2edition,2003. [41] I.C.RustandH.H.Asada.Adual-usevisiblelightapproachtointegratedcommunication andlocalizationofunderwaterrobotswithapplicationtonon-destructivenuclearreactor inspection. RoboticsandAutomation(ICRA),2012IEEEInternationalConferenceon ,pages May2012. [42] GiuseppeSchirripaSpagnolo,LorenzoCozzella,andFabioLeccese.UnderwaterOptical WirelessCommunications:Overview. Sensors(Basel,Switzerland) ,20(8):2261,apr2020. [43] J.S.ShammaandG.Arslan.Dynamic˝ctitiousplay,dynamicgradientplay,anddistributed convergencetonashequilibria. IEEETransactionsonAutomaticControl , March2005. [44] J.A.Simpson,B.L.Hughes,andJ.F.Muth.Smarttransmittersandreceiversforunderwater free-spaceopticalcommunication. SelectedAreasinCommunications,IEEEJournalon , June2012. 138 [45] P.B.Solanki,M.Al-Rubaiai,andX.Tan.ExtendedKalman˝lter-aidedalignmentcontrolfor maintaininglineofsightinopticalcommunication.In 2016AmericanControlConference , pagesJuly2016. [46] P.B.Solanki,M.Al-Rubaiai,andX.Tan.Extendedkalman˝lter-basedactivealignmentcon- trolforledopticalcommunication. IEEE/ASMETransactionsonMechatronics , 1511,Aug2018. [47] P.B.Solanki,S.D.Bopardikar,andX.Tan.Abidirectionalalignmentcontrolapproachfor planarled-basedfree-spaceopticalcommunicationsystems.In 2020IEEE/ASMEInterna- tionalConferenceonAdvancedIntelligentMechatronics(AIM) ,pages2020. [48] P.B.SolankiandX.Tan.ExperimentalimplementationofextendedKalman˝lter-based opticalbeamtrackingwithasinglereceiver.In 2016IEEEInternationalConferenceon AdvancedIntelligentMechatronics ,pagesJuly2016. [49] P.BhanuSolankiandX.Tan.ExtendedKalman˝lter-based3Dactive-alignmentcontrolfor LEDcommunication.In 2018IEEEInternationalConferenceonRoboticsandAutomation (ICRA) ,pagesMay2018. [50] P.B.Solanki,S.D.Bopardikar,andX.Tan.Activealignmentcontrol-basedledcommunication forunderwaterrobots.In 2020IEEE/RSJInternationalConferenceonIntelligentRobotsand Systems(IROS) ,pagesOct2020. [51] PratapBhanuSolankiandXiaoboTan.ExtendedKalmanFilter-AidedActiveBeamTracking forLEDCommunicationin3DSpace.In DynamicSystemsandControlConference ,volume2, 102017.V002T04A009. [52] GokhanSoysalandMuratEfe.Kalman˝lteraidedcooperativeopticalbeamtracking. Ra- dioengineering ,pagesJune2010. [53] M.Stojanovic.Recentadvancesinhigh-speedunderwateracousticcommunications. IEEE JournalofOceanicEngineering ,April1996. 139 [54] SheriN.White,AlanD.Chave,andGeorgeT.Reynolds.Investigationsofambientlight emissionatdeep-seahydrothermalvents. JournalofGeophysicalResearch:SolidEarth , 107(B1):EPM2002. [55] TsaiChenWu,YuChiehChi,HuaiYungWang,ChengTingTsai,andGongRuLin.Bluelaser diodeenablesunderwatercommunicationat12.4gbps. Scienti˝cReports ,7(Januar 2017. [56] RussellB.Wynn,VeerleA.I.Huvenne,TimothyP.LeBas,BramleyJ.Murton,DouglasP. Connelly,BrianJ.Bett,HenryA.Ruhl,KirstyJ.Morris,Je˙reyPeakall,DanielR.Parsons, EstherJ.Sumner,StephenE.Darby,RobertM.Dorrell,andJamesE.Hunt.Autonomous underwatervehicles(AUVs):Theirpast,presentandfuturecontributionstotheadvancement ofmarinegeoscience. MarineGeology ,352:451468,2014.50thAnniversarySpecialIssue. [57] K.YoshidaandT.Tsujimura.Trackingcontrolofthemobileterminalinanactivefree-space opticalcommunicationsystem.In 2006SICE-ICASEInternationalJointConference ,pages Oct2006. 140