NON-LINEAR MULTIMODAL OBJECT TRACKING BASED ON 2D LIDAR DATA

Transcription

1 Metrol. Meas. Syst. Vol. XVI (009), No 3, pp METROLOGY AND MEASUREMENT SYSTEMS Index , ISSN NON-LINEAR MULTIMODAL OBJECT TRACKING BASED ON D LIDAR DATA Mchael Thuy, Fernando Puente León Insttut für Industrelle Informatonstechnk, Karlsruhe Insttute of Technology, Hertzstrasse 16, D Karlsruhe, Germany ( [email protected], , [email protected]) Abstract The contrbuton ntroduces a novel approach for trackng objects based on two-dmensonal ldar data. As a central trackng engne, we employ a partcle-flter-based soluton whch s capable of modellng non-lnear dynamc processes as well as non-gaussan nose dstrbutons for the underlyng process and sensor as well. In contrast to other ldar-based trackng approaches, no newly detected objects have to be assocated to already known objects n an explct manner. Snce our weghtng functon s mult-modal, the assocaton s done by the flter tself. Keywords: fuson, ldar, data assocaton, trackng. 009 Polsh Academy of Scences. All rghts reserved 1. Introducton Object detecton s one of the key abltes of advanced drver assstance systems. A vast lterature on ths subject exsts, and dfferent sensors (radar, ldar, monocular and stereo cameras, etc.) have already been employed to approach a robust object detecton. Generatng object hypotheses wth a monocular camera was shown n [1]. Facng the same problem wth a stereo camera and an approprate calbraton algorthm was e.g. presented n []. Snce acqurng pctures and all attached mage processng often suffers from alternatng ambent lght condtons, the use of dfferent sensor technologes can yeld a drastcal mprovement of system robustness. Takng actve sensors lke radar or ldar (lght detecton and rangng) scanners breaks the lmtaton of the surroundng lght condton due to ther actve sensng prncple [3, 4, 5]. The sensor tself emts a narrowband laser pulse at a specfed angle and measures the tme the lght takes to come back to the sensor. Besdes the very low dependence of the envronmental lght condton, the sensor only delvers a dstance measurement f the lght hts an object. Consequently, every measured pont represents a potental part of an object. Our contrbuton presents an approach to object detecton based on two -D ldar scanners. It does not only encompass an object percepton, but addtonally the objects are tracked over tme as well. Ths proceedng requres an adequate dynamc model for the objects. Snce the amount of data recorded by a -D ldar scanner s much less than the data from an mage sensor, the sgnal processng can take place faster, whch automatcally reduces the reacton tme of a techncal system. Common ldar-based trackng approaches utlze a lnear or an extended Kalman flter to predct the new object poston [6]. In the case of the lnear flter, the underlyng dynamc model allows an object to move forward ndependently of the movement to the left or rght. Consderng the physcal constrants of an automoble, ths model s not approprate. In order to avod these physcal contradctons, one can utlze a non-lnear process model lke the bcycle model. Stll, ths model smplfes the real physcal behavour, but t prevents an Artcle hstory: receved on Jul. 7, 009; accepted on Sep. 11, 009; avalable onlne on Oct., 009.

