Stereo Vision System on Programmable Chip (SVSoC) for Small Robot Navigation

Transcription

1 Stereo Visio System o Programmable Chip (SVSoC) for Small Robot Navigatio LI Migxiag ad JIA Yude School of Computer Sciece ad Techology Beijig Istitute of Techology Beijig 0008 PR CHINA {lmx jiayude}@bit.edu.c Abstract I this paper we preset a stereo visio system o programmable chip (SVSoC) for dese depth mappig ad obstacle detectio at video rate. The system is composed of three miiature CMOS cameras with triagular cofiguratio ad oe FPGA chip for parallel computig. The system algorithm cotais oliear iteratio based cooperative algorithm for high quality dese depth mappig groud extractio ad obstacle locatio from dese depth maps. With the coectio of DSP for movig cotrol the system is mouted o small hexapod robot for obstacle avoidace ad avigatio. with most stereo visio systems our system employs the oliear iteratio based stereo matchig techique called cooperative algorithm [6 7] for high quality 3D mappig at video rate. Idex Terms Triocular Stereo Visio System o Chip Cooperative Matchig Obstacle locatio FPGA. I. INTRODUCTION Obstacle avoidace is oe the most basic ability of mobile robot to operate i ukow or dyamic eviromet. May obstacle avoidace systems are based o sesors that furish direct 3D measuremets such as laser ragefiders ad soar systems [ ]. I some cases stereo visio systems based o Persoal Computer have bee used for dese depth mappig ad obstacle detectio [3 4 5]. I this paper we preset a stereo visio system o programmable chip (SVSoC) for dese depth mappig ad obstacle detectio at video rate. There are five advatages of our systems: () iterative dese depth mappig which ca provide refied ad smooth dese depth maps of terrai for path plaig i ukow eviromet; () video rate computig performace which is particularly useful i case where the stereo head is itself beig servoed while i operatio; (3) idepedet operatio while the host computig resources could be set free for high level computig istead of low level heavily visio processig; (4) small size suitable for small mobile robot; (5) multiple modal outputs cotaiig itesity image dese depth map ad obstacle locatio. II. ARCHITECTURE OF THE SYSTEM Stereo visio system desigig for small mobile robot is a very challegig work requirig small system size as well as high computig performace. Traditioal robot visio system cotais two cameras for acquisitio of stereo images. I cotrast our system is composed of three miiature CMOS cameras with triagular cofiguratio ad implemeted o hardware which barely icreasig hardware cost provides more precise stereo matchig tha biocular stereo. Differet Fig.: SVSoC ad hexapod robot Fig.: O-chip architecture The stereo visio system o programmable chip (SVSoC) is implemeted by oe Field Programmable Gates Array (FPGA) such as Xilix XCVP40. There are three mai modules o chip: iterative stereo visio computig 3D mappig ad filterig ad obstacle detectio ad locatio as show i Fig.. The iterative stereo visio computig module performs pixels dese disparity mappig i 3 disparity levels from three QVGA video iputs at video rate. Fig.3 shows the detailed block of this module. Based o pihole model dese disparity maps are mapped ito 3D space for estimatig obstacle. The coected regio labelig algorithm is employed for the obstacle detectio o hardware ad a set of heuristics are used to recogize the real obstacle

