STUDIES ON TRAJECTORY TRACKING OF TWO LINK PLANAR MANIPULATOR. By Mr. SANE SUBHASH

Transcription

1 STUDIES ON TRAJECTORY TRACKING OF TWO LINK PLANAR MANIPULATOR By Mr. SANE SUBHASH Department of Mechancal Engneerng Natonal Insttute of technology, Rourkela Rourkela , Odsha, INDIA -4

2 STUDIES ON TRAJECTORY TRACKING OF TWO LINK PLANAR MANIPULATOR A Thess Submtted n Partal Fulflment of the Requrements for the Degree of BACHELOR OF TECHNOLOGY IN MECHANICAL ENGINEERING AND MASTER OF TECHNOLOGY IN MECHATRONICS AND AUTOMATION By Mr. SANE SUBHASH 7ME47 Under the supervson of Prof J SRINIVAS Department of Mechancal Engneerng Natonal Insttute of Technology, Rourkela MAY, 5

3 Department of Mechancal Engneerng Natonal Insttute of Technology, Rourkela C E R T I F I C A T E Ths s to certfy that the thess enttled, STUDIES ON TRAJECTORY TRACKING OF TWO LINK PLANAR MANIPULATOR by Mr Sane Subhash, submtted to the Natonal Insttute of Technology, Rourkela for the award of Master of Technology n Mechancal Engneerng wth the specalzaton of Mechatroncs and Automaton, s a record of bonafde research work carred out by hm n the Department of Mechancal Engneerng, under my supervson and gudance. I beleve that ths thess fulfls part of the requrements for the award of the degree of Master of Technology. The results emboded n ths thess have not been submtted for the award of any other degree else other. Dr. (Prof.) J. Srnvas Dept. of Mechancal Engneerng Natonal Insttute of Technology Place: Date : Rourkela-7698, Odsha, INDIA

4 ACKNOWLEDGEMENT Frst and foremost, I wsh to express my sense of grattude and ndebtedness to my supervsor Prof. J. Srnvas for ther nsprng gudance, encouragement, and untrng efforts throughout the course of ths work. Ther tmely help, constructve crtcsm and panstakng efforts made t possble to present the work contaned n ths thess. I express my grattude to the professors of my specalzatons for ther advce and care durng my tenure. I am also very much oblged to the Head of Department of Mechancal Engneerng Prof. S S Mohapatra NIT Rourkela for provdng all the possble facltes towards ths work. Also thanks to the other faculty members n the department. Specally, I extend my deep sense of ndebtedness and grattude to my colleagues, Mr Satsh, Teja and Madhusmta and my senor, Ms. Varlakshm madam for ther helpful company. After the completon of ths Thess, I experence a feelng of achevement and satsfacton. Lookng nto the past I realze how mpossble t was for me to succeed on my own. I wsh to express my deep grattude to all those who extended ther helpng hands towards me n varous ways durng my tenure at NIT Rourkela v

5 ABSTRACT In robotc manpulator control stuatons, hgh accuracy trajectory trackng s one of the challengng aspects. Ths s due to nonlneartes n dynamcs and nput couplng present n the robotc arm. In the present work, a two lnk planar manpulator revolvng n a horzontal plane s consdered. Its knematcs, Jacoban analyss, dynamc equatons are obtaned from modellng. It s proposed to use ths manpulator for followng a desred trajectory by usng an effectve control method. Intally, computed torque control scheme s used to obtan the end effector motons. The dynamc equatons are solved by numercal method and the jont space results are used to obtan the error and ts dervatve. Ths lnearzed error dynamc control uses constant gans and an attempt s made to obtan a correct set of gans n each error cycle to refne the control performance. A scaled prototype s made wth alumnum lnks and jont servos. A mechatronc system wth an arduno mcrocontroller board s employed to drve the servos n ncremental fashon as per the trackng pont and ts nverse knematcs. The computer results are shown for two trajectores namely a straght lne and splne. The errors are reported as a functon of tme and the correspondng jont torques computed n each tme step are plotted. Fnally to llustrate the mechatronc control system on the prototype, a path contanng three ponts s consdered and correspondng errors and repeatablty are presented. v

6 INDEX Lst of Fgures v Lst of Tables..v Chapter Introducton..... Manpulator Analyss and Control... Lterature Revew on Two Lnk Seral Manpulator Objectves of the present work...3 Chapter Mathematcal Modellng Knematc Analyss.5.. Inverse Knematcs Jacoban Dynamcs 9.5. Soluton of Dynamc Equaton...6. Computed Torque Control Method Chapter 3 Results and Dscusson 3.. Workspace Analyss. 3.. Manpulablty Index Statc Force Analyss Inverse Dynamc Analyss Control Results Model Fabrcaton and testng... Chapter 4 Concluson Future Scope.4 References...5 Appendx..6 Appendx..7

