Development and Sensitivity Analysis of a Portable Calibration System for Joint Offset of Industrial Robot

Transcription

1 The 009 I/RSJ Internatonal Conference on Intellgent Robots and Sstems October 11-15, 009 St. Lous, USA Develoment and Senstvt Analss of a Portable Calbraton Sstem for Jont Offset of Industral Robot Yong Lu, Nng X, Janguo Zhao, rc Neves-Rvera, Yun Ja, Bngtuan Gao, Jun Lu Abstract - Ths aer descrbes our udated sstem for ndustral robot jont offset calbraton. The sstem conssts of an IRB1600 ndustral robot, a laser tool attached to the robot s end-effector, a ortable oston-senstve devce (PPD), and a PC based controller. B amng the laser sot to the center of oston-senstve-detector (PSD) on the PPD wth dfferent robot confguratons, the develoed sstem deall mlements our roosed calbraton method called the vrtual lne-based sngle-ont constrant aroach. However, unle our revous aroach, the calbraton method s extended to dentf the offset arameters wth an uncalbrated laser tool. The oston errors of the PPD and the senstvtes of error n the PSD lane to the varaton of jont angles are analed. Two dfferent robot confguraton atterns are comared b mlementng the calbraton method. Both smulaton and real exermental results are consstent wth the mathematcal analss. xermental results wth small ( ) mean and standard devaton of arameters error verf the effectveness of both the senstvt analss and the develoed sstem. Index Terms robot, jont offset calbraton, senstvt, develoment I. INTRODUCTION It s well nown that robot has hgh reeatablt but low accurac. Researchers have been worng to mrove the accurac of ndustral robots. Wth the ndustral robot wdel used n the comlcated tass requrng contnuous ath or recse localaton, eg., arc weldng, cuttng, surger, and anthng else based on offlne rogrammng, etc., accurac of the robot s more and more mortant. Although there are man sources of naccurac, such as gear errors, thermal exanson, and structural deformatons, the man source of the naccurac les n the arameter errors of robot nematcs model. Robot calbraton s an effcent wa to mrove the accurac. There has been consderable research n ths feld. Robot nematc arameters calbraton methodologes and sstems have been develoed [1]-[5]. Ths research wor s artall suorted b ABB Unverst Research Program. Yong Lu s wth Deartment of lectrcal and Comuter ngneerng Mchgan State Unverst ast Lansng, MI 4884, USA, and Nanjng Unverst of Scence and Technolog, Nanjng, Chna (e-mal: lu160@ msu.edu) Nng X s wth the Deartment of lectrcal and Comuter ngneerng, Mchgan State Unverst, ast Lansng, MI 4884, USA, and also servng as an afflate rofessor at the Shenang Insttute of Automaton, Chnese Academ of Scence, Shenang, Chna (e-mal: xn@egr.msu.edu). Janguo Zhao, rc Neves-Rvera, Yun Ja, Bngtuan Gao and Jun Lu are wth Deartment of lectrcal and Comuter ngneerng, Mchgan State Unverst ast Lansng, MI 4884, USA (e-mal: {haoja1, neveser, jaun, carlgao, lujun331}@msu.edu). One nd of method reles on hghl recse equment measurng the robot end-effector ose, eg., coordnate measurement machnes (CMM) [1] and laser tracng sstem []. However, the rocess s tme/manower consumng, and the devce s exensve. The other nd of method moses some constrants on the end-effector to form closed nematc chans [3] [4] [5]. These methods suffer from nexact ostonng and tme consumng. Newman et al. [6] and Chen et al. [] roosed a calbraton method usng laser lne tracng. Gatla et al. [8] descrbed the vrtual closed nematc chan method. The gave the smulated results and dd not mlement a ractcal calbraton sstem. In addton, once the robot s shed from the robot manufacture and nstalled for the user, some nematc arameters, such as the ln length, ln twst and ln offset, related to the mechancal structure of the robot tself, do not change too much, tcall. However, some nematcs arameters such as jont offset mght be changed more often because of the assembl or the relaced motors and encoders. What s more, the jont offset change onl a lttle, then the ostonal accurac s affected sgnfcantl. Accordng to [6][], more than 90% of the ostonal naccurac ssues of the ndustral robot are caused b the robot offset. To ft ths requrement of jont offset calbraton, a new arameter calbraton aroach called vrtual lnes-based sngle-ont constrant (VLBSPC) s roosed and a calbraton devce based on PSD and laser was mlemented n our aer [9]. Unle revous calbraton methods, ths aroach does not need an hscal contact and the develoed devce s affordable. The roosed method deends manl on a laser onter attached on the end-effector of a robot and onl one PSD. However, t s essental to develo a ortable and affordable devce for ndustral robot jont offset calbraton that can be used wdel and frequentl n the user factor, not onl n the robot factor. That s to sa, an offset calbraton sstem that s fast, automated, and hghl recse, most mortant, more low-cost and comact desgn wll have hgh demand n manufacturng usng ndustral robots. In ths aer, the newl develoed calbraton devce s mlemented and resented n detal. Based on the latform, the feedbac errors of the PPD and the senstvtes of varatons of jont values to the robot confguraton and PSD oston are analed. In addton, arameters of the laser lne are changed often because of the nstallaton and laser sot adjustment. Hence, /09/$ I 3838