2 MICHAEL THUY, FERNANDO PUENTE LEÓN: NON-LINEAR MULTIMODAL OBJECT TRACKING BASED... unrealstc movement of the modelled automobles. But snce the extended Kalman flter tres to lnearze the process dynamcs n the current operatng pont, errors wll occur when usng ths approach. To mnmze the errors due to the non-lnear physcal characterstcs of the real object, we employ a partcle flter based on Monte Carlo smulatons for object trackng [7]. Thereby, t s possble to model non-lnear process dynamcs n a very easy and effcent way. The addtonal expense s longer computaton tme, whch s assocated wth the propagaton of a hgh amount of partcles through the non-lnear process model. However, the followng sectons show that t s possble to perform a real-tme trackng wth a suffcent number of partcles. Many approaches use a separate flter for each tracked object. In ths case, the computatonal load becomes non-determnstc. Snce a scene can contan an arbtrary number of objects, the trackng engne has to nstantate the correspondng amount of flters-provded the objects are correctly detected by the sensor. To constran the computatonal load, one could restrct the number of tracked objects. An alternatve would be to use a sngle flter that can handle several objects. Ths can be acheved by mult-modal flters, n whch the probablty densty functon descrbng the state varables objects estmated shows more than one maxmum. Based on vson and ldar data, [8, 9, 10] already showed a soluton capable of trackng several objects wth only a partcle flter. Our contrbuton enhances ths soluton by buldng a new weght functon. Ths weght functon ncorporates all the extracted data comng from the sensor. As a consequence, the clusterng process concernng the assgnment between partcle and measurement s done by the flter tself. By adjustng the coeffcents wthn the weght functon, the sensor-specfc characterstcs can be modelled as well. At the end of the paper, results are provded that demonstrate the acheved trackng outcomes. Havng a scannng cycle of 13.3 ms, the processng can stll be performed n realtme. The paper s organzed as follows: Secton llustrates the basc sensor setup and the data path to the detecton stage. Secton 3 ntroduces the statstcal fundamentals for our contrbuton. The next secton descrbes the chosen process model as well as the new weght functon. Thereafter, Secton 5 characterzes the feature extracton stage. After havng explaned the preprocessng, Secton 6 descrbes the flter. Secton 7 presents expermental trackng results. The last secton closes the paper wth a short concluson.. Fuson of Raw Ldar Data and Inertal Measurement Unt.1. Sensor setup and data processng In our object trackng approach, we use two separate one-layer laser scanners (-D ldars). The two sensors are synchronzed by hardware and sample object dstances wth an angular resoluton of a half degree and an overall angular range of 180 degrees. In order to get clear shapes of the surroundng objects, the frst scanner s placed at the front bumper of the car. The second scanner s fxed at the rear bumper. For an ego-movement wthout a ptch angle, the scannng plane s adjusted parallel to the street. The maxmum scannng dstance s clamed to be 80 m, but t s n fact manly restrcted through the actual object sze. Addtonally, n our contrbuton we use the data comng from an nertal measurement unt (IMU) combned wth a satellte-corrected dfferental global postonng system (DGPS). Consequently, the system delvers the exact ego-poston and, addtonally, the current egoheadng. By calbratng the two ldar sensors wth respect to the IMU's coordnate system, a 360

3 Metrol. Meas. Syst. Vol. XVI (009), No 3, pp frst calculaton of the ego-poston n a fxed world coordnate system as well as of the absolute poston of the detected object s enabled. As notced n [11] and [1], the ldar scanners are connected to the central computer and served by a correspondng hardware drver. The data s then read out of the real-tme database by the ldar trackng, whch s responsble for the tasks of object detecton, assocaton, and trackng... Spatal and temporal regstraton of 3-D laser ponts As already mentoned, the two ldar scanners cooperatvely sense ther envronment,.e. they delver nformaton whch s related to ther coverage area ndependent and can thus be fused complementarly to get evdence about the surroundng. In the present case, combnng the data of both sensors requres to regster them spatally nto a common coordnate system. The spatal regstraton nvolves the process of combnng the hypotheses derved from both the front and rear sensor. Snce they are calbrated one wth respect to the other, the data measured by one of the sensors has to be shfted and rotated accordng to the calbraton parameters. Then, the scannng ponts are descrbed by ther poston wthn a coordnate system n whch the car's reference pont represents the orgn. But stll, the descrpton of the envronment has a relatve character regarded from the fxed world's pont of vew..3. Fxed world observaton model due to coordnate transformaton Transformng the above-mentoned relatve envronment descrpton absolute one requres to ncorporate the absolute ego-poston ( x rel, y rel) nto an ( x ego, y ego) and the current headng α wth respect to the fxed coordnate system, whch s possble thanks to the data avalable from the IMU. To obtan a contnuous Cartesan coordnate system, we transform the ego-poston from ts natve descrpton through longtude and lattude nto UTM coordnates. Startng from that ego-poston we can, by shftng and rotatng, regster the scan ponts wthn a global system. Assumng the rotaton matrx R based on the ego-headng α, one can now calculate the absolute postons of the relatve coordnates ( x rel, y rel): x fx y fx = x ego y ego + R(α) x rel y rel. (1) For the sake of clearness, n the followng we wll denote the coordnates ( x fx, y fx) wthout an ndex. If coordnates refer to relatve coordnates, they wll be marked wth the ndex rel. The great beneft of descrbng the envronment wthn global coordnates s obvous: all the tracked object movements are ndependent of the own ego-movement. Ths ssue s emphaszed n the flter secton. Another tem to be consdered n connecton wth the IMU s ts update rate. Snce the latter s one order of magntude above the scannng rate, no poston nterpolaton s necessary. Otherwse, one would have to nterpolate the ego-movement, for nstance, wth a separate flter. 361