7 Lst of Fgures Fg.: SCARA Selectve Complance Assembly Robot Arm Fg.: Schematc representaton of two-lnk manpulator. Fg.3: CTC control scheme Fg.4: Workspace Fg.5: Manpulablty ndex Fg.6: Jont torque T Fg.7: Jont torque T Fg.8: Jont torques Fg.9: Desred trajectory Fg.: Trajectory trackng of end effector Fg.: Jont angle Fg.: Jont veloctes Fg.3: Control torques Fg.4: Trackng errors Fg.5: Desred trajectory Fg.6: Trajectory trackng of end effector Fg.7: Control torques Fg.8: Trackng errors Fg.9: Prototype model Fg.: Control experment for trackng ponts Fg.: Expermental plots for nverse knematcs v

8 Lst of Tables Table : D-H parameters of two-lnk manpulator Table : Two-lnk arm geometrc parameters Table 3: Inverse knematcs solutons Table 4: Trajectory trackng for nverse knematcs v

9 Chapter INTRODUCTION A robot s manpulator arm can move wthn three axes of x, y and z referrng to base part, vertcal elevaton and horzontal extenson. Many manpulator types are avalable as rectangular, cylndrcal, sphercal, revolute and artculated robotc arm are generally used n ndustres. Fnal goal of those robotc desgns s to accurate tp poston control n spte of the flexblty n a reasonable amount of tme. Many controller algorthms those are, adaptve control, Neural Network (NN), fuzzy logc have been used for tp poston control of two-lnk flexble manpulators. These three methods are very effectve but are more complex and dffcult to desgn and analyse. So a smple PD controller s mplemented on the dynamc model of the flexble manpulator. The proportonal dervatve (PD) controllers are more helpful n ndustry for many years due to ther smplcty of operaton, robustness of performance. Unfortunately, t has been a problem to acheve optmal PD gans because many ndustral plants are often burdened wth problems such as hgh order, tme delays, and nonlneartes. The tunng process need labour and tme consumng. Due to these reasons, t s hghly desrable to fnd new approaches to the tunng of PD controllers. In ths thess a new PD controller desgn s done va root locus based loop shapng approach. In many feld applcatons where techncal support s requred, man-handlng s ether dangerous or s not possble. In such stuatons three or more arm manpulators are commonly used. Some robots are used to nspect dangerous areas or/and to remove and to destroy explosve devces. These robots can be used to make some corrdors through mned battle felds, manpulaton and neutralzaton of the ntact ammunton, nspecton of the vehcles, trans, arplanes and buldngs. For these robots a good functonal actvty s to determnate the dmensons of the workspace, knematcs and control strateges of the robotc arm. Last two decades, concepton and use of robots has evolved from the stuff of scence fcton flms to the realty of computer-controlled electromechancal devces ntegrated nto a wde varety of ndustral envronments. It s routne to see robot manpulators beng used for weldng and pantng car bodes on assembly lnes, stuffng prnted crcut boards wth IC components, nspectng and reparng structures n nuclear, undersea, and underground envronments. Although few of these manpulators are anthropomorphc, our fascnaton wth humanod machnes has not dulled, and people stll envson robots as evolvng nto electromechancal replcas of ourselves.

10 .. Manpulator Analyss and Control A manpulator s a mechancal devce whch can operate remote objects or materal even n the absence of any worker. Lnks and jonts make a long chan n a manpulator, whch can manpulate n ts workspace. The total number of jonts gves the degrees-of-freedom (DOF). Manpulators are classfed nto two types namely: ) Parallel manpulators ) Seral manpulators Most ndustral robots are the seral manpulators whch are desgned as seres of lnks connected by motor actuated jonts from base part to end effector part. These type of manpulators have an anthropomorphc arm structure wth a shoulder, an elbow, and a wrst. The man applcaton for these seral type manpulators n present ndustry s pck and place assembly robot. SCARA s one example for ths type of robot and s shown n Fg.. In ths part of work some exceptonal control methods for the followng control of robot controllers are dscussed. The controllers that are created n ths part address computatonal ssues and the mpacts of actuator moton. Fgure : SCARA Selectve Complance Assembly Robot Arm

