The KCLBOT: Exploiting RGB-D Sensor Inputs for Navigation Environment Building and Mobile Robot Localization

Transcription

1 The KCBOT: Exploiting GB-D Senso Inputs fo Navigation Envionment Building and Mobile obot ocalization egula Pape Evangelos Geogiou 1,*, Jian Dai 1 and Michael uck 1 1 King s College ondon *Coesponding autho evangelos.geogiou@kcl.ac.uk eceived 7 May 211; Accepted 17 Aug 211 Abstact This pape pesents an altenative appoach to implementing a steeo camea configuation fo SAM. The appoach suggested implements a simplified method using a single GB D camea senso mounted on a maneuveable non holonomic mobile obot, the KCBOT, used fo extacting image featue depth infomation while maneuveing. Using a defined quadatic equation, based on the calibation of the camea, a depth computation model is deived base on the HSV colo space map. Using this methodology it is possible to build navigation envionment maps and cay out autonomous mobile obot path following and obstacle avoidance. This pape pesents a calculation model which enables the distance estimation using the GB D senso fom Micosoft.NET mico famewok device. Expeimental esults ae pesented to validate the distance estimation methodology. Keywods Mobile obot, Nonholonomic, GB D Sensos, Self localization, SAM 1. Intoduction One of the fundamental poblems in mobile obot autonomous navigation is map decomposition and building. Thun [1] in his 1997 pape talks about two majo mapping methods fo autonomous obots, which ae gid based and topological methods. Building maps as a mobile obot taveses a navigation envionment, while using a patially composed map to maintain localization is bette known as Simultaneous ocalization and Mapping (SAM). The most common methods of building localization maps ae using occupancy gids [2], featue building [3], and aw senso telemety [4]. In Elfes [2] 1989 pape, an occupancy gid method is pesented using multi sensos to build multidimensional andom fields that maintain stochastic estimates of an occupancy state. In eonad [3] et al. s 1992 pape on featue building fo dynamic maps fo autonomous mobile obots, a citical point is made on senso poblems coming fom noise and impotance on the validation of senso measuements. This pape uses a simila appoach to that pesented by Fang [4] et al, by using a aw senso appoach acquiing telemety infomation via a monocula vision senso. The appoach pesented by this pape uses a simila method, utilizing aw senso telemety via an inexpensive off the shelf GB D camea. An GB D camea is a pojective senso that measues the beaing of image featues and etuns an image with depth telemety, nomally using an HS o HSV colo map. Using the depth infomation fom the GB D 194 Int J Adv obotic Sy, 211, Vol. 8, No. 4,

