Estimation of Automotive Target Trajectories by Kalman Filtering

Transcription

1 Buleinul Şiinţific al Universiăţii "Poliehnica" din imişoara Seria ELECRONICĂ şi ELECOMUNICAŢII RANSACIONS on ELECRONICS and COMMUNICAIONS om 58(72), Fascicola 1, 2013 Esimaion of Auomoive arge rajecories by Kalman Filering Adrian Macaveiu 1 Andrei Campeanu 1 Ioan Nafornia 1 Absrac 24- and 77-GHz auomoive radar sensors have been inroduced ino series producion by car manufacurers. An applicaion aiming o increase raffic safey consiss in deecing and racking Vulnerable Road Users (VRU) and vehicles in fron of he car. his is achieved by using a single 24-GHz radar sensor, capable of measuring range, radial velociy and azimuh angle even in muliple arge scenarios. In his paper a signal processing algorihm for arge racking is presened. arge posiion and speed in Caresian coordinaes are esimaed wih he aid of a Kalman filer. he work is developed under he FP7 EC funded projec ARRAC. Keywords: racking, Kalman filer, auomoive radar In order o measure arge range and velociy a suiable waveform needs o be designed. he classical pulse waveform (Fig. 1) uses he ime delay beween he ransmied and received pulses o calculae he arge range, and he Doppler frequency o measure he velociy, bu i generaes high compuaional complexiy, so i is no suiable for auomoive radar applicaions [1]. A() τ 1 τ 2 I. INRODUCION Advanced Driver Assisance Sysems are based on auomoive radars in he 24- and 77-GHz band because of heir capabiliy o operae in all weaher and lighing scenarios and because hey are cheaper han infrared or video based sysems. his paper focuses on he use of 24-GHz radars for arge racking wih he aid of Kalman filers. Differen ypes of waveforms which can be used in 24-GHz auomoive radars are reviewed in secion II. Secion III gives a possible signal processing srucure for racking based on raw deecions provided by he sensor. An imporan par of he algorihm is he Kalman filer used o esimae he curren arge parameers and o predic he new se of parameers for he nex ieraion. he filer needs o be flexible enough o deal wih several ypes of arge rajecories wih variable speed, so care mus be aken in choosing he appropriae parameers in he filer design sage. We have generaed a number of differen arge movemen pahs in order o es he Kalman filer concerning is capabiliies o esimae arge posiion and velociy. Secion IV presens he scenarios considered for esing and he racking resuls, while secion V summarizes he conclusions. pulse pulse repeiion Fig. 1. Pulse radar waveform ransmi signal Receive Signal o achieve low measuremen ime and o overcome he drawback of pulse waveforms, he coninuous wave (CW) radar was inroduced. here are wo main ypes of CW waveforms: he linear frequency modulaed (LFM) and he frequency shif keying (FSK), boh presened in Fig. 2. When using he LFM waveform, here is a frequency shif produced by wo causes: he ime delay of he received echo signal and he Doppler-Effec. hus, he range and velociy canno be deermined unambiguously from a single chirp signal. Muliple arge scenarios produce ghos arges in he rangevelociy plane. his is parly resolved by considering he FSK waveform, where we can measure he Doppler frequency from he wo differen frequency signals by applying an FF. II. AUOMOIVE RADAR WAVEFORMS 1 Faculy of Elecronics and elecommunicaions, Communicaions Dep. Bd. V. Parvan 2, imisoara, Romania, {adrian.macaveiu,andrei.campeanu,ioan.nafornia}@up.ro