11 .. Lterature Revew Lterature survey was conducted to obtan some nsghts nto varous factors relatng to modellng of planar manpulators for varous ndustral applcatons. In ths work several aspects regardng the knematcs, workspace, Jacoban analyss and dynamc dentfcaton of a two-lnk planar manpulator are studed and presented. De Luca and Sclano [] presented the knematc and dynamc ssues of planar lnk manpulator. Wllam [] descrbed automatc Control Systems, analyss and desgn of seral manpulators. Kwanho [3] explaned the adaptve control of tp pont n a seral lnk robot. Nagrath and Gopal [4] explaned varous ssues lke knematcs, dynamcs, control theores for dfferent seral manpulators. Tokh and Azad [5] dscussed the knematc and modellng of flexble manpulators. Fnally the proposed control method s appled for the flexble manpulator to llustrate the results. Ata [6] presented the revew on varous optmal trajectory plannng control technques for seral manpulators. Islam and Lu [7] proposed a sldng mode control technque to seral manpulator and studed the. Kumara et al. [8] explaned trajectory trackng control of knematcally redundant robot manpulators usng neural network. Moldoveanu et al. [9] explaned a trajectory control of a two-lnk robot manpulator usng varable structure theory. Wang and Deng et al. [] explaned the desgn of artculated nspecton arm wth an embedded camera and nterchangeable tools. Zhu et al. [] presented the structure of a seral lnk robot wth 8 degrees of freedom wth a 3-axes wrst carryng camera. Ionescu [] descrbed an approach of measurement usng a calbrated Cr5 neutron source deployed by n-vessel remote handlng boom and mascot manpulator for JET vacuum vessel. Monochrome CCD cameras were used as mage sensors. Karagulle and Malgaca [3] proposed the effect of flexblty on the trajectory of a planar two-lnk manpulator s studed usng ntegrated computer-aded desgn procedures. Nageshrao et al. [4] explaned the passvty based control method. Detaled algorthm was proposed and mplemented for - DOF manpulator. Ayala and Coelho [5] llustrated an algorthm to test the PID tunng by usng a two degree of freedom robot manpulator..3. Objectves of the present work In present work, the man objectves to be carred out are knematc analyss, nverse knematc analyss, Jacoban analyss, dynamcs and soluton of dynamc equatons. In the part of results and dscussons followng thngs are presented: workspace analyss, 3

12 manpulablty ndex, statc force analyss, nverse dynamc analyss, control results and model fabrcaton and control. For workspace analyss, a C program based on forward knematc equatons s used. An Arduno program s used to control two servo motors for trackng the gven ponts wth n the workspace. A Computed Torque Control scheme s used to obtan the trajectory and a task space set pont control approach s also presented. The thess s organsed as follows: Chapter descrbes mathematcal modellng, where the detals of two lnk manpulator ncludng dynamcs are presented. Also control algorthm based on CTC s gven. Chapter 3 descrbes the results and dscusson as program outputs and expermentally analyss outcomes. Chapter 4 gves the bref concluson. 4

13 Chapter Mathematcal Modellng.. Knematc Analyss Modellng of two-lnk planar manpulator nvolves the lnear and rotatonal dynamcs of the lnks. For smplcty t s assumed that the two-lnk manpulator has two revolute jonts and s an open knematc chan type. Fg. shows the schematc dagram of the two-lnk manpulator. Let L and L be the length of the frst and second lnk respectvely. Angle and represent the jont rotatons of the frst, and second jont respectvely. Fgure : Schematc representaton of two-lnk manpulator Calculatng the poston and orentaton of the end-effector n terms of the jont varables s called as forward knematcs. In order to have forward knematcs for a robot mechansm n a systematc manner, one should use a sutable knematcs model. Denavt-Hartenberg method that uses four parameters s the most common method for descrbng the robot knematcs. The parameters of two-lnk planar manpulator s shown n Table. Generalzed form of homogenous transformaton matrx can be expressed as n equaton (). cos sn Here, s jont angle, s lnk twst T sn cos cos cos sn sn sn cos sn cos a a cos sn a s lnk length and d s jont dstance d () 5

14 as: From the D-H parameters shown n Table the overall transformaton matrx was derved T T T () Table : D-H parameters of two-lnk manpulator Lnk a d θ L θ L θ Usng, For Frst Lnk: C = cos (θ ) S =sn (θ) S =sn (θ) C =cos(θ) C T = S S C LC L S (3) For Second Lnk: C T = S S C LC L S (4) By substtutng eqauatons () and (3) n () The overall manpulator tranformaton matrx s T = C S S C L C L C L S L S (5) For a DOF planar manpulator Px, Py as task coordnates the forward knematc equatons obtaned as the last column elements as follows: 6

