反向运动学、雅克比矩阵与轨迹规划

TheCityCollegeofNewYork1Jizho[e-mail]bianMatrixTrajectoryPlanningIntroductiontoROBOTICSTheCityCollegeofNewYork2Outline•Review–KinematicsModel–InverseKinematics•Example•JacobianMatrix–Singularity•TrajectoryPlanningTheCityCollegeofNewYork3Review•Stepstoderivekinematicsmodel:–AssignD-Hcoordinatesframes–Findlinkparameters–Transformationmatricesofadjacentjoints–Calculatekinematicsmatrix–Whennecessary,EuleranglerepresentationTheCityCollegeofNewYork4Denavit-HartenbergConvention•Numberthejointsfrom1tonstartingwiththebaseandendingwiththeend-effector.•Establishthebasecoordinatesystem.Establisharight-handedorthonormalcoordinatesystematthesupportingbasewithaxislyingalongtheaxisofmotionofjoint1.•Establishjointaxis.AligntheZiwiththeaxisofmotion(rotaryorsliding)ofjointi+1.•Establishtheoriginoftheithcoordinatesystem.LocatetheoriginoftheithcoordinateattheintersectionoftheZi&Zi-1orattheintersectionofcommonnormalbetweentheZi&Zi-1axesandtheZiaxis.•EstablishXiaxis.EstablishoralongthecommonnormalbetweentheZi-1&Ziaxeswhentheyareparallel.•EstablishYiaxis.Assigntocompletetheright-handedcoordinatesystem.•Findthelinkandjointparameters),,(000ZYXiiiiiZZZZX11/)(iiiiiXZXZY/)(0ZTheCityCollegeofNewYork5Review•LinkandJointParameters–Jointangle:theangleofrotationfromtheXi-1axistotheXiaxisabouttheZi-1axis.Itisthejointvariableifjointiisrotary.–Jointdistance:thedistancefromtheoriginofthe(i-1)coordinatesystemtotheintersectionoftheZi-1axisandtheXiaxisalongtheZi-1axis.Itisthejointvariableifjointiisprismatic.–Linklength:thedistancefromtheintersectionoftheZi-1axisandtheXiaxistotheoriginoftheithcoordinatesystemalongtheXiaxis.–Linktwistangle:theangleofrotationfromtheZi-1axistotheZiaxisabouttheXiaxis.iidiaiTheCityCollegeofNewYork6Review•D-Htransformationmatrixforadjacentcoordinateframes,iandi-1.–Thepositionandorientationofthei-thframecoordinatecanbeexpressedinthe(i-1)thframebythefollowing4successiveelementarytransformations:10000),(),(),(),(111iiiiiiiiiiiiiiiiiiiiiiiiiiidCSSaCSCCSCaSSSCCxRaxTzRdzTTSourcecoordinateReferenceCoordinateTheCityCollegeofNewYork7Review•KinematicsEquations–chainproductofsuccessivecoordinatetransformationmatricesof–specifiesthelocationofthen-thcoordinateframew.r.t.thebasecoordinatesystem100010000121100nnnnnnPasnPRTTTTiiT1nT0OrientationmatrixPositionvectorTheCityCollegeofNewYork8Review•ForwardKinematicszyxppp6543211000zzzzyyyyxxxxpasnpasnpasnT•KinematicsTransformationMatrixyandxforyandxforyandxforyandxforxya0909018018090900),(2tanWhyuseEuleranglerepresentation?),(2tanxyaWhatis?TheCityCollegeofNewYork9Review10000000CCSCSSCCSSSC,,,xyzRRRT,,1,xyzRRTR100001000000CSSC100000001000CSSC100000000001CSSC1000000zzzyyyxxxasnasnasn(EquationA)•Yaw-Pitch-RollRepresentation1000000XXXXnaCaSsCsSnCnSXXXXnSnCzyxyxyxyxTheCityCollegeofNewYork10Review0cossinyxnnsincossincoszyxnnnsincossincoscossinyxyxaass•CompareLHSandRHSofEquationA,wehave:),(2tanxynna)sincos,(2tanyzznnna)cossin,cos(sin2tanyxyxssaaaTheCityCollegeofNewYork11InverseKinematics654321655443322110601000TTTTTTpasnpasnpasnTzzzzyyyyxxxx•TransformationMatrixSpecialcasesmaketheclosed-formarmsolutionpossible:1.Threeadjacentjointaxesintersecting(PUMA,Stanford)2.Threeadjacentjointaxesparalleltooneanother(MINIMOVER)Robotdependent,SolutionsnotuniqueSystematicclosed-formsolutioningeneralisnotavailableTheCityCollegeofNewYork12Example•SolvingtheinversekinematicsofStanfordarm655443322110601000TTTTTTpasnpasnpasnTzzzzyyyyxxxx61655443322160110)(TTTTTTTT1000111161yxzyxpCpSXXXpXXXpSpCXXXT10001.03232XXXdCXXXdSXXXTheCityCollegeofNewYork13Example•SolvingtheinversekinematicsofStanfordarm3211sinsincosdppyx1.0cos11yxppSin32cosdpzEquation(1)Equation(2)Equation(3)InEqu.(1),let)(2tan,,sin,cos22xyyxyxppapprrprp21111)/1.0(1)cos(1.0)sin(1.0cossincossinrrr)1.01.0(2tan)(2tan221rappaxy)sincos(2tan112zyxpppa23coszpdTheCityCollegeofNewYork14Example•SolvingtheinversekinematicsofStanfordarm))((2tan2112114zyxyxaSaSaCCaCaSa10000000)()()()(55655460110121132143XXCXXSXXTTTTTTT0)(])([11421124yxzyxaCaSCaSaSaCCS))()(])((21125114211245zyxyxzyxaCaSaCSCaCaSSaSaSaCCCSFromterm(3,3)Fromterm(1,3),(2,3))(2tan555CSaTheCityCollegeofNewYork15Example•SolvingtheinversekinematicsofStanfordarm)(])([])([)}(])([{114211246211251142112456yxzyxzyxyxzyxsCsSCsSsSsCCSCsCsSsCSSsCsSSsSsSsCCCCS100001000000)()()()