2 Figue 1. The Netduino Plus: A Micosoft.Net Mico Famewok Micocontolle senso, it is possible to build a navigation map, which in tun can be used fo mapping, sensing, localizing, and envionment modeling. The focus of the map building and localization methodology is fo small fom facto mobile obots using the Micosoft.NET Mico Famewok [5], whee the map building and localization methods ae pogammed using the limitations of the Windows Pesentation Foundation (WPF) [6]. The methodologies wee tested on Netduino Plus [7] and the GHI ChipwokX Development System [8]. The motivation fo developing these methodologies stems fom the platfom limitations of OpenKinect [9] and obot Opeating System (OS) [1] on.net Mico Famewok micocontolles. An altenative to the GB D camea is the implementation of a steeo camea configuation [11], which utilizes two CMOS GB cameas, to poduce 3D econstuctions of the envionment in its visual ange. The idea behind using a single GD D camea is to simplify the steeo camea appoach, and educe the computation time to extact depth infomation fom an image and use this image fo SAM. 2. The KCBOT: A Self ocalizing Mobile obot 2.1 A Maneuveable Non holonomic Mobile obot The KCBOT [12] is a non holonomic two wheeled autonomous maneuveable mobile obot. The mobile obot is built aound the specifications fo Micomouse obot and the obocup competition. These specifications contibute to the mobile obot s fom, facto and size. This mobile obot holds a complex electonic system to suppot on line path planning, self localization, and even SAM, which is made possible by the on boad senso aay. The mobile obot is loaded with eight obot Electonics SF5 ultasonic anges [13]. The dive system of the mobile obot is suppoted by Nubotics WC 132 WheelCommande Motion Contolle [14] and two WW 1 WheelWatche Encodes [14]. The motos fo the mobile obot ae modified continuous Figue 2. The KCBOT: A Non holonomic Maneuveable Mobile obot. otation sevo motos; these ae equied fo the WheelCommande Motion Contolle. The otation of the mobile obot is measued by the obot Electonics CMPS3 Compass Module [13]. The combination of hadwae electonics and dive mechanics makes the KCBOT, as epesented in Fig. 2, an ideal platfom fo the self localization methodology. In the maneuveable classification of mobile obots [15], the vehicle is defined as being constained to move in the vehicle s fixed heading angle. Fo the vehicle to change maneuve configuation, it needs to otate about itself. As the vehicle taveses on a two dimensional plane, both left and ight wheels follow a path that moves aound the instantaneous cente of cuvatue at the same angle, which can be defined as, and thus the angula velocity of the left and ight wheel otation is deduced as follows: θ ω(icc ) 2 (1) θ ω(icc ) 2 (2) whee is the distance between the centes of the two otating wheels, and the paamete icc is the distance between the mid point of the otating wheels and the instantaneous cente of cuvatue. Using the velocities (1) and (2) of the otating left and ights wheels, θ and θ espectively the instantaneous cente of cuvatue, icc and the cuvatue angle, ω, can be deived as follows: (θ θ ) icc 2(θ θ ) ω (θ θ ) (3) (4) Int J Adv obotic Sy, 211, Vol. 8, No. 4,

3 Using (3) and (4), two singulaities can be identified. When, the adius of instantaneous cente of cuvatue, icc, tends towads infinity and this is the condition when the mobile obot is moving in a staight line. When θ θ, the mobile obot is otating about its own cente, and the adius of instantaneous cente of cuvatue, icc, is null. When the wheels on the mobile obot otate, the quadatue shaft encode etuns a counte tick value; the otation diection of the otating wheel is given by a positive o negative value etuned by the encode. Using the numbes of tick counts etuned, the distance tavelled by the otating left and ight wheel can be deduced in the following way: d d πd ticks (5) es πd ticks (6) es whee and epesent the numbe of encode ticks ticks pulses counted by the left and ight wheel encodes, espectively, since the last sampling, and D is defined as the diamete of the wheels. With esolution of the left and ight shaft encodes, es and es, espectively, it is possible to detemine the distance tavelled by the left and ight otating wheel, d and d. This calculation is shown in (5) and (6). In the field of obotics, holonomicity [16] is demonstated as the elationship between the contollable and total degees of feedom of the mobile obot, as pesented by the mobile obot configuation in Fig. 3. In this case, if the contollable degees of feedom ae equal to the total degees of feedom, then the mobile obot is to be defined as holonomic. Othewise, if the contollable degees of feedom ae less than the total degees of feedom, it is non holonomic. The maneuveable mobile obot has thee degees of feedom, which ae its position in two axes and its oientation elative to a fixed heading angle. This individual holonomic constaint is based on the mobile obot s tanslation and otation in the diection of the axis of symmety and is epesented as follows: whee x c and y cos( ) x sin( ) d (7) c c y c ae Catesian based coodinates of the mobile obot s cente of mass, and descibes the heading angle of the mobile obot, which is efeenced fom the global x axis. To conclude, (7) pesents the pose of the mobile obot. The mobile obot has two contollable degees of feedom, which contol the otational velocity of the left and ight wheels and, advesely with changes in otation, the heading angle of the mobile obot is affected. These constaints ae as follows: θ and y sin( ) x cos( ) θ (8) c c y sin( ) x cos( ) θ (9) c c l whee θ ae the angula displacements of the l ight and left mobile obot wheels, espectively, and whee descibes the adius of the mobile obot s diving wheels. As such, the two wheeled maneuveable mobile obot is a non holonomic system. To conclude, (8) and (9) specify the angula velocity of the mobile obot s left and ight wheels. 2.1 Self ocalization Using a Dual Shaft Encode Configuation By using the quadatue shaft encodes that accumulate the distance tavelled by the wheels, a fom of position can be deduced by deiving the mobile obot s (x, y) Catesian position and the maneuveable vehicle s oientation, with espect to time. The deivation stats by defining and consideing s(t) and (t) to be function of time, which epesents the velocity and oientation of the mobile obot, espectively. The velocity and oientation ae deived fom diffeentiating the position fom as follows: dx s( t).cos( ( t)) dt (1) dy s( t).sin( ( t)) dt (11) Figue 3. The KCBOT in X, Y, and Z Plana Envionment. Evangelos Geogiou, Jian Dai and Michael uck: The KCBOT: Exploiting GB-D Senso Inputs fo Navigation Envionment Building and Mobile obot ocalization 196