15 P x = L cos θ + L cos(θ + θ ) (6) P y = L sn θ + L sn(θ + θ ) (7).. Inverse Knematcs Solvng the nverse knematcs s computatonally expansve and generally takes a very long tme n the real tme control of manpulators. Tasks to be performed by a manpulator are n the Cartesan space, whereas actuators work n jont space. Cartesan space ncludes orentaton matrx and poston vector. However, jont space s represented by jont angles. The converson of the poston and orentaton of a manpulator end-effector from Cartesan space to jont space s called as nverse knematcs problem. There are two solutons approaches namely, geometrc and algebrac used for dervng the nverse knematcs soluton, analytcally. The followng equatons are obtaned by algebrac method. sn θ = a tan (8) cos Where, Cos(θ )= L L (P x +P y -L -L ) (9) Sn(θ )= cos () S θ = atan C () Where, S = C = ( L L L C ) P L P x S S L L L C y L S L L C P x ().3. Jacoban Analyss In trajectory trackng control t s not just enough to determne the jont and end effector coordnates. It s necessary to control the velocty of the end effector or the tool durng the moton. Snce the control acton occurs at the jonts, t s only possble to control the jont veloctes. Therefore, there s a need to be able to take the desred end effector 7

16 veloctes and calculate from them the jont veloctes. Snce we control the robot n jont space and have a tendency to reason about movement n Cartesan space, we need to completely comprehend the mappng from jont space to Cartesan space and the other way around. Forward and nverse knematcs descrbes the statc relatonshp between these spaces. The matrx whch relates changes n jont parameter veloctes to Cartesan veloctes s called the Jacoban Matrx. Ths s a tme-varyng, poston dependent lnear transform. It has a number of columns equal to the number of degrees of freedom n jont space, and a number of rows equal to the number of degrees of freedom n Cartesan space. The Jacoban for ths manpulator s: The matrx whch relates changes n jont parameter speeds to Cartesan speeds s known as the Jacoban Matrx. It depends on tme-varyng, poston dependent lnear transform. It has varous sectons equvalent to the quantty of degrees of freedom n jont space, and number of rows equvalent to the degrees of freedom n Cartesan space. The Jacoban for ths planar manpulator s: By dfferentatng the coordnates wth respect to tme X = J Y X X Here [J ]s Jacoban, [J] = Y Y (3) (4) X = C L +L C (5) X (C L = LC t ) (C L LC + t ) t (6) X = (-S L -L S ) -L S (7) Y = S L +L S (8) Y (S L = LS t ) (S L LS + t ) t (9) Y = (C L +L C ) +L C () 8

