Robot Dynamics: Equations and Algorithms

1 Robot Dynamics: Equationsand AlgorithmsRoy FeatherstoneDepartment of ComputerScienceUniversity of Wales,AberystwythPenglais,AberystwythSY2 33DB,Wales,UKDavid OrinDepartment of ElectricalEngineeringOhioStateUniversity Columbus,OH 43210-1272,USAA bstractThispaper reviewssomeof theaccomplishments inthe eldof Robot dynamicsresearch, fromthedevel-opment of therecursive Newton-Euleralgorithmtothepresent day. Equationsandalgorithmsaregivenforthemost important dynamicscomputations,ex-pressedin a commonnotationto IntroductionMany contributionshave beenmadein theareaofrobot eldof thedynamicsof mecha-nisms,theroboticscommunity hasespeciallyfocusedontheproblemof computationale ciency. Infact,many of themoste cient algorithmsindynamics,thatareapplicableto a wideclassof mechanisms,weredeveloped by roboticsresearchers[23, 33, 10].Whilecomputationale ciencycontinuesto be im-portant forthesimulationandcontrolof increasinglycomplexmechanismsoperatingat higherspeeds,otheraspectsof formulatedwitha compactsetof equationsforeaseof development ,thereshouldbe a clearrelationshipbetweentheseequationsan dtherecur-sive setfromwhich thegreatestcomputationale -ciencyis spatialnotationandspatialoperatoralgebra [11, 29] hasbeenverye ec-tive in ,it is important to developalgorithmswhich have applicability to roboticmech-anismswithgeneralgeometriesa ndjoint [23,33, 10] wereapplica-0c thismaterialis ,permissionto reprint/republishthismaterialforad-verti singor promotionalpurposesor forcreatingnewcollectiveworksforresaleor redistributionto serversor lists,or to reuseany copyrightedcomponent of thisworkin anelectronicversionof a paper thatappearedorig-inallyin Proc.

2 IEEEInt. Automation,SanFrancisco,CA,2000, { is word-for-wordidenticalto theoriginal,buttheformattingis slightlydi single,open-chainmanipulatorswitheitherr o-tationalor prismaticjoints,generaljoint modelshavesincebeendevelopedandappliedto morecomplexcon gurations[11].Thepurposeof thispaper is to reviewsomeof recursivecomputationswillbe presentedin a common,concisenotationin ,thealgorithmsaredirectlyapplicableto a compactform,andthisis followed by a discussionof notpermitusto includereferencesto allof theimportant con-tributionsin the FoundationalWorkin Robot Dy-namicsEarlye ortsin Robot dynamicsweredirectedto ex-pressingtheequationsof motionforrobot manipula-tors,andothersingleopen-chainsy stems,in themoste cient forthemostcommoncomputationsforrobot analysis,control, thissection,emphasiswillbe , spacedoes notpermitusto providea comprehensive reviewof theextensive lit-eraturein theearlybooksin theareaforadditionalreferences[7, 11].}

3 Theclassicapproach to expressingtheequationsofmotionwasbasedon a Lagrangianformulation[18,31] (N4), cient low-orderal-gorithmsweresought forthreemajorcomputations:1. inversedynamicsin which therequiredjoint ac-tuatortorques/forcesarecomputedfroma speci- cationof themanipulator'strajectory(position,velo city, andacceleration),2. forwarddynamicsin which theappliedjoint ac-tuatorstorques/forcesarespeci edandthejointaccelerationsareto be determined,and3. themanipulatorinertiamatrixwhich mapsthejoint accelerationsto thejoint usedin feedforwardcontrol,andforwarddynamicsis (mass)matrixis usedin analysis,in feedbackcontrolto linearizethedynamics,andis anintegralpartof many rstresearcherstodevelopO(N) algorithmsforinversedynamicsforroboticsu seda Newton-Euler(NE)formulationof andVukobratovic[30] developeda recursive NEmethodforhumanlimb dynamics ,andOrinet al.[26] madetherecursive method moree cient by referringforcesandmoments tolocallinkcoordinatesforreal-timecontro lof a legof a ,Walker,andPaul[23] developed a verye cient Recursive NEAlgorithm(RNEA)by referringmostquantitiesto [16] developed anO(N) recursive Lagrangianfor-mulation,butfoundthatit wasmuch lesse cientthantheRNEAin termsof thenumber of multiplica-tionsandadditions/subtraction srequiredin beenmadein e Balafoutiset al.