4 The change in oientation with espect to time is the angula velocity, which is defined in (4) and can be specified as follows: d dt l (12) When (12) is integated, the mobile obot s angle oientation value (t) with espect to time is achieved. The mobile obot s initial angle of oientation () is witten as and is given as follows: b Implementing (18), (19), and (2) povides a solution to the elative position of a maneuveable mobile obot. This is a possible solution to the self localization poblem but is subject to accumulative dift of the position and oientation with no method of e alignment. The accuacy of this method is subject to the sampling ate of the data accumulation, such that if small position o oientation changes ae not ecoded, the position and oientation will be eoneous. 3. Navigation Envionment Building Using a GB D Visual Senso ( ) ( t) b l t (13) The velocity of the mobile obot is equal to the aveage speed of the two wheels and this can be incopoated into (1) and (11), as follows: dx l cos( ( t)) (14) dt 2 dy l.sin( ( t)) (15) dt 2 The next step is to integate (14) and (15) to the initial position of the mobile obot, which is as follows: ( ) ( ) l t l x( t) x sin sin( ) (16) 2( ) b l ( ) ( ) l t l y( t) y cos cos( ) (17) 2( ) b l Equations (16) and (17) descibe the mobile obot s position, whee x() x and y() y ae the mobile obot s initial positions. The next step is to epesent (13), (16) and (17) in tems of the distances that the left and ight wheels have tavesed, which ae defined by d and d. This can be achieved by substituting θ and θ in l (13), (16) and (17) fo d and d, espectively, and also dopping the time constant t to achieve the following: 3.1 A GB D Image Captue using Micosoft s Kinect Senso In ealy Novembe 21, Micosoft eleased the Kinect senso, which has the capability of GB and GB D image captue. Exploiting the pe eleased dives fo this device, it is possible to expot and manipulate both the GB and GB D images fo mapping, sensing, locating and modeling fo SAM. The Kinect depicted in Fig. 4, which is mounted on the KCBOT, is built aound PimeSense s PS18 SoC [2]. The PS18 acts as a contolle fo the I light souce in ode to build the input image with I light coding depth infomation. The light coding in this case is mapped on HSV [17]. The device is configued such that a standad CMOS image senso eceives the pojected I light souce and tansfoms the image to poduce a depth image. The senso has a field of view of 58 Hoizontal (H), 4 Vetical (V), and 7 Diagonal (D). The senso allows fo a maximum image depth size of VGA, which is a esolution of 64x48 and has maximum image thoughput fame ate of 6fps. Finally, the opeational ange of the senso is limited to a minimum of.8m and to a maximum of 3.5m. d d (18) 2 ( d d ) ( ) d d t x( t) x sin sin( ) 2( d d ) b (19) Figue 4. The Kinect Mounting Configuation on the KCBOT. ( d d ) ( ) d d t y( t) y cos cos( ) 2( d d ) b (2) Int J Adv obotic Sy, 211, Vol. 8, No. 4,