2 n ths aer calbraton arameters were extended to both jont offset and the laser lne arameters at the same tme. Calbraton exerments referrng to the confguratons of the robot and PSD oston are dscussed. One s the locaton of the PSD changed, and the other s the dfferent tes of ostons and orentatons of end-effector loadng the laser lnes amng towards the center of the PSD. Both smulaton and real exerments mlemented on an ABB ndustral robot (IRB1600) verfed the effectveness of both the roosed method and the develoed sstem. Ths aer s structured as follows: the udated calbraton sstem s resented n Secton II. The modelng, mang, errors, and senstvtes analss of offset calbraton are descrbed n Secton III. Both smulaton and exermental results are demonstrated n Secton IV. Fnall, we conclude the wor. II. PORTABL CALIBRATION SYSTM Fg. 1 shows the schematc desgn of robot offset calbraton sstem, whch s mlemented and verfed on an ABB robot as laborator test-bed, shown n Fg.. The robot ncludes an ABB robot controller (IRC5) and a 6-DOF manulator (IRB1600). oston-senstve devce s equed wth batter, DC ower, PSD nterface tunng crcut, USB analog acqurng module and USB wreless devce. A sngle segmented PSD s emloed and mounted on the fxture. The fxture s arbtrarl located on the robot worsace. The center ont of the PSD s suosed to be the sngle-ont constrant. The nterface crcut s well desgned and the sgnal tunng board can rocess the raw outut of the laser sot on the PSD surface for two-dmenson oston feedbac. USB-1408FS s used to acqure the analog sgnal from the rocessng board. PC-based controller erforms collectng PSD outut b wreless, PSD-based ostonng servo and calbraton algorthm. Through the networ-based communcaton between robot controller and the PC-based controller, the latter can obtan the current robot oston nformaton (tas sace and jont sace) from the robot controller and send the control command to the robot controller as well as udate the target oston n real-tme. Adator X Y Z nd-effector frame {} Focusable laser and adater Laser beam Portable devce A focusable laser Adater Z Y Robot base frame {B} Robot X Crcut board Robot controller LAN PSD USB A/D PC-based controller Fg.1. Schematc of the ortable calbraton sstem Batter ower USB wreless hub USB wreless (a) The laser and adater attached on the end-effector Sngle Segmented PSD USB wreless hub (b) The ortable calbraton devce Fg.3. Newl develoed calbraton devce III. CALIBRATION MODLING AND ANALYSIS OF RRORS Fg.. ABB robot and newl develoed calbraton sstem The calbraton sstem conssts of batter-owered laser and laser adater, as shown n Fg. 3(a), and ortable oston-senstve devce, as shown n Fg. 3(b). The ortable A. Knematc rror Model The Denavt-Hartenberg [10] s a wdel used conventon for frame of reference n the forward nematcs. A model of the IRB1600 robot accordng to DH conventons s bult [9]. Consder the jont offset, let δ denote the offset value of the th jont, n the DH conventon each homogeneous transformaton s reresented as, 3839