17 X SL LS LS = Y CL LC L C () X = J () = J - X (3).4. Dynamcs The arm dynamcs of Robot handles wth the mathematcal formulaton of the equatons of robot arm moton. One methodology, named Newton-Euler Approach, s employed to derve the equatons of moton. The followng are fnal equatons for jont torques [6] (3) M M C (4) M M C Where M Z Z cos (5) M M Z Z (6) 3 cos M Z 3 (7) C Z sn ( ) (8) C Z (9) sn Z Mr M L r ) ( I I (3) Z (3) M Lr Z 3 M r I (3) Here, r and r are the mass centres of lnks and. I and I are mass moments of nerta of lnks and. It s clear that mass matrx s functon of jont angles and Corols vector components C and C are functons of jont angles and ther tme dervatves. 9

18 .5. Soluton of Dynamc Equatons Above dynamc equatons are used for fndng torques t s called nverse dynamcs. If torques are gven and thetas are requred, t s called forward dynamcs. Forward dynamcs soluton can be obtaned by solvng smultaneously two second order dfferental equatons. Generally numercal methods can be used for solvng these equatons. One of such technque s called Runge-Kutta fourth order method. It s descrbed for solvng the equaton y =f(x) as follows k hf ( x, y ) (33) k k k hf ( x h, y ) (34) k hf ( x h, y ) (35) 3 k 4 ( k3 hf x h, y ) (36) y y ( k k k3 4 ) (37) 6 k Here y =f(x ) s ntal condton..6. Computed torque control method (CTC) The archtecture of a non lnear model based controllng mechansm called the computed torque control. A model based controller s dependent on the model of the system to be controlled. CTC s well known model based controllers that reles on calculaton of system dynamcs matrces teratvely at each controllng step. The matrces M, C, G of the model are recalculated at every teraton. Ths s due to the fact that the dynamcs of a varant system s constantly subject to change. As a result, a problem that becomes more severe for complex systems. The CTC s also respected as a feed-back lnearzaton controller n that t tres to neutralze the effects of system dynamcs n the feed-back loop by cancellng the dynamc terms.

19 Fg.9 descrbes the block dagram of the CTC appled to two lnk planar manpulator robot model. In ths scheme the control process s broken n to two parts, namely the modelbased porton and servo porton. The model-based porton contans a model of non-lnearty. Hence t plays the role of a lnearzaton functon. The procedure can be expressed as: m (38) Where M(q) (39) C( q, q ) q ac G( q) (4) The servo porton on the other hand, takes the form of desred q k e k e (4) ac v p Where desred q ac,e and e are respectvely the desred jont acceleraton, jont poston and velocty servo errors. The values assocated wth the known gans (k p and k v ) are also determned based on a desred performance specfcaton. Fg.3 CTC control Technque

20 Chapter 3 Results and Dscussons 3.. Workspace Analyss The determnaton of the workspaces s very useful n the context of desgn or moton plannng of manpulators. Fg.3 shows the maxmum workspace s obtaned by varyng the jont angles as follows: θ [ -7, 7] and θ [-6, 6]. For fndng the workspace a C program s used. Table.3. shows the manpulator geometrc parameters consdered n the present work for both knematcs and dynamc claculatons. Table : Two-lnk arm geometrc parameters S.No Parameter Length (m) Mass (kg) Moment of Inerta (kg-m ) Lnk Lnk Fg.4 Workspace

21 3.. Manpulablty Index of the manpulator Manpulablty provdes quanttatve measure of closeness of manpulator to sngularty. Arrangement of jont parameters properly mnmzes ncdence of sngulartes. It s mathematcally defned as T Manpulablty= det( J( q) J ( q)) (4) Where and are sngular values of matrc J(q). Fg.5 Manpulablty ndex From Fg.5 t s seen that the entre workspace s not flled but few ponts are only vsble ndcatng that the sngular space s much less than total workspace Statc Force Analyss Manpulator wll produce forces and moments at the end effector n the drecton of x, y and z drectons. Consder F=(F x, F y ) T denotes torques and vector of forces at the end effector. Thus the forces and torques n x, y and z drectons are the components at the end effector. 3

22 [J ] T F F x y (43) By selectng jont space trajectores wth a constant force Fx= N and Fy= N, we can plot the jont torques. Ths analyss can be used for low speed applcatons. Fg.6 Jont torque T 4

23 Jont Torques (Nm) Fg.7 Jont torque T 3.4. Inverse dynamcs analyss Inverse dynamc analyss has been done for the jont space spral trajectory consdered for the controller desgn. Fg.8 llustrate the drvng torques (τ, τ ) of the two lnk manpulator from the knowledge of the trajectory equaton n terms of tme Tme (sec) Fg.8 Jont torques 5

24 3.5 Results of Inverse knematcs Inverse knematc analyss results are obtaned fromanother smple computer program. Here the end coordnates (P x, P y ) are provded as nputs and two set of solutons for jont angles are obtaned as outputs. Table 3: Inverse knematcs solutons (P x, P y ) (cm) θ, θ (degrees) θ, θ (degrees) (5, 5) 9, 76 4, 76 (3,3) 9, 34 7, -47 (,) -6, 9 96,-9 (,5) 5,5 6, Control Results The smulaton results of the computed torque controller (CTC) for a two lnk robotc arm are presented. The program was wrtten n C language (see appendx ). Smulaton results prove that the performance of the conventonal CTC s good and the obtaned fgures llustrate that the trackng abltes of trajectores consdered. Trajectory : Straght lne A straght lne trajectory s tracked n XY plane. The trajectory s defned n jont space drectly as follows: Straght lne Trajectory: q q ( t T t ( )( q T t ) ( ) 3 T 3 3 Where t [,6.] T T sec s tme, ( xp,yp) (.,-.4) s the ntal pont. In smulatons, the control gans are selected as follows: K p K v 6

25 Fg. shows the desred trajectory consdered n a 3-Dmensonal space. Fg.9 Desred Trajectory Fg. shows the fnal result of the trajectory from CTC. Fg. Trajectory trackng of the end-effector Fg. shows the jont angles n every tme second. 7

26 Fg. shows the correspondng jont veloctes Fg. Jont Angles Fg. Jont veloctes Fg.3 shows the calculated jont torques Fg.3 Control Torques Fg. 4 shows the trackng errors at the jonts of the manpulator and t s found the percentage error s less than one. 8

27 Fg.4 Trackng Errors Trajectory : Spral A spral trajectory s tracked n XY plane. The trajectory s defned as follows: Spral Trajectory: q q ( / 5) (t / T (/( ) sn( t / ) / 8) (t / T (/( ) cos( t / ) Where t [.,.6 ] s tme, T=. s the parameter, n smulatons, the control gans are selected as follows: The parameters of the conventonal CTC are set to be: K p K v Fg.5 and 6 show the desred and actual trajectores. Fg.7 shows control torques computed by the algorthm Fg.8 shows the error hstores. It s seen that CTC effectveness s very good after some tme has elapsed. 9

28 Fg.5 Desred Trajectory Fg.6 Trajectory Trackng of end effector Fg.7 Control torques

29 Fg.8 Trackng errors 3.6 Model Fabrcaton and expermental testng In fabrcaton work, two lnks are constructed by usng Alumnum sheet of fnte thckness (3mm). The length of frst lnk called L s 9 cm, and second lnk called L s cm. These are measured from jont to jont. L s connected to the base part of system wth servo motor, L lnk s connected to L through another servo motor. Two standard economy servo motors 4.5kgcm wth plastc gears s used to revolute the lnks. Arduno Uno R3 s used to control the servos wth an arduno program. The end of second lnk s joned wth pencl for tracng n XY plane. Ths s revolvng fully n a horzontal plane. Fg.9 shows the fabrcated set-up. Fg.9 Prototype model

30 Fg. shows the control system based on laptop wth an arduno board based on nverse knematcs Fg. Control experment for trackng the ponts The ponts are traced by gvng the jont angles θ and θ to the arduno mcrocontroller, whch are obtaned by the nverse knematc equatons and are shown n Table.. The correspondng code wrtten n arduno compler s shown below. ===================================== #nclude<servo.h> servo servo ; // defne left servo servo servo ; // defne rght servo vod setup(){ servo.attach(); // set left servo to dgtal pn servo.attach(9); // set rght servo to dgtal pn 9 } vod loop() { // loop through moton test p(); delay(); p(); // example : move forward delay(); // wat mllseconds p(); delay(); } // moton routne for forward, reverse, turns, step vod p() {

31 servo.wrte(); servo.wrte(); } vod p(){ servo.wrte(); servo.wrte(8); } vod p(){ servo.wrte(8); servo.wrte(); } ================================ Intal poston of the end effector s (, ) (along horzontal axs) where the locaton of both the servo motors angular poston are degrees. Table 4: Trajectory trackng for nverse knematcs (P x, P y ) (cm) θ, θ (degrees) θ, θ (degrees) (5, 5) 9, 76 4, 76 (3,3) 9, 34 7, -47 Fg. shows the trajectory traced by the end effector pencl tool wth respect to orgnal ponts. It s seen that at every pont some error s occurrng. Ths may be due to naccuraces n the model settng ncludng the pencl tool algnment as well as due to the nteger values of jont angles gven to the arduno program. Fg. Expermental plot for nverse knematcs 3

32 Chapter 4 CONCLUSION In ths work several aspects regardng the knematcs, workspace, Jacoban analyss and dynamc dentfcaton of a two-lnk planar manpulator were presented. The results obtaned n ths study may be useful for the robot desgnng not only to understand the knematc behavour of the robot for varous confguratons, but also to be able to mplement real tme control of a manpulator. The programs are developed n C language for generaton of workspace ponts, manpulablty ndex based on Jacoban and for nverse knematcs. The results obtaned are plotted separately from data fles. Even though the two lnk manpulator s relatvely smple and straght forward, most of the approaches appled here are common to all the manpulators. The statc force analyss for obtanng jont torques s a mportant problem for desgn of manpulator. Fnally the trajectory trackng problem wth two dfferent trajectores usng CTC control method was llustrated numercally. The errors and correspondng appled torques were shown as a functon of tme. Ths two lnk manpulator analyss and control s an on gong work n mechatroncs and robotcs research. 4.. Future scope The CTC control can be mproved by tunng of poston and velocty gan constants. Also, when a dsturbance such as a payload at the grpper acts on the system, the CTC algorthm requres slght modfcaton by addng addtonal control torque obtaned from dsturbance rejecton observer. Ths can be added to present work as a future scope. The scaled model developed can be mproved further to perform pck and place operatons by attachng a two fnger grpper. Ths can be extended to a redundant four degree of freedom, spatal SCARA manpulator wth one translatonal jont. Durng the control, the manpulator jont torques (hence angles) are to be computed automatcally whle trackng the trajectory s gong on. An mprovsed expermental work s requred. 4

33 References ) A. De Luca and B. Sclano. Closed-Form Dynamc Model of Planar Multlnk Lghtweght Robots. IEEE Trans. Syst., Man, Cybern. 99; vol., no. 4; pp ) A. W. Wllam, Automatc Control Systems Basc Analyss and Desgn, Oxford Unversty Press, 995; pp ) Y. Kwanho. Adaptve Control, In-Tech publcaton, 9; pp ) I. J. Nagrath and M. Gopal, Control Systems Engneerng, New Age Publsher, 7. 5) M. O. Tokh and A.K.M. Azad, Flexble Robot Manpulators Modelng, Smulaton and Control, IET, London, UK, 8. 6) A. A. Ata. Optmal Trajectory Plannng Of Manpulators: A revew. J Engg. Sc. Tech. 6; pp ) S. Islam and X. P. Lu. Robust Sldng Mode Control for Robot Manpulators. IEEE Trans. Ind. Elect. ; vol. 58; pp ) N. Kumara, V. Panwarb and N. Sukavanamc. Neural Network-Based Nonlnear Trackng Control of Knematcally Redundant Robot Manpulators. Math. Comp. Modellng. ; vol. 53; pp ) F. Moldoveanu, V. Comnac and D. Floroa. Trajectory Trackng Control of a Two-Lnk Robot Manpulator usng Varable Structure System Theory. Control Engg. App. Info. 5; vol. 7; pp ) A. Wang and M. Deng. Robust Nonlnear Control Desgn to a Manpulator Based on Operator based Approach. ICIC Express Letters ; vol. 6; pp ) Y. Zhu, J. Qu and J. Tan. Smultaneous Optmzaton of a Two-Lnk Flexble Robot Arm. J. Robo. Syst. ; vol. 8; pp ) F. Ionescu. Modelng and Smulaton n Mechatroncs. IFAS Int. Conf, MCPL 7; pp ) H. Karagulle and L. Malgaca, Analyss For End Pont Vbratons Of Two Lnk Manpulator By Integrated CAD/CAE Procedures. Fnte Elem. Ana. Desgn. 4; vol. 4; pp ) S. P. Nageshrao, G. lopes, D. Jeltsema and R. Babuska. Passvty Based Renforcement Learnng Control of a -DOF Manpulator Arm. Mechatroncs. 4; vol. 4; pp ) H. V. H. Ayala, L. D. S. Coelho. Tunng of PID Controller Appled to a Robotc Manpulator. Exp. Syst. App. ; vol. 39; pp ) A. Wang and M. Deng, Robust non lnear mult varable trackng control desgn to a manpulator wth unknown uncertantes usng operator based robust rght coprme factorzaton, Transactons of nsttute of measurement and control, DOI.77/ , 3. 5