5 GB Image Figue 5. GB and GB D Senso Images. GB D Image As depicted in Fig. 5, the senso is able to acquie GB and GB D images, whee the GB D images ae used fo envionment building. 3.2 Distance Estimation Using an GB D Senso Befoe the GB D images can be used fo envionment map building, the depth infomation fom the image needs to be computed. To achieve this, a function needs to be deived to convet the GB colo values to Hue. Once the Hue value is acquied, it can be used to compute elative distance estimation. Using the model pesented by Joblove and Geenbeg [17], the following equation is implemented. undefined, if C G B mod 6, if M C H B 2, if M G C G 4, if M B C (21) Hee (21), the fomula specifies the tanspose of Hue value, using C M m, which specifies the choma value and whee M max(, G, B) and m min(, G, B) specify the maximum and minimum values of the GB colo, espectively. The convesion function used to convet the tanspose of Hue (21) to the Hue value needed, is as follows. Figue 6. Sample GB D Distance Estimation Images. Sample G B Hue Distance (cm) Table 1. Sample GB D Distance Estimation esults The values equied to build a Hue to distance function ae pesented in Table I. d H H (23) Using Table 1, a distance estimation function is pesented by (23). Using the quadatic equation that descibes distance as a function of Hue, a behavio gaph is poduced, and pesented in Fig. 7. Based on this gaph, when distance is less than 8cm o geate than 35cm, the slope of the cuve changes moe significantly. This confoms to the manufactue s specifications on the limitations of distance estimation above and below these distance anges. H 6. H (22) Now that a convesion fo Hue fom GB has been defined, a function to convet the Hue values to distance is equied. To achieve this, sample images ae captued fom the senso, using a static object with a known distance fom the senso. Using the images pesented in Fig. 6, the aveage GB values fom the static object ae extacted and conveted to a Hue valuation. Figue 7. HSV Exponential Mapping of Distance vs. Hue. Evangelos Geogiou, Jian Dai and Michael uck: The KCBOT: Exploiting GB-D Senso Inputs fo Navigation Envionment Building and Mobile obot ocalization 198

6 3.3 Expeiment Statistical Analysis To validate the distance estimation equation pesented in (23), the GB D senso is used to captue 22 image samples of a static object between a distance ange of 8 to 3cm, with sample steps on 1cm. This selection was made based on the behavio of the cuve pesented in Fig. 7. At the same time as the image is captued, an empiical measuement of distance is taken. The empiical measuement has a toleance of.5mm eo. The images ae pocessed on the Micosoft.NET Mico Famewok micocontolle and stoed on a database on the micocontolle boad, fo analysis. The histogam in Fig 8 demonstates that the eo is oughly nomally distibuted between.3 to.2 and shows some anomaly below.3. Method Evaluation Mean % Confidence Inteval fo Mean (B) % Confidence Inteval fo Mean (UB) % Timmed Mean Median Vaiance.22 Standad Deviation Standad Eo Mean Minimum.394 Maximum ange Intequatile ange.2478 Skewness.33 Kutosis.933 Figue 9. Boxplot of the expeiment data The boxplot pesented in Figue 9 visually shows the distance fom to the mean, which is.67. It also pesents the intequatile ange, which is.248 and minimum (.39) and the maximum (.198) values. Table 2. Statistical analysis of expeimental data The esults pesented in Table 2 show statistical analyses of the 22 samples taken fom the micocontolle. The mean value is elatively low and fo moe accuacy because the data might be skewed the median value is moe useful and is compaably low. The confidence intevals which epesents two standad deviations fom the mean, equivalently pesent a low eo ate. Figue 1. Nomal Q Q plot of eo The Q Q plot depicted in Figue 1 pesents the pefomance of the obseved values against the expected values. This demonstates that the eos ae appoximately nomally distibuted centally, with anomalies at both tail ends. 3.4 Expeiment Validation Figue 8. The Fequency vs. Eo Histogam Using the statistical analysis pesented in the pevious section, a non paametic test is equied to validate the pactical effectiveness of the distance estimation equation (23). The ideal analysis test fo a non paametic independent one sample set of data is the Kolmogoov Sminov test [18] fo significance. Int J Adv obotic Sy, 211, Vol. 8, No. 4,