2 f() ransmi Signal Receive Signal f() ransmi Signal f() he phase shif beween he echoes of he wo alernaing signals inroduced by he ime delay of he received signal allows range measuremen. he drawback of he FSK waveform is ha i canno resolve saionary arges. Also, i offers no range resoluion, meaning i is no able o deec muliple arges. For muliple arge siuaions, a varian of he LFM echnique described in [2] and [3] is applied, by ransmiing a number of chirps wih differen frequency modulaions, ranslaing in differen sweep raes (Fig. 3). he drawback of his ype of waveform is he exended measuremen ime. f() Δf Fig. 2. LFM and FSK waveforms f B f A f() f b chirp f shif Fig. 4. MFSK waveform ransmi Signal Receive Signal chirp f sweep f sweep Fig. 5. he rapid chirp waveform differen radial velociies in he same range gae. he resuling range-velociy diagram has no ambiguiies. III. SIGNAL PROCESSING SRUCURE Fig. 3. LFM waveform used in muliple arge siuaions he combinaion beween FSK and LFM waveform principles, presened in [2] and [4] enables unambiguous range and velociy measuremens in muliple arge scenarios wih he advanage of a shorer measuremen ime. he concep is shown in Fig. 4 and i is called MFSK. I consiss of wo sepwise linearly modulaed signals wih a frequency shif beween hem. hey are ransmied in an inerwined way. he frequency difference (bea frequency) obained from he received signal conains informaion abou range and velociy. he phase shif beween he wo signals A and B measured a he bea frequency also depends on range and velociy. hus, a linear sysem of wo equaions can be solved for finding he wo parameers of ineres unambiguously even in muliple arge environmens. Curren research projecs, like he one described in [5], use a sequence of consecuive chirps as a ransmi signal (Fig. 5). In order o avoid making he difficul phase difference measuremen, his mehod makes use of wo FF operaions. he firs one is done on each received chirp o find he bea frequency and i gives informaion abou arges placed a differen ranges. he second FF is performed for each range gae (which has samples from every chirp) o deermine he Doppler frequency and hus deec arges wih We have proposed he signal processing algorihm given in Fig. 6 for esimaing arge range and velociy and performing he racking operaion for deeced objecs. rv,, r Deecion o objec associaion Kalman filering Fig. 6.Signal processing scheme he radar measures he arge range r, he radial velociy v r and he azimuh angle φ a measuremen Cluer separaion Objec o rack associaion and rack managemen Caresian parameers calculaion Plo valid arge racks

3 imesamp. Besides he deecions which correspond o real arges, here are also reflecions coming from he surrounding environmen such as rees, buildings or raffic signs. hese deecions need o be filered ou so hey are no fed o he following processing blocks. All of he unwaned deecions are labeled as cluer, and hey are eliminaed by evaluaing heir radial velociy. A his poin, scenarios wih a saionary ego vehicle are considered, so i is appropriae o declare ha a very slow moving or saionary deecion is acually cluer and needs o be lef ou. In more complicaed scenarios where he ego vehicle is also maneuvering wih a cerain speed and yaw rae, compensaion is needed in boh radial velociy and azimuh angle for all deecions in order o obains heir posiion parameers relaive o he road coordinae sysem and he acual velociy. Research in his field has repored ha he arges deeced by an auomoive radar sensor exhibi modified range and Doppler profiles han usual radar arges. Indeed, experimens have shown ha in general, a longiudinally moving vehicle has an exended range profile due o is size in comparison o he radar range resoluion (which can be smaller han 1 m). On he oher hand, a vehicle has a poin-shaped velociy profile since he radar sees a single block ravelling a one speed. In opposiion o his, a laerally moving pedesrian (he usual sree crossing scenario) has a poin shaped range profile and an exended velociy profile because i has hree pars which move wih differen frequencies: he arms, orso and legs [6]. his difference beween arge ypes enables he feaure exracion which can be used o classify deeced arges, hus making he auomoive radar a suiable ool for pedesrian recogniion. Anoher remark is ha in hese ypes of scenarios he arge usually has muliple reflecion poins. Based on he above observaions, a firs deecion-oobjec daa associaion block has been inegraed o make sure ha muliple deecions from he same arge are grouped ino he same objec. his is done by building a gae around he objec in he hreeparameer space and analyzing which deecions fall ino ha gae. he resul of his procedure is a lis of objecs which can hen be associaed wih he exising racks by using a neares neighbor echnique. A new rack is iniialized if he curren objec does no belong o any exising rack. Ineviably he oucome of his block will conain a number of false racks in addiion o he rue arge rack. Only he racks corresponding o real arges will be fed o he Kalman filering algorihm. he challenge in auomoive radar sysems is he abiliy o successfully deermine he racks for all deeced arges in real raffic, muliple arge scenarios. In his case, he daa associaion procedure becomes more complicaed since i needs o deal wih objecs which are randomly appearing, disappearing or occluding each oher in he radar field of view. A more suiable daa associaion algorihm called he Hungarian algorihm is presened in [7] and can be applied o objec racking in Caresian coordinaes. he Kalman filering sage has wo goals: he firs is o offer an esimaion of he arge posiion and speed a he curren imesamp. he second goal is o make a predicion abou hese parameers for he nex imesamp +1 which will be used o minimize he esimaion error a he nex ieraion when new measuremens will be available. he soluion is compued in a recursive manner [8], [9]. Based on he heoreical groundwork se in [10] we have implemened in his paper a linear Kalman filer which processes arge range and velociy in Caresian coordinaes. herefore, he sysem sae a he curren imesamp is given by: x x vx y v y, (1) where denoes he ranspose. he arge movemen is described by a consan velociy model. his means ha he sae predicion for imesamp based on he previous sae esimaion a -1 is given by: which is equivalen o: x Ax w, (2) 1 1 x x 0 v x v x w x y y 0 v v w y y 1 y 1 ', (3) where w 1 is he Gaussian sae noise vecor of zero mean and E x covariance marix. A represens he sae ransiion marix and is he measuremen period, chosen o be equal o 38 ms, according o he analysis made in [11] for he MFSK waveform. Because we need o esimae boh arge posiion and speed in Caresian coordinaes, he measuremen marix of he Kalman filer is buil accordingly, o specify ha boh ses of parameers are being measured: H. (4) he measuremen equaion a imesamp is given by: z Hx n, (5) where n is he measuremen noise vecor, which has zero expecaion and E z covariance marix. he values for he measuremen and sae noise covariance marices are chosen in he following way:

4 as in a pracical scenario, he measuremens are highly affeced by noise, so we assign large values o E z. We also keep in mind ha he speed measuremens are more inaccurae ha he range measuremens. his means ha he variance of he speed measuremens should be larger han he variance for he range measuremens. As a general rule, large values of he measuremen noise covariance marix mean ha he sae esimaion is based more on he sae predicion model han on he measured parameers. he oupu of his assumpion is ha he rack will be smooher, so he noise is properly removed, bu he filer will no be able o follow fas variaions of posiion and speed wih grea accuracy. he simulaed racks are generaed in such a way ha hey fi ino he assumpions above. On he oher hand, if he measuremens are noisy, we need an accurae model for he arge movemen. his means ha he sae noise covariance marix will have small values, so he filer is able o correc he deviaions in he measuremens based on he sae ransiion model. In boh cases we have assumed ha he variables affeced by noise are uncorrelaed, so boh covariance marices have diagonal form. he Kalman equaions describe he predicion of he curren sae and he sae esimaion updae: x Ax P A P A E 1 1 x K P H HP H E z x x K z Hx P IK H P 1, (6) single measuremens provided by he radar, he real arge behavior is shown in black, while he Kalman filer oupu is represened in red. I can be seen in Fig. 7a ha he noise is removed successfully up o a poin where he maximum error beween he esimaion and he real arge posiion is much smaller han 1 m, which is he radar range resoluion according o he specificaion in [5]. he speed esimaion also gives good resuls, as can be seen in Fig. 7b and Fig. 7c. he laeral speed is zero because he vehicle is moving in a sraigh line away from he ego vehicle a zero azimuh angle. Fig. 7a. Posiion esimaion where K denoes he Kalman gain, P is he prediced covariance marix of he error and P is he covariance marix of he sae esimaion error. IV. SIMULAION RESULS he MALAB environmen was used o es he proposed racking soluion. Differen siuaions were generaed by simulaing various arge movemen rajecories. he algorihm esimaes he arge posiion and speed in Caresian coordinaes. he measuremen cycle was chosen o be 38 ms, according o exising 24 GHz auomoive radar specificaions. We have generaed 500 samples of received daa, yielding a oal of 19 s of recorded radar measuremens. he ego vehicle is assumed o be saionary and a single valid arge is considered. Depending on he speed, we can consider ha he arge can be associaed wih eiher a vehicle or a pedesrian in real raffic scenarios. he firs scenario simulaes a longiudinally moving arge ha is moving away from he ego vehicle wih a speed of v y = 10 km/h and v x = 0. he racking resuls are presened in Fig. 7. he x markers represen he Fig. 7b. Speed esimaion - v x Fig. 7c. Speed esimaion v y Nex, a laerally moving arge scenario was esed. he arge is passing in fron of he ego vehicle in a sraigh line a v x = 5 km/h. his ime v y = 0. he resuls are presened in Fig. 8.