34 Appendx WORKSPACE AND KINEMATICS PROGRAM C Program for workspace, Jacoban, manpulablty ndex, statc force analyss. #nclude<stdo.h> #nclude<math.h> man() { nt ; double a,a,th,th,p,px,py,th,th,px,py,nvkn; double B,B,B3,B4,J,J,J,J,Man,tou,tou,F,F; FILE *fp; fp=fopen("wspace.c","w"); a=.5; a=.5; p=./7.; F=; F=; //=; clrscr(); for(th=-7;th<7;th=th+5) { for(th=-6;th<6;th=th+5) { Th=th*p/8.; Th=th*p/8.; px=a*cos(th)+a*cos(th+th); py=a*sn(th)+a*sn(th+th); J=-a*sn(Th)-a*sn(Th+Th); J=-a*sn(Th+Th); J=a*cos(Th)+a*cos(Th+Th); J=a*cos(Th+Th); B=J*J+J*J; B=J*J+J*J; B3=J*J+J*J; B4=J*J+J*J; Man=sqrt(B*B4-B*B3); tou=j*f+j*f; tou=j*f+j*f; =+; fprntf(fp,"%f\t%f\t%f\t%f\t%f\t%f\t%f\n",th,th,px,py,man,tou,tou); } } } //Px=.3; //Py=.; //nvkn(px,py); //prntf("=%d\n",); //getch(); //*double nvkn(double px,double py); { double cn,cd,c,s,cn,cd,c,s,a,a,th,th; FILE *fp; cn=(px*px+py*py-a*a-a*a); cd=*a*a; c=cn/cd; s=sqrt(-c*c); TH=atan(s,c); cn=(px*(a+a*cos(th)))+(py*a*sn(th)); cd=(px*px+py*py); c=cn/cd; s=sqrt(-c*c); TH=atan(s,c); fprntf(fp,"%f\t%f\t%f\t%f\n",th,th,px,py); getch();} 6