3 = T 0 0 where use notaton cα cα sα 0 sα sα cα 0 for cos( θ δ ) a a d 1 + and (3) for sn( θ + δ). Combnng the jont offset and the sx coordnate frames, forward nematcs b T e wth the offset error s wrtten as, = TT T T T T. (4) b T e B. Uncalbrated Laser Tool 1 A laser tool, a focusable laser onter wth ts adater, s rgdl attached on the end-effector of the robot. The laser lne s adjusted to roughl algn ts orentaton toward the x-axs n the end-effector frame. Let ( Μ, N, P) denote the unt drecton vector and ( x0, 0, 0) denote the oston of a ont of the laser lne n the end-effector frame. Once the laser onter and the adater s fxed, the laser lne n the end-effector frame s gven b x x0 0 0 = =. (5) M N P Note the values of ( Μ, N, P) and ( x0, 0, 0) are unnown. However, the number of ndeendent arameters s 4 for the laser lne (let κ = { M, N,, 0 0 }). One of vector s deendent and x 0 s the offset along the x-axs n the end-effector frame (let x 0 =0). Jont 1 s deendent to the base frame and jont 6 s deendent to the arameter of laser lne. Thus, the total number of calbraton arameters to be dentfed s 8 (4 jont offsets + 4 laser lne arameters). C. Calbraton Pattern and Calbraton Method b b xb Robot base frame {B} Lne Lne 1 Laser 3 Pattern II Lne Pattern I Laser beam Fg.4. Schematc of calbraton rocedure Lne N PSD The calbraton method reresented n [9] reles manl uon a laser onter attached on the end-effector of a robot and sngle PSD. The calbraton rocedure s amng a laser beam from the laser onter at the same ont from varous ostons and orentatons b usng hbrd vsual/psd servong [11], as shown n Fg. 4. The same ont s the center ont of the PSD, and the coordnates of the ont n the robot base frame are unnown. The calbraton attern of amng the laser at the ont from rght sde of the PSD to left sde s called attern I confguraton. Corresondngl, that from the bac to the front s called attern II confguraton. It s obvous that these atterns determne the robot confguraton. The confguraton effect on calbraton soluton wll be dscussed n the next secton. Suose N sets of jont angle are recorded after PSD based localaton. Combng the matrx equaton (4) and equaton (5), laser lnes wll be reresented n the robot base fame. Let Γ L denote the th laser lne, Ρ denote the ntersecton or the center of the shortest dstance between Γ L and Γ Lj ( j,, j = 1, L, N, = 1, L, M ), and n Ρ Ave denote the mean ont of the total ntersectons Ρ ( = 1, L, M ). The coordnate errors of the onts between Ρ and n Ρ Ave are x denoted as Ψ, Ψ, Ψ n the x,, drectons, searatel. The arameters Φ ( δ, κ ) of jont offset and laser lne are dentfed b mnmng the total sum of the squares of the coordnate errors. M x Φ ( δκ, ) = argmnφ ( δκ, ) ( Ψ + Ψ + Ψ ) (6) = 1 where M s the number of the ntersectons between laser lnes. Note n Ρ Ave s udated durng the mnmaton teraton rocess and Ρ s the center of the lne of the shortest dstance from the lnes between Γ L and Γ Lj f the two lnes do not have a real ntersecton. The soluton for the non-lnear otmaton s Levenberg-Marquardt algorthm (LMA) []. The otmum algorthm s a damed Gauss-Newton method based on the Jacoban J and damng arameter µ. The ste h lm s defned b T J J + µ I h = g () where g ( ) lm T T = J Ψ, Ψ = [ ψ 1, ψ L ψ ], = 1, L, M ; x = ψ + ψ ψ and 0 ψ + µ. D. Measurement Senstvtes of the PPD The actve area of the PSD s 10mm n dameter and the laser sot shootng on the PSD surface s about.5mm n dameter. To ma the PSD sensng devce (both PSD and the condtonng crcut), a laser onter was fxed on a comuter aded robe (Sgnatone CAP 945) erendcular to the PSD surface. The robe allows t to oston the laser beam n DOF (x, ) crossng the PSD surface wth hgh recson (accurac: ±.5 µm, reeatablt: ±1 µm, resoluton: 40 nm er ste). The result of the whole actve area sweeng along the -axs s shown n fgure 5(a) and that of the central area s shown n fgure 5(b). From the fgure 5(a) and 5(b), the 3840

