Development of a FPGA-based Motion Control IC for Robot Arm

Size: px

Start display at page:

Download "Development of a FPGA-based Motion Control IC for Robot Arm"

Deirdre Whitehead
6 years ago
Views:

1 Development of a FPGA-based oton Control IC for Robot Arm Yng-Sheh ung, ember IEEE Southern Tawan Unversty of Technology No. Nan-Ta Street, Yung-ang Tanan County, 7, TAIWAN kung@mal.stut.edu.tw Gua-Sheh Shu Southern Tawan Unversty of Technology No. Nan-Ta Street, Yung-ang Tanan County, 7, TAIWAN m9@mal.stut.edu.tw Abstract A new generaton of Feld Programmable Gate Array (FPGA technologes enables to ntegrate an embedded processor IP (Intellectual Property and an applcaton IP nto a SoPC (System-on-a-Programmable-Chp envronment. A moton control IC for robot arm based on the SoPC envronment s presented n ths paper. The FPGA-based moton control IC has two IPs, a Nos embedded processor IP and an applcaton IP. The Nos processor s used to perform the functon of the command generaton, the nverse knematc computaton and the pont-to-pont moton control. The applcaton IP s used to perform the functons of the fve axs poston control of the robot arm. The former s mplemented by software due to the complcated control algorthm and low samplng frequency control (less than Hz. The latter s mplemented by hardware due to the need of hgh samplng frequency control (control loop: k Hz, PW perpheral crcut: ~8Hz but smple computaton. At last, an expermental system has been set up and some expermental results have been demonstrated. I. INTRODUCTION Robotc control s an exctng and hgh challenge research work n recent year. Several solutons to the mplementaton of dgtal control system for robot manpulator and moble robots are proposed n the lteratures [-]. But, all of those technques use the DSP chp or FPGA chp to realze the software part or hardware part of the robot control system. They ddn t provde an overall hardware /software soluton usng a sngle chp. Nowadays, an embedded processor and applcaton IP desgn can be developed and downloaded nto FPGA to construct a SoPC envronment [5-8]. Therefore, n ths paper, a fully dgtal moton control IC for fve-axs artculated robot arm under ths SoPC envronment s proposed and shown n Fg.. In ths proposed moton control IC, t has two IPs, a Nos embedded processor IP and an applcaton IP. The Nos processor IP s used to mplement the nverse knematc scheme and the pont-to-pont moton trajectory control by software, and the applcaton IP s used to realze the fve-axs poston control by hardware. Therefore, all of the functon needed for buldng up a fully dgtal moton control for a fve-axs artculated robot arm can be ntegrated n a sngle FPGA chp. The FPGA chp here adopts Altera Cyclone EPC, whch has,6 Les, maxmum user I/O pns, total 9,9 RA bts, and a Nos embedded processor whch has a -bt confgurable CPU core, to bytes avalable on chp and maxmum G bytes off-chp memory. At last, an expermental system ncluded by a FPGA expermental board, fve sets of nverter, a set of PC and a ovemaster RV- mcro artculated robot arm producng by tsubsh s set up and verfed the correctness and effectveness of the moton control IC. II. SYSTE DESCRIPTION AND DESIGN The archtecture of the servo control system for a fve-axs artculated robot arm s shown as Fg.. The FPGA s a kernel devce and the detaled desgn s as follows: A. The mathematcal model of robot arm wth actuator The dynamc equaton of the n-lnk robot arm s gven by: [9] ( q + V ( q, + F( m + G( q = τ ( wth (q nertal matrx, V m ( q, Corols /centrpetal vector, G (q gravty vector, F ( frcton vector. The q,, & and τ denote the n-vector of the poston, velocty, acceleraton and generalzed forces, respectvely. The τ, q, n and & are R. The dynamcs of the dc motors that drve the arm lnks are gven by the n decoupled equatons [9] J + B + F + Rτ = v ( wth q = vec{ q }, J = dag{ J } B = dag{ B + b / Ra } dag{ B } ( R = dag{ r }, = dag{ / Ra } v = vce( v, τ = vec( τ where R, a, v,, b are resstance, current, voltage, voltage constant, current constant, respectvely. The J, B, τ, τ are the nertal, vscous, generated torque of the motor, L load torque for the th axs of the robot arm, respectvely. Combned equatons ( and (, the dynamc equatons ncludng robot arm and dc motor can be obtaned. At frst, f the gear rato of the couplng from the th motor to the th arm lnk s r, whch we can defne as follows q = r q or q = R q ( /5/$. 5 IEEE 97

PW Sgnal Inverter- J-axs otor Encoder Sgnal Inverter- J-axs otor J-axs J-axs J-axs Fve-axs Poston control crcuts usng Programmable Logc Devce Inverter-5 J5-axs otor J5-axs J-axs External memory

2 PW Sgnal Inverter- J-axs otor Encoder Sgnal Inverter- J-axs otor J-axs J-axs J-axs Fve-axs Poston control crcuts usng Programmable Logc Devce Inverter-5 J5-axs otor J5-axs J-axs External memory Embedded Processor IP (Nos Processor FPGA-based moton control IC Applcaton IP Inverter- J-axs otor J-axs otor PW Sgnal Inverter- Encoder Sgnal Fg. The archtecture of a FPGA-based moton control system for a fve-axs artculated robot arm Substtutng equaton ( nto (, then nto (, t can obtan ( J + R ( q & + ( B + R Vm ( q, + RF ( q (5 + R F( q + R G( q = R v Due to the gear rato r s often small for obtanng to ncrease torque value on the commercal robot arm, t can help us to smply the formulatons. From equaton (5, the dynamc equaton combnng the motor and arm lnk s gven by r ( J + r m + B + r F = v r d Ra (6 where d = m + V + F + G (7 j j j j, k jk j k Therefore, when the gear rato r s small, the r term n equaton (6 can be neglected, equaton (6 s smplfed by r J + B + r F = v (8 Ra Substtutng equaton ( and ( nto (8, the followng equatons can be gven b (9 J + ( B + + TL = v Ra R a where T s the external force or the dsturbance force L nduced by other arm moton. Therefore, f the gear rato s small, the control block dagram combnng arm lnk, motor actuator and P controller n poston and PI controller n velocty loop at each axs can be represented n Fg.. B. The computaton of the pont-to-pont moton control: In Fg., the moton command of robot arm uses pont-to-pont control scheme wth constant acceleraton /deceleraton trapezod velocty profle. In ths scheme, the desgned parameters are the overall movng dsplacement θ (degree, the maxmum velocty W (degree/s, the acceleraton or deceleraton perod T acc, and the poston command samplng nterval t d. Therefore, based on the trapezod velocty profle, the nstantaneous poston command q at each samplng nterval can be determned by the followng procedure: Step : Computaton of the overall runnng tme. T = AX ( θ /W, θ /W, θ /W, θ /W, θ 5 /W 5, T acc + T acc ( Step : Adjustment of the overall runnng tme to meet the condton of multple of the samplng nterval. N =[T acc /t d ] and N=[T/t d ] ( Where N s the nterpolaton number and [.] refers to Gauss functon wth nteger output value. Therefore, T = N t acc d and T =N t d ( Step : odfcaton of the maxmum velocty L ( θ, θ, θ, θ, θ5 ( W = L /( T T acc ( Step : Calculaton of acceleraton/deceleraton value A = W / T acc (5 Step 5: Calculaton of poston command at md-poston Acceleraton regon: q p = qr + A t (6 where t =n t d, <n N, and q s the ntal poston. r Constant speed regon: q p = qr + W t (7 where t =n t d, <n N, q s the fnal poston at r acceleraton regon and N=N- N. (c Deceleraton regon: q p = qr + ( W t A t (8 where t =n t d, <n N and q s the fnal poston at r constant speed regon. FPGA chp q v + e + + z p p z - Poston velocty controller z controller DC motor and drver th of the arm L s + R a a wth L a b T L - T + J s+ B s Fg. The block dagram of poston controller for th robot arm q q r 98

3 C. Computatonal procedure of the Inverse knematcs of an artculated robot arm Fg. shows the lnk coordnate system of a fve-axs artculated robot usng Denavt-Hartenberg conventon []. The knematc parameters of tsubsh RV- mcro robot arm used n ths paper are lsted n Table I. For the lmt of the paper, the detaled descrpton about the nverse knematcs of the artculated robot s omtted and only the computatonal procedure s shown as follows. The nputs of the followng equatons are space coordnate value x,y,z and the outputs are moton angle at each axs of robot arm. θ =θ = a tan y x (9 Step: 5 [, ] Step: b ± ( x + y b + ( d d z = ( Step: 5 a a θ = cos ( aa Step: ( a + a cosθ ( d d5 z absnθ sn θ = ( b + ( d + d5 z Step5: ( a + a cosθ b a snθ( d d5 z cos θ = ( b + d + d z ( 5 Step6: θ atan [ snθ, cosθ ] = ( Step7: θ = θ θ (5 where a, a 5, d, d 5 are Denavt-Hartenberg parameters. D. The proposed moton control IC for robot arm The nternal archtecture of the moton control IC based on SoPC technology for artculated robot arm s shown n Fg.. The SoPC technology s developed by Altera Corporaton and conssts of a FPGA and a Nos embedded processor. The FPGA chp here adopts Altera Cyclone EPC, whch has,6 LEs, maxmum user I/O pns, total 9,9 RA bts, and a Nos embedded processor whch has a 6-bt or -bt confgurable CPU core, to bytes avalable on chp and maxmum G bytes off-chp memory. In Fg., the moton control IC has two IPs, a Nos embedded processor IP and an applcaton IP. The Nos processor s used to perform the functons of the poston command generaton, nverse knematc computaton and pont-to-pont moton control by software and the flow chart of nterrupt servce routne (ISR for moton control s shown n Fg.. The moton controller s program s coded n C language. The applcaton IP s used to realze the fve-axs poston control by hardware, and ther crcuts nclude a frequency dvder, fve poston/velocty controller crcuts, fve sets of crcut, fve sets of PW crcut and a grpper control crcut. The system frequency (clk, poston control (clk_p and velocty control (clk_s samplng frequency n Fg. are desgned wth 5Hz, 76Hz and 55Hz, respectvely. Fg. 6 s poston/velocty PI controller crcut, whch need 5 sets of multpler, 7 sets of adder/subtracter and 5 sets of D Flp-flop. Fg. 7 s crcut and ts smulated waveform at up-count and down-count condton s shown n Fg. 8. Fg. 9 s a PW crcut and t s desgned wth 8 khz frequency value and.8µs dead-band. The smulated waveforms for PW crcut are shown n Fg.9. The overall crcuts ncluded a Nos embedded processor (5.% and an applcaton IP (.9% n Fg. use 6% utlty (LEs of the Cyclone EPC. d θ y z θ z x y x a a θ z θ y x y y5 z x y x θ z x5 θ 5 z5 Fg. Lnk coordnates of a fve-axs artculated robot TABLE I: Denavt-Hartenberg parameters of the robot arm Inverse nematcs 5 Nos Processor IP Pont to pont moton control θ θ θ θ θ θ 5 q d d ( mm ( mm ( mm ( mm d5 ( 7 mm FPGA-based moton control IC for robot arm Poston command generaton x,y,z θ, θ θ, θ θ 5 q q q q 5 Applcaton IP a ( mm a ( 5 mm a ( 6 mm ( mm ( mm -p -s Sta-J[7..] Poston & velocty -J[7..] controller (J -p -s Sta-J[7..] -J[7..] -p -s Sta-J[7..] -J[7..] -p -s Sta-J[7..] -J[7..] -p -s Sta-J5[7..] -J5[7..] Poston & velocty controller (J Poston & velocty controller (J Poston & velocty controller (J Poston & velocty controller (J5 Grpper-ctrl u-j[..] α α ( π α ( π -J[7..] Frequency -s dvder -p Grpper control PW Generaton Crcut u-j[..] PW Generaton -J[7..] Crcut u-j[..] PW Generaton -J[7..] Crcut u-j[..] PW Generaton -J[7..] Crcut u-j5[..] PW Generaton -J5[7..] Crcut Fg. Internal archtecture of the proposed moton control IC Start of ISR ( each 5Hz Read ncremental poston x,y,z Compute the Inverse knematc equatons n (9~(5 and obtan θ,θ,θ, θ,θ 5 d5 Compute the pont-to-pont moton trajectory by equatons n (~(8 and obtan q,q,q,q,q 5 Send the value of q,q,q,q,q 5 to applcaton IP Return Fg. 5 ISR for moton control of robot arm n Nos ext- GPR GPR Brake-J Brake-J PW-J PW-J PW-J PW-J LS-J PA-J PB-J PZ-J PW-J PW-J PW-J PW-J LS-J PA-J PB-J PZ-J PW-J PW-J PW-J PW-J LS-J PA-J PB-J PZ-J PW-J PW-J PW-J PW-J LS-J PA-J PB-J PZ-J PW-J5 PW-J5 PW-J5 PW-J5 LS-J5 PA-J5 PB-J5 PZ-J5 99

Poston &Velocty controller Sta-Jn [7..] _P Poston Loop SUB Err[5..] _P P [5..] ultply Q( Speed_Com [5.

.] ultply Q( _S SUB _S P[5..] D-FF _S I[5..] ultply _S ultply _S ADD ADD Q( Lmt u_jn [..] Fg.

PA FILTER A D-type FF PHA DLA PHB DECODER DIR PLS COUNTER u PW Generaton Up-down Counter Frequency dvder

4 Poston &Velocty controller Sta-Jn [7..] _P Poston Loop SUB Err[5..] _P P [5..] ultply Q( Speed_Com [5..] Velocty Loop _S D-FF U [..] _Jn [7..] _S _S D-FF Speed_Com [5..] SUB _S C_Jn[5..] _S rpm[5..] ultply Q( _S SUB _S P[5..] D-FF _S I[5..] ultply _S ultply _S ADD ADD Q( Lmt u_jn [..] Fg. 6 Crcut for Poston and velocty loop n Fg. PA FILTER A D-type FF PHA DLA PHB DECODER DIR PLS COUNTER u PW Generaton Up-down Counter Frequency dvder Comparator clk Edge detect counter Comparator logc PW PW PW PW PB FILTER B D-type FF DLB Dead-band value Fg.7 crcut Fg.9 PW crcut 8kHz Fg.8 Smulated waveforms for crcut up-count down-count.8us Dead tme Fg. Smulated waveforms for PW crcut

5 III. EXPERIENTS AND RESULTS The overall expermental system s depcted n Fg., and t ncludes a FPGA expermental board, fve sets of nverter and a tsubsh ovemaster RV- mcro artculated robot. The servo axs of ths mcro artculated robot has fve degrees of freedom (hand sn t ncluded n ths arm. Each axs s drven by a V dc servo motor wth a reducton gear. The operaton range of robot arm are wrst roll ±8 degrees (J5-axs, wrst ptch ±9 degrees (J-axs, elbow rotaton degrees (J-axs, shoulder rotaton degrees (J-axs and wast rotaton degrees (J-axs. The gear ratos for J to J5 axs of the robot are :, :7, :, :86 and :8, respectvely. The maxmum path velocty s mm/s and the lftng capacty s.kg ncludng hand. The total weght of ths robot arm s 9 kg. The FPGA chp adopts Altera Cyclone EPC whch has,6 LEs, maxmum user I/O pns, total 9,9 RA bts, and a Nos embedded processor can download to ths FPGA chp whch has a 6-bt or -bt confgurable CPU core, to bytes avalable on chp and maxmum G bytes off-chp memory. In mplementaton, the PW swtchng frequency and dead-band of nverter, poston control samplng frequency are desgned wth 8k Hz,.8µs and 76 Hz, respectvely. To confrm the effectveness of the proposed servo control IC, the square-wave poston command wth 6 degrees ampltude and seconds perod s used to test the dynamc response performance of the robot arm. The artculator robot s set to a specfed atttude wth postons of jont to jont 5 be at [9º, º, 6º, 5º, º]. In controller desgn, we use the same controller parameter at each axs of robot arm that the P-gan n poston loop are desgned wth., the P-gan and I-gan n velocty loop are chosen by. and.. The expermental results of step response are shown n Fgs. ~5. The rsng tmes of step response are wth ms, 8ms, 8ms, 5ms and 7 ms for J~J5 axs, respectvely. Fgs. ~5 also show the zero steady-state results. To test the trackng performance of a pont-to-pont moton control at man axs of robot arm, the overall dsplacement command wth acceleraton/deceleraton velocty profle s desgned wth, pulses (89 degree, 5, ( degree pulses and, pulses (7 degree for J, J and J-axs, respectvely. The trackng results correspondng wth the aforementoned nput commands are shown n Fgs. 6~8. It can be seen that no matter what n poston or the velocty trackng, the moton trajectory of robot arm can gves a perfect trackng wth target command n J~J axs. Furthermore, the performance of the crcular trajectory trackng under fve-axs smultaneous moton s tested. In ths experment, the functon about pont-to-pont moton control n Fg. s neglected. The crcular trajectory command wll generate equdstant segmented ponts at center wth (,, mm and radus wth 5 mm, and ts trackng result s shown n Fg. 9. The errors of crcular trajectory trackng are less than.5mm and t shows a good moton trackng result under a prescrbed plannng trajectory. Therefore, from Fgs. ~9, the results demonstrate that the proposed moton control IC for robot arm s effectveness and correctness. IV. CONCLUSIONS An applcaton of SoPC technology usng a FPGA and a Nos embedded processor to the moton control IC for an artculated robot arm s successful developed n ths paper. Ths FPGA-based moton control IC allows a fully dgtal moton control for a fve-axs artculated robot arm, such as nverse knematc computaton, pont-to-pont moton controller, poston and velocty controller, PW generaton and crcuts are all ntegrated and realzed n a sngle FPGA chp. The experments are successfully valdated, and the expermental results show a good performance. Poston (degree Poston (degree Poston (degree Poston (degree J-axs response 8 6 command Tme (s Fg. Poston step response at J-axs of robot arm J-axs Tme (s Fg. Poston step response at J-axs of robot arm J-axs Tme (s Fg. Poston step response at J-axs of robot arm J-axs Tme (s Fg. Poston step response at J-axs of robot arm

6 Poston (degree J5-axs Tme (s Fg. 5 Poston step response at J5-axs of robot arm Poston (pulse Velocty (pulse/sec, 5,, 5,,,, -, -, J-axs Poston command Poston Trackng Tme (s Velocty Profle Velocty Trackng -, Tme (s Fg. 6 Poston and velocty response at J-axs of robot arm Poston (pulse velocty (pulse/sec, 5,, 5,,,, J-axs Poston command Poston Trackng Tme (s Velocty Profle Velocty Trackng -, -, -, Tme (s Fg. 7 Poston and velocty response at J- axs of robot arm Poston (pulse Velocty (pulse/sec, 5,, 5, J-axs Poston command Poston Trackng , Tme (s, Velocty Profle Velocty Trackng, -, -, -, Tme (s Fg. 8 Poston and velocty response at J-axs of robot arm 99 Poston command X-error Z-error - - Y- error Tme (s Fg. 9 Crcular trajectory trackng of robot arm under fve-axs smultaneous moton Poston error (mm Z-axs (mm Y-axs (mm Poston trackng ACNOWLEDGEENTS X-axs (mm Ths work was supported by Natonal Scence Councl of the R.O.C. under grant no. NSC 9--E-8-. REFERENCES []. abuka, P. Glaskowsky and J. randa, crocontroller-based Archtecture for Control of a Sx Jonts Robot Arm, IEEE Trans. on Industral Electroncs, vol. 5, no., 988, pp.7-. [] G. Yasuda, crocontroller Implementaton for Dstrbuted oton Control of oble Robots, n Proceedng of Internatonal workshop on Advanced oton Control,, pp. -9. [] T.S. L, S.J. Chang and Y.X. Chen, Implementaton of Human-lke Drvng Sklls by Autonomous Fuzzy Behavor Control on an FPGA-based Car-lke oble Robot, IEEE Trans. on Industral Electroncs, vol. 5, no. 5,, pp [].A. Hannan Bn Azhar and.r. Dmond, Desgn of an FPGA Based adaptve Neural Controller for Intellgent Robot Navgaton, n Proceedng of the Euromcro Symposum on Dgtal System Desgn,. [5] Y.S. ung, P.G. Huang and C.W. Chen, Development of a SOPC for PS Drves, n Proceedng of the 7 th IEEE Internatonal dwest Symposum on Crcuts and Systems,, vol. II, pp. II-9~II-. [6] SOPC World, Altera Corporaton,. [7] Nos datasheet, Verson., Altera Corporaton,. [8] N. Xu, H. Lu, X. Chen and Z. Zhou, Implementaton of DVB Demultplexer System wth System-on-a-programmable-chp FPGA, n Proceedng of the 5 th Internatonal Conference on ASIC,, vol., pp [9] L.W. Tsa, Robot Analyss-The echancs of Seral and Parallel anpulators, John Wley & Sons, Inc, 999. [] R. Schllng, Fundamentals of Robotcs Analyss and control, Prentce-Hall Internatonal Edtons, 998.

Design and Implementation of Trainable Robotic Arm

Design and Implementation of Trainable Robotic Arm Internatonal Journal of Scence, Engneerng and Technology Research (IJSETR) Desgn and Implementaton of Tranable Robotc Arm Mo Mo Aung 1, Saw Aung Nyen Oo 2 1 Master Canddate, Department of Electronc Engneerng,