an algorithm for calculating the inverse jacobian of

Santa Clara UniversityScholar Commons

Mechanical Engineering Master's Theses Engineering Master's Theses

12-12-2016

An Algorithm for Calculating the Inverse Jacobianof Multirobot Systems in a Cluster SpaceFormulationChristopher Jude WaightSanta Clara University

Follow this and additional works at: http://scholarcommons.scu.edu/mech_mstr

Part of the Mechanical Engineering Commons

This Thesis is brought to you for free and open access by the Engineering Master's Theses at Scholar Commons. It has been accepted for inclusion inMechanical Engineering Master's Theses by an authorized administrator of Scholar Commons. For more information, please [email protected].

Recommended CitationWaight, Christopher Jude, "An Algorithm for Calculating the Inverse Jacobian of Multirobot Systems in a Cluster Space Formulation"(2016). Mechanical Engineering Master's Theses. 7.http://scholarcommons.scu.edu/mech_mstr/7

http://scholarcommons.scu.edu?utm_source=scholarcommons.scu.edu%2Fmech_mstr%2F7&utm_medium=PDF&utm_campaign=PDFCoverPages

http://scholarcommons.scu.edu/mech_mstr?utm_source=scholarcommons.scu.edu%2Fmech_mstr%2F7&utm_medium=PDF&utm_campaign=PDFCoverPages

http://scholarcommons.scu.edu/eng_master_theses?utm_source=scholarcommons.scu.edu%2Fmech_mstr%2F7&utm_medium=PDF&utm_campaign=PDFCoverPages

http://scholarcommons.scu.edu/mech_mstr?utm_source=scholarcommons.scu.edu%2Fmech_mstr%2F7&utm_medium=PDF&utm_campaign=PDFCoverPages

http://network.bepress.com/hgg/discipline/293?utm_source=scholarcommons.scu.edu%2Fmech_mstr%2F7&utm_medium=PDF&utm_campaign=PDFCoverPages

http://scholarcommons.scu.edu/mech_mstr/7?utm_source=scholarcommons.scu.edu%2Fmech_mstr%2F7&utm_medium=PDF&utm_campaign=PDFCoverPages

mailto:[email protected]

AnAlgorithmforCalculatingtheInverseJacobianofMultirobotSystemsinaClusterSpaceFormulation

by

Christopher Jude Waight

MASTER OF SCIENCE THESIS

Submitted in partial fulfillment of the requirements for the degree of

Master of Science in Mechanical Engineering School of Engineering Santa Clara University

Santa Clara, California December 12, 2016

iii

AnAlgorithmforCalculatingtheInverseJacobianofMultirobotSystemsinaClusterSpaceFormulation

Christopher Jude Waight

Department of Mechanical Engineering Santa Clara University

2016

Abstract

Multirobot systems have characteristics such as high formation re-configurability that allowthemtoperformdynamic tasks that require real time formationcontrol.These tasks includegradientsensing, object manipulation, and advanced field exploration. In such instances, the Cluster SpaceControlapproachisattractiveasitisbothintuitiveandallowsforfulldegreeoffreedomcontrol.ClusterSpace Control achieves this by redefining a collection of robots as a single geometric entity called acluster. To implement, it requires knowing the inverse Jacobian of the robotic system for use in themaincontrol loop.Historically, the inverse Jacobianhasbeencomputedbyhandwhich isanarduousprocess. However, a set of frame propagation equations that generate both the inverse positionkinematics and inverse Jacobian has recently been developed. These equations have been used tomanuallycompiletheinverseJacobianMatrix.Theobjectiveofthisthesiswastoautomatethisoverallprocess.Todothis,aformalmethodforrepresentingclusterspaceimplementationsusinggraphtheorywasdeveloped.Thisnewgraphicalrepresentationwasusedtodevelopanalgorithmthatcomputesthenewframepropagationequations.Thisalgorithmwasthen implemented inMatlabandthealgorithmand its associated functions were organized into a Matlab toolbox. A collection of several clusterdefinitions were developed to test the algorithm, and the results were verified by comparing to aderivation based technique. The result is the initial version of a Matlab Toolbox that successfullyautomatesthecomputationoftheinverseJacobianMatrixforaclusterofrobots.Keywords:Multirobot,ClusterControl,InverseJacobian,PropagationEquations,Algorithm

iv

Acknowledgements

Iwouldliketothankmyadvisor,Dr.ChrisKitts,forsuggestingexploringthistechnique,andforprovidingtheframeworkneededforthisresearch.Iwouldalsoliketothankhimforhisencouragement,patience,andperspectivesonseveral issuesthatwereencounteredthroughoutthis research. Iwouldalsoliketothankhimforhelpingmetounderstandwhatmycontributiontothefieldis.

IwouldalsoliketothankeveryoneintheSCURoboticsLab.Thanksgoestoformerstudents,asmuchoftheirpreviousresearchwasusedasreferencesformyunderstandings.ThanksgoestocurrentstudentswhoarealsoworkingonClusterControl,andofferinginsightsandexplanationstosomeofthefiner details of the control method. Thanks goes to Alex Mulcahy for providing a test case for thealgorithm that was developed. Thanks goes to all others too, for the encouragement, and for thecompanythroughthelonghoursofwork. I would like to thank my family for their support and enthusiasm as I pursued this degree.Thanksmomanddadforalltheencouragingwords,andtomybrothersandcousinsforunderstandingthe business of my schedule. I would also like to thank Irene and Harry Partridge for all their help,encouragement,support,andforbeingawesomepeople.Withoutthem,noneofthiswouldbepossible. A final thank you to allmy close friends. I will never forget any of the phone calls, the carepackageswith foodandencouragingnotes, themusic recommendations,and for remindingme togobig,ortogohome.

v

TableofContentsAbstract.......................................................................................................................................................iii

Acknowledgements.....................................................................................................................................iv

ListofFigures.............................................................................................................................................vii

ListofTables..............................................................................................................................................viii

Chapter1:Introduction................................................................................................................................1

1.1FormationControlofMultirobotSystems.........................................................................................1

1.2ClusterSpaceControl.........................................................................................................................2

1.3InverseJacobianMatrices..................................................................................................................4

1.4ProjectStatement..............................................................................................................................6

1.5ReadersGuide....................................................................................................................................6

Chapter2:GraphBasedRepresentationsofClusters..................................................................................7

2.1DeterminingSuitableClusterSpaceVariables...................................................................................7

2.2TheHomogeneousTransformintermsofClusterSpace...................................................................8

2.3:GraphBasedRepresentationsofClusters.........................................................................................9

2.4DeterminingtheInverseKinematicsfromtheGraphBasedRepresentations.................................11

2.5AfewnotesontheTreeRepresentationofaclusterspaceformulation........................................12

Chapter3:JacobianPropagationAlgorithm..............................................................................................14

3.1TheInverseJacobianAlgorithm.......................................................................................................14

3.2AnimplementationoftheFormula..................................................................................................16

3.3RunTimeAnalysisofthisalgorithm.................................................................................................21

Chapter4:MatlabImplementationofAlgorithm......................................................................................23

4.1TheCluster_BuilderFolder...............................................................................................................24

4.2TheInverseKinematicsFolder.........................................................................................................27

4.3TheTest_BedFolder.........................................................................................................................28

4.4TheVelocity_Propagation_TechniqueFolder..................................................................................31

Chapter5:Results......................................................................................................................................33

5.1ResultsfromTest_BedExamples.....................................................................................................33

5.2ResultsfromaThirdPartyCluster....................................................................................................33

5.3FurtherTestingII..............................................................................................................................34

Chapter6:Conclusions...............................................................................................................................36

6.1Summary..........................................................................................................................................36

6.2FutureWork.....................................................................................................................................37

vi

References..................................................................................................................................................38

AppendixA–ExamplesusedintheTest_BedFolder................................................................................40

AppendixB–CodeusedinToolbox...........................................................................................................62

B.1TreeDataStructureFolder...............................................................................................................62

B.2Cluster_BuilderFolder......................................................................................................................62

B.3TheInverse_KinematicsFolder........................................................................................................67

B.4TheVelocity_Propagation_TechniqueFolder..................................................................................70

AppendixC-TheInverseJacobianFormula:AProof.................................................................................79

vii

ListofFigures Figure1:AResolvedRateClusterspacecontrolarchitectureforagenericmultirobotsystem…….……..…3

Figure2:AClusterof3robots,withthedistancevariablesM,N,andLshown………………………………..….…8

Figure3:APhysicalViewofExample3,aclusteroftwo2-robotclusters…………………………………………….….9

Figure4:Agroupofunconnectedverticesrepresentingalltheframesinacluster.………………………….……9

Figure5:Two2-RobotClustersrepresentedbythesetofedgesE1.……………………………………………….……10

Figure6:A2robotclusterinaleaderfollowerrelationshipwithanother2robotclusterandafifthrobotinaleaderfollowerrelationshipwithclusterframeC2……………….………………….……10

Figure7:AcompletedClusterGraphofa5RobotCluster………………………………….………………………….…….11

Figure8:ARobotwithtwoleaders………………………………………………………………………………………………………13

Figure9:Initialtreewithallnodescoloredwhite…………………………………………………………………………………17

Figure10:Treewithcurrentnodecoloredyellow………………………………………………………………………………..17

Figure11:Clustertreewithexplorednodecoloredredandcurrentnodeyellow………………………………..18

Figure12:Clustertreewithallnodesfromleaftorootexploredonce.………………………………………………..18

Figure13:Returningtotherobotnodeofthecurrentsubroutine……………………………………………………….18

Figure14:ClusterTreewithExplorednodesaregreenandcurrentnodeisyellow………………………………19

Figure15:Returningtorobotnodebeingcomputedandmarkascurrentnode…………………………………..20

Figure16:Aclustertreewithasinglefullyexploredrobotnode………………………………………………………….20

Figure17:Afullyexploredrobotnodeandanewrobotnodeselected………………………………………………..20

Figure18:Afullyexploredclustertree…………………………………………………………………………………………………21

Figure19:DirectoryoftheClusterSpaceControlInverseJacobianToolbox…………………………………………23

Figure20:Userenteringtreeinformationforexample3intoclusterbuiler2.m…………………………………….25

Figure21:UserenteringHTMmatricesforeachedgeonthetreeofexample3…………………………………...25

Figure22:DisplayofattributesinitiallysavedintheMatlabvariableexample3…………………………………..26

Figure23:Thefunctioninvkin.mbeingusedonexample3variable……………………………………………………...27

Figure24:Thefunctiondirvinvjac.mbeingusedonexample3variable………………………………………………..28

Figure25:APhysicalViewofExample3,aclusteroftwo2-robotclusters…………………………………………...28

Figure26:AGraphrepresentingaclusterofclusters.Cluster{C1}and{C2}formasinglecluster{C}……29

Figure27:Thefunctioninvjacfxn.mbeingusedonexample3variable…………………………………………………32

Figure28:TheAlexCluster.A5robotclusterwithrobots1and2formingthemaincluster,robot4 followingrobot2,robot3followingrobot1,androbot5followingrobot3…………………….….33Figure29:Columns1through7oftheinverseJacobianoftheAlexCluster…………………………………………34

Figure30:Columns8through15oftheinverseJacobianoftheAlexCluster……………………………………….34

viii

ListofTables Table1:AlistofallthefilesintheCluster_Builderfolder……………………………………………………………………24

Table2:Alistofallthevariableattributessavedineachexamplevariable………………………………………..26

Table3:DescriptionofallFilesintheInverseKinematicsfolder…………………………………………………………27

Table4:Examplesinthetestbedfolderandanexplanationoftheirconfiguration……………………………31

Table5:DescriptionsofthefilesintheVelocity_Propagation_TechniqueFolder……………………………….32

Table6:OtherClusterDefinitiontotestRobustnessofToolbox…………………………………………………………35

1

Chapter1:Introduction

Robotshavebeeninstrumentalindevelopingmanyindustriesinmodernculture.Thisisbecausearobotcanbedevelopedtocompletetasksfaster,withhigherprecision,andmorecosteffectivelythanhumanlabor.Itcanworkinharshenvironments,completerepetitivetaskswithlesswearandtearthanhumans,andcanperformtasks thatare farmorecomplicated.As the tasks thata robotcanperformgrow in complexity, so does the task of developing a system to control the robot. This challenge indeveloping a system of control grows even more complicated when multiple robots need to becontrolled at the same time. This has created the current demand for a control approach that isintuitive,reliable,andmostimportantly,scalable.

1.1FormationControlofMultirobotSystems

Amultirobotsystem(MRS)canbedefinedasasetofrobotsoperatinginthesameenvironment

in an independent, cooperative, or competitive manner. The term robot applies to any electro-mechanicalagentthatisguidedbyacomputerprogramorelectroniccircuitry.ThisdefinitionallowstheMRSscopetorangefromagroupoftwosensorequippedactuatorstoalargesetofcomplexhumanoidmachineswithhundredsof sensors andactuators that interactwith theenvironment andeachotherusingverycomplexdecisionmaking[1].

An MRS offers many advantages when compared to a single robot system. Having multiple

robots performing the same task can increase coverage, production scales, and redundancy. Otheradvantages include increasing configurability, more modularity, and the ability to share sensorinformation.SomebenefitsofaMRScanonlybeachievedasaresultofrobotsworkinginacooperativefashiontocompleteataskthatan individual robotcannotcompleteon itsown.Forexample,arobotcanhandoffataskthatitisunabletocompletetoanotherrobotinthesystem.Robotscanevenworkthrough coordinated and cooperative behaviors to accomplish tasks such as manipulating largeunbalanced objects through obstacle filled paths. These advantages can be realized on land basedagents,andalsoforapplicationsinair,sea,andspace[2].

Systemsofmultiplerobotshavetheiroriginsinthelate-1980s,and,todate,havebeenapplied

inseveraldomainsthatrequirecomplexco-ordination.Forexample,theCENTIBOTSprojectcreatedanexperimental demonstration showing a large team of robots (approximately 100) that couldautonomouslypatrolabuildingforanextendedperiodoftime.[4]IntheFIREproject,asimulationofateamofintelligentheterogeneousrobotsthatexploredharsh,humanlyinaccessibleplanetarysurfaceswascreated[5].Otherprojectsincludesimulationsofsatelliteformations[6],atestbedofunderwaterrobot fleets [7], and a test bed of unmanned aerial robots [8]. Implementation ofMRS’s is still in itsinfancy,withnoneofthementionedexamplesoperatingroutinelyduetoongoingtechnicalchallenges.Nonetheless,thestudyofMRS’sisagrowingfieldofinterestespeciallyastechnologicalimprovementsinbothhardwareandsoftwarearedevelopingexponentially[3].

Thecontrolofroboticsystemsisamatterofongoingexplorationanddrawsonworkincontrol

theory,robotics,biology,andartificialintelligence.Thecontrolstrategyemployedisdependentontheclassificationof theMRSbasedonthe levelofawareness,coordination,cooperation,and informationsharingrequired[1].Sometimes,itispossibletocontrolasystemofrobotsbycontrollingthebehaviorofeachrobot independently.Moreoften,however,systemsneedtobestronglycoordinated,andthe

2

systemneeds to combine sensor informationacrossmultiple robots tomakean informeddecisionofhowthesystemasawholeshouldbecontrolled.Severalcontroltechniqueshavebeenproposed,eachwithitsownprosandcons.

Oneversionofcontrolincludestheuseofleaderfollowertechniques,inwhich‘follower’robots

regulatetheirpositionrelativetoadesignated‘leader’robot[9][10].Thisapproachrequiresdiligenceonpartoftheoperatordefiningthesystem,astworobotswithsimilar‘follow’instructionscancompeteto occupy the same space and result in collision. A variation of this technique is to conduct leader-follower chains. In a leader follower chain, one robot may be following a ‘leader’ robot, whilesimultaneously acting as a leader for another robot [11]. These techniques are highly susceptible topropagationerroraserrorsaccumulatedownthe followerchains.Furthermore, the failureofasinglerobotcancausetheentirechaintonolongerfunction.

Anothertechniqueusedforformationcontrolisthecreationofartificialpotentialfields.These

fieldscanbeusedtoattractindividualrobotstotheirdesiredformationlocation,whileothersareusedtorepelthemfromnearbyrobotsorotherobstacles.Inthisapproach,arobot(orrobots)canemitsomesignalthatassignssome‘penalty’tootherrobotsnearbybasedontheirposition.Therobotsreceivingthispenaltyhavesomeheuristic(usuallytominimizepenalty)andcanthenchangeitsownlocationtominimize this penalty. Similarly, the robotmight have a heuristic to get equal penalties from two ormoreotherrobots inthissystem.Thistechniqueiswidelyusedinobstacleavoidancealgorithms,as itallowsrobotstoarrangethemselvesinformationsthatpreventsthemfromcollidingwitheachotherorsomeotherobstacle[12][13][14][15].Thesemethodsallowforfastcomputation,butdonotofferahighlevelofcontrollability.

Multirobot control has also been a growing interest in the field of artificial intelligence.

Algorithms consideringmultiagent systems composed of multiple interacting intelligent agents beingdevelopedandsimulatedforpossibleuseinsearchandrescue,transportation,andreconnaissance[16][17].Thesetechniquestendtobeverycomputationallyexpensive,butwork isbeingdoneto improvethecomputationalcosts[19].

Clusterspacecontrol,acontrolmethodologydevelopedandtestedattheSantaClaraUniversity

RoboticSystemsLaboffersanapproachthatisintuitive,stable,andscalable.Italsoallowsafulldegreeof freedom to be maintained, and results in very precise control over individual robot control. Thisstrategyconceptualizesan‘n’robotsystemasasingleentitycalledacluster.Thedesiredpositionsandmotions of the individual robots are then specified as a function of cluster states [18]. Thismethodcomes at the cost that the controller can be quite complex to develop. This research aims tomakedevelopingnewclusterspacecontrollerseasier. 1.2ClusterSpaceControl