4 MICHAEL THUY, FERNANDO PUENTE LEÓN: NON-LINEAR MULTIMODAL OBJECT TRACKING BASED Trackng Theory of Multmodal Object Trackng 3.1. General formulaton of the posteror densty dstrbuton The Monte Carlo smulaton can be seen as a partcular soluton to the general predcton and trackng problem of the current state vector x t based on all avalable observatons Z t. Therefore, one has to determne the posteror probablty densty functon, whch estmates the dstrbuton of the current state vector based on all measurements: p( Xt Z t), () [ 1 t x, t x, 3 t x, K, n t where X t represents the state varable vectors x t = x ] for all ponts n tme untl the current tme t: X t = { x 0, x 1,K, x t}. (3) [ 1 t z, t z, 3 t z, K, n t Analogously, Z t denotes the observaton vector z t = z ] untl the pont n tme t: Z t = { z 1, z,k, zt}. (4) In the followng, the lkelhood functon s descrbed by: p( z t x t). (5) Assumng an underlyng Markov process and the lkelhood dstrbuton as p( z t x t), we can now apply Bayes' rule to obtan the posteror densty: p( x t Z t)= p ( zt x t) p x t Z p z t Z t 1 ( t 1) ( ) T. (6) For the updatng phase.e. the estmaton of the current state based on all past observatons one can determne the predcton densty dstrbuton as follows: t p( x t Z t 1)= p( x t x t 1) p( x t 1 Z t 1) dx t 1. (7) From a theoretcal pont of vew, the problem s solved. However, a look at Eq. (7) reveals a complex multdmensonal ntegral whch ncorporates an ndefnte number of observatons Z t 1 untl the tme t 1. Consequently, one has to seek for a feasble numercal approach to calculate the posteror densty dstrbuton p( x t Zt 1). 3.. The Monte Carlo approach The basc dea about the sequental Monte Carlo smulaton s to represent the posteror dstrbuton through a lmted amount of partcles. Each partcle can be descrbed through a correspondng state vector wth ts approprate weght. Thus, the posteror dstrbuton s sampled by the partcles. Usng N partcles wth ther state vectors { X t, = 0,K,N} one can nfer the correspondng weghts as follows: 36 w t p ( Xt Z t) ( t Z t), (8) q X T

5 where q(.) represents the mportance densty. But stll we have to ncorporate all the observatons Z t tll tme t. Further proceedng fnally leads to an teratve descrpton of the weghts: w t w t 1 p t ( z x t )p x t x q ( t 1) ( x t x t 1, zt). (9) Now the choce of the mportance densty becomes the most crucal task. Assumng that the mportance densty functon to represent the process model as well as the change of the state vector does not depend on the observatons: one can easly determne the formula for the weghts q( x t x t 1, zt)= q( x t x t 1), (10) w t of the -th partcle: w t p( z t x t ). (11) Eq. (11) means that the correspondng partcle weght w t can be calculated wth the help of the lkelhood functon p( z t x t ). As all the calculatons can now be done sequentally, the flter s named Sequental Importance Samplng flter. After all, an ntal state dstrbuton represented by a certan amount of partcles wth ther correspondng weghts can now be propagated n tme for one tme step t utlzng the process model resp. the process functon. The observaton then leads to the correspondng partcle weght w t. But what now can happen s a depleton of the partcles, such that many partcles feature weghts that tend to zero. Ths leads to an unbalanced sampled posteror dstrbuton. A resamplng strategy prevents ths development. Therefore, after the measurement update, the partcles wth a low weght are omtted, whereas partcles wth hgh weghts are duplcated Fnal trackng flter After the theoretcal ntroducton n the last paragraph, all the ngredents for the flter desgn are dscussed. Based on the knowledge of the underlyng process model q x t x t 1 the characterstc lkelhood functon p ( ) and ( z t x t) for the sensor, one can compose the partcle flter. In contrary to a Kalman or extended Kalman flter, t s now possble to model all nose sources as well as non-gaussan. The multmodal character of the partcle flter occurs due to the non-lnear process model as well as a possble multmodal lkelhood functon. 4. Non-Lnear Observaton and Process Model 4.1. The bcycle process model Metrol. Meas. Syst. Vol. XVI (009), No 3, pp After the fundamentals have been ntroduced, we can now present the underlyng process functon. Frst, we have to defne our state vector x: T x[t] = (x[t], y[t], v[t], a[t], β[t]), (1) 363