2 i the eviromet. The itesity images dese disparity (depth) maps ad obstacle positio are stored i off-chip RAM ad ca be accessed by host through parallel port. Fig.4 ad 5 show the hardware detail of 3D mappig ad filterig ad obstacle detectio ad locatio. e x = 3 e y = z x y 3 e = e e. Therefore we have the rectificatio trasform equatio: T [ c r ] = C R R C [ c r ] T λ. Three camera rectificatio trasforms use the same C ad R ad user ca choose proper C based o the applicatio. The detailed work ca be foud i our paper [8]. Fig.4: 3D mappig ad filterig Fig.5: Obstacle detectio ad locatio Fig.3: Noliear iteratio based stereo visio computig III. NONLINEAR ITERATION BASED REAL-TIME STEREO VISION A. Triocular rectificatio ad parallel geometric mappig Let three cameras deoted as C C ad C 3 respectively. The correspodig images are I I ad I 3. Note that a affie coordiate system ca be fouded by two baselies C C ad C C 3 as show i Fig.6. The origial camera pihole model is give by T λ [ c r ] = CR( X T) where C ad R are the origial itrisic parameter matrix ad rotatio matrix respectively c ad r are colum ad row pixel coordiates i the origial camera coordiate system. Based o the rectified rotatio matrix R ad itrisic matrix C the rebuild camera coordiate system is λ [ c r ] T = C R ( X T) where c ad r are colum ad row coordiates i the rectified camera coordiate system ad R [ ] = e e e x y z where Fig.6: Triocular rectificatio Fig.7: Hardware structure of rectificatio Geerally the coordiate before rectificatio decided by that after rectificatio are ot itegers. D liear iterpolatio is exploited to deal with it. Assume that the rectified images

3 are scaed orderly. Multiple cadidates i disparity ca be available simultaeously by usig parallel hardware cache. After rectificatio the relatioship betwee disparity ad depth is give by f f 3 Depth = = d d where f is the focus legth i C d is the disparity i rectified row betwee image I ad I ad d is the disparity i rectified colum betwee image I ad I 3. If the disparity is defied by oe pixel i the rectified row the cadidates i the rectified colum ca be iterpolated from the output of parallel colum cache. As show i Fig.8 the iput of the asychroous FIFO is multiple cadidates i parallel o hardware ad the output is the cadidates i series but the speed is multiplied. This trade-off betwee circuit area ad speed is sigificative because all origial images ca be stored i oly oe big exteral low speed memory o matter how fast the process i chip is. This reduces system size ad peripheral without performace loss. sig bit is cacelled ad the expoet e is 5-bit siged iteger whose data rage is 6 e 5. The experimet shows that it is eough to represet disparity space durig iteratio without overflow. However the uderflow might be itroduced. The matissa m is 0-bit with a implicit leadig. The radix poit is betwee the bit 9 ad the hidde bit. Durig iteratio the evaluatio of correct matchig is magified ad the others are ihibited to 0. Actually oly high evaluatio is worth to be oticed. To simplify the floatig-poit arithmetic uit the 0 value (state) is also cacelled ad there are oly two special values (states) i our floatig-poit format: 6 Mi =.0 = ad 5 Max = = 0. Deote the data i our floatig-poit format as A. To hadle the problems which might occur i subtractio ad ihibitive iteratio besides the ormal overflow ad uderflow protectio rules we defie two extra operatio rules as follows: Mi if ( A ) i Aj (a) A A = i j A A if ( A > A ) i j i j (b) Mi A = Mi. The rule (a) solves the problem of subtractio i disparity space iitializatio ad the rule (b) is based o the priciple that oce the evaluatio is ihibited to Mi durig iteratio it caot be chage. The highest workig frequecy of the customized 5-bit floatig-poit arithmetic uit is over 30MHz after sythesized by Xilix XST although the speed grade of the FPGA is the lowest. The pipelie latecy of additio subtractio multiplicatio ad divisio are ad 4 clocks respectively icludig the overflow ad uderflow protectio. Fig.8: Hardware structure of parallel geometric mappig. B. Customized floatig-poit represetatio Based o the iterative stereo computig the evaluatio correspodig to correct matchig is magified ad the others are ihibited to 0. This requires ot oly a large iteger space but also a large decimal space. As we kow the data rage of floatig-poit format is much bigger tha that of fixed-poit with same bit-width. However the arithmetic uit of floatig-poit format is much more complicated. Based o hardware pipelie floatig-poit arithmetic uit ca achieve same throughput as fixed-poit but the circuit are much complicated ad more pipelie latecy is eeded. To realize precise computig o hardware ad reduce circuit size disparity space is represeted by floatig-poit format i our system. We customize a floatig-poit format for our system as show i Fig.9. Sice disparity space caot be egative the Fig.9: Customized 5-bit floatig-poit format C. Disparity space iitializatio I our system the metric of iitial disparity space is based o the photometric similarity from rectified stereo pairs which is give by SAD c r d = I c d r I c r + ( ) ( ) ( ) ( c r) I3 c r CC3 CC d I The mai reasos that we use SAD for iitial stereo matchig metric is that SAD computig accords with pipelie structure ad the circuit is much simpler ad smaller tha others such as SSD ad NCC. It should be metioed that the CMOS chips ad les used i our system are exactly the same to miimize the photometric differece amog the cameras. O hardware sice the itesity image from camera chip is 8-bit the disparity space is scaled by ormalized SAD:.

