Ladar-aed Detection and Tracing of Moving Object fro a Ground Vehicle at High Speed Chieh-Chih Wang, Charle Thorpe and rne Suppe Robotic Intitute Carnegie Mellon niverity Pittburgh, P, 15213S Eail: {bobwang, cet, uppe}@c.cu.edu btract Detection and tracing of oving object (DTMO) in crowded urban area fro a ground vehicle at high peed i difficult becaue of a wide variety of target and uncertain poe etiation fro odoetry and GPS/DGPS. In thi paper we preent a olution of the iultaneou localization and apping (SLM) with DTMO proble to accoplih thi ta uing ladar enor and odoetry. With a precie poe etiate and a urrounding ap fro SLM, oving object are detected without a priori nowledge of the target. The interacting ultiple odel (IMM) etiation algorith i ued for odeling the otion of a oving object and to predict it future location. The ultiple hypothei tracing (MHT) ethod i applied to refine detection and data aociation. Experiental reult deontrate that our algorith i reliable and robut to detect and trac pedetrian and different type of oving vehicle in urban area. 1 Introduction Detection and tracing of oving object (DTMO) i one of the ot iportant and challenging proble for driving aitance and autonoou driving. lthough the DTMO proble ha been extenively tudied for everal decade [1, 2, 3, 4, 9, 10], it i till very difficult to accoplih DTMO in crowded urban environent fro a ground vehicle at high peed. One of the ot difficult iue i to eparate oving object and tationary object. In indoor environent, the ot iportant target are people. If caera are ued to detect people, the appearance-baed approache are widely ued and people can be detected no atter if they are oving or not. If laer canner are ued, the featurebaed approache are uually the preferred olution [5, 6, 7]. oth appearance-baed and feature-baed ethod rely on a priori nowledge of target. In urban area, becaue there are any ind of oving object uch a pedetrian, anial, wheelchair, bicycle, otorcycle, car, bue, truc, trailer, etc., it i very difficult to define feature or appearance by uing laer canner. In order to accoplih DTMO fro a oving platfor, a precie localization yte i eential [7, 8, 14]. nfortunately, a good inertial eaureent yte i very expenive and it i nown that GPS and DGPS often fail in the urban area becaue of the urban canyon effect. In the pat decade, the iultaneou localization and apping (SLM) proble ha received ubtantial interet in robotic and I literature [11], which provide a ore precie poe etiate than inertial eaureent yte; and a global conitent urrounding ap without a priori ap and without acce to independent poition inforation. However, ot of the publihed wor on SLM aue that the environent i tatic. In [12], we preented an approach to tacle the SLM proble and the DTMO proble at once. SLM provide ore accurate poe etiation and a urrounding ap, which are ued to detect oving object reliably. SLM can be ore accurate becaue oving object are filtered out of the SLM proce than to the oving object location prediction fro DTMO. SLM and DTMO are utually beneficial, a hown in Figure 1. In [13], we derived the ayeian forula of the SLM with DTMO proble, which provide a olid bai for undertanding and olving thi proble. Our olution of the SLM with DTMO proble atifie both the afety and navigation deand of the driving aitant and autonoou driving yte by uing laer canner and odoetry. Siultaneou Localization and Mapping (SLM) MO detection MO future location prediction Map ccurate poe Detection and Tracing of Moving Object (DTMO) Figure 1: SLM with DTMO Since SLM in urban and uburban area wa addreed in [12], in thi paper the SLM part of the whole proble i treated a a blac box, which provide a
urrounding ap and a better poe etiate than odoetry. In order to etablih the baic terinology ued throughout thi paper, the ayeian forula of the SLM with DTMO proble i briefly introduced and it eaning i explained in Section 2. Section 3 addree our DTMO algorith tep by tep. The reult of experient, carried out with the CM Navlab11 vehicle in crowded urban area, are hown in Section 4, and the concluion i in Section 5. 2 SLM with DTMO The SLM with DTMO proble i not only to olve the SLM proble in dynaic environent but alo to detect and trac thee dynaic object. More pecifically, what we dicu i how to etiate the poe of the robot, build a ap, detect other oving object and to predict their otion, given odoetry and laer canner eaureent fro the robot. In thi ection, the ayeian forula of the SLM with DTMO proble i introduced and it eaning i explained. In addition, local SLM with DTMO i addreed. 2.1 Notation We denote the dicrete tie index by the variable, the vector decribing an odoetry eaureent fro tie 1 to tie by the variable u, a laer canner eaureent fro the vehicle at tie by the variable z, the tate vector decribing the true location of the vehicle at tie by the variable x, and the tochatic ap which contain l feature by the variable M { 1,, l 1 = }. Y { n = y, y } are the location of oving object, of which there are n oving object that appeared inide the enor range at tie. In addition, we define the following et to refer data leading up to tie. = { u0, u1,..., u } = { 1, u }// Odoetry (1) Z = { z 0, z1,..., z } = { Z 1, z }// Ladar (2) X = { x0, x1,..., x } = { X 1, x }/ True. Location (3) where the initial location of the vehicle x 0 i aued nown. 2.2 ayeian Forulation ecaue the environent contain not only tatic object but alo dynaic object, the eaureent fro enor contain inforation fro both tatic and dynaic object. The general recurive forula for SLM with DTMO can be expreed a: p x, Y, M ) (4) ( We tart with the following auption: The vehicle otion odel i Marov. The enor eaureent can be eparated into oving part and tationary part and that they are independent: z = z + z and hence Z = Z + Z (5) Here the enor eaureent belonging to tationary object i denoted by the variable z and the enor eaureent belonging to oving object i denoted by the variable z. When etiating the poterior over the ap and the vehicle poe, the eaureent of oving object carry no inforation, neither do their location Y. Then the general recurive ayeian forula of (4) can be derived and expreed a (See [13] for the derivation of thi equation): p ( Y, M, x ) (6) p z Y, x ) p( Y ) z M, x ) p( M, x ) ( 1 p( 1 1 = p( z Y, x ) p ( Y Y 1 ) p( Y 1 1, 1 ) dy pdate Prediction ( z M, x ) p x u, x 1 ) p( x 1, M 1 1 ) dx pdate Prediction p ( 1 2.3 Solving the SLM with DTMO Proble Fro (6), input to the SLM with DTMO filter are two eparate poterior, one of the conventional SLM for, p ( x 1, M 1, 1 ), and a eparate one for DTMO, p ( Y 1 1, 1 ). The poterior of the SLM part i recovered by: p ( x, M ) = p( Y, M, x ) dy (7) z M, x ) p x u, x ) p( x, M dx p( ( 1 1 1 1 ) 1 and the poterior of the DTMO part i coputed by: p( Y ) DTMO ) = p ( Y, M, x ) dmdx [ p ( z Y, x p( Y Y 1 ) p( Y 1 1, 1 ) dy 1 ] p ( x ) dx (8) Equation (8) how that DTMO hould tae account of the uncertainty in the poe etiate of the robot becaue the laer canner eaureent are directly fro the robot. Figure 2 illutrate the procedure for olving the SLM with DTMO proble. Figure 2(a) how the etiation and the correponding ditribution of the robot poe, a detected oving object poe (in green), SLM
and the ap at tie 1. In Figure 2(b), the robot ove and get a eaureent u fro odoetry. The oving object alo ove but there i no eaureent directly aociated with it otion. nlie uing u and the robot otion odel to predict the robot poe, only previou eaureent aociated with thi oving object are ued to odel the oving object otion and predict it poe. Figure 2(c) how that the robot get a new eaureent z at the new location. Here z contain inforation fro both tationary object and oving object. In Figure 2(d), the etiation and the correponding ditribution of the robot poe and the ap at tie are updated uing inforation only aociated with tationary object. Finally Figure 2(e) how the poe etiate and the correponding ditribution of the oving object are updated uing ore precie inforation fro SLM. Robot Static Object Static Object Moving Object (a) The robot poe, a oving object (b) (in green) and the tochatic ap at tie 1. (b) The robot and the oving object ove. SLM i with repect to the global coordinate frae. Equation (8) how that DTMO ha to proce ore uncertain inforation becaue DTMO ha to tae into account the poe etiate uncertainty fro SLM, which ae data aociation and tracing of oving object ore difficult (See Figure 3). Fortunately for application uch a driving aitant yte, it i not neceary to olve DTMO globally. The abolute poition and velocitie of oving object can be etiated with repect to the teporary global coordinate frae intead of the true global coordinate frae. Figure 3 and Figure 4 illutrate the difference between global SLM with DTMO and local SLM with DTMO. Once a oving object i detected at the firt tie 3, the tet vehicle frae at 3 (O in Figure 4) would be aigned a the teporary global frae for tracing thi oving object. Thi tranfor relative error in a global frae into the equivalent, but ore convenient, repreentation of abolute error in a local frae. o t 3 t 2 t 1 t? t +1 Figure 3: Global SLM with DTMO o t 3 t 2 t 1 t (c) The robot ene object and aociate the. (d) The robot poe and the ap update. o? t +1 Figure 4: Local SLM with DTMO 3 DTMO Ipleentation (e) The oving object poe and otion odel update Figure 2: SLM with DTMO procedure lthough the ditribution are hown by ellipe in Figure 2, the ayeian forula doe not aue that the etiation are Gauian ditribution. 2.2 Local SLM with DTMO The goal of SLM i to build a globally conitent ap, o the uncertainty etiate of the poe of the robot fro ing a precie poe etiate and a urrounding ap fro SLM, the propoed DTMO algorith olve the proble in the following anner. Firt, a new can i egented into everal group uing a iple ditance criterion. With the urrounding ap and the poe etiate fro SLM, oving object (group) are detected by finding inconitencie between the new can and the ap. ing the data aociated with a oving object, the Interacting Multiple Model (IMM) etiation algorith [2, 3, 4] trac and predict the otion of thi oving object with the contant velocity odel and the contant acceleration odel. The ultiple hypothei tracing (MHT) [9, 3] ethod i applied to refine detection and data aociation. 3.1 Scan Segentation
Scan egentation i the firt tep of the DTMO algorith. ecaue the laer can in our application are not dene and the target we want to detect and trac do not have pecific ize and hape, we ue a iple ditance criterion, naely that the ditance between point in two group ut be longer than 1 eter, to egent eaureent point into everal group. Figure 5 how one exaple of can egentation. current laer canner, and then convert the ap fro a rectangular coordinate yte to a polar coordinate yte. Now it i eay to detect oving point by coparing value along the range axi of the polar coordinate yte. group i identified a a potential oving object if the ratio of the nuber of oving point to the nuber of total point i greater than 0.5. Figure 8 how the reult of oving object detection and a red box indicate a oving car recognized by our otionbaed detector. Free Space Local Map New Scan SLM Figure 5: Scan Segentation 3.2 Moving Object Detection Intuitively, any inconitent part fro SLM hould belong to oving object. ut the idea in t totally correct. There are two cae for detecting oving object: Cae 1: Fro previou can or the ap, we now oe pace i not occupied. If we find any object in thi pace, thi object ut be oving. In Figure 6, object ut be a oving object. Object C Object Figure 7: Detection Cae 2. Free Space Local Map SLM New Scan Figure 8: Moving object detection 3.3 Slow Moving Object Detection Object Figure 6: Detection Cae 1. Cae 2: In Figure 7, we can t ay that object i a oving object. Object ay be a new tationary object becaue Object ay have been occluded by object C. What we are ure i that object C i a oving object. lthough we can t tell if object i oving or not by regitering only two can, the previou inforation doe help u to gue the characteritic of object. The oving object detection algorith conit of two part: the firt i to detect oving point; the econd i to cobine the reult fro egentation and oving point detection for deciding which group are potential oving object. Given a new can, the local urrounding ap, and the poe etiate fro local SLM, we firt tranfor the local urrounding ap to the coordinate frae of the Detection of pedetrian at very low peed i difficult but poible by including inforation fro the ap. Fro our experiental data, we found that the data aociated with a pedetrian i very all, generally 2-4 point. lo, the otion of a pedetrian can be too low to be detected by the otion-baed detector. ecaue the ap contain inforation fro previou oving object, we can ay that if a blob i in an area that wa previouly occupied by oving object, thi object can be recognized a a oving object. 3.4 The Interacting Multiple Model (IMM) algorith In SLM, we can ue odoetry and the robot otion odel to predict the future location of the robot. However, in DTMO neither a priori nowledge of oving object otion odel nor odoetry eaureent about oving object i available. Therefore we applied the IMM algorith with the contant velocity (CV) odel
and the contant acceleration (C) odel to odel the otion of a oving object and to predict it future location. Here we ue the Piecewie Contant White cceleration Model fro the CV odel and the Piecewie Contant Wiener Proce cceleration Model for the C odel [2, 3]. The prediction and etiation of a oving object i fro the ixing (cobination) of thee two otion odel. SICK291 Three-Caera SICK221 3.5 Multiple Hypothei Tracing Moving object detection i often erroneou becaue of eaureent error and pitch/roll otion of the tet vehicle. We apply the ultiple-hypothei-tracing (MHT) ethod to filter out wrong detection and refine data aociation. Local SLM i not run for every new can becaue of coputational power liitation and becaue odoetry i good enough locally. Moving object detection initialize the new trac and then trac aociation i accoplihed uing patial and hape inforation. The otion pattern of the hypothei tree branche are ued to prune the hypothei tree. t the next tie of oving object detection the hypothei tree will be confired and new oving object will be detected. Figure 10: The Navlab11 tetbed (a) Frae 31 (b) Frae 51 4 Experiental Reult One SICK LMS221 and two SICK LMS291 laer canner are ounted in variou poition on Navlab11 (Figure 10), doing horizontal profiling. The range data were collected at 37.5 Hz with 0.5 degree reolution. The axiu eaureent range of the canner i 80. The iage fro our three-caera yte are for viualization. Figure 11 how the raw range iage fro the ladar ounting on the front of the tet vehicle. The reult of local SLM with DTMO are hown in Figure 12, Figure 13 and Figure 14. Intead of dicarding inforation fro oving object, a tationary object ap (SO-ap) and a oving object ap (MO-ap) are created to tore inforation fro tationary object and oving object which are hown in blac and red repectively. The olid blue rectangle indicate the location of the tet vehicle. The blue boxe repreent the traced object. In Figure 12, one car (Object ) and one trailer (Object ) are detected and traced, but a pedetrian (Object C) i ied becaue the otion of the pedetrian i too all to detect at the beginning. fter accuulating inforation in the MO-ap, thi pedetrian i uccefully detected and traced, which i hown in Figure 13. Figure 14 how that our algorith egent a trailer (Object ) into three group and trac thee group individually. n algorith to analyze the relationhip of group for erging ultiple trac a a ingle object or plitting a trac into ultiple object i ongoing. (c) Frae 91 Figure 11: Raw range iage fro SICK LMS221. C Navlab11 (a) The reult of SLM with DTMO C (b) The iage fro the three-caera yte Figure 12: Ground Vehicle Detection and Tracing
P-26-7006-02 and P-26-7006-03; by och Corporation; and by SIC Inc. Figure 13: Pedetrian Detection and Tracing Figure 14 5 Concluion We have preented a ethod to accoplih detection and tracing of oving object fro a oving platfor baed on the ayeian forula of SLM with DTMO. Experiental reult have hown that DTMO in crowded urban area fro a ground vehicle at high peed i feaible uing odoetry and laer canner. cnowledgeent We than people in the Navlab group for their help with the experient. Thi wor i partially upported by the Federal Tranit dinitration through the Pennylvania Departent of Tranportation greeent Nuber TEC-116 under the Federal itance Progra C Navlab11 Navlab11 C Reference [1] Y. ar-shalo, Tracing Method in a Multitarget Environent, IEEE Tran. On utoatic Control, Vol. 23, No. 4, ug. 1978. [2] Y. ar-shalo and X.-R. Li, Multitarget- Multienor Tracing: Principle and Technique, YS, Danver, M, 1995. [3] S. lacan and R. Popoli, Deign and nalyi of Modern Tracing Syte, rtech Houe, M, 1999 [4] H.. P. lo and Y. ar-shalo, The Interacting Multiple Model lgorith for Syte with Marovian Switching Coefficient, IEEE Tran. On utoatic Control, Vol. 33, No. 8, ug. 1988. [5]. Fod,. Howard, and M. J Mataric, Laer- aed People Tracer, IEEE Int. Conf. on Robotic and utoation, May, 2002. [6]. Kluge, C. Kohler and E. Praler, Fat and Robut Tracing of Multiple Object with a Laer Range Finder, IEEE Int. Conf. on Robotic and utoation, pp. 1683-88, 2001. [7] M. Lindtro and J.-O. Elundh, Detecting and Tracing Moving Object fro a Mobile Platfor uing a Laer Range Scanner. Proc. Int. Conf. On intelligent Robot and Syte, Oct. 2001. [8] E. Praler, J. Scholz, and P. Fiorini, Robotic Wheelchair for Crowded Public Environent, IEEE Robotic & utoation Magazine, March, 2001. [9] D.. Reid, n lgorith for Tracing Multiple Target, IEEE Tran. On utoatic Control, vol. 24, no. 6, Deceber 1979. [10] D. Schulz, W. urgard, D. Fox and.. Creer. Tracing Multiple Moving Target with a Mobile Robot uing Particle Filter and Statitical Data ociation, IEEE Int. Conf. on Robotic and utoation, pp. 1665-70, 2001. [11] Suer School on SLM 2002, http://www.ca.th.e/slm/ [12] C.-C. Wang and C. Thorpe, Siultaneou Localization nd Mapping with Detection nd Tracing of Moving Object, IEEE Int. Conf. on Robotic and utoation, May, 2002. [13] C.-C. Wang, C. Thorpe and S. Thrun, Online Siultaneou Localization and Mapping with Detection and Tracing of Moving Object: Theory and Reult fro a Ground Vehicle in Crowded rban rea, IEEE Int. Conf. on Robotic and utoation, May, 2003. [14] L. Zhao and C. Thorpe, Qualitative and Quantitative Car Tracing fro a Range Iage Sequence, Proc. of IEEE Conf. on Coputer Viion and Pattern Recognition, 1998.