4 outut voltage of the devce s nonlnear to the oston of laser sot on the PSD and the central area of the PSD has much better lneart and hgh senstvt than other area. At the case of robot calbraton, the center of PSD s desred oston of robot localaton. From fgure 5(b), the senstvt s about 0.1 volt er 0µm wthn the central area (from 0.6mm to 0.6mm). Based on observaton from exerments, the measurement nose s wthn ±0.01 volt and consequentl the error of the oston s ±µm n the x-axs and -axs, resectvel. sot ostons n the PSD lane to varatons of jont 3. Other jont angles have the smlar magntude. However, note that the senstvtes var wth the robot confguraton and actuall t deends on the calbraton sstem Jacoban. Ths s not dscussed n the aer. Fg.6. Changes n the PSD lane wth varaton of jont 3 angle. (a) Fg. 5. (a) Plot of the data of swee along the -axs n the whole actve area. (b) Plot of the data of swee along the -axs n the central area. (b). Localaton Senstvtes Analss n Jont Sace The PSD has the hgh resoluton wth better than 0.1µm n theor, and actuall the PPD has the resoluton of aroxmatel µm under ths exermental condton. The accurac of robot localaton should be between ±µm and ±50µm (reeated accurac) based on PPD feedbac. Thus t s mortant to now the senstvtes of the varatons of the laser sot ostons n the PSD lane to varatons of jont angles at the robot jont sace. Let denote the senstvtes and L denote varaton of the laser sot n dstance wth the varaton Θ of jont angles. Then the senstvtes s gven = L Θ. (8) The oston and orentaton of the end-effector can be reresented b ( Px, P, P, φ, θψ, ) wth resect to the base frame. The laser s fxed on the end-effector and roughl arallel to the x-axs n the end-effector frame. The dstance between the laser onter and PSD surface s varng and aroxmatel l = 600 mm. It s obvous the small varatons n orentaton of laser onter n the end-effector are magnfed on the PSD surface. Wthout loss of generalt, assume the PSD s arallel to the X-Y lane n the base frame and robot locates n the ntal oston. The end-effector (jont 4) s rotated ζ about X-axs w.r.t the base frame. Hence, the varaton of the laser sot on the PSD surface n the -axs s P =l sn( ζ ) (9) Let the varatons P = 0. 0 mm, then ζ =0.00 degree In ths case, f the error from robot localaton based on PSD servong s less than 0.0mm, the error of jont 4 wll be less than 0.00 degree. Fg. 6 shows the varatons of the laser F. ffect of Confguraton on the Soluton Based on the PSD feedbac, the laser beam recsel amed on the center (unnown oston) of the PSD at attern I or attern II. Then N sets of jont angles are recorded, and LMA s emloed to dentf the arameters. The convergent ste hlm s determned b () and s senstve to the Jacoban J (note t s dfferent from manulator Jacoban). The oston of PSD and the calbraton attern la an effect on the Jacoban. It can be roved mathematcall b the relatonsh between the change n the ntersecton of two laser lnes and the varaton of jont angles. Wthout loss of generalt, assume amng a laser beam at the same ont P from two ostons P and A P B. Accordngl the jont angles are recorded as ( θ θ, θ, θ, θ,,) at P and A 11, θ61, θ, θ3, θ4, θ5, θ6, P ( ) θ at, resectvel. Let B P coordnates n the base frame be x P =. Substtutng the jont angles nto (4) and the ont P wth dfferent orentaton can be reresented b 1 1 P ( θ 11, θ, θ31, θ41, θ51, θ61) = T1T T3T4T 5T6T (10) P ( θ, θ, θ3, θ4, θ5, θ6 ) = T1T T3T4T 5T6T (11) where T denote the transformaton matrx of the laser sot frame w.r.t the end-effector frame. Let T denote the transformaton matrx of the laser sot w.r.t the jont frame. Then at the ostons P and A P, B T can be reresented as a11 a a13 1x b11 b b13 x 1 = a a a3 1 T b b b3 () T =. a 31 a3 a33 1 b 31 b3 b Consder the same ont constrants at the two oston, we have 3841