4 ( c r d ) SAD( c r d ) 5 D =. The average i 3 disparity levels is computed by 3 D Average ( c r) = D ( c r i) 3. i= To reduce mis-matchig ad magify the correct matchig the evaluatio is magified ad trucated by the average: D ( c r d ) D ( c r) Average D ( c r d ) =. D ( c r) Average Thus the iitial disparity space is expressed as D ( c r d ) D ( c r d ) F D ( c r d ) = 0 F D ( c r d ) < F where F is the miimum of iitial disparity space. D. Noliear disparity space iteratio The two basic priciples followed durig iteratio are the cotiuity ad uiqueess costrais which are proposed by Marr ad Poggio. Let D deote the disparity space after iteratios. I the cotiuity costrait the poits with geometric similarity which meas they are closer should have similar disparity. Therefore the support area φ ca be defied i disparity space. Cosiderig the plaes i real world are ot always parallel with the plae decided by triocular optical ceters φ should be a cuboid area. I our system φ is area i disparity space. The sum i φ is give by S ( c r d ) = D ( x y z) ( x y z ) φ ( c r d ). = ( ) D x y z z φ y φ x φ I the uiqueess costrai the evaluatios i disparity of same referece poit are mutually exclusive. Therefore the ihibitio area ψ is defied by ( c r d ) = {( c r i) i [ 3] i d} ϕ ad the sum of ψ is expressed as I c r d = S x y z ( ) ( ) = ( x y z ) ϕ ( c r d ) 3 i= S ( c r i) S ( c r d Based o the support area ad ihibitio area defie the ihibitio gai G : α S ( ) ( ) c r d G c r d = I ( c r d ) where α is the gai magificatio coefficiet. I our system α is assiged to.0. To reduce the loss of detail ad the over smoothig caused by the aggregatio of local support the iitial disparity space is used for iterative updatig: D ( c r d ) = D ( c r d ) G ( c r d ). + 0 Based o iterative ihibitio each evaluatio i disparity space receives ihibitio from that i other disparity levels. It is impossible that there are multiple oticeable peaks i ). disparity directio after eough iteratio which represets the disparity space is coverget. Therefore it is equivalet to the covergece that the maximum i disparity is remarkable eough. A static threshold is used for the disparity validatio i our system which achieves acceptable result ad simplifies the circuit. O hardware the aggregatio for S is decomposed ito three cascaded D accumulatio i disparity space row colum ad disparity directio respectively as show i Fig.0. This X-Y-Z separable feather makes the hardware computig for S o limited hardware resource realizable. I additio the sum of I also ca be easily implemeted by the sum of S i disparity directio. To prevet from overflow ad reduce uderflow ihibitio gai is ormalized: S ( ) ( ) c r d G c r d = 3. I ( c r d ) Theoretically it should be a multiple of 3 but a multiple of 3 oly eeds addig 5 to the expoet. At last iitial disparity space D 0 oe of the iputs is sychroized with G for iterative updatig by usig pipelie latecy cache ad outputted with D + simultaeously. Fig.0: Hardware structure of oe disparity space iteratio As we kow i may cases iteratio ad parallelism are coflict processig especially whe iterative algorithms use previous results as the iput i every successive stage. As