5 Fig. 8a. Posiion esimaion Fig. 9b. Speed esimaion - v x Fig. 8b. Speed esimaion - v x Fig. 9c. Speed esimaion v y he nex case shows a arge performing a urn while passing in fron of he ego vehicle: v x varies linearly from -5 o 5 km/h while v y = 10 km/h (Fig. 10). Fig. 8c. Speed esimaion v y he nex scenario depics a arge moving diagonally wih respec o he ego vehicle, a v x = -2 km/h and v y = 10 km/h. he resuls are shown in Fig. 9. Fig. 10a. Posiion esimaion Fig. 9a. Posiion esimaion Fig. 10b. Speed esimaion - v x

6 I can be seen ha he speed esimaion error becomes larger when he variaion law is more complicaed (Fig. 10b and Fig. 11c). his means ha he racker is no enirely able o follow he fas speed variaions wih he Kalman parameers chosen in his simulaion. However, he posiion esimaion is very good in all cases, wih he maximum error being smaller han he radar range resoluion. V. CONCLUSIONS Fig. 10c. Speed esimaion v y he final scenario depics a arge which performs a direcion change and hen resumes back o is lane: v y has a sinusoidal variaion of ampliude 1.5 km/h and v x = 5 km/h. he resuls are presened in Fig. 11. Fig. 11a. Posiion esimaion Fig. 11b. Speed esimaion - v x Fig. 11c. Speed esimaion v y Driver Assisance Sysems use 24- and 77-GHz radar sensors. here has been a significan evoluion in he waveforms used for such sensors, from he classical pulse o he modern rapid chirp waveform, which enables unambiguous range and velociy measuremens in muliple arge scenarios and has a shor measuremen cycle. A racking algorihm using a linear Kalman filer was implemened in his paper and esed wih simulaed arge rajecories, aiming o esimae posiion and speed. he resuls of posiion esimaion are good considering he simpliciy of he arge moion model, wih he maximum error complying wih exising specificaions concerning he radar range resoluion. he speed esimaion resuls also look promising. In more complicaed scenarios where he arge is maneuvering faser and he speed variaion is rapid, he speed esimaion error is larger, bu resuls could be improved by careful selecion of he Kalman algorihm parameers. REFERENCES [1] H. Rohling, Milesones in Radar and he Success Sory of Auomoive Radar Sysems, Proceedings of he Inernaional Radar Symposium, Vilnius, Lihuania, [2] H. Rohling, M.-M. Meinecke, Waveform Design Principles for Auomoive Radar Sysems, Proceedings of he CIE Inernaional Conference on Radar, Beijing, China, [3] K. Pourvoyeur, R. Feger, S. Schuser, A. Selzer, L. Maurer, Ramp Sequence Analysis o Resolve Muli arge Scenarios for a 77-GHz FMCW Radar Sensor, 11 h Inernaional Conference on Informaion Fusion, Cologne, Germany, [4] M.-M. Meinecke, H. Rohling, Combinaion of LFMCW and FSK Modulaion Principles for Auomoive Radar Sysems, German Radar Symposium GRS2000, Berlin, Germany, [5] H. Rohling e al., he ARRAC Archiecure, hp://arrac.org/file.php/arrac_archiecure.pdf , las accessed on [6] S. Heuel, H. Rohling, wo-sage pedesrian classificaion in auomoive radar sysems, 18 h Inernaional Radar Symposium, pp , Leipzig, Germany, [7] H. W. Kuhn, he Hungarian mehod for he assignmen problem, Naval Research Logisics, Vol. 52, pp. 7-21, Wiley Periodicals, [8] P. Kim, Kalman Filers for Beginners: wih MALAB Examples, ranslaed by Lynn Huh, CreaeSpace, [9] A. Campeanu, J. Gál, Adapive signal processing mehods, imisoara, Ed. Poliehnica, 2009 (in Romanian language). [10] Y. Bar-Shalom, X. Rong Li,. Kirubarajan, Esimaion wih Applicaions o racking and Navigaion: heory Algorihms and Sofware, New York, John Wiley & Sons, [11] S. Heuel, H. Rohling, Pedesrian Recogniion in Auomoive Radar Sensors, 14 h Inernaional Radar Symposium, Dresden, Germany, 2013.