Constantinescu E A Croft Industrial Automation Laboratory Department of Mechanical Engineering University of British Columbia Vancouver BC Canada V6T 1Z4 email daniela mechubcca Received January 2 1999 accepted January 21 2000 This article presents ID: 27794
Download Pdf The PPT/PDF document "Smooth and TimeOptimal Trajectory Planni..." is the property of its rightful owner. Permission is granted to download and print the materials on this web site for personal, non-commercial use only, and to display it on your personal computer provided you do not modify the materials and that you retain all copyright notices contained in the materials. By downloading content from our website, you accept the terms of this agreement.
SmoothandTime-OptimalTrajectoryPlanningforIndustrialManipulatorsalongSpecifiedPathsvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvD.Constantinescu,*E.A.CroftIndustrialAutomationLaboratory,DepartmentofMechanicalEngineering,UniversityofBritishColumbia,Vancouver,BC,CanadaV6T1Z4e-mail:danielaReceivedJanuary2,1999;acceptedJanuary21,2000Thisarticlepresentsamethodfordeterminingsmoothandtime-optimalpathcon-strainedtrajectoriesforroboticmanipulatorsandinvestigatestheperformanceofthesetrajectoriesboththroughsimulationsandexperimentsThedesiredsmoothnessofthetrajectoryisimposedthroughlimitsonthetorqueratesThethirdderivativeofthepathparameterwithrespecttotime,thepseudo-jerk,isthecontrolledinputlimitsontheactuatortorquestranslateintostate-dependentlimitsonthepseudo-Thetime-optimalcontrolobjectiveiscastasanoptimizationproblembyusingcubicsplinestoparametrizethestatespacetrajectoryTheoptimizationproblemissolvedusingtheflexibletolerancemethodTheexperimentalresultspresentedshowthattheplannedsmoothtrajectoriesprovidesuperiorfeasibletime-optimal2000JohnWiley&Sons,Inc1.INTRODUCTIONTheneedforincreasedproductivityinpath-follow-ingindustrialroboticapplicationshasbeencom-monlyaddressedintheliteraturebydeterminingpath-constrainedtime-optimalmotionsPCTOMwhileaccountingforactuatortorquelimitstheseformulations,thejointactuatortorquesarethecontrolledinputsandtheopenloopcontrolTowhomallcorrespondenceshouldbeaddressedschemesresultinbang-bangorbang-singular-bang1,3,4PCTOMtrajectoriescomputethemaximumve-locityachievableattherobottipwhilestillfollow-ingtheprescribedpathHowever,implementationofPCTOMinphysicalmanipulatorshasdrawbacks,suchasjointoscillationsduetofinitejointstiffnessandovershootofthenominaltorquelimitsduetounmodelledactuatordynamicsTheresultantextrastrainontherobotactuatorscouldcausethemtofailfrequently,reducingtheproductivityoftheentireworkcell ()()JournalofRoboticSystems175,23324920002000byJohnWileySons,Inc. JournalofRoboticSystemsÐ2000Atthetrajectoryplanninglevel,anumberofdifferenttechniqueshavebeendevisedtoaddresstheproblemofdiscontinuousactuatortorquesmodifiedcostfunction,suchastime-jointtorquesortime-squareofjointtorques,canbeusedtosmooththecontrolsandimprovethetrackingaccu-racy,attheexpenseofmotiontimeAnotherwayofsmoothingthecontrolsistoparametrizethepathbyusingfunctionsthatareatcontinuous,i,continuousinaccelerationCubicsplinesusedforpathparametrizationwithtimeasthecostfunctionresultintrajectoriesthathavecontinuousjointaccelerationsHowever,thelimitsonthejointvariablesareveryconservative,sincetheyremainconstantovertheentireworkIncorporatingtheactuatordynamicsinthisproblemformulationtransformstheactuatorvolt-agesintothelimitedcontrolledinputsTheoptimaltrajectoryisbang-banginthenewcontrolsandtheactuatortorquesarenolongerlimitedAlso,thecaseofsingularcontrolsisnotconsideredsincetheycanbeavoidedbyanappropriateselectionofthepathorbyconvexifyingthesetofadmissibleInthisarticle,amethodispresentedfordeter-miningtime-optimalpath-constrainedmotionssub-jecttolimitsontheactuatortorquesandthefirstderivativeofactuatortorques,or``torqueratesTheresultingtrajectorieswillbecalledsmoothpath-constrainedtime-optimalmotionsSPCTOMtodistinguishthemfromthepath-constrainedtime-optimalmotionsPCTOM,whichdonotcon-sidertorqueratelimitsTheactuatortorqueratelimitsareimposedinviewofthefactthatunlimitedchangesintorquecancausehighlyjerkymotionandseverevibrationsinthearmthatmayleadtothefailureoftheactuatorsthemselvesMoreover,theyareusedasameanstocompensateforstructureflexibilityandinaccuraciesintherobotmodelThisisadesiredfeatureinindustrialapplications,wheretherobotmodelisnotreadilyavailableTherefore,thebenefitoftheSPCTOMtrajectoriesisthattheybetterchar-acterizethedynamiclimitationsofarobotsystemand,hence,aresuitedfordirectimplementationonacommercialrobotusingnonspecializedindustrialGeometriclimitsonrobotmotion,suchasob-staclesandjointlimits,arenotaddressedherein,sincethemotionispath-constrainedThatis,onlythetrajectoryplanningproblemisconsideredpathiseitherimposedbytheapplicationitselforatime-optimalpathcanbedeterminedasinrefUndertheassumptionthatthedesiredpathissmooth,aninitialguessisgeneratedusingsplinesandtheoptimalpathisfoundthroughanuncon-strainedparameteroptimizationThecostfunctioniscomposedofthemotiontimealongthepathpluspenaltytermscorrespondingtoobstaclesandjoint2.SMOOTHPATH-CONSTRAINEDTIME-OPTIMALMOTIONS2.1.ProblemFormulationTheproblemofSPCTOMplanningcanbestatedassubjecttothemanipulatordynamics,...qGqqÈÇÇtheboundaryconditions,.......0,3thepathconstraints,..theactuatortorquelimits,minmaxandtheactuatortorqueratelimits:ÇÇÇminmaxisthenumberofdegreesoffreedomoftheisthevectorofjointpositions,isthevectorofactuatoristhevectoroftorquerates,istheinertiamatrixofthemanipulator,isathird-ordertensorrepresentingthecoefficientsofthecentrifugalandCoriolisforces,isthevectorofgravityterms,andisacontinuouscurveparametrizedby,whichmaybe,forexample,thearclengthTosimplifythedynamics,viscousandstaticfrictiontermshavebeenneglectedHowever,asshownintheexperi-mentsinSection5,theimpositionofsuitabletorqueratelimitscompensatesfortheseandothermodel ConstantinescuandCroft:SmoothandTime-OptimalTrajectoryPlanningIntheaboveformulation,thetorqueratesrepre-senttheboundedcontrolsSincetheLagrangianformoftherobotdynamicsincorporatesonlytheactuatortorques,thethird-orderdynamicsisre-Differentiationof2withrespecttotimeresultsin....qCqqqCqqÈÈÇÇÇ...qCqqEquation7istakenasthedynamicsofthesystem,withrepresentingtheboundedcontrols2.2.PathConstraintsThedynamicsystemdescribedbyEq7has3degreesoffreedomHowever,thepathconstraints4parametrizetheend-effectortippositionbyasingleparameter,reducingtheorderofthesystemto3Toobtainthetorquerateboundsforthere-ducedordersystem,thejointjerkiscomputedasÇÇÈ sJ?ry?qy2dsd2JdJZZZZZXXXXXYYYYYZZZZZ ,10beingtheend-effectorpositionandorienta-beingtheJacobianoftheforwardkinematicsmap,anddenotingthederivativewithrespectto..thepathparameterSubstitutingEqs7and8intoEq6yields....ÇÈÇÇminmax..,12 ..,13 ssM?q-----q?q00000qq?C?qn=1dsdCXTXT ,14 ..Thematricesandthethird-orderarerobotdependentAsshowninSection23,thetorquerateboundsprovideconstraintsontheadmissiblestatesfortheHowever,thetorqueboundsderivedinrefs3and11arestillrequired,since,asthetorquerateboundsbecomeverylarge,thetorqueboundsbe-comethelimitingconstraintForinfinitetorquerates,theproblemreturnstoPCTOMFollowingref3,theactuatortorqueboundsforthereducedordersystemareobtainedsubstituting...thepathconstraints4andEq2intoEq....,16minmax..,17,18..2.3.TorqueLimitsAsdiscussedinref3,foreachvalueofthepath,theactuatortorquebounds16trans-lateintoapolygonalfeasibleregionintheSucharegionisshownschematicallyinFig-ure1fora3-dofmanipulatorAnalytically,theactuatortorqueboundstranslateintolimitsonthepseudo-velocityandthepseudo-acceleration:..max,...ÈÇÈÈÇmin,max, Figure1.Admissibleregionintheplane,afterref JournalofRoboticSystemsÐ2000Thesubscriptisusedtodiscriminatethepseudo-velocityandpseudo-accelerationboundsduetothetorqueconstraints16fromthoseduetothetorquerateconstraints11,whichwillbedenotedwiththeThecurveasrepresentedinthemax,phaseplaneiscalledthevelocitylimitcurveanditrepresentsanupperboundforanyfeasibletrajectoryinthisplaneTheconstraintsonthepseudo-velocityandpseudo-accelerationduetotheactuatortorquelimitsarecomputedasdiscussedin2.4.TorqueRateLimitsAsimilarapproachcanbeusedtodeterminethepseudo-velocity,pseudo-acceleration,andpseudo-jerkboundsduetothetorqueratelimitsThus,forgivenvaluesofthepathparameterandpseudo-,thetorqueratebounds11formapolyg-onalfeasibleregionintheplanesuchastheoneshownschematicallyinFig2fora3-dofmanip-Analytically,thetorquerateboundstrans-lateintopseudo-accelerationandpseudo-jerklimitsinthe...ÈÇÈÈÇmin,max,...&&&ÇÈÇÈminmaxandaconstraintonthepseudo-velocityinthe..,24max,isdefinedasthepseudo-velocitymax,valueforwhichtheadmissibleregioninthe Figure2.Admissibleregionintheplanereducestoapoint...ÈÇÈÇmin,max,max,max,Afterperformingthecalculations,thepseudo-jerkandpseudo-accelerationlimitsresultasiii maxmin,min,26iii minmax,max,27min,jiiijj maxmin,jiij,28max,jiiijj minmax,jiij,29andthepseudo-velocitylimitcanbecomputedbyanumericalsearch2.5.AdmissibleStatesIntheformulationoftheSPCTOMproblempro-posedherein,thetorqueratelimitsareimposedasameansforadjustingthesmoothnessofthetrajec-Hence,theyareindependentoftheactuatortorquelimitsThisindependenceisreflectedinthestatespace,asshowninFigure3Inthisfigure,theactuatortorqueandtorquerateconstraintsforthefirstthreejointsoftheSCORBOTERVIIrobot6,TableIareplottedtogetherinstatespaceforthethreeexampletorqueratelimitsinTableIIThisindependenceoftheactuatortorqueandtorqueratelimitsisreflectedinanewconstraintonthepseudo-velocity,..,30ÇÇÇmax,max, ConstantinescuandCroft:SmoothandTime-OptimalTrajectoryPlanning Figure3.AdmissiblestatesintheTableI.SCORBOTERVIIestimatedkinematicanddynamicparameters wxwxwxwx p 111122223333222wxwxwxwxLinkMasskgxyz ms0.0Is0.00Is0.05Is0.01x1y1z12ms6.6Is0.10Is0.60Is0.62x2y2z23ms4.2Is0.02Is0.20Is0.33x3y3z3 andanewconstraintonthepseudo-acceleration,..min,min,..ÈÈÈmax,max,Equation30definesaglobalvelocitylimitcurve,calledthesmoothmotionvelocitylimitcurveIntheplane,theSMVLCisanupperboundonanyfeasibletrajectoryTheSMVLCcanbecom-putedateachpointalongthepathbyalinesearchusingbisectionthesearcheddomainislimitedfrom..zerotomax,TheSMVLCcorrespondingtothethreeexam-plesinTableIIareplottedinFigure4Asshowninthisfigure,theSMVLCisdeterminedbyacombina-tionofbothactuatortorqueandtorqueratelimitsDependingontherestrictionsofthetorqueratelimits,theycandeterminetheSMVLCalmosten-tirely,asshowninthethirdexample,ortheycanTableII.ImposedactuatortorqueandtorquerateboundsfortheSCORBOTERVII HightorqueMediumtorqueLowtorqueTorqueratelimitsratelimitsratelimitslimitsExample1Example2Example3ÇÇÇwxwxwxwx123 ÇÇÇ1111213ÇÇÇ2212223ÇÇÇ3313233 JournalofRoboticSystemsÐ2000 Figure4.SMVLCfordifferentactuatortorqueratelim-havelittleinfluenceonit,asshowninthefirst2.6.SystemDynamicsThestatesofthereducedsystemareisthescalarcontrolTheSPCTOMplan-ningproblemisreformulatedas,32subjecttothesystemdynamicsxxu,33theboundaryconditions:0000ffff..thestateinequalityconstraints30and31,andthestate-dependentcontrolinequalityconstraints23ThisreformulationshowsthattheSPCTOMproblemisatime-optimalcontrolTOCproblemforafirst-orderlinearsystemwithnonlinearstateandcontrolinequalityconstraintsandpreimposedinitialandfinalstatesMoreover,Equations23,..30,and31emphasizethatthestateandcontrolconstraintsareindependentlyactive,sincethecon-trolsarelimitedonlybythetorquerates,whilethestatesarelimitedbyboththetorqueratesandtheactuatortorques3.SOLUTIONOFTHESPCTOMTOCproblemssimilartotheSPCTOMabovehavebeensolvedeitherbyapplyingPontryiagin'sMaximumPrinciplePMPtoderivethenecessaryconditionsforoptimalityandthenusingmultipleshootingmethodstosolvetheresultingtwopointboundaryvalueproblemTPBVPorbyasearchfortheswitchingpoints,usingeitherdynamicpro-orspecificalgorithmsTwodifficultiesariseintheapplicationoftheseapproachesinthepresentcaseFirst,thecomplexityofthedynamicprogrammingalgorithmsgrowsex-ponentiallywiththephasespacedimension,render-ingthemethodinfeasibleformorethantwodimen-Asdefined,theSPCTOMproblemhasathree-dimensionalphasespaceSecond,theothertwoapproachesbasedonPMPandthesearchfortheswitchingpointsdependonthebang-bangorbang-singular-bangstructureoftheoptimalcon-ThisstructurehasbeenprovenusingresultsfromoptimalcontroltheoryOCTregardingsys-temswithstatedependentcontrolconstraintsHowever,noresultshavebeenprovenusingOCTconcerningthenecessaryoptimalityconditionsforsystemswithstateandcontrolconstraintswhichareindependentlyactiveThus,fortheSPCTOMprob-lem,itisnotguaranteedthattheoptimalcontrolsarebang-bangorbang-singular-bangToresolvethesedifficulties,theSPCTOMtrajec-toryplanningproblemisanalyzedandsolvedhereininthephaseplaneThemotivationisthatinthisplanebothtrajectoryend-pointsarefixed,whileinthetimedomainthefinalpointisfreeThus,theTOCproblemlendsitselftoanonlinearparameteroptimizationinthisphaseplaneThemotiontimeiscomputedas ..,35aretheinitialandthefinalvaluesofthepathparameter,respectivelyTherefore,theSPCTOMinthephaseplaneisthesmoothcurvethatminimizesoverthecurvewhilenotviolatingactuatortorqueandortorqueratelimitsInviewoftheabove,theoptimalmotionisdeterminedbyanoptimizationofabasetrajectoryAsetofcubicsplineswithpreselectedknot-pointlocationsarechosenasthebasetrajectoryfortheCubicpolynomialshavebeenselectedtoapproximatetheSPCTOMbecausetheyarethelowestdegreepolynomialsthatresultinasmooth ConstantinescuandCroft:SmoothandTime-OptimalTrajectoryPlanningcurve,i,continuousanddifferentiableevery-ThelocationoftheknotsalongthepathhavebeenchosentobethesameasthelocationoftheswitchingpointsofthePCTOMFigSincethePCTOMrepresentsthelimitforSPCTOM,theseswitchingpointsare,inthelimit,thesameforSPCTOMandprovideareasonableestimateforthelocationoftheSPCTOMswitchingpointsalongtheparametrizedpathExtraknot-pointscouldbechosen;however,thenumberofthePCTOMtrajectoryswitchingpointscouldbehighandtheadditionofextraknotswouldsignificantlyincreasethenumberofoptimizationTherefore,extraknotswillbeinsertedonlywhenthecorrespondingPCTOMtrajectoryhasonesingleswitchingpointInthiscasetheincreaseincomputationaltimeisnegligiblewhileatrajec-toryparametrizationbyonlytwosplinescouldbeThisstrategyissupportedbysimulationswhichhaveshownthatdoublingthenumberofknotsimprovestheSPCTOMmotiontimebyaroundfortrajectorieswithfiveswitchingpointsandby10fortrajectorieswithonlyoneswitchingThelargerdecreaseinmotiontimeisfortrajectorieswithlargerjerksThevariablesoftheoptimizationaretheend-effectorpseudo-velocitiesatthepreselectedknot-pointsalongthepathandtheslopesofthetrajec-toryinthephaseplaneatthepathend-pointsThesevariablescontrolthemotiontime:thehighertheknot-pointsoverthewholetrajectoryaslocated Figure5.SwitchingpointsofthePCTOMdottedlineandasamplesplinedtrajectorysolidlineinthephaseplane,theshorterthemotiontimetheotherhand,theendslopescontrolthespeedatwhichtheactuatortorquesleaveorapproachtheirstaticequilibriumvaluesTherefore,steeperslopesalsoresultinfastermotionThus,thevectorofoptimizationvariables,,isdefinedastheparameterset, /ds1pf0 ,36dsds //dsdswherethevalueswiththeindexcorrespondtothelimitingPCTOMthedottedlineinFigwhiletheothervaluescorrespondtothesplinedtrajectorythesolidlineThesevariablesarenor-malizedsincetheendslopesvaryoveramuchwiderrangethanthepseudo-velocitiesTheoptimaltrajectoryresultsfromspliningcu-bicpolynomialsinthephaseplanebasedontheoptimizedparametersThetrajectorymustbewithinactuatortorqueandtorqueratelimitsandtakeminimumtimeTheactuatortorqueandtorque..rateconstraintsinEqs16and11thusbecome ..max,37max ..max,38min ..max,39max, ..max,40min,Bythisdefinition,wheneveranyoftheactuatortorquesandortorqueratesexceedsitslimits,therespectiveconstraintbecomesnegativeMoreover,byenforcingtheactuatortorqueandtorquerateconstraintsdirectly,ratherthanthestateandcontrolconstraints,thecomputationsaregreatlyAsformulated,theoptimizationissolvedusingtheflexibletolerancemethodFTMTherearetworeasonsforchoosingthismethodFirst,thederivativesoftheconstraintsandthecostfunction,,motiontime,arenotavailableSecond,theFTMkeepsthesearchclosetotheboundaryoftheadmis- JournalofRoboticSystemsÐ2000sibleregionandcanfindaminimumthatliesex-actlyontheboundaryThedetailsoftheFTMarediscussedintheAppendixandfurtherdetailsonitsimplementationforsolvingtheSPCTOMproblemarepresentedinref4.SIMULATIONSThemethodfordeterminingoptimalSPCTOMhasbeenimplementedinMATLABandsimulationsareperformedconsideringonlythepositionaltheSCORBOTERVIIrobotintheIndustrialAu-tomationLaboratoryIALattheUniversityof..BritishColumbiaUBCFigThus,forthesimu-lationsperformedhere,therobotisanelbowma-nipulatorwiththeDHparametersandtheesti-matedmassesandinertiasgiveninTableITheactuatortorquelimitsarethesameforallthethreeexamplesgiveninthispaper,whilethelimitsonthetorqueratesaredifferent,assucces-sivelyshowninTableII4.1.PlanningPerformanceTodeterminetheinfluenceofthetrajectorysmooth-nessonthemotiontime,astraightlineintherobotworkspaceischosenasthepreimposedpathparametricform,thepathisgivenas Figure6.TheSCORBOTERVIIrobotTheresultingoptimaltrajectoriesforthediffer-entlimitsonthetorqueratesareshowninFigures7,8,and9,respectively,bysolidlinesThedashedlinesrepresentthetime-optimaltrajectoryconsider-ingonlytorquelimitsPCTOMThedottedlinesarethesmoothmotionvelocitylimitcurvesSMVLC,i,thevelocitylimitcurvesdeterminedconsideringbothtorqueandtorqueratelimitscorrespondingactuatortorquesandtorqueratesarealsoplottedinthesefiguresWhilethePCTOMtakes059s,theSPCTOMtakes07sinthefirstexampleHere,thelimitsonthetorquerateswereveryhighinfeasibleandthetrajectoryisdeterminedbythelimitsontheactua-tortorquesIntheidealcase,bothtrajectoriesshouldyieldthesamemotiontimes;however,therearetworeasonsfortheincreaseinmotiontimeforSPCTOM:ithelimitedparametrizationchoseninphaseplaneandiithesignificantdecreaseinpeaktorqueratesforSPCTOMsolidlinescom-paredtoPCTOMdottedlines,asshowninthesemi-log-scaleplotinFigure10Inexamples2and3,themorefeasiblelimitsonthetorqueratespredominateTherefore,thetorqueconstraintsarenotapproachedTheoptimalmotiontimesfortheseexamplesarehigher,0735sand5s,respectivelyTheoptimaltrajectoriesdeterminedthroughtheproposedmethodarenotbang-banginthecontrolsThisisaconsequenceoftheparametrizationinthephaseplaneHowever,asseenfromthefirstexam-plepresented,thechosenparametrizationalonecausesacomparativelysmallincreaseinthemotionAsexpected,themorerestrictivethelimitsontorqueratesare,thehigherthemotiontimeisplanningsimulations,however,givenoindicationoftherelationshipbetweentrajectorysmoothnessandthetrackingperformanceofthecontrollerestablishtrackingperformancefivesimulations,fol-lowedbyfiveexperimentswereperformed4.2.TrackingPerformanceThetwooftheSPCTOMtrajectoriescomputedabove,with``feasible''mediumandlowactuatortorquerates,togetherwiththePCTOMtrajectoryandanoptimizedquinticpolynomialtrajectory,havebeenimplementedonasimulatedmodeloftheSCORBOTERVIIrobotwithfrictioncontrolledbyaproportional-integral-derivativePIDindepen-dentjointcontroller ConstantinescuandCroft:SmoothandTime-OptimalTrajectoryPlanning Figure7.Example1hightorqueratelimits Figure8.Example2mediumtorqueratelimits JournalofRoboticSystemsÐ2000 Figure9.Example3lowtorqueratelimits Figure10.Absolutevaluesofthetorqueratesforthe.SPCTOMinexample1solidlinesandPCTOMdottedBoththerobotmodelandthecontrollerhavebeenbuiltintheMATLABSimulinkToolboxFrictionhasbeenmodeledasCoulombandviscousfriction,withtheCoulombfrictioncoefficients0Nmandtheviscousfrictioncoefficients02NmsforallthreelinksThecontrollerhasbeentunedforcriticaldampingandarisetimeof200msforasamplingfrequencyof200HzInthesimulations,theactuatortorquessaturateat10Nm,whichisthetorquelimitconsideredduringplanningThetrackingperformanceofthePIDcontrollerforallfourtrajectoriesisplottedinFigure11,whiletheplannedandsimulatedactuatortorquesareplottedinFigsTheresultsaresummarizedinTableIIIAsseeninFigure11,duetoactuatortorquesaturation,thecontrollercannotkeeptheend-effec-toronthepathwhenthetorqueratesaretoohighThisisthecasewiththePCTOMtrajectoryandtheSPCTOMtrajectorycorrespondingtotorqueratelimitsof100Nmslabeled`spctom2'inFigThisresultshowsthattorqueratelimitsareex-tremelyimportantfortheabilityofthesystemtotrackaplannedtrajectory,especiallygiveninaccu-ratelyidentifiedormodelledsystemdynamicsexpected,thesmootherthetrajectory,i,thelowerthetorqueratelimits,thehigherthetrackingaccu-racyofthecontrollerforthePCTOMtrajectory,thesimulationpredictsactuatorsaturation,whichre- ConstantinescuandCroft:SmoothandTime-OptimalTrajectoryPlanning Figure11.SimulatedcontrollertrackingperformanceforthePCTOM,quintic,andSPCTOMtrajectoriessultsnotonlyindecreasedtrackingperformance,butalsoinlongermotiontimeTableIIIThesimulationsshowsimilartrackingperfor-mancefortheSPCTOMtrajectorywithlowtorqueratesandthequintictrajectorygeneratedbasedontorqueandvelocitylimitsHowever,theSPCTOMtrajectorytakes15s,comparedto2sforthequinticQuintictrajectoriesarecompletelyspecifiedbytorqueandve-locitylimitsThereportedtrajectorytorquerateforthequintictrajectorydependsontheselimits Figure12.DesiredandsimulatedtorquesforthePC-TOMtrajectory Figure13.DesiredandsimulatedtorquesfortheSPCTOMtrajectoryexample2Ðtorqueratelimitsof100Nm5.EXPERIMENTSAlltheabovetrajectorieshavealsobeenimple-mentedontheSCORBOTERVIIintheIALatUBCTherobotiscontrolledbyaTMS320C32digitalsignalprocessingboard,interfacedwithtwoaxiscontrolcards,eachcapableofhandlingthreeaxesAnOpenArchitectureReal-TimeoperatingsystemORTSisusedintheimplemen-tationofthecontrolalgorithmandinreadingthepre-plannedtrajectoriesandfeedingthemtothe JournalofRoboticSystemsÐ2000 Figure14.DesiredandsimulatedtorquesfortheSPCTOMtrajectoryexample3Ðtorqueratelimitsof10NmcontrolloopatthecontrollerfrequencyTheaxiscontrolcardsandtheORTSweredevelopedbytheManufacturingAutomationLaboratoryMAL,Forthepurposeoftheexperimentsreportedhere,onlythepositionaldegreesoffreedomoftherobotareconsidered,thustherobotistreatedasa3-dofelbowmanipulatorwiththekinematicanddynamicparametersgiveninTableIAtuned,discretePIDalgorithmisusedtoprovidethecon-trollawThissetupsimulatestypicalconditionsinindustry,wheretherobotisequippedwithaclosedarchitecturediscretePIDindependentjointcon-TheresultsoftheexperimentsareplottedinFigures1619,andsummarizedinTableIVTheseexperimentalresultssupportthesimula-tionresultsNamely,forhightorqueratelimits,thecontrollercannotkeeptheend-effectoronthepathFigures16,and17showthattrajectorieswithhigh Figure15.Desiredandsimulatedtorquesforthequintictorqueratesresultinincreasedtrackingerrors,which,inturn,activatethecontroller,saturatingtheWheneverthishappens,theend-effectorleavesthepathSuchatrajectoryisaninfeasibleForthecaseoftheSCORBOTERVIImanipulator,torqueratelimitslessthanoneorderofmagnitudehigherthantheactuatortorquelimitsarerequiredtoensurethattheend-effectorfollowstheplannedpathWhilethisresultismorerestric-tiveforthetorqueratelimitsthanpredictedbythesimulations,itisnottotallyunexpectedDuetothelargeerrorsinvolvedinmodellingthesystem,onewouldexpectthatthesimulationresultswouldoverestimatethesystemcapabilitiesTheexperimentalperformanceoftheSPCTOMtrajectorycorrespondingtothelowtorqueratelim-its,i,10Nms,issimilartoitssimulatedperfor-Thus,whilebeingtrackedbythecontrollerwithsimilaraccuracyandeffortasthequintictra-TableIII.SimulatedresultsforthePCTOM,SPCTOM,andquintictrajectories trackingerrorTorquerateMotiontracking limitstimeerrorjoint1Joint2joint3wxwxwxwxwxwxTrajectoryNmsscm 901981540310SPCTOM21000741401120260SPCTOM3101500640530120Quintic72000510420100 ConstantinescuandCroft:SmoothandTime-OptimalTrajectoryPlanning Figure16.ExperimentalresultsforthePCTOMtrajec-toryimplementedontheSCORBOTERVII Figure17.ExperimentalresultsfortheSPCTOMtrajec-toryexample2Ðtorqueratelimitsof100Nmsimple-mentedontheSCORBOTERVII JournalofRoboticSystemsÐ2000 Figure18.ExperimentalresultsfortheSPCTOMtrajec-toryexample3Ðtorqueratelimitsof10Nmsimple-mentedontheSCORBOTERVII Figure19.ExperimentalresultsforthequintictrajectoryimplementedontheSCORBOTERVII ConstantinescuandCroft:SmoothandTime-OptimalTrajectoryPlanningTableIV.ExperimentalresultsforthePCTOM,SPCTOM,andquintictrajectoriesunderindependentjointPIDcontrol trackingerrorTorquerateMotiontracking limitstimeerrorjoint1joint2joint3wxwxwxwxwxwxTrajectoryNmsscm 014017115SPCTOM21004012515614SPCTOM3101Quintic72 jectory,itresultsinreducedmotiontime1comparedto2sThisindicatesthattorqueratelimitsarepreferablewhendeterminingsmoothtimeoptimalmotionsoverglobalvelocityandaccelera-tionlimitsExperimentswerealsocarriedoutusingpro-portional-derivativePDplusgravitycompensa-tioncontrolTheresultsaresummarizedinTableVandshowthesamecorrelationbetweenthetrackingaccuracyandthetorqueratesalongthetrajectory6.CONCLUSIONSAmethodhasbeenpresentedfordeterminingsmoothandtime-optimalpath-constrainedtrajecto-riesforroboticmanipulatorsThedynamicsofthemanipulatortogetherwithlimitsontheactuatortorquesandtorqueratesareconsideredAbasetrajectoryisconstructedinthephaseplaneusingparametrizedcubicsplinesandasetofinitial,final,andknot-pointconditionsderivedfromPC-TOMwithouttorqueratelimitsThus,theoptimalmotionisobtainedthroughanoptimizationofthisbasetrajectory,subjecttoactuatortorqueandtorqueratelimitsInplanningsimulations,thetrajectorysmooth-nesshasanegativeimpactonthemotiontime,lowertorqueratelimitsresultinginhighermotionHowever,bothcontrollersimulationsandex-perimentshaveshownthat,inpractice,trajectorysmoothnesshasapositiveeffectonboththetrack-ingperformanceofthecontrollerandtheactualmotiontimeMoreover,asmoothlyplannedtra-jectorycancompensateforapoorlymodeledrobotsystem,whichisoftenthecaseinindustrialComparedtoaquinticpolynomialtrajectorywithvelocityandaccelerationlimits,theSPCTOMtrajectoryresultsinafastermotionforsimilartrack-ingperformanceThus,torqueratelimitsareprefer-ablewhenimposingadesireddegreeoftrajectoryTableV.ExperimentalresultsforthePCTOM,SPCTOM,andquintictrajectoriesunderPDplusgravitycompensationcontrol trackingerrorTorquerateMotiontracking limitstimeerrorjoint1joint2joint3wxwxwxwxwxwxTrajectoryNmsscm 030617715SPCTOM21006026315SPCTOM3101Quintic72 JournalofRoboticSystemsÐ2000smoothnessoverquinticpolynomials,sincetheyarenotposture-dependentAPPENDIXAIntheflexibletolerancemethodFTM,theopti-mizationproblem,Subjectto.....issolvedasthefollowingsimplerequivalentprob-lemwithonlyoneconstraint:min:subjectto:isthevalueoftheflexibletolerancecriterionatthstepoftheoptimizationanditalsoservesasacriterionfortheterminationofthesearch,andisapositivefunctionalofalltheequalityandinequalityconstraintsoftheoriginalproblemcostfunctionandtheequalityandinequalityconstraintsinA3maybelinearandornonlinearfunctionsofthevariablesinThevalueofthecostfunctionisimprovedbyusinginformationprovidedbyfeasiblepoints,aswellascertainnonfeasiblepointscalledfeasiblepointsThenear-feasibilitylimitsaremademorerestrictiveasthesearchad-vances,untilinthelimitonlyfeasiblepointsare..InA4below,isusedasameasureoftheconstraintviolation,whileisselectedasaposi-tivedecreasingfunctionofthepointsintheSPCTOM,..suchthatiiii0otherwise,aconstantThetolerancecriterionisusedtoclassifypointsAtthethstepoftheoptimization,apointissaidtobe:Feasible,ifNear-feasible,if0Nonfeasible,ifAsmallvalueofimpliesthatisrela-tivelyneartothefeasibleregion,andalargevalueimpliesthatisrelativelyfarfromthefeasibleregionOnatransitionfrom,themoveissaidtobefeasibleif0,andnon-feasibleifTheFTMentailstwoindependentoptimiza-tions:anouterminimizationofthecostfunctionandaninnerminimizationoftheviolationof..whenevertheminimizationofyieldsaninfeasiblepointTheouteroptimizationofthemotiontimeisimplementedinthispaperusingtheflexiblepolyhedronmethodFPMTheFPMisasearchindimensionswherethepolyhedronchangesshapetomatchthechangingshapeoftheInthevicinityofaminimumthepolyhe-dronshrinks,surroundingtheminimumReplace-mentofaninfeasiblepointwithafeasibleornear-feasibleoneisdonethroughalinesearchusingintervalhalvingThecomputationalrequirementsofthealgo-rithmaresimilartothoseofanonlinearoptimiza-Inthiscase,themainoverheadisrepresentedbytheevaluationoftheconstraintviolationmea-sureinEqThisoverheadisreducedbyevalu-atingtheactuatortorqueandtorqueratelimitsviolationdirectlyratherthanthestateandcontrolconstraintviolationThisworkhasbeensupportedbytheNationalSci-encesandEngineeringResearchCouncilofCanadaandtheFacultyofGraduateStudiesatUBChelpfulsuggestionsofProfessorBBenhabiboftheDepartmentofMechanicalandIndustrialEngineer-ingoftheUniversityofTorontoisgreatlyappreci-Also,theassistanceofProfessorYAltintasandthegraduatestudentsintheMAL,UBC,duringtheexperimentalpartofthisworkisgratefullyacknowl-Bobrow,SDubowsky,andJGibson,Time-opti-malcontrolofroboticmanipulatorsalongspecifiedIntJRoboticsRes41985,3PfeifferandRJohanni,Aconceptformanipulatortrajectoryplanning,IEEEJRoboticsAutomatRA-31987,115 ConstantinescuandCroft:SmoothandTime-OptimalTrajectoryPlanningShillerandHLu,Computationofpathcon-strainedtimeoptimalmotionswithdynamicsingular-ities,ASMEJDynSystMeasControl1141992,ChenandADesrochers,Structureofminimum-timecontrollawforroboticmanipulatorswithcon-strainedpaths,inIEEEIntConfRobotAutomat1989,Li,RLongman,VSchultz,andHImplementingtimeoptimalrobotmaneuversusingrealisticactuatorconstraintsandlearningcontrol,As-trodynamics1998,RA-31998,115Shiller,Time-energyoptimalcontrolofarticulatedsystemswithgeometricpathconstraints,inIEEEIntConfRobotAutomat1994,ppLin,PChang,andJLuh,FormulationandoptimizationofcubicpolynomialjointtrajectoriesforindustrialrobotsIEEETransAutomatContrAC-281983,1066TarkiainenandZShiller,Timeoptimalmotionsofmanipulatorswithactuatordynamics,inIEEEIntConfRobotAutom1993,ppShiller,Onsingulartime-optimalcontrolalongspecifiedpaths,IEEETransRobotAutomat101994,ShillerandSDubowsky,Timeoptimalpathplan-ningforroboticmanipulatorswithobstacles,actuator,gripper,andpayloadconstraints,IntJRobotRes81989,3ShinandNMcKay,Adynamicprogrammingapproachtotrajectoryplanningofroboticmanipula-tors,IEEETransAutomatContrAC-311986,491BockandKPlitt,Amultipleshootingalgo-rithmfordirectsolutionofoptimalcontrolproblems,in9thIFACWorldCongress1984,ppLeitman,Thecalculusofvariationsandoptimalcontrol,PlenumPress,NewYorkandLondon,1981Constantinescu,Smoothtimeoptimaltrajectoryplanningforindustrialmanipulators,Master'sthesis,UniversityofBritishColumbia,1998Himmelblau,AppliedNonlinearProgramming,McGraw-Hill,1989TheMathWorks,Natwik,Massachusetts,MatlabUser'sGuide,1995TheMathWorks,Natwik,Massachusetts,SimulinkToolboxUser'sGuide,1995ErolandYAltintas,Openarchitecturemodulartoolkitformotionandprocesscontrol,inASMEInternationalMechanicalEngineeringCongressandExposition,ASMEPublicationMED,Dallas,Texas,1997,ppNelderandRMead,Asimplexmethodforfunctionminimization,ComputJ41964,308