5 x = 1x (13) = x x = 1x (14) = = 1x (15) = x If the center of PSD locates on the x-axs n the base frame, then = 0, and f attern II (robot move from the front to the bac) s used, thenθ 11 = θ = 0. Thus (13) and (15) can be gven = = (16) x 1x 1 x + = 1x = x (1) If jont has a same varaton θ, let ( 16) c θ + (1) s θ and ( 1) c θ (16) s θ, then we have x = 1x = x = 1x 1 +. = x where θ = θ + θ. These results rove that n ths case x Ψ, Ψ, Ψ are totall not related to the varaton of jont and thus jont offset can not be dentfed. Furthermore, f 0 and attern II s emloed, then x θ 11 = θ 0. Ψ, Ψ, Ψ are stll not senstve to the varaton of jont. If there are some noses added to the jont angles, the real arameters are dffcult to be dentfed because of local mnmum. ven f s not close to ero, the change of jont 1 angle s ver small under the attern II, then the jont 1 angle s close to a constant. Thus t stll suffers from the smlar result. However, t s free from the roblem under attern I because the jont 1 angle changes a lot. IV. XPRIMNTAL RSULTS AND DISCUSSION A. Robot Localaton The real robot s lmted to encoder resoluton and nose. xerments of robot localaton were mlemented on the IRB1600 ndustral robot. Manulator s roughl located wthn the actve area of PSD surface wth re-desgned ose b vsual servo control. Thus controller s swtched to PSD-based servo that recsel ams the laser lne at the center of the PSD. At dfferent unnown PSD oston #1 and #, the exerment of robot localaton s reeated multle tmes wth seven dfferent end-effector ostons and orentatons at attern I and attern II, resectvel. Table I shows the standard devaton of errors n jont sace for robot localaton at oston #1 and oston #. From the table I, generall the standard devaton of errors was ver small (from 10 - to 10-3 ), ndcatng that the robot localaton based on PSD feedbac s stable and recse. In addton, for attern I, the standard devaton of errors at the oston # s much smaller than that at the oston #1. However, the errors are close at oston #1 and # for attern II. These results also show that the senstvtes of varaton of jont values to localaton errors rel on the robot confguraton and the PSD ostons. TABL I TH RSULTS OF ROBOT LOCALIZATION IN JOINT SPAC Jont Pattern II STD ( 10-3 ) Pattern I STD ( 10-3 ) #1 # #1 # J1 (deg) J (deg) J3 (deg) J4 (deg) J5 (deg) J6 (deg) B. Calbraton wth Dfferent Robot Confguratons and Nos Data The ndustral robotc manulator IRB1600 was created n smulaton usng ts DH arameters [9]. The laser onter was fxed on the end-effector toward the X-axs n the end-effector frame, wth the accurate arameters of ( Μ = 1, N = 0, P = 0) and ( x0 = 0, 0 = 0, 0 = 0). The center of PSD was located at the oston (800mm, 100mm, 600mm) w.r.t robot base frame (t s unnown for the soluton). A vrtual PSD was bult as a feedbac to locate the laser beam on the center of the PSD surface. A real robot jont has lmted resoluton and robot localaton nose even though the recse PSD-based feedbac sstem s used. Therefore, we add nose to the jont and the oston of the PSD n order to mae the smulaton more realstc. Combng the encoder resoluton and oston sensor nose (±µm), we can safel assume the maxmum nose of robot localaton s ± 0.05mm (equal to the reeated accurac). Let PSD locate at dfferent ostons and laser amng at the PSD center wth eght ostons and orentatons rotatng about x-axs and -axs n the base frame, resectvel. Coordnates of PSD center are P1 ( 900 mm, 50 mm, 400mm ) w.r.t robot base frame (t s unnown for the soluton). Table II shows the results of the calbraton wth the PSD oston of P1. Column shows the actual offset arameters used b the smulaton. Column 3 shows the mean error of the arameters dentfed b reeatng the exerment fve tmes wth random localaton error wthn ± 0.05mm. Column 4 shows the standard devaton of the arameters errors wth resect to the actual values. Column 5 and Column 6 show the mean and standard devaton of the arameters errors b reeatng the exerment wth attern I confguraton. From column 3 and column 4 mean error and standard devaton error of jont are 1. degree and 0.56 degree, whch s far awa from the actual value of 0.4 degree. Results show offset arameter of jont can not be dentfed and consequentl the offset arameter of jont 3 s affected wth attern II. From column 5 and column 6 mean error and standard devaton error of all arameters are small (10 - ) wth the bg localaton nose of ± 0.05mm. It verf the calbraton method s feasble wth the otmum attern I 384