Cluster Space Control is a control methodology that allows for the specification, control andmonitoringof themotionaMRS [14]. This strategy considers a systemofn robots as a single entity,known as a cluster. This cluster ismodeled as a virtual articulatingmechanismwith a full degree offreedom.

Inthismethodology,wefirstconsiderRtobesetofrobotstateposevariables.Thesevariablesdescribe the position and orientation of each robot relative to the global frame.We then consider aselectionofclusterspacevariables,C, whichdescribethepositionandorientationofoverallcluster,

3

the shape of the cluster, and the orientation of individual robots within the cluster. This set ofvariables,C, can be defined through a formal set of forward kinematic transforms of robot statevariables,R.

C = KIN R (Eq1)

Similarly, we can use the inverse kinematic transforms to take us from cluster space posevariablestorobotstatevariables.

R = INVKIN C (Eq2)

In otherwords, we can control the positions,motions, and even the actuator states of eachrobotcanbespecifiedasafunctionofclusterstatevariables.

InordertomapthevelocitiesfromRobotSpace,R,toClusterSpace,C,thevelocitykinematicsof the systemalsoneed tobeknown.Thevelocity kinematics canbe foundby computing thepartialderivativesofthekinematicequations,gi,withrespecttoeachrobotvariable,ri.

C =c+c,⋮c.

= KIN R/ = J R R =

123143

⋯ 123146

⋮ ⋮127143

⋯ 127146

r+r,⋮r9

(Eq3)

Thismatrixofpartialderivatives,J R ,isknowninClusterSpaceastheJacobianMatrix.Itisalso

referred to as simply the Jacobian. Similarly, the inverse Jacobian,J:+ R , transforms Cluster Spacevelocitiestorobotspacevelocities.TheInverseJacobiancanbefoundbytakingthepartialderivativesoftheinversekinematics,hi,withrespecttoeachclusterspacevariable,ci:

R =r+r,⋮r9

= INVKIN C/ = J:+ C C =

1;31<3

⋯ 1;31<7

⋮ ⋮1;61<3

⋯ 1;61<7

c+c,⋮c.

(Eq4)

UsingKIN,J,andJ-1,aclusterspacecontrollercanbedevelopedasshowninfigure1below:

Figure1:AResolvedRateClusterspacecontrolarchitectureforagenericmultirobotsystem.

4

In thisarchitecture in figure1, thedesiredvelocity control action is inputted in cluster space.

The control actions are converted to robot space using the inverse Jacobian, and then passed to theindividualrobots.Thisexamplearchitecturealsoshowsafeedbackloopreturningtothecontroller.

As figure1 shows,determining the Jacobianand inverse Jacobian is essential todevelopinga

clusterspacecontroller[14][18].However,determiningtheinverseJacobiancanbequitedifficult,andmanyapproacheshavebeensuggestedovertheyears.

It is worth noting also that a different version of a controller for cluster space has been

developed. This other version is a full dynamic controller and uses the transpose Jacobian as areplacementfortheinverseJacobian[30].ThiscontrollerrequiresfurtherworktodevelopasystematicwaytocomputetheforwardJacobian.

1.3InverseJacobianMatrices

TheneedforfindingtheinverseJacobianisnotuniquetodevelopingclusterspacecontrollers.

When developing a controller for serial manipulators, the most common method involves using aJacobianbasedController[20].Asaresult,findingdifferentefficientmethodsofdeterminingtheinverseJacobian is essential for continued work in the field. Several methods for determining the inverseJacobians exist with analytical methods generally being infeasible and computational methodscommonlyyieldinglessaccurateresults.

OnetechniquefordeterminingtheinverseJacobianistosolvefortheinversekinematicsofthe

system,andthencomputethepartialderivatives.Foraseriallinkmanipulator,theclosedforminversekinematicsolutioncanbeobtainedsymbolicallybywritingasystemofequationsdefiningtheforwardkinematic relationships, and then solving the system of equations for the joint angles [20] [21].However, this often cannot be solved in closed form. Geometric and trigonometric methods ofdeterminingananalytic formof inversekinematicsexist,butusuallydonotcontainthefullgeometricdescription.Therefore, thesemethodsprovide solutions thatareonlyvalid in some local vicinity, andoftenallowformultiplesolutions[21][22][24].OthermethodsstillarebasedonDenavit-Hartenberg,and rely on transformation matrices [23]. These approaches are also limited, as they restrict frameassignments throughout the entire robot. As a result, the inverse kinematics are typically solvednumerically[25].

Ontheotherhand,parallel-linkmanipulatorsoftenhaveageometrythatallowforclosedform

solutions to the inverse kinematics. Hence, the inverse Jacobian can be calculated by direct partialdifferentiation. A drawback in these systems is that the forward kinematics are nonlinear, and oftenrequirenumericalapproaches,andasadirectresult,theforwardJacobianissolvednumerically[26].

InsteadofsolvingfortheinverseJacobiandirectly,anotherapproachistosolvefortheanalytic

form of the Jacobian, then invert it. The kinematics can be defined symbolically, then differentiateddirectly,astheJacobianMatrixisamatrixofpartialderivatives[27].Butforsomesystems,asseenwiththecaseoftheparallellinkmanipulators,calculatingthesymbolicformofthekinematicsisnotalwayspossible.Furthermore,whenthismethodispossible,itisnotparticularlyefficientcomputationally,asitrequiresusingacomputationaltoolboxthathasasymbolicdifferentiationlibrary.

5

Several other methods for computing the Jacobian, particularly in serial manipulators, havebeendevelopedovertheyears.OnemethodbyVukobratovicandPotkonjakrecursivelycomputestheJacobian for each joint in themanipulator. It does this bymakinguseof framepropagations. First, itcomputestheJacobianforamanipulatorwiththefirstlinkonly,thenforthefirsttwolinks,andsoonandsoforthupNlinksplustheendeffector.MoreefficientmethodsbasedontheworksofPieperandWhitneyutilizethepropertythatrotationaljointvelocitiesaddlinearly,andtranslationalvelocitiesmaybe determined by taking appropriate cross-products of in the individual joint rate vectors and thepositionvectorfromthatjoint.OthershavealsousedskewsymmetricmatricestocomputetheJacobiangeometrically. These methods have been used widely throughout the field and their computationalefficiencieshavebeencomparedtoeachother[28].

WhentheJacobiancannotbefoundanalytically,itisoftenfoundusingaperturbationmethod

using a first order numerical difference. A small change in each joint (or parameter) is madesystematically,and the resultingmovement isused toobtainanapproximatenumerical solution [25].This method is only sufficient in the vicinity which the Jacobian was determined. Furthermore,difficulties arise when choosing the size of the small difference to use for the method. Too large achangewill result inan inaccuratefunction if thesystemsdynamicsarenonlinear.Toosmallachangewillleadtonumericalproblemsandgreaterinaccuracies[25].

While finding the Jacobian can be challenging on its own, several problems can arise when

inverting the Jacobian Matrix. Typically, problems arise when the matrix is singular or has a poorconditionnumber.Otherproblemsarisewhen the JacobianMatrix isnot square. If the Jacobian is innumerical formbut is not square, aMoore Penrose Inverse, or least squares inverse, canbeused tocomputethepseudo-inverse.Intheeventthatthematrixissingular,orhasapoorconditionnumber,adamped least squaresapproachcanbeused.Thedamped least squaresapproachwillnotyield toanaccurateresult,butusuallyonethatiscloseenoughforengineeringpurposes.[29]Thesetechniquestoinvertingthematrixonlyworknumerically.TheanalyticformoftheJacobianmatrixmaybeevaluatedtomakeuseofthesenumericaltechniques.

Historically,determiningboththekinematicsandinversekinematicsofasystemhasbeenquite

difficult. For this reason, some roboticists use a transpose Jacobian instead to create a dynamiccontrollerinsteadoftheInverseJacobianforuseinaresolvedratecontroller[20].Thistechniquecanbeimplementedboth analytically andnumerically. In a strictly Cartesianmanipulator, the inverse of theJacobian,J,isequaltothetransposeoftheJacobian(JT=J-1).Thisisnottrueforothercases,butoftenadynamiccontrolleryieldssatisfactoryresults.However,poorperformancehasalsobeennotedfromthistypeofcontroller.Clusterspacecontrol,however,isasystemthatallowsforsystematicdeterminationof both the forward kinematics and inverse kinematics, therefore, allowing for quick computation ofboththeforwardandinverseJacobianbycomputingthepartialderivatives.Thisallowsaresolvedratecontrollertobedevelopedsystematically.

This research will present a systematicmethod to compute the analytic inverse Jacobian for

clusterspacecontrolsystemsgivenonlythegeometricdescription.Similarly,totheserialmanipulators,framepropagationwill provide the foundation to theapproach.A collectionofMatlab fileshasbeencreatedtocomputethis inverse Jacobian fora largevarietyofMRSsystems.Creatinganalgorithmtoimplement this technique provides a quick and accurate way to determine the inverse Jacobiancomparedtothemethodofcomputingpartialderivativesoftheinversekinematics.Thisisessentialtofurtherthestudyofclusterspacecontrol.

6

1.4ProjectStatement

Thepurposeofthisresearchistodevelopasoftwaresuitetoimplementanewframepropagationtechnique for generating the inverse Jacobianmatrix for cluster space formationofmobile robots. Incarrying out this work, a significant amount of effort and interest has been directed toward thefollowingtasks:

1) DevelopingasmalllibraryofClusterSpaceFormulationsasatestbed.

2) Formalizingamethodforrepresentingclusterspaceformulationsusinggraphtheory.

3) Using this graphical representation to develop an algorithm for implementing the new framepropagationtechnique.

4) WritinganimplementationofthealgorithminMathworksMatlab.

5) WritingadditionalMatlabfilesthatcomputetheinverseJacobianusingaknowntechnique.

6) Calculating the inverse Jacobian of all examples in the test bed using both techniques andcomparingresultsagainsteachotherasaverificationprocess.

The discovery and successful implementation of this new technique will allow for cluster spacecontrollerstobedevelopedmorerapidly.Furtherworkonthissoftwaresuitecanaddfeaturessuchasautomaticallydevelopinganentirecontrollerforagivencluster,aplotcreatortocomparevelocitiesindifferentspaces,aswellasaddingmoreexamplestotheexamplefolder. 1.5ReadersGuide

This section, Chapter 1, provides a quick introduction to the control of multirobot systems,challenges in developing these controlmethods, and possible drawbacks for each proposedmethod.ThischapteralsointroducestheclustercontrolarchitectureandprovidessomebackgroundoncurrentwaystodeterminetheinverseJacobianofasystem.

Chapter2introducesthehomogeneoustransformationmatrix,andshowshowitcanbeusedto

representaclusterasagraph.Theconventionsforcreatingsuchagraphareoutlined,andasystematicwayofcalculatingtheinversekinematicsofaclusterisshown.

Chapter3willintroducethehomogenoustransformationmatrix,aswellasgiveadescriptionofwhat informationincontains. Itwillthenexplainhowthehomogeneoustransformationmatrixcanbeobtainedforeachrobotandforeachcluster.

Chapter 4 presents a new formula to compute the inverse Jacobian for a cluster spaceformulation.Itthendescribesanalgorithmonhowthisformulacanbeimplemented.

Chapter 5 describes theMatlab implementation of this new formula. It shows the result ofusingthisnewtechniqueonthecontinuingexamplesandcomparestheresultstoananalytictechnique.

Chapter6concludesthereportwithasummaryofthefindingsandpossibledirectionsthiswork

canbeextended.

7

Chapter2:GraphBasedRepresentationsofClusters Inthischapter,thefactorsthatgointoclustervariableselectionarediscussed.Aquickreviewof

the homogeneous transform matrix is covered, followed by a proposal of a new way to representclusters.Theclusteristhencharacterizedasgraphbasedrepresentation,andconventionsareoutlinedtodo this systematically. Finally, the inverse kinematics for the entire robot clusterare computedbynavigatingthroughthegraph. 2.1DeterminingSuitableClusterSpaceVariables

A cluster is a system of “n” robots that are considered a single entity. These robotsmay be

locatedintheplane,orinthreedimensionalspace.Eachrobot,n,isfreetohave“pn”degreesofspatialand orientation freedom. Since the system is considered a single entity, a single frame of reference,known as a cluster frame, can be assigned to the cluster.While the location of the cluster frame istypically chosen as the average location of all the robots in the cluster, this placement is not arequirement. After selection of a suitable cluster frame, cluster space pose variables are chosen todescribe the location and orientation of the cluster frame with respect to the global frame. Othervariablesarechosentodefinethegeometryofthecluster,aswellastherelativerotationofeachrobot(usuallywithrespecttotheclusterframe).

Theactofdefiningtheclustervariablesasequationsofrobotvariablesisknownasfindingthe

Forward Position Kinematics (Eq 1). Asmentioned in section 1, this set of expressions is collectivelydescribedbythefunctionKIN(R).

𝐶 =𝑐+𝑐,⋮𝑐?

= 𝐾𝐼𝑁 𝑅D =

𝑔+(G3,GH,…,GJ)𝑔,(G3,GH,…,GJ)

⋮𝑔?(G3,GH,…,GJ)

(Eq5)

Theclustervariableschosendonotneedtobeindependent,butcollectively,theymustspan

thespace.Inordertofullyspantheclusterspaceofan‘n’robotsystemaminimumof‘f’variablesareneededtodescribethegeometryofthesystem,wherefis:

𝑓 = 𝑝NN

OP+ (Eq6)Thetotalnumberofclusterspacevariablesisq.Havingfewervariablesthanf,(q<f),leadstothe

cluster not being fully defined. Having more than f variables, (q>f), results in one or more of thevariablesbeingover-constrained(somevariableswillbedependentvariables).

Since cluster variables are used in describing the geometry, the cluster variables also have

implicit constraints. For example, consider the cluster variables L,M, and N, where L is the distancebetweenRobot1andRobot2,MisthedistancebetweenRobot1andRobot3,andNisthedistancebetweenRobot2andRobot3.Asa resultof the triangle inequality [24]ofgeometry,LmustbesuchthatL≤M+N.Furthermore,ifL=M+N,itbecomesimpossibletochangethevalueofonlyonevariable.Seefigure2below.

8

Figure2:AClusterof3robots,withthedistancevariablesM,N,andLshown

2.2TheHomogeneousTransformintermsofClusterSpace

Thehomogeneoustransformmatrix,(alsoreferredtoastheTransformationMatrixorthe‘T’matrix)hasthefollowingform,anddescribesthepositionofframe{i}relativetoframe{i-1}:

𝑇OO:+ = 𝑅OO:+ 𝑃OO:+

0 1 (Eq7)

Where 𝑅OO:+ isa3x3Matrixthatdescribestherotationofframe{i}relativetoframe{i-1}and

𝑃OO:+ isa3x1vector thatdescribes thepositionof frame {i} relative to frame {i-1}. In termsofclusterspace control, these transformationmatrices are used in several key ways. First, it can describe therobot’spositionandorientationrelativetotheclusterframewhich ithelpstodefine. Italsodescribesthe relationship between two robots in a leader follower configuration, or two systems in a leaderfollower configuration (robot following robot, robot following a cluster, cluster following anothercluster,etc.)Thesematricesalsodescribetherelationshipbetweenaclusterframeandglobalframe.

ByEuler’sRotationTheorem,anyrotation inthreedimensionalspacecanberepresentedasa

singlerotationaboutsomeaxis.Bythistheorem,wecandecomposetherotation 𝑅OO:+ intotheproductofthreerotations:

𝑅OO:+ = 𝑅OO:+

U(𝛼) ∗ 𝑅OO:+X(𝛽) ∗ 𝑅OO:+

Z(𝛾) (Eq8)Whichcanfurtherbeexpandedasfollows:

𝑅OO:+ = cos(𝛼)cos(𝛽) cos(𝛼)sin(𝛽)𝑠𝑖𝑛(𝛾) − sin(𝛼)cos(𝛾) cos(𝛼)sin(𝛽)cos(𝛾) − sin(𝛼)sin(𝛾)sin(𝛼)cos(𝛽) sin(𝛼)sin(𝛽)sin(𝛾) − cos(𝛼)cos(𝛾) sin(𝛼)sin(𝛽)cos(𝛾) − cos(𝛼)sin(𝛾)−sin(𝛽) cos(𝛽)sin(𝛾) cos(𝛽)cos(𝛾)

(Eq9)

Thisisusefultous,aswecannowdeterminetheanglesforeachrotation,creatingavectorrepresentingtheserotationsasfollows:

𝜃OO:+ = 𝛼𝛽𝛾 (Eq10)

Obtaining these angles can be an involved process, based on the hierarchical depth of the

rotations.

9

Sincetheserotationsareorthogonaltoeachother,rowsarelinearlyindependent,and,theycan

be added and subtracted without affecting each other. This will prove to be very useful in creatingcomputeralgorithmsthatinvolveroboticsystemsthathavemultipledegreesofrotationalfreedom. 2.3:GraphBasedRepresentationsofClusters

A cluster can be represented in many different ways. Naturally, one way of representing aclusteristodrawaphysicalconfigurationoftherobotcluster,andthenaddtheclustervariablestothesketchasangleanddistancedimensions.

Figure3:APhysicalViewofExample3,aclusteroftwo2-robotclusters.

Thisresearchproposesanewrepresentationforclustersspaceformationarchitecture.Thisnew

representationisaformalmethodologyfordefiningtheclusterbasedongraphtheory.Agraphbasedrepresentation lends itself well to graph theory and to algorithmic information theory, allowing forexisting theories and techniques for efficient computation can be applied to the cluster spacearchitecture.Graphswerealsochosenastheyarecapableofcapturinghierarchal informationneededwhileusingthenewpropagationbasedequations.

Torepresentaclusterasagraph,G,firstconsidereachframeusedinthecluster(robotframes,

clusterframe,andglobalframe),asavertex,V.

Figure4:Agroupofunconnectedverticesrepresentingalltheframesinacluster.

10

Then,foreachclusterframe,drawanedgeconnectingthatclusterframetoeachrobotframeor