5 show i Fig.3 repetitious oliear disparity space iteratio is implemeted i our system by multiple iteratio-computig uits cascadig ad cyclig. Firstly the iterfacig ad timig of the iteratio-computig hardware uit must be compatible with itself for cascadig. Secodly the iteratio-computig uit is ruig i full pipelie. Therefore differet iteratios are performed simultaeously ad multiple iteratios ca be executed i parallel. ad D 0 is outputted for post-processig. Fig.3 shows the rectified image from camera C 0 dese disparity map from SVSoC ad also from the SSAD method with 5 5 widow for compariso which is very popular i stereo visio hardware system Covergece Percetage (%) Threshold =.0 Threshold = 4.0 Threshold = 6.0 Threshold = 8.0 Threshold = 0.0 (a) Iteratio Fig.: The relatioship betwee iteratio ad covergece percetage. (b) (c) Fig.3: Rectified image ad dese disparity maps. (a) Rectified image from C 0. (b) Dese disparity map by SSAD. (c) Dese disparity map by oliear iteratio. RMS Error of Adjacet Iteratio (Disparity) Threshold =.0 Threshold = 4.0 Threshold = 6.0 Threshold = 8.0 Threshold = Iteratios Fig.: The relatioship betwee iteratio ad the RMS error of adjacet iteratio. Besides the quality ad precisio of disparity map the performace ad hardware resource cosumig are also the very importat guide lies for embedded real-time system. As show i Fig. ad Fig. cosiderig the dimiished improvemet durig iteratio limited iteratios are performed for the balace amog the performace hardware resource ad result quality. I our system 5 iteratio computig uits are cascaded ad 0 disparity space iteratios are executed i cycles. Two hardware switches i Fig.3 cotrol the data flow durig cycles. Whe performig the iteratio from to 5 the iput is D 0 ad the output is D 5 which is stored i RAM. Durig the iteratio from 6 to 0 D 5 is loaded from RAM IV. ENVIRONMENT MODEL AND OBSTACLE LOCATION A. 3D mappig ad eviromet model The groud floor represetig a movable path is a very importat object for mobile robots i ukow eviromets. I our system we use the rectified camera coordiates X Y Z to locate the obstacle. Our eviromet model is based o followig assumptios: (a) The eviromet is composed of three parts: groud backgroud ad obstacles i the 3D space. (b) The groud is the plae which does ot pass through the origi of the referece camera coordiate system ad the equatio is ax + by + cz'=. () (c) The backgroud plae is orthogoal with the rectified camera optical axis for simplificatio ad its equatio is Z '' = d. (d) Whe the groud plae ad the backgroud plae overlap the visible part is valid. (e) The obstacle is composed of the poits whose Z is smaller tha with the same X ad Y both Z ad Z o the groud ad backgroud respectively. (f) The poits whose Z is bigger tha Z or Z are treated as errors ad igored. Fig.4 depicts the structure of 3D mappig ad filterig o hardware. Based o the pihole model disparity is mapped ito 3D space. The poits whose Z is smaller tha Z ad Z are marked as obstacle tag for obstacle extractio.