6 confguraton. Results justf robot confguraton las a bg effect on the calbraton soluton. The exermental result s consstent wth the mathematcal analss n secton III-F. TABL II SIMULATION RSULTS WITH NOISY DATA AT P1 attern II attern I Parameters Actual Value confguraton Mean rror ( 10 - ) STD ( 10 - ) confguraton Mean rror ( 10 - ) STD ( 10 - ) δ (deg) δ (deg) δ (deg) δ (deg) M N (mm) (mm) C. Calbraton xerment of IRB1600 Robot The calbraton exerment was mlemented on the ABB manulator IRB1600. Based on the oberservaton of the smulaton, attern I confguraton and mxed attern I and attern II (four oses from attern I and other four oses from attern II) were chosen. The exerments are reeated mutle tmes. TABL III OFFST CALIBRATION RSULTS OF IRB1600 ROBOT Parameters Intal Values Mean STD( 10 - ) δ (deg) δ (deg) δ (deg) δ (deg) M N (mm) (mm) Table III shows the results of the offset calbraton exerment mlemented on the ABB manulator IRB1600. Column 3 shows the mean of arameters, and column 4 shows the standard devaton of the arameters from reeated exerments. The standard devaton of the arameters was small (10 - ), ndcatng the stablt of the calbraton method. V. CONCLUSIONS Robot jont offset has a much larger nfluence on robot ostonng accurac after leavng the robot factor for the end user. Usng our roosed vrtual lnes-based sngle-ont constrant aroach, a ortable, low-cost, batter-owered, wreless and automated calbraton sstem was mlemented. The mang of the PPD was erformed, and the error was analed. The errors n jont sace are magnfed n PSD lane, and consequentl the resoluton n the jont sace s mroved. The senstvtes of the calbraton sstem were formulated mathematcall, and verfed attern I was more effcent. Two tes of atterns were exermented, and exermental results show attern I confguraton was a better confguraton for the calbraton ssue. Both smulaton and exermental results verfed the feasblt of the senstvt analss and demonstrated the method can dentf jont offset wth un-calbrated laser tool arameters. ACKNOWLDGMNT The authors would also le to than Dr. George Zhang, Dr. Heng Chen, Dr. Thomas A. Fuhlbrgge, and Ms. Xong L of ABB for ther techncal advce and hel durng the rocess of ths research. RFRNCS [1] M. R. Drels, L. W. Swae, and L. S. Potter, Full-ose calbraton of a robot manulator usng a coordnate measurng machne, Int. J. Adv.Manuf. Techno., vol. 8, no. l, , [] M. Vnce, J. P. Prennnger, and H. Gander, A laser tracng sstem to measure oston and orentaton of robot end effectors under moton, Int. J. Robot. Res., vol. 13, , [3] M. A. Meggolaro, G. Scrffgnano, and S. Dubows, Manulator calbraton usng a sngle endont contact constrant, resented at the 6 th Bennal Mech. Robot. Conf. 000 ASM Desgn ng. Tech. Conf., Baltmore, MD, Se [4] H. Zhuang, S. H. Motaghed, and Z. S. Roth, Robot calbraton wth lanar constrants, n Proc. I Int. Conf. Robot. Autom., Detrot, MI, 1999, [5] M. Its and J. M. Hollerbach, Knematc calbraton usng a lane constrant, n Proc. I Int. Conf. Robot. Autom., 199, [6] W. S. Newman and D.W.Osborn, A new method for nematc arameter calbraton va laser lne, n Proc. I Int. Conf. Robot. Autom., 1993, vol., [] H. Chen, T. Fuhlbrgge, S. Cho, et al. Practcal Industral Robot Zero Offset Calbraton, I Conference on Automaton Scence and ngneerng, 3-6 Aug [8] C. S. Gatla, R. Luma, J. Wood, and G. Starr, Calbraton of ndustral robots b magnfng errors on a dstant lane, resented at the I Int. Conf. Intell. Robots Sst., San Dego, CA, Oct. 9 Nov. 00. [9] Y. Lu, N. X, G. Zhang, X. L, H. Chen, C. Zhang, M.J. Jeffer and T.A. Fuhlbrgge. An Automated Method to Calbrate Industral Robot Jont Offset Usng Vrtual Lne-based Sngle-ont Constrant Aroach. Acceted to the I Conference on Intellgent and Robots and Sstems, 009. [10] J. Denavt, R.S. Hartenberg. "A Knematc Notaton for Lower Par Mechansms Based on Matrces, " J. Al. Mech. ASM, June 1955,. 5-. [11] Y. Lu, N. X, Y. Shen, et al. "Hgh-Accurac Vsual/PSD Hbrd Servong of Robotc Manulator," I /ASM nternatonal conference on AIM, -5 Jul 008. [] Kenneth Levenberg. "A Method for the Soluton of Certan Non-Lnear Problems n Least Squares". The Quarterl of Aled Mathematcs :