clusterframethatisusedtodefinethatclusterframe’sposition.Thissetofedges,E1,shallbecoloredred.Atransformationmatrix,T,thatdescribesthecomponentframe’spositionandorientationrelativetotheclustermustbedefinedforeachoftheseedges.

Figure5:Two2-RobotClustersrepresentedbythesetofedgesE1.Leaderfollowerrelationshipsarethenaddedtothegraph,connectingfollowerverticestotheir

leadervertices.Thissetofedges,E2,shallbecoloredblue.A transformationmatrix,T, thatdescribesthepositionandorientationoftheframeofthefollowerrelativetoitsleadermustbedefinedforeachoftheedgesinthisset.

Figure6:A2robotclusterinaleaderfollowerrelationshipwithanother2robotclusterandafifth

robotinaleaderfollowerrelationshipwithclusterframeC2Finally,asingleedge,E3,isdrawnfromtheprimaryclusterframetotheglobalframe.E3istobe

painted black. A transformationmatrix, T, that describes the position and orientation of the primaryclusterframerelativetotheglobalframemustbedefinedforthisedge.

11

This edge also indicateswhich robot or cluster is leader or follower. The vertex closes to thegroundframeisleader,withtheothervertexontheotherendoftheblueedgeisthefollower.

Figure7:AcompletedClusterGraphofa5RobotCluster.

Theclusteristhenthegraph,G,thatisdescribedbythesetofallverticesandedgesdescribed

byG={V,E1,E2,E3}.Byassigningtheglobalframeastherootframe,thegraphGisnowconsideredatree.

By following this convention, we have a standard way of creating and understanding clustertrees.Forexample,thetreeshowninfigure7canbedescribedas:twosystemsof2-robotclusters,withcluster2(R3andR4)followingcluster1(R1andR2),andafifthrobot,R5,followingcluster2.2.4DeterminingtheInverseKinematicsfromtheGraphBasedRepresentations

BydefiningallnecessaryTmatricesinacluster,wecancomputetheinversekinematicsofthesystem. The inverse position kinematics allow computation of the ‘robot-space’ pose variables, theelementsof𝑅, asa functionof thecluster spacevariables, theelementsof𝐶.Collectively, this setofequationsisknownastheInverseKinematicEquations.ThiscanbedefinedasthefunctionINVKIN(𝐶):

𝑅 =𝑟+𝑟,⋮𝑟g

= 𝐼𝑁𝑉𝐾𝐼𝑁 𝐶D =

ℎ+(j3,jH,…,jk)ℎ,(j3,jH,…,jk)

⋮

ℎl(j3,jH,…,jk)

(Eq11)

In order to compute the inverse Kinematics, not only must we compute the individual

transformationmatricesfromeachvertextoitsparentvertex,buttheentiretransformationfromeachrobotnodeframetotheglobalframemustbedetermined.Thiscanbedonebyperformingadepthfirstsearch from the root node down to the robot nodes. Once a robot node is found, the ‘T’ matricesdefinedalongthepathfromtherootnodetotherobotnodearethenmultipliedtogether.

This,infact,implementsatechniqueproposedbyDr.ChrisKittstocomputeinversekinematics.

In this proposal, the inverse kinematics can be found my finding the product of intermediatehomogeneoustransforms,suchthatthehomogeneoustransformforanyrobot,i,isfoundby:

𝑇OD = 𝑇m+

D ∗ ( 𝑇mnmno3p

qP, ) ∗ 𝑇Omr (Eq12)

12

Whered is thenumberofhierarchical framestepsbetween{G}and{i},and𝐶N is then

th localcluster framebetween{G}and {i}. In thecase thatn<2, theproduct termvanishes. In thecaseof a1robotsystem,itisassumedthattherobotisfollowingsomeclusterframe,evenifthatframeisincidentwiththerobotframe,resultinginaTmatrixwhichisequaltotheidentitymatrix.

Theresultofequation12willalsohavetheformofahomogeneoustransformmatrix.Theresultingmatrixwillyield:

𝑇OD = 𝑅OD 𝑃OD

0 1 (Eq13)

Where 𝑃OD isthepositionvectorofrobotirelativetotheglobalframe,and 𝑅OD istherelativerotationofrobotiwithrespecttotheglobalframe.

From this matrix, the position 𝑥, 𝑦, 𝑎𝑛𝑑𝑧 for each robot i can be recovered from 𝑃OD .Furthermore, 𝑅OD can be decomposed into 𝑅U 𝛼 , 𝑅X 𝛽 , 𝑎𝑛𝑑𝑅Z(𝛾) for each robot i. Finding theseyieldstheinversekinematicequationsofthesystem.

This shows that the tree structure described in section 2.3 accurately produces the inversekinematics for a given cluster. This result lends credibility that thisparticularmethod fordescribingaclusterasagraphisvalid.2.5 AfewnotesontheTreeRepresentationofaclusterspaceformulation

Justlikewithserialmanipulator,thismethodismuchsimplerthanalgebraicallyrearrangingtheforwardkinematicequationstoisolatetheindividualrobotvariables.However,thismethodallowsforthepossibility of some information contained in the forward kinematic equations tobe lost. In otherwords,ifoneweretotrytoisolatetherobotvariablesfromthesystemofequationsobtainedfromthepropagationtechniquefortheinversekinematics,thenmultiplesolutionscanbeobtainedforsomeofthe robot variables. This can possibly be avoided with a more rigorous method of selecting clustervariable(seefutureworkinSection6.2).

There are other similarities of this technique to the propagation technique used by serialmanipulators. If a homogeneous transformation matrix, 𝑇xy , is an edge that describes a path fromvertex{A}tovertex{B},thenthematrix 𝑇xy ,theedgeE2thatisapathfrom{B}to{A},canbefoundbyinverting 𝑇xy .Thisframetransformationtechniqueisalsousedinserialandparallelmanipulators.

𝑇yx = 𝑇:+xy (Eq14)

Using equation 12 and 14, it is possible, although unnecessary, to create an edge from any

vertex in the graph to each vertex in the graph, including itself. This may be useful if you need todescribeonerobotinaclusterrelativetoanother.

Furthermore,it ispossibleforarobottohavetwoleaders.Forexample,infigure7,R3canbe

followingR1ataspecificdistanceandangle,butfollowtheorientationofR2.AnotherexampleisthatrobotR3 canbe told to stayadistanced1 fromR2, andadistanced2 fromR3. If thisoccurs, the ‘T’matricesforeachblueedgewillbeill-defined.Configurationslikethoseshowninfigure7donotpresentaproblemmathematically,andcanbesolvedwithincompleteHTM’slikeequations15and16.ThetrickistopropagatefromR3toR1toC1toG,andthenfromR3toR2toC1toG.Then,tossoutanyresultscontainingxxandyy.Therestofthisresearchwillassumethateachfollowerhasonlyoneleader.

13

Figure8:ARobotwithtwoleaders.

𝑇3.1 =cos(𝑥𝑥) −sin(𝑥𝑥) 𝑙 ∗ cos(𝜃j)sin(𝑥𝑥) cos(𝑥𝑥) 𝑙 ∗ sin(𝜃j)

0 0 1 (Eq15)

𝑇3.2 =cos(𝐴) −sin(𝐴) 𝑦𝑦sin(𝐴) cos(𝐴) 𝑦𝑦0 0 1

(Eq16)

14

Chapter3:JacobianPropagationAlgorithm ThissectionpresentsthegivenformulasusedfordeterminingtheinverseJacobianofacluster.

Thisalsolaysthemathematicalfoundationfortheimplementationoftheseformulae.Aparticularwaytoimplementtheseformulasisthenpresented.Thischapterconcludeswitharuntimeanalysisofthisparticularimplementation. 3.1TheInverseJacobianAlgorithm

TheRobotSpacepositionvariables,𝑅, and theClusterSpacevariables,𝐶, aremappedby the

functionsKIN(Eq1)andINVKIN(Eq2).TomapthevelocitiesfromRobotSpace,𝑅,toClusterSpace,𝐶,the matrix of partial derivatives, 𝐽 𝑅 , known as the Jacobian Matrix is used. Similarly, the inverseJacobian,𝐽:+ 𝑅 ,transformsClusterSpacevelocitiestorobotspacevelocities.

𝐽:+ 𝐶 𝐶 =

p�3pj3

⋯ p�3pjk

⋮ ⋮p�Jpj3

⋯ p�Jpjk

𝑐+𝑐,⋮𝑐?

(Eq17)

Forclusterspacecontrol,theinverseJacobianhistoricallyhasbeencomputedbytakingpartial

derivativesofthe inversekinematics.Thisprocess isatimeconsumingandcomputationallyexpensiveprocess.ThereisaneedforfastercalculationoftheJacobianforusesinhighspeedsystems.

ItwasproposedbyDr.Kittsthateachrowofthematrix,𝛻hn , isarobotspacevelocityvectorthatcanbedevelopedbyperformingavelocitypropagationanalysisforeachrobotn.Thepropagationstartswithdeterminingthevelocityofarobot,n,withafixedframe{n}relativetoitslocalclusterframe{i}. Both the linear velocity of robot n relative to frame {i}, 𝑉NO , and its angular velocity, 𝜔NO ,must bedeterminedandwrittenusingclusterspacevariables.Thesevelocitiesarethenpropagatedfromframeto frame(fromcluster to leaderclusteror toparentcluster).Thepropagationcontinuesuntil the lastframe;thatofglobalframe{G}.Theresultofperformingthesepropagationsforeachrobot leadstoasystem of linear equations in cluster variable that are assembled to form the inverse Jacobian. Thisresultsinmappingclusterspacevelocitiestorobotspacevelocities.

𝐽:+ 𝐶 𝐶 =

𝛻ℎ+𝛻ℎ,⋮

𝛻ℎg

=

𝑉+D 𝜔+D⋮𝑉ND 𝜔ND

(Eq18)

Tofindthelinearandangularvelocitiesofrobotnwithafixedobjectframe{n}inframe{i},the

rateofchangeoftheirlinearandangularpositioncanbetakenasfollows:

𝑉NO = 𝑃NO + 𝜔NO × 𝑃NO (Eq19)

𝜔NO = 𝜃NO (Eq20)

15

Where:𝑉NO =Thetotallinearvelocityoftheframe,n,relativetoframei𝑃NO =Thepositionoftheframe,n,relativetoframei

𝑃NO =Thederivativeof 𝑃NO or𝛻 𝑃N

O 𝜃NO =Theangleofthetwoargumentarctangentofthepositionvector, 𝑃NO 𝜔NO =thederivativeof 𝜃NO ,or∇ 𝜃NO ,or 𝜃NO

Sincethelinearandangularvelocitiesoftheframe{n}inframe{i}areknown,thenitispossible

tocalculatethevelocitiesofthatframerelativetoframe{i-1}asfollows:

𝑉NO:+ = 𝑉OO:+ + 𝑅OO:+ ( 𝑃NO + 𝜔NO:+ × 𝑃NO ) (Eq21)

𝜔NO:+ = 𝜔OO:+ + 𝑅O

O:+ ∗ 𝜔NO (Eq22)Where:

𝑉NO:+ =Thetotallinearvelocityoftheobjectnrelativetoframe{i-1}𝑉OO:+ =Thetotallinearvelocityofframe{i}relativetoframe{i-1}𝑅O

O:+ =Therotationmatrixthatdescribesthefixedrotationofframe{i}relativetoframe{i-1}𝜔OO:+ =theangularvelocityofframe{i}aboutframe{i-1}𝜔NO:+ =theangularvelocityoftheobjectnrelativetoframe{i-1}

Therefore, for any robot, n, with hierarchical depth m, these equations can be applied

recursivelyfromtherobotthroughthehierarchalpathofframesbacktotheglobalframe{G}.Doingsoresultsintheequations:

𝑉ND = ( �qP+

��P+ 𝑅)q

q:+ ( 𝑃��:+ + 𝜔�D × 𝑃��:+ ) (Eq23)

𝜔�D = ( �OP+ 𝑅O:+

O:, ) 𝜔OO:+��P+ (Eq24)

Where: 𝑉ND =Thetotallinearvelocityofrobotnrelativetoglobalframe 𝜔�D =Thetotalangularvelocityofframekrelativetotheglobalframe 𝑅q

q:+ =Therotationmatrixthatdescribesthefixedrotationofframe{j}relativeto{j-1} 𝑃��:+ =Theinstantaneouspositionofframe{k}relativetoframe{k-1}

𝑃��:+ =Therateofchangeofinstantaneouspositionofframe{k}relativetoframe{k-1}

Forequations23and24,jisthenumberofstepsthatthecurrentframeisawayfromtheclusterframe, andm depth level of robot n. It is worth noting that when j = 1, 𝑅q

q:+ becomes the identitymatrix,andwhenk=1, 𝜔�D ,becomesazerovector.

By applying these formulas for each robot in a given cluster, we can determine the inverse

Jacobianofthatentiresystem.AderivationoftheseformulascanbefoundinAppendixC.

16

3.2AnimplementationoftheFormula

Therecursivenatureoftheformulaeinsection3.1lendthemselvestocreatinganalgorithmfor

execution.Inanattempttodothisasquicklyandasefficientlyaspossible,theClusterTreeInverseJacobianalgorithmwascreated.Thisalgorithmisoutlinedasfollows:

invJ=invjacfxn(clusterTree)Initialization { IndexallnodesonclusterTree Createlistofclustervariables Createlistofallrobotnodes CreateemptyinvJmatrixtopopulate }MainLoop {

Foreachnodelistedinlistofallrobotnodes LinearVelocityPropagationSubroutine SaveResulttoinvJmatrix

AngularVelocityPropagationSubroutine SaveResulttoinvJmatrix End

}Termination { Terminateafterallelementsinlistofallrobotnodeshasbeenvisited DisplayfinalinvJMatrixshowingtheinverseJacobian

}

BothsubroutinesintheMainLoopareratherinvolvedsequenceswhichrequiretraversaloftheClusterTreethatistakenasaninput.Adetailedofexplanationoftheinitialization,mainloop,andterminationstepsareasfollows:

Initialization1) Letusstartbycreatingaclustertreeaslistedinsection2.3.ThistreemusthaveallT

matricesdefinedandrecordedasedges.Letallnodesinthetreebegincoloredwhite,withalledgesretainingtheirpreviouscoloration.Nodesonthetreeareindexedbyabreadthfirstsearchalgorithm.

2) Createalistcontainingthederivativesofeachoftheclustervariables.(‘cluster_var’)3) Createalistofalltherobotnodesthatarepresentinthetree,aswellastheirindexinthe

tree.(‘robot_node’)

17

4) Weshallalsocreateablankmatrixmxninsizenamed𝐽:+.Thesizeofmisthesumofallthedegreesoffreedomofalltherobotsinacluster,andnisdeterminedbythenumberofclustervariables.Initializeallentriesinthismatrixto0.

Figure9:Initialtreewithallnodescoloredwhite.

MainLoop1) Visitthefirstelementinthe‘robot_node’list,andcolorthenodeyellow.

Figure10:Treewithcurrentnodecoloredyellow.

2) PerformLinearVelocityPropagationSubroutine

2.1)Createavariable,‘𝑣𝑎𝑟𝐴′ = 𝑒𝑥𝑝𝑟1𝑒𝑥𝑝𝑟2𝑒𝑥𝑝𝑟3

.Eachexpressioninthisvariableis

automaticallyfactoredbythelist‘cluster_var’.Initializeallentriesto0.2.2)Computetheconjugaterotationoftheyellownodefromtheglobalframe.

2.2.1)ExtractthelocalrotationofcurrentframefromtheparentframebyextractingitfromtheHTM.Savethisasthevariable‘RR’.

18

2.2.2)Updatethecurrentnodetotheparentnode,coloringthenewnodeyellow,andtheoldnodered.Repeatstep2.2.1,addingthepreviouslyextractedrotationtothenewrotation(‘RR’=‘RR’+‘newresult’).

Figure11:Clustertreewithexplorednodecoloredredandcurrentnodeyellow2.2.3)Repeatuntilrootnodeiscurrentnode.Saveresultas‘RR’.Turnthisnodered.

Figure12:Clustertreewithallnodesfromleaftorootexploredonce.

2.3)Returntorednodewiththehighestindex,andmakethisnodethecurrentnode,bychangingthecolorofthenodetoyellow.ExtractthelocalpositionofthisnodefromtheHTM,andgetthelocalvelocitybypartialdifferentiation.

19

Figure13:Returningtotherobotnodeofthecurrentsubroutine.

2.4)Rotatethelocalvelocityinstep2.3androtateitbythe‘RR’calculatedinstep2.22.5) Find theglobal angular velocitybydifferentiating the variable ‘RR’ fromstep2.2.Thentakethecrossproductofthatwiththepositionofthecurrentnoderelativetoitsparentnode.2.6)Rotatethecrossproductcalculatedinstep2.5by‘RR’determinedinstep2.22.7)Updatethevalueof‘varA’toincludethetheresultsofsteps2.4and2.6asfollows:‘varA’+=step2.4+step2.62.8)Updatethecurrentnodeonthetreebychangingthecolorofthecurrentnodetothecolorgreenanditsparenttoyellow,thusmakingtheparentthenewcurrentnode.

Figure14:ClusterTreewithExplorednodesaregreenandcurrentnodeisyellow.

2.9)Repeatsteps2.2thru2.8untiltherootnodeisreached.2.10)Getthelocalpositionoftherootnodeanddifferentiateit.

20

2.11)Addtheresultto‘varA’.(‘varA’+=‘step2.10’).Colortherootnodegreen.

3) Savetheresultofstep2.11tothefirstrowsofthematrixnamed𝐽:+belowwherevarbwaslaststored.Ifthisisthefirstiterationoftheloop,savetothetoprowsof𝐽:+.

4) PerformAngularVelocityPropagationSubroutine

4.1)Createavariable,𝑣𝑎𝑟� = 𝑒𝑥𝑝𝑟1𝑒𝑥𝑝𝑟2𝑒𝑥𝑝𝑟3

