An Algorthm to Solve the Inverse Knematcs Problem of a Robotc Manpulator Based on Rotaton Vectors Mohamad Z. Al-az*, Mazn Z. Othman**, and Baker B. Al-Bahr* *AL-Nahran Unversty, Computer Eng. Dep., Baghdad, Iraq **Mosul Unversty, College of echnology, Computer Eng. Dep., Mosul, Iraq ABSRAC hs paper presents an algorthm to solve the nverse knematcs problem for a complex wrst structure sx degree of freedom (6DO robotc manpulator. he last three rotatng axes do not ntersect at one pont and there are off axes n ts coordnate frames. he proposed algorthm based on the rotaton vector concept, whch s also used to descrbe the orentaton of manpulator end-effector. All the possble solutons of the nverse knematcs problem can be obtaned by usng the proposed algorthm whch s tested practcally on the MA robotc manpulator. INDEX ERMS Knematcs, Manpulator, Vector Analyss. I. ROAION VECORS he relatve orentaton representaton by the rotaton vector are based on the Euler theorem whch states that (a dsplacement of a rgd body wth one fxed pont can be descrbed as a rotaton about some axs. he rotaton vector s a vector pontng along ths axs, and ts magntude contans nformaton about the rotaton angle. Consequently, the rotaton vector s only common vector of any two coordnate systems (frames havng the same orgn and dfferng n ther orentaton. hs feature s appled to perform vector transformaton between two frames. he drecton of the rotaton vector defnes the axs around whch one coordnate system has to be rotated to acheve the same orentaton as the other system, whle ts magntude defnes the amount of the rotaton []. he rotaton vector concepts supposes the exstence of a common axs around whch one coordnate system has to be rotated to acheve the same orentaton as another coordnate system. A vector along ths common axs, whose magntude s defned by the amount of the rotaton around ths axs, s called the rotaton vector R whch s gven by []-[3]: θ R u * tan( u *f [R x, R y, R z ] ( where u: he unt vector along axs of rotaton. θ: Angle of rotaton about the u vector. f : Magntude of the rotaton vector, ftan(θ/. A vector v undergong a rotaton R to perform a vector v b can be descrbed as [],[ 3]: R (v R v vb v ( where R: he rotaton vector. v: he orgnal vector. v b: he rotated vector (the old vector n the new coordnate frame. : Cross product operator. x y R R R (3 Case study : Consder a robotc manpulator arm that s shown n g.. he grpper orentaton depends on the robot onts varables θ and θ. he orentaton of the grpper vector changes due to the moton of the robotc manpulator. Suppose the robot arm ntally at X axs and the grpper vector coordnate are ntally v[,,] correspondng to θ θ, whle the fnal poston of θ and θ are 9 o and 6 o respectvely. nd the fnal rotated grpper vector? he system has two rotaton vectors R and R correspondng to the angles of rotaton θ and θ. he frst rotaton R s performed around the Z axs and ts drecton s clockwse. he rotated grpper vector and the rotated rotatng unt vector of the second rotatng axs due to the frst rotaton R must be computed as follows: u [,, ] (4 where u s the rotatng unt vector of the frst rotaton operator. θ R [,, tan( ] [,, ] (5 *R (v R v v v b (6 b R x R y R z (7 b he rotated rotatng axs of the second ont due to the rotaton about the frst ont s computed by: *R (u R u u u b (8 b he second rotaton s done around the shfted Y axs and ts drecton s antclockwse so that a negatve sgn appears on the R. z u [,, ] (9
where u s the rotatng unt vector of the second rotaton operator. θ R [, tan(,] [,,] ( 3 6 R b u b *( f *( tan [,,] ( 3 *R b (vb R b vb vbb vb ( bb he results shows that the grpper vector whch concdes orgnally wth the Y axs, pont now to the negatve X drecton. Z g.. θ θ X Y v he grpper vector Coordnate of revolute robot wth the grpper vector. II. HE PROPOSED ALGORIHM O SOLVE HE INVERSE KINEMAICS he proposed algorthm dvdes the man task (soluton the nverse knematcs problem to subtasks to reduce the complexty of the soluton of the nverse knematcs of the robotc manpulator. he frst step n the proposed algorthm s to defne a set of a Cartesan coordnate frames at the robotc manpulator. Among these coordnate frames there s a one frame s called the global coordnate frame of the system, ths frame s fxed at a statonary pont to be as a reference coordnate frame for the other frames whch are called the local frames. At each ont n the robotc manpulator there s a local coordnate frame. he orgn of ths frame s fxed at the central pont of that onts and the orentaton of ths frame s the same as the orentaton of the global frame (concdence. he rotatng axs of each ont n the manpulator drven by an actuator s called the rotatng axs of that ont of the manpulator. or each rotatng axs there s a unt vector u along the axs of rotaton of that ont whch can be defned as u [u x,u y,u z ], where u x, u y and u z are the unt vectors of the rotaton n the local coordnate frame of that ont whch has the same orentaton of the global coordnate frame. he poston of the end-effector (grpper of the robotc manpulator can be defned as: p [p x,p y,p z ], where the p x, p y and p z are the coordnates of the pont p n the global XYZ coordnate frame. he orentaton of the end-effector of the robotc manpulator can be descrbed by defnng the grpper vectors (components of the grpper of the manpulator. he number of the grpper vectors s equal to the rotatng axes n the grpper structure. Each grpper vector s defned wth respect to the local coordnate frame of ts ont. here are two methods to descrbe the orentaton of the grpper n ths algorthm. In the frst method the descrpton of the grpper s acheved by drect descrpton of the desred grpper vectors, so that t s called drect orentaton descrpton. he ndrect orentaton descrpton s the second method of orentaton descrpton. In ths method the desred orentaton of the grpper s descrbed by defnng the rotaton angles about each rotatng axs n the grpper structure startng from the actual (present orentaton of the grpper to the new orentaton. Case study : Suppose the end-effector of a robotc manpulator wth three onts as llustrated n g. he frst local coordnate frame s fxed at pont (A wth the same orentaton of the global coordnate system. he frst rotaton unt vector u can be defned as u [,,] because the rotaton of ths ont at ths orentaton s done about the X axs. he frst grpper vector wll be v [,,a] where (a s the length of the frst lnk n the grpper structure. he second local coordnate frame s fxed at pont (B. he second rotaton unt vector u s gven by u [,,] ; whle the second grpper vector s v [,,b] where (b s the length of the second lnk n the grpper structure. he thrd local coordnate s fxed at pont (C whch gves the thrd rotaton unt vector u 3 [,,], and the thrd grpper vector v 3 [,d,d] or v 3 [,-d,d] (v 3 s even symmetrcal about the thrd rotatng axs u 3. X G g.. Z G Y G Global coordnate frame X 3 C B Z 3 d d Z X Y A Z Y 3 X Y X 3 Y 3 Y X X he new orentaton. he grpper wth old and new orentatons. By usng the frst method of the orentaton descrpton the new grpper orentaton can be descrbed by defnng the new (desred grpper vectors v, v, and v 3. If t s assumed the new grpper vectors are gven by v [,- b he ntal orentaton. a Z 3 Z Z Y
a,], v [,,b],and v 3 [d,,d], then the new poston of the frst lnk n the grpper structure les at the negatve part of the Y axs, whle the second lnk les at the postve part of the Z axs and the thrd lnk les n the X 3 Z 3 plane at pont x±d and zd. he descrpton of the new grpper orentaton usng the second method of the orentaton descrpton can be done by defnng the rotaton angles about each one of the three rotatng axes n the grpper structure. hus the new orentaton can be obtaned from the frst orentaton by rotatng by an angle π/ around the frst rotatng axs whch concdent to X axs, then rotatng by an angle - π/ about the second rotatng axs whch concdent to X axs and fnally rotatng by an angle ±π/ about the thrd rotatng axs whch concdent to Z 3 axs. So that the rotaton angles are φ π/, φ -π/, and φ 3 ±π/ where φ, φ and φ 3 are the rotaton angles of the frst, second and thrd onts n the grpper. III. COMPUAION O HE DESIRED GRIPER VECOR When the ndrect orentaton descrpton of the grpper vector s used to defne the orentaton of the desred grpper vector, t s mportant to descrbe the grpper orentaton n the drect orentaton descrpton. here are two man steps to compute the desred grpper vectors v d n the drect form from the ndrect form. he frst step computes the actual (present grpper orentaton and actual (present grpper rotatng unt vectors. he second step represents the computaton of the desred grpper vector by applyng the rotaton operator about each rotatng axs n the actual (present grpper orentaton. As startng pont to compute the actual orentaton of the grpper vector, the reset poston and orentaton of the manpulator must be defned. It s recommended to make the reset poston and orentaton of the manpulator at the manpulator structure wth the all onts varables are set to zero. Accordng to the manpulator reset poston the reset values of the components of the grpper vector and the rotatng unt vectors of the grpper rotatng axes must be defned. Now, by usng the present values of the manpulator onts varables, the th actual (present grpper rotatng unt vector u a can be computed by rotatng ts value at the reset poston u about all the prevous rotatng axes n the manpulator by usng the rotaton vectors concept. Symbolcally t can be wrtten as: u a (...(((u... Rk Rk Rk 3 R (3 where,,...i(i: total number of grpper rotatng axes. k: Number of rotatng axes n the arm structure. u a : he th actual (present grpper rotatng unt vector. u : he th grpper rotatng unt vector at the manpulator reset poston. ( u R : Rotate the vector u about the th rotatng axs of the manpulator. R (u R u (u R u (4 θ R [R x, R y, R z ] u tan( u f (5 R R R x y z (6 he th actual grpper vector v a can be computed n the same manner but the rotaton of the th grpper vector at the reset poston v s perform startng from the th rotatng axs n the grpper endng at the frst rotatng axs n the arm structure. Symbolcally t can be wrtten as: va (...(((v... Rk Rk Rk R (7 where v a : he th actual (present grpper vector. v : he th grpper vector at the manpulator reset poston. ( v R : Rotate the grpper vector v about the th rotatng axs of the manpulator. R (v R v (v R v (8 θ R [R x, R y, R z ] u tan( u f (9 R R R x y z ( he frst desred grpper vector can be computed by rotatng the frst actual grpper vector about the frst rotatng unt vector by the value of φ. R (va R va vd va ( φ R [R, R, R ] u tan( x y z a ( R x R y R z (3 he second desred grpper vector s computed by rotatng the rotated second actual grpper vector v r about the rotated second rotatng unt vector u r by the value of. φ he rotated second actual grpper vector v r and the rotated second rotatng grpper unt vector u r are the results of applyng the rotaton operator on the second actual grpper vector v a and the second actual rotatng unt vector u a about the frst actual rotatng unt vector u a by the value of φ respectvely. R (va R va vr va (4
R (u a R u a u r u a (5 R (vr R vr vd vr (6 φ R [R, R, R ] u tan( x y z r (7 R x R y R z (8 he same procedure s appled to compute the other components of the grpper vector. IV. GENERAL SEPS O HE PROPOSED ALGORIHM O SOLVE HE INVERSE KINEMAICS PROBLEM he soluton of the nverse knematcs problem can be found by the followng steps:. he frst step, a Cartesan (rectangular rght handed coordnate system X Y Z s assgned to the base of the manpulator. hs coordnate represents the global coordnate of the manpulator system.. he robotc manpulator structure wll be dvded nto two man parts the frst part (arm ncludes the maor three axes and t extends to the rotatng axs of the fourth lnk at pont o b. hs pont (o b represents the end pont of the maor three lnks wth the off axs (f t exsts due to the fourth lnk structure. Whle the second part (wrst ncludes all the remander of the robotc manpulator structure (from the vrtual pont o b to the end of the end-effector. 3. Dervaton (by usng any mathematcal method the possble sets of the onts varables (angles of the arm structure that attan the end of arm (pont o b to a certan poston (that wll be computed later n step 8. Note that ths mathematcal calculaton s smple because t treats only three lnks and t represents a translaton operaton so that the orentaton of the end pont of arm (o b s not mportant when the end pont of the arm reaches the desred poston. 4. or each of the three maor rotatng axes n the arm structure, the rotaton vector wth respect to the base (global coordnate frame must be defned. 5. or the wrst structure, there are a Cartesan coordnate system at the begnnng of each rotatng axs n the wrst structure wth the same orentaton of the global coordnate system. hese coordnate frames are used to defne the rotatng unt vector of each rotatng axs n the grpper structure. Also there are another Cartesan coordnate frames wth the same orentaton of the global coordnate frame at the begnnng of each component of the grpper vector. hese coordnate frames are used to descrbe the grpper vectors. 6. Computaton the rotaton vectors of the grpper structure for each rotatng axs. Also the ntal grpper vectors and the ntal grpper rotatng unt vectors at the reset poston of the manpulator must be defned. 7. f the ndrect method s used to descrbe the grpper orentaton, the new (desred grper vectors v d must computed as descrbed n the prevous secton. Also the total desred grpper vector v d must be computed as: n v d v d (9 where n s the number of grpper vectors. 8. Determnaton (by usng the equatons whch were derved n step 3 the requred poston of pont o b from the desred grpper vector v d (whch s computed n step 7 and the desred poston of the grpper p as: ob p v d (3 9. Determnaton the sets of the possble solutons of the onts varables of arm structure of the robotc manpulator that s requred to attan the pont o b to the new poston (whch s computed n step 8.. Repetton of steps,, 3, and 4 for each set of solutons of the arm (frst part onts varables that are computed n step 9.. Repetton steps, 3, and 4 for each ont n the wrst structure, startng wth.. Usng the values of the arm ont varables (that are computed n step 9 and the ntal poston of the th grpper vector, calculate the ntermedate values of the th rotated grpper vector v m and the th ntermedate rotatng unt vector u m f the arm s moved form the reset poston to the new poston as computed n step 8. 3. rom the desred grpper vectors v d and the ntermedate grpper vectors v m calculate the requred value of θ whch makes v m concdent to v d by usng the followng formulas: R (vm R vm vd vm (3 R R R x y z (3 θ R [R x, R y, R z ] um tan( u mf (33 substtuted (3 n (33 and solve for f, yelds: umf (vm u mf vm vd vm (34 then θ tan (f (35 4. If I ( no. of the lnks n the wrst structure, then, and append the computed value of the angle θ n step 3 to the arm ont varable and apply the steps -4 to compute θ, and so on. 5. he total onts varables wll be [θ, θ, θ m, θ 4, θ 5, θ n ], where θ, θ θ m are computed n step 9 and
θ 4, θ 5 θ n are computed n steps to 4, and m, n are the number of onts of the arm and wrst respectvely. IIV. SOLUION O INVERSE KINEMAICS PROBLEM O MA MANIPULAOR USING PROPOSED ALGORIHM he proposed algorthm s tested practcally on the MA robotc manpulator, whch s a 6DO wth complex wrst structure as shown n gs. 3, 4. E Z st rotatng axs X Z he schematc dagram of the MA at the reset poston s shown n the g. 5. If t s requred to move the grpper to a new poston [-5,-,5] cm wth respect to the base (global coordnate frame. he desred orentaton of the grpper can be obtaned by rotates the grpper 8 o about the ptch rotatng axs, 5 about the rotated yaw rotatng axs and fnally 35 about the rotated roll rotatng axs. he problem s to determne the values of the sx ont varables of the MA that attan the central pont of the grpper to the desred poston wth the desred orentaton. By applyng the steps n prevous secton, four possble sets of solutons of the nverse knematcs of the MA manpulator can be found. hus there are four sets of the onts varables [θ, θ, θ 3, θ 4, θ 5, θ 6 ] that make the MA attans the desred poston wth the desred orentaton whch are gven n table. he sketch of MA wth these onts varables (solutons are shown n gs. 6, 7, 8, and 9. Y -Z 3 rd rotatng axs Y Z X X -Z nd rotatng axs -Y X 6 Z 6 Y 6 Y g. 3. MA robotc manpulator wth the maor coordnate frames. x 6 th rotatng axs E z 9 Roll 9 Ptch -X g. 5. Sketch of the MA at the reset poston. ABLE I HE OUR SES O SOLUIONS O HE INVERSE KINEMAICS PROBLEM OR MA MANIPULAOR HA AAINED I O POIN [-5,-,5 cm]. θ (deg. θ (deg. θ 3 (deg. θ 4 (deg. θ 5 (deg. θ 6 (deg. st set 97.35-7.89 79.77 8. 7.64 35 nd set 97.35 75.87-79.77 83.9 7.64 35 3 rd set -. 87.89-79.77 7.88 6. 35 4 th set -. 4. 79.77 356.9 6. 35-9 Roll R x 5 y - 9 z 5 C y 4 z 4 x 4 ob z 3 P off x 3 y 3 Y off 5 th rotatng y 5 9 Yaw 4 th rotatng axs -9 Ptch g. 4. Geometrcal structure of the end-effector (wrst of the MA robotc manpulator. g. 6 Sketch of MA manpulator at ont varables [97.3564, -7.8977, 79.7788, 8.89, 7.6436, 35 ].
the nverse knematcs problem of the manpulator whch equals to [,9,,,9, ] as t s shown n g.. g. 7 Sketch of MA manpulator at ont varables [97.3564, 75.87, -79.7788, 83.986, 436, 35 ]. g.. Sketch of the MA robotc manpulator at the ont varables [, 9,,, 9, ] g.8. Sketch of MA manpulator at ont varables [-.96, 87.8977, -79.7788, 7.88, 6.96, 35 ]. g. 9. Sketch of MA manpulator at ont varables [-.96, 4.98, 79.7788, 356.94, 6.96, 35 ]. It s mportant to know that there are not always four sets of solutons. Some tmes there are three, two, one, or no solutons n solvng the nverse knematcs problem of MA manpulator. or example f the desred poston of grpper of MA s [4.4,-5,8] cm wth new orentaton correspondng to rotaton by angles 9, 9 and about ptch, yaw and roll axes wth respect to reset poston of MA. he algorthm gves only a sngle soluton (due to physcal structure of the manpulator for VIII. CONCLUSIONS he proposed algorthm to solve the nverse knematcs of the robotc manpulators has the followng features: he algorthm s based on the rotaton vectors concept.. It can be appled to most types of the robotcs manpulators nclude the complex structure robotc manpulators whch have hgh DO.. It gves all the possble sets of the onts varables of robotc manpulator that attan the endeffector of the manpulator to the desred poston and orentaton. 3. It has a less computaton complexly than other method whch uses the algebrac soluton by usng trgonometrc (non lnear equatons or the numercal technques to fnd the soluton of the nverse knematcs problem. hus the executon tme and the computatonal rate of the proposed algorthm are less than other methods such as the numercal method so that ths algorthm s more sutable to use n the real tme applcatons. he algorthm s used practcally to solve the nverse knematcs problem of the MA robotc manpulator. he results gve four sets of possble solutons by usng the proposed algorthm. Some tmes there are one, two, three, or no solutons dependng on the physcal structure of the manpulator and the desred poston and orentaton of the end-effector of the manpulator. REERENCES [] Y. Koren, "Robotcs for Engneers," McGrw Hll, 3 rd edton, 987. [] Md. Shah Alam, "Rotaton vectors," Journal of heoretcs, vol 3.4, Inc. [3].. Wener, "heoretcal analyss of gmballess nertal reference equpment usng delta modulated nstruments," Ph.D. thess, MI, Cambrdge, Mass, 96.