4 [3]andHeandGoldenberg[15] arerepresentative of thosethatareupto a factorof theRNEA(fora 6-DoFrobot).Walker andOrin[33] usedtheRNEA forinversedynamics[23] as thebasisfore cient 3, laternamedtheComposite-Rigid-BodyAlgorit hm(CRBA)by Feath-erstone[11], computedtheinertialparametersof com-positesetsof rigidbodiesat theouterendof theinertiamatrixwerecomputedverye cientlythroughsuccessive ap-plicationof inversedynamicswiththejoint velocitiessetto zero,andthejoint accelerationssetto zeroora in motionat a time,theinversedynamicsreducesto a much simpli edanalysisof a basesetof linksinstaticequilibriumanda compositerigidbodyin mo-tionat theouterendof theneedto solve a linearsystemof equationswhosesizegrowswithN, thealgorithmwasO(N3). For smallN, the rst-ordertermsdominatedthecomputationso thattheresultwas quitee (N) algorithmforforwarddy-namicswas developed by Vereshchagin[32].

5 Thisalgo-rithmusesa recursive formulato evaluatetheGibbs-Appel formof theequationof motion,andis (ABA),butthepaperwasway aheadof itstimeandlanguishedin obscurity fora ,ArmstrongdevelopedanO(N) algorithmformecha-nismswithsphericaljoin ts [1], andthenFeatherstonedevelopedtheABA[10]. The rstversionof thisal-gorithmwas applicabletomanipulatorswithsingle-degre e-of-freedomjoints,butthesecondincludeda generaljoint modelandwas faster[11]. In termsof thetotalnumber of arithmeticoperationsrequired,theABAwas moree cient thantheCRBAforN >9 [11].Also,usingsimilare cient transformationsandlinkcoordinatesas Featherstone[11, 10], Brandlet al.[8]madefurtherimprovements on theABAso thatit wasroughlycomparableto theCRBAforN= 6. Furthergainshave beenmadein e ciencyover theyears,withMcMillanandOrin[24] beingrepresentative of thosethathave reducedthecomputation(another15%re-ducti on).Thee ciencyoftheCRBA wasdirectlyrelatedtothee ciencyof computingthejoint spaceiner-tia(mass)matrix[33].

6 Featherstone[11] usede -cient transformationsandlinkcoordinatesto reducethecomputationof theinertiamatrixby about30%.A numberof othergainshave beenmadeover theyears[4, 25] givinganoverallimprovement of closetoa factorof two fromthatof [33]. LillyandOrin[22]developedfourmethodsforc omputationof edCompositeRigidBodyMethod includescomputationof themanipulatorJa-cobiansothatit is verye cient forcomputingtheinertiamatrixin [20] developedanoperational-spaceformu-lation of Robot dynamics ,in which theequationsareexpressedin thesamecoordinatesystemthatis usedto commandtherobot:Cartesiancoordinatesando ri-entationof theend-e hybridmotion/forcecontrolandrelatedappli cations[21].Rodriguez[28] recognizedtheparallelsbetweentheconcepts andtechniquesof Kalman lteringandtheforwarddynamicsproblem,andd eveloped thespatialoperatoralgebraframeworkforthe studyof JPL[29] todevelopalternative factorizationsof themassmatrixto derive [17] usedthespatialopera-toralgebraframeworkt o providea uni ,hewas abletocomparethevariousO(N3),O(N2), andO(N) ,Pai,andCloutier[2]usedthespatialoperato rframe-workto unifythederivationof boththeCRBA andtheABA,as two eliminationmethodsto solve thattheABAis worksareallconcernedwithrigid-bodydynami cs,andarethereforeapplicablewheneverarob otmechanismcanbe adequatelymodelledby arigid-body non-rigidbehaviour,like complianceinthejoint bearings,arerelativelyeasyto incorporateinto a rigid-bodymodel.

7 Ad-dressedby Book[6], whodevelopedane cient, re-cursive Lagrangianformulation(using4 4 matrices)of bothinverseandforwarddynamicsforserialch ainswith generalmodalformulationofelasticdisplace ment was Equationsand AlgorithmsThissectionpresents of brevity, equationsarewrittenin spatialnotation;butreaderswhoarenotfamil iarwiththisnotationshouldstillbe abletofollow 1 vectorscontainingboththelinearandangular components of physicalquan-titieslike velocity, techni-calreasons,theyareseparatedinto two vectorspaces:motion-type vectorsinM6andforce-type vectorsinF6. Tensorquantities,like inertia,arerepresentedus-ing6 6 a dualsystemof briefdescriptionof thecurrent versionof spatialalgebracanbe foundin theappendixof [12], anda detaileddescriptionof thepre-viousversionin [11]. Newton-EulerAl-gorithmA generalrobot mechanismwithtreestructurecanbemodelledb y a setofNmovablelinks(rigidbodies),numbered 1: : : N, a xedbaselink,numbered0, andasetofNjoints thatconnectbetweenthelinksso thatjointiconnectsfromlink (i) tolinki, where (i)is thelinknumber of theparent of linkiin thetree,takingthebaselinkas theroot that (i)< i.