.Eachexpressioninthisvariableisautomatically

splitandsortedbythelist‘cluster_var’.Initializeallentriesto0.4.2)Performasearchtofindthegreennodewiththelargestindex.Makethisnodethecurrentnode(colourityellow).

Figure15:Returningtorobotnodebeingcomputedandmarkascurrentnode

4.3)Calculate‘RR’forthecurrentnodesimilarlytostep2.2.Useredagain.

4.4)Calculateglobalangularvelocitybydifferentiatingtheresultofstep3.3.Savetheresultto‘varB’.

5) Savethe‘varB’torowsofthematrixnamed𝐽:+directlybelowwherevarAwasstored.6) Returntorednodewiththehighestindexandcolorthisnodeblack.Restoreallnon-black

nodestothecolorwhite.

Figure16:Aclustertreewithasinglefullyexploredrobotnode

7) Gotothefollowingelementlistedinthe‘robot_node’listandcolorityellow.

21

Figure17:Afullyexploredrobotnodeandanewrobotnodeselected.

8) Repeatsteps2through7ofMainLoop

Termination

1) ContinueRepeatingsteps2through7untilallnodesforelementsinthelist‘robot_node’

arecoloredblack.

Figure18:Afullyexploredclustertree.

Uponcompletionofthisalgorithm,theinverseJacobianwillbeobtainedandsavedintothematrix𝐽:+.Thisimplementationonlysupportsnon-cyclicgraphs,andclustersthataredefinedinaCartesianco-ordinatesystem3.3RunTimeAnalysisofthisalgorithm

Scalability of this algorithm is important to consider with respect to the number of clustervariables,numberofrobots,andhierarchicaldepthspresentinacluster.Toanalyzehowthisalgorithmwould respond to an increase in any of these conditions, runtime analyseswere performed, and thegrowthratesexpressedinBigOnotation.

22

It was determined that the runtime increases linearly to an increase in number of cluster

variables. In the algorithm above, there are no loops that are affected by the length of the list‘cluster_var’. However,thelengthof‘cluster_var’affectshow‘varA’and‘varB’arefactored,andhowmanypartialderivativesneedtobecalculatedforeachstepinvolvingadifferentiation.Ifweassumeaparsing function scales linearly with the number of delimiters, and if we assume that taking partialderivativesscaleslinearlywiththenumberofpartialstaken,thentheabovealgorithmhasaruntimeofO(n)withrespecttonumberofclustervariables.

The algorithm also scales linearly with an increase in the number of robots. The number of

robots in the cluster directly affects the length of the list ‘robot_node’. For each element in this list,subroutineA isperformedexactlyonce,andso issubroutineB.Therefore,thealgorithmabovescaleslinearlywiththenumberofrobots.Therefore,thisalgorithmscaleslikeO(n)withrespecttonumberofrobotsinthecluster.

Anexponentialincreaseoccurswithanincreaseinhierarchaldepth.Foranyvertexatdepthd,

step2.2ofthelinearvelocitypropagationsubroutine,aswellasalloftheangularvelocitypropagationsubroutine, needs to compute with the d-1 vertices above it in the tree already solved. Since thealgorithmdidn’t save theprevious result, it recomputedthis informationevery timethecurrentnodeupdatedtotheparentnode.Therefore,thisalgorithmscaleslikeO(n2)withrespecttooverallhierarchaldepthofthecluster.

23

Chapter4:MatlabImplementationofAlgorithm

This section will describe a Matlab toolbox that was created to test the algorithm stated inChapter 3. It will outline each file used in each of the folders of the toolbox. The firstmajor folder,Cluster_Builder, contains files that guide the user in creating a cluster tree to use for their customclusterspaceconfigurationofrobots.Thesecondmajorfoldercontainsfilesthatcomputethe inversekinematicsandthe inverseJacobiananalyticallybytakingpartialderivativesofthe inversekinematics.Thethirdfoldercontainsalistofexamplesusedforverifyingthealgorithmdescribedinsectionthree.Finally, the fourthmajor foldercontains files that implement thealgorithmstatedabove.TheArchivefolderscontaineddeprecatedfiles,andthe@treefoldercontainsthedefinitionofthetreeclass.

Figure19:DirectoryoftheClusterSpaceControlInverseJacobianToolbox

24

4.1TheCluster_BuilderFolder

The files in this folderallows theuserof the toolbox toenter informationabout their customclusters,andthencomputetheinverseJacobianforthatclusterusingeitherthedirectapproach,orthepropagationbasedapproach.

Tobuildaclustertreeusingthefilesinthisfolder,onemustmanuallycreateaclustertreeandhaveall edgesdefinedand ready tobe inputted. Theusermust alsohavedownloaded the tree classfromtheMathWorkswebsite,asit isnotastandardclass.ThisclasswascreatedbyJean-YvesTinevezon13March2012andupdatedon18Nov2015Itcanbedownloadedfromthelink:http://www.mathworks.com/matlabcentral/fileexchange/35623-tree-data-structure-as-a-matlab-class

Thefileclusterbuilder2.misthengiventhenumberofrobotnodesandnumberofclusternodes

as input, and then guides the user on how to enter all the cluster information into a singleMatlabvariable.Table4listsallthefilesinthisfolder,andprovidesadescriptionofwhateachfiledoes.Figure19and20showshowthisfunctionlooksonscreen.

Table1:AlistofallthefilesintheCluster_BuilderfolderFileName Description

Clusterbuilder2.m

Thisisthemainfunctionthatcallsalltheothersubroutines.Ittakesthenumberofrobotnodesandthenumberofclusternodesasinput,andthefinalresultinavariablecontainingtheclusterinformation.Thefunctioncommandis[obj_out]=clusterbuilder2(num_robots,num_clusters)

Robotnamer.m

ThisprogramnamesalltherobotsasR1,R2,R3.Etc…Thenameoftherobotatnode(n)canlaterberenamedbyusingthecommandexample.txt_tree=example.txt_tree.set(n,’newrobotname’).

Clusternamer.m

ThisprogramnamesalltheclustersasC,C1,C2,C3…,.Thenameoftheclusteratnode‘m’canlaterberenamedbyusingthecommandexample.txt_tree=example.txt_tree.set(‘m’,‘newclustername’).

Variablecreator.m Asks the user to list all variable names that will be used to describegeometricfeaturesofthecluster.Itstoresthisinformationinavector.

Symbolicbank.m Loadsabankofsymbolicvariablestomemory.Italsoloadsallassumptionsonsymbolicvariables.

Treecreator.m Asks user to arrange the robot nodes and cluster nodes into anarborescence. (Tree structure, not a graph) that spans all nodeswith onlyoneroot.Callssubroutinesrobot_tree.mandcluster_tree.m

Robot_tree.m Subroutineoftreecreator.m.ItaddsrobotnodestothetreeCluster_tree.m Subroutineoftreecreator.mItaddsclusternodestothetree.

Htmdefiner.m asks the user to enter the homogeneous transformation matrix thatdescribesthepositionandorientationofeachnoderelativetoitsparent.

Msot.m Compiles all the information into a single structure containing all theattributeslistedintable4.1

25

Figure20:Userenteringtreeinformationforexample3intoclusterbuiler2.m

Figure21:UserenteringHTMmatricesforeachedgeonthetreeofexample3

Oncetheuserfinishestheentrysequenceforthatcluster,theclusterbuilder2.mfunction

outputs a variable as seen in figure 22 with the attributes listed in table 2. The attributesexample.propinvjac,example.inv_kin,andexample.dirinvjacarenotinitiallysavedinthisvariable.TheyareonlycreatedaftertheinversekinematicsortherespectiveJacobianfunctionisusedonthevariable.

26

Table2:Alistofallthevariableattributessavedineachexamplevariable.

Attribute Descriptionexample.robotnodes Avectorcontainingallnodesintheclustertreethatbelongtorobot

nodes.Eachvaluethatdescribesanodeissavedastypedouble.example.clusternodes Avectorcontainingallclustertypenodesintheclustertree.

example.htm_tree

Treetypeclass.Eachnodeofthistreecontainsthehomogeneoustransformationmatrixofthecorrespondingexample.txt_treenoderelativetotheparentofthatnode.Eachcontainsa3x3or4x4arraywhoseelementsarealltypesymbolic.Toviewthetreestructure,usethecommanddisp(example.htm_tree.tostring).Toviewthehomogeneoustransformmatrixataparticularnode(n),usethecommandexample.htm_tree.get(n)

example.txt_tree Containstheinformationabouttheclustertree.Toviewthisattribute,usethecommanddisp(example.txt_tree.tostring).

example.var Avectorofsymbolictypeelements.Thevectorcontainsalltheclustervariablesneededtodescribethesystem

example.propinvjac ContainstheinverseJacobianoftheclusterascomputedbythevelocitypropagationapproach

example.inv_kin Containstheinversekinematicsforeachrobotascomputedbythedirectapproach.

example.dirinvjac ContainstheinverseJacobianoftheclusterascomputedbydirectdifferentiationoftheinversekinematics

Figure22:DisplayofattributesinitiallysavedintheMatlabvariableexample3.

27

4.2TheInverseKinematicsFolder

Thesecondmajor folderof theToolboxcontains files thatexecute thealgorithmdescribed insection3.Thisfoldercontainsfilesthatworkforclusterswith6degreesoffreedom,andanoptimizedsetofsubroutinesforworkingwithclustersthatarelimitedtotheplane.Table3isalistofallthefilesandwhichsteps in thealgorithmthat they refer to.Figures23and24showthe resultofusing thesefunctions.

Table3:DescriptionofallFilesintheInverseKinematicsfolderFileName Descriptioninvkin.m This file computes the inverse kinematics for each robot by tracing a

pathfromeachrobotnodebacktotherootnode.Itthenmultipliesthehomogeneous transform matrices from root back down to the leafnodes (robot nodes). It then saves the results as a variable attribute,example.inv_kin

dirinvjac.m This file first calls the program invkin.m to compute the inversekinematicsofeachrobot.Eachelementinthevectorexample.inv_kinisIt then differentiated by each symbolic element in the vectorexample.syms. The resulting array is saved as the variable attributeexample.dirinvjac

Figure23:Thefunctioninvkin.mbeingusedonexample3variable

28

Figure24:Thefunctiondirvinvjac.mbeingusedonexample3variable

4.3TheTest_BedFolder

ThethirdmajorfolderoftheToolboxisfullofexampleclusters.Eachsampleclusterissavedasavariable,andeachattributeofthevariablecontainsinformationthatdescribesthecluster.Afulllistofalltheattributescanbefoundintable2.

Eachoftheexamplessavedinthetestbedrepresentadifferenttypeofcluster.Theseexamplesvary innumberofrobots,numberofdegreesoffreedom,hierarchicaldepthofrobots,andclustersofdifferent types of configurations. To load an example to the active workspace, use the commandload('example.mat').Allexamplesincludedinthisfolderarelistedintable4.

Figures25and26showthephysicalviewandgraphicalviewofExample3.TheClusterframe,C,

isdeterminedbytwosubclusters,C1andC2.Robot1andRobot2 formthesubclusterC1,andRobot3and Robot4 form subcluster C2. Equations 25 and 33 give the forward and inverse kinematics of thesystemrespectively.Equations26-32givetheHTM’sforeachedgeneededforthegraph.


{C}

29

𝑥j𝑦�𝜙+𝜙,𝜙l𝜙�𝜃j𝑙𝑚𝑛𝜃j+𝜃j,

=

(Z3�ZH�Z��Z�)�

(X3�XH�X��X�)�

𝜃+ −𝜃j+ −𝜃�𝜃, −𝜃j+ −𝜃�𝜃l −𝜃j, −𝜃�𝜃� −𝜃j, −𝜃�

𝑎𝑡𝑎𝑛2 (𝑦� − 𝑦j+ , 𝑥� − (Z3�ZH),

)

(X3�XH),

− 𝑦�,+ (Z3�ZH)

,− 𝑥�

,

(X3�XH),

− 𝑦+,+ (Z3�ZH)

,− 𝑥+

,

(X��X�),

− 𝑦+,+ (Z��Z�)

,− 𝑥+

,

acos (X�:X3 H� Z�:Z3 H:��H:�H))��

acos (X�:X� H� Z�:Z� H:��H:NH))��N

(Eq 25)

Figure26:AGraphrepresentingaclusterofclusters.Herecluster{C1}and{C2}formasinglecluster{C}.

30

𝑇mD = cos(𝜃�) −sin(𝜃�) 𝑥jsin(𝜃�) cos(𝜃�) 𝑦j

0 0 1 (Eq 26)

𝑇m+m =

cos(𝜃j+) −sin(𝜃j+) 𝑙sin(𝜃j+) cos(𝜃j+) 0

0 0 1 (Eq 27)

𝑇m,m =

cos(𝜃j,) −sin(𝜃j,) −𝑙sin(𝜃j,) cos(𝜃j,) 0

0 0 1 (Eq 28)

𝑇�+m+ =

cos(𝜙+) −sin(𝜙+) 𝑚sin(𝜙+) cos(𝜙+) 0

0 0 1 (Eq 29)

𝑇�,m+ =

cos(𝜙,) −sin(𝜙,) −𝑚sin(𝜙,) cos(𝜙,) 0

0 0 1 (Eq 30)

𝑇�lm, =

cos(𝜙l) −sin(𝜙l) 𝑛sin(𝜙l) cos(𝜙l) 0

0 0 1 (Eq 31)

𝑇��m, =

cos(𝜙�) −sin(𝜙�) −𝑛sin(𝜙�) cos(𝜙�) 0

0 0 1 (Eq 32)

𝑥+𝑦+𝜃+𝑥,𝑦,𝜃,𝑥l𝑦l𝜃l𝑥�𝑦�𝜃�

=

𝑚 ∗ cos 𝜃j+ + 𝜃� + 𝑙 ∗ cos 𝜃� + 𝑥j𝑚 ∗ sin 𝜃j+ + 𝜃� + 𝑙 ∗ sin 𝜃� + 𝑦j

𝜃� + 𝜃j+ +𝜙+−𝑚 ∗ cos 𝜃j+ + 𝜃� + 𝑙 ∗ cos 𝜃� + 𝑥j−𝑚 ∗ sin 𝜃j+ + 𝜃� + 𝑙 ∗ sin 𝜃� + 𝑦j

𝜃� + 𝜃j+ +𝜙,𝑛 ∗ cos 𝜃j, + 𝜃� − 𝑙 ∗ cos 𝜃� + 𝑥j𝑛 ∗ sin 𝜃j, + 𝜃� − 𝑙 ∗ sin 𝜃� + 𝑦j

𝜃� + 𝜃j, +𝜙l−𝑛 ∗ cos 𝜃j, + 𝜃� − 𝑙 ∗ cos 𝜃� + 𝑥j−𝑛 ∗ cos 𝜃j, + 𝜃� − 𝑙 ∗ cos 𝜃� + 𝑥j

𝜃� + 𝜃j, +𝜙�

(Eq 33)

31

Table4:ExamplesinthetestbedfolderandanexplanationoftheirconfigurationName NumberofRobots Configuration

Example1 22robotswithclusterframedefinedasmidpointbetweenbothrobots.Therobotsarelimitedtotheplane.

Example2 4

2robotswithclusterframedefinedasmidpointbetweenbothrobotswiththeclusterframeactingasleadertoanotherclusterframe.Thefollowerclusterhas2robotswithitsclusterframedefinedasmidpointbetweentherobotsinthatframe.Therobotsarelimitedtotheplane.

Example3 4Aclusterofclusters.Twoclustersof2robotseachformacluster.Therobotsarelimitedtotheplane.

Example4 2

Aclusterof2robotswithonerobotbeinglocateddirectlyabovetheclusterframeandisfreetohaveadifferentorientationthantheclusterframe.Therobotsarelimitedtotheplane.

Example5 2

Aclusterof2robotswithonerobotbeinglocateddirectlyabovetheclusterframe.Thisrobotisalsoforcedtobealignedwiththeclusterframe.Thissystemisoverconstrained.Therobotsarelimitedtotheplane.

Example6 2Aclusterof2robotswiththeclusterframearbitrarilyplaced.Thissystemwillhaveredundantdegreesoffreedom.Therobotsarelimitedtotheplane.

Example7 2 Example1extendedinto6degreesoffreedomperrobot

Example8 33robotswithclusterframedefinedastheaveragelocationofthethree3robots.Therobotsarelimitedtotheplane.

Example9 33robotswithclusterframeasthemidpointbetweenRobot1andRobot2withRobot3followingtheclusterframe.

Example10 3

3robots.Robot1isit’sownclusterwiththeclusterframeincidentwithrobot1.Thatclusterframeplusrobot2formanotherclusterframelocatedatthemidpoint.Finally,robot3isfollowingtherobot1cluster.Therobotsinthisexamplearealsolimitedtotheplane.

Afulldescriptionofeachcluster,includingthephysicalview,theforwardkinematics,thegraph

basedrepresentation,thehomogeneoustransformmatricesusedandtheinversekinematicscanbefoundintheAppendixB.

4.4TheVelocity_Propagation_TechniqueFolder

Themaincomputationofthissubfolder istoexecutethealgorithmdescribedinsection3.4. Itdoes this by taking the cluster tree thatwas built, and traversing it accordingly. It is also capable ofdetermining the number of degrees of freedom based on the size of the homogeneous transform

32

matrices, and adjusts the computation accordingly. Below is a list of all the files associatedwith thisvelocitypropagationapproach.

Table5:DescriptionsofthefilesintheVelocity_Propagation_TechniqueFolder

FileName Description

Invjacfxn.m

Themainfunctionofthealgorithm. Ittakestheclustertreeasan inputandoutputstheinversejacobianofthesystem.ItalsosavestheinverseJacobianasanattributetotheinput.Thetreeimplementationdescribedinsection3madeusedofdifferentcolornodes.Thistoolboxcircumventscolorusagebyusingadditionalindicesandcounters.Italsodistinguishesbetween3x3and4x4HTMs,andcallsotherfilesasappropriate.