6 B. Robust groud parameter extractio Okada et al [9] proposed a plae extractio from depth map based o radom Hough trasform ad Sabe et al [4] used a differet method. I this sectio we discuss a robust groud parameter extractio. Cosiderig the parameters rage i Eq. the plae is represeted by siφ cosθ X + siφ siθ Y + cosφ Z' = ρ where θ [ 0 π ) φ [ 0 π ) ad ρ [ 0 ). Oe plae sample is composed of three poit samples. If the sampled poits are too close co-liear or the same Z depth i 3D space owig to the discrete z-axis ad mismatches of stereo visio the parameter space could be misled. We use a gradiet sample method for robust groud plae parameter extractio to improve the efficiecy of the plae sample as well as the precisio of parameters. For the fast gradiet computig i disparity map the first derivative of Gaussia i the orietatio 0 o ad 90 o are used: 0 ( x + y ) ( x + y ) G = e = xe ad x 90 ( x + y ) ( x + y ) G = e = ye y ad the first derivative of Gaussia i the orietatio is 0 90 G = cos G + si G which is the orietatio steerable [0]. The covolutio of G represets the differece of origial data. Therefore 0 90 G I = cos ( G I) + si ( G I) ad the gradiet orietatio is the which makes the absolute of G I maximal. Deote G 0 90 I ad G I as a ad b respectively. Thus we have G I = a + b cos( φ) where cosφ = a a + b ad siφ = b a + b. 0 π ad the gradiet Cosiderig the sig costrai [ ) orietatio is = arccos( a a + b ). Oe of the advatages usig orieted steerable filter for gradiet computig is that it is X-Y separable ad symmetrical which simplifies the computig. The mai steps of plae parameter extractio are listed as follows: (a) Radomly sample the first poit A i disparity map. (b) Use G filter to compute the gradiet orietatio at A. (c) Radomly sample the secod poit B alog the orietatio from A ad the step should be bigger tha the threshold. (d) Assume that = π 0 π. (e) Radom sample the third poit C alog the orietatio from M which is the midpoit of A ad B ad the step also should be bigger tha the ad [ ) threshold. (f) Map poits A B ad C ito 3D space compute the plae parameters ad vote i parameter space. (g) Repeat the step to 6 for eough plae samples. (h) Select the peak i parameter space as the result. Radom sample Gradiet sample Fig.4: Parameter space after samples ad votig. Fig.4 shows the parameter space (Hough space) which is from 0 to 0000 for higher precisio after plae samples from the dese disparity map of groud. The width of G filter is 5 ad the threshold of gradiet sample step is 30. As show i Fig.4 the parameter such as theta (red) i gradiet sample is much more smooth ad closer to the groud truth. It ca be iferred that much fewer plae samples also ca give good result by usig gradiet sample. C. Super video rate obstacle detectio ad locatio After groud ad backgroud filterig obstacles ca be located i 3D space by usig coected regio detectio. Ru-legth is the commo method o geeral istructio processor [ ]. For the parallel implemetatio o hardware coected regio labelig algorithm is used. There are four sub-modules implemeted by fiite state machie (FSM) ad o-chip RAM as show i Fig.5. I the first sca a label is assiged to every poit which has obstacle tag. Durig scaig if two poits with differet label are coected based o Four-Coectivity a pair of equal labels is submitted to the label table. The label table records the labels used i the first sca with their equal relatio. After the first sca the equal labels have the same value i label table. The label resort reassigs a value to every equal label group for the idex. I the secod sca a statistic based o the labeled disparity map ad the label idex is performed to locate the obstacles i pixel coordiate. O hardware equal labels cache ad label uiqueess tag are also used to boost the efficiecy of parallel processig. This module performs coected regio detectio at more tha 00 frames per secod (fps) whe workig frequecy is 66MHz. Durig the secod sca the geometrical feathers of coected regio such as boudary (width ad height) positio (vertex coordiates of the rectagle area) area (pixel umber of the regio) desity (ratio of the area ad the boudary) ad disparity average (sum of disparity divided by the area) are computed. Based o domai kowledge the coected regio is filtered through a set of heuristics for the real obstacles. For example if the area desity or boudary is ot satisfied the criterio the coected regio would be

