Vol. 3, No., pp. 00-070) http://dx.doi.og/0.6493/smatsci.0.307 An FPGA Implementation of a Robot Contol System with an Integated 3D Vision System Yi-Ting Chen, Ching-Long Shih,* and Guan-Ting Chen Depatment of Electical Engineeing, National Taiwan Univesity of Science and Technology, Taipei, Taiwan, ROC * Coesponding Autho / E-mail: shihcl@mail.ntust.edu.tw KEYWORDS : FPGA, -based visual sevo, 3D vision, Object tacking Robot decision making and motion contol ae commonly based on visual infomation in vaious applications. based visual sevo is a technique fo vision-based obot contol, which opeates in the 3D wokspace, uses eal-time image pocessing to pefom tasks of featue extaction, and etuns the pose of the object fo positioning contol. In ode to handle the computational buden at the vision senso feedback, we design a FPGA-based motion-vision integated system that employs dedicated hadwae cicuits fo pocessing vision pocessing and motion contol functions. This eseach conducts a peliminay study to exploe the integation of 3D vision and obot motion contol system design based on a single field pogammable gate aay FPGA) chip. The implemented motion-vision embedded system pefoms the following functions: filteing, image statistics, binay mophology, binay object analysis, object 3D position calculation, obot invese kinematics, velocity pofile geneation, feedback counting, and multiple-axes position feedback contol. Manuscipt eceived: Decembe, 04 / Accepted: Januay 7, 0. Intoduction Intelligent obots must espond apidly to thei woking envionment though visual infomation. The extaction of infomation fom visual sensos, the analysis of this infomation, and the activation of an appopiate moto contol command based on this infomation must all occu in eal-time. Visual sevoing is a technique that uses images extacted fom the envionment to contol obots in a closed loop fashion []. Robots with visual feedback ae used in tasks such as mobility, object manipulation, and object tacking. This eseach consides the study of position-based visual sevoing PBVS), in which the visual infomation is used as a feedback and detects the pose of objects in a wok space []. Fo anothe way of epesenting the image feedback function, image-based visual sevoing IBVS) does not use any pose model and nomally occus in D tasks. Among the many applications, PBVS is implemented in industial envionments fo object manipulation opeations on conveyo belts. Fo object tacking, cameas ae often utilied to obtain infomation of the motion of an object that can then be employed in coodination contol. In geneal, the image pocessing takes up to 80% of the computation time of a sevo contol. It is often difficult to meet the high-speed motion contol system pefomance equiements fo eal-time image pocessing in a taditional achitectue of a PC o a DSP. pocessing with a lage matix calculation deteioates system pefomance due to limitations in the computation time of geneal pocessos. The dawback of the CPU-based image pocessing is the lag in the system esponse. That is why a CPUbased vision sevo contol cannot be called a eal-time vision-based sevo system. The mix-coe mode fo a vision-based contolle has been ecently developed fo obotic sevo contol. The basic idea of these studies is to combine the FPGA and DSP/PC fo a sevo system. These cases incopoated the image captue and image pocessing algoithms into the FPGA so as to conque the low-speed computation issue. The DSP o PC is intoduced to constuct the majo functions of the sevo system fo educing its design complexity. Howeve, the mix-coe mode doubles the cost of the sevo system and enlages the contolle footpint. The technology of field pogammable gate aay FPGA) has spead thoughout many application fields lately-fo instance, medical devices, sevo motion contol, mass data pocessing, and paallel computation, etc. The main pefomances of an FPGA ae high-speed, paallel computation and easy cloning algoithm patten. An FPGAbased SOPC can easily solve any issue by paallel computation, especially fo a distibuted stuctue. Unlike a geneal-pupose pocesso that adopts a fixed hadwae design fo maximum compatibility and flexibility, we embed logical pogamming into the hadwae cicuit design diectly. This kind of design helps the lowspeed system satisfy the demand fo sufficient eal-time contol unde a highly paallel achitectue. Using FPGA to conduct eal-time image pocessing and contol algoithms has the advantage of acceleation. Due to the intenal stuctue of FPGA featues, it can be vey easy to achieve distibuted algoithms, wheeby each function block woks at 00
Vol. 3, No., pp. 00-070) the same time, making it favoable fo high-speed digital signal pocessing. The FPGA technology has been applied to multi-axis sevo and motion contol systems [3-7], to image pocessing and steeo vision [8-], to eal-time visual sevo [3], and to vision and motion integated systems [4]. This eseach applies a single FPGA chip in a pototypical tacking contol system fo a obot manipulato by using two cameas to detect the object motion infomation. The implemented motion-vision embedded system pefoms the following functions: filteing, image statistics, binay mophology, binay object analysis, object 3D position calculation, obot invese kinematics, velocity pofile geneation, feedback counting, and multiple-axes position feedback contol.. FPGA system oveview In ode to handle the computational buden at both the image senso and actuato levels, we design a FPGA-based motion-vision integated system. The poposed system employs dedicated hadwae cicuits fo pocessing motion-vision functions. Fig. shows the system block diagam of the FPGA-based obot motion contol and 3D vision system. The obot system consists of a -axis obot manipulato with its base located on a one-axis linea table to enlage the woking space. Two cameas ae installed at the font of the base of the obot manipulato. The image senso is a SXGA fomat 0-bit colo camea, with esolution of 80 04 and fame ate of up to 0 fps. Fig. shows the poposed FPGA obot contolle is conceptually divided into thee diffeent modules: image pocessing module, sevo contol module and system contol module. The image pocessing module is a dedicated logic cicuit implemented within the FPGA. It captues two eal-time images fom two image sensos and extacts impotant infomation by applying filteing, binay mophology, and binay image statistic analysis. The image pocessing module efines and sends the pocessed data to the obot system contol module accoding to the equests fom the system contol module. The sevo contol module is also a dedicated position contol logic cicuit that is implemented within the same FPGA. It eceives position commands fom the obot contol system module and simultaneously contols six DC sevo motos by the digital PID contol law. Sync. Signals Two Sensos Notebook PC RS3 Altea FPGA Development System PWMs 6 DC Moto Dive Cicuits Robot System VGA Monito Video Out Encodes Fig. FPGA-based obot vision and contol system block diagam FPGA Development Hadwae System PC Two Sensos VGA Monito RS3 System Contol Module Nios II 3-bit soft-pocesso) Pocessing Module Veilog) SDRAM 8 M bytes) Object Featue Linea Table Command SRAM K bytes) Joint Command Sevo Contol Module Veilog) PWMs Quadatue Encodes 6 DC Moto Dive Cicuits Robot System Fig. The FPGA obot contolle consists of the image pocessing module, the sevo contol module and the system contol module. 0
Vol. 3, No., pp. 00-070) The system contol module uns on the FPGA-embedded softpocesso Nios II within the same FPGA chip. As compaed to the othe two FPGA modules, the system contol module expands the applicability of the system, because it is based on the softwae C pogamming. The obot system contol module consists of functions of object 3D position, motion planning, invese kinematics, obot joints position tacking command geneation and use seial communication inteface. 3. pocessing module The image pocessing module is a dedicated logic cicuit implemented within the FPGA. It captues two eal-time images fom two image sensos and extacts impotant infomation by applying filteing, colo segmentation, mophology and binay analysis. The image pocessing module efines and sends the pocessed data to the system contol module as soon as the data ae eady. Fig. 3 pesents the functional block diagam of the image pocessing module. Senso Senso Senso Contolle Senso Contolle Raw Data Raw Data Scan-line Buffe Based Pocessing Scan-line Buffe Based Pocessing Binay Data Binay Data SDRAM Contolle Binay Statistics VGA Contolle to NIOS II Embedded Pocesso System Contol Module) VGA Teminal SDRAM Fame Buffes Fig. 3 Functional block diagam of the image pocessing module. The image is input to the FPGA chip line by line, and only a vey small numbe of lines of images ae stoed in line-buffes FIFO). Only n line-buffes ae equied fo an image opeation in a n n window. The aw image data ae fom a SXGA fomat 0-bits colo camea, with a esolution of 80 04 and a fame ate of 0 fps with a MH pixel clock. The aw image is conveted into a 30-bit RGB image fo 640 480 esolution by a local neighbohood of each of the fou scanned pixels. The RGB image is then used fo futhe image pocessing, as shown in Fig. 4. We expess the aveage low-pass filte in a window by P 3 P i, ) i whee pixels in the window ae numbeed fom to in the ode of left to ight and then top to button, in which P 3 is the cente pixel. We obtain the RGB to YCbC colo tansfom as Y Cb C 048 30 7 9 48 87 0 R 6 G 8, 36 B 8 whee 0 R, G, B 03 and 0 Y, Cb, C.Red colo segmentation is pefomed as 98 Cb 7 and60 C 3 P. 3) 0 othewise ) The eosion opeation in a 3 3 window is P 9 P i, 4) i and this is followed by a dilation opeation in the 3 3 window P 9 P i. ) i Finally, we apply a ank ode filte in the window P P i, 3 i 0 othewise to geneate a binay image output that is stoed in SDRAM fo futhe pocessing. Fig. shows the post binay image pocessing. We calculate the cente of the object measued fom Camea,, ),as c Pc, P c c and cpc c. P c c 6) 7) 8) 0
Vol. 3, No., pp. 00-070) We calculate the cente of the object measued fom Camea,, ), in a simila way. c Senso Input RAW to RGB Aveage Lowpass Filte RGB to YCbC 4. System contol module The system contol module eceives 3D object position infomation fom the image pocessing module and sends obot joint position commands to the sevo contol module. Fig. 6 pesents the system contol module that consists of the following tasks: 3D object position computation, obot motion planning, obot manipulato invese kinematics, obot joints position tacking command geneation, and use inteface. Let F be the focus length of the camea, p p is the physical pixel sie, and L is the distance between Camea and Camea. We obtain the distance fom the object to the cente of cameas, y, by c Colo Segment and Binay y c F L, p c c ) 9) Eosion Dilation Rank Ode Filte Binay Output to SDRAM Fig. 4 Scan-line buffe-based image pocessing, with a system pixel clock of MH whee c and c ae the column positions of the object s cente point measued fom Camea and Camea, espectively, as shown in Fig. 7. The object s hoiontal position and vetical position in camea coodinates see Fig. 7) ae x p y c F c c 0) and p y F, c c ) Binay Data Taget Object Height and Cente Point Calculation c, ) & R espectively. We calculate the object height h by p h y R, c ) F Binay Data Taget Object Cente Point Calculation c, ) fom SDRAM ) to System Contol Module ) whee R is the height of the object in the sceen coodinate of Camea. Fig. Post binay image pocessing c, ) End-effecto Piecewise 3D Object x Robot Taget,,,,,, c, yc, ) xe, ye, e) c c, ) Linea Invese R Tajectoy Calculation & H Kinematics to Sevo Planning Geneato Contol fom Module ) Pocessing Module ) Fig. 6 Functional block diagam of the system contol module 03
Vol. 3, No., pp. 00-070) object cente x, y, ) c c c y h h h The fist pat uses the linea table to tack the moveable object such that the object cente position is located in the cente of Camea. The second pat places the obot end-effecto nea the top of the movable object. At each image pocessing ate of 0 H, the 3D object position is updated, the obot end-effecto is e-planned, and the obot s new joint positions ae obtained by solving the invese kinematics. Finally, we geneate the sevo position command by a piecewise linea tajectoy at a command update ate of KH. Camea Camea c R Fig. 7 Calculation of the object s 3D position height h by two cameas L c One-axis Linea Table x x, y, ) and c c c Fig. 8 shows the coodinate system of the -axis obot manipulato and its DH paametes ae in Table I. We solve the invese kinematics poblem given the end-effect position x e, ye, e) and the pitch angle and the oll angle, by finding the joint angles,,,, ), 3 4 Αtan y e, xe) 3) x a a 3 3 cos ) a a 4) 3 Atan s, c ) ) 4 3 6) 7) whee x cos ) x sin ) y a cos ) d sin e e 4 d a sin ) d cos ) e 4 a a3 cos3) a3 sin3 x s x a a cos ) x a sin 3 3 3 3 c x ) Table DH paametes of the -axis obot Joint i i di ai i Home d 0 / 0 0 a 0 0 x4 x d 4 a a 3 a 4 3 x x x 3 3 4 0 d x 0 Fig.8 Coodinate system of the -axis obot manipulato. Sevo contol module The sevo contol module, as illustated in Fig. 9, is a dedicated logic cicuit implemented fo the position PID contol of 6 DC sevo motos and consists of the following: encode signal filte, 3-bit high-speed position decode counte, 7-bit digital PID contolle, - bit PWM geneato and an H-bidge powe convete fo each axis. Fig. 0 pesents the moto position encode low-pass filte hadwae cicuit, which consists of D flip-flops and a two-of-thee voting filte. The position decode up/down counte is incemented each time by the contol signal Inc, Inc A B A B B A B A 8) and is decemented each time by the contol signal Dec, Dec A B A B B A BA 9) The DC moto H-bidge dive cicuit is connected to 4 contol signals as noted in Fig. : PWMH, PWML, PWMH and PWML. The PWM signal is geneated by a standad PWM geneato cicuit with a fequency of 4 KH and an -bit esolution of the duty cycle. 3 3 0 a 0 0 4 4 0 a4 / / d 0 0 0 Note: d 0 mm, a 0 mm, a 3 0 mm, a 4 0 mm, d 30 mm. Command fom Robot Contol Module) y Feedback Digital PID Contolle Up-Down Counte u A B PWM Geneato Digital Low-pass Filte PWMH PWML PWMH PWML CH_A CH_B H-bidge Cicuit M+ M- dc sevo moto Encode The end-effecto taget position planning consists of two pats. Fig.9 Functional block diagam of the sevo contol module 04
Vol. 3, No., pp. 00-070) CH_A CLK D Q D Q D Q D Q J K Q A umax u umax u [ n] sat u) u u u u. min max 3) u u u min min CH_B D Q D Q D Q D Q J K Q B 6. Implementation and expeimental esults CLK PWMH PWMH PWML PWML Fig.0 Moto encode low-pass filte hadwae cicuit V DD dc M+ moto M- Moto Contol CW un PWMH PWM OFF PWML OFF ON CCW un PWMH OFF PWM PWML ON OFF Note : V DD 4 V Fig. DC moto dive cicuit contol method We apply the PID contolle, t de u Pe I e ) d D, 0) 0 dt fo the closed-loop position contol of each axis, whee eo signal e y, P is the popotional gain, I is the integal gain, and D is the deivative gain. Let T be the sampling time. The digital PID contol law is digitied by using the Eule tansfom fo the deivative tem and the bilinea tansfom fo the integal tem, and is shown below Since the poposed system can connect to six dc moto dive cicuits, it can contol a obot system with six diffeent axes. Fig. shows the expeimental obot system, which consists of a -axis obot manipulato with its base located on a one-axis movable linea table. The dedicated logic/contol/image functions fo the obot vision and contol system ae implemented all within the Altea FPGA development platfom DE with Cyclone II EPC3F67C6 FPGA, which has 33,6 logic cells, K bytes of SRAM, and 8M bytes of SDRAM. These functional modules ae designed using the Veilog HDL and the C pogamming language unning in an embedded Nios II soft-pocesso. All achitectues in this wok have been developed using Veilog HDL and have been synthesied by Altea Quatus II EDA tool. The synthesis esults fo all achitectues ae shown in Table, in which the oveall system takes about 43% of total FPGA esouce. In paticula, the image pocessing module takes about 30% of total FPGA esouce. The thoughput shows that the system is capable of locating and detemining an object s 3D position 0 times pe second. The total amount of FPGA esouces used to implement this system allows fo flexibility to enhance the system by implementing othe kinds of algoithms. IT U ) P D T E ). ) We then expess the digital contol law, which is implemented in FPGA, by u u[ n ] b0 e[ n] b e[ n ] b e[ n ], ) whee IT D b0 P T IT D b P T b D T the contolle output u [n] is limited by a satuation function Fig. The expeimental obot system Two COMS image sensos, with a esolution of 80 04 and a 0bit colo image, ae installed at the font base of the manipulato. The data of the cameas ae listed below: ) focus length of the camea, F 4.8 mm ; ) physical pixel sie, p p 3.6μm3.6μm ; and 3) distance between Camea and Camea, L 37. mm. The images ae captued at 0 H using a system pixel clock of MH. Fig. 3 shows the esults of the image pocessing module. The coodinates of the object cente in each image ae sent to a softpocess which detemines the distance fom the object to the cameas. Fig. 4 pesents the measued distance eo computed fom Eq. 9), whee the actual distance eo is % on aveage. 0
Vol. 3, No., pp. 00-070) Table Synthesis esults and device utiliation available used utiliation Logic Cells 33,6 4,307 43% Dedicated Logic Cells 3,768,89 8% Memoy Bits 47660 37,740 66% Pins 478 4 89% Note: system clock 0 MH, pixel clock MH, Nios II clock 00 MH a) st axis a) Aveage low-pass filte b) RGB to YCCb c) Read colo segmentation b) nd axis d) Eosion and dilation e) Rank ode filte f) Computing object cente Fig.3 Results of the image pocessing module 0 c) 3 d axis Actual distance eo mm) 0 0 - -0 0 00 0 00 0 300 30 400 Measued distance mm) Fig.4 Measued distance eos fom 3D vision d) 4 th axis In ode to evaluate the pefomance of the implemented sevo contol module, the system contol module sends specific commands fo tacking contol. The contol pefomance can be fine-tuned by using diffeent PID contol paametes. Fig. shows obot step esponses and piecewise linea tajectoy tacking contol esults, with the digital PID contol paametes in Table 3. Fig. 6 illustates the esponse of the obot s base position tacking of a moving object to the cente position of cameas. The expeimental esults show that the linea tajectoy tacking pefomance is vey well and guaantees eal-time object position tacking as long as the feedback infomation is coectly updated fast enough. Table 3 Robot contol system PID contol gains Gain Table Joint Joint Joint Joint Joint axis 3 4 P 0 60 60 0 9 0 I T D / T 0 0 0 0 e) th axis Fig. step esponses and piecewise constant speed esponses of the obot manipulato, whee the uppe yellow line is the position command, and the lowe blue line is the eal esponse. Fig. 6 Response of the obot s base position blue line) to tack a moving object to the cente position yellow line, the goal position) of cameas. 06
Vol. 3, No., pp. 00-070) In summay, the synthesis tool Altea Quatus II estimates that the system achieves a thoughput of 0 fames pe second 640 480 pixels). This synthesis and the opeation esults have shown that the implemented system is useful to eal-time distance measuements in achieving a good pecision and an adequate thoughput and in being suitable fo most eal-time opeations. 7. Conclusion This pape descibes the development of a obot contol system integated with a steeo vision in eal-time, using a hadwae/softwae co-design appoach. The system is composed of 6 dc sevo motos and two camea, along with an FPGA development boad equipped with a low-cost Altea FPGA, K bytes of SRAM and 8M bytes of SDRAM.A hadwae-softwae co-design appoach was used in ode to implement the most intensive computations diectly in the hadwae, as well as low computation time tasks in a Nios II soft-pofesso. This methodology allows one to enhance the system flexibility. Synthesis esults show that the system occupies less than 4% of an FPGA s logical elements and 67% of its memoy bits. A pefomance of 0 fames pe second was achieved; pointing out that the system is capable of detecting an object and computing its distance at a ate of 0 times pe second. The system pecision has an eo ate of less than %. Theefoe, the poposed obot contol system is capable of detecting an object, measuing its 3D position, and tacking the object in eal-time. The developed system can be adapted fo many obot contol applications. In this wok, we have poposed a single FPAG chip with extenal SRAM and SDRAM fo pocessing eal-time dual image sensos, low-level featue extaction algoithms and a simultaneous contolling obot system that has a total of 6 DC sevo motos. Based on the FPGA embedded system s design flexibility, easy tansplantation and shot development cycle, FPGA will become the new diection fo the development of 3D image pocessing, and have vey boad obot contol applications. ACKNOWLEDGEMENT This wok is suppoted by Taiwan Ministy of Science and Technology gant NSC-0--E-0-00. REFERENCES [] P. Coke, Robotics, Vision and contol: Fundamental algoithms in MATLAB, Spinge Tacts in Advanced Robotics, Spinge Belin Heidelbeg, 0) [] D. Saqui, F. C. Sato, E. R.R. Kato, E. C. Pedino, and R. H. Tsunaki, Mathematical mophology applied in object tacking on position-based visual sevoing, IEEE Intenational Confeence on Systems, Man, and Cybenetics, pp. 4030-40303) DOI: 0.09/SMC.03.688 [3] J. U. Cho, Q. N. Le, and J. W. Jeon, An FPGA-based multiple-axis motion contol chip IEEE Tansactions on Industial Electonics, 6, 86-870 009) DOI: 0.09/TIE.008.00467 [4] Y. S. Kung and C. S. Chen, FPGA-Realiation of a motion contol IC fo obot manipulato, Robot manipulatos, Edited by Maco Ceccaelli Intech, Rijeka, Coatia 008), Chap. 6. DOI: 0.77/6 [] A. Astaloa, J. Láao, U. Bidate, J. Jiméne, and A. Zuloaga, FPGA technology fo multi-axis contol systems Mechatonics, 9, 8-68 009) DOI: 0.06/j.mechatonics.008.09.00 [6] X. Shao and D. Sun, Development of a new obot contolle achitectue with FPGA-based IC design fo impoved highspeed pefomance IEEE Tansactions on Industial Infomatics, 3, 3-3 007) DOI: 0.09/TII.007.9360 [7] E. Monmasson, L. Idkhajine, M.N. Cistea, I. Bahi, A. Tisan, and M.W. Naoua, FPGAs in industial contol applications IEEE Tansactions on Industial Infomatics, 7, 4-43 0) DOI: 0.09/TII.0.3908 [8] S. Jin, J. Cho, X. D. Pham, K. M. Lee, S. K. Pak, M. Kim, and J. W. Jeon, FPGA design and implementation of a eal-time steeo vision system IEEE Tansactions on Cicuits and Systems fo Video Technology, 0, -6 00) DOI: 0.09/TCSVT.009.0683 [9] Y. Miyajima and T. Mauyama, A eal-time steeo vision system with FPGA Field Pogammable Logic and Applications, Lectue Notes in Compute Science, 778, 448-47 003) DOI: 0.007/978-3-40-434-8_44 [0] Y. H. Yu, N. M. Kwok, and Q. P. Ha, FPGA-based eal-time colo tacking fo obotic fomation contol 6th Intenational Symposium on Automation and Robotics in Constuction, pp.-8 009) [] P. J. Lapay, B. Heyman, M. Rosse, and D. Ginhac, High dynamic ange eal-time vision system fo obotic applications IEEE Intenational Confeence on Intelligent Robots and Systems, pp. -, Octobe 0) [] D. Muay and J. J. Little, Using eal-time steeo vision fo mobile obot navigation Autonomous Robots, 8, 6-7 000) DOI: 0.03/A:00898763 [3] E. Malis, F. Chaumette, and S. Boudet, -/ D visual sevoing, IEEE Tansactions on Robotics and Automation,, 38-0 999) DOI: 0.09/70.76034 [4] S. Jin, D. Kim, T. Song, O. N. Le, and J. W. Jeon, An FPGA-based motion-vision integated system fo ealtime machine vision applications IEEE Intenational Symposium on Industial Electonics, pp. 700-70009) DOI: 0.09/ISIE.009.89 07