gettheRR3.m Calculatestheproductofrotationsforthecurrentnode.DoesStep2.2ofSection3.Onlyworkswith3DegreesofFreedom(3DOF)

getthev3.m Calculatesthelocalvelocityofoneframerelativetoitsparent.Performsstep2.3

rotatedwxp3.m Calculates the local angular velocity, and rotates itwith respect to theStep2.5and2.6

getthefinv3.m Calculates the localvelocityof themaincluster framerelative toglobalframe.Thisisthestep2.10andstep2.11

getthewthing3.m Calculates the local angular velocity. This is the same as the angularvelocitypropagationsubroutine.

gettheRR6.m ThesameasgettheRR6,butisonlycalledwhendealingwithmorethan3DOFperrobot.

getthev6.m Doesthesameasgetthev3,butcalledwhenuserentersa4x4matrix.rotatedwxp6.m Doesthesameasrotatedwxp6,butcalledwhenuserentersa4x4matrix.getthefinv6.m Doesthesameasgetthefinv3,butcalledwhenuserentersa4x4matrix.

getthewthing6.m Does the same as getthewething3, but called when user enters a 4x4matrix.

Touse themain function, oneuses the command [ inv_jac_prop, example] =invjacfxn(example).Figure27showswhatthisexecutionlookslikeintheMatlabenvironment.

Figure27:Thefunctioninvjacfxn.mbeingusedonexample3variable

33

Chapter5:Results

This sectionwill state the resultsof implementing thisalgorithm inMatlab. Itwillpresent thefindings from 10 examples that the algorithm developer created, and an example from anotherresearcher in SantaClaraUniversity’sRobotic Systems Lab. Finally, it outlines someadditional testingthatcanbedonetotesttherobustnessofthisMatlabimplementation. 5.1ResultsfromTest_BedExamples

Eachof the ten systems in theTest_BedFolderhad the inverse Jacobian computedusing thefunctionsdirinvjac.mandinvjacfxn.m.Eachresultforeachexamplewascomparedelementbyelement in the matrix to ensure that they are indeed the same. For all ten examples, the matricesproducematched.AnexampleofthesecanbeseeninChapter4infigures24andfigure27.

Aninterestingfindingwasthatbyusingthefunctionwhichusesthenewlycreatedalgorithm,alesscompactformofeachoftheelementsinthematrixwereobtained.Thismaybetheresultofsomeoptimization within Matlab’s built-in ‘Symbolic Math Toolbox’. However, after using simplificationfeatures,itisclearthattheresultswerethesame.Anattemptwasmadetoautomatethisstep,butwasunsuccessful. 5.2ResultsfromaThirdPartyCluster

To remove any bias that may be present in the ten example clusters listed above, it wassuggestedthatthealgorithmbetestedonaclusterdefinitionnotpreviouslyencounteredbytheauthor.AlexMulcahy of Santa Clara University’s Robotic Systems Lab requested an analysis of the followingclustergivenhisuseofthisparticularclusterdefinitioninhisownresearch.

Figure28:TheAlexCluster.A5robotclusterwithrobots1and2formingthemaincluster,robot4followingrobot

2,robot3followingrobot1,androbot5followingrobot3.

34

After discussing the cluster with him for a few minutes, we sketched out the robot tree

together, and then entered the transformationmatrices into the cluster builder program. At first, itappearsthattheresultsareverydifferent.However,afterrunningthesimplifyfunctiononbothresults,more compact formswere of eachwere obtained. In compact form, it is clear that both techniquesyielded the same result for this cluster. This result has significantly helped test against any bias thatmighthavebeencreatedusinganinternaltestbed.

Figure29:Columns1through7oftheinverseJacobianoftheAlexCluster.

Figure30:Columns8through15oftheinverseJacobianoftheAlexCluster.

5.3FurtherTestingII

Currently, there are about 200 variables insymbolicbank file. This means that there areenoughvariablestodescribeaclusterwithuptoabout33robots,eachwith6degreesoffreedom.Thislimitation can be increased by addingmore variables to thesymbolicbank file. However, clustersthatapproachthislimitinsizehavenotyetbeentested.Therearealsoothertypesofclusterdefinitionsthat should also be compatiblewith this implementation, but have not yet been tested. Examples ofthese are recorded in Table 6. This testing may be helpful in demonstrating the robustness of thisimplementationofthealgorithm.

35

Table6:OtherClusterDefinitiontotestRobustnessofToolboxExample Number

ofRobotsConfiguration ReasonforTesting

Example11 20 Eachwith3degreesofFreedom.

Willtesttheabilitytoworkworkwithalargenumberofrobots.

Example12 4 Eachwith6degreesoffreedom,notlimitedtotheplane.Clusterframeatthecentreofthepyramidformed.

Thiswillfullytestitsabilitytoworkwithspacecraftandother6DOFvehicles.

Example13 4 Eachwith6degreesoffreedom,testedinnonstandardaxes.

Resultscouldbeusedtoexpandthefieldofclustercontrolinamathematicalsense.

Example14 16 Aclusterofclustersofclusters.

Testabilitytoscaledepth.

36

Chapter6:Conclusions

ThisChaptersummarizestheworkthathasbeendoneinthisresearch.Itsummarizestheresultsandconclusionsdrawn,aswellasoutlinespossibilitiesforfuturework.6.1Summary

ThisthesishascompleteditsmainobjectiveofautomatingtheprocessofcomputingtheinverseJacobian.Severalstepsweretakeninordertoachievethisobjective.

TheframeequationsthatwerepresentedbyDr.ChrisKittswerestudiedindepthtoguaranteea

throughout understanding of the mathematics involved in this computation. From there, a few testcaseswerecomputedmanuallytoensureproperunderstandingoftheequations.

Once the equations were understood, substantial effort was put into finding a graph based

representation for cluster space formulations. Graphs were considered as they lend themselves toexistingtheoriesfromalgorithmicinformationtheory.Treeswereconsideredparticularlyforclustersasitwasaneasywaytocapturehierarchalinformation.

Oncethisgraphbasedrepresentationwasformalized,analgorithmwasdevelopedthatmakes

useof this representation.Thealgorithm,by traversing thetreeandvisitingrelevantnodes,executedtheframepropagationequations,thusallowingfortheinverseJacobiantobebuilt.

ThisalgorithmwasthenimplementedinMatlab,andallthefilescreatedweresavedtoaMatlab

toolbox. A collection of several cluster definitions were then developed to test the algorithm. Thisincluded defining the formal set of kinematic equations, depicting them as tree structures, anddeterminingthehomogeneoustransformmatricesneeded.

TheresultistheinitialversionofaMatlabToolboxthatsuccessfullyautomatesthecomputation

oftheinverseJacobianMatrixforaclusterofrobots.TheresultswereverifiedbycomparingtheJacobianMatrixthatwasobtainedtooneobtained

byaderivationbasedtechnique.Allresultsweremathematicallyequal,althoughcertainelementsinthematrixweresometimesobtainedinamorecompactform.

Bycompletingthisresearch,asystematicwayfordeterminingtheinverseJacobianofacluster

ofrobotscannowbedonequicklyandsystematically.Thisallowsustobuildcontrollersformoreclusterconfigurationsrapidly.Doingthiscanallowforagroupofrobotstobetestedinsimulationindifferentgeometric descriptions, and may help a researcher decide if he wants a more or less centralizeddescriptionforthegroupofrobotsbeingcontrolled.

ItalsoexpandsonpreviousworkinclustercontrolonJacobiananalysis,suchaspoorcondition

numberavoidance.Rapiddevelopmentofthesematricesmeansthattheycanbestudiedmorereadily.

37

6.2FutureWork

Currentlythistoolboxonlyworksasaproofofconceptand isnotoptimizedforrun-time.TheprogramcanbeoptimizedinMatlab,ortranscribedintoanotherlanguagesuchaspythonforpossiblyfasterruntimes.

Further optimization in computational efficiency can be achieved by modification of the

algorithm.Forexample,theangularvelocitypropagationsubroutineisalreadycomputedaspartofthelinear velocity propagation subroutine. Capturing that information would prevent the need to re-computeit.

Thetoolboxcanalsobedevelopedfurthertoprovidemorevaluetofellowresearchers.FurtherfilescanbewrittenthatwouldautomaticallypopulatethecomputedinverseJacobianintoacontrollerinaSimulinkmodel. ItcanalsobemodifiedtoreplacethesymbolswithactualvaluesandcomputeanumericalvaluefortheinverseJacobianbasedontheanalyticalmodel.

Furtherworkcouldalsoinvolvecreatingatooltohelpresearchescreateaphysicalsketchofthe

multirobot system, and use that information to automatically create a cluster tree, as well as todeterminetheHTM’sforeachnode.

The forward Jacobian is also still computed manually. Finding a faster technique for quicker

computationof the forward Jacobianwoulddirectly compliment thework thathas beendone in thisresearch. Asidefromgraphtheory,othermathematicaltechniquescanbeexplored.Forexample,utilizingcertain features of skew symmetric matrices can also be used to increase efficiency. Forcing ourmatricestobeskewsymmetricmayonlyworkoncertainclusterswithrestrictedtypesofmovements.Thismaybebeneficialwhensmallcomputationsizerequiredatthecostofsomelossinperformance. Other techniques suchasquaternionspromiseeven faster computational efficiency, althoughthismayrequirethinkingofclustersinnewwaysandformalizingawaytothinkofthem.

38

References

[1]Farinelli,Alessandro,LucaIocchi,andDanieleNardi."Multirobotsystems:aclassificationfocusedoncoordination."IEEETransactionsonSystems,Man,andCybernetics,PartB(Cybernetics)34.5(2004):2015-2028.

[2]Kitts,ChristopherA.,andIgnacioMas."Clusterspacespecificationandcontrolofmobilemultirobotsystems."IEEE/ASMETransactionsonMechatronics14.2(2009):207-218.

[3]Parker,LynneE."Currentstateoftheartindistributedautonomousmobilerobotics."DistributedAutonomousRoboticSystems4.SpringerJapan,2000.3-12.

[4]Vincent,Regis,DieterFox,JonathanKo,KurtKonolige,BensonLimketkai,BenoitMorisset,CharlesOrtiz,DirkSchulz,andBenjaminStewart."Distributedmultirobotexploration,mapping,andtaskallocation."AnnalsofMathematicsandArtificialIntelligence52.2-4(2008):229-255.

[5]Anderson,StuartO.,ReidSimmons,andDaniGolberg."Maintaininglineofsightcommunicationsnetworksbetweenplanetaryrovers."IntelligentRobotsandSystems,2003.(IROS2003).Proceedings.2003IEEE/RSJInternationalConferenceon.Vol.3.IEEE,2003:2266-2272.

[6]Ren,Wei,andRandalBeard."Decentralizedschemeforspacecraftformationflyingviathevirtualstructureapproach."JournalofGuidance,Control,andDynamics27.1(2004):73-82.

[7]Kitts,Christopher,ThomasAdamek,MichaelVlahos,AnneMahacek,KillianPoore,JorgeGuerra,MichaelNeumann,MatthewChin,andMikeRasay."Anunderwaterrobotictestbedformulti-vehiclecontrol."2014IEEE/OESAutonomousUnderwaterVehicles(AUV).IEEE,2014:1-8.

[8]Michael,Nathan,DanielMellinger,QuentinLindsey,andVijayKumar.."Thegraspmultiplemicro-uavtestbed."IEEERobotics&AutomationMagazine17.3(2010):56-65.

[9]Mariottini,GianLuca,FabioMorbidi,DomenicoPrattichizzo,NicholasVanderValk,NathanMichael,GeorgePappas,andKostasDaniilidis."Vision-basedlocalizationforleader–followerformationcontrol."IEEETransactionsonRobotics25.6(2009):1431-1438.

[10]Smith,Brian,AyannaHoward,John-MichaelMcNew,JiuguangWang,andMagnusEgerstedt."Multi-robotdeploymentandcoordinationwithembeddedgraphgrammars."AutonomousRobots26.1(2009):79-98.

[11]Das,AveekK.,RafaelFierro,VijayKumar,JamesP.Ostrowski,JohnSpletzer,andCamilloJ.Taylor."Avision-basedformationcontrolframework."IEEEtransactionsonroboticsandautomation18.5(2002):813-825.

[12]Vail,Douglas,andManuelaVeloso."Multi-robotdynamicroleassignmentandcoordinationthroughsharedpotentialfields."Multi-robotsystems(2003):87-98.

[13]Schneider,FrankE.,andDennisWildermuth."Apotentialfieldbasedapproachtomultirobotformationnavigation."Robotics,IntelligentSystemsandSignalProcessing,2003.Proceedings.2003IEEEInternationalConferenceon.Vol.1.IEEE,2003:680-685.

[14]Kitts,ChristopherA.,KyleStanhouse,andPiyaChindaphorn."Clusterspacecollisionavoidanceformobiletwo-robotsystems."2009IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems.IEEE,2009:1941-1948.

39

[15]Leonard,NaomiEhrich,andEdwardFiorelli."Virtualleaders,artificialpotentialsandcoordinatedcontrolofgroups."DecisionandControl,2001.Proceedingsofthe40thIEEEConferenceon.Vol.3.IEEE,2001:2968-2973.

[16]Xie,Xiao-Feng,StephenF.Smith,andGregoryJ.Barlow."Schedule-DrivenCoordinationforReal-TimeTrafficNetworkControl."ICAPS.2012:323-331

[17]Olfati-Saber,Reza."Flockingformulti-agentdynamicsystems:Algorithmsandtheory."IEEETransactionsonautomaticcontrol51.3(2006):401-420.

[18]Mas,Ignacio,andChristopherA.Kitts."DynamicControlofMobileMultirobotSystems:TheClusterSpaceFormulation."IEEEAccess2(2014):558-570.

[19]Ferber,Jacques.Multi-agentsystems:anintroductiontodistributedartificialintelligence.Vol.1.Reading:Addison-Wesley,1999.

[20]Craig,JohnJ.Introductiontorobotics:mechanicsandcontrol.Vol.3.UpperSaddleRiver:PearsonPrenticeHall,2005.

[21]Tokarz,Krzysztof,andSlawoszKieltyka."Geometricapproachtoinversekinematicsforarmmanipulator."Proceedingsofthe14thWSEASinternationalconferenceonSystems(ICS'10):partofthe14thWSEASCSCCmulticonference.Vol.2.2010:682-687.

[22]Lilly,KathrynW.,andDavidE.Orin."Alternateformulationsforthemanipulatorinertiamatrix."TheInternationalJournalofRoboticsResearch10.1(1991):64-74.

[23]Wang,Xuguang,andJeanPierreVerriest."Ageometricalgorithmtopredictthearmreachpostureforcomputer-aidedergonomicevaluation."Thejournalofvisualizationandcomputeranimation9.1(1998):33-47.

[24]Raghavan,Madhusudan,andBernardRoth."Kinematicanalysisofthe6Rmanipulatorofgeneralgeometry."Proc.FifthInt.SymposiumonRoboticsResearch,MITPress,Cambridge.1990:1-28.

[25]Corke,Peter.Robotics,visionandcontrol:fundamentalalgorithmsinMATLAB.Vol.73.Springer,2011.

[26]Tsai,Lung-Wen.Robotanalysis:themechanicsofserialandparallelmanipulators.JohnWiley&Sons,1999.

[27]Weisstein,EricW."Jacobian."FromMathWorld--AWolframWebResource.http://mathworld.wolfram.com/Jacobian.html

[28]Boullion,ThomasL.,andPatrickL.Odell."Generalizedinversematrices."(1971).

[29]Orin,DavidE.,andWilliamW.Schrader."EfficientcomputationoftheJacobianforrobotmanipulators."TheInternationalJournalofRoboticsResearch3.4(1984):66-75.

[30]Mas,Ignacio.Clusterspaceframeworkformulti-robotformationcontrol.Diss.SantaClaraUniversity,2011.

40

AppendixA–ExamplesusedintheTest_BedFolder

Example1:AClusterofTwoRobots PhysicalDiagram:

Figure31:APhysicalviewofaclusterof2robotsforexample1ForwardKinematics:

𝑥j𝑦j𝜙+𝜙,𝜃j𝑙

=

(𝑥++𝑥,)2

(𝑦++𝑦,)2

𝜃+ − 𝜃j𝜃, − 𝜃j

𝑎𝑡𝑎𝑛2 (𝑦, − 𝑦+ , 𝑥, − 𝑥+ )12

𝑦+ − 𝑦, , + 𝑥, − 𝑥+ ,

TreeStructure:

Figure32:AGraphrepresentingaclusterof2robotsforexample1

41

TransformationMatrices:

𝑇mD = cos(𝜃j) −sin(𝜃j) 𝑥jsin(𝜃j) cos(𝜃j) 𝑦j0 0 1

𝑇+m = cos(𝜙+) −sin(𝜙+) 𝑙sin(𝜙+) cos(𝜙+) 0

0 0 1

𝑇,m = cos(𝜙,) −sin(𝜙,) −𝑙sin(𝜙,) cos(𝜙,) 0

0 0 1

𝑇+D = 𝑇mD ∗ 𝑇+m = cos(𝜃j + 𝜙+) −sin(𝜃j + 𝜙+) 𝑙 ∗ cos 𝜃j + 𝑥jsin(𝜃j + 𝜙+) cos(𝜃j + 𝜙+) 𝑙 ∗ sin 𝜃j + 𝑦j

0 0 1

𝑇,D = 𝑇mD ∗ 𝑇,m = cos(𝜃j + 𝜙,) −sin(𝜃j + 𝜙,) −𝑙 ∗ cos 𝜃j + 𝑥j)sin(𝜃j + 𝜙,) cos(𝜃j + 𝜙,) −𝑙 ∗ sin 𝜃j + 𝑦j

0 0 1

InverseKinematics

𝑥+𝑦+𝜃+𝑥,𝑦,𝜃,

=

𝑙 ∗ cos 𝜃j + 𝑥j𝑙 ∗ sin 𝜃j + 𝑦j