7 igored. After 3D mappig the real obstacle positio is located as show i Fig.5. However some problems will arise whe multiple obstacles are occluded i 3D maps. To hadle this situatio multiple disparity layers or multiple backgrouds aalysis is eeded. ACKNOWLEDGEMENTS This work was partially supported by the Natioal Sciece Foudatio of Chia ( ) ad the Chiese High-Tech Program (005AA73707). REFERENCE (a) (c) (d) Fig.5: Obstacle detectio ad locatio. (a) Rectified image. (b) Disparity map before 3D mappig. (c) Coected regios are detected after groud ad backgroud filterig. (d) Real obstacle is located after heuristics filterig. (b) V. ROBOT OBSTACLE AVOIDANCE AND NAVIGATION The Lyx Hexapod II Kit is used as the small robot testbed which is a full size twelve servos hexapod featurig two degrees of freedom per leg as show i Fig.. We also develop the TMS30C540 system board for the robot movemet cotrol ad high level computig. The baselie legth betwee the camera C ad C is 7.6cm which allow for reliable stereo visio ad obstacle detectio at the rage of 30cm to.6m. The hexapod robot with SVSoC is tested o the idoor mosaic floor as show i Fig.6 where DC power supply is used istead of battery. The allowable earest depth is about 60cm ad the obstacle ca be located ad avoided if it is too close to robot. VI. CONCLUSIONS This paper presets the architecture ad implemetatio of the stereo visio system o programmable chip (SVSoC) for small robot avigatio. SVSoC ca perform dese depth mappig with oliear iteratio based triocular stereo visio at video rate. Stereo visio based eviromet modelig groud extractio ad obstacle detectio are itegrated i oe FPGA for multiple modal outputs. Our system is also well suitable for may visio-based systems ad applicatios such as SLAM self-localizatio ad eve eviromet recogitio. []. B. Chemel E. Mutschler H. Schempf. Cyclops: Miiature Robotic Recoaissace System. Proc. of 999 IEEE Iteratioal Coferece o Robotics ad Automatio Detroit Michiga May 999. []. J. Klahold J. Rauteberg U. Ruckert. Cotiuous Soar Sesig for Mobile Mii-Robots. Proc. of 00 IEEE Iteratioal Coferece o Robotics ad Automatio Washito DC May 00. [3]. T. Williamso C. Thorpe. A Triocular Stereo System for Highway Obstacle Detectio. Proc of 999 IEEE Iteratioal Coferece o Robotics ad Automatio Detroit Michiga May 999. [4]. K. Sabe M. Fukuchi J. Gutma T. Ohashi. Obstacle Avoidace ad Path Plaig for Humaoid Robots usig Stereo Visio. Proc of 004 IEEE Iteratioal Coferece o Robotics ad Automatio 004. [5]. D. Burschka S. Lee ad G. Hager. Stereo-Based Obstacle Avoidace i Idoor Eviromets with Active Sesor Re-Calibratio. Proc. of 00 IEEE Iteratioal Coferece o Robotics ad Automatio 00. [6]. C. Lawrece Zitick Takeo Kaade. A Cooperative Algorithm for Stereo Matchig ad Occlusio Detectio. IEEE Trasactios o Patter Aalysis ad Machie Itelligece pp [7]. D. Scharstei R. Szeliski R. Zabih. A Taxoomy ad Evaluatio of Dese Two-Frame Stereo Correspodece Algorithms. IEEE Workshop o Stereo ad Multi-Baselie Visio (SMBV'0) pp [8]. Lupig A Yude Jia Jig Wag Migxiag Li Xiaoxu Zhag. A Efficiet Rectificatio Method for Triocular Stereovisio. ICPR'04 - Volume 4 pp [9]. Kei Okada Satoshi Kagami Masayuki Iaba Hirochika Ioue. Plae Segmet Fider: Algorithm Implemetatio ad Applicatios. Proc. of the 00 IEEE Iteratioal Coferece o Robotics & Automatio Seoul Korea May [0]. W. T. Freema E. H. Adelso. The desig ad use of steerable filters. IEEE Trasactio o Patter Aalysis ad Machie Itelligece. Volume 3 Issue 9 Pages: Sept. 99. []. J. Bruce T. Balch M. Veloso. Fast ad Iexpesive Color Image Segmetatio for Iteractive Robots. Proc. of 000 IEEE/RSJ Iteratioal Coferece o Itelliget Robots ad Systems 000. []. M. Sridhara P. Stoe. Real-time Visio o a Mobile Robot Platform. Proc. of 005 IEEE/RSJ Iteratioal Coferece o Itelliget Robots ad Systems 005. () () (3) (4) Fig.6: Stereo visio based obstacle locatio for avigatio