35 Appendx CTC PROGRAM IN C LANGUAGE C Programmng code for CTC control. #nclude<stdo.h> #nclude<math.h> #defne m. #defne m. #defne a.5 #defne a.5 #defne Z #defne Z #defne Z3 man() { nt ; double kp,kd,kp,kd,px,py,px,py,pxd,pyd,pxdd,pydd,t,r,p,x,xa,xd,cn,cd,c,s; double thd,th,th,cn,cld,c,s,j[][],jt[][],ji[][],e[][],ed[][]; double a,a,f,f; FILE *fp; fp=fopen("dynamc.c","w"); px=.3; py=.3; r=.; p=./7.; =; //desred trajectory for(t=;t<*p;t=t+.) { px=px+r*cos(t); py=py+r*sn(t); pxd=-r*sn(t); pyd=r*cos(t); pxdd=-r*cos(t); pydd=-r*sn(t); //Inverse knematcs c=(px*px+py*py-a*a-a*a)/(*a*a); s=sqrt(-c*c); th=atan(s,c); s=((a+a*c)*py-px*a*s)/(a*a+a*a+*a*a*c); c=(px+s*a*s)/(a+a*c); th=atan(s,c); // JACOBIAN J[][]=-s*a-a*sn(th+th); J[][]=-a*sn(th+th); J[][]=c*a+a*cos(th+th); J[][]=a*cos(th+th); detj=j[][]*j[][]-j[][]*j[][]; JI[][]=J[][]/detJ;JI[][]=-J[][]/detJ; JI[][]=-J[][]/detJ;JI[][]=J[][]/detJ; thd=ji[][]*pxd+ji[][]*pyd; thd=ji[][]*pxd+ji[][]*pyd; f(==) { F=rand(); F=rand(); } else { //compute errors eth=th-tha; eth=th-tha; ethd=thd-thda; 7