𝜃j + 𝜙+−𝑙 ∗ cos 𝜃j + 𝑥j−𝑙 ∗ sin 𝜃j + 𝑦j

𝜃j + 𝜙,

42

Example2:TwoClustersofTwoRobotsinaLeaderFollowerFormation

PhysicalDiagram

Figure33:PhysicalViewof2clustersof2robotseachinaleaderfollowerconfiguration,wherecluster{C1}is

leaderand{C2}isfollower.ForwardKinematics

𝑥j𝑦�𝜙+𝜙,𝜙l𝜙�𝜃j+𝑙𝑚𝑛𝜃j,𝜙j

=

(𝑥++𝑥,)2

(𝑦++𝑦,)2

𝜃+ − 𝜃j+𝜃, − 𝜃j+

𝜃l − 𝜃j+ − 𝜙j𝜃� − 𝜃j+ − 𝜙j

𝑎𝑡𝑎𝑛2 (𝑦, − 𝑦+ , 𝑥, − 𝑥+ )12

𝑦+ − 𝑦, , + 𝑥, − 𝑥+ ,

(𝑥++𝑥,)

2−(𝑥l+𝑥�)

2

,

+(𝑦++𝑦,)

2−(𝑦l+𝑦�)

2

,

12

𝑦� − 𝑦l , + 𝑥� − 𝑥l ,

acos(𝑦l − 𝑦m+ , + 𝑥l − 𝑥m+ , − 4𝑙, − 𝑚,))

4𝑙𝑛− 𝜙j + 𝜋

acos(𝑦j, − 𝑦+ , + 𝑥j, − 𝑥+ , − 4𝑙, − 𝑚,))

4𝑙𝑚

43

TreeDiagram

Figure34:AGraphrepresenting2clustersof2robotseachinaleaderfollowerconfiguration,wherecluster{C1}isleaderand{C2}isfollower.

TransformationMatrices:

𝑇m+D =

cos(𝜃j+) −sin(𝜃j+) 𝑥jsin(𝜃j+) cos(𝜃j+) 𝑦j

0 0 1

𝑇+m+ = cos(𝜙+) −sin(𝜙+) 𝑙sin(𝜙+) cos(𝜙+) 0

0 0 1

𝑇,m+ = 𝑐𝑜𝑠(𝜙,) −𝑠𝑖𝑛(𝜙,) −𝑙𝑠𝑖𝑛(𝜙,) 𝑐𝑜𝑠(𝜙,) 0

0 0 1

𝑇m,m+ =

𝑐𝑜𝑠(𝜃j,) −𝑠𝑖𝑛(𝜃j,) 𝑚 ∗ cos(𝜙j)𝑠𝑖𝑛(𝜃j,) 𝑐𝑜𝑠(𝜃j,) 𝑚 ∗ sin(𝜙j)

0 0 1

𝑇lm, = 𝑐𝑜𝑠(𝜙l) −𝑠𝑖𝑛(𝜙l) 𝑛𝑠𝑖𝑛(𝜙l) 𝑐𝑜𝑠(𝜙l) 0

0 0 1

𝑇�m� = 𝑐𝑜𝑠(𝜙�) −𝑠𝑖𝑛(𝜙�) −𝑛𝑠𝑖𝑛(𝜙�) 𝑐𝑜𝑠(𝜙�) 0

0 0 1

44

InverseKinematics:


=

𝑙 ∗ cos 𝜃j+ + 𝜙j + 𝑥j𝑙 ∗ sin 𝜃j+ + 𝜙j + 𝑦j

𝜃j+ + 𝜙+−𝑙 ∗ cos 𝜃j+ + 𝜙j + 𝑥j−𝑙 ∗ sin 𝜃j+ + 𝜙j + 𝑦j

𝜃j+ + 𝜙,𝑚 ∗ cos 𝜙j + 𝜃j+ + 𝑛 ∗ cos 𝜃j, + 𝜃j+ + 𝑥j𝑚 ∗ cos 𝜙j + 𝜃j+ + 𝑛 ∗ sin 𝜃j, + 𝜃j+ + 𝑦j

𝜃j + 𝜙j + 𝜙l𝑚 ∗ cos 𝜙j + 𝜃j+ − 𝑛 ∗ cos 𝜃j, + 𝜃j+ + 𝑥j𝑚 ∗ cos 𝜙j + 𝜃j+ − 𝑛 ∗ sin 𝜃j, + 𝜃j+ + 𝑦j

𝜃j + 𝜙j + 𝜙�

Example3:AClusteroftwo2-robotclusters

PhysicalDiagram


{C}

45

ForwardKinematics

𝑥j𝑦�𝜙+𝜙,𝜙l𝜙�𝜃j𝑙𝑚𝑛𝜃j+𝜃j,

=

(𝑥++𝑥,+𝑥l+𝑥�)4

(𝑦++𝑦, + 𝑦l+𝑦�)4

𝜃+ − 𝜃j+ − 𝜃m𝜃, − 𝜃j+ − 𝜃m𝜃l − 𝜃j, − 𝜃j𝜃� − 𝜃j, − 𝜃j

𝑎𝑡𝑎𝑛2 (𝑦� − 𝑦j+ , 𝑥� −(𝑥++𝑥,)

2)

(𝑦++𝑦,)2

− 𝑦�,

+(𝑥++𝑥,)

2− 𝑥�

,

(𝑦++𝑦,)2

− 𝑦+,

+(𝑥++𝑥,)

2− 𝑥+

,

(𝑦l+𝑦�)

2− 𝑦+

,

+(𝑥l+𝑥�)

2− 𝑥+

,

acos(𝑦� − 𝑦+ , + 𝑥� − 𝑥+ , − 4𝑙, − 𝑚,))

4𝑙𝑚

acos(𝑦� − 𝑦l , + 𝑥� − 𝑥l , − 4𝑙, − 𝑛,))

4𝑙𝑛

TreeDiagram

Figure36:AGraphrepresentingaclusterofclusters.Herecluster{C1}and{C2}formasinglecluster{C}.

46

TransformationMatrices


𝑇m+m =

cos(𝜃j+) −sin(𝜃j+) 𝑙sin(𝜃j+) cos(𝜃j+) 0

0 0 1

𝑇m,m =

cos(𝜃j,) −sin(𝜃j,) −𝑙sin(𝜃j,) cos(𝜃j,) 0

0 0 1

𝑇�+m+ =

cos(𝜙+) −sin(𝜙+) 𝑚sin(𝜙+) cos(𝜙+) 0

0 0 1

𝑇�,m+ =

cos(𝜙,) −sin(𝜙,) −𝑚sin(𝜙,) cos(𝜙,) 0

0 0 1

𝑇�lm, =

cos(𝜙l) −sin(𝜙l) 𝑛sin(𝜙l) cos(𝜙l) 0

0 0 1

𝑇��m, =

cos(𝜙�) −sin(𝜙�) −𝑛sin(𝜙�) cos(𝜙�) 0

0 0 1

InverseKinematics


=

𝑚 ∗ cos 𝜃j+ + 𝜃j + 𝑙 ∗ cos 𝜃j + 𝑥j𝑚 ∗ sin 𝜃j+ + 𝜃j + 𝑙 ∗ sin 𝜃j + 𝑦j

𝜃j + 𝜃j+ + 𝜙+−𝑚 ∗ cos 𝜃j+ + 𝜃j + 𝑙 ∗ cos 𝜃j + 𝑥j−𝑚 ∗ sin 𝜃j+ + 𝜃j + 𝑙 ∗ sin 𝜃j + 𝑦j

𝜃j + 𝜃j+ + 𝜙,𝑛 ∗ cos 𝜃j, + 𝜃j − 𝑙 ∗ cos 𝜃j + 𝑥j𝑛 ∗ sin 𝜃j, + 𝜃j − 𝑙 ∗ sin 𝜃j + 𝑦j

𝜃j + 𝜃j, + 𝜙l−𝑛 ∗ cos 𝜃j, + 𝜃j − 𝑙 ∗ cos 𝜃j + 𝑥j−𝑛 ∗ cos 𝜃j, + 𝜃j − 𝑙 ∗ cos 𝜃j + 𝑥j

𝜃j + 𝜃j, + 𝜙�

47

Example4:ATwoRobotClusterwiththeClusterframePositionDeterminedbyOneRobot

PhysicalView

Figure37:ATworobotclustersharingasingleclusterframe.TheClusterframeislocatedontopofRobot1.However,robot1isfreetohaveadifferentorientationthantheclusterframe.

ForwardKinematics

𝑥j𝑦j𝜙+𝜙,𝜃j𝑟

=

𝑥+𝑦+


𝑎𝑡𝑎𝑛2 (𝑦, − 𝑦+ , 𝑥, − 𝑥+ )𝑦+ − 𝑦, , + 𝑥, − 𝑥+ ,

TreeDiagram

Figure38:TreeDiagramofaTworobotclustersharingasingleclusterframe.

TheClusterframeislocatedontopofRobot1.

48



𝑇+m = cos(𝜙+) −sin(𝜙+) 0sin(𝜙+) cos(𝜙+) 0

0 0 1

𝑇,m = cos(𝜙,) −sin(𝜙,) 𝑟sin(𝜙,) cos(𝜙,) 0

0 0 1

𝑇+D = 𝑇mD ∗ 𝑇+m = cos(𝜃j + 𝜙+) −sin(𝜃j + 𝜙+) 𝑥jsin(𝜃j + 𝜙+) cos(𝜃j + 𝜙+) 𝑦j

0 0 1

𝑇,D = 𝑇mD ∗ 𝑇,m = cos(𝜃j + 𝜙,) −sin(𝜃j + 𝜙,) r ∗ cos 𝜃j + 𝑥j)sin(𝜃j + 𝜙,) cos(𝜃j + 𝜙,) r ∗ sin 𝜃j + 𝑦j

0 0 1

InverseKinematics:


=

𝑥j𝑦j

𝜃j + 𝜙+𝑟 ∗ cos 𝜃j + 𝑥j𝑟 ∗ sin 𝜃j + 𝑦j

𝜃j + 𝜙,

49

Example5:TwoRobotFormationwithClusterframeincidenttoRobot1

PhysicalDiagram

Figure39:PhysicalviewdiagramofaTworobotclustersharingasingleclusterframe.TheClusterframeislocated

ontopofandorientedwithRobot1.ForwardKinematics

𝑥j𝑦j𝜙+𝜙,𝜃j𝑟

=

𝑥+𝑦+0

𝜃, − 𝜃j𝜃+

𝑦+ − 𝑦, , + 𝑥, − 𝑥+ ,

TreeStructure

Figure40:TreediagramofaTworobotclustersharingasingleclusterframe.TheClusterframeislocatedontopof

andorientedwithRobot1.

50



𝑇+m = 1 0 00 1 00 0 1

𝑇,m = cos(𝜙,) −sin(𝜙,) 𝑟sin(𝜙,) cos(𝜙,) 0

0 0 1

𝑇+D = 𝑇mD ∗ 𝑇+m = cos(𝜃j) −sin(𝜃j) 𝑥jsin(𝜃j) cos(𝜃j) 𝑦j0 0 1

𝑇,D = 𝑇mD ∗ 𝑇,m = cos(𝜃j + 𝜙,) −sin(𝜃j + 𝜙,) r ∗ cos 𝜃j + 𝑥jsin(𝜃j + 𝜙,) cos(𝜃j + 𝜙,) r ∗ sin 𝜃j + 𝑦j

0 0 1

InverseKinematics:


=

𝑥j𝑦j𝜃j

𝑟 ∗ cos 𝜃j + 𝑥j𝑟 ∗ sin 𝜃j + 𝑦j

𝜃j + 𝜙,

51

Example6:ATworobotclusterwitharedundantvariablePhysicalDiagram:

Figure41:AGraphrepresentingaclusterof2robots.

ForwardKinematics

𝑥j𝑦j𝜙+𝜙,𝜃j𝑚𝑛𝑙

=

(𝑥++𝑥,)2

(𝑦++𝑦,)2


𝑎𝑡𝑎𝑛2 (𝑦, − 𝑦+ , 𝑥, − 𝑥+ )𝑦+ − 𝑦j , + 𝑥j − 𝑥+ ,

𝑦j − 𝑦, , + 𝑥, − 𝑥j ,

𝑦+ − 𝑦, , + 𝑥, − 𝑥+ ,

TreeStructure

Figure42:AGraphrepresentingaclusterof2robots.

52



𝑇+m = cos(𝜙+) −sin(𝜙+) 𝑚sin(𝜙+) cos(𝜙+) 0

0 0 1

𝑇,m = cos(𝜙,) −sin(𝜙,) nsin(𝜙,) cos(𝜙,) 0

0 0 1

InverseKinematics


=

𝑚 ∗ cos 𝜃j + 𝑥j𝑚 ∗ sin 𝜃j + 𝑦j

𝜃j + 𝜙+𝑛 ∗ cos 𝜃j + 𝑥j𝑛 ∗ sin 𝜃j + 𝑦j

𝜃j + 𝜙,

Example7:TwoRobotClusterwithextendeddimensionaldegreesofFreedomPhysicalDiagram

Figure43:Asketchofaclusterof2robotsin3D.

53

ForwardKinematics

𝑥j𝑦j𝑧j𝛼+𝛽+𝛾+𝛼,𝛽,𝛾,𝜙j𝜃j𝑙

=

(𝑥++𝑥,)2

(𝑦++𝑦,)2

(𝑧++𝑧,)2𝛼�+𝜙�+𝜃�+𝛼�,𝜙�,𝜃�,

𝑎𝑡𝑎𝑛2 (𝑧, − 𝑧+ , 𝑦+ − 𝑦, , + 𝑥, − 𝑥+ ,)𝑎𝑡𝑎𝑛2 (𝑦, − 𝑦+ , 𝑥, − 𝑥+ )

𝑧+ − 𝑧, , + 𝑦+ − 𝑦, , + 𝑥, − 𝑥+ ,

TreeDiagram

Figure44:TwoRobotsin3DimensionalSpace

TransformationMatricesTheTransformationsforthissystemare:

𝑇mD =

1 0 0 𝑥j0 1 0 𝑦j0 0 1 𝑧j0 0 0 1

𝑇+m =

cos(𝛾+)cos(𝛽+) −cos(𝛾+)sin(𝛽+)𝑠𝑖𝑛(𝛼+) − sin(𝛾+)cos(𝛼+) −cos 𝛼+ sin 𝛽+ cos 𝛾+ + sin(𝛼+)sin(𝛾+) 𝑙 ∗ cos(𝜃j)cos(𝜙j)sin(𝛾+)cos(𝛽+) −sin 𝛾+ sin 𝛽+ 𝑠𝑖𝑛 𝛼+ + cos(𝛾+)cos(𝛼+) −cos 𝛼+ sin 𝛽+ sin 𝛾+ − sin(𝛼+)cos(𝛾+) 𝑙 ∗ sin(𝜃j)cos(𝜙j)−sin(𝛽+) cos(𝛽+)sin(𝛼+) cos(𝛽+)cos(𝛼+) 𝑙 ∗ sin(𝜙j)

0 0 0 1

𝑇,m =

cos(𝛾+)cos(𝛽+) −cos(𝛾+)sin(𝛽+)𝑠𝑖𝑛(𝛼+) − sin(𝛾+)cos(𝛼+) −cos 𝛼+ sin 𝛽+ cos 𝛾+ + sin(𝛼+)sin(𝛾+) −𝑙 ∗ cos(𝜃j)cos(𝜙j)sin(𝛾+)cos(𝛽+) −sin 𝛾+ sin 𝛽+ 𝑠𝑖𝑛 𝛼+ + cos(𝛾+)cos(𝛼+) −cos 𝛼+ sin 𝛽+ sin 𝛾+ − sin(𝛼+)cos(𝛾+) −𝑙 ∗ sin(𝜃j)cos(𝜙j)−sin(𝛽+) cos(𝛽+)sin(𝛼+) cos(𝛽+)cos(𝛼+) −𝑙 ∗ sin(𝜙j)

0 0 0 1

54

InverseKinematics

𝑥+𝑦+𝑧+𝜃�+𝜙�+𝛼�+𝑥,𝑦,𝑧,𝜃�,𝜙�,𝛼�,

=

𝑥j + l ∗ cos(𝜃j)cos(𝜙j)𝑦j + l ∗ sin(𝜙j)cos(𝜃j)

𝑧j + 𝑙 ∗ sin(𝜙j)𝛾+𝛽+𝛼+

𝑥j − l ∗ cos(𝜃j)cos(𝜙j)𝑦j − l ∗ sin(𝜙j)cos(𝜃j)

𝑧j − 𝑙 ∗ sin(𝜙j)𝛾,𝛽,𝛼,

Example8:ThreeRobotClusterwithClusterFrameatCentroidLocationPhysicalView

Figure45:PhysicalviewofathreerobotClusterinformation

55

ForwardKinematics

𝑥j𝑦j𝜃j𝑙𝑚𝜙j𝜙+𝜙,𝜙l𝑟𝑥𝛼𝑎𝑏𝑅

=

(𝑥++𝑥, + 𝑥l)

3

(𝑦++𝑦,+𝑦l)

3𝑎𝑡𝑎𝑛2 (𝑦j − 𝑦+ , 𝑥j − 𝑥+ ) + π/2

(𝑦, − 𝑦+ , + 𝑥, − 𝑥+ ,)(𝑦l − 𝑦+ , + 𝑥l − 𝑥+ ,)