7 The empiical distibution function F fo n independent n and identically distibuted andom vaiables obsevations X is defined as follows: i Whee I X i x equal to 1 if X n 1 F ( x) I (24) n Xi x n i1 descibes the indicato function, which is i x and is equal to othewise. The Kolmogov Sminov statistic [18] fo a cumulative distibution function F( x ) is as follows: D sup F ( x) F( x) (25) n x n Whee sup x descibes the supemum of the set of distances. Fo the analysis test to be effective to be able to eject a null hypothesis, a elatively lage numbe of data is equied. Unde the null hypothesis that the sample oiginates fom the hypothesized distibution F( x ), the distibution is descibed as follows: nd n n sup B( F( t)) (26) Whee B( t ) descibes the Bownian bidge [19]. If F is continuous, then unde the null hypothesis nd n conveges to the Kolmogoov distibution, which does not depend on F. The analysis test is constucted by using the citical values of the Kolmogoov distibution. The null hypothesis is ejected at level if ndn Ka, whee K a is calculated fom the following: t P( K K ) 1 (27) It should also be noted that the asymptotic powe of this analysis test is 1. Fo the expeimental data pesented in this pape the null hypothesis is that the distibution of eo is nomal with a mean value of.6732 and a standad deviation of Based on significance level of.5, a significance of.44 is etuned using the one sample Kolmogoov Sminov test (28). The stength of the etuned significance value allows us to etain the null hypothesis and say that the distibution of eo is nomal with a mean value of.6732 and a standad deviation of a 4. Envionment Building Befoe the autonomous mobile obot can complete any path planning o path following tasks, it equies sufficient infomation about the envionment that it will be tavesing. To povide the mobile obot with this infomation, the detail fom the GB D camea is used to make a plot of the teain, mapping the unobstucted space the mobile obot can utilize. Befoe the GB D image can be used, the noise, esolved as black pixels in the ange of #E4E1Ch to #FFFFFFh, needs to be emoved fom the image. This is achieved by conveting the GB D image to gay scale [2]. This pocess is caied out to potect natual colos in the #E4E1Ch to #FFFFFFh ange. In the GB colo model, a colo image can be epesented by the following intensity function: I ( F, F, F ) GB G B (29) In (29), F is the intensity of the pixel (x,y) in the ed channel, F G is the intensity of pixel (x,y) in the geen channel, and F B is the intensity of pixel (x,y) in the blue channel. Using only the bightness infomation, the colo image can be tansfomed into a gay scale image [2]. The equation that convets a colo pixel to a gay scale pixel is pesented below (25). I.333F.5F.1666F (3) GS G B Afte the image has been conveted to gay scale, as depicted in Fig. 11, the black pixels ae filteed out of the image. Once the image has been stipped of the black noise pixels, as depicted in Fig. 12, the colo detail is equied fo mapping the tavesable envionment. The GB D depth colo infomation fom Fig. 11 is emapped onto the gay scale filteed image; the esult is pesented in Fig. 13. GB D Image Figue 11. Convesion fom GB D to Gay Scale. Gay scale Image Evangelos Geogiou, Jian Dai and Michael uck: The KCBOT: Exploiting GB-D Senso Inputs fo Navigation Envionment Building and Mobile obot ocalization 2