6 MICHAEL THUY, FERNANDO PUENTE LEÓN: NON-LINEAR MULTIMODAL OBJECT TRACKING BASED... T ( ) represents the object reference pont, v the object velocty, a the object where x,y acceleraton, and β the yaw angle. The values are llustrated n Fg. 1. Fg. 1. State values representng the process model. Wth the defnton of these state varables, we can now formulate our non-lnear, tmedscrete process model: v[t] cos(β[t]) 0.5 v[t] cos(β[t]) v[t] sn(β[t]) 0.5 v[t] sn(β[t]) x[t +1] = x[t]+ a[t] T + a[t] T = x[t]+ F(x[t]), (13) whch characterzes the change of the state varables wthn one tme step T. Assumng ths process model as the actual object movement characterstcs, no object can move to the left or to the rght wthout gong forward as well. Ths real physcal behavour prevents a sde-slppng movement of tracked objects. Ths often occurs wth lnear object models and non-accurate object detecton, when one tracked object s splt nto two objects. In order to handle process model errors correctly, we have to expand the process model wth the help of nose sources. Consequently, equaton (13) becomes: x[t +1] = x[t] + F(x[t]) + n[t], (14) where n ncorporates the specfc nose source. As we model the nose terms to be ndependent unbased Gaussans, ther characterstcs are descrbed by ther varance vector r. 4.. The observaton model To complete all ngredents for the flter, we now have to face the observaton model. Ths means analogously to the defnton of state varables the determnng of the values one can extract form the measurement. For the sake of robustness, we only analyze a reference pont ( x obj, y obj) for each detected object. Analyzng the yaw angle leads to bg errors for long dstances. As already mentoned n equaton (11), one has to fnd the characterstc lkelhood functon for the defned observaton values. For ldar sensors, often Gaussan nose models are used, whch leads to the desred two-dmensonal lkelhood functon that only depends on the extracted coordnates x obj and y obj as observaton varables: 364

7 w t p ( z t x t )= A exp ( x obj x ) σ x ( y obj y ) σ. (15) y One has to emphasze once agan that the only data needed by the flter s the absolute object poston. All the other state varables are nferred from the process model. Thus, only x obj and y obj are needed by the lkelhood functon. Stll, the observaton model does not handle more than one object. Snce the feature extracton stage yelds the measured object postons ( x obj,k, y obj,k), one can now expand the lkelhood functon to a multmodal behavour. To ths end, we regard each detected object poston as a hypothess that represents a potental object n space. Consequently, one can formulate the resultng weght functon as follows: w t p( z t x t )= k k ( x obj,k x ) ( y obj,k y A exp ) σ x,k σ y,k, (16) where A k denotes the measurement specfc constant whch adjusts the nfluence of the correspondng object wthn the whole sum. In our contrbuton, we dd not model the sensor varance as a value dependng on the measurand, snce the nfluence s neglgble. The varance tself s determned by experment. 5. Feature Extracton Stage Metrol. Meas. Syst. Vol. XVI (009), No 3, pp The man task of the feature extracton stage s the determnaton of the measurement values needed by the flter,.e. the generaton of an object lst contanng the object coordnates. Snce all the trackng s done n a fxed-world coordnate system, the object poston must be converted to an UTM descrpton. Fg.. Raw data of a ldar scan. A ldar scan from the front ldar can look as shown n Fg.. As denoted n ths fgure, detectng objects lke cars out of the ldar raw ponts can be solved by analysng the dstance of consecutve laser ponts. Snce the laser rays are even for long dstances up to 80 m suffcently close each to another, an object slhouette yelds to ldar ponts whch are very close to each other as well. If other objects appear, t s very lkely that the dstance between two laser hts shows a dstance leap. 365