acos((𝑙, + 𝑚, − (𝑦, − 𝑦l , − 𝑥, − 𝑥l ,)

2𝑙𝑚)

𝜃+ − 𝜃j𝜃, − 𝜃j𝜃l − 𝜃j

𝑙, + 𝑚, − 2𝑙𝑚𝑐𝑜𝑠(𝜙j)

𝑙, +𝑟4

,− 2𝑙𝑟𝑐𝑜𝑠(𝛼)

acos 𝑟, + 𝑙, − 𝑚,

2𝑟𝑙acos 𝑙, + 𝑥, − 𝑟

4,

2𝑙𝑥𝜙j − 𝑎2𝑥3

TreeStructure

Figure46:ClustertreeofathreerobotCluster.

56



𝑇+m = cos(𝜙+) −sin(𝜙+) 0sin(𝜙+) cos(𝜙+) 𝑅

0 0 1

𝑇,m = cos(𝜙,) −sin(𝜙,) −𝑙 ∗ sin(𝑎)sin(𝜙,) cos(𝜙,) R − 𝑙 ∗ cos(𝑎)

0 0 1

𝑇lm = cos(𝜙l) −sin(𝜙l) m ∗ sin(𝑏)sin(𝜙l) cos(𝜙l) R − m ∗ cos(𝑏)

0 0 1

InverseKinematics

𝑥+𝑦+𝜃+𝑥,𝑦,𝜃,𝑥l𝑦l𝜃l

=

−𝑅 ∗ sin 𝜃j + 𝑥j𝑅 ∗ cos 𝜃j + 𝑦j

𝜃j + 𝜙+−𝑙 ∗ 𝑠𝑖𝑛(𝑎 − 𝜃j) − 𝑅 ∗ sin 𝜃j + 𝑥j−𝑙 ∗ 𝑐𝑜𝑠 𝑎 − 𝜃j + 𝑅 ∗ cos 𝜃j + 𝑥j

𝜃j + 𝜙,𝑚 ∗ 𝑠𝑖𝑛(𝑏 − 𝜃j) − 𝑅 ∗ sin 𝜃j + 𝑥j−𝑚 ∗ 𝑠𝑖𝑛 𝑏 − 𝜃j + 𝑅 ∗ cos 𝜃j + 𝑥j

𝜃j + 𝜙l

57

Example9:ThreeRobotCluster,ClusterFramebetweenR1andR2,R3followsClusterFrame

PhysicalDiagram

Figure47:PhysicalSketchofathreerobotCluster.Custerframeisdefinedbyframes1and2,whilethethirdframe

followsthatclusterframe.

ForwardKinematics

𝑥j𝑦j𝜙+𝜙,𝜃j𝑙𝑚𝜙j𝜙l

=

(𝑥++𝑥,)2

(𝑦++𝑦,)2


𝑎𝑡𝑎𝑛2 (𝑦, − 𝑦+ , 𝑥, − 𝑥+ )𝑦+ − 𝑦j , + 𝑥+ − 𝑥j ,

𝑦l − 𝑦j , + 𝑥l − 𝑥j ,

acos(𝑦l − 𝑦+ , + 𝑥l − 𝑥+ , − 𝑙, − 𝑚,))

2𝑙𝑚𝜃l −𝜃j

TreeStructure

Figure48:ClustertreeofathreerobotCluster.Custerframeisdefinedbyframes1and2,whilethethirdframe

followsthatclusterframe.

58



𝑇+m = cos(𝜙+) −sin(𝜙+) 𝑙sin(𝜙+) cos(𝜙+) 0

0 0 1

𝑇,m = cos(𝜙,) −sin(𝜙,) −𝑙sin(𝜙,) cos(𝜙,) 0

0 0 1

𝑇lm = cos(𝜙l) −sin(𝜙l) m ∗ cos(𝜙<)sin(𝜙l) cos(𝜙l) m ∗ sin(𝜙<)

0 0 1

InverseKinematics


=



𝜃j + 𝜙,m ∗ cos 𝜃j+𝜙j + 𝑥jm ∗ sin 𝜃j+𝜙j + 𝑦j

𝜃j + 𝜙l

59

Example10:AThreeRobotClusterinadifferentdefinition

PhysicalDrawing:

Figure69:PhysicalViewofathreerobotCluster.Custerframeisdefinedbyframes1and2,whilethethirdframe

followsRobot1

ForwardKinematics:

𝑥j𝑦j𝜙+𝜙,𝜃j𝑙𝑚𝜙j𝜙l

=

(𝑥++𝑥,)2

(𝑦++𝑦,)2


𝑎𝑡𝑎𝑛2 (𝑦, − 𝑦+ , 𝑥, − 𝑥+ )12

𝑦, − 𝑦+ , + 𝑥, − 𝑥+ ,

𝑦+ − 𝑦l , + 𝑥+ − 𝑥l ,

acos(𝑦l − 𝑦j , + 𝑥l − 𝑥j , − 𝑙, − 𝑚,

2𝑙𝑚)

𝜃l − 𝜃j

60

GraphTree

Figure50:ClustertreeofathreerobotCluster.Custerframeisdefinedbyframes1and2,whilethethirdframe

followsRobot1



𝑇m+m =

1 0 𝑙0 1 00 0 1

𝑇�+m+ =

cos(𝜙+) −sin(𝜙+) 0sin(𝜙+) cos(𝜙+) 0

0 0 1

𝑇�,m =

cos(𝜙,) −sin(𝜙,) −𝑙sin(𝜙,) cos(𝜙,) 0

0 0 1

𝑇�lm+ =

cos(𝜙l) −sin(𝜙l) m ∗ cos(𝜙j)sin(𝜙l) cos(𝜙l) m ∗ sin(𝜙j)

0 0 1

𝑇�+D = 𝑇mD ∗ 𝑇m+

m ∗ 𝑇�+m+ =

cos(𝜃j + 𝜙+) −sin(𝜃j + 𝜙+) 𝑙 ∗ cos 𝜃j + 𝑥jsin(𝜃j + 𝜙+) cos(𝜃j + 𝜙+) 𝑙 ∗ sin 𝜃j + 𝑦j

0 0 1

𝑇�,D = 𝑇mD ∗ 𝑇�,

m = cos(𝜃j + 𝜙,) −sin(𝜃j + 𝜙,) −𝑙 ∗ cos 𝜃j + 𝑥jsin(𝜃j + 𝜙,) cos(𝜃j + 𝜙,) −𝑙 ∗ sin 𝜃j + 𝑦j

0 0 1

𝑇�lD = 𝑇mD ∗ 𝑇m+

m ∗ 𝑇�lm+ =

cos(𝜃j + +𝜙l) −sin(𝜃j + 𝜙l) m ∗ cos 𝜃j+𝜙j + 𝑙 ∗ cos 𝜃j + 𝑥jsin(𝜃j + 𝜙l) cos(𝜃j + 𝜙l) m ∗ sin 𝜃j+𝜙j + 𝑙 ∗ sin 𝜃j + 𝑦j

0 0 1

61

InverseKinematics


=



𝜃j + 𝜙,m ∗ cos 𝜃j+𝜙j + 𝑙 ∗ cos 𝜃j + 𝑥jm ∗ sin 𝜃j+𝜙j + 𝑙 ∗ sin 𝜃j + 𝑦j

𝜃j + 𝜙l

62

AppendixB–CodeusedinToolbox B.1TreeDataStructureFolder TreedataStructureasMATLABClassByJean-YvesTinevez13Mar2012(Update13Feb2016)Aper-valueclassthatimplementsagenerictreedatastructurehttps://www.mathworks.com/matlabcentral/fileexchange/35623-tree-data-structure-as-a-matlab-classDescription:

Atreeisahierarchicaldatastructurewhereeverynodehasexactlyoneparent(expecttheroot)andnoorseveralchildren.Alongwiththisrelationalstructure,eachnodecanstoreanykindofdata.ThisclassimplementsitusingplainMATLABsyntaxandarrays.Mostusefulmethodsareimplemented,usingoverloadingofMATLABfunctionsfortreeobjects.

CJWNotes:

Itisaratherintuitiveclasstouse.Forexample,youcantypefind((a.^2.*b)>(c-5)&d),witha,b,canddbeingtreeobjects.Doingthiswillcomputeanactualsolutionforyou.Aratherlongtutorialisincludedtowalkyouthroughthesetreestructures.Italsoshowsseveralotherfeaturesthatwerenotutilizedinthisresearch.Thetutorialcanbefoundhere:http://tinevez.github.io/matlab-tree/

B.2Cluster_BuilderFolder

B.2.1 cluster_tree.m

function [the_cluster, node_count, cluster_nodes,cc,cp ] = cluster_tree( the_cluster, clustercell,node_count,cluster_nodes,cc,cp ) y = the_cluster.get(cp); if cp>1 y=num2str(cell2mat(y)); end fprintf('How many cluster children does %s have \n', y ) prompt5 = input(' : '); for i=(cc):(prompt5+cc-1) the_cluster = the_cluster.addnode(cp, clustercell(i)); node_count = node_count+1; cluster_nodes = [cluster_nodes, node_count]; end cc= cc+prompt5; cp = cp+1; end

63

B.2.2 clusterbuilder2.m

function [ obj_out ] = clusterbuilder2( num_robots,num_clusters ) % CLUSTERBUILDER2 - create your own cluster tree %num_robots = number of robots in the cluster %num_clusters = number of clusters to consider (main cluster + subclusters) % Check number of inputs. if nargin > 2t error('clusterbuilder2 requires at most 2 inputs'); elseif nargin < 2 error('clusterbuilder 2 requires at least 3 inputs') end robotnamer % Names the Robots R1, R2, R3, etc.... clusternamer % Names the Cluster Frames C, C1, C2, C3, etc... variablecreator % Asks user for the cluster variables to be used symbolicbank % Loads from this variable bank treecreator % Arranges the tree into an Arborescence htmdefiner % Arranges the HTM's msot % Matlab struct with all information of the tree end

B.2.3 clusternamer.m

% Creating a cell with the cluster frame names. Saves them symbollicaly. for ii=1:(num_clusters+1) clusterarray(ii)= 'C'; end clustercellinit = num2cell(clusterarray); clustercell = genvarname(clustercellinit); clustercell = clustercell(2:numel(clustercell)); for ii=1:(num_clusters) clustersyms(ii)= sym(clustercell(ii)); end

64

B.2.4 htmdefiner.m

% Defines the homogenous transforms for each edge in the arborescence t = the_cluster; namt=t; fprintf('The Cluster tree we created is visible above. \n') prompt6 = input('Enter the 3x3 or 4x4 T matrix from cluster frame to Ground Frame \n >> '); tst = prompt6; sz = length(prompt6); if sz <3 error('Not a Valid Homogeneous transform matrix'); elseif sz > 4 error('Not a Valid Homogeneous transform matrix'); else end t=t.set(1,prompt6); for i=2:node_count var1 = t.get(i); var1=num2str(cell2mat(var1)); var2 = t.getparent(i); if var2>1 var2 = namt.get(var2); var2=num2str(cell2mat(var2)); else var2 = namt.get(var2); end fprintf('What is the Transformation from %s to %s \n >>', var1, var2) subt = input(' : '); t=t.set(i,subt); end

B.2.5 msot.m

% Creates a structure type data type containing all the tree information obj_out.robotnodes=robot_nodes; obj_out.clusternodes=cluster_nodes; obj_out.htm_tree=t; obj_out.txt_tree=the_cluster; obj_out.var=variablesyms;

65

B.2.6 robot_tree.m

function [the_cluster, node_count, robot_nodes,rc] = robot_tree(the_cluster,robotcell,node_count,robot_nodes, rc, cp) y = the_cluster.get(cp); if cp>1 y=num2str(cell2mat(y)); end fprintf('How many robot children does %s have? \n', y ) prompt4 = input(' : '); for i=(1+rc):(prompt4+rc) the_cluster = the_cluster.addnode(cp, robotcell(i)); node_count = node_count+1; robot_nodes = [robot_nodes, node_count]; rc = rc+1; end end

B.2.7 robotnamer.m

% This function names all the robots as R1, R2, R3 etc..... for ii=1:(num_robots+1) robotarray(ii)= 'R'; end robotcellinit = num2cell(robotarray); robotcell = genvarname(robotcellinit); robotcell = robotcell(2:numel(robotcell)); for ii=1:(num_robots) robotsyms(ii)= sym(robotcell(ii)); end clear robotcellinit; clear robotarray;

66

B.2.8 symbolicbank.m

% Lists some variables that can be used as cluster variables % If your cluster variable is not listed, please add it here syms a b c d e f g h i j k l m n o p q r s t u v w x y z; syms A B C D E F G H I J K L M N O P Q R S T U V Q X Y Z; syms alpha1 alpha2 alpha3 alpha4 alpha5 alpha6 alpha7 alpha8 alpha9; syms beta1 beta2 beta3 beta4 beta5 beta6 beta7 beta8 beta9 beta10; syms gamma1 gamma2 gamm3 gamma4 gamm5 gamma6 gamma7 gamma8 gamma9 gamma10; syms theta1 theta2 theta3 theta4 theta5 theta6 theta7 theta8 theta9; syms xc yc zc xc1 yc1 zc1 xc2 yc2 zc2 ; syms phic phic1 phic2 phic3 phic4 phic5 phic6 phic7 phic8 phic9 phic10; syms phic11 phic12 phic13 phic14 phic15 phic16 phic17 phic18 phic19 phic20; syms phi1 phi2 phi3 phi4 phi5 phi6 phi7 phi8 phi9 phi10 phi11 phi12 phi13; syms phi14 phi15 phi16 phi17 phi18 phi19 phi20 phi21 phi22 phi23 phi24; syms tc tc1 tc2 tc3 tc4 tc5 tc6 tc7 tc8 tc9 tc10 tc11 tc12 tc13 tc14 tc15; syms tc16 tc17 tc18 tc19 tc20; syms aa bb cc dd ee ff gg hh ii jj kk ll mm nn oo pp qq rr ss tt uu vv; syms ww xx yy zz; syms AA BB CC DD EE FF GG HH II JJ KK LL MM NN OO PP QQ RR SS TT UU VV WW; syms XX YY ZZ; syms d1 d2 d3 d4 d5 d6 d7 d8 d9 d10 d11 d12 d13 d14 d15 d16

B.2.9 treecreator.m

%This step allows us to create a tree % Let us create the base node, C, and start defining downward from there. display('Let us now start creating our cluster tree') the_cluster = tree('C'); node_count=1; robot_nodes= []; cc = 1; % Number of clusters we have assigned so far rc = 0; %Robot counter cp = 1 ;% Current Cluster position cluster_nodes = 1; while (numel(cluster_nodes)<num_clusters) || (numel(robot_nodes)<num_robots) [the_cluster, node_count, robot_nodes,rc] = ... robot_tree( the_cluster,robotcell,node_count,robot_nodes, rc,cp); [the_cluster, node_count, cluster_nodes, cc,cp ] = ... cluster_tree( the_cluster, clustercell,node_count,cluster_nodes, cc,cp); end fprintf(' \n \n \n \n '); disp(the_cluster.tostring) fprintf(' \n \n \n \n ');

67

B.2.10 variablecreator.m

% This program accepts the users input about which variables to use display('What Cluster variables will you use?'); display('Please state your variables with no commas') prompt3 = input('Example: a b c d xc yc phi1 \n >> ', 's'); prompt3 = strsplit(prompt3); for ii=1:numel(prompt3) variablesyms(ii)= sym(prompt3(ii)); end clear ii; assume(variablesyms>0); % Assumes variables are positive only assumeAlso(variablesyms<0.1); % Assumes variables are less than pi B.3TheInverse_KinematicsFolder

B.3.1 dirinvjac.m

function [ d_invjac, cluster ] = dirinvjac(cluster, opt1) % This will use the htm to compute the inverse kinematics, then solve for % the inverse Jacobian % If 2 is chosen in the option 1 field, then the calculation assumed a 4x4 % transformation matrix that can be optimized for a 3x3 case. if nargin > 2 error('dirinvjac requires at most 2 inputs'); elseif nargin ==2 [inv_kin, cluster] = invkin(cluster, opt1); elseif nargin ==1 [inv_kin, cluster] = invkin(cluster); elseif nargin < 1 error('dirinvjac requires at least 1 input') end d_invjac = jacobian(inv_kin,cluster.var); cluster.dirinvjac = d_invjac; end

B.3.2 invkin.m

function [ inv_kin, cluster ] = invkin(cluster, opt1) % This fxn uses the htm to get the inverse kinematics. % It automatically defaults to option0 or option 1 based on the size of the

68

% homogeneous transform matices % Option 2 is an optimized version of Option 0. If selected, the program % assumes the cluster is in the z=0 plane, and optimizes the matrix % multiplication accordingly. if nargin > 2 error('invkin requires at most 2 inputs'); elseif nargin ==1 tst = cluster.htm_tree.get(cluster.robotnodes(1)); sz = length(tst); if sz == 4 opt1 = 0; elseif sz == 3 opt1= 1; else error('This cluster has invalid transformation matrices'); end elseif nargin < 1 error('clusterbuilder 2 requires at least 1 input') end page = 1; y=5; %Initializaing some variables %Option1 == 0 - Case where the matrices are 4x4 matrices if opt1==0 for i=cluster.robotnodes y=i; x = cluster.htm_tree.get(i); y = cluster.htm_tree.getparent(i); while y>0 x = cluster.htm_tree.get(y)*x; y = cluster.htm_tree.getparent(y); end T(:,:,page)=x; page = page+1; end inv_kin = []; page = page-1; for i=1:page ry = asin(-T(3,1,i)); rz =asin(T(2,1,i)/cos(ry)); rx =asin(T(3,2,i)/cos(ry)); inv_kin = [inv_kin; T(1:3,4,i); rx;ry;rz]; inv_kin = simplify(inv_kin); end cluster.inv_kin=inv_kin; %Option1 ==1 - Case where the HTM Matrices are 3x3 case elseif opt1==1

69

