Se ha denunciado esta presentación.
Se está descargando tu SlideShare. ×
Robot Analysis
Obtaining InverseKinematic Solutions with
Numerical Methods and Algorithms
Before robots,humanswere limitedinfunctionalitydue toonlyhavingtwohands,eightfingersandtwo
opposable thumbs. Wi...
Table of Contents
Abstract 1
Contents 2
Introduction 3
Theory 3
Implementation 4
Results 18
Conclusion 18
List of Figure...
Cargando en…3

Eche un vistazo a continuación

1 de 19 Anuncio

Más Contenido Relacionado

Similares a Final Project (20)


Final Project

  1. 1. Robot Analysis Obtaining InverseKinematic Solutions with Numerical Methods and Algorithms Thisdocumentcontainsinformationregardingthe processes involvedinsolvingthe inverse kinematicsproblemforrobots. Chad Ryan Weiss 4/25/2016
  2. 2. 1 Abstract Before robots,humanswere limitedinfunctionalitydue toonlyhavingtwohands,eightfingersandtwo opposable thumbs. Withthis,surgeonshadtorelyon theirnervestoperformthe mostdifficult proceduresknowntoman. One twitchand that couldspell life ordeathforthe patient. Thisisjustone example whererobotshave hadanunbelievableimpactonsociety. Theyare usedtosave livesaswell as to build,create anddestroy. Inan attemptto recreate a tool that isas dexterousasthe humanhand, that isas smart as the human brainand as strongas the hardesttitaniumalloy,scientists, mathematicians,engineersandtechnologistsfromall overthe worldhave dedicatedtheirlivestothe subjectthatis roboticsandto its problems. The major problemthatthispiece of workseekstoaddress isthe inverse kinematicsproblem.
  3. 3. 2 Table of Contents Abstract 1 Contents 2 Introduction 3 Theory 3 Implementation 4 Results 18 Conclusion 18 List of Figures Figure 1: 6 DOF Robot 3 Figure 2: Determiningthe Lengthof Link0 4 Figure 3: Determiningthe Lengthof the SecondLink(Link1) 5 Figure 4: Determiningthe Lengthof Link2 6 Figure 5: Base Revolute JointinAction 7 Figure 6: Initial Settingof Robot 7 Figure 7: LinkAnalysisof Robot 8 Figure 8: JointAnalysisof Robot 9 Figure 9: Jointand LinkAnalysisof Robot 10 Figure 10: OffsetAnalysisof Robot 11 Figure 11: RobotLengthLowerBound 14 Figure 12: RobotLengthUpper Bound 15 Figure 13: RobotRadiusUpperBound 15
  4. 4. 3 Introduction The objective of this documentistoprovide insightof the processesrequiredto developanalgorithm that will calculate the inverse kinematicssolution of a6 degree of freedom(DOF)robot. Ourproblem will onlyconsiderthe firstthree jointsandlinksof the robotwhen obtainingthe inversekinematics solution. Figure 1shows the robotthat we will be workingwith: Theory Like the hammeror the axe,the robot isa tool designedbyman(engineers) toserve a specificpurpose. In orderto designamultipurpose robotlikethe one showninFigure 1,one must considerthe kinematic engineeringrequirements of the robot. Inhighschool physicswe learnedaboutthe basickinematic equations, indynamicswe learnedhowtoanalyze the motionof objectsinaninterconnectedsystem,in roboticswe put thisknowledgetoworkusingforwardandinverse kinematicstoanalyze anddesign robotsso that theycan fulfill theirpurpose. The conceptof forwardversusinverse kinematicsis very importantandcannot be overlookedwhenanalyzingordesigningrobots. Forwardkinematicsissimilartowhatwe learnedindynamics. If we rotate the base X degrees,then rotate the secondjointYdegreesandthe thirdjointZ degrees,wherewillthe robot tipbe located? Itis the studyof howobjectsmove togetherinaninterconnectedsystem. Inverse kinematicsonthe other hand,is the same thingbutwitha differentapproach. The scenariobeing,if Iwantthe robottip to be locatedat the coordinate (A,B,C) withrespecttothe universal (fixed)axis,whatjointrotations (X,Y,Z) wouldbe requiredtogetto thispointinspace? Figure 1: 6 DOF Robot
  5. 5. 4 In theory,itispossible tohave nosolution,one solution,more thanone solutionorinfinitelymany solutions tothistype of problem. Inpractice however, itisnotpossible tohave infinitelymanysolutions due to the constraintsof the robotsuch as the resolutionof the steppermotor,actuatorordriveras well asthe lengthsof the links,etc. Thisprocessof discoveringthe necessaryjointmovementsinorder to reach a specificenddestinationisthe inverse kinematicmethodforsolvingrobotequationsandwe will use thistheoryin thisreport. Implementation Basically,we will be assemblingaMatLab computerprogramthat will tryto get the jointanglesof the robot to converge (the robotend-effector)withthe cylindrical coordinatesof the Cartesianinputs providedbythe user. First,itis importanttoknowthe dimensionsof the robot. Hence,we will beginby calculatingthe lengthsof the links. Refertothe figure below tosee how we determine the lengthof the firstlink. If you lookat Figure 2, youwill see thatthe lengthof the robot base can be determinedbyliningupthe end-effectorwiththe jointaxisof rotation. Since the software wasdesignedtotell usthe coordinatesof the end-effector,liningthe end-effectorupwiththe endpointof link0(the base) will give usthe length of link0. We can determinethe lengthbyobservingthe heightatwhichthe end-effectorhasbeen placed. Hence we can confidentlysaythatthe lengthof link0 is approximately285.277 mm. % Define Robot Variables L0 = 285; % Base length of robot in mm Figure 2: Determining the Length of Link 0
  6. 6. 5 Nowwe will move ontolink1. If you refertothe figure below youwill see how we determinedthe lengthof the linkdirectlyconnectedtothe base link(link1). Figure 3: Determining the Length of the Second Link (Link 1) If you lookcloselyatFigure 3, youwill see thatwe usedthe same methodindeterminingthe lengthof the firstlink(i.e.the base) todetermine the secondlink. We linedup the end-effectorwiththe axisof rotationof the thirdjoint,whichconstitutesthe endof the secondlinkandbeginningof the thirdlink, and we lookedatthe total heightof the robotend-effector. Thistime the Zcoordinate reads545.175 mm. Thislengthisthe lengthof the firstand secondlinkcombined. Tofindthe lengthof the second linkwe simplysubtractthe lengthof the firstlink,whichwe foundpreviously. Sothe lengthof the secondlinkcomesoutto be: 𝐿𝑙𝑖𝑛𝑘1 = 𝐻𝑡𝑜𝑡𝑎𝑙 − 𝐿𝑙𝑖𝑛𝑘0 Equation1 givesusa link1 lengthof 545.175 mm minus285.277 mm or approximately,259.898 mm. We make sure to define thislinkinMatLabwiththe followingcode: % Define Robot Variables L1 = 260; % Link 1 length in mm Finally,todetermine the lastlink,take alookatthe figure below. (1)
  7. 7. 6 Lookingat Figure 4, we see thatthe total heighthasnow reached1035.785 mm at full extension. Using the same methodsas before we candetermine the lengthof the lastlink(link2) byusingthe following equation: 𝐿𝑙𝑖𝑛𝑘2 = 𝐻𝑡𝑜𝑡𝑎𝑙 − 𝐿𝑙𝑖𝑛𝑘0 − 𝐿𝑙𝑖𝑛𝑘1 UsingEquation2, we determinethe lengthof link2to be 1035.785 mm minus545.175 mm or 490.61 mm. We thenrecord thisinformationandstore itinthe MatLab variable C2. % Define Robot Variables L2 = 490; % Link 2 length in mm Nowthat we have determinedthe lengthsof the linksof ourrobotwe can begintoanalyze the robotto come up witha seriesof equationsthatwe canuse to relate the jointanglestothe coordinate systemof choice. To choose the bestcoordinate system,letusconsiderwhattype of robotwe are dealingwith. There are three joints,the base,the secondlinkandthe third. The base jointrotatesaboutthe z-axisata sweep of 165 to -165 degrees.RefertoFigure 5below tosee the base revolute jointinaction. Figure 4: Determining the Length of Link 2 (2)
  8. 8. 7 Basedon thisone joint,itisincrediblysimple tomake thisproblemacylindrical one because the base jointcan act as a fixedangle inourequations. Thisallowsustoskipthe stepof writingan equation for the firstjoint,because inacylindrical coordinate system theta_0equalsalphaor(𝜃0 = 𝛼). Despite this fact, we still have todevelopthe equationsforoursecondandthirdjoints. Todo this,we will analyze the robot some more. Figure 6 shows the initial settingof the robot. The firstthree jointanglesare settozerodegreesandthe Cartesiancoordinatesof the end-effectorare (475.785mm, -82.198mm, 628.840mm). See the figure belowforconfirmationof thisfact. Figure 5: Base Revolute Joint in Action Figure 6: Initial Setting of Robot
  9. 9. 8 Figure 7: Link Analysis of Robot !!! ImportantNote:(RefertoFigure 7) Althoughthe end-effectorcoordinatesshow (475.785, -82.198, 628.840) we can assume thatthe end- effectorcoordinatesof ourrobotreside atthe point(475.785, 0, 628.840). Thisassumptionismade because of our hypothetical situation,the robotendlink(i.e.link2or L_2) can be representedasa long straightarm to the endpoint. In all practicality,thisrobothasan end-effectortool thathasa tip that jutsout intothe y-plane -82.198 mm. We can disregardthisfactfor the purpose of thisanalysis. End Note !!! Nowthat we have takena lookat the robot,we will convertthe Cartesiancoordinatesof the end- effectortoa setof Cylindrical coordinates. Todo this,we mustapplythe followingequations: 𝑟 = √𝑥2 + 𝑦2 𝑙 = 𝑧 The variable r representsthe radiusof the cylinderwhile the variablel representsthe heightof the cylinder. Ina Cartesiancoordinate system,the coordinatesare representedbythe variablesx,yandz whichresemble the length,widthand heightordepthof the three-dimensional object. The cylindrical (3) (4)
  10. 10. 9 coordinate systemisathree dimensional coordinatesystemaswellbutitiswrittenina slightlydifferent format. Cylindrical coordinatesare writteninthe form(radius, alpha, length). Alphaisthe base angle of the cylinder(See Figure 5) andthe l and r variableswere alreadydiscussed. Lookingback at Figure 7 and includingourfirstassumption,the end-effectorCartesiancoordinatesare (475.785, 0, 628.840). In Cylindrical,thissetof pointsbecomes(475.785, 0, 628.840). Eventhoughthey may lookthe same,theyare not! The middle coordinate hasswitchedfromameasure of distance toa measure of degrees. Withthisknowledge,we cannowsetsome initial conditionsthatwill helpuswrite the equationsthat relate the jointanglesof ourrobot to the cylindrical coordinatevariables. Inorderto determine the initial conditions,we have toconducta jointanalysisof the robot. See the figure belowforthisstep. Figure 8: Joint Analysis of Robot Upon lookingatFigure 8, itis immediatelyrecognizedthatTheta_0is Alphaor(𝜃0 = 𝛼). Theta_1 and Theta_2 however,are notso easy. The length and radiuscoordinatesof theend-effectoraredependent on Theta_1 and Theta_2. See the nextfigure fora betterdepictionof thisrelationship.
  11. 11. 10 Figure 9: Joint and Link Analysis of Robot Rememberingthatthisinitial setupof the robotiswhenall of our jointanglesare equal tozero. For that reason itis clearto note that the secondlink(link1) contributestothe lengththe mostwhenits correspondingjoint(i.e.jointangle theta_1) isatzerodegrees. The base linkwill alwayshave afixed lengthinthe L directionof the cylindrical coordinate systembutthe secondlinkwill contribute mostto thisdirectionwhenithasa jointangle theta_1equal to zero. From thiswe can make the following initial conditionstatementsaboutlink1. Whentheta_1 equalszero,the lengthof the robotlink (link 1) isat itsmaximumvalue (inthe L direction). Onthe otherhand,whentheta_1equals90 degrees,the lengthof the robotlink(link1- in the L direction) iszero. Thissoundslike the cosinefunction. It issafe to say that 𝐿1_𝑚𝑎𝑥 = 𝐿1 ∗ cos(𝜃1 ) whentheta_1 isequal tozero. Furthermore,we cansay thatthe lengthof the thirdlink(link2) inthe L directionisat a minimumwhen theta_2 isequal to zero. If youreferto Figure 9,you can clearlysee thatthe lengthof the thirdlinkis greatestinthe R directionwhentheta_2equalszero. Itturns outthat thislinkresemblesthe sine
  12. 12. 11 functionandit issafe to say that 𝐿2_𝑚𝑎𝑥 = 𝐿2 ∗ sin(𝜃2) whentheta_2equals90 degrees. Inall actuality,the lengthof the thirdlink(link2) isgreatestinthe L directionwhen theta_2is-90 degrees, (accordingto the robot andsoftware) hence thislinkisbestrepresentedbyaninvertedsinefunction. Here are the initial conditionequations: For the radial componentsof link1 𝐿1_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿1_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑖𝑛) 𝑤ℎ𝑒𝑛 𝜃1 = 0, sin(0°) = 0 𝐿1_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿1_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑎𝑥) 𝑤ℎ𝑒𝑛 𝜃1 = ±90, abs[sin(±90°)] = 1 For the height(orlength) componentsof link1 𝐿1_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿1_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑖𝑛) 𝑤ℎ𝑒𝑛 𝜃1 = ±90, cos(±90°) = 0 𝐿1_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿1_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑎𝑥) 𝑤ℎ𝑒𝑛 𝜃1 = 0, cos(0°) = 1 * It isactuallyquite possible tohave alengththatcontributestolessthanzerounitsin the L direction because the robotcan reach downwards,hence the minimumRandL componentsaren’texactlymet whentheta_1hits 0 and 90 degreesrespectively. Justthinkhow the absolute minimumof the sine and cosine functionsare actually -1and notzero andpicture each of the linksbendingbelow itsaxisof rotation. There can be a negative lengthasthe robotreachesbelow the base jointaxisof rotation,but there cannotbe a negative radiusof the cylinder. Thatiswhy we put the absolute value function aroundthe sine functionforthe radial componentof link1. Withthese thoughtsinmindthese equationsstill provideaveryaccurate if not perfectmodel of the robotitself. For the radial componentsof link2 𝐿2_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿2_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑖𝑛) 𝑤ℎ𝑒𝑛 𝜃2 = ±90, cos(±90°) = 0 𝐿2_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿2_(𝑟−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑎𝑥) 𝑤ℎ𝑒𝑛 𝜃2 = 0, cos(0°) = 1 For the height(orlength) componentsof link 2 𝐿2_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿2_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑖𝑛) 𝑤ℎ𝑒𝑛 𝜃2 = 0, sin(0°) = 0 𝐿2_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡) = 𝐿2_(𝑙−𝑐𝑜𝑚𝑝𝑜𝑛𝑒𝑛𝑡_𝑚𝑎𝑥) 𝑤ℎ𝑒𝑛 𝜃2 = −90, −sin(−90°) = 1 * Please note thatthe L directioncomponentof the thirdlink(link2) isnotactuallyat a minimumat theta_2 equalszero. Like we saidearlier,the linkscancontribute tothe L directionina negative manner and therefore,the sinefunctionisstill anaccurate wayto resemble thisparticularlink. The radial componentof the thirdlink(link2) isgreatestwhentheta_2equalszeroandfor that reason,the cosine functionis usedtoresemble thislink inthe Rdirection. From these initialconditions, we come upwiththe followingequations: (NextPage)
  13. 13. 12 𝒍 = 𝑳 𝟎 + 𝑳 𝟏 ∗ 𝐜𝐨𝐬( 𝜽 𝟏)− 𝑳 𝟐 ∗ 𝐬𝐢𝐧(𝜽 𝟐 + 𝜽 𝟏) 𝒓 = 𝑳 𝟏 ∗ 𝐚𝐛𝐬[𝐬𝐢𝐧( 𝜽 𝟏)] + 𝑳 𝟐 ∗ 𝐜𝐨 𝐬( 𝜽 𝟐 + 𝜽 𝟏) 𝜶 = 𝜽 𝟎 It isalso importanttonote that the robot has jointangle movement restrictions. −165° ≤ 𝜃0 ≤ 165° −110° ≤ 𝜃1 ≤ 110° −90° ≤ 𝜃2 ≤ 70° Takingintoaccount all of thisinformation,we will use the knowledgeacquiredfromthisanalysisto create an algorithmthatutilizesthe Gauss-Seidel methodforsolvingsimultaneouslinear equationsto come up withan inverse kinematicsolutionforthisparticularrobot. Toaccomplishthis,we mustfirst rewrite the equationsintermsof theta_1andtheta_2. We can simplydefinetheta_0asalpha inour MatLab computerprogram. See the code below. % Convert to Robot theta0 = cylindrical_alpha; theta1 = real(acos((cylindrical_l+L2*sin(theta2)-L0)/L1)); theta2 = real(acos((cylindrical_r - L1*sin(theta1))/L2)); Problem! There isanoffsetthatneedstobe takenintoaccount. See the figure below. Figure 10: Offset Analysis of Robot Figure 10 shows that when theta_2 equalszero,the offset of about 68.840 mmextendsonly in the L direction. Since the offsetis strictly dependent on theta_2, you will see a modified equation for L including another cosine of theta_2 with some coefficient in front. Furthermore, when theta_2 equals -90 degrees, the offset is applied strictly in the R direction. Therefore, the R equationwill have an added sine function for theta_2 with a coefficientequal in magnitude to the total offset displacement. (5) (6) (7)
  14. 14. 13 The modifiedequationsare: 𝒍 = 𝑳 𝟎 + 𝑳 𝟏 ∗ 𝐜𝐨𝐬( 𝜽 𝟏) − 𝑳 𝟐 ∗ 𝐬𝐢 𝐧( 𝜽 𝟐 + 𝜽 𝟏)+ 𝑪 𝟎 ∗ 𝒄𝒐𝒔(𝜽 𝟐 + 𝜽 𝟏) 𝒓 = 𝑳 𝟏 ∗ 𝐚𝐛𝐬[𝐬𝐢𝐧( 𝜽 𝟏)] + 𝑳 𝟐 ∗ 𝐜𝐨 𝐬( 𝜽 𝟐 + 𝜽 𝟏)+ 𝑪 𝟎 ∗ 𝒂𝒃𝒔[𝒔𝒊𝒏( 𝜽 𝟐 + 𝜽 𝟏)] 𝜶 = 𝜽 𝟎 C_0 isthe offsetconstantandisequal to68.840 mm. We will definethisinourMatLab computer program as well asthe three equationsrewrittenintermsof theta_0,theta_1and theta_2 as follows. % Define Robot Variables C0 = 69; % Offset in units mm % Convert to Robot theta0 = cylindrical_alpha; theta1 = real(acos((cylindrical_l+L2*sin(theta2+theta1)-L0- C0*cos(theta2))/L1)); theta2 = real(acos((cylindrical_r-L1*abs(sin(theta1))- C0*abs(sin(theta2)))/L2)); Withthese equations,we have finishedanalyzingourrobot. Withthisknowledge we will proceedto buildthe algorithmthatwill findthe inverse kinematicsolutions(jointangles)fora givenCartesian coordinate input. Refertothe code below fora holisticoverviewof the algorithm. 1) First,we ask the userto enterthe desiredcoordinates % Acquire Cartesian Coordinates x = input('Enter x-coordinate: '); y = input('Enter y-coordinate: '); z = input('Enter z-coordinate: '); 2) Then,we convertto cylindrical usingtheseequations 𝛼 = 𝑡𝑎𝑛−1( 𝑦 𝑥 ) 𝑟 = √𝑥2 + 𝑦2 𝑙 = 𝑧 % Convert to Cylindrical cylindrical_r = sqrt(x^2+y^2); cylindrical_l = z; * Whenconvertingthe Cartesiancoordinatestothe cylindrical alpha,itisimportanttomake sure alpha isin the rightquadrant,otherwise the angle will be off. We dothisbe settingsome conditional if statements. % Making Sure Alpha is in the Correct Quadrant if (x > 0 && y > 0) cylindrical_alpha = atan(y/x)*(180/pi); (8) (9) (10) (11) (12) (13)
  15. 15. 14 end if (x > 0 && y < 0) cylindrical_alpha = (atan(y/x))*(180/pi)+360; end if (x < 0 && y > 0) cylindrical_alpha = (atan(y/x))*(180/pi)+180; end if (x < 0 && y < 0) cylindrical_alpha = (atan(y/x)+pi)*(180/pi); end 3) Now,we establishourrobot’s jointangle movementrestrictionsusingthe following constraints: −165° ≤ 𝜃0 ≤ 165° −110° ≤ 𝜃1 ≤ 110° −90° ≤ 𝜃2 ≤ 70° % Base Joint Limitations if (cylindrical_alpha > 165 && cylindrical_alpha < 195) display('Alpha Out of Range'); break; % Return to top end % Robot Length Limitations if (cylindrical_l < -279 || cylindrical_l > 1035) display('Length Out of Range'); break; end % Robot Radius Limitations if (cylindrical_r > 745) display('Radius Out of Range'); break; end Figure 11: Robot Length Lower Bound
  16. 16. 15 Figure 12: Robot Length Upper Bound Figure 13: Robot Radius Upper Bound You can see fromFigures11 – 13 that the robothas height(length) andradial limitations. Thatiswhy we made sure to define these regionsasinaccessibleinourMatLab computerprogram. In reality, the robotlimitationsshould be examined moreclosely and defined moreconservatively. 4) Define ourconstants(i.e.the individual lengthsof the linksof ourrobotas well asanyoffsets) % Define Robot Variables C0 = 69; % Offset in units mm L0 = 228; % Base length of robot in mm L1 = 326; % Link 1 length in mm L2 = 490; % Link 2 length in mm theta0 = 0; theta1 = 0; theta2 = 0;
  17. 17. 16 5) Define ourrobot’sjointanglesintermsof the cylindrical coordinate systemwe chose % Convert to Robot theta0 = cylindrical_alpha; theta1 = real(acos((cylindrical_l+L2*sin(theta2)-L0-C0*cos(theta2))/L1)); theta2 = real(acos((cylindrical_r-L1*abs(sin(theta1))- C0*abs(sin(theta2)))/L2)); We use the real functionsothat the thetavaluesare strictlyreal and notimaginaryvalues. 6) We double checktoensure the coordinatesare withinthe jointangle limitationranges % Base Joint Limitations if (theta0 > 165 && theta0 < 195) display('Theta0 Out of Range'); break; % Return to top end % Second Joint Limitations if (theta1 > 110 && cylindrical_alpha < 250) display('Theta1 Out of Range'); break; % Return to top end % Base Joint Limitations if (theta2 > 70 && theta2 < 270) display('Theta2 Out of Range'); break; % Return to top end * If the code passesthispoint,thatmeansthe inputisvalidandwithinrange for the robot. 7) We nowapplythe iterative numerical methodforsolvingsimultaneouslinearequations Thispart of the code will make aninitial assumptionabouttheta_1then:  Calculate theta_2  Calculate the cylindrical coordinatesassociatedwiththe assumedtheta_1andnew theta_2  Compare the calculatedcylindrical coordinatestothe inputprovidedbythe user If the calculatedcylindrical coordinatesmeetthe maximumerrorcriteria(alsodefinedinthispartof the code) the program will stopandrecordthe jointangles. If it doesnotmeetthe maximumerrorcriteria it will:  Calculate theta_1basedon the new theta_2  Calculate the cylindrical coordinatesassociatedwith the new theta_1andoldtheta_2  Compare the calculatedcylindrical coordinatestothe inputprovidedbythe user  Repeat Each time the code repeats,itwill use the newlycalculatedthetavalue tocalculate anothernew theta value forthe opposite joint. Thismethod of solvinglinearequationsisalsoknownasthe Gauss-Seidel method. See the code below.
  18. 18. 17 i = 1; % Counter theta1 = 90; % Initial Assumption error_L = 1; error_R = 1; while (error_L || error_R > .05) if (mod(i,2)==0) % Calculate the other Joint Angle theta1 = real(acos((cylindrical_l+L2*sin(theta2)-L0-C0*cos(theta2))/L1)); theta1 = theta1*(180/pi); % Calculate the New Cylindrical Coordinates new_l = L0+L1*cos(theta1)-L2*sin(theta2)+C0*cos(theta2); new_r = L1*abs(sin(theta1))+L2*cos(theta2)+C0*abs(sin(theta2)); % Calculate the Difference error_L = abs(cylindrical_l - new_l); error_R = abs(cylindrical_r - new_r); % Increment Count i = i+1; else % Calculate the Joint Angles theta2 = real(acos((cylindrical_r-L1*abs(sin(theta1))- C0*abs(sin(theta2)))/L2)); theta2 = theta2*(180/pi); % Calculate the New Cylindrical Coordinates new_l = L0+L1*cos(theta1)-L2*sin(theta2)+C0*cos(theta2); new_r = L1*abs(sin(theta1))+L2*cos(theta2)+C0*abs(sin(theta2)); % Calculate the Difference error_L = abs(cylindrical_l - new_l); error_R = abs(cylindrical_r - new_r); % Increment Count i = i+1; end % Terminate Loop After 500000 Counts if i > 500000 break; end end The way thisloopiteratesbetweencalculatingtheta_1andtheta_2 isby usingthe modulusoperator. If the countervariable i isdivisible bytwo,thenitwill calculate theta_1andproceed,otherwise itwill calculate theta_2and proceed. Eitherway,the code was setup to iterate throughbothequationsuntil the maximumnumberof iterationshasbeenreachedorthe maximumerrorcriteriahasbeenmet.
  19. 19. 18 Results UsingEquations8 – 10 (see below),we were able todevelopalinearsystemof equationsrelatingthe cylindrical coordinatestothe robotjointangle movements. 𝒍 = 𝑳 𝟎 + 𝑳 𝟏 ∗ 𝐜𝐨𝐬( 𝜽 𝟏)− 𝑳 𝟐 ∗ 𝐬𝐢 𝐧( 𝜽 𝟐)+ 𝑪 𝟎 ∗ 𝒄𝒐𝒔(𝜽 𝟐) 𝒓 = 𝑳 𝟏 ∗ 𝐚𝐛𝐬[𝐬𝐢𝐧( 𝜽 𝟏)] + 𝑳 𝟐 ∗ 𝐜𝐨 𝐬( 𝜽 𝟐) + 𝑪 𝟎 ∗ 𝒂𝒃𝒔[𝒔𝒊𝒏( 𝜽 𝟐)] 𝜶 = 𝜽 𝟎 Giventhe Cartesiancoordinatesasaninput,we convert themto cylindrical formatusingthe Cartesian to Cylindrical conversionformulasorequations11 – 13 (see below). 𝛼 = 𝑡𝑎𝑛−1( 𝑦 𝑥 ) 𝑟 = √ 𝑥2 + 𝑦2 𝑙 = 𝑧 Once we obtainthe cylindrical coordinatesof the Cartesianinput,we canapplythese (cylindrical) coordinatestoour three equations(equations8– 10). Usingthe substitutionmethodwiththese three equationsallowsustosolve forthe three unknowns. 𝜃1 = 𝑐𝑜𝑠−1[ 𝑙 − 𝐿0 + 𝐿2 ∗ cos( 𝜃2) − 𝐶0 ∗ cos( 𝜃2) 𝐿1 ] Substitutingequation14intoequation 9 givesusequation 15: 𝑟 = 𝐿1 ∗ abs[sin (𝑐𝑜𝑠−1[ 𝑙−𝐿0+𝐿2∗cos( 𝜃2)−𝐶0∗cos( 𝜃2) 𝐿1 ])] + 𝐿2 ∗ cos( 𝜃2) + 𝐶0 ∗ 𝑎𝑏𝑠[𝑠𝑖𝑛( 𝜃2)] From equation15 we can solve fortheta_2 thensubstitute theta_2intoequation14whichwill give us our theta_1 value. Theta_0can thensimplybe settothe cylindrical coordinate alphaandthatwill give us all three of our unknown robotjointangle values(theta_1,theta_2and theta_0). Conclusion We setout to uncoverthe inverse kinematicsolutionof a6 DOF robotfor the first3 linksandjoints. By choosinga cylindrical coordinatesystem,we foundthatitwasmuch easiertoanalyze the robotwith respectto thiscoordinate system. Afteranalyzingthe robot,itsphysical dimensions,limitationsandthe relationshipsbetweenthe jointangle movementsandthe final positionof the end-effector,we were able to come up withan algorithm. Switchingtoa cylindrical coordinatesystemallowedustoreduce the numberof unknownstotwo forthe particularproblem. Relatingthe jointanglestothe final positionof the end-effectorthenallowedustocome upwithtwo equationsthatcouldbe usedtogether to solve forthe tworemainingunknownjointangles(theta_1andtheta_2). (14) (15)