8 MICHAEL THUY, FERNANDO PUENTE LEÓN: NON-LINEAR MULTIMODAL OBJECT TRACKING BASED... Consderng the dstance between two laser ponts belongng to the same object, one can easly magne that the dstance grows wth ncreasng measurement dstance. Ths s due to the equally-spaced angular resoluton of the scanner. Calculatng the dstance-dependent spacng D between two consecutve and ndependent ldar ponts r and r +1 leads to the followng formula: D( r +1, r ) = r + r +1 r r+1cos( α), (17) α The threshold whch should be exceeded by two subsequent ldar ponts n order to separate them nto two objects due to the dstance leap can be descrbed by: d th ( r +1, r ) = d th,0 + d th,1 ( α, r +1, r ). (18) Ths formula can be separated nto two addtve terms. The frst term d th,0 represents a constant bas. The second summand s responsble for the dstance dependence and can be calculated accordng to the next equaton: d th,1( α, r +1,, r ) = tan( α) mn{ r +1 r }. (19) If the dstance between two consecutve laser ponts s less than d th, the ponts are assocated wth the same object. Otherwse, a pont of a new object has been detected. Consequently, the segmentaton result s an obstacle lst, contanng all detected obstacles ncludng ther correspondng scannng ponts. At ths pont, no ldar pont has been omtted, and no a pror shape nformaton has been ncorporated. In order to generate for each object a reference pont, we calculate ts center of gravty, whch results n the object poston ( x obj, y obj). 6. Flter Composton The followng paragraph descrbes the resultng flter, after all ts parts have been ntroduced. The flow for trackng objects s formulated sequentally: 1. All the partcles from tme step t 1 are propagated through the process model.. The data comng from the two ldars at tme step t are regstered n the common fxed world coordnate system. 3. The data segmentaton stage segments the raw ldar data of the current frame. The result s a lst of objects characterzed through ther absolute coordnates ( x obj, y obj). 4. Based on the object features, the mult-modal weght functon accordng to equaton (16) s constructed. 5. Recalculaton of the weghts of each partcle s perfomed. 6. Fnally, the estmated object postons represented by clusters of partcles are extracted out of the state space. Ths can be performed by dynamc k-means clusterng, whch permts an arbtrary number of clusters. The clusterng mechansm automatcally ncreases the number of clusters n the case of hgh dscrepancy of partcles from ther correspondng center of gravty. However, as we are trackng automobles, the clusters bascally do not nterfere, and non-representatve ponts can be omtted to acheve a hgher effcency. 366

9 7. Results Metrol. Meas. Syst. Vol. XVI (009), No 3, pp Ths secton presents the results. The correspondng processng s performed n real tme. Ths means, the presented calculaton can be completed wthn the sensor s sample tme. Therefore, the code has been optmzed for parallel computng on a multcore processor archtecture. Fg. 3 shows the chosen ntal spatal state dstrbuton of the partcle flter. To ths end, 1000 partcles were spread unformly around the ego-poston. Fg. 3. Intal spatal state dstrbuton. Fg. 4 shows the state dstrbuton after object trackng over 140 tme-steps. One can clearly dentfy the resultng clusters, whch represent dfferent objects. The sdebar on the rght sde descrbes the specfc partcle weght. The clusterng process successvely separates the three clusters. Fg. 4. State dstrbuton after sequental trackng. Fg. 5 shows on the rght hand sde the spatal dstrbuton of objects detected especally at the sde of the street. One the left hand sde one can see the orgnal, already regstered ldar scan. Snce t was manly the central barrer and grass whch was wrongly detected as an object, the spatal elongaton s rather hgh. 367