for i=cluster.robotnodes y=i; x = cluster.htm_tree.get(i); y = cluster.htm_tree.getparent(i); while y>0 x = cluster.htm_tree.get(y)*x; y = cluster.htm_tree.getparent(y); end T(:,:,page)=x; page = page+1; end inv_kin = []; page = page-1; for i=1:page inv_kin = [inv_kin; T(1:2,3,i); acos(T(1,1,i))]; inv_kin = simplify(inv_kin); end cluster.inv_kin=inv_kin; %Option 2 - Solving the 4x4 case as the 3x3 case else page =1; y=5; for i=cluster.robotnodes y=i; x = cluster.htm_tree.get(i); x = x([1,2,4],[1,2,4]); y = cluster.htm_tree.getparent(i); while y>0 z= cluster.htm_tree.get(y); x = z([1,2,4],[1,2,4])*x; y = cluster.htm_tree.getparent(y); end T(:,:,page)=x; page = page+1; end inv_kin = []; page = page-1; for i=1:page inv_kin = [inv_kin; T(1:2,4,i); acos(T(1,1,i))]; inv_kin = simplify(inv_kin); end cluster.inv_kin=inv_kin; end

70

B.4TheVelocity_Propagation_TechniqueFolder

B.4.1 gettheRR3.m

function [ RR ] = gettheRR3( t, current_node) % This function computes the product of all rotations from ground to % current node. % Calculates the product of rotations for the current node. % Does Step A.2 of Section 3. Only works with 3 Degrees of Freedom (3DOF) RR = eye(2); y = t.getparent(current_node); while y>0 FTpar = t.get(y); Rpar = FTpar(1:2,1:2); RR = Rpar *RR; y= t.getparent(y); end RR = simplify(RR); end

B.4.2 gettheRR6.m

function [ RR ] = gettheRR6( t, current_node) % This function computes the product of all rotations from ground to % current node. % The same as gettheRR3, but is only called when dealing with more than % 3DOF per robot. RR = eye(3); y = t.getparent(current_node); while y>0 FTpar = t.get(y); Rpar = FTpar(1:3,1:3); RR = Rpar *RR; y= t.getparent(y); end RR = simplify(RR); end

71

B.4.3 getthev3.m

function [ ppjac ,px,py] = getthev3(current_node,t, i,variablesyms,dof) % Calculates the local velocity of one frame relative to its parent. % Performs step A.3 trans = t.get(current_node); px = trans(1,3); py =trans(2,3); ppjac((1+(dof*(i-1))),:) = jacobian(px,variablesyms); ppjac((2+(dof*(i-1))),:) = jacobian(py,variablesyms); end

B.4.4 getthev6.m

function [ ppjac ,px,py,pz] = getthev6( current_node,t, i,variablesyms,tpg) % This function gets the local velocity of each robot. See also geththev3 trans = t.get(current_node); px = trans(1,4); py =trans(2,4); pz = trans(3,4); ppjac((1+(tpg*(i-1))),:) = jacobian(px,variablesyms); ppjac((2+(tpg*(i-1))),:) = jacobian(py,variablesyms); ppjac((3+(tpg*(i-1))),:) = jacobian(pz,variablesyms); end

B.4.5 invjacfxn.m

function [ inv_jac_prop , cluster] = invjacfxn(cluster) % This function solves for the inverse Jacobian using a propagation % technique. It first checks the size of the T matrices, and executes the % algorithm. % This function, in its current form, does not have an optimization for 4x4 % matrices that are limited to the xy plane. % Furthermore, this algorithm doesn't work if a cluster variable is used to % define an actuation state. % Determining to run 3 DOF case or 6 DOF sz = length(cluster.htm_tree.get(cluster.robotnodes(1))); if sz == 4 opt1 = 0; dof = 6; elseif sz == 3 opt1= 1; dof = 3; else error('This cluster has invalid transformation matrices'); end

72

% Propagation Algorithm if opt1 == 1 for i=1:numel(cluster.robotnodes) current_node = cluster.robotnodes(i); parent_node = cluster.htm_tree.getparent(current_node); rrall = zeros(1,numel(cluster.var)); rrall= sym(rrall); rrall2=rrall; while parent_node>0 [RR] = gettheRR3( cluster.htm_tree,current_node); % Product of rotations, RR [ppjac, px, py ] = getthev3( current_node,cluster.htm_tree, i,cluster.var,dof); %local Velocity (linear component) v_rotated((1 + (dof*(i-1))),:) = ppjac((1+(dof*(i-1))),:)*(RR(1,1))+ ppjac((2+(dof*(i-1))),:)*RR(1,2); %Local Velocity v_rotated((2 + (dof*(i-1))),:) = ppjac((1+(dof*(i-1))),:)*(RR(2,1))+ ppjac((2+(dof*(i-1))),:)*RR(2,2); %Rotated [RRwxp, RRwyp] = rotatedwxp3( cluster.htm_tree, current_node, cluster.var,RR,py,px ); % Global angular Velocity rotated rrall = rrall +(v_rotated((1+(dof*(i-1))),:) +RRwxp); % Summing after each iteration rrall2 =rrall2 +(v_rotated((2+(dof*(i-1))),:) +RRwyp); % Summing after each iteration current_node = parent_node; % Updating current node parent_node = cluster.htm_tree.getparent(current_node); %Updating parent node end [wvarray] = thefinv3( cluster.htm_tree, current_node ,cluster.var ); % Adding the final Velocity Term current_node = cluster.robotnodes(i); % Recalling current node [wxarray] = thewthing3( cluster.htm_tree, current_node, cluster.var); % Product of rotations inv_jac_prop((1+(dof*(i-1))),:) = rrall + wvarray(1,:); % Assembling into one matrix inv_jac_prop((2+(dof*(i-1))),:) = rrall2 + wvarray(2,:); % Assembling into one matrix inv_jac_prop((3+(dof*(i-1))),:) = wxarray; % Assembling into one matrix cluster.propinvjac = inv_jac_prop; % Adds jacobian to cluster structure end else

73

for i=1:numel(cluster.robotnodes) current_node = cluster.robotnodes(i); parent_node = cluster.htm_tree.getparent(current_node); rrall = zeros(1,numel(cluster.var)); rrall= sym(rrall); rrall2=rrall; rrall3 = rrall2; while parent_node>0 [RR ] = gettheRR6( cluster.htm_tree,current_node); [ppjac, px, py,pz ] = getthev6( current_node,cluster.htm_tree, i,cluster.var,dof); v_rotated((1 + (dof*(i-1))),:) = ppjac((1+(dof*(i-1))),:)*(RR(1,1))+ ppjac((2+(dof*(i-1))),:)*RR(1,2) + ppjac((3+(dof*(i-1))),:)*RR(1,3); v_rotated((2 + (dof*(i-1))),:) = ppjac((1+(dof*(i-1))),:)*(RR(2,1))+ ppjac((2+(dof*(i-1))),:)*RR(2,2) + ppjac((3+(dof*(i-1))),:)*RR(2,3); v_rotated((3 + (dof*(i-1))),:) = ppjac((1+(dof*(i-1))),:)*(RR(3,1))+ ppjac((2+(dof*(i-1))),:)*RR(3,2) + ppjac((3+(dof*(i-1))),:)*RR(3,3); [RRwxp, RRwyp, RRwzp ] = rotatedwxp6( cluster.htm_tree, current_node, cluster.var,RR,py,px, pz ); rrall = rrall +(v_rotated((1+(dof*(i-1))),:) +RRwxp); rrall2 = rrall2 +(v_rotated((2+(dof*(i-1))),:) +RRwyp); rrall3 = rrall3 + (v_rotated((3+(dof*(i-1))),:) +RRwzp); current_node = parent_node; parent_node = cluster.htm_tree.getparent(current_node); end [wvarray ] = thefinv6( cluster.htm_tree, current_node ,cluster.var ); current_node = cluster.robotnodes(i); [wxarray ] = thewthing6( cluster.htm_tree, current_node, cluster.var ); inv_jac_prop((1+(dof*(i-1))),:) = rrall + wvarray(1,:); inv_jac_prop((2+(dof*(i-1))),:) = rrall2 + wvarray(2,:); inv_jac_prop((3+(dof*(i-1))),:) = rrall3 + wvarray(3,:); inv_jac_prop((4+(dof*(i-1))),:) = wxarray(1,:); inv_jac_prop((5+(dof*(i-1))),:) = wxarray(2,:); inv_jac_prop((6+(dof*(i-1))),:) = wxarray(3,:); cluster.propinvjac = inv_jac_prop; end end end % Further notes % For each Robot in the cluster, the following Algorithm is compiled. % V = V_final + sumall ((product of rotations)*(local_velocity + (global_angular_velocity x local position) ) ) %However, the algorithm expands this as %V = V_final + sumall ((product of rotations)*local_velocity + (product of rotations)*(global_angular_velocity x local position) ) )

74

B.4.6 rotatedwxp3.m

function [ RRwxp, RRwyp ] = rotatedwxp3( t, current_node, variablesyms,RR,py,px ) par =5; wxarray = zeros(1,numel(variablesyms)); while par>0 par = t.getparent(current_node); tpar = t.get(par); rz = acos(tpar(1,1)); for j=1:numel(variablesyms) % To find rz's position in cluster.var check = 2* (variablesyms(j)/rz); if check ==2 break; else end end wxarray(j) = 1; current_node = par; par = t.getparent(current_node); end RRwxp= wxarray* (-RR(1,1)*py +RR(1,2)*px); RRwyp= wxarray* (-RR(2,1)*py +RR(2,2)*px); end

B.4.7 rotatedwxp6.m

function [ RRwxp, RRwyp, RRwzp ] = rotatedwxp6( t, current_node, variablesyms,RR,py,px, pz ) % This function gets the cross product, then multiplies it by the Rotation % Matrices par =5; wxarray = zeros(3,numel(variablesyms)); while par>0 par = t.getparent(current_node); tpar = t.get(par); ry = asin(-tpar(3,1)); rz = asin(tpar(2,1)/cos(ry)); rx = asin(tpar(3,2)/cos(ry)); % Find Rx's position in the cluster.var for j=1:numel(variablesyms) check = 2* (variablesyms(j)/rx); if check ==2 wxarray(1,j) = 1; else

75

wxarray(1,j) = 0; end end % Find Ry's position in the cluster.var for k=1:numel(variablesyms) check = 2* (variablesyms(k)/ry); if check ==2 wxarray(2,k) = 1; else wxarray(2,k) = 0; end end % Find Rz's position in the cluster.var for l=1:numel(variablesyms) check = 2* (variablesyms(l)/rz); if check ==2 wxarray(3,l) = 1; else wxarray(3,l) = 0; end end current_node = par; par = t.getparent(current_node); end RRwxp= (RR(1,1)*(wxarray(2,:)*pz- wxarray(3,:)*py) + RR(1,2)*(wxarray(3,:)*px- wxarray(1,:)*pz) + RR(1,3)*(wxarray(1,:)*py- wxarray(2,:)*px)); RRwyp= (RR(2,1)*(wxarray(2,:)*pz- wxarray(3,:)*py) + RR(2,2)*(wxarray(3,:)*px- wxarray(1,:)*pz) + RR(2,3)*(wxarray(1,:)*py- wxarray(2,:)*px)); RRwzp= (RR(3,1)*(wxarray(2,:)*pz- wxarray(3,:)*py) + RR(3,2)*(wxarray(3,:)*px- wxarray(1,:)*pz) + RR(3,3)*(wxarray(1,:)*py- wxarray(2,:)*px)); end

B.4.8 thefinv3.m

function [ wvarray ] = thefinv3( t, current_node,variablesyms ) % This function calculates the final velocity v1 = t.get(current_node); wxx = v1(1,3); wyy = v1(2,3); % To find ww's position in variably syms wvarray = zeros(1,numel(variablesyms)); for j=1:numel(variablesyms) check = 2* (variablesyms(j)/wxx);

76

if check ==2 break; else end end for k=1:numel(variablesyms) check = 2* (variablesyms(k)/wyy); if check ==2 break; else end end wvarray(1,j) = 1; wvarray(2,k) = 1; end

B.4.9 thefinv6.m

function [ wvarray ] = thefinv6( t, current_node,variablesyms ) % This Function calculates the final velocity v1 = t.get(current_node); wxx = v1(1,4); wyy = v1(2,4); wzz = v1(3,4); %To find ww's position in variably syms wvarray = zeros(1,numel(variablesyms)); for j=1:numel(variablesyms) check = 2* (variablesyms(j)/wxx); if check ==2 break; else end end for k=1:numel(variablesyms) check = 2* (variablesyms(k)/wyy); if check ==2 break; else end end for l=1:numel(variablesyms) check = 2* (variablesyms(l)/wzz); if check ==2 break; else end end wvarray(1,j) = 1; wvarray(2,k) = 1; wvarray(3,l) = 1; end

77

B.4.10 thewthing3.m

function [ wxarray ] = thewthing3( t, current_node, variablesyms ) %THEWTHING3 Calculates the angular velocity % This program calculates the angular velocity for the cluster. It is % called in the function invjacfxn.m par=5; wxarray = zeros(1,numel(variablesyms)); while par>=0 tpar = t.get(current_node); ww = acos(tpar(1,1)); %To find ww's position in variable syms for j=1:numel(variablesyms) check = 2* (variablesyms(j)/ww); if check ==2 wxarray(j) = 1; break else % wxarray(j) = 0; end end if current_node==1 break; else current_node = t.getparent(current_node); par = t.getparent(current_node); end end end

B.4.11 thewthing6.m

function [ wxarray ] = thewthing6( t, current_node, variablesyms ) %THEWTHING6 Calculates the angular velocity % This function is called from the function invjacfxn.m par=5; wxarray = zeros(3,numel(variablesyms)); while par>=0 tpar = t.get(current_node); ry = asin(-tpar(3,1)); rz = asin(tpar(2,1)/cos(ry)); rx = asin(tpar(3,2)/cos(ry)); for j=1:numel(variablesyms) % Gets Ry's position in cluster.var check = 2* (variablesyms(j)/ry);

78

if check ==2 wxarray(2,j) = 1; break; else % wxarray(2,j) = 0; end end for k=1:numel(variablesyms) % Gets Rz's position in cluster.var check = 2* (variablesyms(k)/rz); if check ==2 wxarray(3,k) = 1; break; else % wxarray(3,k) = 0; end end for l=1:numel(variablesyms) % Gets Rx's position in cluster.var check = 2* (variablesyms(l)/rx); if check ==2 wxarray(1,l) = 1; break; else % wxarray(1,l) = 0; end end if current_node==1 break else current_node = t.getparent(current_node); par = t.getparent(current_node); end end end

79

AppendixC-TheInverseJacobianFormula:AProof

ConsidertheHTMofarobotathierarchicaldepthn,inafixedframe,{i},relativetoglobalframe{G}.TheHTM, 𝑇OD ,canbeexpandedintotheform:

𝑇OD = 𝑇m+D ∗ ( 𝑇m�

m�:+NqP, ) ∗ 𝑇O

m� (Eq34)

Thiscanbefurtherexpandedasfollows:

𝑇OD = 𝑅+D 𝑃+D0 1

∗ 𝑅,+ 𝑃,+0 1

∗ 𝑅l, 𝑃l,0 1

∗ … . .∗ 𝑅OO:+ 𝑃OO:+

0 1 (Eq35)

𝑇OD = 𝑅+D 𝑅,+ 𝑅+D 𝑃 + 𝑃+D,+

0 1∗ 𝑅l, 𝑃l,

0 1∗ … . .∗ 𝑅OO:+ 𝑃OO:+

0 1 (Eq36)

𝑇OD = 𝑅+D 𝑅,+ 𝑅l, … 𝑅OO:+ 𝑅+D 𝑅,+ 𝑅l, … 𝑅 𝑃 +OO:+

O:+O:, … . . + 𝑅+D 𝑃 + 𝑃+D,

+

0 1= 𝑅OD 𝑃OD

0 1 (Eq37)

Bytakingalookatthefinalmatrix,thepositionvector 𝑃OD describingthepositionofframe[i}

relativeto{G}canberecovered:

𝑃OD = 𝑅+D 𝑅,+ 𝑅l, … 𝑅 𝑃 +OO:+

O:+O:, … . . + 𝑅+D 𝑃 + 𝑃+D,

+ (Eq38)

Thiscanbewrittenmorecompactlyas:

𝑃ND = ( 𝑅qq:+O

qN:+OP+ ) 𝑃O�+

O + 𝑃+D (Eq39)

Takingaderivativeoftheaboveexpression,thefollowingisobtained:

𝑃OD = ( 𝑅qq:+O

qN:+OP+ ( 𝑃 + ( 𝜔OD × 𝑃OO:+ ))O�+

O + 𝑃+D (Eq40)Thisequationisidenticaltotheequationthatwasobtainedviathepropagationtechnique(Equation23).Letusnowconsiderthefinalmatrixalsoprovidestherotationmatrix, 𝑅ND ,whichgivestherotationofrobotnrelativetotheglobalframe.

𝑅ND = 𝑅+D 𝑅,+ 𝑅l, … 𝑅NO:+ (Eq41)

Fromthisanewvectorcanbecreatedtorepresenttheserotations:

𝜃ND = 𝛼U𝛽X𝛾Z

(Eq42)

Where𝛼U,𝛽X,𝛾Zarethesumofallrotationsaboutthez,y,andxaxisrespectively.For

example,alltherotationsaboutthezaxisarefoundby:

80

𝑅ND U =𝑅+D U 𝜃+D 00

+ 𝑅+D ∗𝑅,+ U 𝜃,+ 00

+ ⋯+ 𝑅+D ∗ 𝑅 ∗,+ … ∗ 𝑅N:+

N:, ∗ 𝑅NN:+U 𝜃OO:+ 00

(Eq43)

Thiscanbewrittenincompactformas

𝑅ND U = ( NOP+ 𝑅O:+

O:, ) 𝜃OO:+ �NP+ (Eq44)

Takingaderivativeofthisyields:

𝑅ND U = 𝜔�D = ( NOP+ 𝑅O:+

O:, ) 𝜃OO:+ �NP+ (Eq45)

Thisequationisalsoidenticaltotheequationthatwasobtainedviathepropagationtechnique

(Equation24).Arrivingatthepropagationequationsbytakingderivativesoftheinversekinematicsshowsthat

thepropagationmethodofcomputingtheinversekinematicequationsisvalid.

an algorithm for calculating the inverse jacobian of

Documents