nd Workshop on Advanced Research and Technolog n Industr Applcatons (WARTIA ) Knematcs Modelng and Analss of MOTOMAN-HP Robot Jou Fe, Chen Huang School of Mechancal Engneerng, Dalan Jaotong Unverst, Dalan, Chna huagnchen@.com Kewords: Rotaton matr; forward knematcs; nverse knematcs; robot workspace Abstract. The mproved knematcs method based on the orthogonalt of rotaton matr and matr block s presented for MOTOMAN - HP robot to mprove the computng speed of the robot knematcs.frstl, accordng to the robot structure, the forward knematcs model s establshed wth D-H method. And for the nverse knematcs, the separaton method of poston and orentaton matr s used to transform comple matr equaton nto eght algebrac equatons to mprove the computng speed. For etraneous roots, the optmal soluton s confrmed b selectng the closest one to the current jont angle wth the solvng order drectl, nstead of computng all the roots. Fnall, the workspace s obstaned b the smulaton and the knematcs analss method s proved to be effcent.. Introducton For the knematcs analss, the forward knematcs equaton s generall establshed b D-H knematcs parameters, but the nverse knematcs s more complcated, and there are some popular methods, e.g. the matr transformaton method based on D-H parameters []. Matr transformaton s wdel appled, but the method needs more homogeneous transformaton matr nverson and repeatedl multplcaton b nverse matr. Another method s geometrc method for the robot structure []. Although t has the advantage of fast operaton speed, t s lmted b the structure of the robot. Screw transformaton and quaternon method [] s more complcated. Neural network [], genetc algorthm [] etc. also have been used to the nverse knematcs. But the stablt and precson are stll unable to meet the requrements of real-tme control. So some modfed methods have been proposed to mprove the operaton rate of matr transformaton method [7-8]. The vector operaton of a nonlnear equaton [9] was also proposed to solve the nverse knematcs problem. For MOTOMAN-HP robot, an nverse knematc method s presented to avod nverse computaton of large-scale matrces based on the orthogonalt theor of the rotaton matr homogeneous matr s decomposed nto rotaton matr and poston matr, the nverson for the matr s transferred nto transposton operaton, mprovng the solvng effcenc. And accordng to the prncple of "mnmum dstance" for each jont angle, the optmal soluton of the nverse knematcs s selected to avod solvng etraneous roots. Fnall, the robot workspace was obtaned b smulaton, and the knematcs method s verfed to be correct.. Forward knematcs analss MOTOMAN - HP s s degrees of freedom ( DOF) seral chan robot wth a sphercal wrst. The coordnate sstem s shown n Fg.. X a O O O X a a O O,O X X Z O Z Z Z X X X Z Z d Z Fg. The coordnate sstem of MOTOMAN-HP. The authors - Publshed b Atlants Press 87
Robot D-H parameters are shown from Table. Table D-H parameters table Jon t ( ) d( mm) ( α ) a ( mm ) range( ) -8~8 9 a = -~ a = 7 -~ d = 79 9 a = -~ -9 -~ 9 -~ Use the homogeneous matr of D-H to descrbe the coordnate transformaton matr of two adjacent lnks, the matr [9] can be epressed as cos sn a sn cosa cos cosα snα snαd R P T = = snsna cossna cosa cosad () where a s the lnk length, d s the dstance between adjacent lnk, R s rotaton matr: cos sn R = sn cosα cos cosα snα () snsnα cossnα cosα P s poston sub-matr as P = [ a; dsn a; d cosa] () Then the knematcs equaton can be n o a p R P n o a p T = T T T T T T = = n o a p where T T s the pose matr; p p p s the poston vector, T n n n, T o o o, T a a a are respectvel orentaton vector of the coordnate sstem of the end operator relatve to the, and as of the reference coordnate sstem. B the D-H parameters n Table. wth the Eq.(), the homogeneous transformaton matr between the adjacent lnks can be wrtten as cos sn cos sn cos sn 7 sn c os sn cos T = T = sn cos T = () cos sn cos sn 79 T = sn cos T = sn s Then n, o, a, p can be derved n,, nde. cos sn T = sn cos. Inverse knematcs soluton. Inverse knematcs method The nverse knematcs of the robot s the process to compute jont angle of each lnk based on gven pose of end-effector operator, whch plas an mportant role n the robot control sstem. The condton that the robot estng a closed form soluton of nverse knematcs s to meet one of two 877
condtons of Peper crters [] for the robot's mechancal structure: () The ases of three adjacent jont need to ntersect to one pont; () The ases of three adjacent jont are parallel to each other. The mechancal structure s desgned wth the aes of the three adjacent jont ntersect, so the closed-form soluton ests. Transformaton matr wth the two adjacent lnk as Eq.() s dvded nto rotaton matr and poston matr, wth nverse rule of parttoned matr the rotaton T sub-matr s orthogonal matr, t meets R = R. Thus, R R P T T ( T) = ( =,,, ) Therefore, nverson computon of the rotaton matr s converted to compute transpose. Thereb, the amount of calculaton sgnfcantl s reduced. To fnd the nverse knematcs soluton, lnk transformaton equaton can be rewrtten as ( T) ( T) T( T) = TTT () The left and rght matres of Eq.() are ccc ss ccs sc cs c+ 79s+ 7 T T T T T T T T T T T R R RR R R RR P + R R P R R P R P = scc cs scs cc ss s 79c + + (7) sc ss c Consderng the structure characterstcs, the frst three jont angles ( ) are used to decde the poston of the end-effector, and then the last three jont angles ( ) determne the orenton of the end-effector, so the soluton of the nverse knematcs can be decomposed nto the poston and posture matres. For the left and rght sdes of Eq.() are equal, and the equatons for the frst three jont angles can be derved b the correspondng element n the poston sub-matrces, and b the posture matr, fve equatons for the last three jont angles can be obtaned. Suppose c = cos and s = sn, then the equatons are as follows: sp cp = c( pc + ps ) + ps = 79s + c + 7 s( pc + ps ) + pc = s 79c c( ca ) sa = ccs sc s( ca ) ca = scs + cc sa + ca = ss ( cn sn )c + ( so ) s= sc ( cn sn ) s ( so ) c = c B solvng the above eght equatons, varous jont angles can be shown as follows: = Atan (p, p ) or A tan(-p,-p ) ; = Atan ( hp hh, hh + ph) ; = Atan ( h, h ) A tan (, 79) or Atan ( h,- -h ) - A tan (, 79) ; = Atan (ca sa,a s + c (ca )) ; = Atan ( g, g) or Atan (- - g, g) ; = Atan ( sc (s n cn ), scc sc ( so )). where h = pc + ps, ( 79 7 ) / 79 = + +, h = c + 79s + 7, h h p h = s 79c, g= s( ca ) ca.. Choosng method of the optmal soluton For an gven end-effector pose, there are eght groups of nverse soluton. But onl a set of defnte soluton s needed, so choose the closest soluton to the last angle from multple sets of nverse soluton wthn the scope of the jont movement. In general, the optmal soluton of nverse s selected after the nverse solutons of all the jont angles are calculated, and the optmal set among all the nverse solutons s that the sum of all the jont angles s smallest. It can be epressed as mn S = mn b (9) = But ths method requres to compute all the solutons, and then to determne a set of the optmal 878 (8) ()
soluton. To reduce the moton tme and ensure the lest change of the jont angle from the current pose to the net pose, onl make sure the change value of ever jont angle s mnmum, and the change sum of all angles s mnmum. Therefore n the process of computng the jont angles of nverse knematcs, for the jont angles that est multple solutons, frstl, to estmate whether the value of these jont angles s wthn the allowed scope, then b mn b to determne the most optmal soluton, usng the optmal soluton to calculate the net jont angle and determne the mnmum value of the net jont angle b the same wa.. Smulaton and verfcaton of knematcs. Smulaton of the robot workspace Workspace s an mportant knematcs ndees for assessng the robot ablt to work. The most common method to calculate the workspace s the monte-carlo method [], the ccle strateg s used to calculate the robot workspace n ths paper. The poston vector of robot end-effector s gven n Eq.(), the scope of the workspace s determned b the poston vector, accordng to p p p and the scope of the robot jont angle to determne the robot workspace. The specfc steps are: () The poston vector: p = cc + 79cs + 7cc + c, p = sc + 79s s + 7sc + s, p = s 79c + 7s. and () The range of the jont angle s as follows: 7 ( π π, π π, π π, π π, π π, π π ) 8 9 9 8 8 all the end-effector coordnates wthn the change scope of the jont angle can be obtaned. () The workspace result s shown n Fg.. (a) XYZ D workspace (b) XOY D workspace (c) YOZ D workspace (d) XOZ D workspace Fg. The robot workspace. Verfcaton of the robot knematcs Frstl, a moton trajector s gven. The trajector wthn s to s: p = sn( t) ; p = ( t sn( t)) + 7 ; = ( s( t)) + from Fg.(a). Secondl, keep the orentaton of the robot end-effector as R = [ p ; ; ]. Accordng to solvng nverse knematcs of MOTOMAN-HP, the changes of the s jont angles are shown n Fg.. B the forward knematcs equaton, the poston vector can be obtaned. The smulaton trajector of the manpulator s shown n Fg.(b). Fnall, comparng wth the result of Fg.(a) and Fg.(b), the trajector curve b the forward knematcs equaton s the same as the curve space gven n Fg.(a), the correctness of the forward and nverse knematcs soluton can be verfed n ths work. 8 - -.. 8 - -.. Fg. (a) The gven space spral curve; (b) The trajactor of the forward knematcs 879
.7 -..7.9 -..7.8 -..7.7 -. theta.7 theta. theta -..7. -..7. -.7.9. -.8.9 -.9 (a) the change curve of angle (b) the change curve of angle (c) the change curve of angle -.....9. theta theta.8.7 theta. -.. -.. -. (d) the change curse of angle (e) the change curse of angle (f) the change curse of angle Fg. The change of jont angle. Summar The knematcs of MOTOMAN - HP robot was studed n the paper, and a knd of method about the knematcs analss based on rotaton matr orthogonalt and matr block thought s put forward. B the smulaton, the robot workspace s calculated, and the correctness of the forward knematcs soluton and nverse knematcs model s gven to provde the theoretcal foundaton for the research of robot control and trajector plannng optmaton. Ths method can also be used to provde the knematcs reference for other ndustral robot wth s-degree- of-freedom. References []. Lu SG, Zhu SQ, et al. Research on real-tme nverse knematcs algorthms for R robots. Control Theor & Applcatons, 8, Vol. (8) No., p.7-. []. Wang QJ, Du JJ. A new soluton for nverse knematcs problems of MOTOMAN robot. Journal of Harbn Insttute of Technolog, vol. (), p. -. []. Fu KS, Gonale RC, Lee CSG. Robotcs: control, sensng, vson, and ntellgence. McGraw-Hll Educaton, 987. []. Lv SZ, et al. Soluton of Screw Equaton for Inverse Knematcs of R Robot Based on Wu's Method. Journal of Mechancal Engneerng, Vol. () No.7, p. -. []. Maorga RK, Sanongboon P. A radal bass functon net-work approach for geometrcall bounded manpulator nverse knematcs computaton, IEEE Internatonal Conference on Intellgent Robots and Sstems., p. -9. []. Huang W, Tan S, L X. Inverse Knematcs of Com-plant Manpulator Based on the Immune Genetc Algo-rthm, Fourth Internatonal Conference on Natural Computaton. IEEE Computer Socet, 8, p. 9-9. [7]. Manocha D, Cann JF. Effcent nverse knematcs for general R manpulators. Robotcs & Automaton IEEE Transactons on, Vol. (99) No., p. 8-7. [8]. Lu H. Real-tme Inverse Knematcs Algorthm Based on Vector Dot Product. Transactons of the Ch-nese Socet for Agrcultural Machner, 9, Vol. (9) No., p. -8. [9]. Cheng YL, Zhu SQ, Lu SG. Inverse Knematcs of R Robots Based on the Orthogonal Character of Rotaton Sub-matr. Robot, Vol. (8) No., p. -. []. Sclano B, Khatb O. Sprnger Handbook of Robotcs. Sprnger, 8. []. Feng Y, Wang YN, Yu HS. Workspace boundar etracton of de-cng robot based on Monte Carlo method. Control Theor & Applcatons, Vol.7 () No.7, p. 89-89. 88