10 MICHAEL THUY, FERNANDO PUENTE LEÓN: NON-LINEAR MULTIMODAL OBJECT TRACKING BASED... Fg. 5. State dstrbuton of a street surrounded by natural objects. Fnally, Fg. 6 shows the error for one mult-modal tracked object. The chart clearly shows the convergng phase followed by a phase of smaller error. Fg. 6. Error n meters plotted over teratons of tracked object. 8. Concluson and outlook In ths contrbuton, we ntroduced a novel approach of mult-modal object trackng. Startng from the sensor setup, we ntroduced our temporal and spatal regstraton of the ldar data. To acheve a global descrpton of the current scene, we transform all the data nto a fxed world coordnate system. In addton, ths ensures that the assumed object model has only to cover the dynamc behavor of the tracked object. The ego-movement does not affect the object measurement n any way. Thereafter, the probablstc fundamentals for object trackng were characterzed. Based upon ths, the Monte Carlo approach for solvng ths problem was presented. The next paragraph descrbed our assumed process model. For the observaton model, we ntroduced the novel mult-modal weght functon, whch combnes all the detected objects. The feature extracton stage completed all the ngredents needed for the trackng. Thereafter, the results secton showed the performance of our approach. To ths end, we gave an overvew over the ntal spatal dstrbuton. Startng from that pont, the objects representng clusters are shaped by the flter. In addton, we gave a dstrbuton for a scenaro 368

11 n whch only typcal off-street objects were detected, whch can be dentfed thanks to ther large spatal dmensons. Acknowledgment The authors gratefully acknowledge support of ths work by the Deutsche Forschungsgemenschaft (German Research Foundaton) wthn the Transregonal Collaboratve Research Centre 8 Cogntve Automobles. References Metrol. Meas. Syst. Vol. XVI (009), No 3, pp [1] C. Hoffman, T. Dang, C. Stller: Vehcle detecton fusng D vsual features. IEEE Intellgent Vehcles Symposum, June 004, pp [] T. Dang: An teratve parameter estmaton method for observaton models wth nonlnear constrants. Metrol Meas Syst, vol. 15, no. 4, 008, pp [3] S. Kammel, J. Zegler, B. Ptzer, M. Werlng, T. Gndele, D. Jagzent, J. Schröder, M. Thuy, M. Goebl, F. von Hundelshausen, O. Pnk, C. Frese, C. Stller: Team Anneway s autonomous system for the DARPA Urban Challenge 007. Internatonal Journal of Feld Robotcs Research, vol. 5, no. 9, 008, pp [4] M. Thuy, A. Saber Tehran, F. Puente León: Bayessche Fuson von Stereobldfolgen und Ldardaten. Bldverarbetung n der Mess- und Automatserungstechnk. VDI-Berchte, VDI Verlag, Düsseldorf, 007, pp [5] S. Wender K. Detmayer: A feature level fuson approach for object classfcaton. IEEE Intellgent Vehcles Symposum, June 007, pp [6] A. Mendes, L. Bento, U. Nunes: Mult-target detecton and trackng wth a laser scanner. IEEE Intellgent Vehcles Symposum, June 004, pp [7] M. Thuy, F. Puente León: Non-lnear, shape ndependent object trackng based on D ldar data. IEEE Intellgent Vehcles Symposum, June 009, pp [8] M. Marron, J. Garca, M. Sotelo, D. Fernandez, D. Pzarro: xpfcp : An extended partcle flter for trackng multple and dynamc objects n complex envronments. IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems, Aug. 005, pp [9] E.B. Koller-Meer, F. Ade: Trackng multple objects usng the condensaton algorthm. Robotcs and Autonomous Systems, vol. 31, 001, pp [10] M. Marron, J. Garca, M. Sotelo, M. Cabello, D. Pzarro, F. Huerta, J. Cerro: Comparng a Kalman flter and a partcle flter n a multple objects trackng applcaton. IEEE Internatonal Symposum on Intellgent Sgnal Processng, Oct. 007, pp [11] M. Goebl, M. Althoff, M. Buss, G. Farber, F. Hecker, B. Hessng, S. Kraus, R. Nagel, F. Puente León, F. Ratte, M. Russ, M. Schwetzer, M. Thuy, C. Wang, H. Wuensche: Desgn and capabltes of the munch cogntve automoble. IEEE Intellgent Vehcles Symposum, June 008, pp [1] M. Thuy, M. Althoff, M. Buss, K. Depold, J. Eberspächer, G. Färber, M. Goebl, B. Heßng, S. Kraus, R. Nagel, Y. Naous, F. Obermeer, F. Puente León, F. Ratte, C. Wang, M. Schwetzer, H.-J. Wünsche: Cogntve Automobles new concepts and deas of the transregonal collaboratve research centre TR- 8. Aktve Scherhet durch Fahrerassstenz, Garchng, Germany, Aprl