8 In thespecialcaseof anunbranchedkinematicchain, (i) =i 1 andthelinksandjoints arenumberedconsecutivelyfromthebaseto we letvibe thevelocity of linki, andvJibe thevelocity acrossjointithenvJi=vi v (i):(1)Thejoint velocity canalsobe described in theformvJi=hi_qi;(2)wherehiis a 6 dimatrixspanningthemotionfree-domsubspac eof jointi,_qiis adi 1 vectorof joint ve-locity variables,anddiis thedegreeof freedom(DoF)of jointi. In thespecialcaseof a 1-DoFjoint,hiis avectordescribingthejoint'saxisof and2 producesvi=v (i)+hi_qi;(3)which is thestandardrecursive formulaforaccelera-tionsis justthetime-derivative of :ai=a (i)+_hi_qi+hi qi;(4)whereaiis theaccelerationof linkiand qiis a vec-torof joint revoluteandprismaticjoints,andmany otherspecialcases,_hi=vi hi:Giventhevelocity andaccelerationof thebase,v0anda0, theseformulascalculatethevelocity andac-celerationof each linkin turn, typicalalgorithmlookslike this:fori= 1toNdovi=v (i)+hi_qi;ai=a (i)+_hi_qi+hi qiendTheproperty (i)< iensuresthatv (i)is motionforlinkiisfi+fxi=Iiai+vi Iivi;(5)whereIiis thespatialinertiaof linki(a 6 6 ma-trix),fiis thenetforceappliedto linkithroughthejoints,andfxiis thesumof allotherforcesactingonlinki.

9 ThisequationcombinesNewton'sandEuler' ofpublishedworksusestandardlinearandangu laraccelerationsinstead,eitherseparately ( [23])or in a 6-Dnotation( ).Thedi erenceis explainedin [11, ].Papersthatusenon-spatialaccelerationsh ave di erentexpressionsin a includecontributionsfromsprings,dampers, force elds,contactwiththeenvironment, andso on;butitsvalueis assumedto be known,or at leastto becalculablefromknownquantities(inwhich casethecalculationoffxiis consideredto be a separateprob-lem).fximay alsoincludethee ectof gravity onlinki; butthespecialcaseof a uniformgravitational eldcanbe simulatedmoste cientlyby impartinga cti-tiousaccelerationto thebase:ifgis thegravitationalaccelerationvectorthenad d gtoa0[23].If we de nefJito be theforcetransmittedfromlink (i) to linkithroughjointi, thenfi=fJi Xj2 (i)fJj(6)where (i) is thesetof childrenof linki: (i) =fjj (j) = and6 producesfJi=Iiai+vi Iivi fxi+Xj2 (i)fJj;(7)which is a recursive formulaforcalculatingjoint forces,startingat typicalimplementationof thisformulalookslike this:fori= 1toNdofJi=Iiai+vi Iivi fxiendfori=Nto1doif (i)6= 0thenfJ (i)=fJ (i)+fJiendThe nalstepis to extractthedi 1 vectorof jointforcevariables, i, fromthespatialvectorof doneby i=hTifJi:(8)Equations3, 4, 7 and8 togetherformtheRNEA fora ciency, theequationsshouldbeevaluatedin we assigna coordi-natesystemto each link,andrepresent each spatialvectorin thecoordinatesof thelinkto which it refers, , 4 and7 needto includecoordinatetrans-formsat appropriateplacesin edequationsarevi=iXM (i)v (i)+hi_qi.

10 Ai=iXM (i)a (i)+_hi_qi+hi qiandfJi=Iiai+vi Iivi+Xj2 (i)iXFjfJj;whereiXM (i)andiXFjarecoordinatetransformationmat ricesformotion-type andforce-type vectors, ,in linkco-ordinates,lookslike this:fori= 1toNdovi=iXM (i)v (i)+hi_qi;ai=iXM (i)a (i)+_hi_qi+hi qi;fJi=Iiai+vi Iivi fxiendfori=Nto1do i=hTifJi;if (i)6= 0thenfJ (i)=fJ (i)+ (i) Algo-rithmAsexplainedin [33, 11], if we expresstheequationof motionof a tree-structurerigid-bodysystemin theformM q+C(q;_q) = ;where q2 Mnis thevectorof generalizedacceler-ations, 2 Fnis thevectorofgeneralizedforces,M:Mn7!Fnis thesystemmassmatrix(joint-spaceinertiama trix),andC2 Fncontainsallof theacceleration-independent terms,thenM q=D(q;_q; q) D(q;_q;0)=D(q;0; q) D(q;0;0);whereD(q;_q; q) is argument canbe settozerobecausethevelocity termscancel.(Gravity andfxtermsalsocancel.)Thisequationimmedi atelygivesus a simplealgorithmforcalculatingM:M i=D(q;0; i) D(q;0;0);i= 1: : : n ;where iis ann 1 vectorwitha 1 in iis columniofM.