36 ethd=thd-thda; //pd control gans kp=; kd=; kp=; kd=; b=thdd+kp*eth+kd*ethd; b=thdd+kp*eth+kd*ethd; Z=m*r*r+m*(a*a+r*r)+I+I; Z=m*a*r; Z3=m*r*r+I; M=Z+*Z*cos(th); M=Z3+Z*cos(th); M=M; M=Z3; C=-Z*sn(th)*(thd*thd+*thd*thd); C=Z*thd*thd*sn(th); //Compute control torques F=M*b+M*b+C; F=M*b+M*b+C; } [tha,thad,tha,thad]=dynam(f,f,th,th,thd,thd); Tor[]=F;Tor[]=F; e[]=eth;e[]=eth; fprntf(fp,'%f\t%f\t%f\t%f\t%f\n",t,f,f,eth,eth); =+; } } dynam(double F,double F,double th,double th,double thd,double thd) { double f,g,j,k; double f,g,j,k,f3,g3,j3,k3,f4,g4,j4,k4; double f(),g(),j(),k(),x,x,x3,x4; //Compute nerta matrx //Z=m*ag*ag+m*(a*a+ag*ag)+I+I;//Z=m*a*ag;//Z3=m*ag*ag+I; //m=z+*z*cos(th); //m=z3+z*cos(th); //m=m; //m=z3; x=th;x=thd;x3=th;x4=thd; for (=:<;++) { f=h*f(t,x,x,x3,x4); g=h*g(t,x,x,x3,x4); j=h*j(t,x,x,x3,x4); k=h*k(t,x,x,x3,x4); f=h*f((t+h/),(x+f/),(x+g/),(x3+j/),(x4+k/)); g=h*g((t+h/),(x+f/),(x+g/),(x3+j/),(x4+k/),f,f); j=h*j((t+h/),(x+f/),(x+g/),(x3+j/),(x4+k/)); k=h*k((t+h/),(x+f/),(x+g/),(x3+j/),(x4+k/),f,f); f3=h*f((t+h/),(x+f/),(x+g/),(x3+j/),(x4+k/)); g3=h*g((t+h/),(x+f/),(x+g/),(x3+j/),(x4+k/),f,f); j3=h*j((t+h/),(x+f/),(x+g/),(x3+j/),(x4+k/)); k3=h*k((t+h/),(x+f/),(x+g/),(x3+j/),(x4+k/),f,f); f4=h*f((t+h),(x+f3),(x+g3),(x3+j3),(x4+k3)); g4=h*g((t+h),(x+f3),(x+g3),(x3+j3),(x4+k3),f,f); j4=h*j((t+h),(x+f3),(x+g3),(x3+j3),(x4+k3)); k4=h*k((t+h),(x+f3),(x+g3),(x3+j3),(x4+k3),f,f); x=x+((f+f4)+*(f+f3))/6.; x=x+((g+g4)+*(g+g3))/6.; x3=x3+((j+j4)+*(j+j3))/6.; x4=x4+((k+k4)+*(k+k3))/6.; } return(); } double f(double t,double q,double qd,double q,double qd) { 8

37 return(qd); } double g(double t,double q,double qd,double q,double qd,double F,double F) { double m,m,m,m,c,c; m=z+*z*cos(q); m=z3+z*cos(q); m=m; m=z3; //Compute coralss matrx c=-z*sn(q)*(qd*qd+*qd*qd); c=z*qd*qd*sn(q); return((f*m-c*m-m*f+c*m)/(m*m-m*m)); } double j(double t,double q,double qd,double q,double qd) { return(qd); } double k(double t,double q,double qd,double q,double qd,double F,double F) { double m,m,m,m,c,c; m=z+*z*cos(q); m=z3+z*cos(q); m=m; m=z3; //Compute coralss matrx c=-z*sn(q)*(qd*qd+*qd*qd); c=z*qd*qd*sn(q); return((f*m-c*m-m*f+c*m)/(m*m-m*m)); } 9