8 Using a simple Bezie path [22], the KCBOT is instucted to tavese a defined path and captue sample images using the GB D camea. Using the mobile obot s ability to self localize, implementing (18), (19), and (2), while captuing and supeimposing the conveted images, using (33), an envionment map is constucted. Figue 12. Convesion fom GB D to Gay Scale. Figue 15. Envionment Map Geneation fom Path Following. Figue 13. emapping Colo on Filteed Gay Scale Image. Input pixel x y GBD pixel pixel pixel (31) The initial esults pesented in Fig. 15, povide the mobile obot with detailed infomation of the envionment and it can easily estimate the Euclidean distance to all obstacles in ange. 1 ( ) cos( ) sin( x sin( ) cos( ) (32) The input matix (31) is multiplied by the otation matix [21] fom (32), whee 9, to poduce a topological view of the tavesable space. Output pixel x GBD y pixel pixel pixel (33) Using the fist two components of (33), a 2 dimensional tavesable space plot is poduced, whee the GB Dpixel is calculated using (23). The geneation of a topological view of the available navigation space, as pesented in Fig. 14, povides the mobile obot with sufficient infomation fo path planning and obstacle avoidance. Input Image Output Image Figue 14. Envionment Map Geneation fom GB D Image. Figue 16. Single sample 3D image econstuction Using the depth infomation fom equation (34) and mapping depth on the pixels of the image in Fig 13 it is possible to build a 3D econstuction of the envionment, which is demonstated in Fig Conclusion The intoduction of the affodable off the shelf GB D camea senso, which captues images with depth infomation mapped in the HSV ange, makes SAM possible with minimal computation. This is validated by the expeimental esults pesented in Fig. 11. The only concen of note at this stage is the toleance levels pesented by the distance estimation behavio gaph in Fig. 6. The limitation is that obstacles.8m away fom the senso will not be esolved accuately. This is only a concen fo navigation in small spaces but if the navigation aea is lage and open spaces ae available, the senso will pove to be a valuable tool fo SAM. Int J Adv obotic Sy, 211, Vol. 8, No. 4,

9 The expeiment esults and statistical analysis is valuable fo anyone wanting to adopt a simila monocula senso appoach. The etention of the null hypothesis validates the use of the senso and the pefomance is acceptable fo small fom facto mobile obots equiing envionment mapping. 6. efeences 1. Thun, S., eaning Metic Topological Maps fo Indoo Mobile obot Navigation. Jounal on Atificial Intelligence, (1): p Elfes, A., Using Occupancy Gids fo Mobile obot Peception and Navigation. Compute Jounal, (6): p eonad, J., Duant Whyte, H., and Cox, I, Dynamic Map Building fo an Autonomous Mobile obot. The Intenational Jounal of obotic eseach, : p Fang, F., Ma, X., and Dai, X. A Multi Senso Fusion SAM Appoach fo Mobile obots. in 25 IEEE Intenational Confeence on Mechatonics and Automation Micosoft.NET Mico Famewok. Available fom: us/netmf/default.aspx. 6. Windows Pesentation Foundation (WPF). 7. Netduino Plus. Available fom: 8. GHI ChipwokX Development System. Available fom: 9. OpenKinect. Available fom: 1. obot Opeating System (OS). Available fom: Chilian, A., and Hischmulle, H. Steeo camea based navigation of mobile obots on ough teain. in Intenational Confeence on Intelligent obots and Systems. 12. Geogiou, E. KCBOT. 21; Available fom: Mobile obot Distance & Oientation Sensos. Available fom: electonics.co.uk. 14. Mobile obot Navigation Sensos. 15. Campion, G., Bastin, G., and D Andea Novel, B, Stuctual Popeties and Classification of Kinematic and Dynamic Models of Wheeled Mobile obots. IEEE Tansactions on obotics and Automation, (1). 16. Kolmanovsky, I.a.M., N., Developments in nonholonomic contol poblems. IEEE Contol Systems Magazine, 1995: p Joblove, G., and Geenbeg, D. Colo spaces fo compute gaphics. in 5th Annual Confeence on Compute Gaphics and Inteactive Techniques. 18. Zhang, G., Wang, X., iang, Y., and i, J., Fast and obust Spectum Sensing via Kolmogoov Sminov Tes. IEEE TANSACTIONS ON COMMUNICATIONS, (12): p Hu,., Zhu, H., Bounded Bownian bidge model fo UWB indoo multipath channel, in IEEE Intenational Symposium on Micowave, Antenna, Popagation and EMC Technologies fo Wieless Communications, 25. MAPE p Kuma, T., and Vema, K., A Theoy Based on Convesion of GB image to Gay image. Intenational Jounal of Compute Applications, 21. 7(2): p Hete, T., and ott, K., Algoithms fo decomposing 3 D othogonal matices into pimitive otations. Jounal on Computes & Gaphics, (5): p Choi, J., Cuy,., and Elkaim, G. Path Planning Based on Bézie Cuve fo Autonomous Gound Vehicles. in Poceedings of the Advances in Electical and Electonics Evangelos Geogiou, Jian Dai and Michael uck: The KCBOT: Exploiting GB-D Senso Inputs fo Navigation Envionment Building and Mobile obot ocalization 22